diff --git a/gtsam/geometry/FundamentalMatrix.h b/gtsam/geometry/FundamentalMatrix.h index b76f95686..8140e3921 100644 --- a/gtsam/geometry/FundamentalMatrix.h +++ b/gtsam/geometry/FundamentalMatrix.h @@ -13,7 +13,33 @@ 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 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; +}; + +/** + * @class GeneralFundamentalMatrix + * @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 GeneralFundamentalMatrix : public FundamentalMatrix { private: Rot3 U_; ///< Left rotation double s_; ///< Scalar parameter for S @@ -21,14 +47,23 @@ class FundamentalMatrix { public: /// Default constructor - FundamentalMatrix() : U_(Rot3()), s_(1.0), V_(Rot3()) {} + GeneralFundamentalMatrix() : U_(Rot3()), s_(1.0), V_(Rot3()) {} - /// Construct from U, V, and scalar s - FundamentalMatrix(const Rot3& U, double s, const Rot3& V) + /** + * @brief Construct from U, V, and scalar s + * + * Initializes the GeneralFundamentalMatrix 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) : U_(U), s_(s), V_(V) {} /// Return the fundamental matrix representation - Matrix3 matrix() const { + Matrix3 matrix() const override { return U_.matrix() * Vector3(1, s_, 1).asDiagonal() * V_.transpose().matrix(); } @@ -36,15 +71,16 @@ class FundamentalMatrix { /// @name Testable /// @{ - /// Print the FundamentalMatrix + /// Print the GeneralFundamentalMatrix 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 { + /// Check if the GeneralFundamentalMatrix is equal to another within a + /// tolerance + bool equals(const GeneralFundamentalMatrix& other, double tol = 1e-9) const { return U_.equals(other.U_, tol) && std::abs(s_ - other.s_) < tol && V_.equals(other.V_, tol); } @@ -57,8 +93,8 @@ class FundamentalMatrix { 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 { + /// Return local coordinates with respect to another GeneralFundamentalMatrix + Vector localCoordinates(const GeneralFundamentalMatrix& F) const { Vector result(7); result.head<3>() = U_.localCoordinates(F.U_); result(3) = F.s_ - s_; // Difference in scalar @@ -66,12 +102,12 @@ class FundamentalMatrix { return result; } - /// Retract the given vector to get a new FundamentalMatrix - FundamentalMatrix retract(const Vector& delta) const { + /// Retract the given vector to get a new GeneralFundamentalMatrix + GeneralFundamentalMatrix 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 GeneralFundamentalMatrix(newU, newS, newV); } /// @} @@ -85,7 +121,7 @@ class 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 SimpleFundamentalMatrix { +class SimpleFundamentalMatrix : FundamentalMatrix { private: EssentialMatrix E_; ///< Essential matrix double fa_; ///< Focal length for left camera @@ -106,7 +142,7 @@ class SimpleFundamentalMatrix { : E_(E), fa_(fa), fb_(fb), ca_(ca), cb_(cb) {} /// Return the fundamental matrix representation - Matrix3 matrix() const { + Matrix3 matrix() const override { Matrix3 Ka, Kb; Ka << fa_, 0, ca_.x(), 0, fa_, ca_.y(), 0, 0, 1; // Left calibration Kb << fb_, 0, cb_.x(), 0, fb_, cb_.y(), 0, 0, 1; // Right calibration @@ -158,8 +194,8 @@ class SimpleFundamentalMatrix { }; 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 c69edaec1..0c132361c 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(FundamentalMatrix) -GTSAM_CONCEPT_MANIFOLD_INST(FundamentalMatrix) +GTSAM_CONCEPT_TESTABLE_INST(GeneralFundamentalMatrix) +GTSAM_CONCEPT_MANIFOLD_INST(GeneralFundamentalMatrix) //************************************************************************* // 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); +GeneralFundamentalMatrix trueF(trueU, trueS, trueV); //************************************************************************* -TEST(FundamentalMatrix, localCoordinates) { +TEST(GeneralFundamentalMatrix, 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); +TEST(GeneralFundamentalMatrix, retract) { + GeneralFundamentalMatrix actual = trueF.retract(Z_7x1); EXPECT(assert_equal(trueF, actual)); } //************************************************************************* -TEST(FundamentalMatrix, RoundTrip) { +TEST(GeneralFundamentalMatrix, RoundTrip) { Vector7 d; d << 0.1, 0.2, 0.3, 0.4, 0.5, 0.6, 0.7; - FundamentalMatrix hx = trueF.retract(d); + GeneralFundamentalMatrix hx = trueF.retract(d); Vector actual = trueF.localCoordinates(hx); EXPECT(assert_equal(d, actual, 1e-8)); }