From 23b96d468063ae07c9dc6b8fff7bd988e0bd470d Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 10 Mar 2025 15:49:35 -0400 Subject: [PATCH 1/3] method to get Expmap coefficients --- gtsam/geometry/Similarity2.cpp | 27 +++++++++++++++++++++++++++ gtsam/geometry/Similarity2.h | 3 +++ 2 files changed, 30 insertions(+) diff --git a/gtsam/geometry/Similarity2.cpp b/gtsam/geometry/Similarity2.cpp index a48a6b6ef7..ec52f01c08 100644 --- a/gtsam/geometry/Similarity2.cpp +++ b/gtsam/geometry/Similarity2.cpp @@ -194,6 +194,33 @@ Similarity2 Similarity2::Align(const Pose2Pairs& abPosePairs) { return internal::AlignGivenR(abPointPairs, aRb_estimate); } +Matrix2 Similarity2::GetV(double theta, double lambda) { + // Derivation from https://ethaneade.com/lie_groups.pdf page 6 + const double lambda2 = lambda * lambda, theta2 = theta * theta; + double A, B, C; + if (theta2 > 1e-9) { + A = sin(theta) / theta; + B = (1 - cos(theta)) / theta2; + C = (1 - A) / theta2; + } else { + // Taylor series expansion for theta=0 + A = 1.0; + B = 0.5 - theta2 / 24.0; + C = 1.0 / 6.0 - theta2 / 120.0; + } + double alpha = 1.0 / (1.0 + theta2 / lambda2); + const double s = exp(lambda); + + double s_inv = 1.0 / s; + double X = alpha * (1 - s_inv) / lambda + (1 - alpha) * (A - lambda * B); + double Y = + alpha * (s_inv - 1 + lambda) / lambda2 + (1 - alpha) * (B - lambda * C); + + Matrix2 V; + V << X, -theta * Y, theta * Y, X; + return V; +} + Vector4 Similarity2::Logmap(const Similarity2& S, // OptionalJacobian<4, 4> Hm) { const Vector2 u = S.t_; diff --git a/gtsam/geometry/Similarity2.h b/gtsam/geometry/Similarity2.h index cc4ffc792e..39aa4363a5 100644 --- a/gtsam/geometry/Similarity2.h +++ b/gtsam/geometry/Similarity2.h @@ -141,6 +141,9 @@ class GTSAM_EXPORT Similarity2 : public LieGroup { using LieAlgebra = Matrix3; + /// Calculate expmap and logmap coefficients. + static Matrix2 GetV(double theta, double lambda); + /** * Log map at the identity * \f$ [t_x, t_y, \delta, \lambda] \f$ From 5ccf9dbd14b8c4e7e60b2c1ad75c0be30afb95fc Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 10 Mar 2025 15:50:09 -0400 Subject: [PATCH 2/3] fix Sim2 Expmap and Logmap --- gtsam/geometry/Similarity2.cpp | 15 ++++++++------- 1 file changed, 8 insertions(+), 7 deletions(-) diff --git a/gtsam/geometry/Similarity2.cpp b/gtsam/geometry/Similarity2.cpp index ec52f01c08..15bb232100 100644 --- a/gtsam/geometry/Similarity2.cpp +++ b/gtsam/geometry/Similarity2.cpp @@ -223,11 +223,11 @@ Matrix2 Similarity2::GetV(double theta, double lambda) { Vector4 Similarity2::Logmap(const Similarity2& S, // OptionalJacobian<4, 4> Hm) { - const Vector2 u = S.t_; const Vector1 w = Rot2::Logmap(S.R_); - const double s = log(S.s_); + const double lambda = log(S.s_); + // In Expmap, t = V * u -> in Logmap, u = V^{-1} * t Vector4 result; - result << u, w, s; + result << GetV(w[0], lambda).inverse() * S.t_, w, lambda; if (Hm) { throw std::runtime_error("Similarity2::Logmap: derivative not implemented"); } @@ -236,13 +236,14 @@ Vector4 Similarity2::Logmap(const Similarity2& S, // Similarity2 Similarity2::Expmap(const Vector4& v, // OptionalJacobian<4, 4> Hm) { - const Vector2 t = v.head<2>(); - const Rot2 R = Rot2::Expmap(v.segment<1>(2)); - const double s = v[3]; + const Vector2 u = v.head<2>(); + const double theta = v[2]; + const double lambda = v[3]; if (Hm) { throw std::runtime_error("Similarity2::Expmap: derivative not implemented"); } - return Similarity2(R, t, s); + const Matrix2 V = GetV(theta, lambda); + return Similarity2(Rot2::Expmap(v.segment<1>(2)), V * u, exp(lambda)); } Matrix4 Similarity2::AdjointMap() const { From 1bb3ec6720dd3178730eadc5e568bc15587fa47d Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 10 Mar 2025 15:50:17 -0400 Subject: [PATCH 3/3] enable test --- gtsam/geometry/tests/testSimilarity2.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/geometry/tests/testSimilarity2.cpp b/gtsam/geometry/tests/testSimilarity2.cpp index 78b25b50f5..3942104e8c 100644 --- a/gtsam/geometry/tests/testSimilarity2.cpp +++ b/gtsam/geometry/tests/testSimilarity2.cpp @@ -79,7 +79,7 @@ TEST(Similarity2, HatAndVee) { /* ************************************************************************* */ // Checks correct exponential map (Expmap) with brute force matrix exponential -TEST_DISABLED(Similarity2, BruteForceExpmap) { +TEST(Similarity2, BruteForceExpmap) { const Vector4 xi(0.1, 0.2, 0.3, 0.4); EXPECT(assert_equal(Similarity2::Expmap(xi), expm(xi), 1e-4)); }