Address review comments

gtsam/geometry/FundamentalMatrix.cpp

@@ -97,20 +97,20 @@ FundamentalMatrix FundamentalMatrix::retract(const Vector& delta) const {
 }
 
 //*************************************************************************
-Matrix3 SimpleFundamentalMatrix::leftK() const {
+Matrix3 SimpleFundamentalMatrix::Ka() const {
   Matrix3 K;
   K << fa_, 0, ca_.x(), 0, fa_, ca_.y(), 0, 0, 1;
   return K;
 }
 
-Matrix3 SimpleFundamentalMatrix::rightK() const {
+Matrix3 SimpleFundamentalMatrix::Kb() const {
   Matrix3 K;
   K << fb_, 0, cb_.x(), 0, fb_, cb_.y(), 0, 0, 1;
   return K;
 }
 
 Matrix3 SimpleFundamentalMatrix::matrix() const {
-  return leftK().transpose().inverse() * E_.matrix() * rightK().inverse();
+  return Ka().transpose().inverse() * E_.matrix() * Kb().inverse();
 }
 
 void SimpleFundamentalMatrix::print(const std::string& s) const {

gtsam/geometry/FundamentalMatrix.h

@@ -68,8 +68,8 @@ class GTSAM_EXPORT FundamentalMatrix {
   template <typename CAL>
   FundamentalMatrix(const CAL& Ka, const Pose3& aPb, const CAL& Kb)
       : FundamentalMatrix(Ka.K().transpose().inverse() *
-                                 EssentialMatrix::FromPose3(aPb).matrix() *
-                                 Kb.K().inverse()) {}
+                          EssentialMatrix::FromPose3(aPb).matrix() *
+                          Kb.K().inverse()) {}
 
   /// Return the fundamental matrix representation
   Matrix3 matrix() const;
@@ -114,25 +114,32 @@ class GTSAM_EXPORT SimpleFundamentalMatrix {
   Point2 ca_;          ///< Principal point for left camera
   Point2 cb_;          ///< Principal point for right camera
 
+  /// Return the left calibration matrix
+  Matrix3 Ka() const;
+
+  /// Return the right calibration matrix
+  Matrix3 Kb() const;
+
  public:
   /// Default constructor
   SimpleFundamentalMatrix()
       : E_(), fa_(1.0), fb_(1.0), ca_(0.0, 0.0), cb_(0.0, 0.0) {}
 
-  /// Construct from essential matrix and focal lengths
+  /**
+   * @brief Construct from essential matrix and focal lengths
+   * @param E Essential matrix
+   * @param fa Focal length for left camera
+   * @param fb Focal length for right camera
+   * @param ca Principal point for left camera
+   * @param cb Principal point for right camera
+   */
   SimpleFundamentalMatrix(const EssentialMatrix& E,  //
-                          double fa, double fb,
-                          const Point2& ca = Point2(0.0, 0.0),
-                          const Point2& cb = Point2(0.0, 0.0))
+                          double fa, double fb, const Point2& ca,
+                          const Point2& cb)
       : E_(E), fa_(fa), fb_(fb), ca_(ca), cb_(cb) {}
 
-  /// Return the left calibration matrix
-  Matrix3 leftK() const;
-
-  /// Return the right calibration matrix
-  Matrix3 rightK() const;
-
   /// Return the fundamental matrix representation
+  /// F = Ka^(-T) * E * Kb^(-1)
   Matrix3 matrix() const;
 
   /// @name Testable

gtsam/sfm/TransferFactor.h

@@ -24,8 +24,8 @@ namespace gtsam {
 
 /**
  * Binary factor in the context of Structure from Motion (SfM).
- * It is used to transfer points between different views based on the
- * fundamental matrices between these views. The factor computes the error
+ * It is used to transfer transfer corresponding points from two views to a
+ * third based on two fundamental matrices. The factor computes the error
  * between the transferred points `pa` and `pb`, and the actual point `pc` in
  * the target view. Jacobians are done using numerical differentiation.
  */

gtsam/sfm/tests/testTransferFactor.cpp

@@ -12,9 +12,6 @@
 
 using namespace gtsam;
 
-double focalLength = 1000;
-Point2 principalPoint(640 / 2, 480 / 2);
-
 //*************************************************************************
 /// Generate three cameras on a circle, looking in
 std::array<Pose3, 3> generateCameraPoses() {
@@ -44,17 +41,36 @@ TripleF<SimpleFundamentalMatrix> generateTripleF(
   return {F[0], F[1], F[2]};  // Return a TripleF instance
 }
 
+//*************************************************************************
+namespace fixture {
+// Generate cameras on a circle
+std::array<Pose3, 3> cameraPoses = generateCameraPoses();
+auto triplet = generateTripleF(cameraPoses);
+double focalLength = 1000;
+Point2 principalPoint(640 / 2, 480 / 2);
+const Cal3_S2 K(focalLength, focalLength, 0.0,  //
+                principalPoint.x(), principalPoint.y());
+}  // namespace fixture
+
+//*************************************************************************
+// Test for getMatrices
+TEST(TransferFactor, GetMatrices) {
+  using namespace fixture;
+  TransferFactor<SimpleFundamentalMatrix> factor{{2, 0}, {1, 2}, {}};
+
+  // Check that getMatrices is correct
+  auto [Fki, Fkj] = factor.getMatrices(triplet.Fca, triplet.Fbc);
+  EXPECT(assert_equal<Matrix3>(triplet.Fca.matrix(), Fki));
+  EXPECT(assert_equal<Matrix3>(triplet.Fbc.matrix().transpose(), Fkj));
+}
+
 //*************************************************************************
 // Test for TransferFactor
 TEST(TransferFactor, Jacobians) {
-  // Generate cameras on a circle
-  std::array<Pose3, 3> cameraPoses = generateCameraPoses();
-  auto triplet = generateTripleF(cameraPoses);
+  using namespace fixture;
 
   // Now project a point into the three cameras
   const Point3 P(0.1, 0.2, 0.3);
-  const Cal3_S2 K(focalLength, focalLength, 0.0,  //
-                  principalPoint.x(), principalPoint.y());
 
   std::array<Point2, 3> p;
   for (size_t i = 0; i < 3; ++i) {
@@ -70,11 +86,6 @@ TEST(TransferFactor, Jacobians) {
       factor1{key12, key01, p[2], p[0], p[1]},
       factor2{key20, key12, p[0], p[1], p[2]};
 
-  // Check that getMatrices is correct
-  auto [Fki, Fkj] = factor2.getMatrices(triplet.Fca, triplet.Fbc);
-  EXPECT(assert_equal<Matrix3>(triplet.Fca.matrix(), Fki));
-  EXPECT(assert_equal<Matrix3>(triplet.Fbc.matrix().transpose(), Fkj));
-
   // Create Values with edge keys
   Values values;
   values.insert(key01, triplet.Fab);
@@ -92,18 +103,13 @@ TEST(TransferFactor, Jacobians) {
 //*************************************************************************
 // Test for TransferFactor with multiple tuples
 TEST(TransferFactor, MultipleTuples) {
-  // Generate cameras on a circle
-  std::array<Pose3, 3> cameraPoses = generateCameraPoses();
-  auto triplet = generateTripleF(cameraPoses);
+  using namespace fixture;
 
   // Now project multiple points into the three cameras
   const size_t numPoints = 5;  // Number of points to test
   std::vector<Point3> points3D;
   std::vector<std::array<Point2, 3>> projectedPoints;
 
-  const Cal3_S2 K(focalLength, focalLength, 0.0,  //
-                  principalPoint.x(), principalPoint.y());
-
   // Generate random 3D points and project them
   for (size_t n = 0; n < numPoints; ++n) {
     Point3 P(0.1 * n, 0.2 * n, 0.3 + 0.1 * n);
@@ -120,15 +126,15 @@ TEST(TransferFactor, MultipleTuples) {
 
   // Create a vector of tuples for the TransferFactor
   EdgeKey key01(0, 1), key12(1, 2), key20(2, 0);
-  std::vector<std::tuple<Point2, Point2, Point2>> tuples0, tuples1, tuples2;
+  std::vector<std::tuple<Point2, Point2, Point2>> tuples;
 
   for (size_t n = 0; n < numPoints; ++n) {
     const auto& p = projectedPoints[n];
-    tuples0.emplace_back(p[1], p[2], p[0]);
+    tuples.emplace_back(p[1], p[2], p[0]);
   }
 
   // Create TransferFactors using the new constructor
-  TransferFactor<SimpleFundamentalMatrix> factor{key01, key20, tuples0};
+  TransferFactor<SimpleFundamentalMatrix> factor{key01, key20, tuples};
 
   // Create Values with edge keys
   Values values;