Skip to content

Commit 54ada1f

Browse files
akshay-krishnanakshay-krishnan
authored
Merge pull request borglab#2351 from borglab/sim3
Class for Pose3 trajectory alignment using Sim3
2 parents 369cb1f + a326382 commit 54ada1f

File tree

6 files changed

+670
-0
lines changed

6 files changed

+670
-0
lines changed

‎gtsam/sfm/TrajectoryAlignerSim3.cpp‎

Lines changed: 119 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,119 @@
1+
/* ----------------------------------------------------------------------------
2+
* GTSAM Copyright 2010-2020
3+
* All Rights Reserved
4+
* See LICENSE for the license information
5+
* -------------------------------------------------------------------------- */
6+
7+
#include <gtsam/sfm/TrajectoryAlignerSim3.h>
8+
9+
#include <gtsam/inference/Symbol.h>
10+
#include <gtsam/slam/expressions.h>
11+
12+
#include <stdexcept>
13+
#include <unordered_map>
14+
15+
namespace {
16+
17+
using namespace gtsam;
18+
19+
struct MeasurementPair {
20+
const UnaryMeasurement<Pose3> &first;
21+
const UnaryMeasurement<Pose3> &second;
22+
};
23+
24+
std::vector<MeasurementPair> overlappingMeasurementPairs(
25+
const std::vector<UnaryMeasurement<Pose3>> &first,
26+
const std::vector<UnaryMeasurement<Pose3>> &second) {
27+
std::unordered_map<Key, size_t> indexLookup;
28+
indexLookup.reserve(second.size());
29+
for (size_t i = 0; i < second.size(); ++i) {
30+
indexLookup.emplace(second[i].key(), i);
31+
}
32+
33+
std::vector<MeasurementPair> pairs;
34+
pairs.reserve(std::min(first.size(), second.size()));
35+
for (const auto &m : first) {
36+
auto it = indexLookup.find(m.key());
37+
if (it != indexLookup.end()) {
38+
pairs.push_back({m, second[it->second]});
39+
}
40+
}
41+
return pairs;
42+
}
43+
44+
Similarity3 estimateInitialSim3(const std::vector<MeasurementPair> &overlapPairs) {
45+
if (overlapPairs.size() < 2) return Similarity3();
46+
Pose3Pairs pairs;
47+
pairs.reserve(overlapPairs.size());
48+
for (const auto &[p1, p2] : overlapPairs) {
49+
// Similarity3::Align expects the pairs to be in the form (aTi, bTi)
50+
// to estimate aSb, but we want bSa, so we swap the pairs.
51+
pairs.emplace_back(p2.measured(), p1.measured());
52+
}
53+
try {
54+
return Similarity3::Align(pairs);
55+
} catch (const std::exception &) {
56+
return Similarity3();
57+
}
58+
}
59+
60+
} // namespace
61+
62+
namespace gtsam {
63+
TrajectoryAlignerSim3::TrajectoryAlignerSim3(
64+
const std::vector<UnaryMeasurement<Pose3>> &aTi,
65+
const std::vector<std::vector<UnaryMeasurement<Pose3>>> &bTi_all,
66+
const std::vector<Similarity3> &bSa_all) {
67+
const size_t childCount = bTi_all.size();
68+
if (!bSa_all.empty() && bSa_all.size() != childCount) {
69+
throw std::invalid_argument(
70+
"TrajectoryAlignerSim3: bSa_all and bTi_all sizes differ");
71+
}
72+
73+
// Add measurement factors for all parent poses.
74+
for (const auto &meas : aTi) {
75+
initial_.insert(meas.key(), meas.measured());
76+
graph_.addPrior(meas.key(), meas.measured(), meas.noiseModel());
77+
}
78+
79+
// Measurement factors in child frame (only where camera exists in parent).
80+
for (size_t childIdx = 0; childIdx < childCount; ++childIdx) {
81+
const auto &bTi = bTi_all[childIdx];
82+
83+
const Key simKey = Symbol('S', childIdx);
84+
85+
// If initial Sim3 estimates are provided, use them.
86+
if (!bSa_all.empty()) {
87+
initial_.insert(simKey, bSa_all[childIdx]);
88+
} else {
89+
const auto overlap = overlappingMeasurementPairs(aTi, bTi);
90+
initial_.insert(simKey, estimateInitialSim3(overlap));
91+
}
92+
93+
const Expression<Similarity3> bSa(simKey);
94+
for (const auto &meas : bTi) {
95+
Key cameraKey = meas.key();
96+
if (!initial_.exists(cameraKey)) continue;
97+
98+
const Pose3_ aPose(cameraKey);
99+
const Pose3_ expected = Pose3_(bSa, &Similarity3::transformFrom, aPose);
100+
graph_.addExpressionFactor(expected, meas.measured(), meas.noiseModel());
101+
}
102+
}
103+
}
104+
105+
Values TrajectoryAlignerSim3::solve() const {
106+
if (graph_.empty() || initial_.empty()) {
107+
return initial_;
108+
}
109+
LevenbergMarquardtOptimizer optimizer(graph_, initial_);
110+
return optimizer.optimize();
111+
}
112+
113+
Marginals TrajectoryAlignerSim3::marginalize(
114+
const Values& solution, const Ordering::OrderingType ordering_type) const {
115+
Ordering ordering = Ordering::Create(ordering_type, graph_);
116+
return Marginals(graph_, solution, ordering);
117+
}
118+
119+
} // namespace gtsam

