From 64579373be8bd1d84536ccce93517315c9297b20 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Tue, 29 Oct 2024 10:58:08 -0700 Subject: [PATCH 1/2] Fix small issues and store scaled s_ --- gtsam/geometry/FundamentalMatrix.cpp | 30 ++++++++----------- gtsam/geometry/FundamentalMatrix.h | 16 ++++++---- .../geometry/tests/testFundamentalMatrix.cpp | 3 ++ 3 files changed, 25 insertions(+), 24 deletions(-) diff --git a/gtsam/geometry/FundamentalMatrix.cpp b/gtsam/geometry/FundamentalMatrix.cpp index 5a50b4fd2..94c72673d 100644 --- a/gtsam/geometry/FundamentalMatrix.cpp +++ b/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 #include +#include 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(); } diff --git a/gtsam/geometry/FundamentalMatrix.h b/gtsam/geometry/FundamentalMatrix.h index 7e87ba62a..a36c815da 100644 --- a/gtsam/geometry/FundamentalMatrix.h +++ b/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,9 +34,11 @@ 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 @@ -44,7 +46,7 @@ class GTSAM_EXPORT FundamentalMatrix { * 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); diff --git a/gtsam/geometry/tests/testFundamentalMatrix.cpp b/gtsam/geometry/tests/testFundamentalMatrix.cpp index 00876e415..eed5da734 100644 --- a/gtsam/geometry/tests/testFundamentalMatrix.cpp +++ b/gtsam/geometry/tests/testFundamentalMatrix.cpp @@ -6,12 +6,15 @@ */ #include +#include #include #include #include #include #include +#include + using namespace std::placeholders; using namespace std; using namespace gtsam; From 5a2f1f889331f004a621610a3583646221732f4a Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Tue, 29 Oct 2024 11:55:55 -0700 Subject: [PATCH 2/2] Get rid of scale --- gtsam/geometry/FundamentalMatrix.cpp | 4 ++-- gtsam/geometry/FundamentalMatrix.h | 4 +--- 2 files changed, 3 insertions(+), 5 deletions(-) diff --git a/gtsam/geometry/FundamentalMatrix.cpp b/gtsam/geometry/FundamentalMatrix.cpp index 94c72673d..406269ff5 100644 --- a/gtsam/geometry/FundamentalMatrix.cpp +++ b/gtsam/geometry/FundamentalMatrix.cpp @@ -59,7 +59,7 @@ FundamentalMatrix::FundamentalMatrix(const Matrix3& F) { void FundamentalMatrix::initialize(const Matrix3& U, double s, const Matrix3& V) { - s_ = s / kScale; + s_ = s; sign_ = 1.0; // Check if U is a reflection and flip U and sign_ if so @@ -82,7 +82,7 @@ void FundamentalMatrix::initialize(const Matrix3& U, double s, } Matrix3 FundamentalMatrix::matrix() const { - return sign_ * U_.matrix() * Vector3(1.0, s_ * kScale, 0).asDiagonal() * + return sign_ * U_.matrix() * Vector3(1.0, s_, 0).asDiagonal() * V_.transpose().matrix(); } diff --git a/gtsam/geometry/FundamentalMatrix.h b/gtsam/geometry/FundamentalMatrix.h index a36c815da..beb2947e2 100644 --- a/gtsam/geometry/FundamentalMatrix.h +++ b/gtsam/geometry/FundamentalMatrix.h @@ -34,11 +34,9 @@ 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 / kScale), V_(Rot3()) {} + FundamentalMatrix() : U_(Rot3()), sign_(1.0), s_(1.0), V_(Rot3()) {} /** * @brief Construct from U, V, and scalar s