diff --git a/gtsam/geometry/Similarity2.cpp b/gtsam/geometry/Similarity2.cpp index a48a6b6ef..15bb23210 100644 --- a/gtsam/geometry/Similarity2.cpp +++ b/gtsam/geometry/Similarity2.cpp @@ -194,13 +194,40 @@ 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_; 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"); } @@ -209,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 { diff --git a/gtsam/geometry/Similarity2.h b/gtsam/geometry/Similarity2.h index cc4ffc792..39aa4363a 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$ diff --git a/gtsam/geometry/tests/testSimilarity2.cpp b/gtsam/geometry/tests/testSimilarity2.cpp index 78b25b50f..3942104e8 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)); }