Remove inheritance, use abc language
							parent
							
								
									dc23c02be8
								
							
						
					
					
						commit
						5b413b84c9
					
				|  | @ -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
 | ||||
|  |  | |||
|  | @ -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 <typename F> | ||||
| 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 <typename F> | ||||
| 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<GeneralFundamentalMatrix> | ||||
|     : public internal::Manifold<GeneralFundamentalMatrix> {}; | ||||
|  |  | |||
|  | @ -7,7 +7,6 @@ | |||
| 
 | ||||
| #include <CppUnitLite/TestHarness.h> | ||||
| #include <gtsam/base/Testable.h> | ||||
| #include <gtsam/base/numericalDerivative.h> | ||||
| #include <gtsam/geometry/FundamentalMatrix.h> | ||||
| #include <gtsam/geometry/Rot3.h> | ||||
| #include <gtsam/geometry/SimpleCamera.h> | ||||
|  | @ -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<Point2>(p[0], triplet.transfer0(p[1], p[2]), 1e-9)); | ||||
|   EXPECT(assert_equal<Point2>(p[1], triplet.transfer1(p[0], p[2]), 1e-9)); | ||||
|   EXPECT(assert_equal<Point2>(p[2], triplet.transfer2(p[0], p[1]), 1e-9)); | ||||
|   EXPECT(assert_equal<Point2>(p[0], triplet.transferToA(p[1], p[2]), 1e-9)); | ||||
|   EXPECT(assert_equal<Point2>(p[1], triplet.transferToB(p[0], p[2]), 1e-9)); | ||||
|   EXPECT(assert_equal<Point2>(p[2], triplet.transferToC(p[0], p[1]), 1e-9)); | ||||
| } | ||||
| 
 | ||||
| //*************************************************************************
 | ||||
|  |  | |||
		Loading…
	
		Reference in New Issue