diff --git a/gtsam/geometry/FundamentalMatrix.cpp b/gtsam/geometry/FundamentalMatrix.cpp index 406269ff57..5e8c26e627 100644 --- a/gtsam/geometry/FundamentalMatrix.cpp +++ b/gtsam/geometry/FundamentalMatrix.cpp @@ -57,32 +57,20 @@ FundamentalMatrix::FundamentalMatrix(const Matrix3& F) { initialize(U, singularValues(1), V); } -void FundamentalMatrix::initialize(const Matrix3& U, double s, - const Matrix3& V) { - s_ = s; - sign_ = 1.0; - - // Check if U is a reflection and flip U and sign_ if so - double detU = U.determinant(); // detU will be -1 or 1 - if (detU < 0) { - U_ = Rot3(-U); - sign_ = -sign_; - } else { - U_ = Rot3(U); - } +void FundamentalMatrix::initialize(Matrix3 U, double s, Matrix3 V) { + // Check if U is a reflection and flip third column if so + if (U.determinant() < 0) U.col(2) *= -1; // Same check for V - double detV = V.determinant(); - if (detV < 0) { - V_ = Rot3(-V); - sign_ = -sign_; - } else { - V_ = Rot3(V); - } + if (V.determinant() < 0) V.col(2) *= -1; + + U_ = Rot3(U); + s_ = s; + V_ = Rot3(V); } Matrix3 FundamentalMatrix::matrix() const { - return sign_ * U_.matrix() * Vector3(1.0, s_, 0).asDiagonal() * + return U_.matrix() * Vector3(1.0, s_, 0).asDiagonal() * V_.transpose().matrix(); } @@ -92,8 +80,8 @@ void FundamentalMatrix::print(const std::string& s) const { bool FundamentalMatrix::equals(const FundamentalMatrix& other, double tol) const { - return U_.equals(other.U_, tol) && sign_ == other.sign_ && - std::abs(s_ - other.s_) < tol && V_.equals(other.V_, tol); + return U_.equals(other.U_, tol) && std::abs(s_ - other.s_) < tol && + V_.equals(other.V_, tol); } Vector FundamentalMatrix::localCoordinates(const FundamentalMatrix& F) const { @@ -105,10 +93,10 @@ Vector FundamentalMatrix::localCoordinates(const FundamentalMatrix& F) 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 FundamentalMatrix(newU, sign_, newS, newV); + const Rot3 newU = U_.retract(delta.head<3>()); + const double newS = s_ + delta(3); + const Rot3 newV = V_.retract(delta.tail<3>()); + return FundamentalMatrix(newU, newS, newV); } //************************************************************************* diff --git a/gtsam/geometry/FundamentalMatrix.h b/gtsam/geometry/FundamentalMatrix.h index beb2947e2c..c114c2b5be 100644 --- a/gtsam/geometry/FundamentalMatrix.h +++ b/gtsam/geometry/FundamentalMatrix.h @@ -20,23 +20,20 @@ namespace gtsam { * * The FundamentalMatrix class encapsulates the fundamental matrix, which * relates corresponding points in stereo images. It is parameterized by two - * rotation matrices (U and V), a scalar parameter (s), and a sign. + * rotation matrices (U and V) and a scalar parameter (s). * Using these values, the fundamental matrix is represented as * - * F = sign * U * diag(1, s, 0) * V^T - * - * We need the `sign` because we use SO(3) for U and V, not O(3). + * F = U * diag(1, s, 0) * V^T */ class GTSAM_EXPORT FundamentalMatrix { private: - Rot3 U_; ///< Left rotation - double sign_; ///< Whether to flip the sign or not - double s_; ///< Scalar parameter for S - Rot3 V_; ///< Right rotation + Rot3 U_; ///< Left rotation + double s_; ///< Scalar parameter for S + Rot3 V_; ///< Right rotation public: /// Default constructor - FundamentalMatrix() : U_(Rot3()), sign_(1.0), s_(1.0), V_(Rot3()) {} + FundamentalMatrix() : U_(Rot3()), s_(1.0), V_(Rot3()) {} /** * @brief Construct from U, V, and scalar s @@ -113,11 +110,11 @@ class GTSAM_EXPORT FundamentalMatrix { /// @} private: /// Private constructor for internal use - FundamentalMatrix(const Rot3& U, double sign, double scaled_s, const Rot3& V) - : U_(U), sign_(sign), s_(scaled_s), V_(V) {} + FundamentalMatrix(const Rot3& U, double s, const Rot3& V) + : U_(U), s_(s), V_(V) {} /// Initialize SO(3) matrices from general O(3) matrices - void initialize(const Matrix3& U, double s, const Matrix3& V); + void initialize(Matrix3 U, double s, Matrix3 V); }; /**