diff --git a/gtsam/geometry/FundamentalMatrix.cpp b/gtsam/geometry/FundamentalMatrix.cpp index d65053d19..5a50b4fd2 100644 --- a/gtsam/geometry/FundamentalMatrix.cpp +++ b/gtsam/geometry/FundamentalMatrix.cpp @@ -26,6 +26,11 @@ Point2 EpipolarTransfer(const Matrix3& Fca, const Point2& pa, // } //************************************************************************* +FundamentalMatrix::FundamentalMatrix(const Matrix& U, double s, + const Matrix& V) { + initialize(U, s, V); +} + FundamentalMatrix::FundamentalMatrix(const Matrix3& F) { // Perform SVD Eigen::JacobiSVD svd(F, Eigen::ComputeFullU | Eigen::ComputeFullV); @@ -47,28 +52,44 @@ FundamentalMatrix::FundamentalMatrix(const Matrix3& F) { "The input matrix does not represent a valid fundamental matrix."); } - // Ensure the second singular value is recorded as s - s_ = singularValues(1); + initialize(U, singularValues(1), V); +} - // Check if U is a reflection - if (U.determinant() < 0) { - U = -U; - s_ = -s_; // Change sign of scalar if U is a reflection +void FundamentalMatrix::initialize(const Matrix3& U, double s, + const Matrix3& V) { + s_ = s; + sign_ = 1.0; + + // Check if U is a reflection and its determinant is close to -1 or 1 + double detU = U.determinant(); + if (std::abs(std::abs(detU) - 1.0) > 1e-9) { + throw std::invalid_argument( + "Matrix U does not have a determinant close to -1 or 1."); + } + if (detU < 0) { + U_ = Rot3(-U); + sign_ = -sign_; // Flip sign if U is a reflection + } else { + U_ = Rot3(U); } - // Check if V is a reflection - if (V.determinant() < 0) { - V = -V; - s_ = -s_; // Change sign of scalar if U is a reflection + // Check if V is a reflection and its determinant is close to -1 or 1 + double detV = V.determinant(); + if (std::abs(std::abs(detV) - 1.0) > 1e-9) { + throw std::invalid_argument( + "Matrix V does not have a determinant close to -1 or 1."); + } + if (detV < 0) { + V_ = Rot3(-V); + sign_ = -sign_; // Flip sign if V is a reflection + } else { + V_ = Rot3(V); } - - // Assign the rotations - U_ = Rot3(U); - V_ = Rot3(V); } Matrix3 FundamentalMatrix::matrix() const { - return U_.matrix() * Vector3(1, s_, 0).asDiagonal() * V_.transpose().matrix(); + return sign_ * U_.matrix() * Vector3(1.0, s_, 0).asDiagonal() * + V_.transpose().matrix(); } void FundamentalMatrix::print(const std::string& s) const { @@ -77,8 +98,8 @@ void FundamentalMatrix::print(const std::string& s) 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); + return U_.equals(other.U_, tol) && sign_ == other.sign_ && + std::abs(s_ - other.s_) < tol && V_.equals(other.V_, tol); } Vector FundamentalMatrix::localCoordinates(const FundamentalMatrix& F) const { @@ -93,7 +114,7 @@ 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 FundamentalMatrix(newU, newS, newV); + return FundamentalMatrix(newU, sign_, newS, newV); } //************************************************************************* diff --git a/gtsam/geometry/FundamentalMatrix.h b/gtsam/geometry/FundamentalMatrix.h index 12cc20b3e..7e87ba62a 100644 --- a/gtsam/geometry/FundamentalMatrix.h +++ b/gtsam/geometry/FundamentalMatrix.h @@ -15,34 +15,36 @@ namespace gtsam { /** * @class FundamentalMatrix - * @brief Represents a general fundamental matrix. + * @brief Represents a fundamental matrix in computer vision, which encodes the + * epipolar geometry between two views. * - * 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. + * The FundamentalMatrix class encapsulates the fundamental matrix, which + * relates corresponding points in stereo images. It is parameterized by two + * rotation matrices (U and V), a scalar parameter (s), and a sign. + * Using these values, the fundamental matrix is represented as + * + * F = sign * U * diag(1, s, 0) * V^T + * + * We need the `sign` because we use SO(3) for U and V, not O(3). */ class GTSAM_EXPORT FundamentalMatrix { private: - Rot3 U_; ///< Left rotation - double s_; ///< Scalar parameter for S - Rot3 V_; ///< Right rotation + Rot3 U_; ///< Left rotation + double sign_; ///< Whether to flip the sign or not + double s_; ///< Scalar parameter for S + Rot3 V_; ///< Right rotation public: /// Default constructor - FundamentalMatrix() : U_(Rot3()), s_(1.0), V_(Rot3()) {} + FundamentalMatrix() : U_(Rot3()), sign_(1.0), s_(1.0), V_(Rot3()) {} /** * @brief Construct from U, V, and scalar s * - * 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 + * Initializes the FundamentalMatrix From the SVD representation + * U*diag(1,s,0)*V^T. It will internally convert to using SO(3). */ - FundamentalMatrix(const Rot3& U, double s, const Rot3& V) - : U_(U), s_(s), V_(V) {} + FundamentalMatrix(const Matrix& U, double s, const Matrix& V); /** * @brief Construct from a 3x3 matrix using SVD @@ -107,6 +109,13 @@ class GTSAM_EXPORT FundamentalMatrix { /// Retract the given vector to get a new FundamentalMatrix FundamentalMatrix retract(const Vector& delta) const; /// @} + private: + /// Private constructor for internal use + FundamentalMatrix(const Rot3& U, double sign, double s, const Rot3& V) + : U_(U), sign_(sign), s_(s), V_(V) {} + + /// Initialize SO(3) matrices from general O(3) matrices + void initialize(const Matrix3& U, double s, const Matrix3& V); }; /** diff --git a/gtsam/geometry/tests/testFundamentalMatrix.cpp b/gtsam/geometry/tests/testFundamentalMatrix.cpp index a8884e791..3e136caa7 100644 --- a/gtsam/geometry/tests/testFundamentalMatrix.cpp +++ b/gtsam/geometry/tests/testFundamentalMatrix.cpp @@ -24,21 +24,48 @@ GTSAM_CONCEPT_MANIFOLD_INST(FundamentalMatrix) Rot3 trueU = Rot3::Yaw(M_PI_2); Rot3 trueV = Rot3::Yaw(M_PI_4); double trueS = 0.5; -FundamentalMatrix trueF(trueU, trueS, trueV); +FundamentalMatrix trueF(trueU.matrix(), trueS, trueV.matrix()); //************************************************************************* -TEST(FundamentalMatrix, 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(FundamentalMatrix, retract) { +TEST(FundamentalMatrix, Retract) { FundamentalMatrix actual = trueF.retract(Z_7x1); EXPECT(assert_equal(trueF, actual)); } +//************************************************************************* +// Test conversion from an F-matrix +TEST(FundamentalMatrix, Conversion) { + const Matrix3 F = trueU.matrix() * Vector3(1, trueS, 0).asDiagonal() * + trueV.matrix().transpose(); + FundamentalMatrix actual(F); + EXPECT(assert_equal(trueF, actual)); +} + +//************************************************************************* +// Test conversion with a *non-rotation* U +TEST(FlippedFundamentalMatrix, Conversion1) { + FundamentalMatrix trueF(trueU.matrix(), trueS, -trueV.matrix()); + const Matrix3 F = trueF.matrix(); + FundamentalMatrix actual(F); + CHECK(assert_equal(F, actual.matrix())); +} + +//************************************************************************* +// Test conversion with a *non-rotation* U +TEST(FlippedFundamentalMatrix, Conversion2) { + FundamentalMatrix trueF(-trueU.matrix(), trueS, trueV.matrix()); + const Matrix3 F = trueF.matrix(); + FundamentalMatrix actual(F); + CHECK(assert_equal(F, actual.matrix())); +} + //************************************************************************* TEST(FundamentalMatrix, RoundTrip) { Vector7 d; @@ -61,14 +88,14 @@ TEST(SimpleStereo, Conversion) { } //************************************************************************* -TEST(SimpleStereo, localCoordinates) { +TEST(SimpleStereo, LocalCoordinates) { Vector expected = Z_7x1; Vector actual = stereoF.localCoordinates(stereoF); EXPECT(assert_equal(expected, actual, 1e-8)); } //************************************************************************* -TEST(SimpleStereo, retract) { +TEST(SimpleStereo, Retract) { SimpleFundamentalMatrix actual = stereoF.retract(Z_9x1); EXPECT(assert_equal(stereoF, actual)); }