Skip to content

Optimize FrameTransformBuffer and fix get_latest_common_time root frame bug #9

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
otamachan merged 1 commit into from
Feb 15, 2026
Merged

Optimize FrameTransformBuffer and fix get_latest_common_time root frame bug #9

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
68 changes: 60 additions & 8 deletions tfl/include/tfl/frame_transform_buffer.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -31,8 +31,9 @@ class FrameTransformBuffer
static constexpr uint32_t kDefaultCapacity = 2000;

explicit FrameTransformBuffer(uint32_t capacity = kDefaultCapacity, bool is_static = false)
: buf_(std::make_unique<TransformData[]>(is_static ? 1 : capacity)),
capacity_(is_static ? 1 : capacity),
: buf_(std::make_unique<TransformData[]>(is_static ? 1 : round_up_pow2(capacity))),
capacity_(is_static ? 1 : round_up_pow2(capacity)),
mask_(capacity_ - 1),
is_static_(is_static)
{
}
Expand All @@ -41,6 +42,7 @@ class FrameTransformBuffer
: seq_(o.seq_.load(std::memory_order_relaxed)),
buf_(std::move(o.buf_)),
capacity_(o.capacity_),
mask_(o.mask_),
head_(o.head_.load(std::memory_order_relaxed)),
size_(o.size_.load(std::memory_order_relaxed)),
is_static_(o.is_static_)
Expand All @@ -53,6 +55,7 @@ class FrameTransformBuffer
seq_.store(o.seq_.load(std::memory_order_relaxed), std::memory_order_relaxed);
buf_ = std::move(o.buf_);
capacity_ = o.capacity_;
mask_ = o.mask_;
head_.store(o.head_.load(std::memory_order_relaxed), std::memory_order_relaxed);
size_.store(o.size_.load(std::memory_order_relaxed), std::memory_order_relaxed);
is_static_ = o.is_static_;
Expand Down Expand Up @@ -92,14 +95,14 @@ class FrameTransformBuffer
// Find insertion point: data should go in timestamp order
// Common case: new data is newest
if (data.stamp_ns >= buf_[h].stamp_ns) {
h = (h + 1) % capacity_;
h = (h + 1) & mask_;
if (sz < capacity_) {
sz++;
}
buf_[h] = data;
} else {
// Out-of-order insert: find correct position
h = (h + 1) % capacity_;
h = (h + 1) & mask_;
if (sz < capacity_) {
sz++;
}
Expand Down Expand Up @@ -187,9 +190,19 @@ class FrameTransformBuffer

FrameID get_parent(TimeNs time) const
{
TransformData d1, d2;
if (get_data(time, d1, d2) > 0) {
return d1.parent_id;
for (int retry = 0; retry < kMaxSeqRetries; ++retry) {
uint64_t s1 = seq_.load(std::memory_order_acquire);
if (s1 & 1) {
continue;
}

FrameID result = get_parent_unsafe(time);

std::atomic_thread_fence(std::memory_order_acquire);
uint64_t s2 = seq_.load(std::memory_order_acquire);
if (s1 == s2) {
return result;
}
}
return INVALID_FRAME;
}
Expand All @@ -211,8 +224,46 @@ class FrameTransformBuffer

private:
static constexpr int kMaxSeqRetries = 64;

static constexpr uint32_t round_up_pow2(uint32_t v)
{
if (v <= 1) return 1;
v--;
v |= v >> 1;
v |= v >> 2;
v |= v >> 4;
v |= v >> 8;
v |= v >> 16;
return v + 1;
}

// Logical index i (0=newest) to physical buffer index
uint32_t phys_idx(uint32_t head, uint32_t i) const { return (head + capacity_ - i) % capacity_; }
uint32_t phys_idx(uint32_t head, uint32_t i) const { return (head + capacity_ - i) & mask_; }

// Lightweight parent lookup — only reads parent_id, no TransformData copy
FrameID get_parent_unsafe(TimeNs time) const
{
if (is_static_) {
return buf_[0].parent_id;
}
uint32_t sz = size_.load(std::memory_order_relaxed);
if (sz == 0) {
return INVALID_FRAME;
}
uint32_t h = head_.load(std::memory_order_relaxed);
if (time == 0 || sz == 1) {
return buf_[h].parent_id;
}
// For non-zero time with multiple entries, check range and return parent
// (parent_id is same for all entries in a frame)
TimeNs newest = buf_[h].stamp_ns;
uint32_t oldest_idx = phys_idx(h, sz - 1);
TimeNs oldest = buf_[oldest_idx].stamp_ns;
if (time > newest || time < oldest) {
return INVALID_FRAME;
}
return buf_[h].parent_id;
}

// Must be called inside SeqLock read section (no seq_ check)
uint8_t get_data_unsafe(TimeNs time, TransformData & d1, TransformData & d2) const
Expand Down Expand Up @@ -310,6 +361,7 @@ class FrameTransformBuffer
alignas(64) std::atomic<uint64_t> seq_{0};
std::unique_ptr<TransformData[]> buf_;
uint32_t capacity_;
uint32_t mask_;
std::atomic<uint32_t> head_{0};
std::atomic<uint32_t> size_{0};
bool is_static_;
Expand Down
2 changes: 1 addition & 1 deletion tfl/include/tfl/transform_buffer.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -64,7 +64,7 @@ class TransformBuffer

std::optional<TransformData> walk_to_top_parent(
FrameID target_id, FrameID source_id, TimeNs time) const;

bool can_walk_to_top_parent(FrameID target_id, FrameID source_id, TimeNs time) const;
TimeNs get_latest_common_time(FrameID target_id, FrameID source_id) const;

uint32_t max_frames_;
Expand Down
115 changes: 99 additions & 16 deletions tfl/src/transform_buffer.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -307,6 +307,77 @@ std::optional<TransformData> TransformBuffer::walk_to_top_parent(
return result;
}

bool TransformBuffer::can_walk_to_top_parent(
FrameID target_id, FrameID source_id, TimeNs time) const
{
if (source_id == target_id) {
return true;
}

if (time == 0) {
time = get_latest_common_time(target_id, source_id);
if (time == 0) {
return false;
}
}

// Phase 1: walk source chain using get_parent only
FrameID frame = source_id;
FrameID top_parent = INVALID_FRAME;
uint32_t depth = 0;

while (frame != INVALID_FRAME && depth < MAX_GRAPH_DEPTH) {
if (frame >= max_frames_) {
return false;
}

if (frame == target_id) {
return true;
}

const FrameID parent = frames_[frame].get_parent(time);
if (parent == INVALID_FRAME) {
top_parent = frame;
break;
}

top_parent = frame;
frame = parent;
depth++;

if (frame == target_id) {
return true;
}
}
if (depth >= MAX_GRAPH_DEPTH) {
return false;
}

// Phase 2: walk target chain using get_parent only
frame = target_id;
depth = 0;

while (frame != top_parent && frame != INVALID_FRAME && depth < MAX_GRAPH_DEPTH) {
if (frame >= max_frames_) {
return false;
}

const FrameID parent = frames_[frame].get_parent(time);
if (parent == INVALID_FRAME) {
return false;
}

frame = parent;
depth++;

if (frame == source_id) {
return true;
}
}

return frame == top_parent;
}

std::optional<TransformData> TransformBuffer::lookup_transform(
const std::string & target, const std::string & source, TimeNs time) const
{
Expand All @@ -326,7 +397,7 @@ bool TransformBuffer::can_transform(
if (t == INVALID_FRAME || s == INVALID_FRAME) {
return false;
}
return walk_to_top_parent(t, s, time).has_value();
return can_walk_to_top_parent(t, s, time);
}

std::optional<TransformData> TransformBuffer::lookup_transform(
Expand Down Expand Up @@ -394,13 +465,16 @@ TimeNs TransformBuffer::get_latest_common_time(FrameID target_id, FrameID source
}

const TimeNs stamp = frames_[frame].get_latest_stamp();
if (!frames_[frame].is_static()) {
if (stamp == 0) {
return 0;
}
if (stamp < common_time) {
common_time = stamp;
}
const bool has_data = (stamp != 0 || frames_[frame].is_static());

if (!has_data) {
// Root frame with no data — record as chain endpoint and stop
source_chain[source_len++] = {0, frame};
break;
}

if (!frames_[frame].is_static() && stamp < common_time) {
common_time = stamp;
}

const FrameID parent = frames_[frame].get_parent(0);
Expand All @@ -421,29 +495,38 @@ TimeNs TransformBuffer::get_latest_common_time(FrameID target_id, FrameID source
}

const TimeNs stamp = frames_[frame].get_latest_stamp();
if (!frames_[frame].is_static()) {
if (stamp == 0) {
return 0;
}
if (stamp < common_time) {
const bool has_data = (stamp != 0 || frames_[frame].is_static());

if (has_data) {
if (!frames_[frame].is_static() && stamp < common_time) {
common_time = stamp;
}
}

const FrameID parent = frames_[frame].get_parent(0);
const FrameID parent = has_data ? frames_[frame].get_parent(0) : INVALID_FRAME;

for (uint32_t i = 0; i < source_len; ++i) {
if (source_chain[i].id == frame || parent == source_chain[i].id) {
bool match = source_chain[i].id == frame;
if (!match && parent != INVALID_FRAME) {
match = (parent == source_chain[i].id);
}
if (match) {
TimeNs result = common_time;
for (uint32_t j = 0; j <= i; ++j) {
if (!frames_[source_chain[j].id].is_static() && source_chain[j].stamp < result) {
if (
source_chain[j].stamp != 0 && !frames_[source_chain[j].id].is_static() &&
source_chain[j].stamp < result) {
result = source_chain[j].stamp;
}
}
return result == std::numeric_limits<TimeNs>::max() ? 0 : result;
}
}

if (!has_data) {
return 0;
}

frame = parent;
depth++;
}
Expand Down
Loading