diff --git a/gtsam/geometry/FundamentalMatrix.cpp b/gtsam/geometry/FundamentalMatrix.cpp index e56d9feb3..b16a3fef3 100644 --- a/gtsam/geometry/FundamentalMatrix.cpp +++ b/gtsam/geometry/FundamentalMatrix.cpp @@ -10,21 +10,20 @@ namespace gtsam { //************************************************************************* -Point2 FundamentalMatrix::transfer(const Matrix3& F01, const Point2& p1, - const Matrix3& F02, const Point2& p2) { - // Create lines in camera 0 from projections of the other two cameras - Vector3 line1 = F01 * Vector3(p1.x(), p1.y(), 1); - Vector3 line2 = F02 * Vector3(p2.x(), p2.y(), 1); +Point2 Transfer(const Matrix3& Fca, const Point2& pa, // + const Matrix3& Fcb, const Point2& pb) { + // Create lines in camera a from projections of the other two cameras + Vector3 line_a = Fca * Vector3(pa.x(), pa.y(), 1); + Vector3 line_b = Fcb * Vector3(pb.x(), pb.y(), 1); // Cross the lines to find the intersection point - Vector3 intersectionPoint = line1.cross(line2); + Vector3 intersectionPoint = line_a.cross(line_b); // Normalize the intersection point intersectionPoint /= intersectionPoint(2); return intersectionPoint.head<2>(); // Return the 2D point } - //************************************************************************* GeneralFundamentalMatrix::GeneralFundamentalMatrix(const Matrix3& F) { // Perform SVD diff --git a/gtsam/geometry/FundamentalMatrix.h b/gtsam/geometry/FundamentalMatrix.h index bf89a4bb8..9eba076a8 100644 --- a/gtsam/geometry/FundamentalMatrix.h +++ b/gtsam/geometry/FundamentalMatrix.h @@ -13,63 +13,6 @@ namespace gtsam { -/** - * @brief Abstract base class for FundamentalMatrix - * - * This class provides a common interface for all types of fundamental matrices. - * It declares a virtual function `matrix()` that must be implemented by derived - * classes. The `matrix()` function returns a 3x3 matrix representation of the - * fundamental matrix. - */ -class GTSAM_EXPORT FundamentalMatrix { - public: - /** - * @brief Returns a 3x3 matrix representation of the fundamental matrix - * - * @return A 3x3 matrix representing the fundamental matrix - */ - virtual Matrix3 matrix() const = 0; - - /** - * @brief Virtual destructor to ensure proper cleanup of derived classes - */ - virtual ~FundamentalMatrix() {} - - /** - * @brief Transfer projections from cameras 1 and 2 to camera 0 - * - * Take two fundamental matrices F01 and F02, and two points p1 and p2, and - * returns the 2D point in camera 0 where the epipolar lines intersect. - */ - static Point2 transfer(const Matrix3& F01, const Point2& p1, - const Matrix3& F02, const Point2& p2); -}; - -/// Represents a set of three fundamental matrices for transferring points -/// between three cameras. -template -struct TripleF { - F F01, F12, F20; - - /// Transfers a point from cameras 1,2 to camera 0. - Point2 transfer0(const Point2& p1, const Point2& p2) { - return FundamentalMatrix::transfer(F01.matrix(), p1, - F20.matrix().transpose(), p2); - } - - /// Transfers a point from camera 0,2 to camera 1. - Point2 transfer1(const Point2& p0, const Point2& p2) { - return FundamentalMatrix::transfer(F01.matrix().transpose(), p0, - F12.matrix(), p2); - } - - /// Transfers a point from camera 0,1 to camera 2. - Point2 transfer2(const Point2& p0, const Point2& p1) { - return FundamentalMatrix::transfer(F20.matrix(), p0, - F12.matrix().transpose(), p1); - } -}; - /** * @class GeneralFundamentalMatrix * @brief Represents a general fundamental matrix. @@ -78,7 +21,7 @@ struct TripleF { * that describes the relationship between two images. It is parameterized by a * left rotation U, a scalar s, and a right rotation V. */ -class GTSAM_EXPORT GeneralFundamentalMatrix : public FundamentalMatrix { +class GTSAM_EXPORT GeneralFundamentalMatrix { private: Rot3 U_; ///< Left rotation double s_; ///< Scalar parameter for S @@ -112,7 +55,7 @@ class GTSAM_EXPORT GeneralFundamentalMatrix : public FundamentalMatrix { GeneralFundamentalMatrix(const Matrix3& F); /// Return the fundamental matrix representation - Matrix3 matrix() const override; + Matrix3 matrix() const; /// @name Testable /// @{ @@ -146,7 +89,7 @@ class GTSAM_EXPORT GeneralFundamentalMatrix : public FundamentalMatrix { * parameterization of the essential matrix and focal lengths for left and right * cameras. Principal points are not part of the manifold but a convenience. */ -class GTSAM_EXPORT SimpleFundamentalMatrix : public FundamentalMatrix { +class GTSAM_EXPORT SimpleFundamentalMatrix { private: EssentialMatrix E_; ///< Essential matrix double fa_; ///< Focal length for left camera @@ -173,7 +116,7 @@ class GTSAM_EXPORT SimpleFundamentalMatrix : public FundamentalMatrix { Matrix3 rightK() const; /// Return the fundamental matrix representation - Matrix3 matrix() const override; + Matrix3 matrix() const; /// @name Testable /// @{ @@ -198,6 +141,37 @@ class GTSAM_EXPORT SimpleFundamentalMatrix : public FundamentalMatrix { /// @} }; +/** + * @brief Transfer projections from cameras a and b to camera c + * + * Take two fundamental matrices Fca and Fcb, and two points pa and pb, and + * returns the 2D point in view (c) where the epipolar lines intersect. + */ +Point2 Transfer(const Matrix3& Fca, const Point2& pa, const Matrix3& Fcb, + const Point2& pb); + +/// Represents a set of three fundamental matrices for transferring points +/// between three cameras. +template +struct TripleF { + F Fab, Fbc, Fca; + + /// Transfers a point from cameras b,c to camera a. + Point2 transferToA(const Point2& pb, const Point2& pc) { + return Transfer(Fab.matrix(), pb, Fca.matrix().transpose(), pc); + } + + /// Transfers a point from camera a,c to camera b. + Point2 transferToB(const Point2& pa, const Point2& pc) { + return Transfer(Fab.matrix().transpose(), pa, Fbc.matrix(), pc); + } + + /// Transfers a point from cameras a,b to camera c. + Point2 transferToC(const Point2& pa, const Point2& pb) { + return Transfer(Fca.matrix(), pa, Fbc.matrix().transpose(), pb); + } +}; + template <> struct traits : public internal::Manifold {}; diff --git a/gtsam/geometry/tests/testFundamentalMatrix.cpp b/gtsam/geometry/tests/testFundamentalMatrix.cpp index 760b3b99c..028db2c1f 100644 --- a/gtsam/geometry/tests/testFundamentalMatrix.cpp +++ b/gtsam/geometry/tests/testFundamentalMatrix.cpp @@ -7,7 +7,6 @@ #include #include -#include #include #include #include @@ -205,9 +204,9 @@ TEST(TripleF, Transfer) { auto triplet = generateTripleF(cameraPoses); // Check that they are all equal - EXPECT(triplet.F01.equals(triplet.F12, 1e-9)); - EXPECT(triplet.F12.equals(triplet.F20, 1e-9)); - EXPECT(triplet.F20.equals(triplet.F01, 1e-9)); + EXPECT(triplet.Fab.equals(triplet.Fbc, 1e-9)); + EXPECT(triplet.Fbc.equals(triplet.Fca, 1e-9)); + EXPECT(triplet.Fca.equals(triplet.Fab, 1e-9)); // Now project a point into the three cameras const Point3 P(0.1, 0.2, 0.3); @@ -222,9 +221,9 @@ TEST(TripleF, Transfer) { } // Check that transfer works - EXPECT(assert_equal(p[0], triplet.transfer0(p[1], p[2]), 1e-9)); - EXPECT(assert_equal(p[1], triplet.transfer1(p[0], p[2]), 1e-9)); - EXPECT(assert_equal(p[2], triplet.transfer2(p[0], p[1]), 1e-9)); + EXPECT(assert_equal(p[0], triplet.transferToA(p[1], p[2]), 1e-9)); + EXPECT(assert_equal(p[1], triplet.transferToB(p[0], p[2]), 1e-9)); + EXPECT(assert_equal(p[2], triplet.transferToC(p[0], p[1]), 1e-9)); } //*************************************************************************