Review

Author: dellaert

gtsam/geometry/Similarity2.cpp

@@ -215,7 +215,7 @@ Matrix2 Similarity2::GetV(double theta, double lambda) {
     return V;
   }
 
-  // general Sim(2) case ---------------------------------------------
+  // general Sim(2) case
   const double d2 = lambda2 + theta2;
   if (d2 < 1e-15)               // both tiny → identity
     return Matrix2::Identity();

gtsam/geometry/Similarity3.cpp

@@ -296,29 +296,29 @@ Matrix4 Similarity3::matrix() const {
 }
 
 namespace {
-  // The 16x7 matrix of vectorized generators for Sim(3).
+// 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();
+    (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 {

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;
@@ -205,13 +209,6 @@ TEST(Similarity2, vec) {
   EXPECT(assert_equal(H_numerical, H_actual, 1e-7));
 }
 
-// In testSimilarity2.cpp
-
-#include <gtsam/base/Vector.h> // For Vector4
-#include <vector> // For std::vector
-
-// ... other tests ...
-
 //******************************************************************************
 TEST(Similarity2, AdjointMap) {
   // Create a non-trivial Similarity2 object
@@ -227,7 +224,6 @@ TEST(Similarity2, 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;

gtsam/geometry/tests/testSimilarity3.cpp

@@ -116,7 +116,6 @@ TEST(Similarity3, 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) {

gtsam/slam/tests/testFrobeniusFactor.cpp

@@ -274,7 +274,7 @@ namespace sim2 {
 /* ************************************************************************* */
 TEST(FrobeniusFactorSimilarity2, evaluateError) {
   using namespace ::sim2;
-  auto factor = FrobeniusFactor<Similarity2>(1, 2, noiseModel::Unit::Create(12));
+  auto factor = FrobeniusFactor<Similarity2>(1, 2, noiseModel::Unit::Create(9));
   Vector actual = factor.evaluateError(P1, P2);
   Vector expected = P2.vec() - P1.vec();
   EXPECT(assert_equal(expected, actual, 1e-9));