diff --git a/gtsam/geometry/FundamentalMatrix.h b/gtsam/geometry/FundamentalMatrix.h index ce2e1e48f..b76f95686 100644 --- a/gtsam/geometry/FundamentalMatrix.h +++ b/gtsam/geometry/FundamentalMatrix.h @@ -1,12 +1,13 @@ /* * @file FundamentalMatrix.h - * @brief FundamentalMatrix class + * @brief FundamentalMatrix classes * @author Frank Dellaert * @date Oct 23, 2024 */ #pragma once +#include #include #include @@ -76,8 +77,92 @@ class FundamentalMatrix { /// @} }; +/** + * @class SimpleFundamentalMatrix + * @brief Class for representing a simple fundamental matrix. + * + * This class represents a simple fundamental matrix, which is a + * 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 { + private: + EssentialMatrix E_; ///< Essential matrix + double fa_; ///< Focal length for left camera + double fb_; ///< Focal length for right camera + Point2 ca_; ///< Principal point for left camera + Point2 cb_; ///< Principal point for right camera + + 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 + SimpleFundamentalMatrix(const EssentialMatrix& E, // + double fa, double fb, + const Point2& ca = Point2(0.0, 0.0), + const Point2& cb = Point2(0.0, 0.0)) + : E_(E), fa_(fa), fb_(fb), ca_(ca), cb_(cb) {} + + /// Return the fundamental matrix representation + Matrix3 matrix() const { + 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 + return Ka * E_.matrix() * Kb.inverse(); + } + + /// @name Testable + /// @{ + + /// Print the SimpleFundamentalMatrix + void print(const std::string& s = "") const { + std::cout << s << "E:\n" + << E_.matrix() << "\nfa: " << fa_ << "\nfb: " << fb_ + << "\nca: " << ca_ << "\ncb: " << cb_ << std::endl; + } + + /// Check equality within a tolerance + bool equals(const SimpleFundamentalMatrix& other, double tol = 1e-9) const { + return E_.equals(other.E_, tol) && std::abs(fa_ - other.fa_) < tol && + std::abs(fb_ - other.fb_) < tol && (ca_ - other.ca_).norm() < tol && + (cb_ - other.cb_).norm() < tol; + } + /// @} + + /// @name Manifold + /// @{ + enum { dimension = 7 }; // 5 for E, 1 for fa, 1 for fb + inline static size_t Dim() { return dimension; } + inline size_t dim() const { return dimension; } + + /// Return local coordinates with respect to another + /// SimpleFundamentalMatrix + Vector localCoordinates(const SimpleFundamentalMatrix& F) const { + Vector result(7); + result.head<5>() = E_.localCoordinates(F.E_); + result(5) = F.fa_ - fa_; // Difference in fa + result(6) = F.fb_ - fb_; // Difference in fb + return result; + } + + /// Retract the given vector to get a new SimpleFundamentalMatrix + SimpleFundamentalMatrix retract(const Vector& delta) const { + EssentialMatrix newE = E_.retract(delta.head<5>()); + double newFa = fa_ + delta(5); // Update fa + double newFb = fb_ + delta(6); // Update fb + return SimpleFundamentalMatrix(newE, newFa, newFb, ca_, cb_); + } + /// @} +}; + template <> struct traits : public internal::Manifold {}; +template <> +struct traits + : public internal::Manifold {}; + } // namespace gtsam diff --git a/gtsam/geometry/tests/testFundamentalMatrix.cpp b/gtsam/geometry/tests/testFundamentalMatrix.cpp index 40b40c582..c69edaec1 100644 --- a/gtsam/geometry/tests/testFundamentalMatrix.cpp +++ b/gtsam/geometry/tests/testFundamentalMatrix.cpp @@ -1,6 +1,6 @@ /* * @file testFundamentalMatrix.cpp - * @brief Test FundamentalMatrix class + * @brief Test FundamentalMatrix classes * @author Frank Dellaert * @date October 23, 2024 */ @@ -48,6 +48,38 @@ TEST(FundamentalMatrix, RoundTrip) { EXPECT(assert_equal(d, actual, 1e-8)); } +//************************************************************************* +// Create essential matrix and focal lengths for +// SimpleFundamentalMatrix +EssentialMatrix trueE; // Assuming a default constructor is available +double trueFa = 1.0; +double trueFb = 1.0; +Point2 trueCa(0.0, 0.0); +Point2 trueCb(0.0, 0.0); +SimpleFundamentalMatrix trueSimpleF(trueE, trueFa, trueFb, trueCa, trueCb); + +//************************************************************************* +TEST(SimpleFundamentalMatrix, localCoordinates) { + Vector expected = Z_7x1; + Vector actual = trueSimpleF.localCoordinates(trueSimpleF); + EXPECT(assert_equal(expected, actual, 1e-8)); +} + +//************************************************************************* +TEST(SimpleFundamentalMatrix, retract) { + SimpleFundamentalMatrix actual = trueSimpleF.retract(Z_9x1); + EXPECT(assert_equal(trueSimpleF, actual)); +} + +//************************************************************************* +TEST(SimpleFundamentalMatrix, RoundTrip) { + Vector7 d; + d << 0.1, 0.2, 0.3, 0.4, 0.5, 0.6, 0.7; + SimpleFundamentalMatrix hx = trueSimpleF.retract(d); + Vector actual = trueSimpleF.localCoordinates(hx); + EXPECT(assert_equal(d, actual, 1e-8)); +} + //************************************************************************* int main() { TestResult tr;