Merge pull request borglab#2183 from borglab/feature/Sim2_update

dellaert · web-flow · commit e68362335178 · 2025-07-01T14:49:05.000+01:00
Similarity2 and Similarity3 updates
diff --git a/gtsam/geometry/Similarity2.cpp b/gtsam/geometry/Similarity2.cpp
@@ -197,27 +197,48 @@ Similarity2 Similarity2::Align(const Pose2Pairs& abPosePairs) {
 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;
+
+  // SE(2) or near-SE(2) case  (|λ| tiny)
+  if (std::abs(lambda) < 1e-9) {
+    double A, B;
+    if (theta2 > 1e-9) {
+      A = sin(theta) / theta;
+      B = (1 - cos(theta)) / theta2;
+    }
+    else {                    // θ ≈ 0  →  series
+      A = 1.0 - theta2 / 6.0;
+      B = 0.5 - theta2 / 24.0;
+    }
+    Matrix2 V;
+    V << A, -theta * B,
+      theta* B, A;
+    return V;
+  }
+
+  // general Sim(2) case
+  const double d2 = lambda2 + theta2;
+  if (d2 < 1e-15)               // both tiny → identity
+    return Matrix2::Identity();
+
+  // rotation scalars (unchanged)
   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;
+  } else {  // θ series
+    A = 1.0 - theta2 / 6.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);
+  const double alpha = lambda2 / (lambda2 + theta2);
+  const double s_inv = exp(-lambda);
+  const double X = alpha * (1 - s_inv) / lambda + (1 - alpha) * (A - lambda * B);
+  const double Y = alpha * (s_inv - 1 + lambda) / lambda2 + (1 - alpha) * (B - lambda * C);
 
   Matrix2 V;
-  V << X, -theta * Y, theta * Y, X;
+  V << X, -theta * Y, theta* Y, X;
   return V;
 }
 
