Conversion

gtsam/geometry/FundamentalMatrix.h

@@ -62,9 +62,58 @@ class GeneralFundamentalMatrix : public FundamentalMatrix {
   GeneralFundamentalMatrix(const Rot3& U, double s, const Rot3& V)
       : U_(U), s_(s), V_(V) {}
 
+  /**
+   * @brief Construct from a 3x3 matrix using SVD
+   *
+   * Initializes the GeneralFundamentalMatrix by performing SVD on the given
+   * matrix and ensuring U and V are not reflections.
+   *
+   * @param F A 3x3 matrix representing the fundamental matrix
+   */
+  GeneralFundamentalMatrix(const Matrix3& F) {
+    // Perform SVD
+    Eigen::JacobiSVD<Matrix3> svd(F, Eigen::ComputeFullU | Eigen::ComputeFullV);
+
+    // Extract U and V
+    Matrix3 U = svd.matrixU();
+    Matrix3 V = svd.matrixV();
+    Vector3 singularValues = svd.singularValues();
+
+    // Scale the singular values
+    double scale = singularValues(0);
+    if (scale != 0) {
+      singularValues /= scale;  // Normalize the first singular value to 1.0
+    }
+
+    // Check if the third singular value is close to zero (valid F condition)
+    if (std::abs(singularValues(2)) > 1e-9) {
+      throw std::invalid_argument(
+          "The input matrix does not represent a valid fundamental matrix.");
+    }
+
+    // Ensure the second singular value is recorded as s
+    s_ = singularValues(1);
+
+    // Check if U is a reflection
+    if (U.determinant() < 0) {
+      U = -U;
+      s_ = -s_;  // Change sign of scalar if U is a reflection
+    }
+
+    // Check if V is a reflection
+    if (V.determinant() < 0) {
+      V = -V;
+      s_ = -s_;  // Change sign of scalar if U is a reflection
+    }
+
+    // Assign the rotations
+    U_ = Rot3(U);
+    V_ = Rot3(V);
+  }
+
   /// Return the fundamental matrix representation
   Matrix3 matrix() const override {
-    return U_.matrix() * Vector3(1, s_, 1).asDiagonal() *
+    return U_.matrix() * Vector3(1, s_, 0).asDiagonal() *
            V_.transpose().matrix();
   }
 
@@ -141,14 +190,24 @@ class SimpleFundamentalMatrix : FundamentalMatrix {
                           const Point2& cb = Point2(0.0, 0.0))
       : E_(E), fa_(fa), fb_(fb), ca_(ca), cb_(cb) {}
 
-  /// Return the fundamental matrix representation
+  /// Return the left calibration matrix
-  Matrix3 matrix() const override {
+  Matrix3 leftK() const {
-    Matrix3 Ka, Kb;
+    Matrix3 K;
-    Ka << fa_, 0, ca_.x(), 0, fa_, ca_.y(), 0, 0, 1;  // Left calibration
+    K << fa_, 0, ca_.x(), 0, fa_, ca_.y(), 0, 0, 1;
-    Kb << fb_, 0, cb_.x(), 0, fb_, cb_.y(), 0, 0, 1;  // Right calibration
+    return K;
-    return Ka * E_.matrix() * Kb.inverse();
   }
 
+  /// Return the right calibration matrix
+  Matrix3 rightK() const {
+    Matrix3 K;
+    K << fb_, 0, cb_.x(), 0, fb_, cb_.y(), 0, 0, 1;
+    return K;
+  }
+
+  /// Return the fundamental matrix representation
+  Matrix3 matrix() const override {
+    return leftK().transpose().inverse() * E_.matrix() * rightK().inverse();
+  }
   /// @name Testable
   /// @{
 

gtsam/geometry/tests/testFundamentalMatrix.cpp

@@ -49,37 +49,122 @@ TEST(GeneralFundamentalMatrix, RoundTrip) {
 }
 
 //*************************************************************************
-// Create essential matrix and focal lengths for
+// Create the simplest SimpleFundamentalMatrix, a stereo pair
-// SimpleFundamentalMatrix
+EssentialMatrix defaultE(Rot3(), Unit3(1, 0, 0));
-EssentialMatrix trueE;  // Assuming a default constructor is available
+Point2 zero(0.0, 0.0);
-double trueFa = 1.0;
+SimpleFundamentalMatrix stereoF(defaultE, 1.0, 1.0, zero, zero);
-double trueFb = 1.0;
-Point2 trueCa(0.0, 0.0);
-Point2 trueCb(0.0, 0.0);
-SimpleFundamentalMatrix trueSimpleF(trueE, trueFa, trueFb, trueCa, trueCb);
 
 //*************************************************************************
-TEST(SimpleFundamentalMatrix, localCoordinates) {
+TEST(SimpleStereo, Conversion) {
+  GeneralFundamentalMatrix convertedF(stereoF.matrix());
+  EXPECT(assert_equal(stereoF.matrix(), convertedF.matrix(), 1e-8));
+}
+
+//*************************************************************************
+TEST(SimpleStereo, localCoordinates) {
   Vector expected = Z_7x1;
-  Vector actual = trueSimpleF.localCoordinates(trueSimpleF);
+  Vector actual = stereoF.localCoordinates(stereoF);
   EXPECT(assert_equal(expected, actual, 1e-8));
 }
 
 //*************************************************************************
-TEST(SimpleFundamentalMatrix, retract) {
+TEST(SimpleStereo, retract) {
-  SimpleFundamentalMatrix actual = trueSimpleF.retract(Z_9x1);
+  SimpleFundamentalMatrix actual = stereoF.retract(Z_9x1);
-  EXPECT(assert_equal(trueSimpleF, actual));
+  EXPECT(assert_equal(stereoF, actual));
 }
 
 //*************************************************************************
-TEST(SimpleFundamentalMatrix, RoundTrip) {
+TEST(SimpleStereo, RoundTrip) {
   Vector7 d;
   d << 0.1, 0.2, 0.3, 0.4, 0.5, 0.6, 0.7;
-  SimpleFundamentalMatrix hx = trueSimpleF.retract(d);
+  SimpleFundamentalMatrix hx = stereoF.retract(d);
-  Vector actual = trueSimpleF.localCoordinates(hx);
+  Vector actual = stereoF.localCoordinates(hx);
   EXPECT(assert_equal(d, actual, 1e-8));
 }
 
+//*************************************************************************
+TEST(SimpleStereo, EpipolarLine) {
+  // Create a point in b
+  Point3 p_b(0, 2, 1);
+  // Convert the point to a horizontal line in a
+  Vector3 l_a = stereoF.matrix() * p_b;
+  // Check if the line is horizontal at height 2
+  EXPECT(assert_equal(Vector3(0, -1, 2), l_a));
+}
+
+//*************************************************************************
+// 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);
+
+//*************************************************************************
+TEST(PixelStereo, Conversion) {
+  auto expected = pixelStereo.matrix();
+
+  GeneralFundamentalMatrix convertedF(pixelStereo.matrix());
+
+  // Check equality of F-matrices up to a scale
+  auto actual = convertedF.matrix();
+  actual *= expected(1, 2) / actual(1, 2);
+  EXPECT(assert_equal(expected, actual, 1e-5));
+}
+
+//*************************************************************************
+TEST(PixelStereo, PointInBToHorizontalLineInA) {
+  // Create a point in b
+  Point3 p_b = Point3(0, 300, 1);
+  // Convert the point to a horizontal line in a
+  Vector3 l_a = pixelStereo.matrix() * p_b;
+  // Check if the line is horizontal at height 2
+  EXPECT(assert_equal(Vector3(0, -0.001, 0.3), l_a));
+}
+
+//*************************************************************************
+// 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);
+
+//*************************************************************************
+TEST(RotatedPixelStereo, Conversion) {
+  auto expected = rotatedPixelStereo.matrix();
+
+  GeneralFundamentalMatrix convertedF(rotatedPixelStereo.matrix());
+
+  // Check equality of F-matrices up to a scale
+  auto actual = convertedF.matrix();
+  actual *= expected(1, 2) / actual(1, 2);
+  EXPECT(assert_equal(expected, actual, 1e-4));
+}
+
+//*************************************************************************
+TEST(RotatedPixelStereo, PointInBToHorizontalLineInA) {
+  // Create a point in b
+  Point3 p_b = Point3(300, 0, 1);
+  // Convert the point to a horizontal line in a
+  Vector3 l_a = rotatedPixelStereo.matrix() * p_b;
+  // Check if the line is horizontal at height 2
+  EXPECT(assert_equal(Vector3(0, -0.001, 0.3), l_a));
+}
+
+//*************************************************************************
+// Now check that principal points also survive conversion
+SimpleFundamentalMatrix stereoWithPrincipalPoints(rotatedE, fa, fb, zero, zero);
+
+//*************************************************************************
+TEST(stereoWithPrincipalPoints, Conversion) {
+  auto expected = stereoWithPrincipalPoints.matrix();
+
+  GeneralFundamentalMatrix convertedF(stereoWithPrincipalPoints.matrix());
+
+  // Check equality of F-matrices up to a scale
+  auto actual = convertedF.matrix();
+  actual *= expected(1, 2) / actual(1, 2);
+  EXPECT(assert_equal(expected, actual, 1e-4));
+}
+
 //*************************************************************************
 int main() {
   TestResult tr;