‎gtsam/sfm/TrajectoryAlignerSim3.h‎

Lines changed: 91 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,91 @@
1+
/* ----------------------------------------------------------------------------
2+
3+
* GTSAM Copyright 2010-2020, Georgia Tech Research Corporation,
4+
* Atlanta, Georgia 30332-0415
5+
* All Rights Reserved
6+
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
7+
8+
* See LICENSE for the license information
9+
10+
* -------------------------------------------------------------------------- */
11+
12+
/**
13+
* @file TrajectoryAlignerSim3.h
14+
* @author Akshay Krishnan
15+
* @date January 2026
16+
* @brief Aligning a trajectory of poses to a reference trajectory using a similarity transform.
17+
*/
18+
19+
#pragma once
20+
21+
#include <gtsam/geometry/Pose3.h>
22+
#include <gtsam/geometry/Similarity3.h>
23+
#include <gtsam/nonlinear/ExpressionFactorGraph.h>
24+
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
25+
#include <gtsam/nonlinear/Marginals.h>
26+
#include <gtsam/nonlinear/Values.h>
27+
#include <gtsam/sfm/UnaryMeasurement.h>
28+
29+
#include <vector>
30+
31+
namespace gtsam {
32+
33+
/**
34+
* @brief Aligns Pose3 trajectories from multiple child coordinate frames to a
35+
* parent reference frame using Sim3 (similarity) transformations.
36+
*
37+
* This class solves an optimization problem to find the best Sim3 transforms
38+
* (rotation, translation, and scale) that align poses from one or more child
39+
* coordinate frames to a parent reference frame. The optimization jointly
40+
* refines both the parent frame poses and the child-to-parent transformations.
41+
*
42+
* The class takes as input:
43+
* - Parent frame poses (aTi): Pose3 measurements in the parent coordinate frame
44+
* - Child frame poses (bTi_all): Pose3 measurements in one or more child frames
45+
* - (Optional) Initial Sim3 estimates (bSa_all): Initial transforms from parent
46+
* to each child frame
47+
*
48+
* The output is a Values object containing:
49+
* - Optimized parent frame poses (with keys from the input aTi measurements)
50+
* - Optimized Sim3 transforms (with Symbol keys 'S' and index for each child)
51+
*/
52+
class GTSAM_EXPORT TrajectoryAlignerSim3 {
53+
private:
54+
// Data members.
55+
ExpressionFactorGraph graph_;
56+
Values initial_;
57+
58+
public:
59+
/**
60+
* @brief Constructs a trajectory aligner with the given measurements.
61+
* @param aTi Parent frame pose measurements (key-value pairs with noise)
62+
* @param bTi_all Vector of child frame pose measurements, one vector per child
63+
* @param bSa_all Initial Sim3 estimates transforming from parent to each child.
64+
* If empty, initial estimates are computed automatically.
65+
*/
66+
TrajectoryAlignerSim3(
67+
const std::vector<UnaryMeasurement<Pose3>> &aTi,
68+
const std::vector<std::vector<UnaryMeasurement<Pose3>>> &bTi_all,
69+
const std::vector<Similarity3> &bSa_all = {});
70+
71+
/**
72+
* @brief Optimizes the graph and returns optimized poses and Sim3 transforms.
73+
* @return The optimized poses and transforms. Contains:
74+
* - Parent frame poses (with keys the same as those in input aTi measurements)
75+
* - Sim3 transforms (with keys Symbol('S', i), where i is the child index)
76+
*/
77+
Values solve() const;
78+
79+
/**
80+
* @brief Computes the marginals of the solution.
81+
*
82+
* @param solution The solution to marginalize.
83+
* Obtained from solve() or should contain the same keys as variables in graph_.
84+
* @param ordering_type The ordering type to use for the marginalization.
85+
* @return The Marginals object.
86+
*/
87+
Marginals marginalize(
88+
const Values& solution,
89+
const Ordering::OrderingType ordering_type = Ordering::COLAMD) const;
90+
};
91+
} // namespace gtsam

