From 90ae543c7f3a4091b8c6cb9717afc65f5aa5af15 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Fri, 25 Oct 2024 07:33:24 -0700 Subject: [PATCH 01/20] Address review Comments --- gtsam/geometry/FundamentalMatrix.cpp | 5 +++-- gtsam/geometry/FundamentalMatrix.h | 10 +++++----- 2 files changed, 8 insertions(+), 7 deletions(-) diff --git a/gtsam/geometry/FundamentalMatrix.cpp b/gtsam/geometry/FundamentalMatrix.cpp index 71ca47418b..57d59d0a04 100644 --- a/gtsam/geometry/FundamentalMatrix.cpp +++ b/gtsam/geometry/FundamentalMatrix.cpp @@ -10,8 +10,8 @@ namespace gtsam { //************************************************************************* -Point2 Transfer(const Matrix3& Fca, const Point2& pa, // - const Matrix3& Fcb, const Point2& pb) { +Point2 EpipolarTransfer(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); @@ -24,6 +24,7 @@ Point2 Transfer(const Matrix3& Fca, const Point2& pa, // return intersectionPoint.head<2>(); // Return the 2D point } + //************************************************************************* FundamentalMatrix::FundamentalMatrix(const Matrix3& F) { // Perform SVD diff --git a/gtsam/geometry/FundamentalMatrix.h b/gtsam/geometry/FundamentalMatrix.h index 718364ca0c..497f95cf77 100644 --- a/gtsam/geometry/FundamentalMatrix.h +++ b/gtsam/geometry/FundamentalMatrix.h @@ -147,8 +147,8 @@ class GTSAM_EXPORT SimpleFundamentalMatrix { * 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. */ -GTSAM_EXPORT Point2 Transfer(const Matrix3& Fca, const Point2& pa, - const Matrix3& Fcb, const Point2& pb); +GTSAM_EXPORT Point2 EpipolarTransfer(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. @@ -158,17 +158,17 @@ struct TripleF { /// 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); + return EpipolarTransfer(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); + return EpipolarTransfer(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); + return EpipolarTransfer(Fca.matrix(), pa, Fbc.matrix().transpose(), pb); } }; From 8a0ab6e094ed3be2387f44610b7fca7a4adbf236 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Thu, 24 Oct 2024 01:09:01 -0700 Subject: [PATCH 02/20] proto-factor with numerical derivative --- .../geometry/tests/testFundamentalMatrix.cpp | 29 +++++++++++++++++++ 1 file changed, 29 insertions(+) diff --git a/gtsam/geometry/tests/testFundamentalMatrix.cpp b/gtsam/geometry/tests/testFundamentalMatrix.cpp index a8884e791e..80e6b1d3af 100644 --- a/gtsam/geometry/tests/testFundamentalMatrix.cpp +++ b/gtsam/geometry/tests/testFundamentalMatrix.cpp @@ -197,6 +197,35 @@ TripleF generateTripleF( return {F[0], F[1], F[2]}; // Return a TripleF instance } +//************************************************************************* + +struct TripletFactor { + using F = FundamentalMatrix; + using SF = SimpleFundamentalMatrix; + Point2 p0, p1, p2; + + /// vector of errors returns 6D vector + Vector evaluateError(const SF& F01, const SF& F12, const SF& F20, // + Matrix* H01, Matrix* H12, Matrix* H20) const { + Vector error(6); + std::function fn = [&](const SF& F01, const SF& F12, + const SF& F20) { + Vector6 error; + error << F::transfer(F01.matrix(), p1, F20.matrix().transpose(), p2) - p0, + F::transfer(F01.matrix().transpose(), p0, F12.matrix(), p2) - p1, + F::transfer(F20.matrix(), p0, F12.matrix().transpose(), p1) - p2; + return error; + }; + if (H01) + *H01 = numericalDerivative31(fn, F01, F12, F20); + if (H12) + *H12 = numericalDerivative32(fn, F01, F12, F20); + if (H20) + *H20 = numericalDerivative33(fn, F01, F12, F20); + return fn(F01, F12, F20); + } +}; + //************************************************************************* TEST(TripleF, Transfer) { // Generate cameras on a circle, as well as fundamental matrices From 00fc2ecc2ba88a1aaf65a234c7761ba018ffc77d Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Thu, 24 Oct 2024 11:43:40 -0700 Subject: [PATCH 03/20] Slight modernization --- examples/SFMdata.h | 82 ++++++++++++++++++++++++++++------------------ 1 file changed, 50 insertions(+), 32 deletions(-) diff --git a/examples/SFMdata.h b/examples/SFMdata.h index 3031828f19..112bd927ce 100644 --- a/examples/SFMdata.h +++ b/examples/SFMdata.h @@ -16,56 +16,74 @@ */ /** - * A structure-from-motion example with landmarks, default function arguments give + * A structure-from-motion example with landmarks, default arguments give: * - The landmarks form a 10 meter cube * - The robot rotates around the landmarks, always facing towards the cube - * Passing function argument allows to specificy an initial position, a pose increment and step count. + * Passing function argument allows to specify an initial position, a pose + * increment and step count. */ #pragma once -// As this is a full 3D problem, we will use Pose3 variables to represent the camera -// positions and Point3 variables (x, y, z) to represent the landmark coordinates. -// Camera observations of landmarks (i.e. pixel coordinates) will be stored as Point2 (x, y). -// We will also need a camera object to hold calibration information and perform projections. -#include +// As this is a full 3D problem, we will use Pose3 variables to represent the +// camera positions and Point3 variables (x, y, z) to represent the landmark +// coordinates. Camera observations of landmarks (i.e. pixel coordinates) will +// be stored as Point2 (x, y). #include +#include -// We will also need a camera object to hold calibration information and perform projections. -#include +// We will also need a camera object to hold calibration information and perform +// projections. #include +#include -/* ************************************************************************* */ -std::vector createPoints() { +namespace gtsam { - // Create the set of ground-truth landmarks - std::vector points; - points.push_back(gtsam::Point3(10.0,10.0,10.0)); - points.push_back(gtsam::Point3(-10.0,10.0,10.0)); - points.push_back(gtsam::Point3(-10.0,-10.0,10.0)); - points.push_back(gtsam::Point3(10.0,-10.0,10.0)); - points.push_back(gtsam::Point3(10.0,10.0,-10.0)); - points.push_back(gtsam::Point3(-10.0,10.0,-10.0)); - points.push_back(gtsam::Point3(-10.0,-10.0,-10.0)); - points.push_back(gtsam::Point3(10.0,-10.0,-10.0)); +/// Create a set of ground-truth landmarks +std::vector createPoints() { + std::vector points; + points.push_back(Point3(10.0, 10.0, 10.0)); + points.push_back(Point3(-10.0, 10.0, 10.0)); + points.push_back(Point3(-10.0, -10.0, 10.0)); + points.push_back(Point3(10.0, -10.0, 10.0)); + points.push_back(Point3(10.0, 10.0, -10.0)); + points.push_back(Point3(-10.0, 10.0, -10.0)); + points.push_back(Point3(-10.0, -10.0, -10.0)); + points.push_back(Point3(10.0, -10.0, -10.0)); return points; } -/* ************************************************************************* */ -std::vector createPoses( - const gtsam::Pose3& init = gtsam::Pose3(gtsam::Rot3::Ypr(M_PI/2,0,-M_PI/2), gtsam::Point3(30, 0, 0)), - const gtsam::Pose3& delta = gtsam::Pose3(gtsam::Rot3::Ypr(0,-M_PI/4,0), gtsam::Point3(sin(M_PI/4)*30, 0, 30*(1-sin(M_PI/4)))), - int steps = 8) { +/** + * Create a set of ground-truth poses + * Default values give a circular trajectory, radius 30 at pi/4 intervals, + * always facing the circle center + */ +std::vector createPoses( + const Pose3& init = Pose3(Rot3::Ypr(M_PI_2, 0, -M_PI_2), {30, 0, 0}), + const Pose3& delta = Pose3(Rot3::Ypr(0, -M_PI_4, 0), + {sin(M_PI_4) * 30, 0, 30 * (1 - sin(M_PI_4))}), + int steps = 8) { + std::vector poses; + poses.reserve(steps); - // Create the set of ground-truth poses - // Default values give a circular trajectory, radius 30 at pi/4 intervals, always facing the circle center - std::vector poses; - int i = 1; poses.push_back(init); - for(; i < steps; ++i) { - poses.push_back(poses[i-1].compose(delta)); + for (int i = 1; i < steps; ++i) { + poses.push_back(poses[i - 1].compose(delta)); } return poses; } + +/** + * Create regularly spaced poses with specified radius and number of cameras + */ +std::vector posesOnCircle(int num_cameras = 8, double radius = 30) { + const Pose3 init(Rot3::Ypr(M_PI_2, 0, -M_PI_2), {radius, 0, 0}); + const double theta = M_PI / num_cameras; + const Pose3 delta( + Rot3::Ypr(0, -2 * theta, 0), + {sin(2 * theta) * radius, 0, radius * (1 - sin(2 * theta))}); + return createPoses(init, delta, num_cameras); +} +} // namespace gtsam \ No newline at end of file From d44cca770d661375eabc11ae20e3d66a90a8e43c Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Thu, 24 Oct 2024 11:44:21 -0700 Subject: [PATCH 04/20] TransferFactor in sfm --- .../geometry/tests/testFundamentalMatrix.cpp | 29 ------ gtsam/sfm/TransferFactor.h | 48 ++++++++++ gtsam/sfm/tests/testTransferFactor.cpp | 95 +++++++++++++++++++ 3 files changed, 143 insertions(+), 29 deletions(-) create mode 100644 gtsam/sfm/TransferFactor.h create mode 100644 gtsam/sfm/tests/testTransferFactor.cpp diff --git a/gtsam/geometry/tests/testFundamentalMatrix.cpp b/gtsam/geometry/tests/testFundamentalMatrix.cpp index 80e6b1d3af..a8884e791e 100644 --- a/gtsam/geometry/tests/testFundamentalMatrix.cpp +++ b/gtsam/geometry/tests/testFundamentalMatrix.cpp @@ -197,35 +197,6 @@ TripleF generateTripleF( return {F[0], F[1], F[2]}; // Return a TripleF instance } -//************************************************************************* - -struct TripletFactor { - using F = FundamentalMatrix; - using SF = SimpleFundamentalMatrix; - Point2 p0, p1, p2; - - /// vector of errors returns 6D vector - Vector evaluateError(const SF& F01, const SF& F12, const SF& F20, // - Matrix* H01, Matrix* H12, Matrix* H20) const { - Vector error(6); - std::function fn = [&](const SF& F01, const SF& F12, - const SF& F20) { - Vector6 error; - error << F::transfer(F01.matrix(), p1, F20.matrix().transpose(), p2) - p0, - F::transfer(F01.matrix().transpose(), p0, F12.matrix(), p2) - p1, - F::transfer(F20.matrix(), p0, F12.matrix().transpose(), p1) - p2; - return error; - }; - if (H01) - *H01 = numericalDerivative31(fn, F01, F12, F20); - if (H12) - *H12 = numericalDerivative32(fn, F01, F12, F20); - if (H20) - *H20 = numericalDerivative33(fn, F01, F12, F20); - return fn(F01, F12, F20); - } -}; - //************************************************************************* TEST(TripleF, Transfer) { // Generate cameras on a circle, as well as fundamental matrices diff --git a/gtsam/sfm/TransferFactor.h b/gtsam/sfm/TransferFactor.h new file mode 100644 index 0000000000..43142e7622 --- /dev/null +++ b/gtsam/sfm/TransferFactor.h @@ -0,0 +1,48 @@ +/* ---------------------------------------------------------------------------- + * GTSAM Copyright 2010-2024, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + * See LICENSE for the license information + * -------------------------------------------------------------------------- */ + +/* + * @file TransferFactor.h + * @brief TransferFactor class + * @author Frank Dellaert + * @date October 24, 2024 + */ + +#pragma once + +#include +#include +#include + +namespace gtsam { + +template +struct TransferFactor { + Point2 p0, p1, p2; + + /// vector of errors returns 6D vector + Vector evaluateError(const F& F01, const F& F12, const F& F20, // + Matrix* H01, Matrix* H12, Matrix* H20) const { + Vector error(6); + std::function fn = [&](const F& F01, const F& F12, + const F& F20) { + Vector6 error; + error << // + F::transfer(F01.matrix(), p1, F20.matrix().transpose(), p2) - p0, + F::transfer(F01.matrix().transpose(), p0, F12.matrix(), p2) - p1, + F::transfer(F20.matrix(), p0, F12.matrix().transpose(), p1) - p2; + return error; + }; + if (H01) *H01 = numericalDerivative31(fn, F01, F12, F20); + if (H12) *H12 = numericalDerivative32(fn, F01, F12, F20); + if (H20) *H20 = numericalDerivative33(fn, F01, F12, F20); + return fn(F01, F12, F20); + } +}; + +} // namespace gtsam diff --git a/gtsam/sfm/tests/testTransferFactor.cpp b/gtsam/sfm/tests/testTransferFactor.cpp new file mode 100644 index 0000000000..3c3e896931 --- /dev/null +++ b/gtsam/sfm/tests/testTransferFactor.cpp @@ -0,0 +1,95 @@ +/* + * @file testTransferFactor.cpp + * @brief Test TransferFactor class + * @author Your Name + * @date October 23, 2024 + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +using namespace gtsam; + +//************************************************************************* +/// Generate three cameras on a circle, looking in +std::array generateCameraPoses() { + std::array cameraPoses; + const double radius = 1.0; + for (int i = 0; i < 3; ++i) { + double angle = i * 2.0 * M_PI / 3.0; + double c = cos(angle), s = sin(angle); + Rot3 aRb({-s, c, 0}, {0, 0, -1}, {-c, -s, 0}); + cameraPoses[i] = {aRb, Point3(radius * c, radius * s, 0)}; + } + return cameraPoses; +} + +// Function to generate a TripleF from camera poses +TripleF generateTripleF( + const std::array& cameraPoses) { + std::array F; + for (size_t i = 0; i < 3; ++i) { + size_t j = (i + 1) % 3; + const Pose3 iPj = cameraPoses[i].between(cameraPoses[j]); + EssentialMatrix E(iPj.rotation(), Unit3(iPj.translation())); + F[i] = {E, 1000.0, 1000.0, Point2(640 / 2, 480 / 2), + Point2(640 / 2, 480 / 2)}; + } + return {F[0], F[1], F[2]}; // Return a TripleF instance +} + +double focalLength = 1000; +Point2 principalPoint(640 / 2, 480 / 2); + +// Test for TransferFactor +TEST(TransferFactor, Jacobians) { + // Generate cameras on a circle + std::array cameraPoses = generateCameraPoses(); + auto triplet = generateTripleF(cameraPoses); + + // 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) { + // Project the point into each camera + PinholeCameraCal3_S2 camera(cameraPoses[i], K); + p[i] = camera.project(P); + } + + // Create a TransferFactor + TransferFactor factor{p[0], p[1], p[2]}; + Matrix H01, H12, H20; + Vector error = factor.evaluateError(triplet.F01, triplet.F12, triplet.F20, + &H01, &H12, &H20); + std::cout << "Error: " << error << std::endl; + std::cout << H01 << std::endl << std::endl; + std::cout << H12 << std::endl << std::endl; + std::cout << H20 << std::endl; + + // Check Jacobians + Values values; + values.insert(0, triplet.F01); + values.insert(1, triplet.F12); + values.insert(2, triplet.F20); + // EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-5, 1e-7); +} + +//************************************************************************* +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +//************************************************************************* From 34182bddda092b7db84899e7cf8cc7e497bfa64e Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Thu, 24 Oct 2024 12:21:37 -0700 Subject: [PATCH 05/20] TripletError vs TransferFactor --- gtsam/sfm/TransferFactor.h | 22 ++++++++++++++++++++-- gtsam/sfm/tests/testTransferFactor.cpp | 17 ++++++++++++----- 2 files changed, 32 insertions(+), 7 deletions(-) diff --git a/gtsam/sfm/TransferFactor.h b/gtsam/sfm/TransferFactor.h index 43142e7622..cec7a445ec 100644 --- a/gtsam/sfm/TransferFactor.h +++ b/gtsam/sfm/TransferFactor.h @@ -22,13 +22,12 @@ namespace gtsam { template -struct TransferFactor { +struct TripletError { Point2 p0, p1, p2; /// vector of errors returns 6D vector Vector evaluateError(const F& F01, const F& F12, const F& F20, // Matrix* H01, Matrix* H12, Matrix* H20) const { - Vector error(6); std::function fn = [&](const F& F01, const F& F12, const F& F20) { Vector6 error; @@ -45,4 +44,23 @@ struct TransferFactor { } }; +template +struct TransferFactor { + Point2 p0, p1, p2; + + /// vector of errors returns 2D vector + Vector evaluateError(const F& F12, const F& F20, // + Matrix* H12, Matrix* H20) const { + std::function fn = [&](const F& F12, const F& F20) { + Vector2 error; + error << // + F::transfer(F20.matrix(), p0, F12.matrix().transpose(), p1) - p2; + return error; + }; + if (H12) *H12 = numericalDerivative21(fn, F12, F20); + if (H20) *H20 = numericalDerivative22(fn, F12, F20); + return fn(F12, F20); + } +}; + } // namespace gtsam diff --git a/gtsam/sfm/tests/testTransferFactor.cpp b/gtsam/sfm/tests/testTransferFactor.cpp index 3c3e896931..44b03ad5dc 100644 --- a/gtsam/sfm/tests/testTransferFactor.cpp +++ b/gtsam/sfm/tests/testTransferFactor.cpp @@ -70,18 +70,25 @@ TEST(TransferFactor, Jacobians) { } // Create a TransferFactor - TransferFactor factor{p[0], p[1], p[2]}; + TripletError error{p[0], p[1], p[2]}; Matrix H01, H12, H20; - Vector error = factor.evaluateError(triplet.F01, triplet.F12, triplet.F20, - &H01, &H12, &H20); - std::cout << "Error: " << error << std::endl; + Vector e = error.evaluateError(triplet.F01, triplet.F12, triplet.F20, &H01, + &H12, &H20); + std::cout << "Error: " << e << std::endl; std::cout << H01 << std::endl << std::endl; std::cout << H12 << std::endl << std::endl; std::cout << H20 << std::endl; + // Create a TransferFactor + TransferFactor factor{p[0], p[1], p[2]}; + Matrix H0, H1; + Vector e2 = factor.evaluateError(triplet.F12, triplet.F20, &H0, &H1); + std::cout << "Error: " << e2 << std::endl; + std::cout << H0 << std::endl << std::endl; + std::cout << H1 << std::endl << std::endl; + // Check Jacobians Values values; - values.insert(0, triplet.F01); values.insert(1, triplet.F12); values.insert(2, triplet.F20); // EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-5, 1e-7); From 5f9eeb44157e231fecebb99bd0602dc6abc3792e Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Thu, 24 Oct 2024 13:09:41 -0700 Subject: [PATCH 06/20] Clean up/format --- gtsam/inference/LabeledSymbol.h | 130 ++++++++++++++++++++------------ 1 file changed, 80 insertions(+), 50 deletions(-) diff --git a/gtsam/inference/LabeledSymbol.h b/gtsam/inference/LabeledSymbol.h index 5a7a98c1d9..eb663ef9b3 100644 --- a/gtsam/inference/LabeledSymbol.h +++ b/gtsam/inference/LabeledSymbol.h @@ -19,9 +19,11 @@ #pragma once -#include +#include #include +#include + namespace gtsam { /** @@ -33,113 +35,141 @@ namespace gtsam { * which allows expressing "Pose 7 from robot B" as "xB7". */ class GTSAM_EXPORT LabeledSymbol { -protected: + protected: unsigned char c_, label_; std::uint64_t j_; -public: - /** Default constructor */ + public: + /// @name Constructors + /// @{ + + /// Default constructor LabeledSymbol(); - /** Copy constructor */ + /// Copy constructor LabeledSymbol(const LabeledSymbol& key); - /** Constructor */ + /// Constructor fro characters c and label, and integer j LabeledSymbol(unsigned char c, unsigned char label, std::uint64_t j); - /** Constructor that decodes an integer gtsam::Key */ - LabeledSymbol(gtsam::Key key); + /// Constructor that decodes an integer Key + LabeledSymbol(Key key); - /** Cast to integer */ - operator gtsam::Key() const; + /// @} + /// @name Testable + /// @{ - // Testable Requirements + /// Prints the LabeledSymbol with an optional prefix string. void print(const std::string& s = "") const; + /// Checks if this LabeledSymbol is equal to another, tolerance is ignored. bool equals(const LabeledSymbol& expected, double tol = 0.0) const { return (*this) == expected; } - /** return the integer version */ - gtsam::Key key() const { return (gtsam::Key) *this; } + /// @} + /// @name API + /// @{ - /** Retrieve label character */ + /// Cast to Key + operator Key() const; + + /// return the integer version + Key key() const { return (Key) * this; } + + /// Retrieve label character inline unsigned char label() const { return label_; } - /** Retrieve key character */ + /// Retrieve key character inline unsigned char chr() const { return c_; } - /** Retrieve key index */ + /// Retrieve key index inline size_t index() const { return j_; } - /** Create a string from the key */ + /// Create a string from the key operator std::string() const; - /** Comparison for use in maps */ + /// Output stream operator that can be used with key_formatter (see Key.h). + friend GTSAM_EXPORT std::ostream& operator<<(std::ostream&, + const LabeledSymbol&); + + /// @} + /// @name Comparison + /// @{ + bool operator<(const LabeledSymbol& comp) const; bool operator==(const LabeledSymbol& comp) const; - bool operator==(gtsam::Key comp) const; + bool operator==(Key comp) const; bool operator!=(const LabeledSymbol& comp) const; - bool operator!=(gtsam::Key comp) const; + bool operator!=(Key comp) const; - /** Return a filter function that returns true when evaluated on a gtsam::Key whose - * character (when converted to a LabeledSymbol) matches \c c. Use this with the - * Values::filter() function to retrieve all key-value pairs with the + /** Return a filter function that returns true when evaluated on a Key whose + * character (when converted to a LabeledSymbol) matches \c c. Use this with + * the Values::filter() function to retrieve all key-value pairs with the * requested character. */ - // Checks only the type - static std::function TypeTest(unsigned char c); + /// @} + /// @name Advanced API + /// @{ + + /// Checks only the type + static std::function TypeTest(unsigned char c); - // Checks only the robot ID (label_) - static std::function LabelTest(unsigned char label); + /// Checks only the robot ID (label_) + static std::function LabelTest(unsigned char label); - // Checks both type and the robot ID - static std::function TypeLabelTest(unsigned char c, unsigned char label); + /// Checks both type and the robot ID + static std::function TypeLabelTest(unsigned char c, + unsigned char label); - // Converts to upper/lower versions of labels + /// Converts to upper/lower versions of labels LabeledSymbol upper() const { return LabeledSymbol(c_, toupper(label_), j_); } LabeledSymbol lower() const { return LabeledSymbol(c_, tolower(label_), j_); } - // Create a new symbol with a different character. - LabeledSymbol newChr(unsigned char c) const { return LabeledSymbol(c, label_, j_); } - - // Create a new symbol with a different label. - LabeledSymbol newLabel(unsigned char label) const { return LabeledSymbol(c_, label, j_); } + /// Create a new symbol with a different character. + LabeledSymbol newChr(unsigned char c) const { + return LabeledSymbol(c, label_, j_); + } - /// Output stream operator that can be used with key_formatter (see Key.h). - friend GTSAM_EXPORT std::ostream &operator<<(std::ostream &, const LabeledSymbol &); + /// Create a new symbol with a different label. + LabeledSymbol newLabel(unsigned char label) const { + return LabeledSymbol(c_, label, j_); + } -private: + /// @} + private: #ifdef GTSAM_ENABLE_BOOST_SERIALIZATION - /** Serialization function */ + /// Serialization function friend class boost::serialization::access; - template - void serialize(ARCHIVE & ar, const unsigned int /*version*/) { - ar & BOOST_SERIALIZATION_NVP(c_); - ar & BOOST_SERIALIZATION_NVP(label_); - ar & BOOST_SERIALIZATION_NVP(j_); + template + void serialize(ARCHIVE& ar, const unsigned int /*version*/) { + ar& BOOST_SERIALIZATION_NVP(c_); + ar& BOOST_SERIALIZATION_NVP(label_); + ar& BOOST_SERIALIZATION_NVP(j_); } #endif -}; // \class LabeledSymbol +}; // \class LabeledSymbol /** Create a symbol key from a character, label and index, i.e. xA5. */ inline Key mrsymbol(unsigned char c, unsigned char label, size_t j) { - return (Key)LabeledSymbol(c,label,j); + return (Key)LabeledSymbol(c, label, j); } /** Return the character portion of a symbol key. */ inline unsigned char mrsymbolChr(Key key) { return LabeledSymbol(key).chr(); } /** Return the label portion of a symbol key. */ -inline unsigned char mrsymbolLabel(Key key) { return LabeledSymbol(key).label(); } +inline unsigned char mrsymbolLabel(Key key) { + return LabeledSymbol(key).label(); +} /** Return the index portion of a symbol key. */ inline size_t mrsymbolIndex(Key key) { return LabeledSymbol(key).index(); } /// traits -template<> struct traits : public Testable {}; - -} // \namespace gtsam +template <> +struct traits : public Testable {}; +} // namespace gtsam From bec4afa3f8543525b0532dbe8c7c285a158516cb Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Thu, 24 Oct 2024 13:09:58 -0700 Subject: [PATCH 07/20] EdgeKey for view graphs etc... --- gtsam/inference/EdgeKey.cpp | 33 ++++++++++++ gtsam/inference/EdgeKey.h | 74 +++++++++++++++++++++++++++ gtsam/inference/tests/testEdgeKey.cpp | 57 +++++++++++++++++++++ 3 files changed, 164 insertions(+) create mode 100644 gtsam/inference/EdgeKey.cpp create mode 100644 gtsam/inference/EdgeKey.h create mode 100644 gtsam/inference/tests/testEdgeKey.cpp diff --git a/gtsam/inference/EdgeKey.cpp b/gtsam/inference/EdgeKey.cpp new file mode 100644 index 0000000000..d44f7a2a27 --- /dev/null +++ b/gtsam/inference/EdgeKey.cpp @@ -0,0 +1,33 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file EdgeKey.cpp + * @date Oct 24, 2024 + * @author: Frank Dellaert + * @author: Akshay Krishnan + */ + +#include + +namespace gtsam { + +// Output stream operator implementation +GTSAM_EXPORT std::ostream& operator<<(std::ostream& os, const EdgeKey& key) { + os << "{" << key.i() << ", " << key.j() << "}"; + return os; +} + +void EdgeKey::print(const std::string& s) const { + std::cout << s << *this << std::endl; +} + +} // namespace gtsam diff --git a/gtsam/inference/EdgeKey.h b/gtsam/inference/EdgeKey.h new file mode 100644 index 0000000000..13de55f9ee --- /dev/null +++ b/gtsam/inference/EdgeKey.h @@ -0,0 +1,74 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +#pragma once + +/** + * @file EdgeKey.h + * @date Oct 24, 2024 + * @author: Frank Dellaert + * @author: Akshay Krishnan + */ + +#include +#include + +namespace gtsam { +class GTSAM_EXPORT EdgeKey { + protected: + std::uint32_t i_; ///< Upper 32 bits + std::uint32_t j_; ///< Lower 32 bits + + public: + /// @name Constructors + /// @{ + + /// Default constructor + EdgeKey() : i_(0), j_(0) {} + + /// Constructor + EdgeKey(std::uint32_t i, std::uint32_t j) : i_(i), j_(j) {} + + /// @} + /// @name API + /// @{ + + /// Cast to Key + operator Key() const { return ((std::uint64_t)i_ << 32) | j_; } + + /// Retrieve high 32 bits + inline std::uint32_t i() const { return i_; } + + /// Retrieve low 32 bits + inline std::uint32_t j() const { return j_; } + + /// Output stream operator + friend GTSAM_EXPORT std::ostream& operator<<(std::ostream&, const EdgeKey&); + + /// @} + /// @name Testable + /// @{ + + /// Prints the EdgeKey with an optional prefix string. + void print(const std::string& s = "") const; + + /// Checks if this EdgeKey is equal to another, tolerance is ignored. + bool equals(const EdgeKey& expected, double tol = 0.0) const { + return (*this) == expected; + } + /// @} +}; + +/// traits +template <> +struct traits : public Testable {}; + +} // namespace gtsam diff --git a/gtsam/inference/tests/testEdgeKey.cpp b/gtsam/inference/tests/testEdgeKey.cpp new file mode 100644 index 0000000000..88fafbdd1f --- /dev/null +++ b/gtsam/inference/tests/testEdgeKey.cpp @@ -0,0 +1,57 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/* + * @file testEdgeKey.cpp + * @date Oct 24, 2024 + * @author: Frank Dellaert + * @author: Akshay Krishnan + */ + +#include +#include + +#include + +using namespace std; +using namespace gtsam; + +/* ************************************************************************* */ +TEST(EdgeKey, Construction) { + EdgeKey edge(1, 2); + EXPECT(edge.i() == 1); + EXPECT(edge.j() == 2); +} + +/* ************************************************************************* */ +TEST(EdgeKey, Equality) { + EdgeKey edge1(1, 2); + EdgeKey edge2(1, 2); + EdgeKey edge3(2, 3); + + EXPECT(assert_equal(edge1, edge2)); + EXPECT(!edge1.equals(edge3)); +} + +/* ************************************************************************* */ +TEST(EdgeKey, StreamOutput) { + EdgeKey edge(1, 2); + std::ostringstream oss; + oss << edge; + EXPECT("{1, 2}" == oss.str()); +} + +/* ************************************************************************* */ +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +/* ************************************************************************* */ From 68c9da8c4e7b3c333dfe394e5b58a38e4bd1d9b5 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Thu, 24 Oct 2024 13:21:29 -0700 Subject: [PATCH 08/20] More cleanup --- gtsam/inference/LabeledSymbol.h | 26 ++++++++++++++------------ gtsam/sfm/TranslationFactor.h | 4 ++-- gtsam/sfm/TranslationRecovery.h | 4 ++-- 3 files changed, 18 insertions(+), 16 deletions(-) diff --git a/gtsam/inference/LabeledSymbol.h b/gtsam/inference/LabeledSymbol.h index eb663ef9b3..927fb76692 100644 --- a/gtsam/inference/LabeledSymbol.h +++ b/gtsam/inference/LabeledSymbol.h @@ -68,7 +68,7 @@ class GTSAM_EXPORT LabeledSymbol { } /// @} - /// @name API + /// @name Standard API /// @{ /// Cast to Key @@ -103,15 +103,13 @@ class GTSAM_EXPORT LabeledSymbol { bool operator!=(const LabeledSymbol& comp) const; bool operator!=(Key comp) const; - /** Return a filter function that returns true when evaluated on a Key whose - * character (when converted to a LabeledSymbol) matches \c c. Use this with - * the Values::filter() function to retrieve all key-value pairs with the - * requested character. - */ - /// @} - /// @name Advanced API + /// @name Filtering /// @{ + /// Return a filter function that returns true when evaluated on a Key whose + /// character (when converted to a LabeledSymbol) matches \c c. Use this with + /// the Values::filter() function to retrieve all key-value pairs with the + /// requested character. /// Checks only the type static std::function TypeTest(unsigned char c); @@ -123,6 +121,10 @@ class GTSAM_EXPORT LabeledSymbol { static std::function TypeLabelTest(unsigned char c, unsigned char label); + /// @} + /// @name Advanced API + /// @{ + /// Converts to upper/lower versions of labels LabeledSymbol upper() const { return LabeledSymbol(c_, toupper(label_), j_); } LabeledSymbol lower() const { return LabeledSymbol(c_, tolower(label_), j_); } @@ -152,20 +154,20 @@ class GTSAM_EXPORT LabeledSymbol { #endif }; // \class LabeledSymbol -/** Create a symbol key from a character, label and index, i.e. xA5. */ +/// Create a symbol key from a character, label and index, i.e. xA5. inline Key mrsymbol(unsigned char c, unsigned char label, size_t j) { return (Key)LabeledSymbol(c, label, j); } -/** Return the character portion of a symbol key. */ +/// Return the character portion of a symbol key. inline unsigned char mrsymbolChr(Key key) { return LabeledSymbol(key).chr(); } -/** Return the label portion of a symbol key. */ +/// Return the label portion of a symbol key. inline unsigned char mrsymbolLabel(Key key) { return LabeledSymbol(key).label(); } -/** Return the index portion of a symbol key. */ +/// Return the index portion of a symbol key. inline size_t mrsymbolIndex(Key key) { return LabeledSymbol(key).index(); } /// traits diff --git a/gtsam/sfm/TranslationFactor.h b/gtsam/sfm/TranslationFactor.h index 33bc82f5a9..cfb9cff20d 100644 --- a/gtsam/sfm/TranslationFactor.h +++ b/gtsam/sfm/TranslationFactor.h @@ -55,7 +55,7 @@ class TranslationFactor : public NoiseModelFactorN { : Base(noiseModel, a, b), measured_w_aZb_(w_aZb.point3()) {} /** - * @brief Caclulate error: (norm(Tb - Ta) - measurement) + * @brief Calculate error: (norm(Tb - Ta) - measurement) * where Tb and Ta are Point3 translations and measurement is * the Unit3 translation direction from a to b. * @@ -120,7 +120,7 @@ class BilinearAngleTranslationFactor using NoiseModelFactor2::evaluateError; /** - * @brief Caclulate error: (scale * (Tb - Ta) - measurement) + * @brief Calculate error: (scale * (Tb - Ta) - measurement) * where Tb and Ta are Point3 translations and measurement is * the Unit3 translation direction from a to b. * diff --git a/gtsam/sfm/TranslationRecovery.h b/gtsam/sfm/TranslationRecovery.h index a91ef01f94..f821da9755 100644 --- a/gtsam/sfm/TranslationRecovery.h +++ b/gtsam/sfm/TranslationRecovery.h @@ -119,7 +119,7 @@ class GTSAM_EXPORT TranslationRecovery { * @param betweenTranslations relative translations (with scale) between 2 * points in world coordinate frame known a priori. * @param rng random number generator - * @param intialValues (optional) initial values from a prior + * @param initialValues (optional) initial values from a prior * @return Values */ Values initializeRandomly( @@ -156,7 +156,7 @@ class GTSAM_EXPORT TranslationRecovery { * points in world coordinate frame known a priori. Unlike * relativeTranslations, zero-magnitude betweenTranslations are not treated as * hard constraints. - * @param initialValues intial values for optimization. Initializes randomly + * @param initialValues initial values for optimization. Initializes randomly * if not provided. * @return Values */ From e6bfcada40375bd8a742e111bcdfa58d994b0e3b Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Thu, 24 Oct 2024 13:23:00 -0700 Subject: [PATCH 09/20] True factor --- gtsam/sfm/TransferFactor.h | 11 +++++++++-- gtsam/sfm/tests/testTransferFactor.cpp | 8 ++++---- 2 files changed, 13 insertions(+), 6 deletions(-) diff --git a/gtsam/sfm/TransferFactor.h b/gtsam/sfm/TransferFactor.h index cec7a445ec..fd65db8b9a 100644 --- a/gtsam/sfm/TransferFactor.h +++ b/gtsam/sfm/TransferFactor.h @@ -45,12 +45,19 @@ struct TripletError { }; template -struct TransferFactor { +class TransferFactor : public NoiseModelFactorN { Point2 p0, p1, p2; + public: + // Constructor + TransferFactor(Key key1, Key key2, const Point2& p0, const Point2& p1, + const Point2& p2, const SharedNoiseModel& model = nullptr) + : NoiseModelFactorN(model, key1, key2), p0(p0), p1(p1), p2(p2) {} + /// vector of errors returns 2D vector Vector evaluateError(const F& F12, const F& F20, // - Matrix* H12, Matrix* H20) const { + OptionalMatrixType H12 = nullptr, + OptionalMatrixType H20 = nullptr) const override { std::function fn = [&](const F& F12, const F& F20) { Vector2 error; error << // diff --git a/gtsam/sfm/tests/testTransferFactor.cpp b/gtsam/sfm/tests/testTransferFactor.cpp index 44b03ad5dc..afceb8fdb3 100644 --- a/gtsam/sfm/tests/testTransferFactor.cpp +++ b/gtsam/sfm/tests/testTransferFactor.cpp @@ -80,7 +80,7 @@ TEST(TransferFactor, Jacobians) { std::cout << H20 << std::endl; // Create a TransferFactor - TransferFactor factor{p[0], p[1], p[2]}; + TransferFactor factor{0, 1, p[0], p[1], p[2]}; Matrix H0, H1; Vector e2 = factor.evaluateError(triplet.F12, triplet.F20, &H0, &H1); std::cout << "Error: " << e2 << std::endl; @@ -89,9 +89,9 @@ TEST(TransferFactor, Jacobians) { // Check Jacobians Values values; - values.insert(1, triplet.F12); - values.insert(2, triplet.F20); - // EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-5, 1e-7); + values.insert(0, triplet.F12); + values.insert(1, triplet.F20); + EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-5, 1e-7); } //************************************************************************* From ade1207334a7d2acc42f6879501b1a9e63c3d3ed Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Thu, 24 Oct 2024 14:50:31 -0700 Subject: [PATCH 10/20] Use EdgeKey logic --- gtsam/sfm/TransferFactor.h | 105 ++++++++++++++++--------- gtsam/sfm/tests/testTransferFactor.cpp | 55 ++++++------- 2 files changed, 91 insertions(+), 69 deletions(-) diff --git a/gtsam/sfm/TransferFactor.h b/gtsam/sfm/TransferFactor.h index fd65db8b9a..0f9f95c132 100644 --- a/gtsam/sfm/TransferFactor.h +++ b/gtsam/sfm/TransferFactor.h @@ -17,56 +17,83 @@ #include #include +#include #include namespace gtsam { -template -struct TripletError { - Point2 p0, p1, p2; - - /// vector of errors returns 6D vector - Vector evaluateError(const F& F01, const F& F12, const F& F20, // - Matrix* H01, Matrix* H12, Matrix* H20) const { - std::function fn = [&](const F& F01, const F& F12, - const F& F20) { - Vector6 error; - error << // - F::transfer(F01.matrix(), p1, F20.matrix().transpose(), p2) - p0, - F::transfer(F01.matrix().transpose(), p0, F12.matrix(), p2) - p1, - F::transfer(F20.matrix(), p0, F12.matrix().transpose(), p1) - p2; - return error; - }; - if (H01) *H01 = numericalDerivative31(fn, F01, F12, F20); - if (H12) *H12 = numericalDerivative32(fn, F01, F12, F20); - if (H20) *H20 = numericalDerivative33(fn, F01, F12, F20); - return fn(F01, F12, F20); - } -}; - +/** + * 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 + * between the transferred points `pi` and `pj`, and the actual point `pk` in + * the target view. Jacobians are done using numerical differentiation. + */ template class TransferFactor : public NoiseModelFactorN { - Point2 p0, p1, p2; + EdgeKey key1_, key2_; ///< the two EdgeKeys + Point2 pi, pj, pk; ///< The points in the three views public: - // Constructor - TransferFactor(Key key1, Key key2, const Point2& p0, const Point2& p1, - const Point2& p2, const SharedNoiseModel& model = nullptr) - : NoiseModelFactorN(model, key1, key2), p0(p0), p1(p1), p2(p2) {} + /** + * @brief Constructor for the TransferFactor class. + * + * Uses EdgeKeys to determine how to use the two fundamental matrix unknowns + * F1 and F2, to transfer points pi and pj to the third view, and minimize the + * difference with pk. + * + * The edge keys must represent valid edges for the transfer operation, + * specifically one of the following configurations: + * - (i, k) and (j, k) + * - (i, k) and (k, j) + * - (k, i) and (j, k) + * - (k, i) and (k, j) + * + * @param key1 First EdgeKey specifying F1: (i, k) or (k, i). + * @param key2 Second EdgeKey specifying F2: (j, k) or (k, j). + * @param pi The point in the first view (i). + * @param pj The point in the second view (j). + * @param pk The point in the third (and transfer target) view (k). + * @param model An optional SharedNoiseModel that defines the noise model + * for this factor. Defaults to nullptr. + */ + TransferFactor(EdgeKey key1, EdgeKey key2, const Point2& pi, const Point2& pj, + const Point2& pk, const SharedNoiseModel& model = nullptr) + : NoiseModelFactorN(model, key1, key2), + key1_(key1), + key2_(key2), + pi(pi), + pj(pj), + pk(pk) {} + + // Create Matrix3 objects based on EdgeKey configurations + std::pair getMatrices(const F& F1, const F& F2) const { + // Fill Fki and Fkj based on EdgeKey configurations + if (key1_.i() == key2_.i()) { + return {F1.matrix(), F2.matrix()}; + } else if (key1_.i() == key2_.j()) { + return {F1.matrix(), F2.matrix().transpose()}; + } else if (key1_.j() == key2_.i()) { + return {F1.matrix().transpose(), F2.matrix()}; + } else if (key1_.j() == key2_.j()) { + return {F1.matrix().transpose(), F2.matrix().transpose()}; + } else { + throw std::runtime_error( + "TransferFactor: invalid EdgeKey configuration."); + } + } /// vector of errors returns 2D vector - Vector evaluateError(const F& F12, const F& F20, // - OptionalMatrixType H12 = nullptr, - OptionalMatrixType H20 = nullptr) const override { - std::function fn = [&](const F& F12, const F& F20) { - Vector2 error; - error << // - F::transfer(F20.matrix(), p0, F12.matrix().transpose(), p1) - p2; - return error; + Vector evaluateError(const F& F1, const F& F2, + OptionalMatrixType H1 = nullptr, + OptionalMatrixType H2 = nullptr) const override { + std::function transfer = [&](const F& F1, const F& F2) { + auto [Fki, Fkj] = getMatrices(F1, F2); + return F::transfer(Fki, pi, Fkj, pj); }; - if (H12) *H12 = numericalDerivative21(fn, F12, F20); - if (H20) *H20 = numericalDerivative22(fn, F12, F20); - return fn(F12, F20); + if (H1) *H1 = numericalDerivative21(transfer, F1, F2); + if (H2) *H2 = numericalDerivative22(transfer, F1, F2); + return transfer(F1, F2) - pk; } }; diff --git a/gtsam/sfm/tests/testTransferFactor.cpp b/gtsam/sfm/tests/testTransferFactor.cpp index afceb8fdb3..7bcd71b1fe 100644 --- a/gtsam/sfm/tests/testTransferFactor.cpp +++ b/gtsam/sfm/tests/testTransferFactor.cpp @@ -6,20 +6,15 @@ */ #include -#include -#include -#include -#include -#include -#include #include -#include -#include #include #include using namespace gtsam; +double focalLength = 1000; +Point2 principalPoint(640 / 2, 480 / 2); + //************************************************************************* /// Generate three cameras on a circle, looking in std::array generateCameraPoses() { @@ -34,6 +29,7 @@ std::array generateCameraPoses() { return cameraPoses; } +//************************************************************************* // Function to generate a TripleF from camera poses TripleF generateTripleF( const std::array& cameraPoses) { @@ -48,9 +44,7 @@ TripleF generateTripleF( return {F[0], F[1], F[2]}; // Return a TripleF instance } -double focalLength = 1000; -Point2 principalPoint(640 / 2, 480 / 2); - +//************************************************************************* // Test for TransferFactor TEST(TransferFactor, Jacobians) { // Generate cameras on a circle @@ -70,28 +64,29 @@ TEST(TransferFactor, Jacobians) { } // Create a TransferFactor - TripletError error{p[0], p[1], p[2]}; - Matrix H01, H12, H20; - Vector e = error.evaluateError(triplet.F01, triplet.F12, triplet.F20, &H01, - &H12, &H20); - std::cout << "Error: " << e << std::endl; - std::cout << H01 << std::endl << std::endl; - std::cout << H12 << std::endl << std::endl; - std::cout << H20 << std::endl; + EdgeKey key01(0, 1), key12(1, 2), key20(2, 0); + TransferFactor // + factor0{key01, key20, p[1], p[2], p[0]}, + factor1{key12, key01, p[2], p[0], p[1]}, + factor2{key20, key12, p[0], p[1], p[2]}; - // Create a TransferFactor - TransferFactor factor{0, 1, p[0], p[1], p[2]}; - Matrix H0, H1; - Vector e2 = factor.evaluateError(triplet.F12, triplet.F20, &H0, &H1); - std::cout << "Error: " << e2 << std::endl; - std::cout << H0 << std::endl << std::endl; - std::cout << H1 << std::endl << std::endl; + // 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)); - // Check Jacobians + // Create Values with edge keys Values values; - values.insert(0, triplet.F12); - values.insert(1, triplet.F20); - EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-5, 1e-7); + values.insert(key01, triplet.Fab); + values.insert(key12, triplet.Fbc); + values.insert(key20, triplet.Fca); + + // Check error and Jacobians + for (auto&& f : {factor0, factor1, factor2}) { + Vector error = f.unwhitenedError(values); + EXPECT(assert_equal(Z_2x1, error)); + EXPECT_CORRECT_FACTOR_JACOBIANS(f, values, 1e-5, 1e-7); + } } //************************************************************************* From bf47ef3432e41e46b96b17a3e2a7ca73b31247b8 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Thu, 24 Oct 2024 15:08:29 -0700 Subject: [PATCH 11/20] Got rid of inheritance --- gtsam/sfm/TransferFactor.h | 17 ++++++++++------- gtsam/sfm/tests/testTransferFactor.cpp | 2 +- 2 files changed, 11 insertions(+), 8 deletions(-) diff --git a/gtsam/sfm/TransferFactor.h b/gtsam/sfm/TransferFactor.h index 0f9f95c132..5c46f9b9bd 100644 --- a/gtsam/sfm/TransferFactor.h +++ b/gtsam/sfm/TransferFactor.h @@ -67,15 +67,18 @@ class TransferFactor : public NoiseModelFactorN { pk(pk) {} // Create Matrix3 objects based on EdgeKey configurations - std::pair getMatrices(const F& F1, const F& F2) const { + static std::pair getMatrices(const EdgeKey& key1, + const F& F1, + const EdgeKey& key2, + const F& F2) { // Fill Fki and Fkj based on EdgeKey configurations - if (key1_.i() == key2_.i()) { + if (key1.i() == key2.i()) { return {F1.matrix(), F2.matrix()}; - } else if (key1_.i() == key2_.j()) { + } else if (key1.i() == key2.j()) { return {F1.matrix(), F2.matrix().transpose()}; - } else if (key1_.j() == key2_.i()) { + } else if (key1.j() == key2.i()) { return {F1.matrix().transpose(), F2.matrix()}; - } else if (key1_.j() == key2_.j()) { + } else if (key1.j() == key2.j()) { return {F1.matrix().transpose(), F2.matrix().transpose()}; } else { throw std::runtime_error( @@ -88,8 +91,8 @@ class TransferFactor : public NoiseModelFactorN { OptionalMatrixType H1 = nullptr, OptionalMatrixType H2 = nullptr) const override { std::function transfer = [&](const F& F1, const F& F2) { - auto [Fki, Fkj] = getMatrices(F1, F2); - return F::transfer(Fki, pi, Fkj, pj); + auto [Fki, Fkj] = getMatrices(key1_, F1, key2_, F2); + return Transfer(Fki, pi, Fkj, pj); }; if (H1) *H1 = numericalDerivative21(transfer, F1, F2); if (H2) *H2 = numericalDerivative22(transfer, F1, F2); diff --git a/gtsam/sfm/tests/testTransferFactor.cpp b/gtsam/sfm/tests/testTransferFactor.cpp index 7bcd71b1fe..d26bad960a 100644 --- a/gtsam/sfm/tests/testTransferFactor.cpp +++ b/gtsam/sfm/tests/testTransferFactor.cpp @@ -71,7 +71,7 @@ TEST(TransferFactor, Jacobians) { factor2{key20, key12, p[0], p[1], p[2]}; // Check that getMatrices is correct - auto [Fki, Fkj] = factor2.getMatrices(triplet.Fca, triplet.Fbc); + auto [Fki, Fkj] = factor2.getMatrices(key20, triplet.Fca, key12, triplet.Fbc); EXPECT(assert_equal(triplet.Fca.matrix(), Fki)); EXPECT(assert_equal(triplet.Fbc.matrix().transpose(), Fkj)); From 6f2b49d80d4713c22e86e9888c5a59b02a0a0371 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Thu, 24 Oct 2024 16:40:35 -0700 Subject: [PATCH 12/20] Additional API --- gtsam/inference/EdgeKey.cpp | 7 +++++-- gtsam/inference/EdgeKey.h | 7 +++++++ 2 files changed, 12 insertions(+), 2 deletions(-) diff --git a/gtsam/inference/EdgeKey.cpp b/gtsam/inference/EdgeKey.cpp index d44f7a2a27..f8caf8ce1b 100644 --- a/gtsam/inference/EdgeKey.cpp +++ b/gtsam/inference/EdgeKey.cpp @@ -20,9 +20,12 @@ namespace gtsam { -// Output stream operator implementation +EdgeKey::operator std::string() const { + return "{" + std::to_string(i_) + ", " + std::to_string(j_) + "}"; +} + GTSAM_EXPORT std::ostream& operator<<(std::ostream& os, const EdgeKey& key) { - os << "{" << key.i() << ", " << key.j() << "}"; + os << (std::string)key; return os; } diff --git a/gtsam/inference/EdgeKey.h b/gtsam/inference/EdgeKey.h index 13de55f9ee..bf1bf60300 100644 --- a/gtsam/inference/EdgeKey.h +++ b/gtsam/inference/EdgeKey.h @@ -37,6 +37,10 @@ class GTSAM_EXPORT EdgeKey { /// Constructor EdgeKey(std::uint32_t i, std::uint32_t j) : i_(i), j_(j) {} + EdgeKey(Key key) + : i_(static_cast(key >> 32)), + j_(static_cast(key)) {} + /// @} /// @name API /// @{ @@ -50,6 +54,9 @@ class GTSAM_EXPORT EdgeKey { /// Retrieve low 32 bits inline std::uint32_t j() const { return j_; } + /** Create a string from the key */ + operator std::string() const; + /// Output stream operator friend GTSAM_EXPORT std::ostream& operator<<(std::ostream&, const EdgeKey&); From 77754fd69b9a984f4419089d2975f6ebe825cce0 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Thu, 24 Oct 2024 16:40:53 -0700 Subject: [PATCH 13/20] Example with SimpleF --- examples/SFMdata.h | 27 ++++++-- examples/ViewGraphExample.cpp | 113 ++++++++++++++++++++++++++++++++++ 2 files changed, 134 insertions(+), 6 deletions(-) create mode 100644 examples/ViewGraphExample.cpp diff --git a/examples/SFMdata.h b/examples/SFMdata.h index 112bd927ce..f2561b4900 100644 --- a/examples/SFMdata.h +++ b/examples/SFMdata.h @@ -78,12 +78,27 @@ std::vector createPoses( /** * Create regularly spaced poses with specified radius and number of cameras */ -std::vector posesOnCircle(int num_cameras = 8, double radius = 30) { - const Pose3 init(Rot3::Ypr(M_PI_2, 0, -M_PI_2), {radius, 0, 0}); - const double theta = M_PI / num_cameras; - const Pose3 delta( - Rot3::Ypr(0, -2 * theta, 0), - {sin(2 * theta) * radius, 0, radius * (1 - sin(2 * theta))}); +std::vector posesOnCircle(int num_cameras = 8, double R = 30) { + const double theta = 2 * M_PI / num_cameras; + + // Initial pose at angle 0, position (R, 0, 0), facing the center with Y-axis + // pointing down + const Pose3 init(Rot3::Ypr(M_PI_2, 0, -M_PI_2), {R, 0, 0}); + + // Delta rotation: rotate by -theta around Z-axis (counterclockwise movement) + Rot3 delta_rotation = Rot3::Ypr(0, -theta, 0); + + // Delta translation in world frame + Vector3 delta_translation_world(R * (cos(theta) - 1), R * sin(theta), 0); + + // Transform delta translation to local frame of the camera + Vector3 delta_translation_local = + init.rotation().inverse() * delta_translation_world; + + // Define delta pose + const Pose3 delta(delta_rotation, delta_translation_local); + + // Generate poses using createPoses return createPoses(init, delta, num_cameras); } } // namespace gtsam \ No newline at end of file diff --git a/examples/ViewGraphExample.cpp b/examples/ViewGraphExample.cpp new file mode 100644 index 0000000000..6d51319424 --- /dev/null +++ b/examples/ViewGraphExample.cpp @@ -0,0 +1,113 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file ViewGraphExample.cpp + * @brief View-graph calibration on a simulated dataset, a la Sweeney 2015 + * @author Frank Dellaert + * @author October 2024 + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#include "SFMdata.h" +#include "gtsam/geometry/EssentialMatrix.h" +#include "gtsam/inference/Key.h" + +using namespace std; +using namespace gtsam; + +/* ************************************************************************* */ +int main(int argc, char* argv[]) { + // Define the camera calibration parameters + Cal3_S2::shared_ptr K(new Cal3_S2(50.0, 50.0, 0.0, 50.0, 50.0)); + + // Create the set of 8 ground-truth landmarks + vector points = createPoints(); + + // Create the set of 4 ground-truth poses + vector poses = posesOnCircle(4, 30); + + // Simulate measurements from each camera pose + std::array, 4> p; + for (size_t i = 0; i < 4; ++i) { + GTSAM_PRINT(poses[i]); + PinholeCamera camera(poses[i], *K); + for (size_t j = 0; j < 8; ++j) { + cout << "Camera index: " << i << ", Landmark index: " << j << endl; + p[i][j] = camera.project(points[j]); + } + } + + // Create a factor graph + NonlinearFactorGraph graph; + + using Factor = TransferFactor; + for (size_t a = 0; a < 4; ++a) { + size_t b = (a + 1) % 4; // Next camera + size_t c = (a + 2) % 4; // Camera after next + for (size_t j = 0; j < 4; ++j) { + graph.emplace_shared(EdgeKey(a, c), EdgeKey(b, c), p[a][j], + p[b][j], p[c][j]); + graph.emplace_shared(EdgeKey(a, b), EdgeKey(b, c), p[a][j], + p[c][j], p[b][j]); + graph.emplace_shared(EdgeKey(a, c), EdgeKey(a, b), p[c][j], + p[b][j], p[a][j]); + } + } + + auto formatter = [](Key key) { + EdgeKey edge(key); + return (std::string)edge; + }; + + graph.print("Factor Graph:\n", formatter); + + // Create the data structure to hold the initial estimate to the solution + Values initialEstimate; + const Point2 center(50, 50); + auto E1 = EssentialMatrix::FromPose3(poses[0].between(poses[1])); + auto E2 = EssentialMatrix::FromPose3(poses[0].between(poses[2])); + for (size_t a = 0; a < 4; ++a) { + size_t b = (a + 1) % 4; // Next camera + size_t c = (a + 2) % 4; // Camera after next + initialEstimate.insert(EdgeKey(a, b), + SimpleFundamentalMatrix(E1, 50, 50, center, center)); + initialEstimate.insert(EdgeKey(a, c), + SimpleFundamentalMatrix(E2, 50, 50, center, center)); + } + initialEstimate.print("Initial Estimates:\n", formatter); + // graph.printErrors(initialEstimate, "errors: ", formatter); + + /* Optimize the graph and print results */ + LevenbergMarquardtParams params; + params.setlambdaInitial(1000.0); // Initialize lambda to a high value + params.setVerbosityLM("SUMMARY"); + Values result = + LevenbergMarquardtOptimizer(graph, initialEstimate, params).optimize(); + result.print("Final results:\n", formatter); + cout << "initial error = " << graph.error(initialEstimate) << endl; + cout << "final error = " << graph.error(result) << endl; + + return 0; +} +/* ************************************************************************* */ From 7d95505d11fd03b2d4ff06da11f8ac9720b2621b Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Thu, 24 Oct 2024 17:04:11 -0700 Subject: [PATCH 14/20] Init from pose and Ks --- gtsam/geometry/FundamentalMatrix.cpp | 4 +--- gtsam/geometry/FundamentalMatrix.h | 17 +++++++++++++++++ 2 files changed, 18 insertions(+), 3 deletions(-) diff --git a/gtsam/geometry/FundamentalMatrix.cpp b/gtsam/geometry/FundamentalMatrix.cpp index 57d59d0a04..af322a3c08 100644 --- a/gtsam/geometry/FundamentalMatrix.cpp +++ b/gtsam/geometry/FundamentalMatrix.cpp @@ -72,9 +72,7 @@ Matrix3 FundamentalMatrix::matrix() const { } void FundamentalMatrix::print(const std::string& s) const { - std::cout << s << "U:\n" - << U_.matrix() << "\ns: " << s_ << "\nV:\n" - << V_.matrix() << std::endl; + std::cout << s << matrix() << std::endl; } bool FundamentalMatrix::equals(const FundamentalMatrix& other, diff --git a/gtsam/geometry/FundamentalMatrix.h b/gtsam/geometry/FundamentalMatrix.h index 497f95cf77..f8aa37bf82 100644 --- a/gtsam/geometry/FundamentalMatrix.h +++ b/gtsam/geometry/FundamentalMatrix.h @@ -54,6 +54,23 @@ class GTSAM_EXPORT FundamentalMatrix { */ FundamentalMatrix(const Matrix3& F); + /** + * @brief Construct from calibration matrices Ka, Kb, and pose aPb + * + * Initializes the GeneralFundamentalMatrix from the given calibration + * matrices Ka and Kb, and the pose aPb. + * + * @tparam CAL Calibration type, expected to have a matrix() method + * @param Ka Calibration matrix for the left camera + * @param aPb Pose from the left to the right camera + * @param Kb Calibration matrix for the right camera + */ + template + GeneralFundamentalMatrix(const CAL& Ka, const Pose3& aPb, const CAL& Kb) + : GeneralFundamentalMatrix(Ka.K().transpose().inverse() * + EssentialMatrix::FromPose3(aPb).matrix() * + Kb.K().inverse()) {} + /// Return the fundamental matrix representation Matrix3 matrix() const; From ca199f9c08406f5d363579e667a70d083710aaf4 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Thu, 24 Oct 2024 17:04:24 -0700 Subject: [PATCH 15/20] Switch to general F --- examples/SFMExample.cpp | 4 +-- examples/ViewGraphExample.cpp | 48 +++++++++++++++++++++++------------ 2 files changed, 34 insertions(+), 18 deletions(-) diff --git a/examples/SFMExample.cpp b/examples/SFMExample.cpp index fddca81694..0b4dc4036f 100644 --- a/examples/SFMExample.cpp +++ b/examples/SFMExample.cpp @@ -39,7 +39,7 @@ // Finally, once all of the factors have been added to our factor graph, we will want to // solve/optimize to graph to find the best (Maximum A Posteriori) set of variable values. // GTSAM includes several nonlinear optimizers to perform this step. Here we will use a -// trust-region method known as Powell's Degleg +// trust-region method known as Powell's Dogleg #include // The nonlinear solvers within GTSAM are iterative solvers, meaning they linearize the @@ -57,7 +57,7 @@ using namespace gtsam; /* ************************************************************************* */ int main(int argc, char* argv[]) { // Define the camera calibration parameters - Cal3_S2::shared_ptr K(new Cal3_S2(50.0, 50.0, 0.0, 50.0, 50.0)); + auto K = std::make_shared(50.0, 50.0, 0.0, 50.0, 50.0); // Define the camera observation noise model auto measurementNoise = diff --git a/examples/ViewGraphExample.cpp b/examples/ViewGraphExample.cpp index 6d51319424..7b5ed0fc6e 100644 --- a/examples/ViewGraphExample.cpp +++ b/examples/ViewGraphExample.cpp @@ -30,7 +30,6 @@ #include #include "SFMdata.h" -#include "gtsam/geometry/EssentialMatrix.h" #include "gtsam/inference/Key.h" using namespace std; @@ -39,7 +38,7 @@ using namespace gtsam; /* ************************************************************************* */ int main(int argc, char* argv[]) { // Define the camera calibration parameters - Cal3_S2::shared_ptr K(new Cal3_S2(50.0, 50.0, 0.0, 50.0, 50.0)); + Cal3_S2 K(50.0, 50.0, 0.0, 50.0, 50.0); // Create the set of 8 ground-truth landmarks vector points = createPoints(); @@ -47,25 +46,36 @@ int main(int argc, char* argv[]) { // Create the set of 4 ground-truth poses vector poses = posesOnCircle(4, 30); + // Calculate ground truth fundamental matrices, 1 and 2 poses apart + auto F1 = GeneralFundamentalMatrix(K, poses[0].between(poses[1]), K); + auto F2 = GeneralFundamentalMatrix(K, poses[0].between(poses[2]), K); + // Simulate measurements from each camera pose std::array, 4> p; for (size_t i = 0; i < 4; ++i) { - GTSAM_PRINT(poses[i]); - PinholeCamera camera(poses[i], *K); + PinholeCamera camera(poses[i], K); for (size_t j = 0; j < 8; ++j) { - cout << "Camera index: " << i << ", Landmark index: " << j << endl; p[i][j] = camera.project(points[j]); } } - // Create a factor graph + // This section of the code is inspired by the work of Sweeney et al. + // [link](sites.cs.ucsb.edu/~holl/pubs/Sweeney-2015-ICCV.pdf) on view-graph + // calibration. The graph is made up of transfer factors that enforce the + // epipolar constraint between corresponding points across three views, as + // described in the paper. Rather than adding one ternary error term per point + // in a triplet, we add three binary factors for sparsity during optimization. + // In this version, we only include triplets between 3 successive cameras. NonlinearFactorGraph graph; - - using Factor = TransferFactor; + using Factor = TransferFactor; for (size_t a = 0; a < 4; ++a) { size_t b = (a + 1) % 4; // Next camera size_t c = (a + 2) % 4; // Camera after next for (size_t j = 0; j < 4; ++j) { + // Add transfer factors between views a, b, and c. Note that the EdgeKeys + // are crucial in performing the transfer in the right direction. We use + // exactly 8 unique EdgeKeys, corresponding to 8 unknown fundamental + // matrices we will optimize for. graph.emplace_shared(EdgeKey(a, c), EdgeKey(b, c), p[a][j], p[b][j], p[c][j]); graph.emplace_shared(EdgeKey(a, b), EdgeKey(b, c), p[a][j], @@ -82,18 +92,19 @@ int main(int argc, char* argv[]) { graph.print("Factor Graph:\n", formatter); + // Create a delta vector to perturb the ground truth + // We can't really go far before convergence becomes problematic :-( + Vector7 delta; + delta << 1, 2, 3, 4, 5, 6, 7; + delta *= 5e-5; + // Create the data structure to hold the initial estimate to the solution Values initialEstimate; - const Point2 center(50, 50); - auto E1 = EssentialMatrix::FromPose3(poses[0].between(poses[1])); - auto E2 = EssentialMatrix::FromPose3(poses[0].between(poses[2])); for (size_t a = 0; a < 4; ++a) { size_t b = (a + 1) % 4; // Next camera size_t c = (a + 2) % 4; // Camera after next - initialEstimate.insert(EdgeKey(a, b), - SimpleFundamentalMatrix(E1, 50, 50, center, center)); - initialEstimate.insert(EdgeKey(a, c), - SimpleFundamentalMatrix(E2, 50, 50, center, center)); + initialEstimate.insert(EdgeKey(a, b), F1.retract(delta)); + initialEstimate.insert(EdgeKey(a, c), F2.retract(delta)); } initialEstimate.print("Initial Estimates:\n", formatter); // graph.printErrors(initialEstimate, "errors: ", formatter); @@ -104,10 +115,15 @@ int main(int argc, char* argv[]) { params.setVerbosityLM("SUMMARY"); Values result = LevenbergMarquardtOptimizer(graph, initialEstimate, params).optimize(); - result.print("Final results:\n", formatter); + cout << "initial error = " << graph.error(initialEstimate) << endl; cout << "final error = " << graph.error(result) << endl; + result.print("Final results:\n", formatter); + + cout << "Ground Truth F1:\n" << F1.matrix() << endl; + cout << "Ground Truth F2:\n" << F2.matrix() << endl; + return 0; } /* ************************************************************************* */ From 19c75cb95cdeadafdb154c01894d0702d9ed6c99 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Thu, 24 Oct 2024 17:36:53 -0700 Subject: [PATCH 16/20] Use FundamentalMatrix now --- examples/ViewGraphExample.cpp | 6 +++--- gtsam/geometry/FundamentalMatrix.h | 6 +++--- 2 files changed, 6 insertions(+), 6 deletions(-) diff --git a/examples/ViewGraphExample.cpp b/examples/ViewGraphExample.cpp index 7b5ed0fc6e..d1c7113bcc 100644 --- a/examples/ViewGraphExample.cpp +++ b/examples/ViewGraphExample.cpp @@ -47,8 +47,8 @@ int main(int argc, char* argv[]) { vector poses = posesOnCircle(4, 30); // Calculate ground truth fundamental matrices, 1 and 2 poses apart - auto F1 = GeneralFundamentalMatrix(K, poses[0].between(poses[1]), K); - auto F2 = GeneralFundamentalMatrix(K, poses[0].between(poses[2]), K); + auto F1 = FundamentalMatrix(K, poses[0].between(poses[1]), K); + auto F2 = FundamentalMatrix(K, poses[0].between(poses[2]), K); // Simulate measurements from each camera pose std::array, 4> p; @@ -67,7 +67,7 @@ int main(int argc, char* argv[]) { // in a triplet, we add three binary factors for sparsity during optimization. // In this version, we only include triplets between 3 successive cameras. NonlinearFactorGraph graph; - using Factor = TransferFactor; + using Factor = TransferFactor; for (size_t a = 0; a < 4; ++a) { size_t b = (a + 1) % 4; // Next camera size_t c = (a + 2) % 4; // Camera after next diff --git a/gtsam/geometry/FundamentalMatrix.h b/gtsam/geometry/FundamentalMatrix.h index f8aa37bf82..8a3ba86b82 100644 --- a/gtsam/geometry/FundamentalMatrix.h +++ b/gtsam/geometry/FundamentalMatrix.h @@ -57,7 +57,7 @@ class GTSAM_EXPORT FundamentalMatrix { /** * @brief Construct from calibration matrices Ka, Kb, and pose aPb * - * Initializes the GeneralFundamentalMatrix from the given calibration + * Initializes the FundamentalMatrix from the given calibration * matrices Ka and Kb, and the pose aPb. * * @tparam CAL Calibration type, expected to have a matrix() method @@ -66,8 +66,8 @@ class GTSAM_EXPORT FundamentalMatrix { * @param Kb Calibration matrix for the right camera */ template - GeneralFundamentalMatrix(const CAL& Ka, const Pose3& aPb, const CAL& Kb) - : GeneralFundamentalMatrix(Ka.K().transpose().inverse() * + FundamentalMatrix(const CAL& Ka, const Pose3& aPb, const CAL& Kb) + : FundamentalMatrix(Ka.K().transpose().inverse() * EssentialMatrix::FromPose3(aPb).matrix() * Kb.K().inverse()) {} From be68e07ddb22a7ac82e350e33d27aa23fb66b6a5 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Thu, 24 Oct 2024 17:54:04 -0700 Subject: [PATCH 17/20] Use abc terminology --- gtsam/sfm/TransferFactor.h | 59 ++++++++++++-------------- gtsam/sfm/tests/testTransferFactor.cpp | 2 +- 2 files changed, 29 insertions(+), 32 deletions(-) diff --git a/gtsam/sfm/TransferFactor.h b/gtsam/sfm/TransferFactor.h index 5c46f9b9bd..8acde7b88f 100644 --- a/gtsam/sfm/TransferFactor.h +++ b/gtsam/sfm/TransferFactor.h @@ -26,59 +26,56 @@ 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 - * between the transferred points `pi` and `pj`, and the actual point `pk` in + * between the transferred points `pa` and `pb`, and the actual point `pc` in * the target view. Jacobians are done using numerical differentiation. */ template class TransferFactor : public NoiseModelFactorN { EdgeKey key1_, key2_; ///< the two EdgeKeys - Point2 pi, pj, pk; ///< The points in the three views + Point2 pa, pb, pc; ///< The points in the three views public: /** * @brief Constructor for the TransferFactor class. * * Uses EdgeKeys to determine how to use the two fundamental matrix unknowns - * F1 and F2, to transfer points pi and pj to the third view, and minimize the - * difference with pk. + * F1 and F2, to transfer points pa and pb to the third view, and minimize the + * difference with pc. * * The edge keys must represent valid edges for the transfer operation, * specifically one of the following configurations: - * - (i, k) and (j, k) - * - (i, k) and (k, j) - * - (k, i) and (j, k) - * - (k, i) and (k, j) + * - (a, c) and (b, c) + * - (a, c) and (c, b) + * - (c, a) and (b, c) + * - (c, a) and (c, b) * - * @param key1 First EdgeKey specifying F1: (i, k) or (k, i). - * @param key2 Second EdgeKey specifying F2: (j, k) or (k, j). - * @param pi The point in the first view (i). - * @param pj The point in the second view (j). - * @param pk The point in the third (and transfer target) view (k). + * @param key1 First EdgeKey specifying F1: (a, c) or (c, a). + * @param key2 Second EdgeKey specifying F2: (b, c) or (c, b). + * @param pa The point in the first view (a). + * @param pb The point in the second view (b). + * @param pc The point in the third (and transfer target) view (c). * @param model An optional SharedNoiseModel that defines the noise model * for this factor. Defaults to nullptr. */ - TransferFactor(EdgeKey key1, EdgeKey key2, const Point2& pi, const Point2& pj, - const Point2& pk, const SharedNoiseModel& model = nullptr) + TransferFactor(EdgeKey key1, EdgeKey key2, const Point2& pa, const Point2& pb, + const Point2& pc, const SharedNoiseModel& model = nullptr) : NoiseModelFactorN(model, key1, key2), key1_(key1), key2_(key2), - pi(pi), - pj(pj), - pk(pk) {} + pa(pa), + pb(pb), + pc(pc) {} // Create Matrix3 objects based on EdgeKey configurations - static std::pair getMatrices(const EdgeKey& key1, - const F& F1, - const EdgeKey& key2, - const F& F2) { - // Fill Fki and Fkj based on EdgeKey configurations - if (key1.i() == key2.i()) { + std::pair getMatrices(const F& F1, const F& F2) const { + // Fill Fca and Fcb based on EdgeKey configurations + if (key1_.i() == key2_.i()) { return {F1.matrix(), F2.matrix()}; - } else if (key1.i() == key2.j()) { + } else if (key1_.i() == key2_.j()) { return {F1.matrix(), F2.matrix().transpose()}; - } else if (key1.j() == key2.i()) { + } else if (key1_.j() == key2_.i()) { return {F1.matrix().transpose(), F2.matrix()}; - } else if (key1.j() == key2.j()) { + } else if (key1_.j() == key2_.j()) { return {F1.matrix().transpose(), F2.matrix().transpose()}; } else { throw std::runtime_error( @@ -91,13 +88,13 @@ class TransferFactor : public NoiseModelFactorN { OptionalMatrixType H1 = nullptr, OptionalMatrixType H2 = nullptr) const override { std::function transfer = [&](const F& F1, const F& F2) { - auto [Fki, Fkj] = getMatrices(key1_, F1, key2_, F2); - return Transfer(Fki, pi, Fkj, pj); + auto [Fca, Fcb] = getMatrices(F1, F2); + return Transfer(Fca, pa, Fcb, pb); }; if (H1) *H1 = numericalDerivative21(transfer, F1, F2); if (H2) *H2 = numericalDerivative22(transfer, F1, F2); - return transfer(F1, F2) - pk; + return transfer(F1, F2) - pc; } }; -} // namespace gtsam +} // namespace gtsam \ No newline at end of file diff --git a/gtsam/sfm/tests/testTransferFactor.cpp b/gtsam/sfm/tests/testTransferFactor.cpp index d26bad960a..7bcd71b1fe 100644 --- a/gtsam/sfm/tests/testTransferFactor.cpp +++ b/gtsam/sfm/tests/testTransferFactor.cpp @@ -71,7 +71,7 @@ TEST(TransferFactor, Jacobians) { factor2{key20, key12, p[0], p[1], p[2]}; // Check that getMatrices is correct - auto [Fki, Fkj] = factor2.getMatrices(key20, triplet.Fca, key12, triplet.Fbc); + auto [Fki, Fkj] = factor2.getMatrices(triplet.Fca, triplet.Fbc); EXPECT(assert_equal(triplet.Fca.matrix(), Fki)); EXPECT(assert_equal(triplet.Fbc.matrix().transpose(), Fkj)); From 38ac1b48374009a0dfe753e5024e7dc3adc311ac Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Thu, 24 Oct 2024 19:17:23 -0700 Subject: [PATCH 18/20] Batched version --- examples/ViewGraphExample.cpp | 35 +++++++++------ gtsam/sfm/TransferFactor.h | 60 +++++++++++++++++--------- gtsam/sfm/tests/testTransferFactor.cpp | 58 +++++++++++++++++++++++++ 3 files changed, 118 insertions(+), 35 deletions(-) diff --git a/examples/ViewGraphExample.cpp b/examples/ViewGraphExample.cpp index d1c7113bcc..c23ac084c9 100644 --- a/examples/ViewGraphExample.cpp +++ b/examples/ViewGraphExample.cpp @@ -13,7 +13,7 @@ * @file ViewGraphExample.cpp * @brief View-graph calibration on a simulated dataset, a la Sweeney 2015 * @author Frank Dellaert - * @author October 2024 + * @date October 2024 */ #include @@ -68,21 +68,28 @@ int main(int argc, char* argv[]) { // In this version, we only include triplets between 3 successive cameras. NonlinearFactorGraph graph; using Factor = TransferFactor; + for (size_t a = 0; a < 4; ++a) { size_t b = (a + 1) % 4; // Next camera size_t c = (a + 2) % 4; // Camera after next - for (size_t j = 0; j < 4; ++j) { - // Add transfer factors between views a, b, and c. Note that the EdgeKeys - // are crucial in performing the transfer in the right direction. We use - // exactly 8 unique EdgeKeys, corresponding to 8 unknown fundamental - // matrices we will optimize for. - graph.emplace_shared(EdgeKey(a, c), EdgeKey(b, c), p[a][j], - p[b][j], p[c][j]); - graph.emplace_shared(EdgeKey(a, b), EdgeKey(b, c), p[a][j], - p[c][j], p[b][j]); - graph.emplace_shared(EdgeKey(a, c), EdgeKey(a, b), p[c][j], - p[b][j], p[a][j]); + + // Vectors to collect tuples for each factor + std::vector> tuples1, tuples2, tuples3; + + // Collect data for the three factors + for (size_t j = 0; j < 8; ++j) { + tuples1.emplace_back(p[a][j], p[b][j], p[c][j]); + tuples2.emplace_back(p[a][j], p[c][j], p[b][j]); + tuples3.emplace_back(p[c][j], p[b][j], p[a][j]); } + + // Add transfer factors between views a, b, and c. Note that the EdgeKeys + // are crucial in performing the transfer in the right direction. We use + // exactly 8 unique EdgeKeys, corresponding to 8 unknown fundamental + // matrices we will optimize for. + graph.emplace_shared(EdgeKey(a, c), EdgeKey(b, c), tuples1); + graph.emplace_shared(EdgeKey(a, b), EdgeKey(b, c), tuples2); + graph.emplace_shared(EdgeKey(a, c), EdgeKey(a, b), tuples3); } auto formatter = [](Key key) { @@ -96,7 +103,7 @@ int main(int argc, char* argv[]) { // We can't really go far before convergence becomes problematic :-( Vector7 delta; delta << 1, 2, 3, 4, 5, 6, 7; - delta *= 5e-5; + delta *= 1e-5; // Create the data structure to hold the initial estimate to the solution Values initialEstimate; @@ -107,7 +114,7 @@ int main(int argc, char* argv[]) { initialEstimate.insert(EdgeKey(a, c), F2.retract(delta)); } initialEstimate.print("Initial Estimates:\n", formatter); - // graph.printErrors(initialEstimate, "errors: ", formatter); + graph.printErrors(initialEstimate, "errors: ", formatter); /* Optimize the graph and print results */ LevenbergMarquardtParams params; diff --git a/gtsam/sfm/TransferFactor.h b/gtsam/sfm/TransferFactor.h index 8acde7b88f..35ef8c1ae4 100644 --- a/gtsam/sfm/TransferFactor.h +++ b/gtsam/sfm/TransferFactor.h @@ -32,22 +32,14 @@ namespace gtsam { template class TransferFactor : public NoiseModelFactorN { EdgeKey key1_, key2_; ///< the two EdgeKeys - Point2 pa, pb, pc; ///< The points in the three views + std::vector> + triplets_; ///< Point triplets public: /** - * @brief Constructor for the TransferFactor class. + * @brief Constructor for a single triplet of points * - * Uses EdgeKeys to determine how to use the two fundamental matrix unknowns - * F1 and F2, to transfer points pa and pb to the third view, and minimize the - * difference with pc. - * - * The edge keys must represent valid edges for the transfer operation, - * specifically one of the following configurations: - * - (a, c) and (b, c) - * - (a, c) and (c, b) - * - (c, a) and (b, c) - * - (c, a) and (c, b) + * @note: batching all points for the same transfer will be much faster. * * @param key1 First EdgeKey specifying F1: (a, c) or (c, a). * @param key2 Second EdgeKey specifying F2: (b, c) or (c, b). @@ -62,9 +54,25 @@ class TransferFactor : public NoiseModelFactorN { : NoiseModelFactorN(model, key1, key2), key1_(key1), key2_(key2), - pa(pa), - pb(pb), - pc(pc) {} + triplets_({std::make_tuple(pa, pb, pc)}) {} + + /** + * @brief Constructor that accepts a vector of point triplets. + * + * @param key1 First EdgeKey specifying F1: (a, c) or (c, a). + * @param key2 Second EdgeKey specifying F2: (b, c) or (c, b). + * @param triplets A vector of triplets containing (pa, pb, pc). + * @param model An optional SharedNoiseModel that defines the noise model + * for this factor. Defaults to nullptr. + */ + TransferFactor( + EdgeKey key1, EdgeKey key2, + const std::vector>& triplets, + const SharedNoiseModel& model = nullptr) + : NoiseModelFactorN(model, key1, key2), + key1_(key1), + key2_(key2), + triplets_(triplets) {} // Create Matrix3 objects based on EdgeKey configurations std::pair getMatrices(const F& F1, const F& F2) const { @@ -83,17 +91,27 @@ class TransferFactor : public NoiseModelFactorN { } } - /// vector of errors returns 2D vector + /// vector of errors returns 2*N vector Vector evaluateError(const F& F1, const F& F2, OptionalMatrixType H1 = nullptr, OptionalMatrixType H2 = nullptr) const override { - std::function transfer = [&](const F& F1, const F& F2) { + std::function transfer = [&](const F& F1, + const F& F2) { + Vector errors(2 * triplets_.size()); + size_t idx = 0; auto [Fca, Fcb] = getMatrices(F1, F2); - return Transfer(Fca, pa, Fcb, pb); + for (const auto& tuple : triplets_) { + const auto& [pa, pb, pc] = tuple; + Point2 transferredPoint = Transfer(Fca, pa, Fcb, pb); + Vector2 error = transferredPoint - pc; + errors.segment<2>(idx) = error; + idx += 2; + } + return errors; }; - if (H1) *H1 = numericalDerivative21(transfer, F1, F2); - if (H2) *H2 = numericalDerivative22(transfer, F1, F2); - return transfer(F1, F2) - pc; + if (H1) *H1 = numericalDerivative21(transfer, F1, F2); + if (H2) *H2 = numericalDerivative22(transfer, F1, F2); + return transfer(F1, F2); } }; diff --git a/gtsam/sfm/tests/testTransferFactor.cpp b/gtsam/sfm/tests/testTransferFactor.cpp index 7bcd71b1fe..6092ec4562 100644 --- a/gtsam/sfm/tests/testTransferFactor.cpp +++ b/gtsam/sfm/tests/testTransferFactor.cpp @@ -89,6 +89,64 @@ 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); + + // 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); + points3D.push_back(P); + + std::array p; + for (size_t i = 0; i < 3; ++i) { + // Project the point into each camera + PinholeCameraCal3_S2 camera(cameraPoses[i], K); + p[i] = camera.project(P); + } + projectedPoints.push_back(p); + } + + // Create a vector of tuples for the TransferFactor + EdgeKey key01(0, 1), key12(1, 2), key20(2, 0); + std::vector> tuples0, tuples1, tuples2; + + for (size_t n = 0; n < numPoints; ++n) { + const auto& p = projectedPoints[n]; + tuples0.emplace_back(p[1], p[2], p[0]); + } + + // Create TransferFactors using the new constructor + TransferFactor factor{key01, key20, tuples0}; + + // Create Values with edge keys + Values values; + values.insert(key01, triplet.Fab); + values.insert(key12, triplet.Fbc); + values.insert(key20, triplet.Fca); + + // Check error and Jacobians for multiple tuples + Vector error = factor.unwhitenedError(values); + // The error vector should be of size 2 * numPoints + EXPECT_LONGS_EQUAL(2 * numPoints, error.size()); + // Since the points are perfectly matched, the error should be zero + EXPECT(assert_equal(Vector::Zero(2 * numPoints), error, 1e-9)); + + // Check the Jacobians + EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-5, 1e-7); +} + //************************************************************************* int main() { TestResult tr; From 00cb637db34e80e3cab6bd316c5602b9e342a51a Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Fri, 25 Oct 2024 07:36:16 -0700 Subject: [PATCH 19/20] Renamed method --- gtsam/sfm/TransferFactor.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/sfm/TransferFactor.h b/gtsam/sfm/TransferFactor.h index 35ef8c1ae4..fb2dd63b0a 100644 --- a/gtsam/sfm/TransferFactor.h +++ b/gtsam/sfm/TransferFactor.h @@ -102,7 +102,7 @@ class TransferFactor : public NoiseModelFactorN { auto [Fca, Fcb] = getMatrices(F1, F2); for (const auto& tuple : triplets_) { const auto& [pa, pb, pc] = tuple; - Point2 transferredPoint = Transfer(Fca, pa, Fcb, pb); + Point2 transferredPoint = EpipolarTransfer(Fca, pa, Fcb, pb); Vector2 error = transferredPoint - pc; errors.segment<2>(idx) = error; idx += 2; From cbbe2d2e887659ac1c7ad1d96c45264272435777 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Fri, 25 Oct 2024 20:48:02 -0700 Subject: [PATCH 20/20] 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 af322a3c08..d65053d196 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 8a3ba86b82..6f04f45e8e 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 fb2dd63b0a..d2085f57ea 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 6092ec4562..2dca12c2ac 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;