refactor(backend): keep the unmerged graph odometric; derive merged poses via per-node transforms#157
Draft
harelb wants to merge 7 commits into
Draft
Conversation
have_loopclosures_ is latched forever, so after the first LC every spin ran a full KimeraRpgo batch solve (1.86s x 314 spins for ONE LC in the box_7 run = 41% of backend time), starving the queue and lagging the published DSG by 15-20 min. Solve only when new LC factors arrived; between solves deform with the cached optimizer values (temp values cover not-yet-solved nodes).
backend/optimization and backend/mesh_update were never registered, so dsg_pgmo_status.csv optimize_time/mesh_update_time were always NaN.
The cleanup-hook invocation, the forced-active flag restore, and the new-node status clear all ran inside the per-functor loop, so only the first functor of each spin observed forced-active nodes and new-node status, and earlier functors' cleanup hooks re-ran once per remaining functor. Move all three after the loop so they happen once per spin.
…lues updateFromValues wrote the optimized position through the unmerged layer view before cloning into the merged graph, leaking optimized state into the unmerged (odometric) graph. Clone first and only write the merged graph, and find place merges on the merged graph like the object tests already do.
… via per-node transforms Partially supersedes the narrow init_pos cache: with the in-place unmerged position write removed, the unmerged graph is odometric by construction and IS the source of original values, so the cache no longer needs to freeze per-node state. - the deform callback no longer writes into the unmerged graph; it reads the odometric source attributes and writes position, bounding box, and (new) world_R_object field-wise into the merged graph only, via applyNodeDeformation (field-wise so merged-only state, e.g. merge-accumulated mesh connections, survives) - NodeCache drops init_pos freezing, refreshes unconditionally, and records each node's last odometric->optimized transform - the traversability merge hook re-applies the surviving node's last transform to attributes rebuilt from odometric unmerged constituents, keeping merged parents in the optimized frame after applyMerges/updateAllMergeAttributes - documents the odometric invariant on unmerged_graph_/source_graph_ (relies on every post-LC spin receiving optimizer values - fresh from a solve or cached, per the new-loop-closure gating - so merged active nodes reset by mergeGraph are re-derived)
Pass-0 merge finding ran on the source (unmerged) graph, which is odometric: duplicates revealed by a loop closure only overlap in the optimized frame, so post-LC duplicates were never proposed. Run pass 0 on the target graph like the exhaustive passes already do (applyMerges already remaps proposals through prior merges). Move the forced-active hack before the graph merge so NEW-but-archived nodes are also active in the target graph's views, and restore the flag on the target copies at the end of the spin (they are never re-cloned once the source node is archived again, so the flag would otherwise stick forever).
The image-storage rework of UpdateObjectsFunctor::call stopped writing mesh-derived geometry back into the unmerged graph (clone-first), so find_merges run against the unmerged graph sees stale zero-extent boxes and proposes nothing. Merges are found on the merged graph, which is where the updated geometry lives; point the test helper there to match (the DsgUpdater pass-0 call site changes separately).
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment
1 participant
Add this suggestion to a batch that can be applied as a single commit.This suggestion is invalid because no changes were made to the code.Suggestions cannot be applied while the pull request is closed.Suggestions cannot be applied while viewing a subset of changes.Only one suggestion per line can be applied in a batch.Add this suggestion to a batch that can be applied as a single commit.Applying suggestions on deleted lines is not supported.You must change the existing code in this line in order to create a valid suggestion.Outdated suggestions cannot be applied.This suggestion has been applied or marked resolved.Suggestions cannot be applied from pending reviews.Suggestions cannot be applied on multi-line comments.Suggestions cannot be applied while the pull request is queued to merge.Suggestion cannot be applied right now. Please check back later.
Summary
Eliminates deformation compounding by construction: the backend's unmerged/source graph is never written by deformation — it stays odometric and IS the source of original values. The deform callback reads odometric source attributes and writes position, bounding box, and (new)
world_R_objectfield-wise into the merged graph only (applyNodeDeformation, unit-testable free function).NodeCacherecords each node's last odometric→optimized transform so merge hooks can bring attributes rebuilt from odometric constituents into the optimized frame.Supersedes-in-spirit the narrower cached-copy approach of #153 (that PR fixes the object-stamp bug within the old design; this one removes the class of bug — happy to coordinate which lands, cc that discussion).
Depends on #154 (stacked on its branch): with the old always-solve-after-first-LC behavior gone, post-LC spins receive cached optimizer values, which is what re-derives merged active nodes reset by
mergeGraph. The two were developed and validated together.Commits
test_dsg_updater.cppupdateFromValuesno longer mutates unmerged places (+test)applyNodeDeformation,NodeCache::last_transform, traversability merge hook re-applies the survivor's last transform; imports the fulltest_deformation_interpolator.cppsuite (no-compounding, transform caching, field-wise writes, bbox/rotation)find_mergeson the merged graph (duplicates revealed by an LC only overlap in the optimized frame) + forced-active flags restored on target copies too (a stuck flag blinds archived-only merge candidate searches)Semantics reviewers should weigh in on
KhronosObjectAttributestrajectory fields (first/last observed positions) are not transformed.UpdateObjectsFunctorfalls back to attribute positions whenmesh_connectionsis empty; behavior preserved but worth a look.Validation