@@ -247,7 +268,25 @@ Similarity2 Similarity2::Expmap(const Vector4& v,  //
 }
 
 Matrix4 Similarity2::AdjointMap() const {
-  throw std::runtime_error("Similarity2::AdjointMap not implemented");
+  const Matrix2& R = R_.matrix();
+  const Point2& t = t_;
+  const double& s = s_;
+
+  Matrix4 Adj = Matrix4::Identity(); // Start with Identity
+
+  // Top-left 2x2 block: s * R
+  Adj.block<2, 2>(0, 0) = s * R;
+
+  // Top-right coupling terms, derived from T*Hat(xi)*T_inv
+  // Column w.r.t 'w': maps to [-s*J*t]
+  Adj(0, 2) = s * t.y();
+  Adj(1, 2) = -s * t.x();
+
+  // Column w.r.t 'lambda': maps to [-s*t]
+  Adj(0, 3) = -s * t.x();
+  Adj(1, 3) = -s * t.y();
+
+  return Adj;
 }
 
 Matrix3 Similarity2::Hat(const Vector4 &xi) {
@@ -268,6 +307,7 @@ Vector4 Similarity2::Vee(const Matrix3 &Xi) {
   xi[3] = -Xi(2, 2);
   return xi;
 }
+
 std::ostream& operator<<(std::ostream& os, const Similarity2& p) {
   os << "[" << p.rotation().theta() << " " << p.translation().transpose() << " "
      << p.scale() << "]\';";
@@ -281,4 +321,38 @@ Matrix3 Similarity2::matrix() const {
   return T;
 }
 
+// In Similarity2.cpp, e.g., after the #includes
+
+namespace {
+  // The 9x4 matrix of vectorized generators for Sim(2).
+  // The columns correspond to perturbations in [u_x, u_y, w, lambda].
+  // P_sim2 = [vec(G_ux), vec(G_uy), vec(G_w), vec(G_lambda)]
+  static const Eigen::Matrix<double, 9, 4> P_sim2 =
+    (Eigen::Matrix<double, 9, 4>() <<
+      // u_x u_y w lambda
+      0, 0, 0, 0,  // row 0
+      0, 0, 1, 0,  // row 1
+      0, 0, 0, 0,  // row 2
+      0, 0, -1, 0,  // row 3
+      0, 0, 0, 0,  // row 4
+      0, 0, 0, 0,  // row 5
+      1, 0, 0, 0,  // row 6
+      0, 1, 0, 0,  // row 7
+      0, 0, 0, -1   // row 8
+      ).finished();
+} // namespace
+
+//******************************************************************************
+Vector9 Similarity2::vec(OptionalJacobian<9, 4> H) const {
+  const Matrix3 T = this->matrix();
+  if (H) {
+    // The Jacobian is given by the formula H = (I_3 ⊗ T) * P_sim2
+    // where P_sim2 is the matrix of vectorized generators.
+    // This can be implemented efficiently with block-wise multiplication.
+    *H << T * P_sim2.block<3, 4>(0, 0),
+      T* P_sim2.block<3, 4>(3, 0),
+      T* P_sim2.block<3, 4>(6, 0);
+  }
+  return Eigen::Map<const Vector9>(T.data());
+}
 }  // namespace gtsam
diff --git a/gtsam/geometry/Similarity2.h b/gtsam/geometry/Similarity2.h
@@ -185,6 +185,9 @@ class GTSAM_EXPORT Similarity2 : public LieGroup<Similarity2, 4> {
   /// Calculate 4*4 matrix group equivalent
   Matrix3 matrix() const;
 
+  /// Return vectorized Similarity2 matrix in column order
+  Vector9 vec(OptionalJacobian<9, 4> H = {}) const;
+
   /// Return a GTSAM rotation
   Rot2 rotation() const { return R_; }
 
diff --git a/gtsam/geometry/Similarity3.cpp b/gtsam/geometry/Similarity3.cpp
@@ -295,4 +295,42 @@ Matrix4 Similarity3::matrix() const {
   return T;
 }
 
+namespace {
+// The 16x7 matrix of vectorized generators for Sim(3).
+// The columns correspond to perturbations in [w_x, w_y, w_z, u_x, u_y, u_z, lambda].
+// P_sim3 = [vec(G_wx), vec(G_wy), vec(G_wz), vec(G_ux), vec(G_uy), vec(G_uz), vec(G_lambda)]
+  static const Eigen::Matrix<double, 16, 7> P_sim3 =
+    (Eigen::Matrix<double, 16, 7>() <<
+      // wx,  wy,  wz,  ux,  uy,  uz,  la
+      0, 0, 0, 0, 0, 0, 0,
+      0, 0, 1, 0, 0, 0, 0,
+      0, -1, 0, 0, 0, 0, 0,
+      0, 0, 0, 0, 0, 0, 0,
+      0, 0, -1, 0, 0, 0, 0,
+      0, 0, 0, 0, 0, 0, 0,
+      1, 0, 0, 0, 0, 0, 0,
+      0, 0, 0, 0, 0, 0, 0,
+      0, 1, 0, 0, 0, 0, 0,
+      -1, 0, 0, 0, 0, 0, 0,
+      0, 0, 0, 0, 0, 0, 0,
+      0, 0, 0, 0, 0, 0, 0,
+      0, 0, 0, 1, 0, 0, 0,
+      0, 0, 0, 0, 1, 0, 0,
+      0, 0, 0, 0, 0, 1, 0,
+      0, 0, 0, 0, 0, 0, -1
+      ).finished();
+} // namespace
+
+Similarity3::Vector16 Similarity3::vec(OptionalJacobian<16, 7> H) const {
+  const Matrix4 T = this->matrix();
+  if (H) {
+    // The Jacobian is given by the formula H = (I_4 ⊗ T) * P_sim3
+    // where P_sim3 is the matrix of vectorized generators.
+    *H << T * P_sim3.block<4, 7>(0, 0), T* P_sim3.block<4, 7>(4, 0),
+      T* P_sim3.block<4, 7>(8, 0), T* P_sim3.block<4, 7>(12, 0);
+  }
+  return Eigen::Map<const Vector16>(T.data());
+}
+
+
 } // namespace gtsam
diff --git a/gtsam/geometry/Similarity3.h b/gtsam/geometry/Similarity3.h
@@ -34,12 +34,15 @@ class Pose3;
  * 3D similarity transform
  */
 class GTSAM_EXPORT Similarity3 : public LieGroup<Similarity3, 7> {
+ public:
   /// @name Pose Concept
   /// @{
   typedef Rot3 Rotation;
   typedef Point3 Translation;
   /// @}
 
+  using Vector16 = Eigen::Matrix<double, 16, 1>;
+
  private:
   Rot3 R_;
   Point3 t_;
@@ -64,6 +67,9 @@ class GTSAM_EXPORT Similarity3 : public LieGroup<Similarity3, 7> {
   /// Construct from matrix [R t; 0 s^-1]
   Similarity3(const Matrix4& T);
 
+  /// Vectorize 4x4 matrix into a 16-dim vector.
+  Vector16 vec(OptionalJacobian<16, 7> H = {}) const;
+
   /// @}
   /// @name Testable
   /// @{
diff --git a/gtsam/geometry/tests/testSimilarity2.cpp b/gtsam/geometry/tests/testSimilarity2.cpp
@@ -15,13 +15,17 @@
   * @author Varun Agrawal
   */
 
-#include <CppUnitLite/TestHarness.h>
+#include <gtsam/geometry/Similarity2.h>
+
 #include <gtsam/base/Testable.h>
 #include <gtsam/base/numericalDerivative.h>
 #include <gtsam/base/testLie.h>
-#include <gtsam/geometry/Similarity2.h>
+#include <gtsam/base/Vector.h>
+
+#include <CppUnitLite/TestHarness.h>
 
 #include <functional>
+#include <vector>
 
 using namespace std::placeholders;
 using namespace gtsam;
@@ -178,6 +182,70 @@ TEST(Similarity2, TransformFrom_Pose2) {
   EXPECT(assert_equal(expected, actual, 1e-9));
 }
 
+//******************************************************************************
+TEST(Similarity2, vec) {
+  const double theta = 0.3;
+  const Rot2 R_test = Rot2::fromAngle(theta);
+  const Point2 t_test(0.2, 0.7);
+  const double s_test = 4.0;
+  const Similarity2 sim(R_test, t_test, s_test);
+
+  // 1. Test the Value
+  Vector9 expected_vec;
+  const double c = cos(theta), si = sin(theta);
+  expected_vec << c, si, 0,               // First column
+    -si, c, 0,                            // Second column
+    t_test.x(), t_test.y(), 1.0 / s_test; // Third column
+  Vector9 actual_vec = sim.vec();
+  EXPECT(assert_equal(expected_vec, actual_vec, 1e-9));
+
+  // 2. Test the Jacobian
+  Matrix94 H_actual;
+  sim.vec(H_actual);
+  auto vec_fun = [](const Similarity2& sim_arg) -> Vector9 {
+    return sim_arg.vec();
+    };
+  Matrix H_numerical = numericalDerivative11<Vector9, Similarity2, 4>(vec_fun, sim);
+  EXPECT(assert_equal(H_numerical, H_actual, 1e-7));
+}
+
+//******************************************************************************
+TEST(Similarity2, AdjointMap) {
+  // Create a non-trivial Similarity2 object
+  const Rot2 R = Rot2::fromAngle(-0.7);
+  const Point2 t(1.5, -2.3);
+  const double s = 0.5;
+  const Similarity2 sim(R, t, s);
+
+  // 1. Calculate the Adjoint map using the fast, closed-form function.
+  // This is the "actual" value we want to test.
+  Matrix4 actual_Adj = sim.AdjointMap();
+
+  // 2. Calculate the Adjoint map using the general, "brute force" definition.
+  // This is the "expected" ground truth.
+  // Ad_i = Vee(T * Hat(e_i) * T_inv)
+
+  // Get the 4 generators G_i = Hat(e_i) for sim(2)
+  std::vector<Matrix3> G;
+  for (int i = 0; i < 4; ++i) {
+    G.push_back(Similarity2::Hat(Vector4::Unit(i)));
+  }
+
+  // Calculate T and its inverse
+  const Matrix3 T = sim.matrix();
+  const Matrix3 T_inv = sim.inverse().matrix(); // Or calculate analytically
+
+  // Loop through columns to build the expected Adjoint matrix
+  Matrix4 expected_Adj;
+  for (int i = 0; i < 4; ++i) {
+    Matrix3 T_Gi_Tinv = T * G[i] * T_inv;
+    expected_Adj.col(i) = Similarity2::Vee(T_Gi_Tinv);
+  }
+
+  // 3. Assert that the two matrices are equal.
+  EXPECT(assert_equal(expected_Adj, actual_Adj, 1e-9));
+}
+
 //******************************************************************************
 int main() {
   TestResult tr;
diff --git a/gtsam/geometry/tests/testSimilarity3.cpp b/gtsam/geometry/tests/testSimilarity3.cpp
@@ -111,13 +111,30 @@ TEST(Similarity3, BruteForceExpmap) {
 
 //******************************************************************************
 TEST(Similarity3, AdjointMap) {
+  // 1. Calculate the Adjoint map using the fast, closed-form function.
+  Matrix7 actual = T2.AdjointMap();
+
+  // 2. Calculate the Adjoint map using the general, "brute force" definition.
+  // Ad_i = Vee(T * Hat(e_i) * T_inv)
+  // Get the 7 generators G_i = Hat(e_i) for sim(3)
+  std::vector<Matrix4> G;
+  for (int i = 0; i < 7; ++i) {
+    G.push_back(Similarity3::Hat(Vector7::Unit(i)));
+  }
+
+  // Calculate T and its inverse
   const Matrix4 T = T2.matrix();
-  // Check Ad with actual definition
-  Vector7 delta;
-  delta << 0.1, 0.2, 0.3, 0.4, 0.5, 0.6, 0.7;
-  Matrix4 W = Similarity3::Hat(delta);
-  Matrix4 TW = Similarity3::Hat(T2.AdjointMap() * delta);
-  EXPECT(assert_equal(TW, Matrix4(T * W * T.inverse()), 1e-9));
+  const Matrix4 T_inv = T2.inverse().matrix();
+
+  // Loop through columns to build the expected Adjoint matrix
+  Matrix7 expected;
+  for (int i = 0; i < 7; ++i) {
+    Matrix4 T_Gi_Tinv = T * G[i] * T_inv;
+    expected.col(i) = Similarity3::Vee(T_Gi_Tinv);
+  }
+
+  // 3. Assert that the two matrices are equal.
+  EXPECT(assert_equal(expected, actual));
 }
 
 //******************************************************************************
@@ -552,6 +569,32 @@ TEST(Similarity3 , LieGroupDerivatives) {
   CHECK_LIE_GROUP_DERIVATIVES(T2, T3);
 }
 
+//******************************************************************************
+TEST(Similarity3, vec) {
+  const Rot3 R_test = Rot3::Rodrigues(0.1, 0.2, 0.3);
+  const Point3 t_test(0.4, 0.5, 0.6);
+  const double s_test = 0.7;
+  const Similarity3 sim(R_test, t_test, s_test);
+
+  // 1. Test the Value
+  Similarity3::Vector16 expected_vec;
+  const Matrix3 R = R_test.matrix();
+  expected_vec << R.col(0), 0.0, R.col(1), 0.0, R.col(2), 0.0, t_test.x(),
+    t_test.y(), t_test.z(), 1.0 / s_test;
+  Similarity3::Vector16 actual_vec = sim.vec();
+  EXPECT(assert_equal(expected_vec, actual_vec, 1e-9));
+
+  // 2. Test the Jacobian
+  Matrix H_actual(16, 7);
+  sim.vec(H_actual);
+  auto vec_fun = [](const Similarity3& sim_arg) -> Similarity3::Vector16 {
+    return sim_arg.vec();
+    };
+  Matrix H_numerical = numericalDerivative11<Similarity3::Vector16, Similarity3, 7>(vec_fun, sim);
+  EXPECT(assert_equal(H_numerical, H_actual, 1e-7));
+}
+
+
 //******************************************************************************
 int main() {
   TestResult tr;
diff --git a/gtsam/slam/tests/testFrobeniusFactor.cpp b/gtsam/slam/tests/testFrobeniusFactor.cpp
