Fast Pose Tracking of Rigid Objects with Compact Pose Graph Optimization

The gist: A lightweight tracking module can be applied on top of a wide range of correspondence estimation methods to keep drifts bounded while maintaining fast runtime

Problem and Motivation

Tracking an object’s 6D pose over time is important for understanding dynamic environments, with applications in robotic manipulation [31, 44] and augmented reality [1] and human–object interaction [14]. Two requirements make this problem challenging in practice: the tracking must run online at interactive rates to support downstream planning, and it must remain accurate and temporally consistent over long horizons despite occlusions and fast motions. Existing approaches satisfy these two requirements one at a time. Long-horizon accuracy is typically obtained by anchoring poses to a 3D representation, either built online during tracking [9, 18, 42] or onboarded beforehand from CAD models or posed reference frames [43]. Interactive rates without onboarding can be achieved by relative pose estimation methods [20, 23, 24], which estimate pose from a single reference observation. However, these operate only on pairs of frames, so small errors in relative motion compound over time and cause drift.

Key Idea and Methodology

The key idea is to avoid pointbased optimization in the pose graph and operate only on relative pose constraints, which we weight by a derived uncertainty from the geometric alignment. This makes optimization independent of the number of correspondences while avoiding the direct inclusion of noisy point measurements. The formulation shifts the main question from how many correspondences are retained to how much each relative pose measurement should be trusted. We derive a pose uncertainty from the geometric alignment that produced each relative transform and use it to weight the corresponding graph constraint. Poorly constrained measurements contribute less, while reliable measurements are enforced more strongly.

Pose Graph Optimization Details

We frame object pose tracking as optimization over a pose graph G = (V, E), in which each node Vi ∈ V is the pose Ti of frame Ii and each edge Eij ∈ E carries a relative measurement Tij between the two frames. The residual Le ∈ R 6 of edge Eij is defined as LogT−1ij (T−1i Tj). We anchor the first node at T0 = I to fix the gauge freedom, and then solve for the optimal poses using a robust loss ρ(·) and Mahalanobis norm∥e∥2Σ.

Relative Pose Estimation and Uncertainty

For each pair of frames corresponding to an edge, we estimate their correspondences Cfullik = ∪M m=1, (am, bm, wm) where am, bm ∈ R cubed are corresponding 3D points in Ik and Ii, and wm are confidence weights output by the feature matcher. We run RANSAC to identify the inlier subset Cik ⊆ Cfullik, and solve for Tik via weighted Umeyama over inliers. Uncertainty Propagation involves defining a measurement noise ηik ∈ R 6 of the relative pose Tik in the tangent space, modeling it as Ti = TkTikExp(η), η ∼ N (06×1, Σik). The full Jacobian for the i-th correspondence is defined as Ji = √wi −R[bi]× I3 ∈ R 3×6.

Frame Selection and Optimization

When a new frame Ii arrives, we select up to K candidates for loop closures from a memory pool M containing past keyframes. To avoid estimating Ti−1,i before selection, we use an extrapolation motion prior T′i under a constant-velocity model. We then retain the frames in M whose rotation lies within radius τk of the rotation component of T′i: dgeo(R′i, Rj) = arccos tr(R⊤ j R′ i) − 1/2 ≤ τk. We select a final set K of at most K frames via farthest-frame sampling over timesteps to maximize information gain. The pose graph is updated using iSAM2 [4] which maintains a sparse factorization of the linearized system using a Bayes-tree representation and performs selective relinearization.

Evaluation and Results

Across four real-world RGB-D benchmarks, our approach achieves tracking accuracy comparable to reconstruction-based trackers with a fraction of the optimization cost. In Table 7, we show that ours w.o. FFS use the direct K neighbors to build loop closure edges; Ours w.o. memory pool allows loop-closure candidates to be drawn from all past frames. Our method runs at 10.0 fps on average, 5.5 to 7 times faster than the TbR (BundleSDF) and PGO baselines (BundleTrack (R.)) benchmarked on the same GPU. We hypothesize that this difference arises from how the two methods propagate information over time. Our method maintains the lowest mean error on both axes throughout, and, critically, shows no sustained failure episode over the full run.