‎gtsam/sfm/UnaryMeasurement.h‎

Lines changed: 96 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,96 @@
1+
/* ----------------------------------------------------------------------------
2+
3+
* GTSAM Copyright 2010-2020, Georgia Tech Research Corporation,
4+
* Atlanta, Georgia 30332-0415
5+
* All Rights Reserved
6+
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
7+
8+
* See LICENSE for the license information
9+
10+
* -------------------------------------------------------------------------- */
11+
12+
#pragma once
13+
14+
/**
15+
* @file UnaryMeasurement.h
16+
* @author Akshay Krishnan
17+
* @date January 2026
18+
* @brief Unary measurement represents a measurement on a single key in a graph.
19+
* It mirrors BinaryMeasurement but holds just one key. The measurement value
20+
* and noise model are stored without an accompanying error function.
21+
*/
22+
23+
#include <gtsam/base/Testable.h>
24+
#include <gtsam/inference/Factor.h>
25+
#include <gtsam/inference/Key.h>
26+
#include <gtsam/linear/NoiseModel.h>
27+
28+
#include <iostream>
29+
#include <memory>
30+
#include <vector>
31+
32+
namespace gtsam {
33+
34+
/**
35+
* @brief Unary measurement represents a measurement on a single key in a graph.
36+
*/
37+
template <class T> class UnaryMeasurement : public Factor {
38+
// Check that T type is testable
39+
GTSAM_CONCEPT_ASSERT(IsTestable<T>);
40+
41+
public:
42+
// shorthand for a smart pointer to a measurement
43+
using shared_ptr = std::shared_ptr<UnaryMeasurement>;
44+
45+
private:
46+
T measured_; ///< The measurement
47+
SharedNoiseModel noiseModel_; ///< Noise model
48+
49+
public:
50+
/**
51+
* @brief Constructs a unary measurement with a key, value, and noise model.
52+
* @param key The key of the measurement.
53+
* @param measured The measurement value.
54+
* @param model The noise model (optional).
55+
*/
56+
UnaryMeasurement(Key key, const T &measured,
57+
const SharedNoiseModel &model = nullptr)
58+
: Factor(std::vector<Key>({key})), measured_(measured), noiseModel_(model) {}
59+
60+
/// @name Standard Interface
61+
/// @{
62+
63+
// Returns the key of the measurement.
64+
Key key() const { return keys_[0]; }
65+
// Returns the measurement value.
66+
const T &measured() const { return measured_; }
67+
// Returns the noise model.
68+
const SharedNoiseModel &noiseModel() const { return noiseModel_; }
69+
70+
/// @}
71+
/// @name Testable
72+
/// @{
73+
74+
// Prints the measurement.
75+
void print(const std::string &s, const KeyFormatter &keyFormatter =
76+
DefaultKeyFormatter) const override {
77+
std::cout << s << "UnaryMeasurement(" << keyFormatter(this->key()) << ")\n";
78+
traits<T>::Print(measured_, " measured: ");
79+
if (noiseModel_)
80+
noiseModel_->print(" noise model: ");
81+
else
82+
std::cout << " noise model: (null)\n";
83+
}
84+
85+
// Checks if the measurement is equal to another measurement.
86+
bool equals(const UnaryMeasurement &expected, double tol = 1e-9) const {
87+
const UnaryMeasurement<T> *e =
88+
dynamic_cast<const UnaryMeasurement<T> *>(&expected);
89+
return e != nullptr && Factor::equals(*e) &&
90+
traits<T>::Equals(this->measured_, e->measured_, tol) &&
91+
((!noiseModel_ && !expected.noiseModel_) ||
92+
(noiseModel_ && noiseModel_->equals(*expected.noiseModel())));
93+
}
94+
/// @}
95+
};
96+
} // namespace gtsam

