diff --git a/gtsam/geometry/FundamentalMatrix.h b/gtsam/geometry/FundamentalMatrix.h new file mode 100644 index 000000000..ce2e1e48f --- /dev/null +++ b/gtsam/geometry/FundamentalMatrix.h @@ -0,0 +1,83 @@ +/* + * @file FundamentalMatrix.h + * @brief FundamentalMatrix class + * @author Frank Dellaert + * @date Oct 23, 2024 + */ + +#pragma once + +#include +#include + +namespace gtsam { + +class FundamentalMatrix { + private: + Rot3 U_; ///< Left rotation + double s_; ///< Scalar parameter for S + Rot3 V_; ///< Right rotation + + public: + /// Default constructor + FundamentalMatrix() : U_(Rot3()), s_(1.0), V_(Rot3()) {} + + /// Construct from U, V, and scalar s + FundamentalMatrix(const Rot3& U, double s, const Rot3& V) + : U_(U), s_(s), V_(V) {} + + /// Return the fundamental matrix representation + Matrix3 matrix() const { + return U_.matrix() * Vector3(1, s_, 1).asDiagonal() * + V_.transpose().matrix(); + } + + /// @name Testable + /// @{ + + /// Print the FundamentalMatrix + void print(const std::string& s = "") const { + std::cout << s << "U:\n" + << U_.matrix() << "\ns: " << s_ << "\nV:\n" + << V_.matrix() << std::endl; + } + + /// Check if the FundamentalMatrix is equal to another within a tolerance + bool equals(const FundamentalMatrix& other, double tol = 1e-9) const { + return U_.equals(other.U_, tol) && std::abs(s_ - other.s_) < tol && + V_.equals(other.V_, tol); + } + + /// @} + + /// @name Manifold + /// @{ + enum { dimension = 7 }; // 3 for U, 1 for s, 3 for V + inline static size_t Dim() { return dimension; } + inline size_t dim() const { return dimension; } + + /// Return local coordinates with respect to another FundamentalMatrix + Vector localCoordinates(const FundamentalMatrix& F) const { + Vector result(7); + result.head<3>() = U_.localCoordinates(F.U_); + result(3) = F.s_ - s_; // Difference in scalar + result.tail<3>() = V_.localCoordinates(F.V_); + return result; + } + + /// Retract the given vector to get a new 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); + } + + /// @} +}; + +template <> +struct traits + : public internal::Manifold {}; + +} // namespace gtsam diff --git a/gtsam/geometry/tests/testFundamentalMatrix.cpp b/gtsam/geometry/tests/testFundamentalMatrix.cpp new file mode 100644 index 000000000..40b40c582 --- /dev/null +++ b/gtsam/geometry/tests/testFundamentalMatrix.cpp @@ -0,0 +1,56 @@ +/* + * @file testFundamentalMatrix.cpp + * @brief Test FundamentalMatrix class + * @author Frank Dellaert + * @date October 23, 2024 + */ + +#include +#include +#include +#include +#include +#include + +using namespace std::placeholders; +using namespace std; +using namespace gtsam; + +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; +FundamentalMatrix trueF(trueU, trueS, trueV); + +//************************************************************************* +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) { + FundamentalMatrix actual = trueF.retract(Z_7x1); + EXPECT(assert_equal(trueF, actual)); +} + +//************************************************************************* +TEST(FundamentalMatrix, RoundTrip) { + Vector7 d; + d << 0.1, 0.2, 0.3, 0.4, 0.5, 0.6, 0.7; + FundamentalMatrix hx = trueF.retract(d); + Vector actual = trueF.localCoordinates(hx); + EXPECT(assert_equal(d, actual, 1e-8)); +} + +//************************************************************************* +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +/* ************************************************************************* */