Merge branch 'feature/wrapF' into feature/essential_transfer

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);
 }
 
 //*************************************************************************

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);
 };
 
 /**