Skip to content

Commit fae071d

Browse files
varunagrawalvarunagrawal
authored
Merge pull request #2067 from mhalber/features/similarity_python_bindings
Python bindings: Enable use of Similarity2/Similarity3 in nonlinear optimization
2 parents b8dbcba + 1575968 commit fae071d

File tree

5 files changed

+97
-6
lines changed

5 files changed

+97
-6
lines changed

‎gtsam/geometry/Similarity2.h‎

Lines changed: 13 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -200,6 +200,19 @@ class GTSAM_EXPORT Similarity2 : public LieGroup<Similarity2, 4> {
200200
/// Dimensionality of tangent space = 4 DOF
201201
inline size_t dim() const { return 4; }
202202

203+
private:
204+
205+
#if GTSAM_ENABLE_BOOST_SERIALIZATION
206+
/** Serialization function */
207+
friend class boost::serialization::access;
208+
template<class Archive>
209+
void serialize(Archive & ar, const unsigned int /*version*/) {
210+
ar & BOOST_SERIALIZATION_NVP(R_);
211+
ar & BOOST_SERIALIZATION_NVP(t_);
212+
ar & BOOST_SERIALIZATION_NVP(s_);
213+
}
214+
#endif
215+
203216
/// @}
204217
};
205218

‎gtsam/nonlinear/nonlinear.i‎

