Fix small issues and store scaled s_

Author: dellaert

gtsam/geometry/FundamentalMatrix.cpp

@@ -2,10 +2,12 @@
  * @file FundamentalMatrix.cpp
  * @brief FundamentalMatrix classes
  * @author Frank Dellaert
- * @date Oct 23, 2024
+ * @date October 2024
  */
 
+#include <gtsam/base/Matrix.h>
 #include <gtsam/geometry/FundamentalMatrix.h>
+#include <gtsam/geometry/Point2.h>
 
 namespace gtsam {
 
@@ -26,8 +28,8 @@ Point2 EpipolarTransfer(const Matrix3& Fca, const Point2& pa,  //
 }
 
 //*************************************************************************
-FundamentalMatrix::FundamentalMatrix(const Matrix& U, double s,
-                                     const Matrix& V) {
+FundamentalMatrix::FundamentalMatrix(const Matrix3& U, double s,
+                                     const Matrix3& V) {
   initialize(U, s, V);
 }
 
@@ -57,38 +59,30 @@ FundamentalMatrix::FundamentalMatrix(const Matrix3& F) {
 
 void FundamentalMatrix::initialize(const Matrix3& U, double s,
                                    const Matrix3& V) {
-  s_ = s;
+  s_ = s / kScale;
   sign_ = 1.0;
 
-  // Check if U is a reflection and its determinant is close to -1 or 1
-  double detU = U.determinant();
-  if (std::abs(std::abs(detU) - 1.0) > 1e-9) {
-    throw std::invalid_argument(
-        "Matrix U does not have a determinant close to -1 or 1.");
-  }
+  // 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_;  // Flip sign if U is a reflection
+    sign_ = -sign_;
   } else {
     U_ = Rot3(U);
   }
 
-  // Check if V is a reflection and its determinant is close to -1 or 1
+  // Same check for V
   double detV = V.determinant();
-  if (std::abs(std::abs(detV) - 1.0) > 1e-9) {
-    throw std::invalid_argument(
-        "Matrix V does not have a determinant close to -1 or 1.");
-  }
   if (detV < 0) {
     V_ = Rot3(-V);
-    sign_ = -sign_;  // Flip sign if V is a reflection
+    sign_ = -sign_;
   } else {
     V_ = Rot3(V);
   }
 }
 
 Matrix3 FundamentalMatrix::matrix() const {
-  return sign_ * U_.matrix() * Vector3(1.0, s_, 0).asDiagonal() *
+  return sign_ * U_.matrix() * Vector3(1.0, s_ * kScale, 0).asDiagonal() *
          V_.transpose().matrix();
 }
 

gtsam/geometry/FundamentalMatrix.h

@@ -2,7 +2,7 @@
  * @file FundamentalMatrix.h
  * @brief FundamentalMatrix classes
  * @author Frank Dellaert
- * @date Oct 23, 2024
+ * @date October 2024
  */
 
 #pragma once
@@ -34,17 +34,19 @@ class GTSAM_EXPORT FundamentalMatrix {
   double s_;     ///< Scalar parameter for S
   Rot3 V_;       ///< Right rotation
 
+  static constexpr double kScale = 1000;  // s is stored in s_ as s/kScale
+
  public:
   /// Default constructor
-  FundamentalMatrix() : U_(Rot3()), sign_(1.0), s_(1.0), V_(Rot3()) {}
+  FundamentalMatrix() : U_(Rot3()), sign_(1.0), s_(1.0 / kScale), V_(Rot3()) {}
 
   /**
    * @brief Construct from U, V, and scalar s
    *
    * Initializes the FundamentalMatrix From the SVD representation
    * U*diag(1,s,0)*V^T. It will internally convert to using SO(3).
    */
-  FundamentalMatrix(const Matrix& U, double s, const Matrix& V);
+  FundamentalMatrix(const Matrix3& U, double s, const Matrix3& V);
 
   /**
    * @brief Construct from a 3x3 matrix using SVD
@@ -60,7 +62,9 @@ class GTSAM_EXPORT FundamentalMatrix {
    * @brief Construct from essential matrix and calibration matrices
    *
    * Initializes the FundamentalMatrix from the given essential matrix E
-   * and calibration matrices Ka and Kb.
+   * and calibration matrices Ka and Kb, using
+   *   F = Ka^(-T) * E * Kb^(-1)
+   * and then calls constructor that decomposes F via SVD.
    *
    * @param E Essential matrix
    * @param Ka Calibration matrix for the left camera
@@ -111,8 +115,8 @@ class GTSAM_EXPORT FundamentalMatrix {
   /// @}
  private:
   /// Private constructor for internal use
-  FundamentalMatrix(const Rot3& U, double sign, double s, const Rot3& V)
-      : U_(U), sign_(sign), s_(s), V_(V) {}
+  FundamentalMatrix(const Rot3& U, double sign, double scaled_s, const Rot3& V)
+      : U_(U), sign_(sign), s_(scaled_s), V_(V) {}
 
   /// Initialize SO(3) matrices from general O(3) matrices
   void initialize(const Matrix3& U, double s, const Matrix3& V);

gtsam/geometry/tests/testFundamentalMatrix.cpp

@@ -6,12 +6,15 @@
  */
 
 #include <CppUnitLite/TestHarness.h>
+#include <gtsam/base/Matrix.h>
 #include <gtsam/base/Testable.h>
 #include <gtsam/geometry/FundamentalMatrix.h>
 #include <gtsam/geometry/Rot3.h>
 #include <gtsam/geometry/SimpleCamera.h>
 #include <gtsam/geometry/Unit3.h>
 
+#include <cmath>
+
 using namespace std::placeholders;
 using namespace std;
 using namespace gtsam;