diff --git a/gtsam/geometry/FundamentalMatrix.cpp b/gtsam/geometry/FundamentalMatrix.cpp index 71ca47418..57d59d0a0 100644 --- a/gtsam/geometry/FundamentalMatrix.cpp +++ b/gtsam/geometry/FundamentalMatrix.cpp @@ -10,8 +10,8 @@ namespace gtsam { //************************************************************************* -Point2 Transfer(const Matrix3& Fca, const Point2& pa, // - const Matrix3& Fcb, const Point2& pb) { +Point2 EpipolarTransfer(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); @@ -24,6 +24,7 @@ Point2 Transfer(const Matrix3& Fca, const Point2& pa, // return intersectionPoint.head<2>(); // Return the 2D point } + //************************************************************************* FundamentalMatrix::FundamentalMatrix(const Matrix3& F) { // Perform SVD diff --git a/gtsam/geometry/FundamentalMatrix.h b/gtsam/geometry/FundamentalMatrix.h index 718364ca0..497f95cf7 100644 --- a/gtsam/geometry/FundamentalMatrix.h +++ b/gtsam/geometry/FundamentalMatrix.h @@ -147,8 +147,8 @@ class GTSAM_EXPORT SimpleFundamentalMatrix { * 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. */ -GTSAM_EXPORT Point2 Transfer(const Matrix3& Fca, const Point2& pa, - const Matrix3& Fcb, const Point2& pb); +GTSAM_EXPORT Point2 EpipolarTransfer(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. @@ -158,17 +158,17 @@ struct TripleF { /// 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); + return EpipolarTransfer(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); + return EpipolarTransfer(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); + return EpipolarTransfer(Fca.matrix(), pa, Fbc.matrix().transpose(), pb); } };