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

Just FundamentalMatrix

release/4.3a0
Frank Dellaert 2024-10-24 17:30:30 -07:00
parent 9773fbc629
commit 502dcc480b
3 changed files with 36 additions and 38 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

18
gtsam/geometry/FundamentalMatrix.cpp
View File

@ -25,7 +25,7 @@ Point2 Transfer(const Matrix3& Fca, const Point2& pa, //
return intersectionPoint.head<2>(); // Return the 2D point
}
//*************************************************************************
GeneralFundamentalMatrix::GeneralFundamentalMatrix(const Matrix3& F) {
FundamentalMatrix::FundamentalMatrix(const Matrix3& F) {
// Perform SVD
Eigen::JacobiSVD<Matrix3> svd(F, Eigen::ComputeFullU | Eigen::ComputeFullV);
@ -66,24 +66,23 @@ GeneralFundamentalMatrix::GeneralFundamentalMatrix(const Matrix3& F) {
V_ = Rot3(V);
}
Matrix3 GeneralFundamentalMatrix::matrix() const {
Matrix3 FundamentalMatrix::matrix() const {
return U_.matrix() * Vector3(1, s_, 0).asDiagonal() * V_.transpose().matrix();
}
void GeneralFundamentalMatrix::print(const std::string& s) const {
void FundamentalMatrix::print(const std::string& s) const {
std::cout << s << "U:\n"
<< U_.matrix() << "\ns: " << s_ << "\nV:\n"
<< V_.matrix() << std::endl;
}
bool GeneralFundamentalMatrix::equals(const GeneralFundamentalMatrix& other,
double tol) 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);
}
Vector GeneralFundamentalMatrix::localCoordinates(
const GeneralFundamentalMatrix& F) const {
Vector FundamentalMatrix::localCoordinates(const FundamentalMatrix& F) const {
Vector result(7);
result.head<3>() = U_.localCoordinates(F.U_);
result(3) = F.s_ - s_; // Difference in scalar
@ -91,12 +90,11 @@ Vector GeneralFundamentalMatrix::localCoordinates(
return result;
}
GeneralFundamentalMatrix GeneralFundamentalMatrix::retract(
const Vector& delta) const {
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 GeneralFundamentalMatrix(newU, newS, newV);
return FundamentalMatrix(newU, newS, newV);
}
//*************************************************************************

32
gtsam/geometry/FundamentalMatrix.h
View File

@ -14,14 +14,14 @@
namespace gtsam {
/**
* @class GeneralFundamentalMatrix
* @class FundamentalMatrix
* @brief Represents a general fundamental matrix.
*
* 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.
*/
class GTSAM_EXPORT GeneralFundamentalMatrix {
class GTSAM_EXPORT FundamentalMatrix {
private:
Rot3 U_; ///< Left rotation
double s_; ///< Scalar parameter for S
@ -29,42 +29,42 @@ class GTSAM_EXPORT GeneralFundamentalMatrix {
public:
/// Default constructor
GeneralFundamentalMatrix() : U_(Rot3()), s_(1.0), V_(Rot3()) {}
FundamentalMatrix() : U_(Rot3()), s_(1.0), V_(Rot3()) {}
/**
* @brief Construct from U, V, and scalar s
*
* Initializes the GeneralFundamentalMatrix with the given left rotation U,
* 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
*/
GeneralFundamentalMatrix(const Rot3& U, double s, const Rot3& V)
FundamentalMatrix(const Rot3& U, double s, const Rot3& V)
: U_(U), s_(s), V_(V) {}
/**
* @brief Construct from a 3x3 matrix using SVD
*
* Initializes the GeneralFundamentalMatrix by performing SVD on the given
* Initializes the FundamentalMatrix by performing SVD on the given
* matrix and ensuring U and V are not reflections.
*
* @param F A 3x3 matrix representing the fundamental matrix
*/
GeneralFundamentalMatrix(const Matrix3& F);
FundamentalMatrix(const Matrix3& F);
/// Return the fundamental matrix representation
Matrix3 matrix() const;
/// @name Testable
/// @{
/// Print the GeneralFundamentalMatrix
/// Print the FundamentalMatrix
void print(const std::string& s = "") const;
/// Check if the GeneralFundamentalMatrix is equal to another within a
/// Check if the FundamentalMatrix is equal to another within a
/// tolerance
bool equals(const GeneralFundamentalMatrix& other, double tol = 1e-9) const;
bool equals(const FundamentalMatrix& other, double tol = 1e-9) const;
/// @}
/// @name Manifold
@ -73,11 +73,11 @@ class GTSAM_EXPORT GeneralFundamentalMatrix {
inline static size_t Dim() { return dimension; }
inline size_t dim() const { return dimension; }
/// Return local coordinates with respect to another GeneralFundamentalMatrix
Vector localCoordinates(const GeneralFundamentalMatrix& F) const;
/// Return local coordinates with respect to another FundamentalMatrix
Vector localCoordinates(const FundamentalMatrix& F) const;
/// Retract the given vector to get a new GeneralFundamentalMatrix
GeneralFundamentalMatrix retract(const Vector& delta) const;
/// Retract the given vector to get a new FundamentalMatrix
FundamentalMatrix retract(const Vector& delta) const;
/// @}
};
@ -173,8 +173,8 @@ struct TripleF {
};
template <>
struct traits<GeneralFundamentalMatrix>
: public internal::Manifold<GeneralFundamentalMatrix> {};
struct traits<FundamentalMatrix>
: public internal::Manifold<FundamentalMatrix> {};
template <>
struct traits<SimpleFundamentalMatrix>

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

@ -16,34 +16,34 @@ using namespace std::placeholders;
using namespace std;
using namespace gtsam;
GTSAM_CONCEPT_TESTABLE_INST(GeneralFundamentalMatrix)
GTSAM_CONCEPT_MANIFOLD_INST(GeneralFundamentalMatrix)
GTSAM_CONCEPT_TESTABLE_INST(FundamentalMatrix)
GTSAM_CONCEPT_MANIFOLD_INST(FundamentalMatrix)
//*************************************************************************
// Create two rotations and corresponding fundamental matrix F
Rot3 trueU = Rot3::Yaw(M_PI_2);
Rot3 trueV = Rot3::Yaw(M_PI_4);
double trueS = 0.5;
GeneralFundamentalMatrix trueF(trueU, trueS, trueV);
FundamentalMatrix trueF(trueU, trueS, trueV);
//*************************************************************************
TEST(GeneralFundamentalMatrix, 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(GeneralFundamentalMatrix, retract) {
GeneralFundamentalMatrix actual = trueF.retract(Z_7x1);
TEST(FundamentalMatrix, retract) {
FundamentalMatrix actual = trueF.retract(Z_7x1);
EXPECT(assert_equal(trueF, actual));
}
//*************************************************************************
TEST(GeneralFundamentalMatrix, RoundTrip) {
TEST(FundamentalMatrix, RoundTrip) {
Vector7 d;
d << 0.1, 0.2, 0.3, 0.4, 0.5, 0.6, 0.7;
GeneralFundamentalMatrix hx = trueF.retract(d);
FundamentalMatrix hx = trueF.retract(d);
Vector actual = trueF.localCoordinates(hx);
EXPECT(assert_equal(d, actual, 1e-8));
}
@ -56,7 +56,7 @@ SimpleFundamentalMatrix stereoF(defaultE, 1.0, 1.0, zero, zero);
//*************************************************************************
TEST(SimpleStereo, Conversion) {
GeneralFundamentalMatrix convertedF(stereoF.matrix());
FundamentalMatrix convertedF(stereoF.matrix());
EXPECT(assert_equal(stereoF.matrix(), convertedF.matrix(), 1e-8));
}
@ -103,7 +103,7 @@ SimpleFundamentalMatrix pixelStereo(defaultE, focalLength, focalLength, zero,
TEST(PixelStereo, Conversion) {
auto expected = pixelStereo.matrix();
GeneralFundamentalMatrix convertedF(pixelStereo.matrix());
FundamentalMatrix convertedF(pixelStereo.matrix());
// Check equality of F-matrices up to a scale
auto actual = convertedF.matrix();
@ -132,7 +132,7 @@ SimpleFundamentalMatrix rotatedPixelStereo(rotatedE, focalLength, focalLength,
TEST(RotatedPixelStereo, Conversion) {
auto expected = rotatedPixelStereo.matrix();
GeneralFundamentalMatrix convertedF(rotatedPixelStereo.matrix());
FundamentalMatrix convertedF(rotatedPixelStereo.matrix());
// Check equality of F-matrices up to a scale
auto actual = convertedF.matrix();
@ -161,7 +161,7 @@ SimpleFundamentalMatrix stereoWithPrincipalPoints(rotatedE, focalLength,
TEST(stereoWithPrincipalPoints, Conversion) {
auto expected = stereoWithPrincipalPoints.matrix();
GeneralFundamentalMatrix convertedF(stereoWithPrincipalPoints.matrix());
FundamentalMatrix convertedF(stereoWithPrincipalPoints.matrix());
// Check equality of F-matrices up to a scale
auto actual = convertedF.matrix();