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

32
gtsam/geometry/FundamentalMatrix.h
View File

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

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

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