diff --git a/gtsam/geometry/FundamentalMatrix.h b/gtsam/geometry/FundamentalMatrix.h index 8140e3921..1080cd87a 100644 --- a/gtsam/geometry/FundamentalMatrix.h +++ b/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 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 - Matrix3 matrix() const override { - Matrix3 Ka, Kb; - Ka << fa_, 0, ca_.x(), 0, fa_, ca_.y(), 0, 0, 1; // Left calibration - Kb << fb_, 0, cb_.x(), 0, fb_, cb_.y(), 0, 0, 1; // Right calibration - return Ka * E_.matrix() * Kb.inverse(); + /// Return the left calibration matrix + Matrix3 leftK() const { + Matrix3 K; + K << fa_, 0, ca_.x(), 0, fa_, ca_.y(), 0, 0, 1; + return K; } + /// 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 /// @{ diff --git a/gtsam/geometry/tests/testFundamentalMatrix.cpp b/gtsam/geometry/tests/testFundamentalMatrix.cpp index 0c132361c..bab57b148 100644 --- a/gtsam/geometry/tests/testFundamentalMatrix.cpp +++ b/gtsam/geometry/tests/testFundamentalMatrix.cpp @@ -49,37 +49,122 @@ TEST(GeneralFundamentalMatrix, RoundTrip) { } //************************************************************************* -// Create essential matrix and focal lengths for -// SimpleFundamentalMatrix -EssentialMatrix trueE; // Assuming a default constructor is available -double trueFa = 1.0; -double trueFb = 1.0; -Point2 trueCa(0.0, 0.0); -Point2 trueCb(0.0, 0.0); -SimpleFundamentalMatrix trueSimpleF(trueE, trueFa, trueFb, trueCa, trueCb); +// Create the simplest SimpleFundamentalMatrix, a stereo pair +EssentialMatrix defaultE(Rot3(), Unit3(1, 0, 0)); +Point2 zero(0.0, 0.0); +SimpleFundamentalMatrix stereoF(defaultE, 1.0, 1.0, zero, zero); //************************************************************************* -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) { - SimpleFundamentalMatrix actual = trueSimpleF.retract(Z_9x1); - EXPECT(assert_equal(trueSimpleF, actual)); +TEST(SimpleStereo, retract) { + SimpleFundamentalMatrix actual = stereoF.retract(Z_9x1); + 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); - Vector actual = trueSimpleF.localCoordinates(hx); + SimpleFundamentalMatrix hx = stereoF.retract(d); + 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;