Skip to content

Commit

Permalink
Fix small issues and store scaled s_
Browse files Browse the repository at this point in the history
  • Loading branch information
dellaert committed Oct 29, 2024
1 parent 0836852 commit 6457937
Show file tree
Hide file tree
Showing 3 changed files with 25 additions and 24 deletions.
30 changes: 12 additions & 18 deletions gtsam/geometry/FundamentalMatrix.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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 {

Expand All @@ -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);
}

Expand Down Expand Up @@ -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();
}

Expand Down
16 changes: 10 additions & 6 deletions gtsam/geometry/FundamentalMatrix.h
Original file line number Diff line number Diff line change
Expand Up @@ -2,7 +2,7 @@
* @file FundamentalMatrix.h
* @brief FundamentalMatrix classes
* @author Frank Dellaert
* @date Oct 23, 2024
* @date October 2024
*/

#pragma once
Expand Down Expand Up @@ -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
Expand All @@ -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
Expand Down Expand Up @@ -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);
Expand Down
3 changes: 3 additions & 0 deletions gtsam/geometry/tests/testFundamentalMatrix.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down

0 comments on commit 6457937

Please sign in to comment.