From 82224a611bf9082c8c278bfdf75f2f484943fbfd Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Wed, 23 Oct 2024 23:36:10 -0700 Subject: [PATCH] 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; /// @} };