diff --git a/gtsam/base/Manifold.h b/gtsam/base/Manifold.h index 815c8b288..cb30fa9c0 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 diff --git a/gtsam/geometry/FundamentalMatrix.cpp b/gtsam/geometry/FundamentalMatrix.cpp new file mode 100644 index 000000000..71ca47418 --- /dev/null +++ b/gtsam/geometry/FundamentalMatrix.cpp @@ -0,0 +1,150 @@ +/* + * @file FundamentalMatrix.cpp + * @brief FundamentalMatrix classes + * @author Frank Dellaert + * @date Oct 23, 2024 + */ + +#include + +namespace gtsam { + +//************************************************************************* +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 = line_a.cross(line_b); + + // Normalize the intersection point + intersectionPoint /= intersectionPoint(2); + + return intersectionPoint.head<2>(); // Return the 2D point +} +//************************************************************************* +FundamentalMatrix::FundamentalMatrix(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 FundamentalMatrix::matrix() const { + return U_.matrix() * Vector3(1, s_, 0).asDiagonal() * V_.transpose().matrix(); +} + +void FundamentalMatrix::print(const std::string& s) const { + std::cout << s << "U:\n" + << U_.matrix() << "\ns: " << s_ << "\nV:\n" + << V_.matrix() << std::endl; +} + +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 FundamentalMatrix::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; +} + +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); +} + +//************************************************************************* +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 new file mode 100644 index 000000000..718364ca0 --- /dev/null +++ b/gtsam/geometry/FundamentalMatrix.h @@ -0,0 +1,183 @@ +/* + * @file FundamentalMatrix.h + * @brief FundamentalMatrix classes + * @author Frank Dellaert + * @date Oct 23, 2024 + */ + +#pragma once + +#include +#include +#include + +namespace gtsam { + +/** + * @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 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()) {} + + /** + * @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 + */ + 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 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 + */ + FundamentalMatrix(const Matrix3& F); + + /// Return the fundamental matrix representation + Matrix3 matrix() const; + + /// @name Testable + /// @{ + /// Print the FundamentalMatrix + void print(const std::string& s = "") const; + + /// Check if the FundamentalMatrix is equal to another within a + /// tolerance + bool equals(const FundamentalMatrix& other, double tol = 1e-9) const; + /// @} + + /// @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; + + /// Retract the given vector to get a new FundamentalMatrix + FundamentalMatrix retract(const Vector& delta) const; + /// @} +}; + +/** + * @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 GTSAM_EXPORT 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 left calibration matrix + Matrix3 leftK() const; + + /// Return the right calibration matrix + Matrix3 rightK() const; + + /// Return the fundamental matrix representation + Matrix3 matrix() const; + + /// @name Testable + /// @{ + /// Print the SimpleFundamentalMatrix + void print(const std::string& s = "") const; + + /// Check equality within a tolerance + bool equals(const SimpleFundamentalMatrix& other, double tol = 1e-9) const; + /// @} + + /// @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; + + /// Retract the given vector to get a new SimpleFundamentalMatrix + SimpleFundamentalMatrix retract(const Vector& delta) const; + /// @} +}; + +/** + * @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. + */ +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. +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 {}; + +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..a8884e791 --- /dev/null +++ b/gtsam/geometry/tests/testFundamentalMatrix.cpp @@ -0,0 +1,234 @@ +/* + * @file testFundamentalMatrix.cpp + * @brief Test FundamentalMatrix classes + * @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)); +} + +//************************************************************************* +// 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(SimpleStereo, Conversion) { + FundamentalMatrix convertedF(stereoF.matrix()); + EXPECT(assert_equal(stereoF.matrix(), convertedF.matrix(), 1e-8)); +} + +//************************************************************************* +TEST(SimpleStereo, localCoordinates) { + Vector expected = Z_7x1; + Vector actual = stereoF.localCoordinates(stereoF); + EXPECT(assert_equal(expected, actual, 1e-8)); +} + +//************************************************************************* +TEST(SimpleStereo, retract) { + SimpleFundamentalMatrix actual = stereoF.retract(Z_9x1); + EXPECT(assert_equal(stereoF, actual)); +} + +//************************************************************************* +TEST(SimpleStereo, RoundTrip) { + Vector7 d; + d << 0.1, 0.2, 0.3, 0.4, 0.5, 0.6, 0.7; + 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 focalLength = 1000; +SimpleFundamentalMatrix pixelStereo(defaultE, focalLength, focalLength, zero, + zero); + +//************************************************************************* +TEST(PixelStereo, Conversion) { + auto expected = pixelStereo.matrix(); + + FundamentalMatrix 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, focalLength, focalLength, + zero, zero); + +//************************************************************************* +TEST(RotatedPixelStereo, Conversion) { + auto expected = rotatedPixelStereo.matrix(); + + FundamentalMatrix 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 +Point2 principalPoint(640 / 2, 480 / 2); +SimpleFundamentalMatrix stereoWithPrincipalPoints(rotatedE, focalLength, + focalLength, principalPoint, + principalPoint); + +//************************************************************************* +TEST(stereoWithPrincipalPoints, Conversion) { + auto expected = stereoWithPrincipalPoints.matrix(); + + FundamentalMatrix 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)); +} + +//************************************************************************* +/// Generate three cameras on a circle, looking in +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; +} + +//************************************************************************* +/// 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; + 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]}; // Return a TripleF instance +} + +//************************************************************************* +TEST(TripleF, Transfer) { + // Generate cameras on a circle, as well as fundamental matrices + auto cameraPoses = generateCameraPoses(); + auto triplet = generateTripleF(cameraPoses); + + // Check that they are all equal + 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); + 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); + } + + // Check that transfer works + 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)); +} + +//************************************************************************* +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +//*************************************************************************