Verify triplet epipolar transfer

gtsam/geometry/FundamentalMatrix.h

@@ -213,9 +213,10 @@ class SimpleFundamentalMatrix : FundamentalMatrix {
 
   /// Print the SimpleFundamentalMatrix
   void print(const std::string& s = "") const {
-    std::cout << s << "E:\n"
+    std::cout << s << " E:\n"
               << E_.matrix() << "\nfa: " << fa_ << "\nfb: " << fb_
-              << "\nca: " << ca_ << "\ncb: " << cb_ << std::endl;
+              << "\nca: " << ca_.transpose() << "\ncb: " << cb_.transpose()
+              << std::endl;
   }
 
   /// Check equality within a tolerance

gtsam/geometry/tests/testFundamentalMatrix.cpp

@@ -10,6 +10,7 @@
 #include <gtsam/base/numericalDerivative.h>
 #include <gtsam/geometry/FundamentalMatrix.h>
 #include <gtsam/geometry/Rot3.h>
+#include <gtsam/geometry/SimpleCamera.h>
 #include <gtsam/geometry/Unit3.h>
 
 using namespace std::placeholders;
@@ -95,9 +96,9 @@ TEST(SimpleStereo, EpipolarLine) {
 //*************************************************************************
 // Create a stereo pair, but in pixels not normalized coordinates.
 // We're still using zero principal points here.
-double fa = 1000;
-double fb = 1000;
-SimpleFundamentalMatrix pixelStereo(defaultE, fa, fb, zero, zero);
+double focalLength = 1000;
+SimpleFundamentalMatrix pixelStereo(defaultE, focalLength, focalLength, zero,
+                                    zero);
 
 //*************************************************************************
 TEST(PixelStereo, Conversion) {
@@ -125,7 +126,8 @@ TEST(PixelStereo, PointInBToHorizontalLineInA) {
 // Create a stereo pair with the right camera rotated 90 degrees
 Rot3 aRb = Rot3::Rz(M_PI_2);  // Rotate 90 degrees around the Z-axis
 EssentialMatrix rotatedE(aRb, Unit3(1, 0, 0));
-SimpleFundamentalMatrix rotatedPixelStereo(rotatedE, fa, fb, zero, zero);
+SimpleFundamentalMatrix rotatedPixelStereo(rotatedE, focalLength, focalLength,
+                                           zero, zero);
 
 //*************************************************************************
 TEST(RotatedPixelStereo, Conversion) {
@@ -151,7 +153,10 @@ TEST(RotatedPixelStereo, PointInBToHorizontalLineInA) {
 
 //*************************************************************************
 // Now check that principal points also survive conversion
-SimpleFundamentalMatrix stereoWithPrincipalPoints(rotatedE, fa, fb, zero, zero);
+Point2 principalPoint(640 / 2, 480 / 2);
+SimpleFundamentalMatrix stereoWithPrincipalPoints(rotatedE, focalLength,
+                                                  focalLength, principalPoint,
+                                                  principalPoint);
 
 //*************************************************************************
 TEST(stereoWithPrincipalPoints, Conversion) {
@@ -165,9 +170,71 @@ TEST(stereoWithPrincipalPoints, Conversion) {
   EXPECT(assert_equal(expected, actual, 1e-4));
 }
 
+//*************************************************************************
+std::array<Pose3, 3> generateCameraPoses() {
+  std::array<Pose3, 3> cameraPoses;
+  const double radius = 1.0;
+  for (int i = 0; i < 3; ++i) {
+    double angle = i * 2.0 * M_PI / 3.0;
+    double c = cos(angle), s = sin(angle);
+    Rot3 aRb({-s, c, 0}, {0, 0, -1}, {-c, -s, 0});
+    cameraPoses[i] = {aRb, Point3(radius * c, radius * s, 0)};
+  }
+  return cameraPoses;
+}
+
+std::tuple<SimpleFundamentalMatrix, SimpleFundamentalMatrix,
+           SimpleFundamentalMatrix>
+generateFs(const std::array<Pose3, 3> &cameraPoses) {
+  std::array<SimpleFundamentalMatrix, 3> F;
+  for (size_t i = 0; i < 3; ++i) {
+    size_t j = (i + 1) % 3;
+    const Pose3 iPj = cameraPoses[i].between(cameraPoses[j]);
+    EssentialMatrix E(iPj.rotation(), Unit3(iPj.translation()));
+    F[i] = {E, focalLength, focalLength, principalPoint, principalPoint};
+  }
+  return {F[0], F[1], F[2]};
+}
+
+//*************************************************************************
+TEST(Triplet, Transfer) {
+  // Generate cameras on a circle, as well as fundamental matrices
+  auto cameraPoses = generateCameraPoses();
+  auto [F01, F12, F20] = generateFs(cameraPoses);
+
+  // Check that they are all equal
+  EXPECT(F01.equals(F12, 1e-9));
+  EXPECT(F12.equals(F20, 1e-9));
+  EXPECT(F20.equals(F01, 1e-9));
+
+  // 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) {
+    // Project the point into each camera
+    PinholeCameraCal3_S2 camera(cameraPoses[i], K);
+    p[i] = camera.project(P);
+  }
+
+  // Create lines in camera 0 from projections 1 and 2
+  Vector3 line1 = F01.matrix() * Vector3(p[1].x(), p[1].y(), 1);
+  Vector3 line2 = F20.matrix().transpose() * Vector3(p[2].x(), p[2].y(), 1);
+
+  // Cross the lines to find the intersection point
+  Vector3 intersectionPoint = line1.cross(line2);
+
+  // Normalize the intersection point
+  intersectionPoint /= intersectionPoint(2);
+
+  // Compare the intersection point with the original projection in camera 0
+  EXPECT(assert_equal<Point2>(p[0], intersectionPoint.head<2>(), 1e-9));
+}
 //*************************************************************************
 int main() {
   TestResult tr;
   return TestRegistry::runAllTests(tr);
 }
-/* ************************************************************************* */
+//*************************************************************************