12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Fix issues with SVD constructor

release/4.3a0
Frank Dellaert 2024-10-28 12:00:54 -07:00
parent ced6d5721d
commit b45c55c9f4
3 changed files with 96 additions and 39 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

57
gtsam/geometry/FundamentalMatrix.cpp
View File

@ -26,6 +26,11 @@ Point2 EpipolarTransfer(const Matrix3& Fca, const Point2& pa, //
}
//*************************************************************************
FundamentalMatrix::FundamentalMatrix(const Matrix& U, double s,
const Matrix& V) {
initialize(U, s, V);
}
FundamentalMatrix::FundamentalMatrix(const Matrix3& F) {
// Perform SVD
Eigen::JacobiSVD<Matrix3> svd(F, Eigen::ComputeFullU | Eigen::ComputeFullV);
@ -47,28 +52,44 @@ FundamentalMatrix::FundamentalMatrix(const Matrix3& F) {
"The input matrix does not represent a valid fundamental matrix.");
}
// Ensure the second singular value is recorded as s
s_ = singularValues(1);
initialize(U, singularValues(1), V);
}
// Check if U is a reflection
if (U.determinant() < 0) {
U = -U;
s_ = -s_; // Change sign of scalar if U is a reflection
void FundamentalMatrix::initialize(const Matrix3& U, double s,
const Matrix3& V) {
s_ = s;
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.");
}
if (detU < 0) {
U_ = Rot3(-U);
sign_ = -sign_; // Flip sign if U is a reflection
} else {
U_ = Rot3(U);
}
// Check if V is a reflection
if (V.determinant() < 0) {
V = -V;
s_ = -s_; // Change sign of scalar if U is a reflection
// Check if V is a reflection and its determinant is close to -1 or 1
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
} else {
V_ = Rot3(V);
}
// Assign the rotations
U_ = Rot3(U);
V_ = Rot3(V);
}
Matrix3 FundamentalMatrix::matrix() const {
return U_.matrix() * Vector3(1, s_, 0).asDiagonal() * V_.transpose().matrix();
return sign_ * U_.matrix() * Vector3(1.0, s_, 0).asDiagonal() *
V_.transpose().matrix();
}
void FundamentalMatrix::print(const std::string& s) const {
@ -77,8 +98,8 @@ void FundamentalMatrix::print(const std::string& s) const {
bool FundamentalMatrix::equals(const FundamentalMatrix& other,
double tol) const {
return U_.equals(other.U_, tol) && std::abs(s_ - other.s_) < tol &&
V_.equals(other.V_, tol);
return U_.equals(other.U_, tol) && sign_ == other.sign_ &&
std::abs(s_ - other.s_) < tol && V_.equals(other.V_, tol);
}
Vector FundamentalMatrix::localCoordinates(const FundamentalMatrix& F) const {
@ -93,7 +114,7 @@ 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, newS, newV);
return FundamentalMatrix(newU, sign_, newS, newV);
}
//*************************************************************************

41
gtsam/geometry/FundamentalMatrix.h
View File

@ -15,34 +15,36 @@ namespace gtsam {
/**
* @class FundamentalMatrix
* @brief Represents a general fundamental matrix.
* @brief Represents a fundamental matrix in computer vision, which encodes the
* epipolar geometry between two views.
*
* This class represents a general fundamental matrix, which is a 3x3 matrix
* that describes the relationship between two images. It is parameterized by a
* left rotation U, a scalar s, and a right rotation V.
* 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.
* 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).
*/
class GTSAM_EXPORT FundamentalMatrix {
private:
Rot3 U_; ///< Left rotation
double s_; ///< Scalar parameter for S
Rot3 V_; ///< Right rotation
Rot3 U_; ///< Left rotation
double sign_; ///< Whether to flip the sign or not
double s_; ///< Scalar parameter for S
Rot3 V_; ///< Right rotation
public:
/// Default constructor
FundamentalMatrix() : U_(Rot3()), s_(1.0), V_(Rot3()) {}
FundamentalMatrix() : U_(Rot3()), sign_(1.0), s_(1.0), V_(Rot3()) {}
/**
* @brief Construct from U, V, and scalar s
*
* Initializes the FundamentalMatrix with the given left rotation U,
* scalar s, and right rotation V.
*
* @param U Left rotation matrix
* @param s Scalar parameter for the fundamental matrix
* @param V Right rotation matrix
* Initializes the FundamentalMatrix From the SVD representation
* U*diag(1,s,0)*V^T. It will internally convert to using SO(3).
*/
FundamentalMatrix(const Rot3& U, double s, const Rot3& V)
: U_(U), s_(s), V_(V) {}
FundamentalMatrix(const Matrix& U, double s, const Matrix& V);
/**
* @brief Construct from a 3x3 matrix using SVD
@ -107,6 +109,13 @@ class GTSAM_EXPORT FundamentalMatrix {
/// Retract the given vector to get a new FundamentalMatrix
FundamentalMatrix retract(const Vector& delta) const;
/// @}
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) {}
/// Initialize SO(3) matrices from general O(3) matrices
void initialize(const Matrix3& U, double s, const Matrix3& V);
};
/**

37
gtsam/geometry/tests/testFundamentalMatrix.cpp
View File

@ -24,21 +24,48 @@ GTSAM_CONCEPT_MANIFOLD_INST(FundamentalMatrix)
Rot3 trueU = Rot3::Yaw(M_PI_2);
Rot3 trueV = Rot3::Yaw(M_PI_4);
double trueS = 0.5;
FundamentalMatrix trueF(trueU, trueS, trueV);
FundamentalMatrix trueF(trueU.matrix(), trueS, trueV.matrix());
//*************************************************************************
TEST(FundamentalMatrix, localCoordinates) {
TEST(FundamentalMatrix, LocalCoordinates) {
Vector expected = Z_7x1; // Assuming 7 dimensions for U, V, and s
Vector actual = trueF.localCoordinates(trueF);
EXPECT(assert_equal(expected, actual, 1e-8));
}
//*************************************************************************
TEST(FundamentalMatrix, retract) {
TEST(FundamentalMatrix, Retract) {
FundamentalMatrix actual = trueF.retract(Z_7x1);
EXPECT(assert_equal(trueF, actual));
}
//*************************************************************************
// Test conversion from an F-matrix
TEST(FundamentalMatrix, Conversion) {
const Matrix3 F = trueU.matrix() * Vector3(1, trueS, 0).asDiagonal() *
trueV.matrix().transpose();
FundamentalMatrix actual(F);
EXPECT(assert_equal(trueF, actual));
}
//*************************************************************************
// Test conversion with a *non-rotation* U
TEST(FlippedFundamentalMatrix, Conversion1) {
FundamentalMatrix trueF(trueU.matrix(), trueS, -trueV.matrix());
const Matrix3 F = trueF.matrix();
FundamentalMatrix actual(F);
CHECK(assert_equal(F, actual.matrix()));
}
//*************************************************************************
// Test conversion with a *non-rotation* U
TEST(FlippedFundamentalMatrix, Conversion2) {
FundamentalMatrix trueF(-trueU.matrix(), trueS, trueV.matrix());
const Matrix3 F = trueF.matrix();
FundamentalMatrix actual(F);
CHECK(assert_equal(F, actual.matrix()));
}
//*************************************************************************
TEST(FundamentalMatrix, RoundTrip) {
Vector7 d;
@ -61,14 +88,14 @@ TEST(SimpleStereo, Conversion) {
}
//*************************************************************************
TEST(SimpleStereo, localCoordinates) {
TEST(SimpleStereo, LocalCoordinates) {
Vector expected = Z_7x1;
Vector actual = stereoF.localCoordinates(stereoF);
EXPECT(assert_equal(expected, actual, 1e-8));
}
//*************************************************************************
TEST(SimpleStereo, retract) {
TEST(SimpleStereo, Retract) {
SimpleFundamentalMatrix actual = stereoF.retract(Z_9x1);
EXPECT(assert_equal(stereoF, actual));
}