Fix issues with SVD constructor
parent
ced6d5721d
commit
b45c55c9f4
|
@ -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) {
|
FundamentalMatrix::FundamentalMatrix(const Matrix3& F) {
|
||||||
// Perform SVD
|
// Perform SVD
|
||||||
Eigen::JacobiSVD<Matrix3> svd(F, Eigen::ComputeFullU | Eigen::ComputeFullV);
|
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.");
|
"The input matrix does not represent a valid fundamental matrix.");
|
||||||
}
|
}
|
||||||
|
|
||||||
// Ensure the second singular value is recorded as s
|
initialize(U, singularValues(1), V);
|
||||||
s_ = singularValues(1);
|
}
|
||||||
|
|
||||||
// Check if U is a reflection
|
void FundamentalMatrix::initialize(const Matrix3& U, double s,
|
||||||
if (U.determinant() < 0) {
|
const Matrix3& V) {
|
||||||
U = -U;
|
s_ = s;
|
||||||
s_ = -s_; // Change sign of scalar if U is a reflection
|
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
|
// Check if V is a reflection and its determinant is close to -1 or 1
|
||||||
if (V.determinant() < 0) {
|
double detV = V.determinant();
|
||||||
V = -V;
|
if (std::abs(std::abs(detV) - 1.0) > 1e-9) {
|
||||||
s_ = -s_; // Change sign of scalar if U is a reflection
|
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 {
|
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 {
|
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,
|
bool FundamentalMatrix::equals(const FundamentalMatrix& other,
|
||||||
double tol) const {
|
double tol) const {
|
||||||
return U_.equals(other.U_, tol) && std::abs(s_ - other.s_) < tol &&
|
return U_.equals(other.U_, tol) && sign_ == other.sign_ &&
|
||||||
V_.equals(other.V_, tol);
|
std::abs(s_ - other.s_) < tol && V_.equals(other.V_, tol);
|
||||||
}
|
}
|
||||||
|
|
||||||
Vector FundamentalMatrix::localCoordinates(const FundamentalMatrix& F) const {
|
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>());
|
Rot3 newU = U_.retract(delta.head<3>());
|
||||||
double newS = s_ + delta(3); // Update scalar
|
double newS = s_ + delta(3); // Update scalar
|
||||||
Rot3 newV = V_.retract(delta.tail<3>());
|
Rot3 newV = V_.retract(delta.tail<3>());
|
||||||
return FundamentalMatrix(newU, newS, newV);
|
return FundamentalMatrix(newU, sign_, newS, newV);
|
||||||
}
|
}
|
||||||
|
|
||||||
//*************************************************************************
|
//*************************************************************************
|
||||||
|
|
|
@ -15,34 +15,36 @@ namespace gtsam {
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* @class FundamentalMatrix
|
* @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
|
* The FundamentalMatrix class encapsulates the fundamental matrix, which
|
||||||
* that describes the relationship between two images. It is parameterized by a
|
* relates corresponding points in stereo images. It is parameterized by two
|
||||||
* left rotation U, a scalar s, and a right rotation V.
|
* 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 {
|
class GTSAM_EXPORT FundamentalMatrix {
|
||||||
private:
|
private:
|
||||||
Rot3 U_; ///< Left rotation
|
Rot3 U_; ///< Left rotation
|
||||||
double s_; ///< Scalar parameter for S
|
double sign_; ///< Whether to flip the sign or not
|
||||||
Rot3 V_; ///< Right rotation
|
double s_; ///< Scalar parameter for S
|
||||||
|
Rot3 V_; ///< Right rotation
|
||||||
|
|
||||||
public:
|
public:
|
||||||
/// Default constructor
|
/// 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
|
* @brief Construct from U, V, and scalar s
|
||||||
*
|
*
|
||||||
* Initializes the FundamentalMatrix with the given left rotation U,
|
* Initializes the FundamentalMatrix From the SVD representation
|
||||||
* scalar s, and right rotation V.
|
* U*diag(1,s,0)*V^T. It will internally convert to using SO(3).
|
||||||
*
|
|
||||||
* @param U Left rotation matrix
|
|
||||||
* @param s Scalar parameter for the fundamental matrix
|
|
||||||
* @param V Right rotation matrix
|
|
||||||
*/
|
*/
|
||||||
FundamentalMatrix(const Rot3& U, double s, const Rot3& V)
|
FundamentalMatrix(const Matrix& U, double s, const Matrix& V);
|
||||||
: U_(U), s_(s), V_(V) {}
|
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* @brief Construct from a 3x3 matrix using SVD
|
* @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
|
/// Retract the given vector to get a new FundamentalMatrix
|
||||||
FundamentalMatrix retract(const Vector& delta) const;
|
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);
|
||||||
};
|
};
|
||||||
|
|
||||||
/**
|
/**
|
||||||
|
|
|
@ -24,21 +24,48 @@ GTSAM_CONCEPT_MANIFOLD_INST(FundamentalMatrix)
|
||||||
Rot3 trueU = Rot3::Yaw(M_PI_2);
|
Rot3 trueU = Rot3::Yaw(M_PI_2);
|
||||||
Rot3 trueV = Rot3::Yaw(M_PI_4);
|
Rot3 trueV = Rot3::Yaw(M_PI_4);
|
||||||
double trueS = 0.5;
|
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 expected = Z_7x1; // Assuming 7 dimensions for U, V, and s
|
||||||
Vector actual = trueF.localCoordinates(trueF);
|
Vector actual = trueF.localCoordinates(trueF);
|
||||||
EXPECT(assert_equal(expected, actual, 1e-8));
|
EXPECT(assert_equal(expected, actual, 1e-8));
|
||||||
}
|
}
|
||||||
|
|
||||||
//*************************************************************************
|
//*************************************************************************
|
||||||
TEST(FundamentalMatrix, retract) {
|
TEST(FundamentalMatrix, Retract) {
|
||||||
FundamentalMatrix actual = trueF.retract(Z_7x1);
|
FundamentalMatrix actual = trueF.retract(Z_7x1);
|
||||||
EXPECT(assert_equal(trueF, actual));
|
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) {
|
TEST(FundamentalMatrix, RoundTrip) {
|
||||||
Vector7 d;
|
Vector7 d;
|
||||||
|
@ -61,14 +88,14 @@ TEST(SimpleStereo, Conversion) {
|
||||||
}
|
}
|
||||||
|
|
||||||
//*************************************************************************
|
//*************************************************************************
|
||||||
TEST(SimpleStereo, localCoordinates) {
|
TEST(SimpleStereo, LocalCoordinates) {
|
||||||
Vector expected = Z_7x1;
|
Vector expected = Z_7x1;
|
||||||
Vector actual = stereoF.localCoordinates(stereoF);
|
Vector actual = stereoF.localCoordinates(stereoF);
|
||||||
EXPECT(assert_equal(expected, actual, 1e-8));
|
EXPECT(assert_equal(expected, actual, 1e-8));
|
||||||
}
|
}
|
||||||
|
|
||||||
//*************************************************************************
|
//*************************************************************************
|
||||||
TEST(SimpleStereo, retract) {
|
TEST(SimpleStereo, Retract) {
|
||||||
SimpleFundamentalMatrix actual = stereoF.retract(Z_9x1);
|
SimpleFundamentalMatrix actual = stereoF.retract(Z_9x1);
|
||||||
EXPECT(assert_equal(stereoF, actual));
|
EXPECT(assert_equal(stereoF, actual));
|
||||||
}
|
}
|
||||||
|
|
Loading…
Reference in New Issue