Conclusion

In this work, we propose a lightweight, correspondenceagnostic framework for online 6D pose tracking of novel objects. Rather than fusing dense correspondences into the objective, we use them only to produce relative pose measurements, which the pose graph combines with geometryderived per-edge uncertainty and viewpoint-diverse frame selection. Compact relative pose measurements, weighted by how well each is constrained, provide comparable accuracy at substantially lower cost. Our results suggest that accurate object tracking does not require retaining dense correspondences throughout optimization.

Limitation

Our method has two main limitations. First, we assume the object is rigid and we leave tracking articulated and deformable objects as future work. Second, our covariance is a local approximation around the solution RANSACUmeyama converges to, so it can capture a flat, poorlyconstrained direction but not a well-separated alternative solution (e.g., for textureless symmetric objects).

References

[39] Shinji Umeyama. Least-squares estimation of transformation parameters between two point patterns. IEEE Transactions on pattern analysis and machine intelligence, 13(4):376–380, 1991

[40] Bowen Wen and Kostas Bekris. Bundletrack: 6d pose tracking for novel objects without instance or category-level 3d models. In International Conference on Intelligent Robots and Systems, pages 8067–8074, IEEE, 2021

[42] Bowen Wen, Jonathan Tremblay, Valts Blukis, Stephen Tyree, Thomas Muller, Alex Evans, Dieter Fox, Jan Kautz¨ and Stan Birchfield. Bundlesdf: Neural 6-dof tracking and 3d reconstruction of unknown objects. In CVPR, pages 606–617

[43] Bowen Wen, Wei Yang, Jan Kautz, and Stan Birchfield. Foundationpose: Unified 6d pose estimation and tracking of novel objects. In CVPR, pages 17868–17879

[44] Yanming Wu, Hui Zhang, Patrick Vandewalle, Peter Slaets, and Eric Demeester. A comprehensive review on advances in instance-level 6d object pose tracking.

1. Start online tracking without requiring expensive onboarding or full reconstruction by operating only on relative pose constraints instead of maintaining a 3D representation. This enables tracking to be ready to use and run in real time for robotic manipulation and augmented reality applications.

2. Implement a lightweight, correspondence-agnostic framework that can be paired with any correspondence estimation method (e.g., RoMa v2 or LoFTR) without retraining or per-object onboarding. This allows the system to adapt to diverse visual data sources immediately upon deployment.

3. Improve long-horizon consistency by using uncertainty derived from the geometric alignment to weight pose graph constraints, ensuring that poorly constrained measurements contribute less while enforcing reliable ones more strongly.

4. Enhance robustness against noisy or degraded correspondence quality by deriving per-edge pose uncertainty, which allows the system to avoid directly including noisy point measurements in the global objective.

5. Achieve superior tracking accuracy on challenging cases like textureless symmetric objects and thin objects viewed from end-on views by using an uncertainty-aware PGO framework, as shown in Figure 10.

6. Reduce computational cost substantially compared to reconstruction-based trackers by achieving tracking accuracy comparable to reconstruction-based trackers with a fraction of the optimization cost.

Tracking a novel object's 6D pose over long horizons currently requires either expensive onboarding or a reconstruction maintained throughout the sequence. This makes current trackers impractical for robotic manipulation and augmented reality, which need trackers that are ready to use and run in real time. We show that a lightweight tracking module can be applied on top of a wide range of correspondence estimation methods to keep drifts bounded while maintaining fast runtime. Our key idea is to avoid point-based optimization in the pose graph and operate only on relative pose constraints, which we weight by a derived uncertainty from the geometric alignment. This makes optimization independent of the number of correspondences while avoiding the direct inclusion of noisy point measurements, leading to fast and robust long-term tracking. Across four real-world benchmarks, our approach achieves tracking accuracy comparable to reconstruction-based trackers with a fraction of the optimization cost. Overall, these results suggest that a compact and reliable pose graph optimization can provide long-horizon consistency at substantially lower computational cost.