diff --git a/gtsam/geometry/FundamentalMatrix.h b/gtsam/geometry/FundamentalMatrix.h index acd713020..32acb3be0 100644 --- a/gtsam/geometry/FundamentalMatrix.h +++ b/gtsam/geometry/FundamentalMatrix.h @@ -29,6 +29,66 @@ class FundamentalMatrix { * @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) { + // 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); + + // Cross the lines to find the intersection point + Vector3 intersectionPoint = line1.cross(line2); + + // Normalize the intersection point + intersectionPoint /= intersectionPoint(2); + + return intersectionPoint.head<2>(); // Return the 2D point + } +}; + +/// Represents a set of three fundamental matrices for transferring points +/// between three cameras. +template +struct TripleF { + F F01, F12, F20; + + /// Transfers a point from two cameras to another. + template + Point2 transfer(const Point2& point1, const Point2& point2) { + static_assert(Index < 3, "Index must be less than 3"); + } + + /// Specialization for transferring a point from cameras 1,2 to camera 0. + template <> + Point2 transfer<0>(const Point2& p1, const Point2& p2) { + return FundamentalMatrix::transfer(F01.matrix(), p1, + F20.matrix().transpose(), p2); + } + + /// Specialization for transferring a point from camera 0,2 to camera 1. + template <> + Point2 transfer<1>(const Point2& p0, const Point2& p2) { + return FundamentalMatrix::transfer(F01.matrix().transpose(), p0, + F12.matrix(), p2); + } + + /// Specialization for transferring a point from camera 0,1 to camera 2. + template <> + Point2 transfer<2>(const Point2& p0, const Point2& p1) { + return FundamentalMatrix::transfer(F01.matrix(), p0, + F12.matrix().transpose(), p1); + } }; /** diff --git a/gtsam/geometry/tests/testFundamentalMatrix.cpp b/gtsam/geometry/tests/testFundamentalMatrix.cpp index 40a0b4767..f5a578911 100644 --- a/gtsam/geometry/tests/testFundamentalMatrix.cpp +++ b/gtsam/geometry/tests/testFundamentalMatrix.cpp @@ -171,6 +171,7 @@ TEST(stereoWithPrincipalPoints, Conversion) { } //************************************************************************* +/// Generate three cameras on a circle, looking in std::array generateCameraPoses() { std::array cameraPoses; const double radius = 1.0; @@ -183,9 +184,10 @@ std::array generateCameraPoses() { return cameraPoses; } -std::tuple -generateFs(const std::array &cameraPoses) { +//************************************************************************* +/// Function to generate a TripleF from camera poses +TripleF generateTripleF( + const std::array& cameraPoses) { std::array F; for (size_t i = 0; i < 3; ++i) { size_t j = (i + 1) % 3; @@ -193,19 +195,19 @@ generateFs(const std::array &cameraPoses) { EssentialMatrix E(iPj.rotation(), Unit3(iPj.translation())); F[i] = {E, focalLength, focalLength, principalPoint, principalPoint}; } - return {F[0], F[1], F[2]}; + return {F[0], F[1], F[2]}; // Return a TripleF instance } //************************************************************************* -TEST(Triplet, Transfer) { +TEST(TripleF, Transfer) { // Generate cameras on a circle, as well as fundamental matrices auto cameraPoses = generateCameraPoses(); - auto [F01, F12, F20] = generateFs(cameraPoses); + auto triplet = generateTripleF(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)); + EXPECT(triplet.F01.equals(triplet.F12, 1e-9)); + EXPECT(triplet.F12.equals(triplet.F20, 1e-9)); + EXPECT(triplet.F20.equals(triplet.F01, 1e-9)); // Now project a point into the three cameras const Point3 P(0.1, 0.2, 0.3); @@ -219,19 +221,12 @@ TEST(Triplet, Transfer) { 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(p[0], intersectionPoint.head<2>(), 1e-9)); + // Check that transfer works + EXPECT(assert_equal(p[0], triplet.transfer<0>(p[1], p[2]), 1e-9)); + EXPECT(assert_equal(p[1], triplet.transfer<1>(p[0], p[2]), 1e-9)); + EXPECT(assert_equal(p[2], triplet.transfer<2>(p[0], p[1]), 1e-9)); } + //************************************************************************* int main() { TestResult tr;