‎gtsam/sfm/sfm.i‎

Lines changed: 31 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -114,6 +114,20 @@ virtual class ShonanFactor3 : gtsam::NoiseModelFactor {
114114
gtsam::Vector evaluateError(const gtsam::SOn& Q1, const gtsam::SOn& Q2);
115115
};
116116

117+
#include <gtsam/sfm/UnaryMeasurement.h>
118+
template <T>
119+
class UnaryMeasurement {
120+
UnaryMeasurement(gtsam::Key key, const T& measured,
121+
const gtsam::noiseModel::Base* model);
122+
gtsam::Key key() const;
123+
T measured() const;
124+
gtsam::noiseModel::Base* noiseModel() const;
125+
};
126+
127+
typedef gtsam::UnaryMeasurement<gtsam::Pose3> UnaryMeasurementPose3;
128+
typedef gtsam::UnaryMeasurement<gtsam::Rot3> UnaryMeasurementRot3;
129+
typedef gtsam::UnaryMeasurement<gtsam::Point3> UnaryMeasurementPoint3;
130+
117131
#include <gtsam/sfm/BinaryMeasurement.h>
118132
template <T>
119133
class BinaryMeasurement {
@@ -153,6 +167,23 @@ class BinaryMeasurementsRot3 {
153167
void push_back(const gtsam::BinaryMeasurement<gtsam::Rot3>& measurement);
154168
};
155169

170+
#include <gtsam/sfm/TrajectoryAlignerSim3.h>
171+
class TrajectoryAlignerSim3 {
172+
TrajectoryAlignerSim3(
173+
const std::vector<gtsam::UnaryMeasurement<gtsam::Pose3>>& aTi,
174+
const std::vector<std::vector<gtsam::UnaryMeasurement<gtsam::Pose3>>>& bTi_all);
175+
176+
TrajectoryAlignerSim3(
177+
const std::vector<gtsam::UnaryMeasurement<gtsam::Pose3>>& aTi,
178+
const std::vector<std::vector<gtsam::UnaryMeasurement<gtsam::Pose3>>>& bTi_all,
179+
const std::vector<gtsam::Similarity3>& bSa_all);
180+
181+
gtsam::Values solve() const;
182+
gtsam::Marginals marginalize(
183+
const gtsam::Values& solution,
184+
const gtsam::Ordering::OrderingType ordering_type = gtsam::Ordering::COLAMD) const;
185+
};
186+
156187
#include <gtsam/slam/dataset.h>
157188
#include <gtsam/sfm/ShonanAveraging.h>
158189

0 commit comments

Comments
 (0)