From 502dcc480bff63ec1bd8bb561c7e65b021c1a800 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Thu, 24 Oct 2024 17:30:30 -0700 Subject: [PATCH] Just FundamentalMatrix --- gtsam/geometry/FundamentalMatrix.cpp | 18 +++++------ gtsam/geometry/FundamentalMatrix.h | 32 +++++++++---------- .../geometry/tests/testFundamentalMatrix.cpp | 24 +++++++------- 3 files changed, 36 insertions(+), 38 deletions(-) diff --git a/gtsam/geometry/FundamentalMatrix.cpp b/gtsam/geometry/FundamentalMatrix.cpp index b16a3fef32..71ca47418b 100644 --- a/gtsam/geometry/FundamentalMatrix.cpp +++ b/gtsam/geometry/FundamentalMatrix.cpp @@ -25,7 +25,7 @@ Point2 Transfer(const Matrix3& Fca, const Point2& pa, // return intersectionPoint.head<2>(); // Return the 2D point } //************************************************************************* -GeneralFundamentalMatrix::GeneralFundamentalMatrix(const Matrix3& F) { +FundamentalMatrix::FundamentalMatrix(const Matrix3& F) { // Perform SVD Eigen::JacobiSVD svd(F, Eigen::ComputeFullU | Eigen::ComputeFullV); @@ -66,24 +66,23 @@ GeneralFundamentalMatrix::GeneralFundamentalMatrix(const Matrix3& F) { V_ = Rot3(V); } -Matrix3 GeneralFundamentalMatrix::matrix() const { +Matrix3 FundamentalMatrix::matrix() const { return U_.matrix() * Vector3(1, s_, 0).asDiagonal() * V_.transpose().matrix(); } -void GeneralFundamentalMatrix::print(const std::string& s) const { +void FundamentalMatrix::print(const std::string& s) const { std::cout << s << "U:\n" << U_.matrix() << "\ns: " << s_ << "\nV:\n" << V_.matrix() << std::endl; } -bool GeneralFundamentalMatrix::equals(const GeneralFundamentalMatrix& other, - double tol) const { +bool FundamentalMatrix::equals(const FundamentalMatrix& other, + double tol) const { return U_.equals(other.U_, tol) && std::abs(s_ - other.s_) < tol && V_.equals(other.V_, tol); } -Vector GeneralFundamentalMatrix::localCoordinates( - const GeneralFundamentalMatrix& F) const { +Vector FundamentalMatrix::localCoordinates(const FundamentalMatrix& F) const { Vector result(7); result.head<3>() = U_.localCoordinates(F.U_); result(3) = F.s_ - s_; // Difference in scalar @@ -91,12 +90,11 @@ Vector GeneralFundamentalMatrix::localCoordinates( return result; } -GeneralFundamentalMatrix GeneralFundamentalMatrix::retract( - const Vector& delta) const { +FundamentalMatrix FundamentalMatrix::retract(const Vector& delta) const { Rot3 newU = U_.retract(delta.head<3>()); double newS = s_ + delta(3); // Update scalar Rot3 newV = V_.retract(delta.tail<3>()); - return GeneralFundamentalMatrix(newU, newS, newV); + return FundamentalMatrix(newU, newS, newV); } //************************************************************************* diff --git a/gtsam/geometry/FundamentalMatrix.h b/gtsam/geometry/FundamentalMatrix.h index ff7f4c8372..718364ca0c 100644 --- a/gtsam/geometry/FundamentalMatrix.h +++ b/gtsam/geometry/FundamentalMatrix.h @@ -14,14 +14,14 @@ namespace gtsam { /** - * @class GeneralFundamentalMatrix + * @class FundamentalMatrix * @brief Represents a general fundamental matrix. * * This class represents a general fundamental matrix, which is a 3x3 matrix * 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 { +class GTSAM_EXPORT FundamentalMatrix { private: Rot3 U_; ///< Left rotation double s_; ///< Scalar parameter for S @@ -29,42 +29,42 @@ class GTSAM_EXPORT GeneralFundamentalMatrix { public: /// Default constructor - GeneralFundamentalMatrix() : U_(Rot3()), s_(1.0), V_(Rot3()) {} + FundamentalMatrix() : U_(Rot3()), s_(1.0), V_(Rot3()) {} /** * @brief Construct from U, V, and scalar s * - * Initializes the GeneralFundamentalMatrix with the given left rotation U, + * Initializes the FundamentalMatrix with the given left rotation U, * scalar s, and right rotation V. * * @param U Left rotation matrix * @param s Scalar parameter for the fundamental matrix * @param V Right rotation matrix */ - GeneralFundamentalMatrix(const Rot3& U, double s, const Rot3& V) + FundamentalMatrix(const Rot3& U, double s, const Rot3& V) : U_(U), s_(s), V_(V) {} /** * @brief Construct from a 3x3 matrix using SVD * - * Initializes the GeneralFundamentalMatrix by performing SVD on the given + * Initializes the FundamentalMatrix by performing SVD on the given * matrix and ensuring U and V are not reflections. * * @param F A 3x3 matrix representing the fundamental matrix */ - GeneralFundamentalMatrix(const Matrix3& F); + FundamentalMatrix(const Matrix3& F); /// Return the fundamental matrix representation Matrix3 matrix() const; /// @name Testable /// @{ - /// Print the GeneralFundamentalMatrix + /// Print the FundamentalMatrix void print(const std::string& s = "") const; - /// Check if the GeneralFundamentalMatrix is equal to another within a + /// Check if the FundamentalMatrix is equal to another within a /// tolerance - bool equals(const GeneralFundamentalMatrix& other, double tol = 1e-9) const; + bool equals(const FundamentalMatrix& other, double tol = 1e-9) const; /// @} /// @name Manifold @@ -73,11 +73,11 @@ class GTSAM_EXPORT GeneralFundamentalMatrix { inline static size_t Dim() { return dimension; } inline size_t dim() const { return dimension; } - /// Return local coordinates with respect to another GeneralFundamentalMatrix - Vector localCoordinates(const GeneralFundamentalMatrix& F) const; + /// Return local coordinates with respect to another FundamentalMatrix + Vector localCoordinates(const FundamentalMatrix& F) const; - /// Retract the given vector to get a new GeneralFundamentalMatrix - GeneralFundamentalMatrix retract(const Vector& delta) const; + /// Retract the given vector to get a new FundamentalMatrix + FundamentalMatrix retract(const Vector& delta) const; /// @} }; @@ -173,8 +173,8 @@ struct TripleF { }; template <> -struct traits - : public internal::Manifold {}; +struct traits + : public internal::Manifold {}; template <> struct traits diff --git a/gtsam/geometry/tests/testFundamentalMatrix.cpp b/gtsam/geometry/tests/testFundamentalMatrix.cpp index 028db2c1f8..a8884e791e 100644 --- a/gtsam/geometry/tests/testFundamentalMatrix.cpp +++ b/gtsam/geometry/tests/testFundamentalMatrix.cpp @@ -16,34 +16,34 @@ using namespace std::placeholders; using namespace std; using namespace gtsam; -GTSAM_CONCEPT_TESTABLE_INST(GeneralFundamentalMatrix) -GTSAM_CONCEPT_MANIFOLD_INST(GeneralFundamentalMatrix) +GTSAM_CONCEPT_TESTABLE_INST(FundamentalMatrix) +GTSAM_CONCEPT_MANIFOLD_INST(FundamentalMatrix) //************************************************************************* // Create two rotations and corresponding fundamental matrix F Rot3 trueU = Rot3::Yaw(M_PI_2); Rot3 trueV = Rot3::Yaw(M_PI_4); double trueS = 0.5; -GeneralFundamentalMatrix trueF(trueU, trueS, trueV); +FundamentalMatrix trueF(trueU, trueS, trueV); //************************************************************************* -TEST(GeneralFundamentalMatrix, localCoordinates) { +TEST(FundamentalMatrix, localCoordinates) { Vector expected = Z_7x1; // Assuming 7 dimensions for U, V, and s Vector actual = trueF.localCoordinates(trueF); EXPECT(assert_equal(expected, actual, 1e-8)); } //************************************************************************* -TEST(GeneralFundamentalMatrix, retract) { - GeneralFundamentalMatrix actual = trueF.retract(Z_7x1); +TEST(FundamentalMatrix, retract) { + FundamentalMatrix actual = trueF.retract(Z_7x1); EXPECT(assert_equal(trueF, actual)); } //************************************************************************* -TEST(GeneralFundamentalMatrix, RoundTrip) { +TEST(FundamentalMatrix, RoundTrip) { Vector7 d; d << 0.1, 0.2, 0.3, 0.4, 0.5, 0.6, 0.7; - GeneralFundamentalMatrix hx = trueF.retract(d); + FundamentalMatrix hx = trueF.retract(d); Vector actual = trueF.localCoordinates(hx); EXPECT(assert_equal(d, actual, 1e-8)); } @@ -56,7 +56,7 @@ SimpleFundamentalMatrix stereoF(defaultE, 1.0, 1.0, zero, zero); //************************************************************************* TEST(SimpleStereo, Conversion) { - GeneralFundamentalMatrix convertedF(stereoF.matrix()); + FundamentalMatrix convertedF(stereoF.matrix()); EXPECT(assert_equal(stereoF.matrix(), convertedF.matrix(), 1e-8)); } @@ -103,7 +103,7 @@ SimpleFundamentalMatrix pixelStereo(defaultE, focalLength, focalLength, zero, TEST(PixelStereo, Conversion) { auto expected = pixelStereo.matrix(); - GeneralFundamentalMatrix convertedF(pixelStereo.matrix()); + FundamentalMatrix convertedF(pixelStereo.matrix()); // Check equality of F-matrices up to a scale auto actual = convertedF.matrix(); @@ -132,7 +132,7 @@ SimpleFundamentalMatrix rotatedPixelStereo(rotatedE, focalLength, focalLength, TEST(RotatedPixelStereo, Conversion) { auto expected = rotatedPixelStereo.matrix(); - GeneralFundamentalMatrix convertedF(rotatedPixelStereo.matrix()); + FundamentalMatrix convertedF(rotatedPixelStereo.matrix()); // Check equality of F-matrices up to a scale auto actual = convertedF.matrix(); @@ -161,7 +161,7 @@ SimpleFundamentalMatrix stereoWithPrincipalPoints(rotatedE, focalLength, TEST(stereoWithPrincipalPoints, Conversion) { auto expected = stereoWithPrincipalPoints.matrix(); - GeneralFundamentalMatrix convertedF(stereoWithPrincipalPoints.matrix()); + FundamentalMatrix convertedF(stereoWithPrincipalPoints.matrix()); // Check equality of F-matrices up to a scale auto actual = convertedF.matrix();