Address review comments
							parent
							
								
									00cb637db3
								
							
						
					
					
						commit
						cbbe2d2e88
					
				|  | @ -97,20 +97,20 @@ FundamentalMatrix FundamentalMatrix::retract(const Vector& delta) const { | |||
| } | ||||
| 
 | ||||
| //*************************************************************************
 | ||||
| Matrix3 SimpleFundamentalMatrix::leftK() const { | ||||
| Matrix3 SimpleFundamentalMatrix::Ka() const { | ||||
|   Matrix3 K; | ||||
|   K << fa_, 0, ca_.x(), 0, fa_, ca_.y(), 0, 0, 1; | ||||
|   return K; | ||||
| } | ||||
| 
 | ||||
| Matrix3 SimpleFundamentalMatrix::rightK() const { | ||||
| Matrix3 SimpleFundamentalMatrix::Kb() const { | ||||
|   Matrix3 K; | ||||
|   K << fb_, 0, cb_.x(), 0, fb_, cb_.y(), 0, 0, 1; | ||||
|   return K; | ||||
| } | ||||
| 
 | ||||
| Matrix3 SimpleFundamentalMatrix::matrix() const { | ||||
|   return leftK().transpose().inverse() * E_.matrix() * rightK().inverse(); | ||||
|   return Ka().transpose().inverse() * E_.matrix() * Kb().inverse(); | ||||
| } | ||||
| 
 | ||||
| void SimpleFundamentalMatrix::print(const std::string& s) const { | ||||
|  |  | |||
|  | @ -68,8 +68,8 @@ class GTSAM_EXPORT FundamentalMatrix { | |||
|   template <typename CAL> | ||||
|   FundamentalMatrix(const CAL& Ka, const Pose3& aPb, const CAL& Kb) | ||||
|       : FundamentalMatrix(Ka.K().transpose().inverse() * | ||||
|                                  EssentialMatrix::FromPose3(aPb).matrix() * | ||||
|                                  Kb.K().inverse()) {} | ||||
|                           EssentialMatrix::FromPose3(aPb).matrix() * | ||||
|                           Kb.K().inverse()) {} | ||||
| 
 | ||||
|   /// Return the fundamental matrix representation
 | ||||
|   Matrix3 matrix() const; | ||||
|  | @ -114,25 +114,32 @@ class GTSAM_EXPORT SimpleFundamentalMatrix { | |||
|   Point2 ca_;          ///< Principal point for left camera
 | ||||
|   Point2 cb_;          ///< Principal point for right camera
 | ||||
| 
 | ||||
|   /// Return the left calibration matrix
 | ||||
|   Matrix3 Ka() const; | ||||
| 
 | ||||
|   /// Return the right calibration matrix
 | ||||
|   Matrix3 Kb() const; | ||||
| 
 | ||||
|  public: | ||||
|   /// Default constructor
 | ||||
|   SimpleFundamentalMatrix() | ||||
|       : E_(), fa_(1.0), fb_(1.0), ca_(0.0, 0.0), cb_(0.0, 0.0) {} | ||||
| 
 | ||||
|   /// Construct from essential matrix and focal lengths
 | ||||
|   /**
 | ||||
|    * @brief Construct from essential matrix and focal lengths | ||||
|    * @param E Essential matrix | ||||
|    * @param fa Focal length for left camera | ||||
|    * @param fb Focal length for right camera | ||||
|    * @param ca Principal point for left camera | ||||
|    * @param cb Principal point for right camera | ||||
|    */ | ||||
|   SimpleFundamentalMatrix(const EssentialMatrix& E,  //
 | ||||
|                           double fa, double fb, | ||||
|                           const Point2& ca = Point2(0.0, 0.0), | ||||
|                           const Point2& cb = Point2(0.0, 0.0)) | ||||
|                           double fa, double fb, const Point2& ca, | ||||
|                           const Point2& cb) | ||||
|       : E_(E), fa_(fa), fb_(fb), ca_(ca), cb_(cb) {} | ||||
| 
 | ||||
|   /// Return the left calibration matrix
 | ||||
|   Matrix3 leftK() const; | ||||
| 
 | ||||
|   /// Return the right calibration matrix
 | ||||
|   Matrix3 rightK() const; | ||||
| 
 | ||||
|   /// Return the fundamental matrix representation
 | ||||
|   /// F = Ka^(-T) * E * Kb^(-1)
 | ||||
|   Matrix3 matrix() const; | ||||
| 
 | ||||
|   /// @name Testable
 | ||||
|  |  | |||
|  | @ -24,8 +24,8 @@ namespace gtsam { | |||
| 
 | ||||
| /**
 | ||||
|  * Binary factor in the context of Structure from Motion (SfM). | ||||
|  * It is used to transfer points between different views based on the | ||||
|  * fundamental matrices between these views. The factor computes the error | ||||
|  * It is used to transfer transfer corresponding points from two views to a | ||||
|  * third based on two fundamental matrices. The factor computes the error | ||||
|  * between the transferred points `pa` and `pb`, and the actual point `pc` in | ||||
|  * the target view. Jacobians are done using numerical differentiation. | ||||
|  */ | ||||
|  |  | |||
|  | @ -12,9 +12,6 @@ | |||
| 
 | ||||
| using namespace gtsam; | ||||
| 
 | ||||
| double focalLength = 1000; | ||||
| Point2 principalPoint(640 / 2, 480 / 2); | ||||
| 
 | ||||
| //*************************************************************************
 | ||||
| /// Generate three cameras on a circle, looking in
 | ||||
| std::array<Pose3, 3> generateCameraPoses() { | ||||
|  | @ -44,17 +41,36 @@ TripleF<SimpleFundamentalMatrix> generateTripleF( | |||
|   return {F[0], F[1], F[2]};  // Return a TripleF instance
 | ||||
| } | ||||
| 
 | ||||
| //*************************************************************************
 | ||||
| namespace fixture { | ||||
| // Generate cameras on a circle
 | ||||
| std::array<Pose3, 3> cameraPoses = generateCameraPoses(); | ||||
| auto triplet = generateTripleF(cameraPoses); | ||||
| double focalLength = 1000; | ||||
| Point2 principalPoint(640 / 2, 480 / 2); | ||||
| const Cal3_S2 K(focalLength, focalLength, 0.0,  //
 | ||||
|                 principalPoint.x(), principalPoint.y()); | ||||
| }  // namespace fixture
 | ||||
| 
 | ||||
| //*************************************************************************
 | ||||
| // Test for getMatrices
 | ||||
| TEST(TransferFactor, GetMatrices) { | ||||
|   using namespace fixture; | ||||
|   TransferFactor<SimpleFundamentalMatrix> factor{{2, 0}, {1, 2}, {}}; | ||||
| 
 | ||||
|   // Check that getMatrices is correct
 | ||||
|   auto [Fki, Fkj] = factor.getMatrices(triplet.Fca, triplet.Fbc); | ||||
|   EXPECT(assert_equal<Matrix3>(triplet.Fca.matrix(), Fki)); | ||||
|   EXPECT(assert_equal<Matrix3>(triplet.Fbc.matrix().transpose(), Fkj)); | ||||
| } | ||||
| 
 | ||||
| //*************************************************************************
 | ||||
| // Test for TransferFactor
 | ||||
| TEST(TransferFactor, Jacobians) { | ||||
|   // Generate cameras on a circle
 | ||||
|   std::array<Pose3, 3> cameraPoses = generateCameraPoses(); | ||||
|   auto triplet = generateTripleF(cameraPoses); | ||||
|   using namespace fixture; | ||||
| 
 | ||||
|   // Now project a point into the three cameras
 | ||||
|   const Point3 P(0.1, 0.2, 0.3); | ||||
|   const Cal3_S2 K(focalLength, focalLength, 0.0,  //
 | ||||
|                   principalPoint.x(), principalPoint.y()); | ||||
| 
 | ||||
|   std::array<Point2, 3> p; | ||||
|   for (size_t i = 0; i < 3; ++i) { | ||||
|  | @ -70,11 +86,6 @@ TEST(TransferFactor, Jacobians) { | |||
|       factor1{key12, key01, p[2], p[0], p[1]}, | ||||
|       factor2{key20, key12, p[0], p[1], p[2]}; | ||||
| 
 | ||||
|   // Check that getMatrices is correct
 | ||||
|   auto [Fki, Fkj] = factor2.getMatrices(triplet.Fca, triplet.Fbc); | ||||
|   EXPECT(assert_equal<Matrix3>(triplet.Fca.matrix(), Fki)); | ||||
|   EXPECT(assert_equal<Matrix3>(triplet.Fbc.matrix().transpose(), Fkj)); | ||||
| 
 | ||||
|   // Create Values with edge keys
 | ||||
|   Values values; | ||||
|   values.insert(key01, triplet.Fab); | ||||
|  | @ -92,18 +103,13 @@ TEST(TransferFactor, Jacobians) { | |||
| //*************************************************************************
 | ||||
| // Test for TransferFactor with multiple tuples
 | ||||
| TEST(TransferFactor, MultipleTuples) { | ||||
|   // Generate cameras on a circle
 | ||||
|   std::array<Pose3, 3> cameraPoses = generateCameraPoses(); | ||||
|   auto triplet = generateTripleF(cameraPoses); | ||||
|   using namespace fixture; | ||||
| 
 | ||||
|   // Now project multiple points into the three cameras
 | ||||
|   const size_t numPoints = 5;  // Number of points to test
 | ||||
|   std::vector<Point3> points3D; | ||||
|   std::vector<std::array<Point2, 3>> projectedPoints; | ||||
| 
 | ||||
|   const Cal3_S2 K(focalLength, focalLength, 0.0,  //
 | ||||
|                   principalPoint.x(), principalPoint.y()); | ||||
| 
 | ||||
|   // Generate random 3D points and project them
 | ||||
|   for (size_t n = 0; n < numPoints; ++n) { | ||||
|     Point3 P(0.1 * n, 0.2 * n, 0.3 + 0.1 * n); | ||||
|  | @ -120,15 +126,15 @@ TEST(TransferFactor, MultipleTuples) { | |||
| 
 | ||||
|   // Create a vector of tuples for the TransferFactor
 | ||||
|   EdgeKey key01(0, 1), key12(1, 2), key20(2, 0); | ||||
|   std::vector<std::tuple<Point2, Point2, Point2>> tuples0, tuples1, tuples2; | ||||
|   std::vector<std::tuple<Point2, Point2, Point2>> tuples; | ||||
| 
 | ||||
|   for (size_t n = 0; n < numPoints; ++n) { | ||||
|     const auto& p = projectedPoints[n]; | ||||
|     tuples0.emplace_back(p[1], p[2], p[0]); | ||||
|     tuples.emplace_back(p[1], p[2], p[0]); | ||||
|   } | ||||
| 
 | ||||
|   // Create TransferFactors using the new constructor
 | ||||
|   TransferFactor<SimpleFundamentalMatrix> factor{key01, key20, tuples0}; | ||||
|   TransferFactor<SimpleFundamentalMatrix> factor{key01, key20, tuples}; | ||||
| 
 | ||||
|   // Create Values with edge keys
 | ||||
|   Values values; | ||||
|  |  | |||
		Loading…
	
		Reference in New Issue