Lines changed: 12 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -17,6 +17,8 @@ namespace gtsam {
1717
#include <gtsam/geometry/Point3.h>
1818
#include <gtsam/geometry/Pose2.h>
1919
#include <gtsam/geometry/Pose3.h>
20+
#include <gtsam/geometry/Similarity2.h>
21+
#include <gtsam/geometry/Similarity3.h>
2022
#include <gtsam/geometry/Rot2.h>
2123
#include <gtsam/geometry/Rot3.h>
2224
#include <gtsam/geometry/SO3.h>
@@ -74,6 +76,8 @@ class NonlinearFactorGraph {
7476
gtsam::Rot3,
7577
gtsam::Pose2,
7678
gtsam::Pose3,
79+
gtsam::Similarity2,
80+
gtsam::Similarity3,
7781
gtsam::Cal3_S2,
7882
gtsam::Cal3f,
7983
gtsam::Cal3Bundler,
@@ -544,7 +548,7 @@ class ISAM2 {
544548
bool valueExists(gtsam::Key key) const;
545549
gtsam::Values calculateEstimate() const;
546550
template <VALUE = {gtsam::Point2, gtsam::Rot2, gtsam::Pose2, gtsam::Point3,
547-
gtsam::Rot3, gtsam::Pose3, gtsam::Cal3_S2, gtsam::Cal3DS2,
551+
gtsam::Rot3, gtsam::Pose3, gtsam::Similarity2, gtsam::Similarity3, gtsam::Cal3_S2, gtsam::Cal3DS2,
548552
gtsam::Cal3f, gtsam::Cal3Bundler,
549553
gtsam::EssentialMatrix, gtsam::FundamentalMatrix, gtsam::SimpleFundamentalMatrix,
550554
gtsam::PinholeCamera<gtsam::Cal3_S2>,
@@ -609,6 +613,8 @@ template <T = {double,
609613
gtsam::Rot3,
610614
gtsam::Pose2,
611615
gtsam::Pose3,
616+
gtsam::Similarity2,
617+
gtsam::Similarity3,
612618
gtsam::Unit3,
613619
gtsam::Cal3_S2,
614620
gtsam::Cal3DS2,
@@ -633,7 +639,7 @@ virtual class PriorFactor : gtsam::NoiseModelFactor {
633639
#include <gtsam/nonlinear/NonlinearEquality.h>
634640
template <T = {gtsam::Point2, gtsam::StereoPoint2, gtsam::Point3, gtsam::Rot2,
635641
gtsam::SO3, gtsam::SO4, gtsam::SOn, gtsam::Rot3, gtsam::Pose2,
636-
gtsam::Pose3, gtsam::Cal3_S2, gtsam::CalibratedCamera,
642+
gtsam::Pose3, gtsam::Similarity2, gtsam::Similarity3, gtsam::Cal3_S2, gtsam::CalibratedCamera,
637643
gtsam::PinholeCamera<gtsam::Cal3_S2>,
638644
gtsam::PinholeCamera<gtsam::Cal3Bundler>,
639645
gtsam::PinholeCamera<gtsam::Cal3Fisheye>,
@@ -651,7 +657,7 @@ virtual class NonlinearEquality : gtsam::NoiseModelFactor {
651657

652658
template <T = {gtsam::Point2, gtsam::StereoPoint2, gtsam::Point3, gtsam::Rot2,
653659
gtsam::SO3, gtsam::SO4, gtsam::SOn, gtsam::Rot3, gtsam::Pose2,
654-
gtsam::Pose3, gtsam::Cal3_S2, gtsam::CalibratedCamera,
660+
gtsam::Pose3, gtsam::Similarity2, gtsam::Similarity3, gtsam::Cal3_S2, gtsam::CalibratedCamera,
655661
gtsam::PinholeCamera<gtsam::Cal3_S2>,
656662
gtsam::PinholeCamera<gtsam::Cal3Bundler>,
657663
gtsam::PinholeCamera<gtsam::Cal3Fisheye>,
@@ -720,7 +726,7 @@ virtual class BatchFixedLagSmoother : gtsam::FixedLagSmoother {
720726
gtsam::NonlinearFactorGraph getFactors() const;
721727

722728
template <VALUE = {gtsam::Point2, gtsam::Rot2, gtsam::Pose2, gtsam::Point3,
723-
gtsam::Rot3, gtsam::Pose3, gtsam::Cal3_S2, gtsam::Cal3DS2,
729+
gtsam::Rot3, gtsam::Pose3, gtsam::Similarity2, gtsam::Similarity3, gtsam::Cal3_S2, gtsam::Cal3DS2,
724730
gtsam::Vector, gtsam::Matrix}>
725731
VALUE calculateEstimate(size_t key) const;
726732
};
@@ -732,6 +738,8 @@ template <T = {gtsam::Point2,
732738
gtsam::Rot3,
733739
gtsam::Pose2,
734740
gtsam::Pose3,
741+
gtsam::Similarity2,
742+
gtsam::Similarity3,
735743
gtsam::NavState,
736744
gtsam::imuBias::ConstantBias}>
737745
virtual class ExtendedKalmanFilter {

‎gtsam/nonlinear/values.i‎

Lines changed: 10 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -17,6 +17,8 @@ namespace gtsam {
1717
#include <gtsam/geometry/Point3.h>
1818
#include <gtsam/geometry/Pose2.h>
1919
#include <gtsam/geometry/Pose3.h>
20+
#include <gtsam/geometry/Similarity2.h>
21+
#include <gtsam/geometry/Similarity3.h>
2022
#include <gtsam/geometry/Rot2.h>
2123
#include <gtsam/geometry/Rot3.h>
2224
#include <gtsam/geometry/SO3.h>
@@ -80,6 +82,8 @@ class Values {
8082
void insert(size_t j, const gtsam::SOn& P);
8183
void insert(size_t j, const gtsam::Rot3& rot3);
8284
void insert(size_t j, const gtsam::Pose3& pose3);
85+
void insert(size_t j, const gtsam::Similarity2& similarity2);
86+
void insert(size_t j, const gtsam::Similarity3& similarity3);
8387
void insert(size_t j, const gtsam::Unit3& unit3);
8488
void insert(size_t j, const gtsam::Cal3Bundler& cal3bundler);
8589
void insert(size_t j, const gtsam::Cal3f& cal3f);
@@ -122,6 +126,8 @@ class Values {
122126
void update(size_t j, const gtsam::SOn& P);
123127
void update(size_t j, const gtsam::Rot3& rot3);
124128
void update(size_t j, const gtsam::Pose3& pose3);
129+
void update(size_t j, const gtsam::Similarity2& similarity2);
130+
void update(size_t j, const gtsam::Similarity3& similarity3);
125131
void update(size_t j, const gtsam::Unit3& unit3);
126132
void update(size_t j, const gtsam::Cal3Bundler& cal3bundler);
127133
void update(size_t j, const gtsam::Cal3f& cal3f);
@@ -161,6 +167,8 @@ class Values {
161167
void insert_or_assign(size_t j, const gtsam::SOn& P);
162168
void insert_or_assign(size_t j, const gtsam::Rot3& rot3);
163169
void insert_or_assign(size_t j, const gtsam::Pose3& pose3);
170+
void insert_or_assign(size_t j, const gtsam::Similarity2& similarity2);
171+
void insert_or_assign(size_t j, const gtsam::Similarity3& similarity3);
164172
void insert_or_assign(size_t j, const gtsam::Unit3& unit3);
165173
void insert_or_assign(size_t j, const gtsam::Cal3Bundler& cal3bundler);
166174
void insert_or_assign(size_t j, const gtsam::Cal3f& cal3f);
@@ -196,6 +204,8 @@ class Values {
196204
gtsam::SOn,
197205
gtsam::Rot3,
198206
gtsam::Pose3,
207+
gtsam::Similarity2,
208+
gtsam::Similarity3,
199209
gtsam::Unit3,
200210
gtsam::Cal3Bundler,
201211
gtsam::Cal3f,

‎gtsam/slam/slam.i‎

Lines changed: 2 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -7,13 +7,14 @@ namespace gtsam {
77
#include <gtsam/geometry/Cal3DS2.h>
88
#include <gtsam/geometry/SO4.h>
99
#include <gtsam/navigation/ImuBias.h>
10+
#include <gtsam/geometry/Similarity2.h>
1011
#include <gtsam/geometry/Similarity3.h>
1112

1213
// ######
1314

1415
#include <gtsam/slam/BetweenFactor.h>
1516
template <T = {double, gtsam::Vector, gtsam::Point2, gtsam::Point3, gtsam::Rot2, gtsam::SO3,
16-
gtsam::SO4, gtsam::Rot3, gtsam::Pose2, gtsam::Pose3, gtsam::Similarity3,
17+
gtsam::SO4, gtsam::Rot3, gtsam::Pose2, gtsam::Pose3, gtsam::Similarity2, gtsam::Similarity3,
1718
gtsam::imuBias::ConstantBias}>
1819
virtual class BetweenFactor : gtsam::NoiseModelFactor {
1920
BetweenFactor(size_t key1, size_t key2, const T& relativePose,

‎python/gtsam/tests/test_Sim3.py‎

Lines changed: 60 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -16,7 +16,7 @@
1616
from gtsam.utils.test_case import GtsamTestCase
1717

1818
import gtsam
19-
from gtsam import Point3, Pose3, Rot3, Similarity3
19+
from gtsam import Point3, Pose3, Rot3, Similarity3, BetweenFactorSimilarity3, NonlinearFactorGraph, Values, LevenbergMarquardtOptimizer, LevenbergMarquardtParams
2020

2121

2222
class TestSim3(GtsamTestCase):
@@ -130,6 +130,65 @@ def test_align_poses_scaled_squares(self):
130130
for aTi, bTi in zip(aTi_list, bTi_list):
131131
self.gtsamAssertEquals(aTi, aSb.transformFrom(bTi))
132132

133+
def test_sim3_optimization(self)->None:
134+
# Create a PriorFactor with a Sim3 prior
135+
prior = Similarity3(Rot3.Ypr(0.1, 0.2, 0.3), Point3(1, 2, 3), 4)
136+
model = gtsam.noiseModel.Isotropic.Sigma(7, 1)
137+
138+
# Create graph
139+
graph = NonlinearFactorGraph()
140+
graph.addPriorSimilarity3(1, prior, model)
141+
142+
# Create initial estimate with Identity transform
143+
initial = Values()
144+
initial.insert(1, Similarity3())
145+
146+
# Optimize
147+
params = LevenbergMarquardtParams()
148+
params.setVerbosityLM("TRYCONFIG")
149+
result = LevenbergMarquardtOptimizer(graph, initial).optimize()
150+
151+
# After optimization, result should be prior
152+
self.gtsamAssertEquals(prior, result.atSimilarity3(1), 1e-4)
153+
154+
def test_sim3_optimization2(self) -> None:
155+
prior = Similarity3()
156+
m1 = Similarity3(Rot3.Ypr(np.pi / 4.0, 0, 0), Point3(2.0, 0, 0), 1.0)
157+
m2 = Similarity3(Rot3.Ypr(np.pi / 2.0, 0, 0), Point3(np.sqrt(8) * 0.9, 0, 0), 1.0)
158+
m3 = Similarity3(Rot3.Ypr(3 * np.pi / 4.0, 0, 0), Point3(np.sqrt(32) * 0.8, 0, 0), 1.0)
159+
m4 = Similarity3(Rot3.Ypr(np.pi / 2.0, 0, 0), Point3(6 * 0.7, 0, 0), 1.0)
160+
loop = Similarity3(1.42)
161+
162+
priorNoise = gtsam.noiseModel.Isotropic.Sigma(7, 0.01)
163+
betweenNoise = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 10]))
164+
betweenNoise2 = gtsam.noiseModel.Diagonal.Sigmas(np.array([ 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 1.0]))
165+
b1 = BetweenFactorSimilarity3(1, 2, m1, betweenNoise)
166+
b2 = BetweenFactorSimilarity3(2, 3, m2, betweenNoise)
167+
b3 = BetweenFactorSimilarity3(3, 4, m3, betweenNoise)
168+
b4 = BetweenFactorSimilarity3(4, 5, m4, betweenNoise)
169+
lc = BetweenFactorSimilarity3(5, 1, loop, betweenNoise2)
170+
171+
# Create graph
172+
graph = NonlinearFactorGraph()
173+
graph.addPriorSimilarity3(1, prior, priorNoise)
174+
graph.add(b1)
175+
graph.add(b2)
176+
graph.add(b3)
177+
graph.add(b4)
178+
graph.add(lc)
179+
180+
# graph.print("Full Graph\n");
181+
initial=Values()
182+
initial.insert(1, prior)
183+
initial.insert(2, Similarity3(Rot3.Ypr(np.pi / 2.0, 0, 0), Point3(1, 0, 0), 1.1))
184+
initial.insert(3, Similarity3(Rot3.Ypr(2.0 * np.pi / 2.0, 0, 0), Point3(0.9, 1.1, 0), 1.2))
185+
initial.insert(4, Similarity3(Rot3.Ypr(3.0 * np.pi / 2.0, 0, 0), Point3(0, 1, 0), 1.3))
186+
initial.insert(5, Similarity3(Rot3.Ypr(4.0 * np.pi / 2.0, 0, 0), Point3(0, 0, 0), 1.0))
187+
188+
189+
result = LevenbergMarquardtOptimizer(graph, initial).optimizeSafely()
190+
self.gtsamAssertEquals(Similarity3(0.7), result.atSimilarity3(5), 0.4)
191+
133192
def test_align_via_Sim3_to_poses_skydio32(self) -> None:
134193
"""Ensure scale estimate of Sim(3) object is non-negative.
135194

0 commit comments

Comments
 (0)