Merge pull request #2445 from borglab/sim3-points

TrajectoryAlignerSim3 - add point3 constraints if available, optionally use GNC

Author: dellaert

gtsam/sfm/TrajectoryAlignerSim3.cpp

@@ -4,9 +4,10 @@
  * See LICENSE for the license information
  * -------------------------------------------------------------------------- */
 
-#include <gtsam/sfm/TrajectoryAlignerSim3.h>
-
 #include <gtsam/inference/Symbol.h>
+#include <gtsam/nonlinear/GncOptimizer.h>
+#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
+#include <gtsam/sfm/TrajectoryAlignerSim3.h>
 #include <gtsam/slam/expressions.h>
 
 #include <stdexcept>
@@ -22,7 +23,7 @@ struct MeasurementPair {
 };
 
 std::vector<MeasurementPair> overlappingMeasurementPairs(
-    const std::vector<UnaryMeasurement<Pose3>> &first, 
+    const std::vector<UnaryMeasurement<Pose3>> &first,
     const std::vector<UnaryMeasurement<Pose3>> &second) {
   std::unordered_map<Key, size_t> indexLookup;
   indexLookup.reserve(second.size());
@@ -41,7 +42,8 @@ std::vector<MeasurementPair> overlappingMeasurementPairs(
   return pairs;
 }
 
-Similarity3 estimateInitialSim3(const std::vector<MeasurementPair> &overlapPairs) {
+Similarity3 estimateInitialSim3(
+    const std::vector<MeasurementPair> &overlapPairs) {
   if (overlapPairs.size() < 2) return Similarity3();
   Pose3Pairs pairs;
   pairs.reserve(overlapPairs.size());
@@ -60,10 +62,15 @@ Similarity3 estimateInitialSim3(const std::vector<MeasurementPair> &overlapPairs
 }  // namespace
 
 namespace gtsam {
+
 TrajectoryAlignerSim3::TrajectoryAlignerSim3(
-    const std::vector<UnaryMeasurement<Pose3>> &aTi, 
+    const std::vector<UnaryMeasurement<Pose3>> &aTi,
     const std::vector<std::vector<UnaryMeasurement<Pose3>>> &bTi_all,
-    const std::vector<Similarity3> &bSa_all) {
+    const std::vector<Similarity3> &bSa_all, const bool use_gnc_optimizer,
+    const std::vector<std::vector<std::pair<Point3, Point3>>>
+        &overlapping_points,
+    const double point3_factor_sigma)
+    : use_gnc_optimizer_(use_gnc_optimizer) {
   const size_t childCount = bTi_all.size();
   if (!bSa_all.empty() && bSa_all.size() != childCount) {
     throw std::invalid_argument(
@@ -89,7 +96,7 @@ TrajectoryAlignerSim3::TrajectoryAlignerSim3(
       const auto overlap = overlappingMeasurementPairs(aTi, bTi);
       initial_.insert(simKey, estimateInitialSim3(overlap));
     }
-    
+
     const Expression<Similarity3> bSa(simKey);
     for (const auto &meas : bTi) {
       Key cameraKey = meas.key();
@@ -100,18 +107,42 @@ TrajectoryAlignerSim3::TrajectoryAlignerSim3(
       graph_.addExpressionFactor(expected, meas.measured(), meas.noiseModel());
     }
   }
+
+  // Any point3-point3 constraints between overlapping points.
+  if (!overlapping_points.empty()) {
+    if (overlapping_points.size() != childCount) {
+      throw std::invalid_argument(
+          "TrajectoryAlignerSim3: overlapping_points and bTi_all sizes differ");
+    }
+    for (size_t childIdx = 0; childIdx < childCount; ++childIdx) {
+      const auto &overlap = overlapping_points[childIdx];
+      const Key simKey = Symbol('S', childIdx);
+      const Expression<Similarity3> bSa(simKey);
+      for (const auto &[p1, p2] : overlap) {
+        const Point3_ p1_(p1);
+        const Point3_ b_p1_(bSa, &Similarity3::transformFrom, p1_);
+        graph_.addExpressionFactor(
+            b_p1_, p2, noiseModel::Isotropic::Sigma(3, point3_factor_sigma));
+      }
+    }
+  }
 }
 
 Values TrajectoryAlignerSim3::solve() const {
-  if (graph_.empty() || initial_.empty()) {
-    return initial_;
+  if (graph_.empty() || initial_.empty()) return initial_;
+
+  if (use_gnc_optimizer_) {
+    GncOptimizer<GncParams<LevenbergMarquardtParams>> optimizer(graph_,
+                                                                initial_);
+    return optimizer.optimize();
+  } else {
+    LevenbergMarquardtOptimizer optimizer(graph_, initial_);
+    return optimizer.optimize();
   }
-  LevenbergMarquardtOptimizer optimizer(graph_, initial_);
-  return optimizer.optimize();
 }
 
 Marginals TrajectoryAlignerSim3::marginalize(
-  const Values& solution, const Ordering::OrderingType ordering_type) const {
+    const Values &solution, const Ordering::OrderingType ordering_type) const {
   Ordering ordering = Ordering::Create(ordering_type, graph_);
   return Marginals(graph_, solution, ordering);
 }

gtsam/sfm/TrajectoryAlignerSim3.h

@@ -13,7 +13,8 @@
  * @file TrajectoryAlignerSim3.h
  * @author Akshay Krishnan
  * @date January 2026
- * @brief Aligning a trajectory of poses to a reference trajectory using a similarity transform.
+ * @brief Aligning a trajectory of poses to a reference trajectory using a
+ * similarity transform.
  */
 
 #pragma once
@@ -54,38 +55,50 @@ class GTSAM_EXPORT TrajectoryAlignerSim3 {
   // Data members.
   ExpressionFactorGraph graph_;
   Values initial_;
+  bool use_gnc_optimizer_;
 
  public:
   /**
    * @brief Constructs a trajectory aligner with the given measurements.
    * @param aTi Parent frame pose measurements (key-value pairs with noise)
-   * @param bTi_all Vector of child frame pose measurements, one vector per child
-   * @param bSa_all Initial Sim3 estimates transforming from parent to each child.
-   *                If empty, initial estimates are computed automatically.
+   * @param bTi_all Vector of child frame pose measurements, one vector per
+   * child
+   * @param bSa_all Initial Sim3 estimates transforming from parent to each
+   * child. If empty, initial estimates are computed automatically.
+   * @param use_gnc_optimizer Whether to use GncOptimizer for optimization.
+   * @param overlapping_points Vector of overlapping point3-point3 constraints,
+   * one vector per child.
+   * @param point3_factor_sigma Sigma for point3-point3 constraints.
    */
   TrajectoryAlignerSim3(
-    const std::vector<UnaryMeasurement<Pose3>> &aTi, 
-    const std::vector<std::vector<UnaryMeasurement<Pose3>>> &bTi_all, 
-    const std::vector<Similarity3> &bSa_all = {});
+      const std::vector<UnaryMeasurement<Pose3>> &aTi,
+      const std::vector<std::vector<UnaryMeasurement<Pose3>>> &bTi_all,
+      const std::vector<Similarity3> &bSa_all = {},
+      const bool use_gnc_optimizer = false,
+      const std::vector<std::vector<std::pair<Point3, Point3>>>
+          &overlapping_points = {},
+      const double point3_factor_sigma = 1e-2);
 
   /**
    * @brief Optimizes the graph and returns optimized poses and Sim3 transforms.
    * @return The optimized poses and transforms. Contains:
-   * - Parent frame poses (with keys the same as those in input aTi measurements)
+   * - Parent frame poses (with keys the same as those in input aTi
+   * measurements)
    * - Sim3 transforms (with keys Symbol('S', i), where i is the child index)
    */
   Values solve() const;
 
   /**
    * @brief Computes the marginals of the solution.
-   * 
-   * @param solution The solution to marginalize. 
-   * Obtained from solve() or should contain the same keys as variables in graph_.
+   *
+   * @param solution The solution to marginalize.
+   * Obtained from solve() or should contain the same keys as variables in
+   * graph_.
    * @param ordering_type The ordering type to use for the marginalization.
    * @return The Marginals object.
    */
   Marginals marginalize(
-    const Values& solution, 
-    const Ordering::OrderingType ordering_type = Ordering::COLAMD) const;
+      const Values &solution,
+      const Ordering::OrderingType ordering_type = Ordering::COLAMD) const;
 };
 }  // namespace gtsam

gtsam/sfm/sfm.i

@@ -179,6 +179,24 @@ class TrajectoryAlignerSim3 {
       const std::vector<std::vector<gtsam::UnaryMeasurement<gtsam::Pose3>>>& bTi_all,
       const std::vector<gtsam::Similarity3>& bSa_all);
 
+  TrajectoryAlignerSim3(
+    const std::vector<gtsam::UnaryMeasurement<gtsam::Pose3>>& aTi,
+    const std::vector<std::vector<gtsam::UnaryMeasurement<gtsam::Pose3>>>& bTi_all,
+    const std::vector<gtsam::Similarity3>& bSa_all, const bool use_gnc_optimizer);
+
+  TrajectoryAlignerSim3(    
+    const std::vector<gtsam::UnaryMeasurement<gtsam::Pose3>>& aTi,
+    const std::vector<std::vector<gtsam::UnaryMeasurement<gtsam::Pose3>>>& bTi_all,
+    const std::vector<gtsam::Similarity3>& bSa_all, const bool use_gnc_optimizer,
+    const std::vector<std::vector<std::pair<gtsam::Point3, gtsam::Point3>>> &overlapping_points);
+
+  TrajectoryAlignerSim3(
+    const std::vector<gtsam::UnaryMeasurement<gtsam::Pose3>>& aTi,
+    const std::vector<std::vector<gtsam::UnaryMeasurement<gtsam::Pose3>>>& bTi_all,
+    const std::vector<gtsam::Similarity3>& bSa_all, const bool use_gnc_optimizer,
+    const std::vector<std::vector<std::pair<gtsam::Point3, gtsam::Point3>>> &overlapping_points,
+    const double point3_factor_sigma);
+
   gtsam::Values solve() const;
   gtsam::Marginals marginalize(
     const gtsam::Values& solution, 

gtsam/sfm/tests/testTrajectoryAlignerSim3.cpp

@@ -24,7 +24,6 @@
 #include <gtsam/slam/expressions.h>
 #include <gtsam/nonlinear/utilities.h>
 
-#include <random>
 #include <vector>
 
 using namespace gtsam;
@@ -91,6 +90,10 @@ bool simClose(const Similarity3& expected, const Similarity3& actual,
   return assert_equal<Similarity3>(expected, actual, tol);
 }
 
+double simErrorNorm(const Similarity3& expected, const Similarity3& actual) {
+  return Similarity3::Logmap(expected.inverse() * actual).norm();
+}
+
 }  // namespace
 
 /* ************************************************************************* */
@@ -236,6 +239,52 @@ TEST(TrajectoryAlignerSim3, ThreeChildrenNoisy) {
   }
 }
 
+TEST(TrajectoryAlignerSim3, TwoChildrenWithOverlappingPoints) {
+  const auto parent = makeParentValues();
+  const auto child1 = transformValues(gtSim1, parent);
+  const auto child2 = transformValues(gtSim2, parent);
+
+  // Add noise to pose measurements only.
+  PoseMeasurements aTi = makeMeasurements(perturbPoses(parent), 8e-2);
+  PoseMeasurements b1 = makeMeasurements(perturbPoses(child1), 8e-2);
+  PoseMeasurements b2 = makeMeasurements(perturbPoses(child2), 8e-2);
+  ChildrenPoses bTi_all{b1, b2};
+
+  std::vector<Similarity3> sims{perturbSim3(gtSim1), perturbSim3(gtSim2)};
+
+  // 4 exact overlapping Point3 correspondences per child.
+  const std::vector<Point3> parentPoints{
+      Point3(0.25, -0.10, 0.80), Point3(1.00, 0.30, -0.20),
+      Point3(-0.40, 0.50, 0.15), Point3(0.70, -0.60, 0.25)};
+
+  std::vector<std::pair<Point3, Point3>> overlap1, overlap2;
+  overlap1.reserve(parentPoints.size());
+  overlap2.reserve(parentPoints.size());
+  for (const auto& pA : parentPoints) {
+    overlap1.emplace_back(pA, gtSim1.transformFrom(pA));
+    overlap2.emplace_back(pA, gtSim2.transformFrom(pA));
+  }
+  std::vector<std::vector<std::pair<Point3, Point3>>> overlappingPoints{
+      overlap1, overlap2};
+
+  // Optimize without point correspondences.
+  TrajectoryAlignerSim3 alignerNoPoints(aTi, bTi_all, sims);
+  const Values resultNoPoints = alignerNoPoints.solve();
+  const double simErrorNoPoints =
+      simErrorNorm(gtSim1, resultNoPoints.at<Similarity3>(Symbol('S', 0))) +
+      simErrorNorm(gtSim2, resultNoPoints.at<Similarity3>(Symbol('S', 1)));
+
+  // Optimize with exact point correspondences.
+  TrajectoryAlignerSim3 alignerWithPoints(aTi, bTi_all, sims, false, overlappingPoints,
+                                          1e-3);
+  const Values resultWithPoints = alignerWithPoints.solve();
+  const double simErrorWithPoints =
+      simErrorNorm(gtSim1, resultWithPoints.at<Similarity3>(Symbol('S', 0))) +
+      simErrorNorm(gtSim2, resultWithPoints.at<Similarity3>(Symbol('S', 1)));
+
+  EXPECT(simErrorWithPoints < simErrorNoPoints);
+}
+
 /* ************************************************************************* */
 int main() {
   TestResult tr;

python/gtsam/tests/test_TrajectoryAlignerSim3.py

@@ -59,6 +59,35 @@ def test_perfect_alignment_without_initial_sim(self):
         recovered = result.atSimilarity3(gtsam.Symbol("S", 0).key())
         self.assertTrue(gt_bSa.equals(recovered, 1e-6))
 
+    def test_perfect_alignment_without_initial_sim_with_points(self):
+        parent = make_parent_poses()
+        gt_bSa = gtsam.Similarity3(
+            gtsam.Rot3.RzRyRx(0.2, -0.1, 0.05),
+            gtsam.Point3(0.3, -0.2, 0.1),
+            1.4,
+        )
+        child = transform_poses(gt_bSa, parent)
+
+        aTi = make_measurements(parent)
+        bTi_all = [make_measurements(child)]
+
+        parent_points = [
+            gtsam.Point3(0.25, -0.10, 0.80),
+            gtsam.Point3(1.00, 0.30, -0.20),
+            gtsam.Point3(-0.40, 0.50, 0.15),
+            gtsam.Point3(0.70, -0.60, 0.25),
+        ]
+        overlap_child = [(p, gt_bSa.transformFrom(p)) for p in parent_points]
+        overlapping_points = [overlap_child]
+
+        aligner = gtsam.TrajectoryAlignerSim3(
+            aTi, bTi_all, [], False, overlapping_points, 1e-3
+        )
+        result = aligner.solve()
+
+        recovered = result.atSimilarity3(gtsam.Symbol("S", 0).key())
+        self.assertTrue(gt_bSa.equals(recovered, 1e-6))
+
     def test_noisy_alignment_with_initial_guess(self):
         parent = make_parent_poses()
         gt_bSa = gtsam.Similarity3(