From cbbe2d2e887659ac1c7ad1d96c45264272435777 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Fri, 25 Oct 2024 20:48:02 -0700 Subject: [PATCH] Address review comments --- gtsam/geometry/FundamentalMatrix.cpp | 6 ++-- gtsam/geometry/FundamentalMatrix.h | 31 +++++++++------- gtsam/sfm/TransferFactor.h | 4 +-- gtsam/sfm/tests/testTransferFactor.cpp | 50 ++++++++++++++------------ 4 files changed, 52 insertions(+), 39 deletions(-) diff --git a/gtsam/geometry/FundamentalMatrix.cpp b/gtsam/geometry/FundamentalMatrix.cpp index af322a3c0..d65053d19 100644 --- a/gtsam/geometry/FundamentalMatrix.cpp +++ b/gtsam/geometry/FundamentalMatrix.cpp @@ -97,20 +97,20 @@ FundamentalMatrix FundamentalMatrix::retract(const Vector& delta) const { } //************************************************************************* -Matrix3 SimpleFundamentalMatrix::leftK() const { +Matrix3 SimpleFundamentalMatrix::Ka() const { Matrix3 K; K << fa_, 0, ca_.x(), 0, fa_, ca_.y(), 0, 0, 1; return K; } -Matrix3 SimpleFundamentalMatrix::rightK() const { +Matrix3 SimpleFundamentalMatrix::Kb() const { Matrix3 K; K << fb_, 0, cb_.x(), 0, fb_, cb_.y(), 0, 0, 1; return K; } Matrix3 SimpleFundamentalMatrix::matrix() const { - return leftK().transpose().inverse() * E_.matrix() * rightK().inverse(); + return Ka().transpose().inverse() * E_.matrix() * Kb().inverse(); } void SimpleFundamentalMatrix::print(const std::string& s) const { diff --git a/gtsam/geometry/FundamentalMatrix.h b/gtsam/geometry/FundamentalMatrix.h index 8a3ba86b8..6f04f45e8 100644 --- a/gtsam/geometry/FundamentalMatrix.h +++ b/gtsam/geometry/FundamentalMatrix.h @@ -68,8 +68,8 @@ class GTSAM_EXPORT FundamentalMatrix { template FundamentalMatrix(const CAL& Ka, const Pose3& aPb, const CAL& Kb) : FundamentalMatrix(Ka.K().transpose().inverse() * - EssentialMatrix::FromPose3(aPb).matrix() * - Kb.K().inverse()) {} + EssentialMatrix::FromPose3(aPb).matrix() * + Kb.K().inverse()) {} /// Return the fundamental matrix representation Matrix3 matrix() const; @@ -114,25 +114,32 @@ class GTSAM_EXPORT SimpleFundamentalMatrix { Point2 ca_; ///< Principal point for left camera Point2 cb_; ///< Principal point for right camera + /// Return the left calibration matrix + Matrix3 Ka() const; + + /// Return the right calibration matrix + Matrix3 Kb() const; + public: /// Default constructor SimpleFundamentalMatrix() : E_(), fa_(1.0), fb_(1.0), ca_(0.0, 0.0), cb_(0.0, 0.0) {} - /// Construct from essential matrix and focal lengths + /** + * @brief Construct from essential matrix and focal lengths + * @param E Essential matrix + * @param fa Focal length for left camera + * @param fb Focal length for right camera + * @param ca Principal point for left camera + * @param cb Principal point for right camera + */ SimpleFundamentalMatrix(const EssentialMatrix& E, // - double fa, double fb, - const Point2& ca = Point2(0.0, 0.0), - const Point2& cb = Point2(0.0, 0.0)) + double fa, double fb, const Point2& ca, + const Point2& cb) : E_(E), fa_(fa), fb_(fb), ca_(ca), cb_(cb) {} - /// Return the left calibration matrix - Matrix3 leftK() const; - - /// Return the right calibration matrix - Matrix3 rightK() const; - /// Return the fundamental matrix representation + /// F = Ka^(-T) * E * Kb^(-1) Matrix3 matrix() const; /// @name Testable diff --git a/gtsam/sfm/TransferFactor.h b/gtsam/sfm/TransferFactor.h index fb2dd63b0..d2085f57e 100644 --- a/gtsam/sfm/TransferFactor.h +++ b/gtsam/sfm/TransferFactor.h @@ -24,8 +24,8 @@ namespace gtsam { /** * Binary factor in the context of Structure from Motion (SfM). - * It is used to transfer points between different views based on the - * fundamental matrices between these views. The factor computes the error + * It is used to transfer transfer corresponding points from two views to a + * third based on two fundamental matrices. The factor computes the error * between the transferred points `pa` and `pb`, and the actual point `pc` in * the target view. Jacobians are done using numerical differentiation. */ diff --git a/gtsam/sfm/tests/testTransferFactor.cpp b/gtsam/sfm/tests/testTransferFactor.cpp index 6092ec456..2dca12c2a 100644 --- a/gtsam/sfm/tests/testTransferFactor.cpp +++ b/gtsam/sfm/tests/testTransferFactor.cpp @@ -12,9 +12,6 @@ using namespace gtsam; -double focalLength = 1000; -Point2 principalPoint(640 / 2, 480 / 2); - //************************************************************************* /// Generate three cameras on a circle, looking in std::array generateCameraPoses() { @@ -44,17 +41,36 @@ TripleF generateTripleF( return {F[0], F[1], F[2]}; // Return a TripleF instance } +//************************************************************************* +namespace fixture { +// Generate cameras on a circle +std::array cameraPoses = generateCameraPoses(); +auto triplet = generateTripleF(cameraPoses); +double focalLength = 1000; +Point2 principalPoint(640 / 2, 480 / 2); +const Cal3_S2 K(focalLength, focalLength, 0.0, // + principalPoint.x(), principalPoint.y()); +} // namespace fixture + +//************************************************************************* +// Test for getMatrices +TEST(TransferFactor, GetMatrices) { + using namespace fixture; + TransferFactor factor{{2, 0}, {1, 2}, {}}; + + // Check that getMatrices is correct + auto [Fki, Fkj] = factor.getMatrices(triplet.Fca, triplet.Fbc); + EXPECT(assert_equal(triplet.Fca.matrix(), Fki)); + EXPECT(assert_equal(triplet.Fbc.matrix().transpose(), Fkj)); +} + //************************************************************************* // Test for TransferFactor TEST(TransferFactor, Jacobians) { - // Generate cameras on a circle - std::array cameraPoses = generateCameraPoses(); - auto triplet = generateTripleF(cameraPoses); + using namespace fixture; // Now project a point into the three cameras const Point3 P(0.1, 0.2, 0.3); - const Cal3_S2 K(focalLength, focalLength, 0.0, // - principalPoint.x(), principalPoint.y()); std::array p; for (size_t i = 0; i < 3; ++i) { @@ -70,11 +86,6 @@ TEST(TransferFactor, Jacobians) { factor1{key12, key01, p[2], p[0], p[1]}, factor2{key20, key12, p[0], p[1], p[2]}; - // Check that getMatrices is correct - auto [Fki, Fkj] = factor2.getMatrices(triplet.Fca, triplet.Fbc); - EXPECT(assert_equal(triplet.Fca.matrix(), Fki)); - EXPECT(assert_equal(triplet.Fbc.matrix().transpose(), Fkj)); - // Create Values with edge keys Values values; values.insert(key01, triplet.Fab); @@ -92,18 +103,13 @@ TEST(TransferFactor, Jacobians) { //************************************************************************* // Test for TransferFactor with multiple tuples TEST(TransferFactor, MultipleTuples) { - // Generate cameras on a circle - std::array cameraPoses = generateCameraPoses(); - auto triplet = generateTripleF(cameraPoses); + using namespace fixture; // Now project multiple points into the three cameras const size_t numPoints = 5; // Number of points to test std::vector points3D; std::vector> projectedPoints; - const Cal3_S2 K(focalLength, focalLength, 0.0, // - principalPoint.x(), principalPoint.y()); - // Generate random 3D points and project them for (size_t n = 0; n < numPoints; ++n) { Point3 P(0.1 * n, 0.2 * n, 0.3 + 0.1 * n); @@ -120,15 +126,15 @@ TEST(TransferFactor, MultipleTuples) { // Create a vector of tuples for the TransferFactor EdgeKey key01(0, 1), key12(1, 2), key20(2, 0); - std::vector> tuples0, tuples1, tuples2; + std::vector> tuples; for (size_t n = 0; n < numPoints; ++n) { const auto& p = projectedPoints[n]; - tuples0.emplace_back(p[1], p[2], p[0]); + tuples.emplace_back(p[1], p[2], p[0]); } // Create TransferFactors using the new constructor - TransferFactor factor{key01, key20, tuples0}; + TransferFactor factor{key01, key20, tuples}; // Create Values with edge keys Values values;