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

Remove inheritance, use abc language

release/4.3a0
Frank Dellaert 2024-10-24 15:13:45 -07:00
parent dc23c02be8
commit 5b413b84c9
3 changed files with 47 additions and 75 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

13
gtsam/geometry/FundamentalMatrix.cpp
View File

@ -10,21 +10,20 @@
namespace gtsam {
//*************************************************************************
Point2 FundamentalMatrix::transfer(const Matrix3& F01, const Point2& p1,
const Matrix3& F02, const Point2& p2) {
// Create lines in camera 0 from projections of the other two cameras
Vector3 line1 = F01 * Vector3(p1.x(), p1.y(), 1);
Vector3 line2 = F02 * Vector3(p2.x(), p2.y(), 1);
Point2 Transfer(const Matrix3& Fca, const Point2& pa, //
const Matrix3& Fcb, const Point2& pb) {
// Create lines in camera a from projections of the other two cameras
Vector3 line_a = Fca * Vector3(pa.x(), pa.y(), 1);
Vector3 line_b = Fcb * Vector3(pb.x(), pb.y(), 1);
// Cross the lines to find the intersection point
Vector3 intersectionPoint = line1.cross(line2);
Vector3 intersectionPoint = line_a.cross(line_b);
// Normalize the intersection point
intersectionPoint /= intersectionPoint(2);
return intersectionPoint.head<2>(); // Return the 2D point
}
//*************************************************************************
GeneralFundamentalMatrix::GeneralFundamentalMatrix(const Matrix3& F) {
// Perform SVD

96
gtsam/geometry/FundamentalMatrix.h
View File

@ -13,63 +13,6 @@
namespace gtsam {
/**
* @brief Abstract base class for FundamentalMatrix
*
* This class provides a common interface for all types of fundamental matrices.
* It declares a virtual function `matrix()` that must be implemented by derived
* classes. The `matrix()` function returns a 3x3 matrix representation of the
* fundamental matrix.
*/
class GTSAM_EXPORT FundamentalMatrix {
public:
/**
* @brief Returns a 3x3 matrix representation of the fundamental matrix
*
* @return A 3x3 matrix representing the fundamental matrix
*/
virtual Matrix3 matrix() const = 0;
/**
* @brief Virtual destructor to ensure proper cleanup of derived classes
*/
virtual ~FundamentalMatrix() {}
/**
* @brief Transfer projections from cameras 1 and 2 to camera 0
*
* Take two fundamental matrices F01 and F02, and two points p1 and p2, and
* returns the 2D point in camera 0 where the epipolar lines intersect.
*/
static Point2 transfer(const Matrix3& F01, const Point2& p1,
const Matrix3& F02, const Point2& p2);
};
/// Represents a set of three fundamental matrices for transferring points
/// between three cameras.
template <typename F>
struct TripleF {
F F01, F12, F20;
/// Transfers a point from cameras 1,2 to camera 0.
Point2 transfer0(const Point2& p1, const Point2& p2) {
return FundamentalMatrix::transfer(F01.matrix(), p1,
F20.matrix().transpose(), p2);
}
/// Transfers a point from camera 0,2 to camera 1.
Point2 transfer1(const Point2& p0, const Point2& p2) {
return FundamentalMatrix::transfer(F01.matrix().transpose(), p0,
F12.matrix(), p2);
}
/// Transfers a point from camera 0,1 to camera 2.
Point2 transfer2(const Point2& p0, const Point2& p1) {
return FundamentalMatrix::transfer(F20.matrix(), p0,
F12.matrix().transpose(), p1);
}
};
/**
* @class GeneralFundamentalMatrix
* @brief Represents a general fundamental matrix.
@ -78,7 +21,7 @@ struct TripleF {
* 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 : public FundamentalMatrix {
class GTSAM_EXPORT GeneralFundamentalMatrix {
private:
Rot3 U_; ///< Left rotation
double s_; ///< Scalar parameter for S
@ -112,7 +55,7 @@ class GTSAM_EXPORT GeneralFundamentalMatrix : public FundamentalMatrix {
GeneralFundamentalMatrix(const Matrix3& F);
/// Return the fundamental matrix representation
Matrix3 matrix() const override;
Matrix3 matrix() const;
/// @name Testable
/// @{
@ -146,7 +89,7 @@ class GTSAM_EXPORT GeneralFundamentalMatrix : public FundamentalMatrix {
* parameterization of the essential matrix and focal lengths for left and right
* cameras. Principal points are not part of the manifold but a convenience.
*/
class GTSAM_EXPORT SimpleFundamentalMatrix : public FundamentalMatrix {
class GTSAM_EXPORT SimpleFundamentalMatrix {
private:
EssentialMatrix E_; ///< Essential matrix
double fa_; ///< Focal length for left camera
@ -173,7 +116,7 @@ class GTSAM_EXPORT SimpleFundamentalMatrix : public FundamentalMatrix {
Matrix3 rightK() const;
/// Return the fundamental matrix representation
Matrix3 matrix() const override;
Matrix3 matrix() const;
/// @name Testable
/// @{
@ -198,6 +141,37 @@ class GTSAM_EXPORT SimpleFundamentalMatrix : public FundamentalMatrix {
/// @}
};
/**
* @brief Transfer projections from cameras a and b to camera c
*
* Take two fundamental matrices Fca and Fcb, and two points pa and pb, and
* returns the 2D point in view (c) where the epipolar lines intersect.
*/
Point2 Transfer(const Matrix3& Fca, const Point2& pa, const Matrix3& Fcb,
const Point2& pb);
/// Represents a set of three fundamental matrices for transferring points
/// between three cameras.
template <typename F>
struct TripleF {
F Fab, Fbc, Fca;
/// Transfers a point from cameras b,c to camera a.
Point2 transferToA(const Point2& pb, const Point2& pc) {
return Transfer(Fab.matrix(), pb, Fca.matrix().transpose(), pc);
}
/// Transfers a point from camera a,c to camera b.
Point2 transferToB(const Point2& pa, const Point2& pc) {
return Transfer(Fab.matrix().transpose(), pa, Fbc.matrix(), pc);
}
/// Transfers a point from cameras a,b to camera c.
Point2 transferToC(const Point2& pa, const Point2& pb) {
return Transfer(Fca.matrix(), pa, Fbc.matrix().transpose(), pb);
}
};
template <>
struct traits<GeneralFundamentalMatrix>
: public internal::Manifold<GeneralFundamentalMatrix> {};

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

@ -7,7 +7,6 @@
#include <CppUnitLite/TestHarness.h>
#include <gtsam/base/Testable.h>
#include <gtsam/base/numericalDerivative.h>
#include <gtsam/geometry/FundamentalMatrix.h>
#include <gtsam/geometry/Rot3.h>
#include <gtsam/geometry/SimpleCamera.h>
@ -205,9 +204,9 @@ TEST(TripleF, Transfer) {
auto triplet = generateTripleF(cameraPoses);
// Check that they are all equal
EXPECT(triplet.F01.equals(triplet.F12, 1e-9));
EXPECT(triplet.F12.equals(triplet.F20, 1e-9));
EXPECT(triplet.F20.equals(triplet.F01, 1e-9));
EXPECT(triplet.Fab.equals(triplet.Fbc, 1e-9));
EXPECT(triplet.Fbc.equals(triplet.Fca, 1e-9));
EXPECT(triplet.Fca.equals(triplet.Fab, 1e-9));
// Now project a point into the three cameras
const Point3 P(0.1, 0.2, 0.3);
@ -222,9 +221,9 @@ TEST(TripleF, Transfer) {
}
// Check that transfer works
EXPECT(assert_equal<Point2>(p[0], triplet.transfer0(p[1], p[2]), 1e-9));
EXPECT(assert_equal<Point2>(p[1], triplet.transfer1(p[0], p[2]), 1e-9));
EXPECT(assert_equal<Point2>(p[2], triplet.transfer2(p[0], p[1]), 1e-9));
EXPECT(assert_equal<Point2>(p[0], triplet.transferToA(p[1], p[2]), 1e-9));
EXPECT(assert_equal<Point2>(p[1], triplet.transferToB(p[0], p[2]), 1e-9));
EXPECT(assert_equal<Point2>(p[2], triplet.transferToC(p[0], p[1]), 1e-9));
}
//*************************************************************************