From ff9a39911727567e5dbb0f26d8ac28745a101f06 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Wed, 23 Oct 2024 15:25:05 -0700 Subject: [PATCH 01/14] Spelling --- gtsam/base/Manifold.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/base/Manifold.h b/gtsam/base/Manifold.h index 815c8b2880..cb30fa9c01 100644 --- a/gtsam/base/Manifold.h +++ b/gtsam/base/Manifold.h @@ -162,7 +162,7 @@ struct FixedDimension { typedef const int value_type; static const int value = traits::dimension; static_assert(value != Eigen::Dynamic, - "FixedDimension instantiated for dymanically-sized type."); + "FixedDimension instantiated for dynamically-sized type."); }; } // \ namespace gtsam From f6ed30d49882b1e2aec14433af44f290938e86bf Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Wed, 23 Oct 2024 15:25:20 -0700 Subject: [PATCH 02/14] Initial check-in --- gtsam/geometry/FundamentalMatrix.h | 83 +++++++++++++++++++ .../geometry/tests/testFundamentalMatrix.cpp | 56 +++++++++++++ 2 files changed, 139 insertions(+) create mode 100644 gtsam/geometry/FundamentalMatrix.h create mode 100644 gtsam/geometry/tests/testFundamentalMatrix.cpp diff --git a/gtsam/geometry/FundamentalMatrix.h b/gtsam/geometry/FundamentalMatrix.h new file mode 100644 index 0000000000..ce2e1e48f6 --- /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 0000000000..40b40c5827 --- /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); +} +/* ************************************************************************* */ From f104951494197212b600f8af6417188483fe8380 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Wed, 23 Oct 2024 15:56:22 -0700 Subject: [PATCH 03/14] Simple F version --- gtsam/geometry/FundamentalMatrix.h | 87 ++++++++++++++++++- .../geometry/tests/testFundamentalMatrix.cpp | 34 +++++++- 2 files changed, 119 insertions(+), 2 deletions(-) diff --git a/gtsam/geometry/FundamentalMatrix.h b/gtsam/geometry/FundamentalMatrix.h index ce2e1e48f6..b76f956867 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 40b40c5827..c69edaec1c 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; From 76175feb9551244b7229269a7b8599bc1020346d Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Wed, 23 Oct 2024 16:01:28 -0700 Subject: [PATCH 04/14] Base class --- gtsam/geometry/FundamentalMatrix.h | 70 ++++++++++++++----- .../geometry/tests/testFundamentalMatrix.cpp | 16 ++--- 2 files changed, 61 insertions(+), 25 deletions(-) diff --git a/gtsam/geometry/FundamentalMatrix.h b/gtsam/geometry/FundamentalMatrix.h index b76f956867..8140e3921b 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()) {} - - /// Construct from U, V, and scalar s - FundamentalMatrix(const Rot3& U, double s, const Rot3& V) + GeneralFundamentalMatrix() : U_(Rot3()), s_(1.0), V_(Rot3()) {} + + /** + * @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 c69edaec1c..0c132361c6 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)); } From 32980f3e09d0d5c18df073b5c6d0fdda30a97f6c Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Wed, 23 Oct 2024 18:04:06 -0700 Subject: [PATCH 05/14] Conversion --- gtsam/geometry/FundamentalMatrix.h | 71 ++++++++++- .../geometry/tests/testFundamentalMatrix.cpp | 117 +++++++++++++++--- 2 files changed, 166 insertions(+), 22 deletions(-) diff --git a/gtsam/geometry/FundamentalMatrix.h b/gtsam/geometry/FundamentalMatrix.h index 8140e3921b..1080cd87a8 100644 --- a/gtsam/geometry/FundamentalMatrix.h +++ b/gtsam/geometry/FundamentalMatrix.h @@ -62,9 +62,58 @@ class GeneralFundamentalMatrix : public FundamentalMatrix { GeneralFundamentalMatrix(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 + * matrix and ensuring U and V are not reflections. + * + * @param F A 3x3 matrix representing the fundamental matrix + */ + GeneralFundamentalMatrix(const Matrix3& F) { + // Perform SVD + Eigen::JacobiSVD svd(F, Eigen::ComputeFullU | Eigen::ComputeFullV); + + // Extract U and V + Matrix3 U = svd.matrixU(); + Matrix3 V = svd.matrixV(); + Vector3 singularValues = svd.singularValues(); + + // Scale the singular values + double scale = singularValues(0); + if (scale != 0) { + singularValues /= scale; // Normalize the first singular value to 1.0 + } + + // Check if the third singular value is close to zero (valid F condition) + if (std::abs(singularValues(2)) > 1e-9) { + throw std::invalid_argument( + "The input matrix does not represent a valid fundamental matrix."); + } + + // Ensure the second singular value is recorded as s + s_ = singularValues(1); + + // Check if U is a reflection + if (U.determinant() < 0) { + U = -U; + s_ = -s_; // Change sign of scalar if U is a reflection + } + + // Check if V is a reflection + if (V.determinant() < 0) { + V = -V; + s_ = -s_; // Change sign of scalar if U is a reflection + } + + // Assign the rotations + U_ = Rot3(U); + V_ = Rot3(V); + } + /// Return the fundamental matrix representation Matrix3 matrix() const override { - return U_.matrix() * Vector3(1, s_, 1).asDiagonal() * + return U_.matrix() * Vector3(1, s_, 0).asDiagonal() * V_.transpose().matrix(); } @@ -141,14 +190,24 @@ class SimpleFundamentalMatrix : FundamentalMatrix { const Point2& cb = Point2(0.0, 0.0)) : E_(E), fa_(fa), fb_(fb), ca_(ca), cb_(cb) {} + /// Return the left calibration matrix + Matrix3 leftK() const { + Matrix3 K; + K << fa_, 0, ca_.x(), 0, fa_, ca_.y(), 0, 0, 1; + return K; + } + + /// Return the right calibration matrix + Matrix3 rightK() const { + Matrix3 K; + K << fb_, 0, cb_.x(), 0, fb_, cb_.y(), 0, 0, 1; + return K; + } + /// Return the fundamental matrix representation 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 - return Ka * E_.matrix() * Kb.inverse(); + return leftK().transpose().inverse() * E_.matrix() * rightK().inverse(); } - /// @name Testable /// @{ diff --git a/gtsam/geometry/tests/testFundamentalMatrix.cpp b/gtsam/geometry/tests/testFundamentalMatrix.cpp index 0c132361c6..bab57b1482 100644 --- a/gtsam/geometry/tests/testFundamentalMatrix.cpp +++ b/gtsam/geometry/tests/testFundamentalMatrix.cpp @@ -49,37 +49,122 @@ TEST(GeneralFundamentalMatrix, RoundTrip) { } //************************************************************************* -// 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); +// Create the simplest SimpleFundamentalMatrix, a stereo pair +EssentialMatrix defaultE(Rot3(), Unit3(1, 0, 0)); +Point2 zero(0.0, 0.0); +SimpleFundamentalMatrix stereoF(defaultE, 1.0, 1.0, zero, zero); //************************************************************************* -TEST(SimpleFundamentalMatrix, localCoordinates) { +TEST(SimpleStereo, Conversion) { + GeneralFundamentalMatrix convertedF(stereoF.matrix()); + EXPECT(assert_equal(stereoF.matrix(), convertedF.matrix(), 1e-8)); +} + +//************************************************************************* +TEST(SimpleStereo, localCoordinates) { Vector expected = Z_7x1; - Vector actual = trueSimpleF.localCoordinates(trueSimpleF); + Vector actual = stereoF.localCoordinates(stereoF); EXPECT(assert_equal(expected, actual, 1e-8)); } //************************************************************************* -TEST(SimpleFundamentalMatrix, retract) { - SimpleFundamentalMatrix actual = trueSimpleF.retract(Z_9x1); - EXPECT(assert_equal(trueSimpleF, actual)); +TEST(SimpleStereo, retract) { + SimpleFundamentalMatrix actual = stereoF.retract(Z_9x1); + EXPECT(assert_equal(stereoF, actual)); } //************************************************************************* -TEST(SimpleFundamentalMatrix, RoundTrip) { +TEST(SimpleStereo, 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); + SimpleFundamentalMatrix hx = stereoF.retract(d); + Vector actual = stereoF.localCoordinates(hx); EXPECT(assert_equal(d, actual, 1e-8)); } +//************************************************************************* +TEST(SimpleStereo, EpipolarLine) { + // Create a point in b + Point3 p_b(0, 2, 1); + // Convert the point to a horizontal line in a + Vector3 l_a = stereoF.matrix() * p_b; + // Check if the line is horizontal at height 2 + EXPECT(assert_equal(Vector3(0, -1, 2), l_a)); +} + +//************************************************************************* +// Create a stereo pair, but in pixels not normalized coordinates. +// We're still using zero principal points here. +double fa = 1000; +double fb = 1000; +SimpleFundamentalMatrix pixelStereo(defaultE, fa, fb, zero, zero); + +//************************************************************************* +TEST(PixelStereo, Conversion) { + auto expected = pixelStereo.matrix(); + + GeneralFundamentalMatrix convertedF(pixelStereo.matrix()); + + // Check equality of F-matrices up to a scale + auto actual = convertedF.matrix(); + actual *= expected(1, 2) / actual(1, 2); + EXPECT(assert_equal(expected, actual, 1e-5)); +} + +//************************************************************************* +TEST(PixelStereo, PointInBToHorizontalLineInA) { + // Create a point in b + Point3 p_b = Point3(0, 300, 1); + // Convert the point to a horizontal line in a + Vector3 l_a = pixelStereo.matrix() * p_b; + // Check if the line is horizontal at height 2 + EXPECT(assert_equal(Vector3(0, -0.001, 0.3), l_a)); +} + +//************************************************************************* +// Create a stereo pair with the right camera rotated 90 degrees +Rot3 aRb = Rot3::Rz(M_PI_2); // Rotate 90 degrees around the Z-axis +EssentialMatrix rotatedE(aRb, Unit3(1, 0, 0)); +SimpleFundamentalMatrix rotatedPixelStereo(rotatedE, fa, fb, zero, zero); + +//************************************************************************* +TEST(RotatedPixelStereo, Conversion) { + auto expected = rotatedPixelStereo.matrix(); + + GeneralFundamentalMatrix convertedF(rotatedPixelStereo.matrix()); + + // Check equality of F-matrices up to a scale + auto actual = convertedF.matrix(); + actual *= expected(1, 2) / actual(1, 2); + EXPECT(assert_equal(expected, actual, 1e-4)); +} + +//************************************************************************* +TEST(RotatedPixelStereo, PointInBToHorizontalLineInA) { + // Create a point in b + Point3 p_b = Point3(300, 0, 1); + // Convert the point to a horizontal line in a + Vector3 l_a = rotatedPixelStereo.matrix() * p_b; + // Check if the line is horizontal at height 2 + EXPECT(assert_equal(Vector3(0, -0.001, 0.3), l_a)); +} + +//************************************************************************* +// Now check that principal points also survive conversion +SimpleFundamentalMatrix stereoWithPrincipalPoints(rotatedE, fa, fb, zero, zero); + +//************************************************************************* +TEST(stereoWithPrincipalPoints, Conversion) { + auto expected = stereoWithPrincipalPoints.matrix(); + + GeneralFundamentalMatrix convertedF(stereoWithPrincipalPoints.matrix()); + + // Check equality of F-matrices up to a scale + auto actual = convertedF.matrix(); + actual *= expected(1, 2) / actual(1, 2); + EXPECT(assert_equal(expected, actual, 1e-4)); +} + //************************************************************************* int main() { TestResult tr; From 3036ed80677c0114c757792cd0639c6cdde41d25 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Wed, 23 Oct 2024 21:07:22 -0700 Subject: [PATCH 06/14] Verify triplet epipolar transfer --- gtsam/geometry/FundamentalMatrix.h | 5 +- .../geometry/tests/testFundamentalMatrix.cpp | 79 +++++++++++++++++-- 2 files changed, 76 insertions(+), 8 deletions(-) diff --git a/gtsam/geometry/FundamentalMatrix.h b/gtsam/geometry/FundamentalMatrix.h index 1080cd87a8..acd7130206 100644 --- a/gtsam/geometry/FundamentalMatrix.h +++ b/gtsam/geometry/FundamentalMatrix.h @@ -213,9 +213,10 @@ class SimpleFundamentalMatrix : FundamentalMatrix { /// Print the SimpleFundamentalMatrix void print(const std::string& s = "") const { - std::cout << s << "E:\n" + std::cout << s << " E:\n" << E_.matrix() << "\nfa: " << fa_ << "\nfb: " << fb_ - << "\nca: " << ca_ << "\ncb: " << cb_ << std::endl; + << "\nca: " << ca_.transpose() << "\ncb: " << cb_.transpose() + << std::endl; } /// Check equality within a tolerance diff --git a/gtsam/geometry/tests/testFundamentalMatrix.cpp b/gtsam/geometry/tests/testFundamentalMatrix.cpp index bab57b1482..40a0b47677 100644 --- a/gtsam/geometry/tests/testFundamentalMatrix.cpp +++ b/gtsam/geometry/tests/testFundamentalMatrix.cpp @@ -10,6 +10,7 @@ #include #include #include +#include #include using namespace std::placeholders; @@ -95,9 +96,9 @@ TEST(SimpleStereo, EpipolarLine) { //************************************************************************* // Create a stereo pair, but in pixels not normalized coordinates. // We're still using zero principal points here. -double fa = 1000; -double fb = 1000; -SimpleFundamentalMatrix pixelStereo(defaultE, fa, fb, zero, zero); +double focalLength = 1000; +SimpleFundamentalMatrix pixelStereo(defaultE, focalLength, focalLength, zero, + zero); //************************************************************************* TEST(PixelStereo, Conversion) { @@ -125,7 +126,8 @@ TEST(PixelStereo, PointInBToHorizontalLineInA) { // Create a stereo pair with the right camera rotated 90 degrees Rot3 aRb = Rot3::Rz(M_PI_2); // Rotate 90 degrees around the Z-axis EssentialMatrix rotatedE(aRb, Unit3(1, 0, 0)); -SimpleFundamentalMatrix rotatedPixelStereo(rotatedE, fa, fb, zero, zero); +SimpleFundamentalMatrix rotatedPixelStereo(rotatedE, focalLength, focalLength, + zero, zero); //************************************************************************* TEST(RotatedPixelStereo, Conversion) { @@ -151,7 +153,10 @@ TEST(RotatedPixelStereo, PointInBToHorizontalLineInA) { //************************************************************************* // Now check that principal points also survive conversion -SimpleFundamentalMatrix stereoWithPrincipalPoints(rotatedE, fa, fb, zero, zero); +Point2 principalPoint(640 / 2, 480 / 2); +SimpleFundamentalMatrix stereoWithPrincipalPoints(rotatedE, focalLength, + focalLength, principalPoint, + principalPoint); //************************************************************************* TEST(stereoWithPrincipalPoints, Conversion) { @@ -165,9 +170,71 @@ TEST(stereoWithPrincipalPoints, Conversion) { EXPECT(assert_equal(expected, actual, 1e-4)); } +//************************************************************************* +std::array generateCameraPoses() { + std::array cameraPoses; + const double radius = 1.0; + for (int i = 0; i < 3; ++i) { + double angle = i * 2.0 * M_PI / 3.0; + double c = cos(angle), s = sin(angle); + Rot3 aRb({-s, c, 0}, {0, 0, -1}, {-c, -s, 0}); + cameraPoses[i] = {aRb, Point3(radius * c, radius * s, 0)}; + } + return cameraPoses; +} + +std::tuple +generateFs(const std::array &cameraPoses) { + std::array F; + for (size_t i = 0; i < 3; ++i) { + size_t j = (i + 1) % 3; + const Pose3 iPj = cameraPoses[i].between(cameraPoses[j]); + EssentialMatrix E(iPj.rotation(), Unit3(iPj.translation())); + F[i] = {E, focalLength, focalLength, principalPoint, principalPoint}; + } + return {F[0], F[1], F[2]}; +} + +//************************************************************************* +TEST(Triplet, Transfer) { + // Generate cameras on a circle, as well as fundamental matrices + auto cameraPoses = generateCameraPoses(); + auto [F01, F12, F20] = generateFs(cameraPoses); + + // Check that they are all equal + EXPECT(F01.equals(F12, 1e-9)); + EXPECT(F12.equals(F20, 1e-9)); + EXPECT(F20.equals(F01, 1e-9)); + + // Now project a point into the three cameras + const Point3 P(0.1, 0.2, 0.3); + const Cal3_S2 K(focalLength, focalLength, 0.0, // + principalPoint.x(), principalPoint.y()); + + std::array p; + for (size_t i = 0; i < 3; ++i) { + // Project the point into each camera + PinholeCameraCal3_S2 camera(cameraPoses[i], K); + p[i] = camera.project(P); + } + + // Create lines in camera 0 from projections 1 and 2 + Vector3 line1 = F01.matrix() * Vector3(p[1].x(), p[1].y(), 1); + Vector3 line2 = F20.matrix().transpose() * Vector3(p[2].x(), p[2].y(), 1); + + // Cross the lines to find the intersection point + Vector3 intersectionPoint = line1.cross(line2); + + // Normalize the intersection point + intersectionPoint /= intersectionPoint(2); + + // Compare the intersection point with the original projection in camera 0 + EXPECT(assert_equal(p[0], intersectionPoint.head<2>(), 1e-9)); +} //************************************************************************* int main() { TestResult tr; return TestRegistry::runAllTests(tr); } -/* ************************************************************************* */ +//************************************************************************* From 22c6b854f7e32bea59d6fac36b7f6997f282dc90 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Wed, 23 Oct 2024 22:56:07 -0700 Subject: [PATCH 07/14] TripleF and transfer --- gtsam/geometry/FundamentalMatrix.h | 60 +++++++++++++++++++ .../geometry/tests/testFundamentalMatrix.cpp | 37 +++++------- 2 files changed, 76 insertions(+), 21 deletions(-) diff --git a/gtsam/geometry/FundamentalMatrix.h b/gtsam/geometry/FundamentalMatrix.h index acd7130206..32acb3be02 100644 --- a/gtsam/geometry/FundamentalMatrix.h +++ b/gtsam/geometry/FundamentalMatrix.h @@ -29,6 +29,66 @@ class FundamentalMatrix { * @return A 3x3 matrix representing the fundamental matrix */ virtual Matrix3 matrix() const = 0; + + /** + * @brief Virtual destructor to ensure proper cleanup of derived classes + */ + virtual ~FundamentalMatrix() {} + + /** + * @brief Transfer projections from cameras 1 and 2 to camera 0 + * + * Take two fundamental matrices F01 and F02, and two points p1 and p2, and + * returns the 2D point in camera 0 where the epipolar lines intersect. + */ + static Point2 transfer(const Matrix3& F01, const Point2& p1, + const Matrix3& F02, const Point2& p2) { + // Create lines in camera 0 from projections of the other two cameras + Vector3 line1 = F01 * Vector3(p1.x(), p1.y(), 1); + Vector3 line2 = F02 * Vector3(p2.x(), p2.y(), 1); + + // Cross the lines to find the intersection point + Vector3 intersectionPoint = line1.cross(line2); + + // Normalize the intersection point + intersectionPoint /= intersectionPoint(2); + + return intersectionPoint.head<2>(); // Return the 2D point + } +}; + +/// Represents a set of three fundamental matrices for transferring points +/// between three cameras. +template +struct TripleF { + F F01, F12, F20; + + /// Transfers a point from two cameras to another. + template + Point2 transfer(const Point2& point1, const Point2& point2) { + static_assert(Index < 3, "Index must be less than 3"); + } + + /// Specialization for transferring a point from cameras 1,2 to camera 0. + template <> + Point2 transfer<0>(const Point2& p1, const Point2& p2) { + return FundamentalMatrix::transfer(F01.matrix(), p1, + F20.matrix().transpose(), p2); + } + + /// Specialization for transferring a point from camera 0,2 to camera 1. + template <> + Point2 transfer<1>(const Point2& p0, const Point2& p2) { + return FundamentalMatrix::transfer(F01.matrix().transpose(), p0, + F12.matrix(), p2); + } + + /// Specialization for transferring a point from camera 0,1 to camera 2. + template <> + Point2 transfer<2>(const Point2& p0, const Point2& p1) { + return FundamentalMatrix::transfer(F01.matrix(), p0, + F12.matrix().transpose(), p1); + } }; /** diff --git a/gtsam/geometry/tests/testFundamentalMatrix.cpp b/gtsam/geometry/tests/testFundamentalMatrix.cpp index 40a0b47677..f5a5789117 100644 --- a/gtsam/geometry/tests/testFundamentalMatrix.cpp +++ b/gtsam/geometry/tests/testFundamentalMatrix.cpp @@ -171,6 +171,7 @@ TEST(stereoWithPrincipalPoints, Conversion) { } //************************************************************************* +/// Generate three cameras on a circle, looking in std::array generateCameraPoses() { std::array cameraPoses; const double radius = 1.0; @@ -183,9 +184,10 @@ std::array generateCameraPoses() { return cameraPoses; } -std::tuple -generateFs(const std::array &cameraPoses) { +//************************************************************************* +/// Function to generate a TripleF from camera poses +TripleF generateTripleF( + const std::array& cameraPoses) { std::array F; for (size_t i = 0; i < 3; ++i) { size_t j = (i + 1) % 3; @@ -193,19 +195,19 @@ generateFs(const std::array &cameraPoses) { EssentialMatrix E(iPj.rotation(), Unit3(iPj.translation())); F[i] = {E, focalLength, focalLength, principalPoint, principalPoint}; } - return {F[0], F[1], F[2]}; + return {F[0], F[1], F[2]}; // Return a TripleF instance } //************************************************************************* -TEST(Triplet, Transfer) { +TEST(TripleF, Transfer) { // Generate cameras on a circle, as well as fundamental matrices auto cameraPoses = generateCameraPoses(); - auto [F01, F12, F20] = generateFs(cameraPoses); + auto triplet = generateTripleF(cameraPoses); // Check that they are all equal - EXPECT(F01.equals(F12, 1e-9)); - EXPECT(F12.equals(F20, 1e-9)); - EXPECT(F20.equals(F01, 1e-9)); + EXPECT(triplet.F01.equals(triplet.F12, 1e-9)); + EXPECT(triplet.F12.equals(triplet.F20, 1e-9)); + EXPECT(triplet.F20.equals(triplet.F01, 1e-9)); // Now project a point into the three cameras const Point3 P(0.1, 0.2, 0.3); @@ -219,19 +221,12 @@ TEST(Triplet, Transfer) { p[i] = camera.project(P); } - // Create lines in camera 0 from projections 1 and 2 - Vector3 line1 = F01.matrix() * Vector3(p[1].x(), p[1].y(), 1); - Vector3 line2 = F20.matrix().transpose() * Vector3(p[2].x(), p[2].y(), 1); - - // Cross the lines to find the intersection point - Vector3 intersectionPoint = line1.cross(line2); - - // Normalize the intersection point - intersectionPoint /= intersectionPoint(2); - - // Compare the intersection point with the original projection in camera 0 - EXPECT(assert_equal(p[0], intersectionPoint.head<2>(), 1e-9)); + // Check that transfer works + EXPECT(assert_equal(p[0], triplet.transfer<0>(p[1], p[2]), 1e-9)); + EXPECT(assert_equal(p[1], triplet.transfer<1>(p[0], p[2]), 1e-9)); + EXPECT(assert_equal(p[2], triplet.transfer<2>(p[0], p[1]), 1e-9)); } + //************************************************************************* int main() { TestResult tr; From 984232defb6971fb1e6ad365c59ef6670f59de0c Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Wed, 23 Oct 2024 23:21:02 -0700 Subject: [PATCH 08/14] Kill templates --- gtsam/geometry/FundamentalMatrix.h | 21 ++++++------------- .../geometry/tests/testFundamentalMatrix.cpp | 6 +++--- 2 files changed, 9 insertions(+), 18 deletions(-) diff --git a/gtsam/geometry/FundamentalMatrix.h b/gtsam/geometry/FundamentalMatrix.h index 32acb3be02..8e952c214d 100644 --- a/gtsam/geometry/FundamentalMatrix.h +++ b/gtsam/geometry/FundamentalMatrix.h @@ -63,29 +63,20 @@ template struct TripleF { F F01, F12, F20; - /// Transfers a point from two cameras to another. - template - Point2 transfer(const Point2& point1, const Point2& point2) { - static_assert(Index < 3, "Index must be less than 3"); - } - - /// Specialization for transferring a point from cameras 1,2 to camera 0. - template <> - Point2 transfer<0>(const Point2& p1, const Point2& p2) { + /// Transfers a point from cameras 1,2 to camera 0. + Point2 transfer0(const Point2& p1, const Point2& p2) { return FundamentalMatrix::transfer(F01.matrix(), p1, F20.matrix().transpose(), p2); } - /// Specialization for transferring a point from camera 0,2 to camera 1. - template <> - Point2 transfer<1>(const Point2& p0, const Point2& p2) { + /// Transfers a point from camera 0,2 to camera 1. + Point2 transfer1(const Point2& p0, const Point2& p2) { return FundamentalMatrix::transfer(F01.matrix().transpose(), p0, F12.matrix(), p2); } - /// Specialization for transferring a point from camera 0,1 to camera 2. - template <> - Point2 transfer<2>(const Point2& p0, const Point2& p1) { + /// Transfers a point from camera 0,1 to camera 2. + Point2 transfer2(const Point2& p0, const Point2& p1) { return FundamentalMatrix::transfer(F01.matrix(), p0, F12.matrix().transpose(), p1); } diff --git a/gtsam/geometry/tests/testFundamentalMatrix.cpp b/gtsam/geometry/tests/testFundamentalMatrix.cpp index f5a5789117..760b3b99c8 100644 --- a/gtsam/geometry/tests/testFundamentalMatrix.cpp +++ b/gtsam/geometry/tests/testFundamentalMatrix.cpp @@ -222,9 +222,9 @@ TEST(TripleF, Transfer) { } // Check that transfer works - EXPECT(assert_equal(p[0], triplet.transfer<0>(p[1], p[2]), 1e-9)); - EXPECT(assert_equal(p[1], triplet.transfer<1>(p[0], p[2]), 1e-9)); - EXPECT(assert_equal(p[2], triplet.transfer<2>(p[0], p[1]), 1e-9)); + EXPECT(assert_equal(p[0], triplet.transfer0(p[1], p[2]), 1e-9)); + EXPECT(assert_equal(p[1], triplet.transfer1(p[0], p[2]), 1e-9)); + EXPECT(assert_equal(p[2], triplet.transfer2(p[0], p[1]), 1e-9)); } //************************************************************************* From 82224a611bf9082c8c278bfdf75f2f484943fbfd Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Wed, 23 Oct 2024 23:36:10 -0700 Subject: [PATCH 09/14] cpp file --- gtsam/geometry/FundamentalMatrix.cpp | 133 +++++++++++++++++++++++++++ gtsam/geometry/FundamentalMatrix.h | 126 ++++--------------------- 2 files changed, 149 insertions(+), 110 deletions(-) create mode 100644 gtsam/geometry/FundamentalMatrix.cpp diff --git a/gtsam/geometry/FundamentalMatrix.cpp b/gtsam/geometry/FundamentalMatrix.cpp new file mode 100644 index 0000000000..c25082fc07 --- /dev/null +++ b/gtsam/geometry/FundamentalMatrix.cpp @@ -0,0 +1,133 @@ +/* + * @file FundamentalMatrix.cpp + * @brief FundamentalMatrix classes + * @author Frank Dellaert + * @date Oct 23, 2024 + */ + +#include + +namespace gtsam { + +GeneralFundamentalMatrix::GeneralFundamentalMatrix(const Matrix3& F) { + // Perform SVD + Eigen::JacobiSVD svd(F, Eigen::ComputeFullU | Eigen::ComputeFullV); + + // Extract U and V + Matrix3 U = svd.matrixU(); + Matrix3 V = svd.matrixV(); + Vector3 singularValues = svd.singularValues(); + + // Scale the singular values + double scale = singularValues(0); + if (scale != 0) { + singularValues /= scale; // Normalize the first singular value to 1.0 + } + + // Check if the third singular value is close to zero (valid F condition) + if (std::abs(singularValues(2)) > 1e-9) { + throw std::invalid_argument( + "The input matrix does not represent a valid fundamental matrix."); + } + + // Ensure the second singular value is recorded as s + s_ = singularValues(1); + + // Check if U is a reflection + if (U.determinant() < 0) { + U = -U; + s_ = -s_; // Change sign of scalar if U is a reflection + } + + // Check if V is a reflection + if (V.determinant() < 0) { + V = -V; + s_ = -s_; // Change sign of scalar if U is a reflection + } + + // Assign the rotations + U_ = Rot3(U); + V_ = Rot3(V); +} + +Matrix3 GeneralFundamentalMatrix::matrix() const { + return U_.matrix() * Vector3(1, s_, 0).asDiagonal() * V_.transpose().matrix(); +} + +void GeneralFundamentalMatrix::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 { + 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 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; +} + +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 GeneralFundamentalMatrix(newU, newS, newV); +} + +Matrix3 SimpleFundamentalMatrix::leftK() const { + Matrix3 K; + K << fa_, 0, ca_.x(), 0, fa_, ca_.y(), 0, 0, 1; + return K; +} + +Matrix3 SimpleFundamentalMatrix::rightK() const { + Matrix3 K; + K << fb_, 0, cb_.x(), 0, fb_, cb_.y(), 0, 0, 1; + return K; +} + +Matrix3 SimpleFundamentalMatrix::matrix() const { + return leftK().transpose().inverse() * E_.matrix() * rightK().inverse(); +} + +void SimpleFundamentalMatrix::print(const std::string& s) const { + std::cout << s << " E:\n" + << E_.matrix() << "\nfa: " << fa_ << "\nfb: " << fb_ + << "\nca: " << ca_.transpose() << "\ncb: " << cb_.transpose() + << std::endl; +} + +bool SimpleFundamentalMatrix::equals(const SimpleFundamentalMatrix& other, + double tol) 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; +} + +Vector SimpleFundamentalMatrix::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; +} + +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_); +} + +} // namespace gtsam diff --git a/gtsam/geometry/FundamentalMatrix.h b/gtsam/geometry/FundamentalMatrix.h index 8e952c214d..322d47193e 100644 --- a/gtsam/geometry/FundamentalMatrix.h +++ b/gtsam/geometry/FundamentalMatrix.h @@ -121,70 +121,19 @@ class GeneralFundamentalMatrix : public FundamentalMatrix { * * @param F A 3x3 matrix representing the fundamental matrix */ - GeneralFundamentalMatrix(const Matrix3& F) { - // Perform SVD - Eigen::JacobiSVD svd(F, Eigen::ComputeFullU | Eigen::ComputeFullV); - - // Extract U and V - Matrix3 U = svd.matrixU(); - Matrix3 V = svd.matrixV(); - Vector3 singularValues = svd.singularValues(); - - // Scale the singular values - double scale = singularValues(0); - if (scale != 0) { - singularValues /= scale; // Normalize the first singular value to 1.0 - } - - // Check if the third singular value is close to zero (valid F condition) - if (std::abs(singularValues(2)) > 1e-9) { - throw std::invalid_argument( - "The input matrix does not represent a valid fundamental matrix."); - } - - // Ensure the second singular value is recorded as s - s_ = singularValues(1); - - // Check if U is a reflection - if (U.determinant() < 0) { - U = -U; - s_ = -s_; // Change sign of scalar if U is a reflection - } - - // Check if V is a reflection - if (V.determinant() < 0) { - V = -V; - s_ = -s_; // Change sign of scalar if U is a reflection - } - - // Assign the rotations - U_ = Rot3(U); - V_ = Rot3(V); - } + GeneralFundamentalMatrix(const Matrix3& F); /// Return the fundamental matrix representation - Matrix3 matrix() const override { - return U_.matrix() * Vector3(1, s_, 0).asDiagonal() * - V_.transpose().matrix(); - } + Matrix3 matrix() const override; /// @name Testable /// @{ - /// 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; - } + void print(const std::string& s = "") 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); - } - + bool equals(const GeneralFundamentalMatrix& other, double tol = 1e-9) const; /// @} /// @name Manifold @@ -194,22 +143,10 @@ class GeneralFundamentalMatrix : public FundamentalMatrix { inline size_t dim() const { return dimension; } /// 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 - result.tail<3>() = V_.localCoordinates(F.V_); - return result; - } + Vector localCoordinates(const GeneralFundamentalMatrix& F) 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 GeneralFundamentalMatrix(newU, newS, newV); - } - + GeneralFundamentalMatrix retract(const Vector& delta) const; /// @} }; @@ -221,7 +158,7 @@ class GeneralFundamentalMatrix : public 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 : FundamentalMatrix { +class SimpleFundamentalMatrix : public FundamentalMatrix { private: EssentialMatrix E_; ///< Essential matrix double fa_; ///< Focal length for left camera @@ -242,40 +179,21 @@ class SimpleFundamentalMatrix : FundamentalMatrix { : E_(E), fa_(fa), fb_(fb), ca_(ca), cb_(cb) {} /// Return the left calibration matrix - Matrix3 leftK() const { - Matrix3 K; - K << fa_, 0, ca_.x(), 0, fa_, ca_.y(), 0, 0, 1; - return K; - } + Matrix3 leftK() const; /// Return the right calibration matrix - Matrix3 rightK() const { - Matrix3 K; - K << fb_, 0, cb_.x(), 0, fb_, cb_.y(), 0, 0, 1; - return K; - } + Matrix3 rightK() const; /// Return the fundamental matrix representation - Matrix3 matrix() const override { - return leftK().transpose().inverse() * E_.matrix() * rightK().inverse(); - } + Matrix3 matrix() const override; + /// @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_.transpose() << "\ncb: " << cb_.transpose() - << std::endl; - } + void print(const std::string& s = "") const; /// 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; - } + bool equals(const SimpleFundamentalMatrix& other, double tol = 1e-9) const; /// @} /// @name Manifold @@ -284,23 +202,11 @@ class SimpleFundamentalMatrix : FundamentalMatrix { 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; - } + /// Return local coordinates with respect to another SimpleFundamentalMatrix + Vector localCoordinates(const SimpleFundamentalMatrix& F) const; /// 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_); - } + SimpleFundamentalMatrix retract(const Vector& delta) const; /// @} }; From 0431299df6ebd50c92b3440f468cced2b0435ea4 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Wed, 23 Oct 2024 23:59:32 -0700 Subject: [PATCH 10/14] Move transfer to cpp --- gtsam/geometry/FundamentalMatrix.cpp | 20 ++++++++++++++++++++ gtsam/geometry/FundamentalMatrix.h | 14 +------------- 2 files changed, 21 insertions(+), 13 deletions(-) diff --git a/gtsam/geometry/FundamentalMatrix.cpp b/gtsam/geometry/FundamentalMatrix.cpp index c25082fc07..e56d9feb38 100644 --- a/gtsam/geometry/FundamentalMatrix.cpp +++ b/gtsam/geometry/FundamentalMatrix.cpp @@ -9,6 +9,23 @@ namespace gtsam { +//************************************************************************* +Point2 FundamentalMatrix::transfer(const Matrix3& F01, const Point2& p1, + const Matrix3& F02, const Point2& p2) { + // Create lines in camera 0 from projections of the other two cameras + Vector3 line1 = F01 * Vector3(p1.x(), p1.y(), 1); + Vector3 line2 = F02 * Vector3(p2.x(), p2.y(), 1); + + // Cross the lines to find the intersection point + Vector3 intersectionPoint = line1.cross(line2); + + // Normalize the intersection point + intersectionPoint /= intersectionPoint(2); + + return intersectionPoint.head<2>(); // Return the 2D point +} + +//************************************************************************* GeneralFundamentalMatrix::GeneralFundamentalMatrix(const Matrix3& F) { // Perform SVD Eigen::JacobiSVD svd(F, Eigen::ComputeFullU | Eigen::ComputeFullV); @@ -83,6 +100,7 @@ GeneralFundamentalMatrix GeneralFundamentalMatrix::retract( return GeneralFundamentalMatrix(newU, newS, newV); } +//************************************************************************* Matrix3 SimpleFundamentalMatrix::leftK() const { Matrix3 K; K << fa_, 0, ca_.x(), 0, fa_, ca_.y(), 0, 0, 1; @@ -130,4 +148,6 @@ SimpleFundamentalMatrix SimpleFundamentalMatrix::retract( return SimpleFundamentalMatrix(newE, newFa, newFb, ca_, cb_); } +//************************************************************************* + } // namespace gtsam diff --git a/gtsam/geometry/FundamentalMatrix.h b/gtsam/geometry/FundamentalMatrix.h index 322d47193e..83277d27ef 100644 --- a/gtsam/geometry/FundamentalMatrix.h +++ b/gtsam/geometry/FundamentalMatrix.h @@ -42,19 +42,7 @@ class FundamentalMatrix { * returns the 2D point in camera 0 where the epipolar lines intersect. */ static Point2 transfer(const Matrix3& F01, const Point2& p1, - const Matrix3& F02, const Point2& p2) { - // Create lines in camera 0 from projections of the other two cameras - Vector3 line1 = F01 * Vector3(p1.x(), p1.y(), 1); - Vector3 line2 = F02 * Vector3(p2.x(), p2.y(), 1); - - // Cross the lines to find the intersection point - Vector3 intersectionPoint = line1.cross(line2); - - // Normalize the intersection point - intersectionPoint /= intersectionPoint(2); - - return intersectionPoint.head<2>(); // Return the 2D point - } + const Matrix3& F02, const Point2& p2); }; /// Represents a set of three fundamental matrices for transferring points From dc23c02be80a744a590242809cc1438010a35d83 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Thu, 24 Oct 2024 00:41:17 -0700 Subject: [PATCH 11/14] Fix transfer2, add export --- gtsam/geometry/FundamentalMatrix.h | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/gtsam/geometry/FundamentalMatrix.h b/gtsam/geometry/FundamentalMatrix.h index 83277d27ef..bf89a4bb8c 100644 --- a/gtsam/geometry/FundamentalMatrix.h +++ b/gtsam/geometry/FundamentalMatrix.h @@ -21,7 +21,7 @@ namespace gtsam { * classes. The `matrix()` function returns a 3x3 matrix representation of the * fundamental matrix. */ -class FundamentalMatrix { +class GTSAM_EXPORT FundamentalMatrix { public: /** * @brief Returns a 3x3 matrix representation of the fundamental matrix @@ -65,7 +65,7 @@ struct TripleF { /// Transfers a point from camera 0,1 to camera 2. Point2 transfer2(const Point2& p0, const Point2& p1) { - return FundamentalMatrix::transfer(F01.matrix(), p0, + return FundamentalMatrix::transfer(F20.matrix(), p0, F12.matrix().transpose(), p1); } }; @@ -78,7 +78,7 @@ struct TripleF { * 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 { +class GTSAM_EXPORT GeneralFundamentalMatrix : public FundamentalMatrix { private: Rot3 U_; ///< Left rotation double s_; ///< Scalar parameter for S @@ -146,7 +146,7 @@ class GeneralFundamentalMatrix : public 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 : public FundamentalMatrix { +class GTSAM_EXPORT SimpleFundamentalMatrix : public FundamentalMatrix { private: EssentialMatrix E_; ///< Essential matrix double fa_; ///< Focal length for left camera From 5b413b84c95f34313f3331f04db6881f97fd5732 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Thu, 24 Oct 2024 15:13:45 -0700 Subject: [PATCH 12/14] Remove inheritance, use abc language --- gtsam/geometry/FundamentalMatrix.cpp | 13 ++- gtsam/geometry/FundamentalMatrix.h | 96 +++++++------------ .../geometry/tests/testFundamentalMatrix.cpp | 13 ++- 3 files changed, 47 insertions(+), 75 deletions(-) diff --git a/gtsam/geometry/FundamentalMatrix.cpp b/gtsam/geometry/FundamentalMatrix.cpp index e56d9feb38..b16a3fef32 100644 --- a/gtsam/geometry/FundamentalMatrix.cpp +++ b/gtsam/geometry/FundamentalMatrix.cpp @@ -10,21 +10,20 @@ namespace gtsam { //************************************************************************* -Point2 FundamentalMatrix::transfer(const Matrix3& F01, const Point2& p1, - const Matrix3& F02, const Point2& p2) { - // Create lines in camera 0 from projections of the other two cameras - Vector3 line1 = F01 * Vector3(p1.x(), p1.y(), 1); - Vector3 line2 = F02 * Vector3(p2.x(), p2.y(), 1); +Point2 Transfer(const Matrix3& Fca, const Point2& pa, // + const Matrix3& Fcb, const Point2& pb) { + // Create lines in camera a from projections of the other two cameras + Vector3 line_a = Fca * Vector3(pa.x(), pa.y(), 1); + Vector3 line_b = Fcb * Vector3(pb.x(), pb.y(), 1); // Cross the lines to find the intersection point - Vector3 intersectionPoint = line1.cross(line2); + Vector3 intersectionPoint = line_a.cross(line_b); // Normalize the intersection point intersectionPoint /= intersectionPoint(2); return intersectionPoint.head<2>(); // Return the 2D point } - //************************************************************************* GeneralFundamentalMatrix::GeneralFundamentalMatrix(const Matrix3& F) { // Perform SVD diff --git a/gtsam/geometry/FundamentalMatrix.h b/gtsam/geometry/FundamentalMatrix.h index bf89a4bb8c..9eba076a86 100644 --- a/gtsam/geometry/FundamentalMatrix.h +++ b/gtsam/geometry/FundamentalMatrix.h @@ -13,63 +13,6 @@ 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 GTSAM_EXPORT 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; - - /** - * @brief Virtual destructor to ensure proper cleanup of derived classes - */ - virtual ~FundamentalMatrix() {} - - /** - * @brief Transfer projections from cameras 1 and 2 to camera 0 - * - * Take two fundamental matrices F01 and F02, and two points p1 and p2, and - * returns the 2D point in camera 0 where the epipolar lines intersect. - */ - static Point2 transfer(const Matrix3& F01, const Point2& p1, - const Matrix3& F02, const Point2& p2); -}; - -/// Represents a set of three fundamental matrices for transferring points -/// between three cameras. -template -struct TripleF { - F F01, F12, F20; - - /// Transfers a point from cameras 1,2 to camera 0. - Point2 transfer0(const Point2& p1, const Point2& p2) { - return FundamentalMatrix::transfer(F01.matrix(), p1, - F20.matrix().transpose(), p2); - } - - /// Transfers a point from camera 0,2 to camera 1. - Point2 transfer1(const Point2& p0, const Point2& p2) { - return FundamentalMatrix::transfer(F01.matrix().transpose(), p0, - F12.matrix(), p2); - } - - /// Transfers a point from camera 0,1 to camera 2. - Point2 transfer2(const Point2& p0, const Point2& p1) { - return FundamentalMatrix::transfer(F20.matrix(), p0, - F12.matrix().transpose(), p1); - } -}; - /** * @class GeneralFundamentalMatrix * @brief Represents a general fundamental matrix. @@ -78,7 +21,7 @@ struct TripleF { * 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 : public FundamentalMatrix { +class GTSAM_EXPORT GeneralFundamentalMatrix { private: Rot3 U_; ///< Left rotation double s_; ///< Scalar parameter for S @@ -112,7 +55,7 @@ class GTSAM_EXPORT GeneralFundamentalMatrix : public FundamentalMatrix { GeneralFundamentalMatrix(const Matrix3& F); /// Return the fundamental matrix representation - Matrix3 matrix() const override; + Matrix3 matrix() const; /// @name Testable /// @{ @@ -146,7 +89,7 @@ class GTSAM_EXPORT GeneralFundamentalMatrix : public 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 GTSAM_EXPORT SimpleFundamentalMatrix : public FundamentalMatrix { +class GTSAM_EXPORT SimpleFundamentalMatrix { private: EssentialMatrix E_; ///< Essential matrix double fa_; ///< Focal length for left camera @@ -173,7 +116,7 @@ class GTSAM_EXPORT SimpleFundamentalMatrix : public FundamentalMatrix { Matrix3 rightK() const; /// Return the fundamental matrix representation - Matrix3 matrix() const override; + Matrix3 matrix() const; /// @name Testable /// @{ @@ -198,6 +141,37 @@ class GTSAM_EXPORT SimpleFundamentalMatrix : public FundamentalMatrix { /// @} }; +/** + * @brief Transfer projections from cameras a and b to camera c + * + * Take two fundamental matrices Fca and Fcb, and two points pa and pb, and + * returns the 2D point in view (c) where the epipolar lines intersect. + */ +Point2 Transfer(const Matrix3& Fca, const Point2& pa, const Matrix3& Fcb, + const Point2& pb); + +/// Represents a set of three fundamental matrices for transferring points +/// between three cameras. +template +struct TripleF { + F Fab, Fbc, Fca; + + /// Transfers a point from cameras b,c to camera a. + Point2 transferToA(const Point2& pb, const Point2& pc) { + return Transfer(Fab.matrix(), pb, Fca.matrix().transpose(), pc); + } + + /// Transfers a point from camera a,c to camera b. + Point2 transferToB(const Point2& pa, const Point2& pc) { + return Transfer(Fab.matrix().transpose(), pa, Fbc.matrix(), pc); + } + + /// Transfers a point from cameras a,b to camera c. + Point2 transferToC(const Point2& pa, const Point2& pb) { + return Transfer(Fca.matrix(), pa, Fbc.matrix().transpose(), pb); + } +}; + template <> struct traits : public internal::Manifold {}; diff --git a/gtsam/geometry/tests/testFundamentalMatrix.cpp b/gtsam/geometry/tests/testFundamentalMatrix.cpp index 760b3b99c8..028db2c1f8 100644 --- a/gtsam/geometry/tests/testFundamentalMatrix.cpp +++ b/gtsam/geometry/tests/testFundamentalMatrix.cpp @@ -7,7 +7,6 @@ #include #include -#include #include #include #include @@ -205,9 +204,9 @@ TEST(TripleF, Transfer) { auto triplet = generateTripleF(cameraPoses); // Check that they are all equal - EXPECT(triplet.F01.equals(triplet.F12, 1e-9)); - EXPECT(triplet.F12.equals(triplet.F20, 1e-9)); - EXPECT(triplet.F20.equals(triplet.F01, 1e-9)); + EXPECT(triplet.Fab.equals(triplet.Fbc, 1e-9)); + EXPECT(triplet.Fbc.equals(triplet.Fca, 1e-9)); + EXPECT(triplet.Fca.equals(triplet.Fab, 1e-9)); // Now project a point into the three cameras const Point3 P(0.1, 0.2, 0.3); @@ -222,9 +221,9 @@ TEST(TripleF, Transfer) { } // Check that transfer works - EXPECT(assert_equal(p[0], triplet.transfer0(p[1], p[2]), 1e-9)); - EXPECT(assert_equal(p[1], triplet.transfer1(p[0], p[2]), 1e-9)); - EXPECT(assert_equal(p[2], triplet.transfer2(p[0], p[1]), 1e-9)); + EXPECT(assert_equal(p[0], triplet.transferToA(p[1], p[2]), 1e-9)); + EXPECT(assert_equal(p[1], triplet.transferToB(p[0], p[2]), 1e-9)); + EXPECT(assert_equal(p[2], triplet.transferToC(p[0], p[1]), 1e-9)); } //************************************************************************* From 9773fbc6294c2c2d8e9026668858b10311333603 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Thu, 24 Oct 2024 17:05:31 -0700 Subject: [PATCH 13/14] Add missing export --- gtsam/geometry/FundamentalMatrix.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/gtsam/geometry/FundamentalMatrix.h b/gtsam/geometry/FundamentalMatrix.h index 9eba076a86..ff7f4c8372 100644 --- a/gtsam/geometry/FundamentalMatrix.h +++ b/gtsam/geometry/FundamentalMatrix.h @@ -147,8 +147,8 @@ class GTSAM_EXPORT SimpleFundamentalMatrix { * Take two fundamental matrices Fca and Fcb, and two points pa and pb, and * returns the 2D point in view (c) where the epipolar lines intersect. */ -Point2 Transfer(const Matrix3& Fca, const Point2& pa, const Matrix3& Fcb, - const Point2& pb); +GTSAM_EXPORT Point2 Transfer(const Matrix3& Fca, const Point2& pa, + const Matrix3& Fcb, const Point2& pb); /// Represents a set of three fundamental matrices for transferring points /// between three cameras. From 502dcc480bff63ec1bd8bb561c7e65b021c1a800 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Thu, 24 Oct 2024 17:30:30 -0700 Subject: [PATCH 14/14] 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();