From 879bd2996312bf567269fa65c44e563b70ecf889 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Fri, 25 Oct 2024 10:17:33 -0700 Subject: [PATCH 01/34] TransferEdges base class --- gtsam/sfm/TransferFactor.h | 102 ++++++++++++++++++++----------------- 1 file changed, 56 insertions(+), 46 deletions(-) diff --git a/gtsam/sfm/TransferFactor.h b/gtsam/sfm/TransferFactor.h index d2085f57e..13de4eb29 100644 --- a/gtsam/sfm/TransferFactor.h +++ b/gtsam/sfm/TransferFactor.h @@ -22,6 +22,34 @@ namespace gtsam { +/** + * Base class that holds the EdgeKeys and provides the getMatrices method. + */ +template +struct TransferEdges { + EdgeKey edge1, edge2; ///< The two EdgeKeys. + + TransferEdges(EdgeKey edge1, EdgeKey edge2) : edge1(edge1), edge2(edge2) {} + + // Create Matrix3 objects based on EdgeKey configurations. + std::pair getMatrices(const F& F1, const F& F2) const { + // Fill Fca and Fcb based on EdgeKey configurations. + if (edge1.i() == edge2.i()) { + return {F1.matrix(), F2.matrix()}; + } else if (edge1.i() == edge2.j()) { + return {F1.matrix(), F2.matrix().transpose()}; + } else if (edge1.j() == edge2.i()) { + return {F1.matrix().transpose(), F2.matrix()}; + } else if (edge1.j() == edge2.j()) { + return {F1.matrix().transpose(), F2.matrix().transpose()}; + } else { + throw std::runtime_error( + "TransferEdges: invalid EdgeKey configuration between edge1 (" + + std::string(edge1) + ") and edge2 (" + std::string(edge2) + ")."); + } + } +}; + /** * Binary factor in the context of Structure from Motion (SfM). * It is used to transfer transfer corresponding points from two views to a @@ -30,76 +58,57 @@ namespace gtsam { * the target view. Jacobians are done using numerical differentiation. */ template -class TransferFactor : public NoiseModelFactorN { - EdgeKey key1_, key2_; ///< the two EdgeKeys - std::vector> - triplets_; ///< Point triplets +class TransferFactor : public NoiseModelFactorN, public TransferEdges { + using Base = NoiseModelFactorN; + using Triplet = std::tuple; + std::vector triplets_; ///< Point triplets. public: /** - * @brief Constructor for a single triplet of points + * @brief Constructor for a single triplet of points. * - * @note: batching all points for the same transfer will be much faster. + * @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). + * @param edge1 First EdgeKey specifying F1: (a, c) or (c, a). + * @param edge2 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& pa, const Point2& pb, - const Point2& pc, const SharedNoiseModel& model = nullptr) - : NoiseModelFactorN(model, key1, key2), - key1_(key1), - key2_(key2), + TransferFactor(EdgeKey edge1, EdgeKey edge2, const Point2& pa, + const Point2& pb, const Point2& pc, + const SharedNoiseModel& model = nullptr) + : Base(model, edge1, edge2), + TransferEdges(edge1, edge2), 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 edge1 First EdgeKey specifying F1: (a, c) or (c, a). + * @param edge2 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), + TransferFactor(EdgeKey edge1, EdgeKey edge2, + const std::vector& triplets, + const SharedNoiseModel& model = nullptr) + : Base(model, edge1, edge2), + TransferEdges(edge1, edge2), triplets_(triplets) {} - // Create Matrix3 objects based on EdgeKey configurations - 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()) { - 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 2*N 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 errorFunction = [&](const F& F1, + const F& F2) { Vector errors(2 * triplets_.size()); size_t idx = 0; - auto [Fca, Fcb] = getMatrices(F1, F2); + auto [Fca, Fcb] = this->getMatrices(F1, F2); for (const auto& tuple : triplets_) { const auto& [pa, pb, pc] = tuple; Point2 transferredPoint = EpipolarTransfer(Fca, pa, Fcb, pb); @@ -109,9 +118,10 @@ class TransferFactor : public NoiseModelFactorN { } return errors; }; - if (H1) *H1 = numericalDerivative21(transfer, F1, F2); - if (H2) *H2 = numericalDerivative22(transfer, F1, F2); - return transfer(F1, F2); + if (H1) *H1 = numericalDerivative21(errorFunction, F1, F2); + if (H2) *H2 = numericalDerivative22(errorFunction, F1, F2); + return errorFunction(F1, F2); + } }; From b33518c4e27cb5197ea4f7eb03cf68c2e280e2dc Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Fri, 25 Oct 2024 11:26:43 -0700 Subject: [PATCH 02/34] EssentialTransferFactor --- gtsam/sfm/TransferFactor.h | 186 ++++++++++++++++++------- gtsam/sfm/tests/testTransferFactor.cpp | 76 ++++++++-- 2 files changed, 205 insertions(+), 57 deletions(-) diff --git a/gtsam/sfm/TransferFactor.h b/gtsam/sfm/TransferFactor.h index 13de4eb29..9704810dd 100644 --- a/gtsam/sfm/TransferFactor.h +++ b/gtsam/sfm/TransferFactor.h @@ -16,37 +16,63 @@ #pragma once #include +#include #include #include +#include #include +#include + namespace gtsam { /** * Base class that holds the EdgeKeys and provides the getMatrices method. */ template -struct TransferEdges { - EdgeKey edge1, edge2; ///< The two EdgeKeys. +class TransferEdges { + protected: + EdgeKey edge1_, edge2_; ///< The two EdgeKeys. + uint32_t c_; ///< The transfer target - TransferEdges(EdgeKey edge1, EdgeKey edge2) : edge1(edge1), edge2(edge2) {} + public: + TransferEdges(EdgeKey edge1, EdgeKey edge2) + : edge1_(edge1), edge2_(edge2), c_(viewC(edge1, edge2)) {} - // Create Matrix3 objects based on EdgeKey configurations. - std::pair getMatrices(const F& F1, const F& F2) const { - // Fill Fca and Fcb based on EdgeKey configurations. - if (edge1.i() == edge2.i()) { - return {F1.matrix(), F2.matrix()}; - } else if (edge1.i() == edge2.j()) { - return {F1.matrix(), F2.matrix().transpose()}; - } else if (edge1.j() == edge2.i()) { - return {F1.matrix().transpose(), F2.matrix()}; - } else if (edge1.j() == edge2.j()) { - return {F1.matrix().transpose(), F2.matrix().transpose()}; - } else { + /// Returns the view A index based on the EdgeKeys + static size_t viewA(const EdgeKey& edge1, const EdgeKey& edge2) { + size_t c = viewC(edge1, edge2); + return (edge1.i() == c) ? edge1.j() : edge1.i(); + } + + /// Returns the view B index based on the EdgeKeys + static size_t viewB(const EdgeKey& edge1, const EdgeKey& edge2) { + size_t c = viewC(edge1, edge2); + return (edge2.i() == c) ? edge2.j() : edge2.i(); + } + + /// Returns the view C index based on the EdgeKeys + static size_t viewC(const EdgeKey& edge1, const EdgeKey& edge2) { + if (edge1.i() == edge2.i() || edge1.i() == edge2.j()) + return edge1.i(); + else if (edge1.j() == edge2.i() || edge1.j() == edge2.j()) + return edge1.j(); + else throw std::runtime_error( - "TransferEdges: invalid EdgeKey configuration between edge1 (" + - std::string(edge1) + ") and edge2 (" + std::string(edge2) + ")."); - } + "EssentialTransferFactor: No common key in edge keys."); + } + + /// Create Matrix3 objects based on EdgeKey configurations. + std::pair getMatrices(const F& F1, const F& F2) const { + // Determine whether to transpose F1 + const Matrix3 Fca = + edge1_.i() == c_ ? F1.matrix() : F1.matrix().transpose(); + + // Determine whether to transpose F2 + const Matrix3 Fcb = + edge2_.i() == c_ ? F2.matrix() : F2.matrix().transpose(); + + return {Fca, Fcb}; } }; @@ -64,26 +90,6 @@ class TransferFactor : public NoiseModelFactorN, public TransferEdges { std::vector triplets_; ///< Point triplets. public: - /** - * @brief Constructor for a single triplet of points. - * - * @note Batching all points for the same transfer will be much faster. - * - * @param edge1 First EdgeKey specifying F1: (a, c) or (c, a). - * @param edge2 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 edge1, EdgeKey edge2, const Point2& pa, - const Point2& pb, const Point2& pc, - const SharedNoiseModel& model = nullptr) - : Base(model, edge1, edge2), - TransferEdges(edge1, edge2), - triplets_({std::make_tuple(pa, pb, pc)}) {} - /** * @brief Constructor that accepts a vector of point triplets. * @@ -104,25 +110,107 @@ class TransferFactor : public NoiseModelFactorN, public TransferEdges { Vector evaluateError(const F& F1, const F& F2, OptionalMatrixType H1 = nullptr, OptionalMatrixType H2 = nullptr) const override { - std::function errorFunction = [&](const F& F1, - const F& F2) { + std::function errorFunction = [&](const F& f1, + const F& f2) { Vector errors(2 * triplets_.size()); size_t idx = 0; - auto [Fca, Fcb] = this->getMatrices(F1, F2); - for (const auto& tuple : triplets_) { - const auto& [pa, pb, pc] = tuple; - Point2 transferredPoint = EpipolarTransfer(Fca, pa, Fcb, pb); - Vector2 error = transferredPoint - pc; - errors.segment<2>(idx) = error; + auto [Fca, Fcb] = this->getMatrices(f1, f2); + for (const auto& [pa, pb, pc] : triplets_) { + errors.segment<2>(idx) = EpipolarTransfer(Fca, pa, Fcb, pb) - pc; idx += 2; } return errors; }; - if (H1) *H1 = numericalDerivative21(errorFunction, F1, F2); - if (H2) *H2 = numericalDerivative22(errorFunction, F1, F2); - return errorFunction(F1, F2); + if (H1) *H1 = numericalDerivative21(errorFunction, F1, F2); + if (H2) *H2 = numericalDerivative22(errorFunction, F1, F2); + return errorFunction(F1, F2); } }; +/** + * @class EssentialTransferFactor + * @brief Transfers points between views using essential matrices, optimizes for + * calibrations of the views, as well. Note that the EssentialMatrixFactor4 does + * something similar but without transfer. + * + * @note Derives calibration keys from edges, and uses symbol 'k'. + * + * This factor is templated on the calibration class K and extends + * the TransferFactor for EssentialMatrices. It involves two essential matrices + * and three calibration objects (Ka, Kb, Kc). The evaluateError + * function calibrates the image points, calls the base class's transfer method, + * and computes the error using bulk numerical differentiation. + */ +template +class EssentialTransferFactor + : public NoiseModelFactorN, + TransferEdges { + using EM = EssentialMatrix; + using Triplet = std::tuple; + std::vector triplets_; ///< Point triplets + + public: + using Base = NoiseModelFactorN; + using This = EssentialTransferFactor; + using shared_ptr = std::shared_ptr; + + /** + * @brief Constructor that accepts a vector of point triplets. + * + * @param edge1 First EdgeKey specifying E1: (a, c) or (c, a) + * @param edge2 Second EdgeKey specifying E2: (b, c) or (c, b) + * @param triplets A vector of triplets containing (pa, pb, pc) + * @param model An optional SharedNoiseModel + */ + EssentialTransferFactor(EdgeKey edge1, EdgeKey edge2, + const std::vector& triplets, + const SharedNoiseModel& model = nullptr) + : Base(model, edge1, edge2, + Symbol('k', viewA(edge1, edge2)), // calibration key for view a + Symbol('k', viewB(edge1, edge2)), // calibration key for view b + Symbol('k', viewC(edge1, edge2))), // calibration key for target c + TransferEdges(edge1, edge2), + triplets_(triplets) {} + + /// Evaluate error function + Vector evaluateError(const EM& E1, const EM& E2, const K& Ka, const K& Kb, + const K& Kc, OptionalMatrixType H1 = nullptr, + OptionalMatrixType H2 = nullptr, + OptionalMatrixType H3 = nullptr, + OptionalMatrixType H4 = nullptr, + OptionalMatrixType H5 = nullptr) const override { + std::function + errorFunction = [&](const EM& e1, const EM& e2, const K& kA, + const K& kB, const K& kC) { + Vector errors(2 * triplets_.size()); + size_t idx = 0; + auto [Eca, Ecb] = this->getMatrices(e1, e2); + for (const auto& [pa, pb, pc] : triplets_) { + const Point2 pA = kA.calibrate(pa); + const Point2 pB = kB.calibrate(pb); + const Point2 pC = EpipolarTransfer(Eca, pA, Ecb, pB); + errors.segment<2>(idx) = kC.uncalibrate(pC) - pc; + idx += 2; + } + return errors; + }; + + // Compute error + Vector errors = errorFunction(E1, E2, Ka, Kb, Kc); + + // Compute Jacobians if requested + if (H1) *H1 = numericalDerivative51(errorFunction, E1, E2, Ka, Kb, Kc); + if (H2) *H2 = numericalDerivative52(errorFunction, E1, E2, Ka, Kb, Kc); + if (H3) *H3 = numericalDerivative53(errorFunction, E1, E2, Ka, Kb, Kc); + if (H4) *H4 = numericalDerivative54(errorFunction, E1, E2, Ka, Kb, Kc); + if (H5) *H5 = numericalDerivative55(errorFunction, E1, E2, Ka, Kb, Kc); + + return errors; + } + + /// Return the dimension of the factor + size_t dim() const override { return 2 * triplets_.size(); } +}; + } // namespace gtsam \ No newline at end of file diff --git a/gtsam/sfm/tests/testTransferFactor.cpp b/gtsam/sfm/tests/testTransferFactor.cpp index 2dca12c2a..b8eed3b3a 100644 --- a/gtsam/sfm/tests/testTransferFactor.cpp +++ b/gtsam/sfm/tests/testTransferFactor.cpp @@ -1,8 +1,8 @@ /* * @file testTransferFactor.cpp - * @brief Test TransferFactor class - * @author Your Name - * @date October 23, 2024 + * @brief Test TransferFactor classes + * @author Frank Dellaert + * @date October 2024 */ #include @@ -11,9 +11,10 @@ #include using namespace gtsam; +using symbol_shorthand::K; //************************************************************************* -/// Generate three cameras on a circle, looking in +/// Generate three cameras on a circle, looking inwards std::array generateCameraPoses() { std::array cameraPoses; const double radius = 1.0; @@ -82,9 +83,9 @@ TEST(TransferFactor, Jacobians) { // Create a TransferFactor 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]}; + 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 Values with edge keys Values values; @@ -153,9 +154,68 @@ TEST(TransferFactor, MultipleTuples) { EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-5, 1e-7); } +//************************************************************************* +// Test for EssentialTransferFactor +TEST(EssentialTransferFactor, Jacobians) { + // Generate cameras on a circle + std::array cameraPoses = generateCameraPoses(); + + // Create calibration + const Cal3_S2 commonK(focalLength, focalLength, 0.0, principalPoint.x(), + principalPoint.y()); + + // Create cameras + std::array, 3> cameras; + for (size_t i = 0; i < 3; ++i) { + cameras[i] = PinholeCamera(cameraPoses[i], commonK); + } + + // Create EssentialMatrices between cameras + EssentialMatrix E01 = + EssentialMatrix::FromPose3(cameraPoses[0].between(cameraPoses[1])); + EssentialMatrix E02 = + EssentialMatrix::FromPose3(cameraPoses[0].between(cameraPoses[2])); + + // Project a point into the three cameras + const Point3 P(0.1, 0.2, 0.3); + std::array p; + for (size_t i = 0; i < 3; ++i) { + p[i] = cameras[i].project(P); + } + + // Create EdgeKeys + EdgeKey key01(0, 1); + EdgeKey key02(0, 2); + + // Create an EssentialTransferFactor + EssentialTransferFactor factor(key01, key02, {{p[1], p[2], p[0]}}); + + // Create Values and insert variables + Values values; + values.insert(key01, E01); // Essential matrix between views 0 and 1 + values.insert(key02, E02); // Essential matrix between views 0 and 2 + values.insert(K(1), commonK); // Calibration for view A (view 1) + values.insert(K(2), commonK); // Calibration for view B (view 2) + values.insert(K(0), commonK); // Calibration for view C (view 0) + + // Check error + Matrix H1, H2, H3, H4, H5; + Vector error = factor.evaluateError(E01, E02, commonK, commonK, commonK, // + &H1, &H2, &H3, &H4, &H5); + EXPECT(H1.rows() == 2 && H1.cols() == 5) + EXPECT(H2.rows() == 2 && H2.cols() == 5) + EXPECT(H3.rows() == 2 && H3.cols() == 5) + EXPECT(H4.rows() == 2 && H4.cols() == 5) + EXPECT(H5.rows() == 2 && H5.cols() == 5) + EXPECT(assert_equal(Vector::Zero(2), error, 1e-9)) + + // Check Jacobians + EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-5, 1e-7); +} + //************************************************************************* int main() { TestResult tr; return TestRegistry::runAllTests(tr); } -//************************************************************************* +//************************************************************************* \ No newline at end of file From 9335d2c0dd46df7d23a302ba0345150184f34757 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Fri, 25 Oct 2024 12:14:36 -0700 Subject: [PATCH 03/34] Copy example --- examples/EssentialViewGraphExample.cpp | 136 +++++++++++++++++++++++++ 1 file changed, 136 insertions(+) create mode 100644 examples/EssentialViewGraphExample.cpp diff --git a/examples/EssentialViewGraphExample.cpp b/examples/EssentialViewGraphExample.cpp new file mode 100644 index 000000000..a7cac5dbf --- /dev/null +++ b/examples/EssentialViewGraphExample.cpp @@ -0,0 +1,136 @@ +/* ---------------------------------------------------------------------------- + + * 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 EssentialViewGraphExample.cpp + * @brief View-graph calibration on a simulated dataset, a la Sweeney 2015 + * @author Frank Dellaert + * @date October 2024 + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#include "SFMdata.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 K(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); + + // Calculate ground truth fundamental matrices, 1 and 2 poses apart + 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; + for (size_t i = 0; i < 4; ++i) { + PinholeCamera camera(poses[i], K); + for (size_t j = 0; j < 8; ++j) { + p[i][j] = camera.project(points[j]); + } + } + + // 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; + + 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 + + // 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) { + EdgeKey edge(key); + return (std::string)edge; + }; + + 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 *= 1e-5; + + // Create the data structure to hold the initial estimate to the solution + Values initialEstimate; + 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), F1.retract(delta)); + initialEstimate.insert(EdgeKey(a, c), F2.retract(delta)); + } + 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(); + + 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 5b94956a59ce1381164b2e1d5774ed6a50c21a84 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Fri, 25 Oct 2024 12:20:16 -0700 Subject: [PATCH 04/34] New example now uses EssentialTransferFactor --- examples/EssentialViewGraphExample.cpp | 84 +++++++++++++------------- 1 file changed, 43 insertions(+), 41 deletions(-) diff --git a/examples/EssentialViewGraphExample.cpp b/examples/EssentialViewGraphExample.cpp index a7cac5dbf..e6948cfab 100644 --- a/examples/EssentialViewGraphExample.cpp +++ b/examples/EssentialViewGraphExample.cpp @@ -17,28 +17,30 @@ */ #include +#include #include #include #include #include #include +#include #include #include #include -#include +#include // Contains EssentialTransferFactor #include -#include "SFMdata.h" -#include "gtsam/inference/Key.h" +#include "SFMdata.h" // For createPoints() and posesOnCircle() using namespace std; using namespace gtsam; +using namespace symbol_shorthand; // For K(symbol) -/* ************************************************************************* */ +// Main function int main(int argc, char* argv[]) { // Define the camera calibration parameters - Cal3_S2 K(50.0, 50.0, 0.0, 50.0, 50.0); + Cal3_S2 K_initial(50.0, 50.0, 0.0, 50.0, 50.0); // Create the set of 8 ground-truth landmarks vector points = createPoints(); @@ -46,28 +48,22 @@ 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 = FundamentalMatrix(K, poses[0].between(poses[1]), K); - auto F2 = FundamentalMatrix(K, poses[0].between(poses[2]), K); + // Calculate ground truth essential matrices, 1 and 2 poses apart + auto E1 = EssentialMatrix::FromPose3(poses[0].between(poses[1])); + auto E2 = EssentialMatrix::FromPose3(poses[0].between(poses[2])); // Simulate measurements from each camera pose std::array, 4> p; for (size_t i = 0; i < 4; ++i) { - PinholeCamera camera(poses[i], K); + PinholeCamera camera(poses[i], K_initial); for (size_t j = 0; j < 8; ++j) { p[i][j] = camera.project(points[j]); } } - // 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. + // Create the factor graph NonlinearFactorGraph graph; - using Factor = TransferFactor; + using Factor = EssentialTransferFactor; for (size_t a = 0; a < 4; ++a) { size_t b = (a + 1) % 4; // Next camera @@ -83,54 +79,60 @@ int main(int argc, char* argv[]) { 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. + // Add transfer factors between views a, b, and c. 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); } + // Formatter for printing keys auto formatter = [](Key key) { - EdgeKey edge(key); - return (std::string)edge; + if (Symbol(key).chr() == 'k') { + return (string)Symbol(key); + } else { + EdgeKey edge(key); + return (std::string)edge; + } }; 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 *= 1e-5; + // Create a delta vector to perturb the ground truth (small perturbation) + Vector5 delta; + delta << 1, 1, 1, 1, 1; + delta *= 1e-2; - // Create the data structure to hold the initial estimate to the solution + // Create the initial estimate for essential matrices Values initialEstimate; 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), F1.retract(delta)); - initialEstimate.insert(EdgeKey(a, c), F2.retract(delta)); + initialEstimate.insert(EdgeKey(a, b), E1.retract(delta)); + initialEstimate.insert(EdgeKey(a, c), E2.retract(delta)); } - initialEstimate.print("Initial Estimates:\n", formatter); - graph.printErrors(initialEstimate, "errors: ", formatter); - /* Optimize the graph and print results */ + // Insert initial calibrations (using K symbol) + for (size_t i = 0; i < 4; ++i) { + initialEstimate.insert(K(i), K_initial); + } + + initialEstimate.print("Initial Estimates:\n", formatter); + graph.printErrors(initialEstimate, "Initial Errors:\n", 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(); - cout << "initial error = " << graph.error(initialEstimate) << endl; - cout << "final error = " << graph.error(result) << endl; + cout << "Initial error = " << graph.error(initialEstimate) << endl; + cout << "Final error = " << graph.error(result) << endl; - result.print("Final results:\n", formatter); + result.print("Final Results:\n", formatter); - cout << "Ground Truth F1:\n" << F1.matrix() << endl; - cout << "Ground Truth F2:\n" << F2.matrix() << endl; + cout << "Ground Truth E1:\n" << E1.matrix() << endl; + cout << "Ground Truth E2:\n" << E2.matrix() << endl; return 0; -} -/* ************************************************************************* */ +} \ No newline at end of file From 049191fe1d030445b87fafe248199039a714054d Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Fri, 25 Oct 2024 21:05:09 -0700 Subject: [PATCH 05/34] Use fixture --- gtsam/sfm/tests/testTransferFactor.cpp | 49 ++++++++++---------------- 1 file changed, 18 insertions(+), 31 deletions(-) diff --git a/gtsam/sfm/tests/testTransferFactor.cpp b/gtsam/sfm/tests/testTransferFactor.cpp index b8eed3b3a..b1655e0a9 100644 --- a/gtsam/sfm/tests/testTransferFactor.cpp +++ b/gtsam/sfm/tests/testTransferFactor.cpp @@ -49,8 +49,12 @@ 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()); +const Cal3_S2 cal(focalLength, focalLength, 0.0, // + principalPoint.x(), principalPoint.y()); +// Create cameras +auto f = [](const Pose3& pose) { return PinholeCamera(pose, cal); }; +std::array, 3> cameras = { + f(cameraPoses[0]), f(cameraPoses[1]), f(cameraPoses[2])}; } // namespace fixture //************************************************************************* @@ -72,12 +76,9 @@ TEST(TransferFactor, Jacobians) { // Now project a point into the three cameras const Point3 P(0.1, 0.2, 0.3); - 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); + p[i] = cameras[i].project(P); } // Create a TransferFactor @@ -108,19 +109,14 @@ TEST(TransferFactor, MultipleTuples) { // Now project multiple points into the three cameras const size_t numPoints = 5; // Number of points to test - std::vector points3D; std::vector> projectedPoints; // 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); + p[i] = cameras[i].project(P); } projectedPoints.push_back(p); } @@ -157,18 +153,7 @@ TEST(TransferFactor, MultipleTuples) { //************************************************************************* // Test for EssentialTransferFactor TEST(EssentialTransferFactor, Jacobians) { - // Generate cameras on a circle - std::array cameraPoses = generateCameraPoses(); - - // Create calibration - const Cal3_S2 commonK(focalLength, focalLength, 0.0, principalPoint.x(), - principalPoint.y()); - - // Create cameras - std::array, 3> cameras; - for (size_t i = 0; i < 3; ++i) { - cameras[i] = PinholeCamera(cameraPoses[i], commonK); - } + using namespace fixture; // Create EssentialMatrices between cameras EssentialMatrix E01 = @@ -192,16 +177,18 @@ TEST(EssentialTransferFactor, Jacobians) { // Create Values and insert variables Values values; - values.insert(key01, E01); // Essential matrix between views 0 and 1 - values.insert(key02, E02); // Essential matrix between views 0 and 2 - values.insert(K(1), commonK); // Calibration for view A (view 1) - values.insert(K(2), commonK); // Calibration for view B (view 2) - values.insert(K(0), commonK); // Calibration for view C (view 0) + values.insert(key01, E01); // Essential matrix between views 0 and 1 + values.insert(key02, E02); // Essential matrix between views 0 and 2 + values.insert(K(1), cal); // Calibration for view A (view 1) + values.insert(K(2), cal); // Calibration for view B (view 2) + values.insert(K(0), cal); // Calibration for view C (view 0) // Check error Matrix H1, H2, H3, H4, H5; - Vector error = factor.evaluateError(E01, E02, commonK, commonK, commonK, // - &H1, &H2, &H3, &H4, &H5); + Vector error = + factor.evaluateError(E01, E02, // + fixture::cal, fixture::cal, fixture::cal, // + &H1, &H2, &H3, &H4, &H5); EXPECT(H1.rows() == 2 && H1.cols() == 5) EXPECT(H2.rows() == 2 && H2.cols() == 5) EXPECT(H3.rows() == 2 && H3.cols() == 5) From 5eb858b7294b43216dd18b13fdadb254b2658137 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 26 Oct 2024 10:06:45 -0700 Subject: [PATCH 06/34] Naming convention --- examples/EssentialViewGraphExample.cpp | 2 +- gtsam/sfm/TransferFactor.h | 18 +++++++++--------- 2 files changed, 10 insertions(+), 10 deletions(-) diff --git a/examples/EssentialViewGraphExample.cpp b/examples/EssentialViewGraphExample.cpp index e6948cfab..91b2bb042 100644 --- a/examples/EssentialViewGraphExample.cpp +++ b/examples/EssentialViewGraphExample.cpp @@ -11,7 +11,7 @@ /** * @file EssentialViewGraphExample.cpp - * @brief View-graph calibration on a simulated dataset, a la Sweeney 2015 + * @brief View-graph calibration with essential matrices. * @author Frank Dellaert * @date October 2024 */ diff --git a/gtsam/sfm/TransferFactor.h b/gtsam/sfm/TransferFactor.h index 9704810dd..2964c01ad 100644 --- a/gtsam/sfm/TransferFactor.h +++ b/gtsam/sfm/TransferFactor.h @@ -37,22 +37,22 @@ class TransferEdges { public: TransferEdges(EdgeKey edge1, EdgeKey edge2) - : edge1_(edge1), edge2_(edge2), c_(viewC(edge1, edge2)) {} + : edge1_(edge1), edge2_(edge2), c_(ViewC(edge1, edge2)) {} /// Returns the view A index based on the EdgeKeys - static size_t viewA(const EdgeKey& edge1, const EdgeKey& edge2) { - size_t c = viewC(edge1, edge2); + static size_t ViewA(const EdgeKey& edge1, const EdgeKey& edge2) { + size_t c = ViewC(edge1, edge2); return (edge1.i() == c) ? edge1.j() : edge1.i(); } /// Returns the view B index based on the EdgeKeys - static size_t viewB(const EdgeKey& edge1, const EdgeKey& edge2) { - size_t c = viewC(edge1, edge2); + static size_t ViewB(const EdgeKey& edge1, const EdgeKey& edge2) { + size_t c = ViewC(edge1, edge2); return (edge2.i() == c) ? edge2.j() : edge2.i(); } /// Returns the view C index based on the EdgeKeys - static size_t viewC(const EdgeKey& edge1, const EdgeKey& edge2) { + static size_t ViewC(const EdgeKey& edge1, const EdgeKey& edge2) { if (edge1.i() == edge2.i() || edge1.i() == edge2.j()) return edge1.i(); else if (edge1.j() == edge2.i() || edge1.j() == edge2.j()) @@ -167,9 +167,9 @@ class EssentialTransferFactor const std::vector& triplets, const SharedNoiseModel& model = nullptr) : Base(model, edge1, edge2, - Symbol('k', viewA(edge1, edge2)), // calibration key for view a - Symbol('k', viewB(edge1, edge2)), // calibration key for view b - Symbol('k', viewC(edge1, edge2))), // calibration key for target c + Symbol('k', ViewA(edge1, edge2)), // calibration key for view a + Symbol('k', ViewB(edge1, edge2)), // calibration key for view b + Symbol('k', ViewC(edge1, edge2))), // calibration key for target c TransferEdges(edge1, edge2), triplets_(triplets) {} From a8a229c10cfbeaf5b665d1d13a00605a9e1dcc42 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 26 Oct 2024 10:19:13 -0700 Subject: [PATCH 07/34] Modernize/format --- python/gtsam/examples/SFMExample.py | 39 ++++++++++++++++------------- 1 file changed, 21 insertions(+), 18 deletions(-) diff --git a/python/gtsam/examples/SFMExample.py b/python/gtsam/examples/SFMExample.py index c8d1f1271..d8d0ae1df 100644 --- a/python/gtsam/examples/SFMExample.py +++ b/python/gtsam/examples/SFMExample.py @@ -9,20 +9,22 @@ See LICENSE for the license information A structure-from-motion problem on a simulated dataset """ -import gtsam import matplotlib.pyplot as plt import numpy as np + +import gtsam from gtsam import symbol_shorthand + L = symbol_shorthand.L X = symbol_shorthand.X from gtsam.examples import SFMdata -from gtsam import (Cal3_S2, DoglegOptimizer, - GenericProjectionFactorCal3_S2, Marginals, - NonlinearFactorGraph, PinholeCameraCal3_S2, Point3, - Pose3, PriorFactorPoint3, PriorFactorPose3, Rot3, Values) from gtsam.utils import plot +from gtsam import (Cal3_S2, DoglegOptimizer, GenericProjectionFactorCal3_S2, + Marginals, NonlinearFactorGraph, PinholeCameraCal3_S2, + PriorFactorPoint3, PriorFactorPose3, Values) + def main(): """ @@ -43,7 +45,7 @@ def main(): 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 The nonlinear solvers within GTSAM are iterative solvers, meaning they linearize the nonlinear functions around an initial linearization point, then solve the linear system @@ -78,8 +80,7 @@ def main(): camera = PinholeCameraCal3_S2(pose, K) for j, point in enumerate(points): measurement = camera.project(point) - factor = GenericProjectionFactorCal3_S2( - measurement, measurement_noise, X(i), L(j), K) + factor = GenericProjectionFactorCal3_S2(measurement, measurement_noise, X(i), L(j), K) graph.push_back(factor) # Because the structure-from-motion problem has a scale ambiguity, the problem is still under-constrained @@ -88,28 +89,29 @@ def main(): point_noise = gtsam.noiseModel.Isotropic.Sigma(3, 0.1) factor = PriorFactorPoint3(L(0), points[0], point_noise) graph.push_back(factor) - graph.print('Factor Graph:\n') + graph.print("Factor Graph:\n") # Create the data structure to hold the initial estimate to the solution # Intentionally initialize the variables off from the ground truth initial_estimate = Values() + rng = np.random.default_rng() for i, pose in enumerate(poses): - transformed_pose = pose.retract(0.1*np.random.randn(6,1)) + transformed_pose = pose.retract(0.1 * rng.standard_normal(6).reshape(6, 1)) initial_estimate.insert(X(i), transformed_pose) for j, point in enumerate(points): - transformed_point = point + 0.1*np.random.randn(3) + transformed_point = point + 0.1 * rng.standard_normal(3) initial_estimate.insert(L(j), transformed_point) - initial_estimate.print('Initial Estimates:\n') + initial_estimate.print("Initial Estimates:\n") # Optimize the graph and print results params = gtsam.DoglegParams() - params.setVerbosity('TERMINATION') + params.setVerbosity("TERMINATION") optimizer = DoglegOptimizer(graph, initial_estimate, params) - print('Optimizing:') + print("Optimizing:") result = optimizer.optimize() - result.print('Final results:\n') - print('initial error = {}'.format(graph.error(initial_estimate))) - print('final error = {}'.format(graph.error(result))) + result.print("Final results:\n") + print("initial error = {}".format(graph.error(initial_estimate))) + print("final error = {}".format(graph.error(result))) marginals = Marginals(graph, result) plot.plot_3d_points(1, result, marginals=marginals) @@ -117,5 +119,6 @@ def main(): plot.set_axes_equal(1) plt.show() -if __name__ == '__main__': + +if __name__ == "__main__": main() From 546c5712189fd7304976e2a9fb34ba9b63570bc1 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 26 Oct 2024 10:30:18 -0700 Subject: [PATCH 08/34] Clarify stubgen need --- python/README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/python/README.md b/python/README.md index e81be74fc..b80c95261 100644 --- a/python/README.md +++ b/python/README.md @@ -13,7 +13,7 @@ For instructions on updating the version of the [wrap library](https://github.co use the `-DGTSAM_PYTHON_VERSION=3.6` option when running `cmake` otherwise the default interpreter will be used. - If the interpreter is inside an environment (such as an anaconda environment or virtualenv environment), then the environment should be active while building GTSAM. -- This wrapper needs `pyparsing(>=2.4.2)`, and `numpy(>=1.11.0)`. These can be installed as follows: +- This wrapper needs [pyparsing(>=2.4.2)](https://github.com/pyparsing/pyparsing), [pybind-stubgen>=2.5.1](https://github.com/sizmailov/pybind11-stubgen) and [numpy(>=1.11.0)](https://numpy.org/). These can all be installed as follows: ```bash pip install -r /python/dev_requirements.txt From e05a990d1c7c63d5a4ad9cc032dd75a7df53f8a8 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 26 Oct 2024 11:56:33 -0700 Subject: [PATCH 09/34] EdgeKey wrapper --- gtsam/inference/EdgeKey.h | 5 ++++- gtsam/inference/inference.i | 13 +++++++++++++ 2 files changed, 17 insertions(+), 1 deletion(-) diff --git a/gtsam/inference/EdgeKey.h b/gtsam/inference/EdgeKey.h index bf1bf6030..3d95e4c06 100644 --- a/gtsam/inference/EdgeKey.h +++ b/gtsam/inference/EdgeKey.h @@ -46,7 +46,10 @@ class GTSAM_EXPORT EdgeKey { /// @{ /// Cast to Key - operator Key() const { return ((std::uint64_t)i_ << 32) | j_; } + Key key() const { return ((std::uint64_t)i_ << 32) | j_; } + + /// Cast to Key + operator Key() const { return key(); } /// Retrieve high 32 bits inline std::uint32_t i() const { return i_; } diff --git a/gtsam/inference/inference.i b/gtsam/inference/inference.i index b25e2fd62..5084355a8 100644 --- a/gtsam/inference/inference.i +++ b/gtsam/inference/inference.i @@ -96,6 +96,19 @@ unsigned char mrsymbolChr(size_t key); unsigned char mrsymbolLabel(size_t key); size_t mrsymbolIndex(size_t key); +#include +class EdgeKey { + EdgeKey(std::uint32_t i, std::uint32_t j); + EdgeKey(size_t key); + EdgeKey(const gtsam::EdgeKey& key); + + std::uint32_t i() const; + std::uint32_t j() const; + size_t key() const; + + void print(string s = "") const; +}; + #include class Ordering { /// Type of ordering to use From b99fa19ad8639a4949e8586565342e10fea6ea19 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 26 Oct 2024 11:56:59 -0700 Subject: [PATCH 10/34] Copy dirty examples --- python/CMakeLists.txt | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/python/CMakeLists.txt b/python/CMakeLists.txt index 9070d191b..f0fc3f796 100644 --- a/python/CMakeLists.txt +++ b/python/CMakeLists.txt @@ -156,6 +156,11 @@ foreach(test_file ${GTSAM_PYTHON_TEST_FILES}) get_filename_component(test_file_name ${test_file} NAME) configure_file(${test_file} "${GTSAM_MODULE_PATH}/tests/${test_file_name}" COPYONLY) endforeach() +file(GLOB GTSAM_PYTHON_TEST_FILES "${CMAKE_CURRENT_SOURCE_DIR}/gtsam/examples/*.py") +foreach(test_file ${GTSAM_PYTHON_TEST_FILES}) + get_filename_component(test_file_name ${test_file} NAME) + configure_file(${test_file} "${GTSAM_MODULE_PATH}/tests/${test_file_name}" COPYONLY) +endforeach() file(GLOB GTSAM_PYTHON_UTIL_FILES "${CMAKE_CURRENT_SOURCE_DIR}/gtsam/utils/*.py") foreach(util_file ${GTSAM_PYTHON_UTIL_FILES}) configure_file(${util_file} "${GTSAM_MODULE_PATH}/utils/${test_file}" COPYONLY) From b8506e06fec95892c5fd16c222f7a16bf3be6da9 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 26 Oct 2024 11:57:28 -0700 Subject: [PATCH 11/34] wrap new factor --- gtsam/sfm/sfm.i | 10 ++++++++++ 1 file changed, 10 insertions(+) diff --git a/gtsam/sfm/sfm.i b/gtsam/sfm/sfm.i index 142e65d7e..1223ef13c 100644 --- a/gtsam/sfm/sfm.i +++ b/gtsam/sfm/sfm.i @@ -75,6 +75,16 @@ bool writeBAL(string filename, gtsam::SfmData& data); gtsam::Values initialCamerasEstimate(const gtsam::SfmData& db); gtsam::Values initialCamerasAndPointsEstimate(const gtsam::SfmData& db); +#include +#include +#include +template +virtual class EssentialTransferFactor : gtsam::NoiseModelFactor { + EssentialTransferFactor(gtsam::EdgeKey edge1, gtsam::EdgeKey edge2, + const std::vector>& triplets + ); +}; + #include virtual class ShonanFactor3 : gtsam::NoiseModelFactor { From 4057e41a808412e68e2e2bb104158784f54b8827 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 26 Oct 2024 11:58:38 -0700 Subject: [PATCH 12/34] Python version of EssentialViewGraphExample --- examples/EssentialViewGraphExample.cpp | 4 +- .../examples/EssentialViewGraphExample.py | 126 ++++++++++++++++++ 2 files changed, 128 insertions(+), 2 deletions(-) create mode 100644 python/gtsam/examples/EssentialViewGraphExample.py diff --git a/examples/EssentialViewGraphExample.cpp b/examples/EssentialViewGraphExample.cpp index 91b2bb042..af254a94d 100644 --- a/examples/EssentialViewGraphExample.cpp +++ b/examples/EssentialViewGraphExample.cpp @@ -131,8 +131,8 @@ int main(int argc, char* argv[]) { result.print("Final Results:\n", formatter); - cout << "Ground Truth E1:\n" << E1.matrix() << endl; - cout << "Ground Truth E2:\n" << E2.matrix() << endl; + E1.print("Ground Truth E1:\n"); + E2.print("Ground Truth E2:\n"); return 0; } \ No newline at end of file diff --git a/python/gtsam/examples/EssentialViewGraphExample.py b/python/gtsam/examples/EssentialViewGraphExample.py new file mode 100644 index 000000000..2d3df815c --- /dev/null +++ b/python/gtsam/examples/EssentialViewGraphExample.py @@ -0,0 +1,126 @@ +""" + 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 + + Solve a structure-from-motion problem from a "Bundle Adjustment in the Large" file + Author: Frank Dellaert (Python: Akshay Krishnan, John Lambert, Varun Agrawal) +""" + +""" +Python version of EssentialViewGraphExample.cpp +View-graph calibration with essential matrices. +Author: Frank Dellaert +Date: October 2024 +""" + +import numpy as np +from gtsam.examples import SFMdata + +import gtsam +from gtsam import Cal3_S2, EdgeKey, EssentialMatrix +from gtsam import EssentialTransferFactorCal3_S2 as Factor +from gtsam import (LevenbergMarquardtOptimizer, LevenbergMarquardtParams, + NonlinearFactorGraph, PinholeCameraCal3_S2, Point2, Point3, + Pose3, Values, symbol_shorthand) + +# For symbol shorthand (e.g., X(0), L(1)) +K = symbol_shorthand.K + + +# Formatter function for printing keys +def formatter(key): + sym = gtsam.Symbol(key) + if sym.chr() == ord("K"): + return str(sym) + elif sym.chr() == ord("E"): + idx = sym.index() + a = idx // 10 + b = idx % 10 + return f"E({a},{b})" + else: + return str(sym) + + +def main(): + # Define the camera calibration parameters + K_initial = Cal3_S2(50.0, 50.0, 0.0, 50.0, 50.0) + + # Create the set of 8 ground-truth landmarks + points = SFMdata.createPoints() + + # Create the set of 4 ground-truth poses + poses = SFMdata.posesOnCircle(4, 30) + + # Calculate ground truth essential matrices, 1 and 2 poses apart + E1 = EssentialMatrix.FromPose3(poses[0].between(poses[1])) + E2 = EssentialMatrix.FromPose3(poses[0].between(poses[2])) + + # Simulate measurements from each camera pose + p = [[None for _ in range(8)] for _ in range(4)] + for i in range(4): + camera = PinholeCameraCal3_S2(poses[i], K_initial) + for j in range(8): + p[i][j] = camera.project(points[j]) + + # Create the factor graph + graph = NonlinearFactorGraph() + + for a in range(4): + b = (a + 1) % 4 # Next camera + c = (a + 2) % 4 # Camera after next + + # Collect data for the three factors + tuples1 = [] + tuples2 = [] + tuples3 = [] + + for j in range(8): + tuples1.append((p[a][j], p[b][j], p[c][j])) + tuples2.append((p[a][j], p[c][j], p[b][j])) + tuples3.append((p[c][j], p[b][j], p[a][j])) + + # Add transfer factors between views a, b, and c. + graph.add(Factor(EdgeKey(a, c), EdgeKey(b, c), tuples1)) + graph.add(Factor(EdgeKey(a, b), EdgeKey(b, c), tuples2)) + graph.add(Factor(EdgeKey(a, c), EdgeKey(a, b), tuples3)) + + # Create a delta vector to perturb the ground truth (small perturbation) + delta = np.ones(5) * 1e-2 + + # Create the initial estimate for essential matrices + initialEstimate = Values() + for a in range(4): + b = (a + 1) % 4 # Next camera + c = (a + 2) % 4 # Camera after next + initialEstimate.insert(EdgeKey(a, b).key(), E1.retract(delta)) + initialEstimate.insert(EdgeKey(a, c).key(), E2.retract(delta)) + + # Insert initial calibrations + for i in range(4): + initialEstimate.insert(K(i), K_initial) + + # Optimize the graph and print results + params = LevenbergMarquardtParams() + params.setlambdaInitial(1000.0) # Initialize lambda to a high value + params.setVerbosityLM("SUMMARY") + optimizer = LevenbergMarquardtOptimizer(graph, initialEstimate, params) + result = optimizer.optimize() + + print("Initial error = ", graph.error(initialEstimate)) + print("Final error = ", graph.error(result)) + + # Print final results + print("Final Results:") + result.print("", formatter) + + # Print ground truth essential matrices + print("Ground Truth E1:\n", E1) + print("Ground Truth E2:\n", E2) + + +if __name__ == "__main__": + main() From cd6c0b0a6905e71dc126ad426d150fccf7850211 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 26 Oct 2024 14:55:30 -0700 Subject: [PATCH 13/34] Split off Cal3f from Cal3Bundler --- gtsam/geometry/Cal3Bundler.cpp | 6 +- gtsam/geometry/Cal3Bundler.h | 41 +++---- gtsam/geometry/Cal3f.cpp | 106 +++++++++++++++++ gtsam/geometry/Cal3f.h | 141 +++++++++++++++++++++++ gtsam/geometry/geometry.i | 124 ++++++++++---------- gtsam/geometry/tests/testCal3Bundler.cpp | 40 ++++--- gtsam/geometry/tests/testCal3f.cpp | 132 +++++++++++++++++++++ 7 files changed, 483 insertions(+), 107 deletions(-) create mode 100644 gtsam/geometry/Cal3f.cpp create mode 100644 gtsam/geometry/Cal3f.h create mode 100644 gtsam/geometry/tests/testCal3f.cpp diff --git a/gtsam/geometry/Cal3Bundler.cpp b/gtsam/geometry/Cal3Bundler.cpp index 7e008aec3..1520e596a 100644 --- a/gtsam/geometry/Cal3Bundler.cpp +++ b/gtsam/geometry/Cal3Bundler.cpp @@ -56,10 +56,8 @@ void Cal3Bundler::print(const std::string& s) const { /* ************************************************************************* */ bool Cal3Bundler::equals(const Cal3Bundler& K, double tol) const { - const Cal3* base = dynamic_cast(&K); - return (Cal3::equals(*base, tol) && std::fabs(k1_ - K.k1_) < tol && - std::fabs(k2_ - K.k2_) < tol && std::fabs(u0_ - K.u0_) < tol && - std::fabs(v0_ - K.v0_) < tol); + return Cal3f::equals(static_cast(K), tol) && + std::fabs(k1_ - K.k1_) < tol && std::fabs(k2_ - K.k2_) < tol; } /* ************************************************************************* */ diff --git a/gtsam/geometry/Cal3Bundler.h b/gtsam/geometry/Cal3Bundler.h index 725c1481f..081688d48 100644 --- a/gtsam/geometry/Cal3Bundler.h +++ b/gtsam/geometry/Cal3Bundler.h @@ -19,7 +19,7 @@ #pragma once -#include +#include #include namespace gtsam { @@ -29,22 +29,18 @@ namespace gtsam { * @ingroup geometry * \nosubgrouping */ -class GTSAM_EXPORT Cal3Bundler : public Cal3 { +class GTSAM_EXPORT Cal3Bundler : public Cal3f { private: - double k1_ = 0.0f, k2_ = 0.0f; ///< radial distortion - double tol_ = 1e-5; ///< tolerance value when calibrating + double k1_ = 0.0, k2_ = 0.0; ///< radial distortion coefficients + double tol_ = 1e-5; ///< tolerance value when calibrating - // NOTE: We use the base class fx to represent the common focal length. - // Also, image center parameters (u0, v0) are not optimized - // but are treated as constants. + // Note: u0 and v0 are constants and not optimized. public: enum { dimension = 3 }; - - ///< shared pointer to stereo calibration object using shared_ptr = std::shared_ptr; - /// @name Standard Constructors + /// @name Constructors /// @{ /// Default constructor @@ -61,9 +57,9 @@ class GTSAM_EXPORT Cal3Bundler : public Cal3 { */ Cal3Bundler(double f, double k1, double k2, double u0 = 0, double v0 = 0, double tol = 1e-5) - : Cal3(f, f, 0, u0, v0), k1_(k1), k2_(k2), tol_(tol) {} + : Cal3f(f, u0, v0), k1_(k1), k2_(k2), tol_(tol) {} - ~Cal3Bundler() override {} + ~Cal3Bundler() override = default; /// @} /// @name Testable @@ -77,24 +73,18 @@ class GTSAM_EXPORT Cal3Bundler : public Cal3 { void print(const std::string& s = "") const override; /// assert equality up to a tolerance - bool equals(const Cal3Bundler& K, double tol = 10e-9) const; + bool equals(const Cal3Bundler& K, double tol = 1e-9) const; /// @} /// @name Standard Interface /// @{ - /// distorsion parameter k1 + /// distortion parameter k1 inline double k1() const { return k1_; } - /// distorsion parameter k2 + /// distortion parameter k2 inline double k2() const { return k2_; } - /// image center in x - inline double px() const { return u0_; } - - /// image center in y - inline double py() const { return v0_; } - Matrix3 K() const override; ///< Standard 3*3 calibration matrix Vector4 k() const; ///< Radial distortion parameters (4 of them, 2 0) @@ -113,8 +103,7 @@ class GTSAM_EXPORT Cal3Bundler : public Cal3 { /** * Convert a pixel coordinate to ideal coordinate xy - * @param p point in image coordinates - * @param tol optional tolerance threshold value for iterative minimization + * @param pi point in image coordinates * @param Dcal optional 2*3 Jacobian wrpt Cal3Bundler parameters * @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates * @return point in intrinsic coordinates @@ -135,14 +124,14 @@ class GTSAM_EXPORT Cal3Bundler : public Cal3 { /// @name Manifold /// @{ - /// return DOF, dimensionality of tangent space + /// Return DOF, dimensionality of tangent space size_t dim() const override { return Dim(); } - /// return DOF, dimensionality of tangent space + /// Return DOF, dimensionality of tangent space inline static size_t Dim() { return dimension; } /// Update calibration with tangent space delta - inline Cal3Bundler retract(const Vector& d) const { + Cal3Bundler retract(const Vector& d) const { return Cal3Bundler(fx_ + d(0), k1_ + d(1), k2_ + d(2), u0_, v0_); } diff --git a/gtsam/geometry/Cal3f.cpp b/gtsam/geometry/Cal3f.cpp new file mode 100644 index 000000000..319155dc9 --- /dev/null +++ b/gtsam/geometry/Cal3f.cpp @@ -0,0 +1,106 @@ +/* ---------------------------------------------------------------------------- + + * 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 + + * -------------------------------------------------------------------------- */ + +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010 + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file Cal3f.cpp + * @brief Implementation file for Cal3f class + * @author Frank Dellaert + * @date October 2024 + */ + +#include +#include +#include + +#include + +namespace gtsam { + +/* ************************************************************************* */ +Cal3f::Cal3f(double f, double u0, double v0) : Cal3(f, f, 0.0, u0, v0) {} + +/* ************************************************************************* */ +std::ostream& operator<<(std::ostream& os, const Cal3f& cal) { + os << "f: " << cal.fx_ << ", px: " << cal.u0_ << ", py: " << cal.v0_; + return os; +} + +/* ************************************************************************* */ +void Cal3f::print(const std::string& s) const { + if (!s.empty()) std::cout << s << " "; + std::cout << *this << std::endl; +} + +/* ************************************************************************* */ +bool Cal3f::equals(const Cal3f& K, double tol) const { + return Cal3::equals(static_cast(K), tol); +} + +/* ************************************************************************* */ +Vector1 Cal3f::vector() const { + Vector1 v; + v << fx_; + return v; +} + +/* ************************************************************************* */ +Point2 Cal3f::uncalibrate(const Point2& p, OptionalJacobian<2, 1> Dcal, + OptionalJacobian<2, 2> Dp) const { + assert(fx_ == fy_ && s_ == 0.0); + const double x = p.x(), y = p.y(); + const double u = fx_ * x + u0_; + const double v = fx_ * y + v0_; + + if (Dcal) { + Dcal->resize(2, 1); + (*Dcal) << x, y; + } + + if (Dp) { + Dp->resize(2, 2); + (*Dp) << fx_, 0.0, // + 0.0, fx_; + } + + return Point2(u, v); +} + +/* ************************************************************************* */ +Point2 Cal3f::calibrate(const Point2& pi, OptionalJacobian<2, 1> Dcal, + OptionalJacobian<2, 2> Dp) const { + assert(fx_ == fy_ && s_ == 0.0); + const double u = pi.x(), v = pi.y(); + double delta_u = u - u0_, delta_v = v - v0_; + double inv_f = 1.0 / fx_; + Point2 point(inv_f * delta_u, inv_f * delta_v); + + if (Dcal) { + Dcal->resize(2, 1); + (*Dcal) << -inv_f * point.x(), -inv_f * point.y(); + } + + if (Dp) { + Dp->resize(2, 2); + (*Dp) << inv_f, 0.0, // + 0.0, inv_f; + } + + return point; +} + +} // namespace gtsam \ No newline at end of file diff --git a/gtsam/geometry/Cal3f.h b/gtsam/geometry/Cal3f.h new file mode 100644 index 000000000..e30da688c --- /dev/null +++ b/gtsam/geometry/Cal3f.h @@ -0,0 +1,141 @@ +/* ---------------------------------------------------------------------------- + + * 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 + + * -------------------------------------------------------------------------- */ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010 + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file Cal3f.h + * @brief Calibration model with a single focal length and zero skew. + * @author Frank Dellaert + */ + +#pragma once + +#include + +namespace gtsam { + +/** + * @brief Calibration model with a single focal length and zero skew. + * @ingroup geometry + * \nosubgrouping + */ +class GTSAM_EXPORT Cal3f : public Cal3 { + public: + enum { dimension = 1 }; + using shared_ptr = std::shared_ptr; + + /// @name Constructors + /// @{ + + /// Default constructor + Cal3f() = default; + + /** + * Constructor + * @param f focal length + * @param u0 image center x-coordinate (considered a constant) + * @param v0 image center y-coordinate (considered a constant) + */ + Cal3f(double f, double u0, double v0); + + ~Cal3f() override = default; + + /// @} + /// @name Testable + /// @{ + + /// Output stream operator + friend std::ostream& operator<<(std::ostream& os, const Cal3f& cal); + + /// print with optional string + void print(const std::string& s = "") const override; + + /// assert equality up to a tolerance + bool equals(const Cal3f& K, double tol = 1e-9) const; + + /// @} + /// @name Standard Interface + /// @{ + + /// focal length + inline double f() const { return fx_; } + + /// vectorized form (column-wise) + Vector1 vector() const; + + /** + * @brief: convert intrinsic coordinates xy to image coordinates uv + * Version of uncalibrate with derivatives + * @param p point in intrinsic coordinates + * @param Dcal optional 2*1 Jacobian wrpt focal length + * @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates + * @return point in image coordinates + */ + Point2 uncalibrate(const Point2& p, OptionalJacobian<2, 1> Dcal = {}, + OptionalJacobian<2, 2> Dp = {}) const; + + /** + * Convert a pixel coordinate to ideal coordinate xy + * @param pi point in image coordinates + * @param Dcal optional 2*1 Jacobian wrpt focal length + * @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates + * @return point in intrinsic coordinates + */ + Point2 calibrate(const Point2& pi, OptionalJacobian<2, 1> Dcal = {}, + OptionalJacobian<2, 2> Dp = {}) const; + + /// @} + /// @name Manifold + /// @{ + + /// Return DOF, dimensionality of tangent space + size_t dim() const override { return Dim(); } + + /// Return DOF, dimensionality of tangent space + inline static size_t Dim() { return dimension; } + + /// Update calibration with tangent space delta + Cal3f retract(const Vector& d) const { return Cal3f(fx_ + d(0), u0_, v0_); } + + /// Calculate local coordinates to another calibration + Vector1 localCoordinates(const Cal3f& T2) const { + return Vector1(T2.fx_ - fx_); + } + + /// @} + + private: +#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(Archive& ar, const unsigned int /*version*/) { + ar& BOOST_SERIALIZATION_NVP(fx_); + ar& BOOST_SERIALIZATION_NVP(u0_); + ar& BOOST_SERIALIZATION_NVP(v0_); + } +#endif + + /// @} +}; + +template <> +struct traits : public internal::Manifold {}; + +template <> +struct traits : public internal::Manifold {}; + +} // namespace gtsam \ No newline at end of file diff --git a/gtsam/geometry/geometry.i b/gtsam/geometry/geometry.i index a90de48e5..6421ce264 100644 --- a/gtsam/geometry/geometry.i +++ b/gtsam/geometry/geometry.i @@ -644,8 +644,36 @@ class EssentialMatrix { double error(gtsam::Vector vA, gtsam::Vector vB); }; +#include +virtual class Cal3 { + // Standard Constructors + Cal3(); + Cal3(double fx, double fy, double s, double u0, double v0); + Cal3(gtsam::Vector v); + + // Testable + void print(string s = "Cal3") const; + bool equals(const gtsam::Cal3& rhs, double tol) const; + + // Standard Interface + double fx() const; + double fy() const; + double aspectRatio() const; + double skew() const; + double px() const; + double py() const; + gtsam::Point2 principalPoint() const; + gtsam::Vector vector() const; + gtsam::Matrix K() const; + gtsam::Matrix inverse() const; + + // Manifold + static size_t Dim(); + size_t dim() const; +}; + #include -class Cal3_S2 { +virtual class Cal3_S2 : gtsam::Cal3 { // Standard Constructors Cal3_S2(); Cal3_S2(double fx, double fy, double s, double u0, double v0); @@ -672,23 +700,12 @@ class Cal3_S2 { Eigen::Ref Dcal, Eigen::Ref Dp) const; - // Standard Interface - double fx() const; - double fy() const; - double skew() const; - double px() const; - double py() const; - gtsam::Point2 principalPoint() const; - gtsam::Vector vector() const; - gtsam::Matrix K() const; - gtsam::Matrix inverse() const; - // enabling serialization functionality void serialize() const; }; #include -virtual class Cal3DS2_Base { +virtual class Cal3DS2_Base : gtsam::Cal3 { // Standard Constructors Cal3DS2_Base(); @@ -696,16 +713,8 @@ virtual class Cal3DS2_Base { void print(string s = "") const; // Standard Interface - double fx() const; - double fy() const; - double skew() const; - double px() const; - double py() const; double k1() const; double k2() const; - gtsam::Matrix K() const; - gtsam::Vector k() const; - gtsam::Vector vector() const; // Action on Point2 gtsam::Point2 uncalibrate(const gtsam::Point2& p) const; @@ -785,7 +794,7 @@ virtual class Cal3Unified : gtsam::Cal3DS2_Base { }; #include -class Cal3Fisheye { +virtual class Cal3Fisheye : gtsam::Cal3 { // Standard Constructors Cal3Fisheye(); Cal3Fisheye(double fx, double fy, double s, double u0, double v0, double k1, @@ -797,8 +806,6 @@ class Cal3Fisheye { bool equals(const gtsam::Cal3Fisheye& rhs, double tol) const; // Manifold - static size_t Dim(); - size_t dim() const; gtsam::Cal3Fisheye retract(gtsam::Vector v) const; gtsam::Vector localCoordinates(const gtsam::Cal3Fisheye& c) const; @@ -813,35 +820,23 @@ class Cal3Fisheye { Eigen::Ref Dp) const; // Standard Interface - double fx() const; - double fy() const; - double skew() const; double k1() const; double k2() const; double k3() const; double k4() const; - double px() const; - double py() const; - gtsam::Point2 principalPoint() const; - gtsam::Vector vector() const; - gtsam::Vector k() const; - gtsam::Matrix K() const; - gtsam::Matrix inverse() const; // enabling serialization functionality void serialize() const; }; #include -class Cal3_S2Stereo { +virtual class Cal3_S2Stereo : gtsam::Cal3{ // Standard Constructors Cal3_S2Stereo(); Cal3_S2Stereo(double fx, double fy, double s, double u0, double v0, double b); Cal3_S2Stereo(gtsam::Vector v); // Manifold - static size_t Dim(); - size_t dim() const; gtsam::Cal3_S2Stereo retract(gtsam::Vector v) const; gtsam::Vector localCoordinates(const gtsam::Cal3_S2Stereo& c) const; @@ -850,35 +845,23 @@ class Cal3_S2Stereo { bool equals(const gtsam::Cal3_S2Stereo& K, double tol) const; // Standard Interface - double fx() const; - double fy() const; - double skew() const; - double px() const; - double py() const; - gtsam::Matrix K() const; - gtsam::Point2 principalPoint() const; double baseline() const; gtsam::Vector6 vector() const; - gtsam::Matrix inverse() const; }; #include -class Cal3Bundler { +virtual class Cal3f : gtsam::Cal3 { // Standard Constructors - Cal3Bundler(); - Cal3Bundler(double fx, double k1, double k2, double u0, double v0); - Cal3Bundler(double fx, double k1, double k2, double u0, double v0, - double tol); + Cal3f(); + Cal3f(double fx, double u0, double v0); // Testable void print(string s = "") const; - bool equals(const gtsam::Cal3Bundler& rhs, double tol) const; + bool equals(const gtsam::Cal3f& rhs, double tol) const; // Manifold - static size_t Dim(); - size_t dim() const; - gtsam::Cal3Bundler retract(gtsam::Vector v) const; - gtsam::Vector localCoordinates(const gtsam::Cal3Bundler& c) const; + gtsam::Cal3f retract(gtsam::Vector v) const; + gtsam::Vector localCoordinates(const gtsam::Cal3f& c) const; // Action on Point2 gtsam::Point2 calibrate(const gtsam::Point2& p) const; @@ -891,15 +874,32 @@ class Cal3Bundler { Eigen::Ref Dp) const; // Standard Interface - double fx() const; - double fy() const; + double f() const; + + // enabling serialization functionality + void serialize() const; +}; + + +#include +virtual class Cal3Bundler : gtsam::Cal3f { + // Standard Constructors + Cal3Bundler(); + Cal3Bundler(double fx, double k1, double k2, double u0, double v0); + Cal3Bundler(double fx, double k1, double k2, double u0, double v0, + double tol); + + // Testable + void print(string s = "") const; + bool equals(const gtsam::Cal3Bundler& rhs, double tol) const; + + // Manifold + gtsam::Cal3Bundler retract(gtsam::Vector v) const; + gtsam::Vector localCoordinates(const gtsam::Cal3Bundler& c) const; + + // Standard Interface double k1() const; double k2() const; - double px() const; - double py() const; - gtsam::Vector vector() const; - gtsam::Vector k() const; - gtsam::Matrix K() const; // enabling serialization functionality void serialize() const; diff --git a/gtsam/geometry/tests/testCal3Bundler.cpp b/gtsam/geometry/tests/testCal3Bundler.cpp index 020cab2f9..15e633923 100644 --- a/gtsam/geometry/tests/testCal3Bundler.cpp +++ b/gtsam/geometry/tests/testCal3Bundler.cpp @@ -29,7 +29,7 @@ static Cal3Bundler K(500, 1e-3, 1e-3, 1000, 2000); static Point2 p(2, 3); /* ************************************************************************* */ -TEST(Cal3Bundler, vector) { +TEST(Cal3Bundler, Vector) { Cal3Bundler K; Vector expected(3); expected << 1, 0, 0; @@ -37,16 +37,19 @@ TEST(Cal3Bundler, vector) { } /* ************************************************************************* */ -TEST(Cal3Bundler, uncalibrate) { +TEST(Cal3Bundler, Uncalibrate) { Vector v = K.vector(); double r = p.x() * p.x() + p.y() * p.y(); - double g = v[0] * (1 + v[1] * r + v[2] * r * r); - Point2 expected(1000 + g * p.x(), 2000 + g * p.y()); + double distortion = 1.0 + v[1] * r + v[2] * r * r; + double u = K.px() + v[0] * distortion * p.x(); + double v_coord = K.py() + v[0] * distortion * p.y(); + Point2 expected(u, v_coord); Point2 actual = K.uncalibrate(p); CHECK(assert_equal(expected, actual)); } -TEST(Cal3Bundler, calibrate) { +/* ************************************************************************* */ +TEST(Cal3Bundler, Calibrate) { Point2 pn(0.5, 0.5); Point2 pi = K.uncalibrate(pn); Point2 pn_hat = K.calibrate(pi); @@ -63,11 +66,11 @@ Point2 calibrate_(const Cal3Bundler& k, const Point2& pt) { } /* ************************************************************************* */ -TEST(Cal3Bundler, DuncalibrateDefault) { +TEST(Cal3Bundler, DUncalibrateDefault) { Cal3Bundler trueK(1, 0, 0); Matrix Dcal, Dp; Point2 actual = trueK.uncalibrate(p, Dcal, Dp); - Point2 expected = p; + Point2 expected(p); // Since K is identity, uncalibrate should return p CHECK(assert_equal(expected, actual, 1e-7)); Matrix numerical1 = numericalDerivative21(uncalibrate_, trueK, p); Matrix numerical2 = numericalDerivative22(uncalibrate_, trueK, p); @@ -76,7 +79,7 @@ TEST(Cal3Bundler, DuncalibrateDefault) { } /* ************************************************************************* */ -TEST(Cal3Bundler, DcalibrateDefault) { +TEST(Cal3Bundler, DCalibrateDefault) { Cal3Bundler trueK(1, 0, 0); Matrix Dcal, Dp; Point2 pn(0.5, 0.5); @@ -90,11 +93,11 @@ TEST(Cal3Bundler, DcalibrateDefault) { } /* ************************************************************************* */ -TEST(Cal3Bundler, DuncalibratePrincipalPoint) { +TEST(Cal3Bundler, DUncalibratePrincipalPoint) { Cal3Bundler K(5, 0, 0, 2, 2); Matrix Dcal, Dp; Point2 actual = K.uncalibrate(p, Dcal, Dp); - Point2 expected(12, 17); + Point2 expected(2.0 + 5.0 * p.x(), 2.0 + 5.0 * p.y()); CHECK(assert_equal(expected, actual, 1e-7)); Matrix numerical1 = numericalDerivative21(uncalibrate_, K, p); Matrix numerical2 = numericalDerivative22(uncalibrate_, K, p); @@ -103,7 +106,7 @@ TEST(Cal3Bundler, DuncalibratePrincipalPoint) { } /* ************************************************************************* */ -TEST(Cal3Bundler, DcalibratePrincipalPoint) { +TEST(Cal3Bundler, DCalibratePrincipalPoint) { Cal3Bundler K(2, 0, 0, 2, 2); Matrix Dcal, Dp; Point2 pn(0.5, 0.5); @@ -117,11 +120,18 @@ TEST(Cal3Bundler, DcalibratePrincipalPoint) { } /* ************************************************************************* */ -TEST(Cal3Bundler, Duncalibrate) { +TEST(Cal3Bundler, DUncalibrate) { Matrix Dcal, Dp; Point2 actual = K.uncalibrate(p, Dcal, Dp); - Point2 expected(2182, 3773); + // Compute expected value manually + Vector v = K.vector(); + double r2 = p.x() * p.x() + p.y() * p.y(); + double distortion = 1.0 + v[1] * r2 + v[2] * r2 * r2; + Point2 expected( + K.px() + v[0] * distortion * p.x(), + K.py() + v[0] * distortion * p.y()); CHECK(assert_equal(expected, actual, 1e-7)); + Matrix numerical1 = numericalDerivative21(uncalibrate_, K, p); Matrix numerical2 = numericalDerivative22(uncalibrate_, K, p); CHECK(assert_equal(numerical1, Dcal, 1e-7)); @@ -129,7 +139,7 @@ TEST(Cal3Bundler, Duncalibrate) { } /* ************************************************************************* */ -TEST(Cal3Bundler, Dcalibrate) { +TEST(Cal3Bundler, DCalibrate) { Matrix Dcal, Dp; Point2 pn(0.5, 0.5); Point2 pi = K.uncalibrate(pn); @@ -145,7 +155,7 @@ TEST(Cal3Bundler, Dcalibrate) { TEST(Cal3Bundler, assert_equal) { CHECK(assert_equal(K, K, 1e-7)); } /* ************************************************************************* */ -TEST(Cal3Bundler, retract) { +TEST(Cal3Bundler, Retract) { Cal3Bundler expected(510, 2e-3, 2e-3, 1000, 2000); EXPECT_LONGS_EQUAL(3, expected.dim()); diff --git a/gtsam/geometry/tests/testCal3f.cpp b/gtsam/geometry/tests/testCal3f.cpp new file mode 100644 index 000000000..d21d39f7f --- /dev/null +++ b/gtsam/geometry/tests/testCal3f.cpp @@ -0,0 +1,132 @@ +/* ---------------------------------------------------------------------------- + + * 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 testCal3F.cpp + * @brief Unit tests for the Cal3f calibration model. + */ + +#include +#include +#include +#include +#include + +using namespace gtsam; + +GTSAM_CONCEPT_TESTABLE_INST(Cal3f) +GTSAM_CONCEPT_MANIFOLD_INST(Cal3f) + +static Cal3f K(500.0, 1000.0, 2000.0); // f = 500, u0 = 1000, v0 = 2000 +static Point2 p(2.0, 3.0); + +/* ************************************************************************* */ +TEST(Cal3f, Vector) { + Cal3f K(1.0, 0.0, 0.0); + Vector expected(1); + expected << 1.0; + CHECK(assert_equal(expected, K.vector())); +} + +/* ************************************************************************* */ +TEST(Cal3f, Uncalibrate) { + // Expected output: apply the intrinsic calibration matrix to point p + Matrix3 K_matrix = K.K(); + Vector3 p_homogeneous(p.x(), p.y(), 1.0); + Vector3 expected_homogeneous = K_matrix * p_homogeneous; + Point2 expected(expected_homogeneous.x() / expected_homogeneous.z(), + expected_homogeneous.y() / expected_homogeneous.z()); + + Point2 actual = K.uncalibrate(p); + CHECK(assert_equal(expected, actual)); +} + +/* ************************************************************************* */ +TEST(Cal3f, Calibrate) { + Point2 pi = K.uncalibrate(p); + Point2 pn = K.calibrate(pi); + CHECK(traits::Equals(p, pn, 1e-9)); +} + +/* ************************************************************************* */ +Point2 uncalibrate_(const Cal3f& k, const Point2& pt) { + return k.uncalibrate(pt); +} + +Point2 calibrate_(const Cal3f& k, const Point2& pt) { return k.calibrate(pt); } + +/* ************************************************************************* */ +TEST(Cal3f, DUncalibrate) { + Cal3f K(500.0, 1000.0, 2000.0); + Matrix Dcal, Dp; + Point2 actual = K.uncalibrate(p, Dcal, Dp); + + // Expected value computed manually + Point2 expected = Point2(K.px() + K.fx() * p.x(), K.py() + K.fx() * p.y()); + CHECK(assert_equal(expected, actual, 1e-9)); + + // Compute numerical derivatives + Matrix numerical1 = numericalDerivative21(uncalibrate_, K, p); + Matrix numerical2 = numericalDerivative22(uncalibrate_, K, p); + CHECK(assert_equal(numerical1, Dcal, 1e-6)); + CHECK(assert_equal(numerical2, Dp, 1e-6)); +} + +/* ************************************************************************* */ +TEST(Cal3f, DCalibrate) { + Point2 pi = K.uncalibrate(p); + Matrix Dcal, Dp; + Point2 actual = K.calibrate(pi, Dcal, Dp); + CHECK(assert_equal(p, actual, 1e-9)); + + // Compute numerical derivatives + Matrix numerical1 = numericalDerivative21(calibrate_, K, pi); + Matrix numerical2 = numericalDerivative22(calibrate_, K, pi); + CHECK(assert_equal(numerical1, Dcal, 1e-6)); + CHECK(assert_equal(numerical2, Dp, 1e-6)); +} + +/* ************************************************************************* */ +TEST(Cal3f, Manifold) { + Cal3f K1(500.0, 1000.0, 2000.0); + Vector1 delta; + delta << 10.0; // Increment focal length by 10 + + Cal3f K2 = K1.retract(delta); + CHECK(assert_equal(510.0, K2.fx(), 1e-9)); + CHECK(assert_equal(K1.px(), K2.px(), 1e-9)); + CHECK(assert_equal(K1.py(), K2.py(), 1e-9)); + + Vector1 delta_computed = K1.localCoordinates(K2); + CHECK(assert_equal(delta, delta_computed, 1e-9)); +} + +/* ************************************************************************* */ +TEST(Cal3f, Assert_equal) { + CHECK(assert_equal(K, K, 1e-9)); + Cal3f K2(500.0, 1000.0, 2000.0); + CHECK(assert_equal(K, K2, 1e-9)); +} + +/* ************************************************************************* */ +TEST(Cal3f, Print) { + Cal3f cal(500.0, 1000.0, 2000.0); + std::stringstream os; + os << "f: " << cal.fx() << ", px: " << cal.px() << ", py: " << cal.py(); + + EXPECT(assert_stdout_equal(os.str(), cal)); +} + +/* ************************************************************************* */ +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +/* ************************************************************************* */ \ No newline at end of file From 68c63ca04302451350f45c8f2e17a676a5de7f14 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 26 Oct 2024 15:57:42 -0700 Subject: [PATCH 14/34] Python version of ViewGraphExample.cpp --- gtsam/sfm/sfm.i | 11 +- .../examples/EssentialViewGraphExample.py | 19 ++- python/gtsam/examples/ViewGraphExample.py | 114 ++++++++++++++++++ 3 files changed, 132 insertions(+), 12 deletions(-) create mode 100644 python/gtsam/examples/ViewGraphExample.py diff --git a/gtsam/sfm/sfm.i b/gtsam/sfm/sfm.i index 1223ef13c..80479eb78 100644 --- a/gtsam/sfm/sfm.i +++ b/gtsam/sfm/sfm.i @@ -76,9 +76,18 @@ gtsam::Values initialCamerasEstimate(const gtsam::SfmData& db); gtsam::Values initialCamerasAndPointsEstimate(const gtsam::SfmData& db); #include +#include +template +virtual class TransferFactor : gtsam::NoiseModelFactor { + TransferFactor(gtsam::EdgeKey edge1, gtsam::EdgeKey edge2, + const std::vector>& triplets + ); +}; + #include +#include #include -template +template virtual class EssentialTransferFactor : gtsam::NoiseModelFactor { EssentialTransferFactor(gtsam::EdgeKey edge1, gtsam::EdgeKey edge2, const std::vector>& triplets diff --git a/python/gtsam/examples/EssentialViewGraphExample.py b/python/gtsam/examples/EssentialViewGraphExample.py index 2d3df815c..a3c313674 100644 --- a/python/gtsam/examples/EssentialViewGraphExample.py +++ b/python/gtsam/examples/EssentialViewGraphExample.py @@ -24,25 +24,20 @@ import gtsam from gtsam import Cal3_S2, EdgeKey, EssentialMatrix from gtsam import EssentialTransferFactorCal3_S2 as Factor from gtsam import (LevenbergMarquardtOptimizer, LevenbergMarquardtParams, - NonlinearFactorGraph, PinholeCameraCal3_S2, Point2, Point3, - Pose3, Values, symbol_shorthand) + NonlinearFactorGraph, PinholeCameraCal3_S2, Values) # For symbol shorthand (e.g., X(0), L(1)) -K = symbol_shorthand.K +K = gtsam.symbol_shorthand.K # Formatter function for printing keys def formatter(key): sym = gtsam.Symbol(key) - if sym.chr() == ord("K"): - return str(sym) - elif sym.chr() == ord("E"): - idx = sym.index() - a = idx // 10 - b = idx % 10 - return f"E({a},{b})" + if sym.chr() == ord("k"): + return f"k{sym.index()}" else: - return str(sym) + edge = EdgeKey(key) + return f"({edge.i()},{edge.j()})" def main(): @@ -88,6 +83,8 @@ def main(): graph.add(Factor(EdgeKey(a, b), EdgeKey(b, c), tuples2)) graph.add(Factor(EdgeKey(a, c), EdgeKey(a, b), tuples3)) + graph.print("graph", formatter) + # Create a delta vector to perturb the ground truth (small perturbation) delta = np.ones(5) * 1e-2 diff --git a/python/gtsam/examples/ViewGraphExample.py b/python/gtsam/examples/ViewGraphExample.py new file mode 100644 index 000000000..fe4d78e3c --- /dev/null +++ b/python/gtsam/examples/ViewGraphExample.py @@ -0,0 +1,114 @@ +""" + 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 + + Solve a structure-from-motion problem from a "Bundle Adjustment in the Large" file + Author: Frank Dellaert (Python: Akshay Krishnan, John Lambert, Varun Agrawal) +""" + +""" +Python version of ViewGraphExample.cpp +View-graph calibration on a simulated dataset, a la Sweeney 2015 +Author: Frank Dellaert +Date: October 2024 +""" + +import numpy as np +from gtsam.examples import SFMdata + +from gtsam import (Cal3_S2, EdgeKey, FundamentalMatrix, + LevenbergMarquardtOptimizer, LevenbergMarquardtParams, + NonlinearFactorGraph, PinholeCameraCal3_S2) +from gtsam import TransferFactorFundamentalMatrix as Factor +from gtsam import Values + + +# Formatter function for printing keys +def formatter(key): + edge = EdgeKey(key) + return f"({edge.i()},{edge.j()})" + + +def main(): + # Define the camera calibration parameters + K = Cal3_S2(50.0, 50.0, 0.0, 50.0, 50.0) + + # Create the set of 8 ground-truth landmarks + points = SFMdata.createPoints() + + # Create the set of 4 ground-truth poses + poses = SFMdata.posesOnCircle(4, 30) + + # Calculate ground truth fundamental matrices, 1 and 2 poses apart + F1 = FundamentalMatrix(K, poses[0].between(poses[1]), K) + F2 = FundamentalMatrix(K, poses[0].between(poses[2]), K) + + # Simulate measurements from each camera pose + p = [[None for _ in range(8)] for _ in range(4)] + for i in range(4): + camera = PinholeCameraCal3_S2(poses[i], K) + for j in range(8): + p[i][j] = camera.project(points[j]) + + # Create the factor graph + graph = NonlinearFactorGraph() + + for a in range(4): + b = (a + 1) % 4 # Next camera + c = (a + 2) % 4 # Camera after next + + # Vectors to collect tuples for each factor + tuples1 = [] + tuples2 = [] + tuples3 = [] + + # Collect data for the three factors + for j in range(8): + tuples1.append((p[a][j], p[b][j], p[c][j])) + tuples2.append((p[a][j], p[c][j], p[b][j])) + tuples3.append((p[c][j], p[b][j], p[a][j])) + + # Add transfer factors between views a, b, and c. + graph.add(Factor(EdgeKey(a, c), EdgeKey(b, c), tuples1)) + graph.add(Factor(EdgeKey(a, b), EdgeKey(b, c), tuples2)) + graph.add(Factor(EdgeKey(a, c), EdgeKey(a, b), tuples3)) + + # Print the factor graph + graph.print("Factor Graph:\n", formatter) + + # Create a delta vector to perturb the ground truth + delta = np.array([1, 2, 3, 4, 5, 6, 7]) * 1e-5 + + # Create the data structure to hold the initial estimate to the solution + initialEstimate = Values() + for a in range(4): + b = (a + 1) % 4 # Next camera + c = (a + 2) % 4 # Camera after next + initialEstimate.insert(EdgeKey(a, b).key(), F1.retract(delta)) + initialEstimate.insert(EdgeKey(a, c).key(), F2.retract(delta)) + + initialEstimate.print("Initial Estimates:\n", formatter) + graph.printErrors(initialEstimate, "Initial Errors:\n", formatter) + + # Optimize the graph and print results + params = LevenbergMarquardtParams() + params.setLambdaInitial(1000.0) # Initialize lambda to a high value + params.setVerbosityLM("SUMMARY") + optimizer = LevenbergMarquardtOptimizer(graph, initialEstimate, params) + result = optimizer.optimize() + + print(f"Initial error = {graph.error(initialEstimate)}") + print(f"Final error = {graph.error(result)}") + + result.print("Final Results:\n", formatter) + + print("Ground Truth F1:\n", F1.matrix()) + print("Ground Truth F2:\n", F2.matrix()) + + +if __name__ == "__main__": + main() From 5b0580ff5f309b8a7d47333d00b2617444035867 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 26 Oct 2024 16:10:47 -0700 Subject: [PATCH 15/34] Small issues --- gtsam/nonlinear/nonlinear.i | 5 +-- gtsam/nonlinear/values.i | 44 ++++++++++++++++--- .../examples/EssentialViewGraphExample.py | 3 -- python/gtsam/examples/ViewGraphExample.py | 5 +-- 4 files changed, 41 insertions(+), 16 deletions(-) diff --git a/gtsam/nonlinear/nonlinear.i b/gtsam/nonlinear/nonlinear.i index f493fe8f6..80f694e97 100644 --- a/gtsam/nonlinear/nonlinear.i +++ b/gtsam/nonlinear/nonlinear.i @@ -537,9 +537,8 @@ class ISAM2 { gtsam::Values calculateEstimate() const; template , gtsam::PinholeCamera, gtsam::PinholeCamera, diff --git a/gtsam/nonlinear/values.i b/gtsam/nonlinear/values.i index 3582a3249..a75fe5ae1 100644 --- a/gtsam/nonlinear/values.i +++ b/gtsam/nonlinear/values.i @@ -4,13 +4,14 @@ namespace gtsam { +#include #include #include +#include #include #include -#include -#include #include +#include #include #include #include @@ -80,17 +81,24 @@ class Values { void insert(size_t j, const gtsam::Rot3& rot3); void insert(size_t j, const gtsam::Pose3& pose3); void insert(size_t j, const gtsam::Unit3& unit3); + void insert(size_t j, const gtsam::Cal3f& cal3f); void insert(size_t j, const gtsam::Cal3_S2& cal3_s2); void insert(size_t j, const gtsam::Cal3DS2& cal3ds2); void insert(size_t j, const gtsam::Cal3Bundler& cal3bundler); void insert(size_t j, const gtsam::Cal3Fisheye& cal3fisheye); void insert(size_t j, const gtsam::Cal3Unified& cal3unified); - void insert(size_t j, const gtsam::EssentialMatrix& essential_matrix); + void insert(size_t j, const gtsam::EssentialMatrix& E); + void insert(size_t j, const gtsam::FundamentalMatrix& F); + void insert(size_t j, const gtsam::SimpleFundamentalMatrix& F); + void insert(size_t j, const gtsam::PinholeCamera& camera); void insert(size_t j, const gtsam::PinholeCamera& camera); + void insert(size_t j, const gtsam::PinholeCamera& camera); void insert(size_t j, const gtsam::PinholeCamera& camera); void insert(size_t j, const gtsam::PinholeCamera& camera); void insert(size_t j, const gtsam::PinholeCamera& camera); + void insert(size_t j, const gtsam::PinholePose& camera); void insert(size_t j, const gtsam::PinholePose& camera); + void insert(size_t j, const gtsam::PinholePose& camera); void insert(size_t j, const gtsam::PinholePose& camera); void insert(size_t j, const gtsam::PinholePose& camera); void insert(size_t j, const gtsam::PinholePose& camera); @@ -115,17 +123,24 @@ class Values { void update(size_t j, const gtsam::Rot3& rot3); void update(size_t j, const gtsam::Pose3& pose3); void update(size_t j, const gtsam::Unit3& unit3); + void update(size_t j, const gtsam::Cal3f& cal3f); void update(size_t j, const gtsam::Cal3_S2& cal3_s2); void update(size_t j, const gtsam::Cal3DS2& cal3ds2); void update(size_t j, const gtsam::Cal3Bundler& cal3bundler); void update(size_t j, const gtsam::Cal3Fisheye& cal3fisheye); void update(size_t j, const gtsam::Cal3Unified& cal3unified); - void update(size_t j, const gtsam::EssentialMatrix& essential_matrix); + void update(size_t j, const gtsam::EssentialMatrix& E); + void update(size_t j, const gtsam::FundamentalMatrix& F); + void update(size_t j, const gtsam::SimpleFundamentalMatrix& F); + void update(size_t j, const gtsam::PinholeCamera& camera); void update(size_t j, const gtsam::PinholeCamera& camera); + void update(size_t j, const gtsam::PinholeCamera& camera); void update(size_t j, const gtsam::PinholeCamera& camera); void update(size_t j, const gtsam::PinholeCamera& camera); void update(size_t j, const gtsam::PinholeCamera& camera); + void update(size_t j, const gtsam::PinholePose& camera); void update(size_t j, const gtsam::PinholePose& camera); + void update(size_t j, const gtsam::PinholePose& camera); void update(size_t j, const gtsam::PinholePose& camera); void update(size_t j, const gtsam::PinholePose& camera); void update(size_t j, const gtsam::PinholePose& camera); @@ -147,23 +162,33 @@ class Values { void insert_or_assign(size_t j, const gtsam::Rot3& rot3); void insert_or_assign(size_t j, const gtsam::Pose3& pose3); void insert_or_assign(size_t j, const gtsam::Unit3& unit3); + void insert_or_assign(size_t j, const gtsam::Cal3f& cal3f); void insert_or_assign(size_t j, const gtsam::Cal3_S2& cal3_s2); void insert_or_assign(size_t j, const gtsam::Cal3DS2& cal3ds2); void insert_or_assign(size_t j, const gtsam::Cal3Bundler& cal3bundler); void insert_or_assign(size_t j, const gtsam::Cal3Fisheye& cal3fisheye); void insert_or_assign(size_t j, const gtsam::Cal3Unified& cal3unified); + void insert_or_assign(size_t j, const gtsam::EssentialMatrix& E); + void insert_or_assign(size_t j, const gtsam::FundamentalMatrix& F); + void insert_or_assign(size_t j, const gtsam::SimpleFundamentalMatrix& F); void insert_or_assign(size_t j, - const gtsam::EssentialMatrix& essential_matrix); + const gtsam::PinholeCamera& camera); void insert_or_assign(size_t j, const gtsam::PinholeCamera& camera); + void insert_or_assign(size_t j, + const gtsam::PinholeCamera& camera); void insert_or_assign(size_t j, const gtsam::PinholeCamera& camera); void insert_or_assign(size_t j, const gtsam::PinholeCamera& camera); void insert_or_assign(size_t j, const gtsam::PinholeCamera& camera); + void insert_or_assign(size_t j, + const gtsam::PinholePose& camera); void insert_or_assign(size_t j, const gtsam::PinholePose& camera); + void insert_or_assign(size_t j, + const gtsam::PinholePose& camera); void insert_or_assign(size_t j, const gtsam::PinholePose& camera); void insert_or_assign(size_t j, @@ -185,17 +210,24 @@ class Values { gtsam::Rot3, gtsam::Pose3, gtsam::Unit3, + gtsam::Cal3f, gtsam::Cal3_S2, gtsam::Cal3DS2, gtsam::Cal3Bundler, gtsam::Cal3Fisheye, gtsam::Cal3Unified, - gtsam::EssentialMatrix, + gtsam::EssentialMatrix, + gtsam::FundamentalMatrix, + gtsam::SimpleFundamentalMatrix, + gtsam::PinholeCamera, gtsam::PinholeCamera, + gtsam::PinholeCamera, gtsam::PinholeCamera, gtsam::PinholeCamera, gtsam::PinholeCamera, + gtsam::PinholePose, gtsam::PinholePose, + gtsam::PinholePose, gtsam::PinholePose, gtsam::PinholePose, gtsam::PinholePose, diff --git a/python/gtsam/examples/EssentialViewGraphExample.py b/python/gtsam/examples/EssentialViewGraphExample.py index a3c313674..d7d2cda38 100644 --- a/python/gtsam/examples/EssentialViewGraphExample.py +++ b/python/gtsam/examples/EssentialViewGraphExample.py @@ -5,9 +5,6 @@ Authors: Frank Dellaert, et al. (see THANKS for the full author list) See LICENSE for the license information - - Solve a structure-from-motion problem from a "Bundle Adjustment in the Large" file - Author: Frank Dellaert (Python: Akshay Krishnan, John Lambert, Varun Agrawal) """ """ diff --git a/python/gtsam/examples/ViewGraphExample.py b/python/gtsam/examples/ViewGraphExample.py index fe4d78e3c..0ba41762e 100644 --- a/python/gtsam/examples/ViewGraphExample.py +++ b/python/gtsam/examples/ViewGraphExample.py @@ -5,9 +5,6 @@ Authors: Frank Dellaert, et al. (see THANKS for the full author list) See LICENSE for the license information - - Solve a structure-from-motion problem from a "Bundle Adjustment in the Large" file - Author: Frank Dellaert (Python: Akshay Krishnan, John Lambert, Varun Agrawal) """ """ @@ -96,7 +93,7 @@ def main(): # Optimize the graph and print results params = LevenbergMarquardtParams() - params.setLambdaInitial(1000.0) # Initialize lambda to a high value + params.setlambdaInitial(1000.0) # Initialize lambda to a high value params.setVerbosityLM("SUMMARY") optimizer = LevenbergMarquardtOptimizer(graph, initialEstimate, params) result = optimizer.optimize() From fcf51faf0c835cfa1c3d632e85890bf92befba46 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 26 Oct 2024 16:39:58 -0700 Subject: [PATCH 16/34] Comparison script --- python/gtsam/examples/ViewGraphComparison.py | 259 +++++++++++++++++++ 1 file changed, 259 insertions(+) create mode 100644 python/gtsam/examples/ViewGraphComparison.py diff --git a/python/gtsam/examples/ViewGraphComparison.py b/python/gtsam/examples/ViewGraphComparison.py new file mode 100644 index 000000000..f9c433595 --- /dev/null +++ b/python/gtsam/examples/ViewGraphComparison.py @@ -0,0 +1,259 @@ +""" + Compare the Fundamental Matrix and Essential Matrix methods for optimizing the view-graph. + It measures the distance from the ground truth matrices in terms of the norm of local coordinates (geodesic distance) + on the respective manifolds. It also plots the final error of the optimization. + Author: Frank Dellaert (with heavy assist from ChatGPT) + Date: October 2024 +""" + +import matplotlib.pyplot as plt +import numpy as np +from gtsam.examples import SFMdata + +import gtsam +import argparse +from gtsam import ( + Cal3_S2, + EdgeKey, + EssentialMatrix, + FundamentalMatrix, + LevenbergMarquardtOptimizer, + LevenbergMarquardtParams, + NonlinearFactorGraph, + PinholeCameraCal3_S2, + Values, +) + +# For symbol shorthand (e.g., K(0), K(1)) +K_sym = gtsam.symbol_shorthand.K + +# Methods to compare +methods = ["FundamentalMatrix", "EssentialMatrix"] + + +# Formatter function for printing keys +def formatter(key): + sym = gtsam.Symbol(key) + if sym.chr() == ord("k"): + return f"k{sym.index()}" + else: + edge = EdgeKey(key) + return f"({edge.i()},{edge.j()})" + + +# Function to simulate data +def simulate_data(num_cameras): + # Define the camera calibration parameters + K = Cal3_S2(50.0, 50.0, 0.0, 50.0, 50.0) + + # Create the set of 8 ground-truth landmarks + points = SFMdata.createPoints() + + # Create the set of ground-truth poses + poses = SFMdata.posesOnCircle(num_cameras, 30) + + # Simulate measurements from each camera pose + measurements = [[None for _ in range(len(points))] for _ in range(num_cameras)] + for i in range(num_cameras): + camera = PinholeCameraCal3_S2(poses[i], K) + for j in range(len(points)): + measurements[i][j] = camera.project(points[j]) + + return points, poses, measurements, K + + +# Function to compute ground truth matrices +def compute_ground_truth_matrices(method, poses, K): + if method == "FundamentalMatrix": + F1 = FundamentalMatrix(K, poses[0].between(poses[1]), K) + F2 = FundamentalMatrix(K, poses[0].between(poses[2]), K) + return F1, F2 + elif method == "EssentialMatrix": + E1 = EssentialMatrix.FromPose3(poses[0].between(poses[1])) + E2 = EssentialMatrix.FromPose3(poses[0].between(poses[2])) + return E1, E2 + else: + raise ValueError(f"Unknown method {method}") + + +# Function to build the factor graph +def build_factor_graph(method, num_cameras, measurements): + graph = NonlinearFactorGraph() + + if method == "FundamentalMatrix": + # Use TransferFactorFundamentalMatrix + FactorClass = gtsam.TransferFactorFundamentalMatrix + elif method == "EssentialMatrix": + # Use EssentialTransferFactorCal3_S2 + FactorClass = gtsam.EssentialTransferFactorCal3_S2 + else: + raise ValueError(f"Unknown method {method}") + + for a in range(num_cameras): + b = (a + 1) % num_cameras # Next camera + c = (a + 2) % num_cameras # Camera after next + + # Vectors to collect tuples for each factor + tuples1 = [] + tuples2 = [] + tuples3 = [] + + # Collect data for the three factors + for j in range(len(measurements[0])): + tuples1.append((measurements[a][j], measurements[b][j], measurements[c][j])) + tuples2.append((measurements[a][j], measurements[c][j], measurements[b][j])) + tuples3.append((measurements[c][j], measurements[b][j], measurements[a][j])) + + # Add transfer factors between views a, b, and c. + graph.add(FactorClass(EdgeKey(a, c), EdgeKey(b, c), tuples1)) + graph.add(FactorClass(EdgeKey(a, b), EdgeKey(b, c), tuples2)) + graph.add(FactorClass(EdgeKey(a, c), EdgeKey(a, b), tuples3)) + + return graph + + +# Function to get initial estimates +def get_initial_estimate(method, num_cameras, ground_truth_matrices, K_initial): + initialEstimate = Values() + + if method == "FundamentalMatrix": + F1, F2 = ground_truth_matrices + for a in range(num_cameras): + b = (a + 1) % num_cameras # Next camera + c = (a + 2) % num_cameras # Camera after next + initialEstimate.insert(EdgeKey(a, b).key(), F1) + initialEstimate.insert(EdgeKey(a, c).key(), F2) + elif method == "EssentialMatrix": + E1, E2 = ground_truth_matrices + for a in range(num_cameras): + b = (a + 1) % num_cameras # Next camera + c = (a + 2) % num_cameras # Camera after next + initialEstimate.insert(EdgeKey(a, b).key(), E1) + initialEstimate.insert(EdgeKey(a, c).key(), E2) + # Insert initial calibrations + for i in range(num_cameras): + initialEstimate.insert(K_sym(i), K_initial) + else: + raise ValueError(f"Unknown method {method}") + + return initialEstimate + + +# Function to optimize the graph +def optimize(graph, initialEstimate): + params = LevenbergMarquardtParams() + params.setlambdaInitial(1000.0) # Initialize lambda to a high value + params.setVerbosityLM("SUMMARY") + optimizer = LevenbergMarquardtOptimizer(graph, initialEstimate, params) + result = optimizer.optimize() + return result + + +# Function to compute distances from ground truth +def compute_distances(method, result, ground_truth_matrices, num_cameras): + distances = [] + if method == "FundamentalMatrix": + F1, F2 = ground_truth_matrices + for a in range(num_cameras): + b = (a + 1) % num_cameras + c = (a + 2) % num_cameras + key_ab = EdgeKey(a, b).key() + key_ac = EdgeKey(a, c).key() + F_est_ab = result.atFundamentalMatrix(key_ab) + F_est_ac = result.atFundamentalMatrix(key_ac) + # Compute local coordinates + dist_ab = np.linalg.norm(F1.localCoordinates(F_est_ab)) + dist_ac = np.linalg.norm(F2.localCoordinates(F_est_ac)) + distances.append(dist_ab) + distances.append(dist_ac) + elif method == "EssentialMatrix": + E1, E2 = ground_truth_matrices + for a in range(num_cameras): + b = (a + 1) % num_cameras + c = (a + 2) % num_cameras + key_ab = EdgeKey(a, b).key() + key_ac = EdgeKey(a, c).key() + E_est_ab = result.atEssentialMatrix(key_ab) + E_est_ac = result.atEssentialMatrix(key_ac) + # Compute local coordinates + dist_ab = np.linalg.norm(E1.localCoordinates(E_est_ab)) + dist_ac = np.linalg.norm(E2.localCoordinates(E_est_ac)) + distances.append(dist_ab) + distances.append(dist_ac) + else: + raise ValueError(f"Unknown method {method}") + return distances + + +# Function to plot results +def plot_results(results): + methods = list(results.keys()) + final_errors = [results[method]["final_error"] for method in methods] + distances = [np.mean(results[method]["distances"]) for method in methods] + + fig, ax1 = plt.subplots() + + color = "tab:red" + ax1.set_xlabel("Method") + ax1.set_ylabel("Final Error", color=color) + ax1.bar(methods, final_errors, color=color, alpha=0.6) + ax1.tick_params(axis="y", labelcolor=color) + + ax2 = ax1.twinx() + color = "tab:blue" + ax2.set_ylabel("Mean Geodesic Distance", color=color) + ax2.plot(methods, distances, color=color, marker="o") + ax2.tick_params(axis="y", labelcolor=color) + + plt.title("Comparison of Methods") + fig.tight_layout() + plt.show() + + +# Main function +def main(): + # Parse command line arguments + parser = argparse.ArgumentParser(description="Compare Fundamental and Essential Matrix Methods") + parser.add_argument("--num_cameras", type=int, default=4, help="Number of cameras (default: 4)") + args = parser.parse_args() + + # Initialize results dictionary + results = {} + + for method in methods: + print(f"Running method: {method}") + + # Simulate data + points, poses, measurements, K_initial = simulate_data(args.num_cameras) + + # Compute ground truth matrices + ground_truth_matrices = compute_ground_truth_matrices(method, poses, K_initial) + + # Build the factor graph + graph = build_factor_graph(method, args.num_cameras, measurements) + + # Get initial estimates + initialEstimate = get_initial_estimate(method, args.num_cameras, ground_truth_matrices, K_initial) + + # Optimize the graph + result = optimize(graph, initialEstimate) + + # Compute distances from ground truth + distances = compute_distances(method, result, ground_truth_matrices, args.num_cameras) + + # Compute final error + final_error = graph.error(result) + + # Store results + results[method] = {"distances": distances, "final_error": final_error} + + print(f"Method: {method}") + print(f"Final Error: {final_error}") + print(f"Mean Geodesic Distance: {np.mean(distances)}\n") + + # Plot results + plot_results(results) + + +if __name__ == "__main__": + main() From 5ce728ab46266f1bad3de90738d81428f73b895c Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 26 Oct 2024 16:55:48 -0700 Subject: [PATCH 17/34] Compare in F-manifold --- python/gtsam/examples/ViewGraphComparison.py | 77 +++++++++++--------- 1 file changed, 41 insertions(+), 36 deletions(-) diff --git a/python/gtsam/examples/ViewGraphComparison.py b/python/gtsam/examples/ViewGraphComparison.py index f9c433595..fdaf02861 100644 --- a/python/gtsam/examples/ViewGraphComparison.py +++ b/python/gtsam/examples/ViewGraphComparison.py @@ -1,7 +1,7 @@ """ Compare the Fundamental Matrix and Essential Matrix methods for optimizing the view-graph. It measures the distance from the ground truth matrices in terms of the norm of local coordinates (geodesic distance) - on the respective manifolds. It also plots the final error of the optimization. + on the F-manifold. It also plots the final error of the optimization. Author: Frank Dellaert (with heavy assist from ChatGPT) Date: October 2024 """ @@ -113,18 +113,18 @@ def build_factor_graph(method, num_cameras, measurements): # Function to get initial estimates -def get_initial_estimate(method, num_cameras, ground_truth_matrices, K_initial): +def get_initial_estimate(method, num_cameras, ground_truth, K): initialEstimate = Values() if method == "FundamentalMatrix": - F1, F2 = ground_truth_matrices + F1, F2 = ground_truth for a in range(num_cameras): b = (a + 1) % num_cameras # Next camera c = (a + 2) % num_cameras # Camera after next initialEstimate.insert(EdgeKey(a, b).key(), F1) initialEstimate.insert(EdgeKey(a, c).key(), F2) elif method == "EssentialMatrix": - E1, E2 = ground_truth_matrices + E1, E2 = ground_truth for a in range(num_cameras): b = (a + 1) % num_cameras # Next camera c = (a + 2) % num_cameras # Camera after next @@ -132,7 +132,7 @@ def get_initial_estimate(method, num_cameras, ground_truth_matrices, K_initial): initialEstimate.insert(EdgeKey(a, c).key(), E2) # Insert initial calibrations for i in range(num_cameras): - initialEstimate.insert(K_sym(i), K_initial) + initialEstimate.insert(K_sym(i), K) else: raise ValueError(f"Unknown method {method}") @@ -150,38 +150,43 @@ def optimize(graph, initialEstimate): # Function to compute distances from ground truth -def compute_distances(method, result, ground_truth_matrices, num_cameras): +def compute_distances(method, result, ground_truth, num_cameras, K): distances = [] + if method == "FundamentalMatrix": - F1, F2 = ground_truth_matrices - for a in range(num_cameras): - b = (a + 1) % num_cameras - c = (a + 2) % num_cameras - key_ab = EdgeKey(a, b).key() - key_ac = EdgeKey(a, c).key() - F_est_ab = result.atFundamentalMatrix(key_ab) - F_est_ac = result.atFundamentalMatrix(key_ac) - # Compute local coordinates - dist_ab = np.linalg.norm(F1.localCoordinates(F_est_ab)) - dist_ac = np.linalg.norm(F2.localCoordinates(F_est_ac)) - distances.append(dist_ab) - distances.append(dist_ac) + F1, F2 = ground_truth elif method == "EssentialMatrix": - E1, E2 = ground_truth_matrices - for a in range(num_cameras): - b = (a + 1) % num_cameras - c = (a + 2) % num_cameras - key_ab = EdgeKey(a, b).key() - key_ac = EdgeKey(a, c).key() - E_est_ab = result.atEssentialMatrix(key_ab) - E_est_ac = result.atEssentialMatrix(key_ac) - # Compute local coordinates - dist_ab = np.linalg.norm(E1.localCoordinates(E_est_ab)) - dist_ac = np.linalg.norm(E2.localCoordinates(E_est_ac)) - distances.append(dist_ab) - distances.append(dist_ac) + E1, E2 = ground_truth + # Convert ground truth EssentialMatrices to FundamentalMatrices using GTSAM method + F1 = gtsam.FundamentalMatrix(K, E1, K) + F2 = gtsam.FundamentalMatrix(K, E2, K) else: raise ValueError(f"Unknown method {method}") + + for a in range(num_cameras): + b = (a + 1) % num_cameras + c = (a + 2) % num_cameras + key_ab = EdgeKey(a, b).key() + key_ac = EdgeKey(a, c).key() + + if method == "FundamentalMatrix": + F_est_ab = result.atFundamentalMatrix(key_ab) + F_est_ac = result.atFundamentalMatrix(key_ac) + elif method == "EssentialMatrix": + E_est_ab = result.atEssentialMatrix(key_ab) + E_est_ac = result.atEssentialMatrix(key_ac) + # Convert estimated EssentialMatrices to FundamentalMatrices using GTSAM method + F_est_ab = gtsam.FundamentalMatrix(K, E_est_ab, K) + F_est_ac = gtsam.FundamentalMatrix(K, E_est_ac, K) + else: + raise ValueError(f"Unknown method {method}") + + # Compute local coordinates (geodesic distance on the F-manifold) + dist_ab = np.linalg.norm(F1.localCoordinates(F_est_ab)) + dist_ac = np.linalg.norm(F2.localCoordinates(F_est_ac)) + distances.append(dist_ab) + distances.append(dist_ac) + return distances @@ -224,22 +229,22 @@ def main(): print(f"Running method: {method}") # Simulate data - points, poses, measurements, K_initial = simulate_data(args.num_cameras) + points, poses, measurements, K = simulate_data(args.num_cameras) # Compute ground truth matrices - ground_truth_matrices = compute_ground_truth_matrices(method, poses, K_initial) + ground_truth = compute_ground_truth_matrices(method, poses, K) # Build the factor graph graph = build_factor_graph(method, args.num_cameras, measurements) # Get initial estimates - initialEstimate = get_initial_estimate(method, args.num_cameras, ground_truth_matrices, K_initial) + initialEstimate = get_initial_estimate(method, args.num_cameras, ground_truth, K) # Optimize the graph result = optimize(graph, initialEstimate) # Compute distances from ground truth - distances = compute_distances(method, result, ground_truth_matrices, args.num_cameras) + distances = compute_distances(method, result, ground_truth, args.num_cameras, K) # Compute final error final_error = graph.error(result) From 3cc6ebedd2c48a69966b89c80d269257113e92a1 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 26 Oct 2024 23:39:19 -0700 Subject: [PATCH 18/34] Order matters with inheritance --- gtsam/nonlinear/values.i | 61 ++++++++++++++++------------------------ 1 file changed, 24 insertions(+), 37 deletions(-) diff --git a/gtsam/nonlinear/values.i b/gtsam/nonlinear/values.i index a75fe5ae1..99548d8ec 100644 --- a/gtsam/nonlinear/values.i +++ b/gtsam/nonlinear/values.i @@ -68,7 +68,7 @@ class Values { // gtsam::Value at(size_t j) const; // The order is important: gtsam::Vector has to precede Point2/Point3 so `atVector` - // can work for those fixed-size vectors. + // can work for those fixed-size vectors. Same apparently for Cal3Bundler and Cal3f. void insert(size_t j, gtsam::Vector vector); void insert(size_t j, gtsam::Matrix matrix); void insert(size_t j, const gtsam::Point2& point2); @@ -81,25 +81,25 @@ class Values { void insert(size_t j, const gtsam::Rot3& rot3); void insert(size_t j, const gtsam::Pose3& pose3); void insert(size_t j, const gtsam::Unit3& unit3); + void insert(size_t j, const gtsam::Cal3Bundler& cal3bundler); void insert(size_t j, const gtsam::Cal3f& cal3f); void insert(size_t j, const gtsam::Cal3_S2& cal3_s2); void insert(size_t j, const gtsam::Cal3DS2& cal3ds2); - void insert(size_t j, const gtsam::Cal3Bundler& cal3bundler); void insert(size_t j, const gtsam::Cal3Fisheye& cal3fisheye); void insert(size_t j, const gtsam::Cal3Unified& cal3unified); void insert(size_t j, const gtsam::EssentialMatrix& E); void insert(size_t j, const gtsam::FundamentalMatrix& F); void insert(size_t j, const gtsam::SimpleFundamentalMatrix& F); + void insert(size_t j, const gtsam::PinholeCamera& camera); void insert(size_t j, const gtsam::PinholeCamera& camera); void insert(size_t j, const gtsam::PinholeCamera& camera); void insert(size_t j, const gtsam::PinholeCamera& camera); - void insert(size_t j, const gtsam::PinholeCamera& camera); void insert(size_t j, const gtsam::PinholeCamera& camera); void insert(size_t j, const gtsam::PinholeCamera& camera); + void insert(size_t j, const gtsam::PinholePose& camera); void insert(size_t j, const gtsam::PinholePose& camera); void insert(size_t j, const gtsam::PinholePose& camera); void insert(size_t j, const gtsam::PinholePose& camera); - void insert(size_t j, const gtsam::PinholePose& camera); void insert(size_t j, const gtsam::PinholePose& camera); void insert(size_t j, const gtsam::PinholePose& camera); void insert(size_t j, const gtsam::imuBias::ConstantBias& constant_bias); @@ -123,25 +123,25 @@ class Values { void update(size_t j, const gtsam::Rot3& rot3); void update(size_t j, const gtsam::Pose3& pose3); void update(size_t j, const gtsam::Unit3& unit3); + void update(size_t j, const gtsam::Cal3Bundler& cal3bundler); void update(size_t j, const gtsam::Cal3f& cal3f); void update(size_t j, const gtsam::Cal3_S2& cal3_s2); void update(size_t j, const gtsam::Cal3DS2& cal3ds2); - void update(size_t j, const gtsam::Cal3Bundler& cal3bundler); void update(size_t j, const gtsam::Cal3Fisheye& cal3fisheye); void update(size_t j, const gtsam::Cal3Unified& cal3unified); void update(size_t j, const gtsam::EssentialMatrix& E); void update(size_t j, const gtsam::FundamentalMatrix& F); void update(size_t j, const gtsam::SimpleFundamentalMatrix& F); + void update(size_t j, const gtsam::PinholeCamera& camera); void update(size_t j, const gtsam::PinholeCamera& camera); void update(size_t j, const gtsam::PinholeCamera& camera); void update(size_t j, const gtsam::PinholeCamera& camera); - void update(size_t j, const gtsam::PinholeCamera& camera); void update(size_t j, const gtsam::PinholeCamera& camera); void update(size_t j, const gtsam::PinholeCamera& camera); + void update(size_t j, const gtsam::PinholePose& camera); void update(size_t j, const gtsam::PinholePose& camera); void update(size_t j, const gtsam::PinholePose& camera); void update(size_t j, const gtsam::PinholePose& camera); - void update(size_t j, const gtsam::PinholePose& camera); void update(size_t j, const gtsam::PinholePose& camera); void update(size_t j, const gtsam::PinholePose& camera); void update(size_t j, const gtsam::imuBias::ConstantBias& constant_bias); @@ -162,41 +162,28 @@ class Values { void insert_or_assign(size_t j, const gtsam::Rot3& rot3); void insert_or_assign(size_t j, const gtsam::Pose3& pose3); void insert_or_assign(size_t j, const gtsam::Unit3& unit3); + void insert_or_assign(size_t j, const gtsam::Cal3Bundler& cal3bundler); void insert_or_assign(size_t j, const gtsam::Cal3f& cal3f); void insert_or_assign(size_t j, const gtsam::Cal3_S2& cal3_s2); void insert_or_assign(size_t j, const gtsam::Cal3DS2& cal3ds2); - void insert_or_assign(size_t j, const gtsam::Cal3Bundler& cal3bundler); void insert_or_assign(size_t j, const gtsam::Cal3Fisheye& cal3fisheye); void insert_or_assign(size_t j, const gtsam::Cal3Unified& cal3unified); void insert_or_assign(size_t j, const gtsam::EssentialMatrix& E); void insert_or_assign(size_t j, const gtsam::FundamentalMatrix& F); void insert_or_assign(size_t j, const gtsam::SimpleFundamentalMatrix& F); - void insert_or_assign(size_t j, - const gtsam::PinholeCamera& camera); - void insert_or_assign(size_t j, - const gtsam::PinholeCamera& camera); - void insert_or_assign(size_t j, - const gtsam::PinholeCamera& camera); - void insert_or_assign(size_t j, - const gtsam::PinholeCamera& camera); - void insert_or_assign(size_t j, - const gtsam::PinholeCamera& camera); - void insert_or_assign(size_t j, - const gtsam::PinholeCamera& camera); - void insert_or_assign(size_t j, - const gtsam::PinholePose& camera); - void insert_or_assign(size_t j, - const gtsam::PinholePose& camera); - void insert_or_assign(size_t j, - const gtsam::PinholePose& camera); - void insert_or_assign(size_t j, - const gtsam::PinholePose& camera); - void insert_or_assign(size_t j, - const gtsam::PinholePose& camera); - void insert_or_assign(size_t j, - const gtsam::PinholePose& camera); - void insert_or_assign(size_t j, - const gtsam::imuBias::ConstantBias& constant_bias); + void insert_or_assign(size_t j, const gtsam::PinholeCamera& camera); + void insert_or_assign(size_t j, const gtsam::PinholeCamera& camera); + void insert_or_assign(size_t j, const gtsam::PinholeCamera& camera); + void insert_or_assign(size_t j, const gtsam::PinholeCamera& camera); + void insert_or_assign(size_t j, const gtsam::PinholeCamera& camera); + void insert_or_assign(size_t j, const gtsam::PinholeCamera& camera); + void insert_or_assign(size_t j, const gtsam::PinholePose& camera); + void insert_or_assign(size_t j, const gtsam::PinholePose& camera); + void insert_or_assign(size_t j, const gtsam::PinholePose& camera); + void insert_or_assign(size_t j, const gtsam::PinholePose& camera); + void insert_or_assign(size_t j, const gtsam::PinholePose& camera); + void insert_or_assign(size_t j, const gtsam::PinholePose& camera); + void insert_or_assign(size_t j, const gtsam::imuBias::ConstantBias& constant_bias); void insert_or_assign(size_t j, const gtsam::NavState& nav_state); void insert_or_assign(size_t j, double c); @@ -210,25 +197,25 @@ class Values { gtsam::Rot3, gtsam::Pose3, gtsam::Unit3, + gtsam::Cal3Bundler, gtsam::Cal3f, gtsam::Cal3_S2, gtsam::Cal3DS2, - gtsam::Cal3Bundler, gtsam::Cal3Fisheye, gtsam::Cal3Unified, gtsam::EssentialMatrix, gtsam::FundamentalMatrix, gtsam::SimpleFundamentalMatrix, + gtsam::PinholeCamera, gtsam::PinholeCamera, gtsam::PinholeCamera, gtsam::PinholeCamera, - gtsam::PinholeCamera, gtsam::PinholeCamera, gtsam::PinholeCamera, + gtsam::PinholePose, gtsam::PinholePose, gtsam::PinholePose, gtsam::PinholePose, - gtsam::PinholePose, gtsam::PinholePose, gtsam::PinholePose, gtsam::imuBias::ConstantBias, From 0477d206951bdcb5a10f292f8a8c4748bbe641d9 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 26 Oct 2024 23:39:28 -0700 Subject: [PATCH 19/34] Fix export --- gtsam/geometry/Cal3f.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/geometry/Cal3f.h b/gtsam/geometry/Cal3f.h index e30da688c..f053f3d11 100644 --- a/gtsam/geometry/Cal3f.h +++ b/gtsam/geometry/Cal3f.h @@ -58,7 +58,7 @@ class GTSAM_EXPORT Cal3f : public Cal3 { /// @{ /// Output stream operator - friend std::ostream& operator<<(std::ostream& os, const Cal3f& cal); + GTSAM_EXPORT friend std::ostream& operator<<(std::ostream& os, const Cal3f& cal); /// print with optional string void print(const std::string& s = "") const override; From b9dc9ae4594bd1b80feb39f14e83fbcd117bade2 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 26 Oct 2024 23:53:20 -0700 Subject: [PATCH 20/34] Switch to using Cal3f --- examples/EssentialViewGraphExample.cpp | 10 ++-- examples/ViewGraphExample.cpp | 2 +- .../examples/EssentialViewGraphExample.py | 12 ++-- python/gtsam/examples/ViewGraphComparison.py | 57 ++++++++----------- python/gtsam/examples/ViewGraphExample.py | 8 +-- 5 files changed, 41 insertions(+), 48 deletions(-) diff --git a/examples/EssentialViewGraphExample.cpp b/examples/EssentialViewGraphExample.cpp index af254a94d..e21a36e16 100644 --- a/examples/EssentialViewGraphExample.cpp +++ b/examples/EssentialViewGraphExample.cpp @@ -16,7 +16,7 @@ * @date October 2024 */ -#include +#include #include #include #include @@ -40,7 +40,7 @@ using namespace symbol_shorthand; // For K(symbol) // Main function int main(int argc, char* argv[]) { // Define the camera calibration parameters - Cal3_S2 K_initial(50.0, 50.0, 0.0, 50.0, 50.0); + Cal3f cal(50.0, 50.0, 50.0); // Create the set of 8 ground-truth landmarks vector points = createPoints(); @@ -55,7 +55,7 @@ int main(int argc, char* argv[]) { // Simulate measurements from each camera pose std::array, 4> p; for (size_t i = 0; i < 4; ++i) { - PinholeCamera camera(poses[i], K_initial); + PinholeCamera camera(poses[i], cal); for (size_t j = 0; j < 8; ++j) { p[i][j] = camera.project(points[j]); } @@ -63,7 +63,7 @@ int main(int argc, char* argv[]) { // Create the factor graph NonlinearFactorGraph graph; - using Factor = EssentialTransferFactor; + using Factor = EssentialTransferFactor; for (size_t a = 0; a < 4; ++a) { size_t b = (a + 1) % 4; // Next camera @@ -113,7 +113,7 @@ int main(int argc, char* argv[]) { // Insert initial calibrations (using K symbol) for (size_t i = 0; i < 4; ++i) { - initialEstimate.insert(K(i), K_initial); + initialEstimate.insert(K(i), cal); } initialEstimate.print("Initial Estimates:\n", formatter); diff --git a/examples/ViewGraphExample.cpp b/examples/ViewGraphExample.cpp index 23393fa20..a33e1853d 100644 --- a/examples/ViewGraphExample.cpp +++ b/examples/ViewGraphExample.cpp @@ -114,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/python/gtsam/examples/EssentialViewGraphExample.py b/python/gtsam/examples/EssentialViewGraphExample.py index d7d2cda38..184d3f7e3 100644 --- a/python/gtsam/examples/EssentialViewGraphExample.py +++ b/python/gtsam/examples/EssentialViewGraphExample.py @@ -18,10 +18,10 @@ import numpy as np from gtsam.examples import SFMdata import gtsam -from gtsam import Cal3_S2, EdgeKey, EssentialMatrix -from gtsam import EssentialTransferFactorCal3_S2 as Factor +from gtsam import Cal3f, EdgeKey, EssentialMatrix +from gtsam import EssentialTransferFactorCal3f as Factor from gtsam import (LevenbergMarquardtOptimizer, LevenbergMarquardtParams, - NonlinearFactorGraph, PinholeCameraCal3_S2, Values) + NonlinearFactorGraph, PinholeCameraCal3f, Values) # For symbol shorthand (e.g., X(0), L(1)) K = gtsam.symbol_shorthand.K @@ -39,7 +39,7 @@ def formatter(key): def main(): # Define the camera calibration parameters - K_initial = Cal3_S2(50.0, 50.0, 0.0, 50.0, 50.0) + cal = Cal3f(50.0, 50.0, 50.0) # Create the set of 8 ground-truth landmarks points = SFMdata.createPoints() @@ -54,7 +54,7 @@ def main(): # Simulate measurements from each camera pose p = [[None for _ in range(8)] for _ in range(4)] for i in range(4): - camera = PinholeCameraCal3_S2(poses[i], K_initial) + camera = PinholeCameraCal3f(poses[i], cal) for j in range(8): p[i][j] = camera.project(points[j]) @@ -95,7 +95,7 @@ def main(): # Insert initial calibrations for i in range(4): - initialEstimate.insert(K(i), K_initial) + initialEstimate.insert(K(i), cal) # Optimize the graph and print results params = LevenbergMarquardtParams() diff --git a/python/gtsam/examples/ViewGraphComparison.py b/python/gtsam/examples/ViewGraphComparison.py index fdaf02861..dd9a3f329 100644 --- a/python/gtsam/examples/ViewGraphComparison.py +++ b/python/gtsam/examples/ViewGraphComparison.py @@ -6,26 +6,19 @@ Date: October 2024 """ +import argparse + import matplotlib.pyplot as plt import numpy as np from gtsam.examples import SFMdata import gtsam -import argparse -from gtsam import ( - Cal3_S2, - EdgeKey, - EssentialMatrix, - FundamentalMatrix, - LevenbergMarquardtOptimizer, - LevenbergMarquardtParams, - NonlinearFactorGraph, - PinholeCameraCal3_S2, - Values, -) +from gtsam import (Cal3f, EdgeKey, EssentialMatrix, FundamentalMatrix, + LevenbergMarquardtOptimizer, LevenbergMarquardtParams, + NonlinearFactorGraph, PinholeCameraCal3f, Values) # For symbol shorthand (e.g., K(0), K(1)) -K_sym = gtsam.symbol_shorthand.K +K = gtsam.symbol_shorthand.K # Methods to compare methods = ["FundamentalMatrix", "EssentialMatrix"] @@ -44,7 +37,7 @@ def formatter(key): # Function to simulate data def simulate_data(num_cameras): # Define the camera calibration parameters - K = Cal3_S2(50.0, 50.0, 0.0, 50.0, 50.0) + cal = Cal3f(50.0, 50.0, 50.0) # Create the set of 8 ground-truth landmarks points = SFMdata.createPoints() @@ -55,18 +48,18 @@ def simulate_data(num_cameras): # Simulate measurements from each camera pose measurements = [[None for _ in range(len(points))] for _ in range(num_cameras)] for i in range(num_cameras): - camera = PinholeCameraCal3_S2(poses[i], K) + camera = PinholeCameraCal3f(poses[i], cal) for j in range(len(points)): measurements[i][j] = camera.project(points[j]) - return points, poses, measurements, K + return points, poses, measurements, cal # Function to compute ground truth matrices -def compute_ground_truth_matrices(method, poses, K): +def compute_ground_truth(method, poses, cal): if method == "FundamentalMatrix": - F1 = FundamentalMatrix(K, poses[0].between(poses[1]), K) - F2 = FundamentalMatrix(K, poses[0].between(poses[2]), K) + F1 = FundamentalMatrix(cal.K(), poses[0].between(poses[1]), cal.K()) + F2 = FundamentalMatrix(cal.K(), poses[0].between(poses[2]), cal.K()) return F1, F2 elif method == "EssentialMatrix": E1 = EssentialMatrix.FromPose3(poses[0].between(poses[1])) @@ -84,8 +77,8 @@ def build_factor_graph(method, num_cameras, measurements): # Use TransferFactorFundamentalMatrix FactorClass = gtsam.TransferFactorFundamentalMatrix elif method == "EssentialMatrix": - # Use EssentialTransferFactorCal3_S2 - FactorClass = gtsam.EssentialTransferFactorCal3_S2 + # Use EssentialTransferFactorCal3f + FactorClass = gtsam.EssentialTransferFactorCal3f else: raise ValueError(f"Unknown method {method}") @@ -113,7 +106,7 @@ def build_factor_graph(method, num_cameras, measurements): # Function to get initial estimates -def get_initial_estimate(method, num_cameras, ground_truth, K): +def get_initial_estimate(method, num_cameras, ground_truth, cal): initialEstimate = Values() if method == "FundamentalMatrix": @@ -132,7 +125,7 @@ def get_initial_estimate(method, num_cameras, ground_truth, K): initialEstimate.insert(EdgeKey(a, c).key(), E2) # Insert initial calibrations for i in range(num_cameras): - initialEstimate.insert(K_sym(i), K) + initialEstimate.insert(K(i), cal) else: raise ValueError(f"Unknown method {method}") @@ -150,7 +143,7 @@ def optimize(graph, initialEstimate): # Function to compute distances from ground truth -def compute_distances(method, result, ground_truth, num_cameras, K): +def compute_distances(method, result, ground_truth, num_cameras, cal): distances = [] if method == "FundamentalMatrix": @@ -158,8 +151,8 @@ def compute_distances(method, result, ground_truth, num_cameras, K): elif method == "EssentialMatrix": E1, E2 = ground_truth # Convert ground truth EssentialMatrices to FundamentalMatrices using GTSAM method - F1 = gtsam.FundamentalMatrix(K, E1, K) - F2 = gtsam.FundamentalMatrix(K, E2, K) + F1 = gtsam.FundamentalMatrix(cal.K(), E1, cal.K()) + F2 = gtsam.FundamentalMatrix(cal.K(), E2, cal.K()) else: raise ValueError(f"Unknown method {method}") @@ -176,8 +169,8 @@ def compute_distances(method, result, ground_truth, num_cameras, K): E_est_ab = result.atEssentialMatrix(key_ab) E_est_ac = result.atEssentialMatrix(key_ac) # Convert estimated EssentialMatrices to FundamentalMatrices using GTSAM method - F_est_ab = gtsam.FundamentalMatrix(K, E_est_ab, K) - F_est_ac = gtsam.FundamentalMatrix(K, E_est_ac, K) + F_est_ab = gtsam.FundamentalMatrix(cal.K(), E_est_ab, cal.K()) + F_est_ac = gtsam.FundamentalMatrix(cal.K(), E_est_ac, cal.K()) else: raise ValueError(f"Unknown method {method}") @@ -229,22 +222,22 @@ def main(): print(f"Running method: {method}") # Simulate data - points, poses, measurements, K = simulate_data(args.num_cameras) + points, poses, measurements, cal = simulate_data(args.num_cameras) # Compute ground truth matrices - ground_truth = compute_ground_truth_matrices(method, poses, K) + ground_truth = compute_ground_truth(method, poses, cal) # Build the factor graph graph = build_factor_graph(method, args.num_cameras, measurements) # Get initial estimates - initialEstimate = get_initial_estimate(method, args.num_cameras, ground_truth, K) + initialEstimate = get_initial_estimate(method, args.num_cameras, ground_truth, cal) # Optimize the graph result = optimize(graph, initialEstimate) # Compute distances from ground truth - distances = compute_distances(method, result, ground_truth, args.num_cameras, K) + distances = compute_distances(method, result, ground_truth, args.num_cameras, cal) # Compute final error final_error = graph.error(result) diff --git a/python/gtsam/examples/ViewGraphExample.py b/python/gtsam/examples/ViewGraphExample.py index 0ba41762e..2dc41ec4b 100644 --- a/python/gtsam/examples/ViewGraphExample.py +++ b/python/gtsam/examples/ViewGraphExample.py @@ -32,7 +32,7 @@ def formatter(key): def main(): # Define the camera calibration parameters - K = Cal3_S2(50.0, 50.0, 0.0, 50.0, 50.0) + cal = Cal3_S2(50.0, 50.0, 0.0, 50.0, 50.0) # Create the set of 8 ground-truth landmarks points = SFMdata.createPoints() @@ -41,13 +41,13 @@ def main(): poses = SFMdata.posesOnCircle(4, 30) # Calculate ground truth fundamental matrices, 1 and 2 poses apart - F1 = FundamentalMatrix(K, poses[0].between(poses[1]), K) - F2 = FundamentalMatrix(K, poses[0].between(poses[2]), K) + F1 = FundamentalMatrix(cal.K(), poses[0].between(poses[1]), cal.K()) + F2 = FundamentalMatrix(cal.K(), poses[0].between(poses[2]), cal.K()) # Simulate measurements from each camera pose p = [[None for _ in range(8)] for _ in range(4)] for i in range(4): - camera = PinholeCameraCal3_S2(poses[i], K) + camera = PinholeCameraCal3_S2(poses[i], cal) for j in range(8): p[i][j] = camera.project(points[j]) From 77dfa446a9bcbdda6e3787989d12d7a814a4cb32 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 27 Oct 2024 00:34:40 -0700 Subject: [PATCH 21/34] Run many trials --- python/gtsam/examples/ViewGraphComparison.py | 112 ++++++++++++------- 1 file changed, 70 insertions(+), 42 deletions(-) diff --git a/python/gtsam/examples/ViewGraphComparison.py b/python/gtsam/examples/ViewGraphComparison.py index dd9a3f329..11048ccca 100644 --- a/python/gtsam/examples/ViewGraphComparison.py +++ b/python/gtsam/examples/ViewGraphComparison.py @@ -34,8 +34,8 @@ def formatter(key): return f"({edge.i()},{edge.j()})" -# Function to simulate data -def simulate_data(num_cameras): +def simulate_geometry(num_cameras): + """simulate geometry (points and poses)""" # Define the camera calibration parameters cal = Cal3f(50.0, 50.0, 50.0) @@ -45,14 +45,20 @@ def simulate_data(num_cameras): # Create the set of ground-truth poses poses = SFMdata.posesOnCircle(num_cameras, 30) - # Simulate measurements from each camera pose - measurements = [[None for _ in range(len(points))] for _ in range(num_cameras)] - for i in range(num_cameras): - camera = PinholeCameraCal3f(poses[i], cal) - for j in range(len(points)): - measurements[i][j] = camera.project(points[j]) + return points, poses, cal - return points, poses, measurements, cal + +def simulate_data(points, poses, cal, rng, noise_std): + """Simulate measurements from each camera pose""" + measurements = [[None for _ in points] for _ in poses] + for i, pose in enumerate(poses): + camera = PinholeCameraCal3f(pose, cal) + for j, point in enumerate(points): + projection = camera.project(point) + noise = rng.normal(0, noise_std, size=2) + measurements[i][j] = projection + noise + + return measurements # Function to compute ground truth matrices @@ -69,15 +75,13 @@ def compute_ground_truth(method, poses, cal): raise ValueError(f"Unknown method {method}") -# Function to build the factor graph def build_factor_graph(method, num_cameras, measurements): + """build the factor graph""" graph = NonlinearFactorGraph() if method == "FundamentalMatrix": - # Use TransferFactorFundamentalMatrix FactorClass = gtsam.TransferFactorFundamentalMatrix elif method == "EssentialMatrix": - # Use EssentialTransferFactorCal3f FactorClass = gtsam.EssentialTransferFactorCal3f else: raise ValueError(f"Unknown method {method}") @@ -105,9 +109,10 @@ def build_factor_graph(method, num_cameras, measurements): return graph -# Function to get initial estimates def get_initial_estimate(method, num_cameras, ground_truth, cal): + """get initial estimate for method""" initialEstimate = Values() + total_dimension = 0 if method == "FundamentalMatrix": F1, F2 = ground_truth @@ -116,6 +121,7 @@ def get_initial_estimate(method, num_cameras, ground_truth, cal): c = (a + 2) % num_cameras # Camera after next initialEstimate.insert(EdgeKey(a, b).key(), F1) initialEstimate.insert(EdgeKey(a, c).key(), F2) + total_dimension += F1.dim() + F2.dim() elif method == "EssentialMatrix": E1, E2 = ground_truth for a in range(num_cameras): @@ -123,34 +129,38 @@ def get_initial_estimate(method, num_cameras, ground_truth, cal): c = (a + 2) % num_cameras # Camera after next initialEstimate.insert(EdgeKey(a, b).key(), E1) initialEstimate.insert(EdgeKey(a, c).key(), E2) + total_dimension += E1.dim() + E2.dim() # Insert initial calibrations for i in range(num_cameras): initialEstimate.insert(K(i), cal) + total_dimension += cal.dim() else: raise ValueError(f"Unknown method {method}") + print(f"Total dimension of the problem: {total_dimension}") return initialEstimate -# Function to optimize the graph def optimize(graph, initialEstimate): + """optimize the graph""" params = LevenbergMarquardtParams() params.setlambdaInitial(1000.0) # Initialize lambda to a high value + params.setAbsoluteErrorTol(1.0) params.setVerbosityLM("SUMMARY") optimizer = LevenbergMarquardtOptimizer(graph, initialEstimate, params) result = optimizer.optimize() return result -# Function to compute distances from ground truth def compute_distances(method, result, ground_truth, num_cameras, cal): + """Compute geodesic distances from ground truth""" distances = [] if method == "FundamentalMatrix": F1, F2 = ground_truth elif method == "EssentialMatrix": E1, E2 = ground_truth - # Convert ground truth EssentialMatrices to FundamentalMatrices using GTSAM method + # Convert ground truth EssentialMatrices to FundamentalMatrices F1 = gtsam.FundamentalMatrix(cal.K(), E1, cal.K()) F2 = gtsam.FundamentalMatrix(cal.K(), E2, cal.K()) else: @@ -168,11 +178,9 @@ def compute_distances(method, result, ground_truth, num_cameras, cal): elif method == "EssentialMatrix": E_est_ab = result.atEssentialMatrix(key_ab) E_est_ac = result.atEssentialMatrix(key_ac) - # Convert estimated EssentialMatrices to FundamentalMatrices using GTSAM method + # Convert estimated EssentialMatrices to FundamentalMatrices F_est_ab = gtsam.FundamentalMatrix(cal.K(), E_est_ab, cal.K()) F_est_ac = gtsam.FundamentalMatrix(cal.K(), E_est_ac, cal.K()) - else: - raise ValueError(f"Unknown method {method}") # Compute local coordinates (geodesic distance on the F-manifold) dist_ab = np.linalg.norm(F1.localCoordinates(F_est_ab)) @@ -183,8 +191,8 @@ def compute_distances(method, result, ground_truth, num_cameras, cal): return distances -# Function to plot results def plot_results(results): + """plot results""" methods = list(results.keys()) final_errors = [results[method]["final_error"] for method in methods] distances = [np.mean(results[method]["distances"]) for method in methods] @@ -213,41 +221,61 @@ def main(): # Parse command line arguments parser = argparse.ArgumentParser(description="Compare Fundamental and Essential Matrix Methods") parser.add_argument("--num_cameras", type=int, default=4, help="Number of cameras (default: 4)") + parser.add_argument("--nr_trials", type=int, default=5, help="Number of trials (default: 5)") + parser.add_argument("--seed", type=int, default=42, help="Random seed (default: 42)") + parser.add_argument("--noise_std", type=float, default=1.0, help="Standard deviation of noise (default: 1.0)") args = parser.parse_args() - # Initialize results dictionary - results = {} + # Initialize the random number generator + rng = np.random.default_rng(seed=args.seed) - for method in methods: - print(f"Running method: {method}") + # Initialize results dictionary + results = {method: {"distances": [], "final_error": []} for method in methods} + + # Simulate geometry + points, poses, cal = simulate_geometry(args.num_cameras) + + # Compute ground truth matrices + ground_truth = {method: compute_ground_truth(method, poses, cal) for method in methods} + + # Get initial estimates + initial_estimate = { + method: get_initial_estimate(method, args.num_cameras, ground_truth[method], cal) for method in methods + } + + for trial in range(args.nr_trials): + print(f"\nTrial {trial + 1}/{args.nr_trials}") # Simulate data - points, poses, measurements, cal = simulate_data(args.num_cameras) + measurements = simulate_data(points, poses, cal, rng, args.noise_std) - # Compute ground truth matrices - ground_truth = compute_ground_truth(method, poses, cal) + for method in methods: + print(f"\nRunning method: {method}") - # Build the factor graph - graph = build_factor_graph(method, args.num_cameras, measurements) + # Build the factor graph + graph = build_factor_graph(method, args.num_cameras, measurements) - # Get initial estimates - initialEstimate = get_initial_estimate(method, args.num_cameras, ground_truth, cal) + # Optimize the graph + result = optimize(graph, initial_estimate[method]) - # Optimize the graph - result = optimize(graph, initialEstimate) + # Compute distances from ground truth + distances = compute_distances(method, result, ground_truth[method], args.num_cameras, cal) - # Compute distances from ground truth - distances = compute_distances(method, result, ground_truth, args.num_cameras, cal) + # Compute final error + final_error = graph.error(result) - # Compute final error - final_error = graph.error(result) + # Store results + results[method]["distances"].extend(distances) + results[method]["final_error"].append(final_error) - # Store results - results[method] = {"distances": distances, "final_error": final_error} + print(f"Method: {method}") + print(f"Final Error: {final_error:.3f}") + print(f"Mean Geodesic Distance: {np.mean(distances):.3f}\n") - print(f"Method: {method}") - print(f"Final Error: {final_error}") - print(f"Mean Geodesic Distance: {np.mean(distances)}\n") + # Average results over trials + for method in methods: + results[method]["final_error"] = np.mean(results[method]["final_error"]) + results[method]["distances"] = np.mean(results[method]["distances"]) # Plot results plot_results(results) From 398ff31d54dfbd365f31979025fe0d96a39cb262 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 27 Oct 2024 00:39:43 -0700 Subject: [PATCH 22/34] Perturb --- python/gtsam/examples/ViewGraphComparison.py | 16 +++++++++++----- 1 file changed, 11 insertions(+), 5 deletions(-) diff --git a/python/gtsam/examples/ViewGraphComparison.py b/python/gtsam/examples/ViewGraphComparison.py index 11048ccca..3f51d29a5 100644 --- a/python/gtsam/examples/ViewGraphComparison.py +++ b/python/gtsam/examples/ViewGraphComparison.py @@ -109,7 +109,7 @@ def build_factor_graph(method, num_cameras, measurements): return graph -def get_initial_estimate(method, num_cameras, ground_truth, cal): +def get_initial_estimate(method, num_cameras, ground_truth, cal, std=1e-6): """get initial estimate for method""" initialEstimate = Values() total_dimension = 0 @@ -119,16 +119,22 @@ def get_initial_estimate(method, num_cameras, ground_truth, cal): for a in range(num_cameras): b = (a + 1) % num_cameras # Next camera c = (a + 2) % num_cameras # Camera after next - initialEstimate.insert(EdgeKey(a, b).key(), F1) - initialEstimate.insert(EdgeKey(a, c).key(), F2) + # Retract with delta drawn from zero-mean normal + F1_perturbed = F1.retract(np.random.normal(0, std, F1.dim())) + F2_perturbed = F2.retract(np.random.normal(0, 1e-4, F2.dim())) + initialEstimate.insert(EdgeKey(a, b).key(), F1_perturbed) + initialEstimate.insert(EdgeKey(a, c).key(), F2_perturbed) total_dimension += F1.dim() + F2.dim() elif method == "EssentialMatrix": E1, E2 = ground_truth for a in range(num_cameras): b = (a + 1) % num_cameras # Next camera c = (a + 2) % num_cameras # Camera after next - initialEstimate.insert(EdgeKey(a, b).key(), E1) - initialEstimate.insert(EdgeKey(a, c).key(), E2) + # Retract with delta drawn from zero-mean normal + E1_perturbed = E1.retract(np.random.normal(0, std, E1.dim())) + E2_perturbed = E2.retract(np.random.normal(0, 1e-4, E2.dim())) + initialEstimate.insert(EdgeKey(a, b).key(), E1_perturbed) + initialEstimate.insert(EdgeKey(a, c).key(), E2_perturbed) total_dimension += E1.dim() + E2.dim() # Insert initial calibrations for i in range(num_cameras): From 39d74d1d2a594320541b3e9c24e13e7ab929a07e Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Mon, 28 Oct 2024 12:41:52 -0700 Subject: [PATCH 23/34] Add missing template --- gtsam/geometry/geometry.i | 1 + 1 file changed, 1 insertion(+) diff --git a/gtsam/geometry/geometry.i b/gtsam/geometry/geometry.i index 6421ce264..42d2fe550 100644 --- a/gtsam/geometry/geometry.i +++ b/gtsam/geometry/geometry.i @@ -1071,6 +1071,7 @@ typedef gtsam::PinholeCamera PinholeCameraCal3_S2; typedef gtsam::PinholeCamera PinholeCameraCal3DS2; typedef gtsam::PinholeCamera PinholeCameraCal3Unified; typedef gtsam::PinholeCamera PinholeCameraCal3Bundler; +typedef gtsam::PinholeCamera PinholeCameraCal3f; typedef gtsam::PinholeCamera PinholeCameraCal3Fisheye; #include From c0bac3cac576c9cbdec9c8c7903e6366fe2c2999 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Mon, 28 Oct 2024 13:00:28 -0700 Subject: [PATCH 24/34] Error checking --- python/gtsam/examples/ViewGraphComparison.py | 25 ++++++++++++++++++-- 1 file changed, 23 insertions(+), 2 deletions(-) diff --git a/python/gtsam/examples/ViewGraphComparison.py b/python/gtsam/examples/ViewGraphComparison.py index 3f51d29a5..e2cd77c95 100644 --- a/python/gtsam/examples/ViewGraphComparison.py +++ b/python/gtsam/examples/ViewGraphComparison.py @@ -63,13 +63,24 @@ def simulate_data(points, poses, cal, rng, noise_std): # Function to compute ground truth matrices def compute_ground_truth(method, poses, cal): + F1 = FundamentalMatrix(cal.K(), poses[0].between(poses[1]), cal.K()) + F2 = FundamentalMatrix(cal.K(), poses[0].between(poses[2]), cal.K()) if method == "FundamentalMatrix": - F1 = FundamentalMatrix(cal.K(), poses[0].between(poses[1]), cal.K()) - F2 = FundamentalMatrix(cal.K(), poses[0].between(poses[2]), cal.K()) return F1, F2 elif method == "EssentialMatrix": E1 = EssentialMatrix.FromPose3(poses[0].between(poses[1])) E2 = EssentialMatrix.FromPose3(poses[0].between(poses[2])) + + # Assert that E1.matrix and F1 are the same + invK = np.linalg.inv(cal.K()) + G1 = invK.transpose() @ E1.matrix() @ invK + G2 = invK.transpose() @ E2.matrix() @ invK + assert np.allclose( + G1 / np.linalg.norm(G1), F1.matrix() / np.linalg.norm(F1.matrix()) + ), "E1 and F1 are not the same" + assert np.allclose( + G2 / np.linalg.norm(G2), F2.matrix() / np.linalg.norm(F2.matrix()) + ), "E2 and F2 are not the same" return E1, E2 else: raise ValueError(f"Unknown method {method}") @@ -261,6 +272,16 @@ def main(): # Build the factor graph graph = build_factor_graph(method, args.num_cameras, measurements) + # Assert that the initial error is the same for both methods: + if trial == 0 and method == methods[0]: + initial_error = graph.error(initial_estimate[method]) + else: + current_error = graph.error(initial_estimate[method]) + if not np.allclose(initial_error, current_error): + print(f"Initial error for {method} ({current_error}) does not match ") + print(f"initial error for {methods[0]} ({initial_error})") + exit(1) + # Optimize the graph result = optimize(graph, initial_estimate[method]) From 60ce938e31b76b34a822ad4b5090407cdedf3cd1 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Mon, 28 Oct 2024 15:58:13 -0700 Subject: [PATCH 25/34] Add prior templates --- gtsam/nonlinear/nonlinear.i | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/gtsam/nonlinear/nonlinear.i b/gtsam/nonlinear/nonlinear.i index 80f694e97..09c234630 100644 --- a/gtsam/nonlinear/nonlinear.i +++ b/gtsam/nonlinear/nonlinear.i @@ -75,13 +75,20 @@ class NonlinearFactorGraph { gtsam::Pose2, gtsam::Pose3, gtsam::Cal3_S2, + gtsam::Cal3f, + gtsam::Cal3Bundler, gtsam::Cal3Fisheye, gtsam::Cal3Unified, gtsam::CalibratedCamera, + gtsam::EssentialMatrix, + gtsam::FundamentalMatrix, + gtsam::SimpleFundamentalMatrix, gtsam::PinholeCamera, + gtsam::PinholeCamera, gtsam::PinholeCamera, gtsam::PinholeCamera, gtsam::PinholeCamera, + gtsam::PinholeCamera, gtsam::imuBias::ConstantBias}> void addPrior(size_t key, const T& prior, const gtsam::noiseModel::Base* noiseModel); From bbba497ca10cf6f378957b09438ba6f51504243e Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Mon, 28 Oct 2024 15:58:48 -0700 Subject: [PATCH 26/34] Add prior, use calibrations --- python/gtsam/examples/ViewGraphComparison.py | 54 +++++++++++--------- 1 file changed, 29 insertions(+), 25 deletions(-) diff --git a/python/gtsam/examples/ViewGraphComparison.py b/python/gtsam/examples/ViewGraphComparison.py index e2cd77c95..d11fdf6f9 100644 --- a/python/gtsam/examples/ViewGraphComparison.py +++ b/python/gtsam/examples/ViewGraphComparison.py @@ -71,7 +71,7 @@ def compute_ground_truth(method, poses, cal): E1 = EssentialMatrix.FromPose3(poses[0].between(poses[1])) E2 = EssentialMatrix.FromPose3(poses[0].between(poses[2])) - # Assert that E1.matrix and F1 are the same + # Assert that E1.matrix and F1 are the same, with known calibration invK = np.linalg.inv(cal.K()) G1 = invK.transpose() @ E1.matrix() @ invK G2 = invK.transpose() @ E2.matrix() @ invK @@ -94,6 +94,10 @@ def build_factor_graph(method, num_cameras, measurements): FactorClass = gtsam.TransferFactorFundamentalMatrix elif method == "EssentialMatrix": FactorClass = gtsam.EssentialTransferFactorCal3f + # add priors on all calibrations: + for i in range(num_cameras): + model = gtsam.noiseModel.Isotropic.Sigma(1, 10.0) + graph.addPriorCal3f(K(i), Cal3f(50.0, 50.0, 50.0), model) else: raise ValueError(f"Unknown method {method}") @@ -120,7 +124,7 @@ def build_factor_graph(method, num_cameras, measurements): return graph -def get_initial_estimate(method, num_cameras, ground_truth, cal, std=1e-6): +def get_initial_estimate(method, num_cameras, ground_truth, cal): """get initial estimate for method""" initialEstimate = Values() total_dimension = 0 @@ -130,22 +134,16 @@ def get_initial_estimate(method, num_cameras, ground_truth, cal, std=1e-6): for a in range(num_cameras): b = (a + 1) % num_cameras # Next camera c = (a + 2) % num_cameras # Camera after next - # Retract with delta drawn from zero-mean normal - F1_perturbed = F1.retract(np.random.normal(0, std, F1.dim())) - F2_perturbed = F2.retract(np.random.normal(0, 1e-4, F2.dim())) - initialEstimate.insert(EdgeKey(a, b).key(), F1_perturbed) - initialEstimate.insert(EdgeKey(a, c).key(), F2_perturbed) + initialEstimate.insert(EdgeKey(a, b).key(), F1) + initialEstimate.insert(EdgeKey(a, c).key(), F2) total_dimension += F1.dim() + F2.dim() elif method == "EssentialMatrix": E1, E2 = ground_truth for a in range(num_cameras): b = (a + 1) % num_cameras # Next camera c = (a + 2) % num_cameras # Camera after next - # Retract with delta drawn from zero-mean normal - E1_perturbed = E1.retract(np.random.normal(0, std, E1.dim())) - E2_perturbed = E2.retract(np.random.normal(0, 1e-4, E2.dim())) - initialEstimate.insert(EdgeKey(a, b).key(), E1_perturbed) - initialEstimate.insert(EdgeKey(a, c).key(), E2_perturbed) + initialEstimate.insert(EdgeKey(a, b).key(), E1) + initialEstimate.insert(EdgeKey(a, c).key(), E2) total_dimension += E1.dim() + E2.dim() # Insert initial calibrations for i in range(num_cameras): @@ -158,14 +156,17 @@ def get_initial_estimate(method, num_cameras, ground_truth, cal, std=1e-6): return initialEstimate -def optimize(graph, initialEstimate): +def optimize(graph, initialEstimate, method): """optimize the graph""" params = LevenbergMarquardtParams() - params.setlambdaInitial(1000.0) # Initialize lambda to a high value - params.setAbsoluteErrorTol(1.0) + params.setlambdaInitial(1e10) # Initialize lambda to a high value + params.setlambdaUpperBound(1e10) + # params.setAbsoluteErrorTol(0.1) params.setVerbosityLM("SUMMARY") optimizer = LevenbergMarquardtOptimizer(graph, initialEstimate, params) result = optimizer.optimize() + # if method == "EssentialMatrix": + # result.print("Final results:\n", formatter) return result @@ -195,9 +196,15 @@ def compute_distances(method, result, ground_truth, num_cameras, cal): elif method == "EssentialMatrix": E_est_ab = result.atEssentialMatrix(key_ab) E_est_ac = result.atEssentialMatrix(key_ac) + + # Retrieve correct calibrations from result: + cal_a = result.atCal3f(K(a)) + cal_b = result.atCal3f(K(b)) + cal_c = result.atCal3f(K(c)) + # Convert estimated EssentialMatrices to FundamentalMatrices - F_est_ab = gtsam.FundamentalMatrix(cal.K(), E_est_ab, cal.K()) - F_est_ac = gtsam.FundamentalMatrix(cal.K(), E_est_ac, cal.K()) + F_est_ab = gtsam.FundamentalMatrix(cal_a.K(), E_est_ab, cal_b.K()) + F_est_ac = gtsam.FundamentalMatrix(cal_a.K(), E_est_ac, cal_c.K()) # Compute local coordinates (geodesic distance on the F-manifold) dist_ab = np.linalg.norm(F1.localCoordinates(F_est_ab)) @@ -240,7 +247,7 @@ def main(): parser.add_argument("--num_cameras", type=int, default=4, help="Number of cameras (default: 4)") parser.add_argument("--nr_trials", type=int, default=5, help="Number of trials (default: 5)") parser.add_argument("--seed", type=int, default=42, help="Random seed (default: 42)") - parser.add_argument("--noise_std", type=float, default=1.0, help="Standard deviation of noise (default: 1.0)") + parser.add_argument("--noise_std", type=float, default=0.5, help="Standard deviation of noise (default: 0.5)") args = parser.parse_args() # Initialize the random number generator @@ -273,17 +280,14 @@ def main(): graph = build_factor_graph(method, args.num_cameras, measurements) # Assert that the initial error is the same for both methods: - if trial == 0 and method == methods[0]: - initial_error = graph.error(initial_estimate[method]) + if method == methods[0]: + error0 = graph.error(initial_estimate[method]) else: current_error = graph.error(initial_estimate[method]) - if not np.allclose(initial_error, current_error): - print(f"Initial error for {method} ({current_error}) does not match ") - print(f"initial error for {methods[0]} ({initial_error})") - exit(1) + assert np.allclose(error0, current_error) # Optimize the graph - result = optimize(graph, initial_estimate[method]) + result = optimize(graph, initial_estimate[method], method) # Compute distances from ground truth distances = compute_distances(method, result, ground_truth[method], args.num_cameras, cal) From aa0db60a52700b51d4514c665550905363f9cd84 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Tue, 29 Oct 2024 09:21:19 -0700 Subject: [PATCH 27/34] Add SimpleF case --- python/gtsam/examples/ViewGraphComparison.py | 79 +++++++++++--------- 1 file changed, 45 insertions(+), 34 deletions(-) diff --git a/python/gtsam/examples/ViewGraphComparison.py b/python/gtsam/examples/ViewGraphComparison.py index d11fdf6f9..5c3c2c176 100644 --- a/python/gtsam/examples/ViewGraphComparison.py +++ b/python/gtsam/examples/ViewGraphComparison.py @@ -1,7 +1,9 @@ """ - Compare the Fundamental Matrix and Essential Matrix methods for optimizing the view-graph. - It measures the distance from the ground truth matrices in terms of the norm of local coordinates (geodesic distance) - on the F-manifold. It also plots the final error of the optimization. + Compare several methods for optimizing the view-graph. + We measure the distance from the ground truth in terms of the norm of + local coordinates (geodesic distance) on the F-manifold. + We also plot the final error of the optimization. + Author: Frank Dellaert (with heavy assist from ChatGPT) Date: October 2024 """ @@ -13,15 +15,24 @@ import numpy as np from gtsam.examples import SFMdata import gtsam -from gtsam import (Cal3f, EdgeKey, EssentialMatrix, FundamentalMatrix, - LevenbergMarquardtOptimizer, LevenbergMarquardtParams, - NonlinearFactorGraph, PinholeCameraCal3f, Values) +from gtsam import ( + Cal3f, + EdgeKey, + EssentialMatrix, + FundamentalMatrix, + LevenbergMarquardtOptimizer, + LevenbergMarquardtParams, + NonlinearFactorGraph, + PinholeCameraCal3f, + SimpleFundamentalMatrix, + Values, +) # For symbol shorthand (e.g., K(0), K(1)) K = gtsam.symbol_shorthand.K # Methods to compare -methods = ["FundamentalMatrix", "EssentialMatrix"] +methods = ["FundamentalMatrix", "SimpleFundamentalMatrix", "EssentialMatrix"] # Formatter function for printing keys @@ -63,41 +74,38 @@ def simulate_data(points, poses, cal, rng, noise_std): # Function to compute ground truth matrices def compute_ground_truth(method, poses, cal): - F1 = FundamentalMatrix(cal.K(), poses[0].between(poses[1]), cal.K()) - F2 = FundamentalMatrix(cal.K(), poses[0].between(poses[2]), cal.K()) + E1 = EssentialMatrix.FromPose3(poses[0].between(poses[1])) + E2 = EssentialMatrix.FromPose3(poses[0].between(poses[2])) + F1 = FundamentalMatrix(cal.K(), E1, cal.K()) + F2 = FundamentalMatrix(cal.K(), E2, cal.K()) if method == "FundamentalMatrix": return F1, F2 + elif method == "SimpleFundamentalMatrix": + f = cal.fx() + c = cal.principalPoint() + SF1 = SimpleFundamentalMatrix(E1, f, f, c, c) + SF2 = SimpleFundamentalMatrix(E2, f, f, c, c) + return SF1, SF2 elif method == "EssentialMatrix": - E1 = EssentialMatrix.FromPose3(poses[0].between(poses[1])) - E2 = EssentialMatrix.FromPose3(poses[0].between(poses[2])) - - # Assert that E1.matrix and F1 are the same, with known calibration - invK = np.linalg.inv(cal.K()) - G1 = invK.transpose() @ E1.matrix() @ invK - G2 = invK.transpose() @ E2.matrix() @ invK - assert np.allclose( - G1 / np.linalg.norm(G1), F1.matrix() / np.linalg.norm(F1.matrix()) - ), "E1 and F1 are not the same" - assert np.allclose( - G2 / np.linalg.norm(G2), F2.matrix() / np.linalg.norm(F2.matrix()) - ), "E2 and F2 are not the same" return E1, E2 else: raise ValueError(f"Unknown method {method}") -def build_factor_graph(method, num_cameras, measurements): +def build_factor_graph(method, num_cameras, measurements, cal): """build the factor graph""" graph = NonlinearFactorGraph() if method == "FundamentalMatrix": FactorClass = gtsam.TransferFactorFundamentalMatrix + elif method == "SimpleFundamentalMatrix": + FactorClass = gtsam.TransferFactorSimpleFundamentalMatrix elif method == "EssentialMatrix": FactorClass = gtsam.EssentialTransferFactorCal3f # add priors on all calibrations: for i in range(num_cameras): model = gtsam.noiseModel.Isotropic.Sigma(1, 10.0) - graph.addPriorCal3f(K(i), Cal3f(50.0, 50.0, 50.0), model) + graph.addPriorCal3f(K(i), cal, model) else: raise ValueError(f"Unknown method {method}") @@ -129,7 +137,7 @@ def get_initial_estimate(method, num_cameras, ground_truth, cal): initialEstimate = Values() total_dimension = 0 - if method == "FundamentalMatrix": + if method == "FundamentalMatrix" or method == "SimpleFundamentalMatrix": F1, F2 = ground_truth for a in range(num_cameras): b = (a + 1) % num_cameras # Next camera @@ -174,13 +182,13 @@ def compute_distances(method, result, ground_truth, num_cameras, cal): """Compute geodesic distances from ground truth""" distances = [] - if method == "FundamentalMatrix": + if method == "FundamentalMatrix" or method == "SimpleFundamentalMatrix": F1, F2 = ground_truth elif method == "EssentialMatrix": E1, E2 = ground_truth # Convert ground truth EssentialMatrices to FundamentalMatrices - F1 = gtsam.FundamentalMatrix(cal.K(), E1, cal.K()) - F2 = gtsam.FundamentalMatrix(cal.K(), E2, cal.K()) + F1 = FundamentalMatrix(cal.K(), E1, cal.K()) + F2 = FundamentalMatrix(cal.K(), E2, cal.K()) else: raise ValueError(f"Unknown method {method}") @@ -193,6 +201,9 @@ def compute_distances(method, result, ground_truth, num_cameras, cal): if method == "FundamentalMatrix": F_est_ab = result.atFundamentalMatrix(key_ab) F_est_ac = result.atFundamentalMatrix(key_ac) + elif method == "SimpleFundamentalMatrix": + F_est_ab = result.atSimpleFundamentalMatrix(key_ab) + F_est_ac = result.atSimpleFundamentalMatrix(key_ac) elif method == "EssentialMatrix": E_est_ab = result.atEssentialMatrix(key_ab) E_est_ac = result.atEssentialMatrix(key_ac) @@ -203,8 +214,8 @@ def compute_distances(method, result, ground_truth, num_cameras, cal): cal_c = result.atCal3f(K(c)) # Convert estimated EssentialMatrices to FundamentalMatrices - F_est_ab = gtsam.FundamentalMatrix(cal_a.K(), E_est_ab, cal_b.K()) - F_est_ac = gtsam.FundamentalMatrix(cal_a.K(), E_est_ac, cal_c.K()) + F_est_ab = FundamentalMatrix(cal_a.K(), E_est_ab, cal_b.K()) + F_est_ac = FundamentalMatrix(cal_a.K(), E_est_ac, cal_c.K()) # Compute local coordinates (geodesic distance on the F-manifold) dist_ab = np.linalg.norm(F1.localCoordinates(F_est_ab)) @@ -219,7 +230,7 @@ def plot_results(results): """plot results""" methods = list(results.keys()) final_errors = [results[method]["final_error"] for method in methods] - distances = [np.mean(results[method]["distances"]) for method in methods] + distances = [results[method]["distances"] for method in methods] fig, ax1 = plt.subplots() @@ -277,14 +288,14 @@ def main(): print(f"\nRunning method: {method}") # Build the factor graph - graph = build_factor_graph(method, args.num_cameras, measurements) + graph = build_factor_graph(method, args.num_cameras, measurements, cal) - # Assert that the initial error is the same for both methods: + # Assert that the initial error is the same for all methods: if method == methods[0]: error0 = graph.error(initial_estimate[method]) else: current_error = graph.error(initial_estimate[method]) - assert np.allclose(error0, current_error) + assert np.allclose(error0, current_error), "Initial errors do not match among methods." # Optimize the graph result = optimize(graph, initial_estimate[method], method) From 874fb5919f1c79ef1d8135550aec0e5cb1074d98 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Tue, 29 Oct 2024 09:34:33 -0700 Subject: [PATCH 28/34] Show iterations and add extra points --- python/gtsam/examples/ViewGraphComparison.py | 39 +++++++++++++++----- 1 file changed, 29 insertions(+), 10 deletions(-) diff --git a/python/gtsam/examples/ViewGraphComparison.py b/python/gtsam/examples/ViewGraphComparison.py index 5c3c2c176..9410240ba 100644 --- a/python/gtsam/examples/ViewGraphComparison.py +++ b/python/gtsam/examples/ViewGraphComparison.py @@ -45,7 +45,7 @@ def formatter(key): return f"({edge.i()},{edge.j()})" -def simulate_geometry(num_cameras): +def simulate_geometry(num_cameras, rng, num_random_points=12): """simulate geometry (points and poses)""" # Define the camera calibration parameters cal = Cal3f(50.0, 50.0, 50.0) @@ -53,6 +53,10 @@ def simulate_geometry(num_cameras): # Create the set of 8 ground-truth landmarks points = SFMdata.createPoints() + # Create extra random points in the -10,10 cube around the origin + extra_points = rng.uniform(-10, 10, (num_random_points, 3)) + points.extend([gtsam.Point3(p) for p in extra_points]) + # Create the set of ground-truth poses poses = SFMdata.posesOnCircle(num_cameras, 30) @@ -173,9 +177,8 @@ def optimize(graph, initialEstimate, method): params.setVerbosityLM("SUMMARY") optimizer = LevenbergMarquardtOptimizer(graph, initialEstimate, params) result = optimizer.optimize() - # if method == "EssentialMatrix": - # result.print("Final results:\n", formatter) - return result + iterations = optimizer.iterations() + return result, iterations def compute_distances(method, result, ground_truth, num_cameras, cal): @@ -231,6 +234,7 @@ def plot_results(results): methods = list(results.keys()) final_errors = [results[method]["final_error"] for method in methods] distances = [results[method]["distances"] for method in methods] + iterations = [results[method]["iterations"] for method in methods] fig, ax1 = plt.subplots() @@ -243,10 +247,21 @@ def plot_results(results): ax2 = ax1.twinx() color = "tab:blue" ax2.set_ylabel("Mean Geodesic Distance", color=color) - ax2.plot(methods, distances, color=color, marker="o") + ax2.plot(methods, distances, color=color, marker="o", linestyle="-") ax2.tick_params(axis="y", labelcolor=color) - plt.title("Comparison of Methods") + # Annotate the blue data points with the average number of iterations + for i, method in enumerate(methods): + ax2.annotate( + f"{iterations[i]:.1f}", + (i, distances[i]), + textcoords="offset points", + xytext=(20, -5), + ha="center", + color=color, + ) + + plt.title("Comparison of Methods (Labels show avg iterations)") fig.tight_layout() plt.show() @@ -256,6 +271,7 @@ def main(): # Parse command line arguments parser = argparse.ArgumentParser(description="Compare Fundamental and Essential Matrix Methods") parser.add_argument("--num_cameras", type=int, default=4, help="Number of cameras (default: 4)") + parser.add_argument("--num_extra_points", type=int, default=12, help="Number of extra random points (default: 12)") parser.add_argument("--nr_trials", type=int, default=5, help="Number of trials (default: 5)") parser.add_argument("--seed", type=int, default=42, help="Random seed (default: 42)") parser.add_argument("--noise_std", type=float, default=0.5, help="Standard deviation of noise (default: 0.5)") @@ -265,10 +281,10 @@ def main(): rng = np.random.default_rng(seed=args.seed) # Initialize results dictionary - results = {method: {"distances": [], "final_error": []} for method in methods} + results = {method: {"distances": [], "final_error": [], "iterations": []} for method in methods} # Simulate geometry - points, poses, cal = simulate_geometry(args.num_cameras) + points, poses, cal = simulate_geometry(args.num_cameras, rng, args.num_extra_points) # Compute ground truth matrices ground_truth = {method: compute_ground_truth(method, poses, cal) for method in methods} @@ -298,7 +314,7 @@ def main(): assert np.allclose(error0, current_error), "Initial errors do not match among methods." # Optimize the graph - result = optimize(graph, initial_estimate[method], method) + result, iterations = optimize(graph, initial_estimate[method], method) # Compute distances from ground truth distances = compute_distances(method, result, ground_truth[method], args.num_cameras, cal) @@ -309,15 +325,18 @@ def main(): # Store results results[method]["distances"].extend(distances) results[method]["final_error"].append(final_error) + results[method]["iterations"].append(iterations) print(f"Method: {method}") print(f"Final Error: {final_error:.3f}") - print(f"Mean Geodesic Distance: {np.mean(distances):.3f}\n") + print(f"Mean Geodesic Distance: {np.mean(distances):.3f}") + print(f"Number of Iterations: {iterations}\n") # Average results over trials for method in methods: results[method]["final_error"] = np.mean(results[method]["final_error"]) results[method]["distances"] = np.mean(results[method]["distances"]) + results[method]["iterations"] = np.mean(results[method]["iterations"]) # Plot results plot_results(results) From 3e14b7194f69734d84462f5b5e880df481d04d3a Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Tue, 29 Oct 2024 10:30:22 -0700 Subject: [PATCH 29/34] Add CalibratedEssentialMatrix case --- python/gtsam/examples/ViewGraphComparison.py | 71 +++++++++++++------- 1 file changed, 45 insertions(+), 26 deletions(-) diff --git a/python/gtsam/examples/ViewGraphComparison.py b/python/gtsam/examples/ViewGraphComparison.py index 9410240ba..da8b06803 100644 --- a/python/gtsam/examples/ViewGraphComparison.py +++ b/python/gtsam/examples/ViewGraphComparison.py @@ -32,7 +32,7 @@ from gtsam import ( K = gtsam.symbol_shorthand.K # Methods to compare -methods = ["FundamentalMatrix", "SimpleFundamentalMatrix", "EssentialMatrix"] +methods = ["FundamentalMatrix", "SimpleFundamentalMatrix", "EssentialMatrix", "CalibratedEssentialMatrix"] # Formatter function for printing keys @@ -90,7 +90,7 @@ def compute_ground_truth(method, poses, cal): SF1 = SimpleFundamentalMatrix(E1, f, f, c, c) SF2 = SimpleFundamentalMatrix(E2, f, f, c, c) return SF1, SF2 - elif method == "EssentialMatrix": + elif method == "EssentialMatrix" or method == "CalibratedEssentialMatrix": return E1, E2 else: raise ValueError(f"Unknown method {method}") @@ -110,9 +110,18 @@ def build_factor_graph(method, num_cameras, measurements, cal): for i in range(num_cameras): model = gtsam.noiseModel.Isotropic.Sigma(1, 10.0) graph.addPriorCal3f(K(i), cal, model) + elif method == "CalibratedEssentialMatrix": + FactorClass = gtsam.TransferFactorEssentialMatrix + # No priors on calibration needed else: raise ValueError(f"Unknown method {method}") + if method == "CalibratedEssentialMatrix": + # Calibrate measurements using ground truth calibration + z = [[cal.calibrate(m) for m in cam_measurements] for cam_measurements in measurements] + else: + z = measurements + for a in range(num_cameras): b = (a + 1) % num_cameras # Next camera c = (a + 2) % num_cameras # Camera after next @@ -124,9 +133,9 @@ def build_factor_graph(method, num_cameras, measurements, cal): # Collect data for the three factors for j in range(len(measurements[0])): - tuples1.append((measurements[a][j], measurements[b][j], measurements[c][j])) - tuples2.append((measurements[a][j], measurements[c][j], measurements[b][j])) - tuples3.append((measurements[c][j], measurements[b][j], measurements[a][j])) + tuples1.append((z[a][j], z[b][j], z[c][j])) + tuples2.append((z[a][j], z[c][j], z[b][j])) + tuples3.append((z[c][j], z[b][j], z[a][j])) # Add transfer factors between views a, b, and c. graph.add(FactorClass(EdgeKey(a, c), EdgeKey(b, c), tuples1)) @@ -141,7 +150,7 @@ def get_initial_estimate(method, num_cameras, ground_truth, cal): initialEstimate = Values() total_dimension = 0 - if method == "FundamentalMatrix" or method == "SimpleFundamentalMatrix": + if method in ["FundamentalMatrix", "SimpleFundamentalMatrix"]: F1, F2 = ground_truth for a in range(num_cameras): b = (a + 1) % num_cameras # Next camera @@ -149,7 +158,7 @@ def get_initial_estimate(method, num_cameras, ground_truth, cal): initialEstimate.insert(EdgeKey(a, b).key(), F1) initialEstimate.insert(EdgeKey(a, c).key(), F2) total_dimension += F1.dim() + F2.dim() - elif method == "EssentialMatrix": + elif method in ["EssentialMatrix", "CalibratedEssentialMatrix"]: E1, E2 = ground_truth for a in range(num_cameras): b = (a + 1) % num_cameras # Next camera @@ -157,12 +166,14 @@ def get_initial_estimate(method, num_cameras, ground_truth, cal): initialEstimate.insert(EdgeKey(a, b).key(), E1) initialEstimate.insert(EdgeKey(a, c).key(), E2) total_dimension += E1.dim() + E2.dim() + else: + raise ValueError(f"Unknown method {method}") + + if method == "EssentialMatrix": # Insert initial calibrations for i in range(num_cameras): initialEstimate.insert(K(i), cal) total_dimension += cal.dim() - else: - raise ValueError(f"Unknown method {method}") print(f"Total dimension of the problem: {total_dimension}") return initialEstimate @@ -185,15 +196,7 @@ def compute_distances(method, result, ground_truth, num_cameras, cal): """Compute geodesic distances from ground truth""" distances = [] - if method == "FundamentalMatrix" or method == "SimpleFundamentalMatrix": - F1, F2 = ground_truth - elif method == "EssentialMatrix": - E1, E2 = ground_truth - # Convert ground truth EssentialMatrices to FundamentalMatrices - F1 = FundamentalMatrix(cal.K(), E1, cal.K()) - F2 = FundamentalMatrix(cal.K(), E2, cal.K()) - else: - raise ValueError(f"Unknown method {method}") + F1, F2 = ground_truth["FundamentalMatrix"] for a in range(num_cameras): b = (a + 1) % num_cameras @@ -201,17 +204,21 @@ def compute_distances(method, result, ground_truth, num_cameras, cal): key_ab = EdgeKey(a, b).key() key_ac = EdgeKey(a, c).key() + if method in ["EssentialMatrix", "CalibratedEssentialMatrix"]: + E_est_ab = result.atEssentialMatrix(key_ab) + E_est_ac = result.atEssentialMatrix(key_ac) + + # Compute estimated FundamentalMatrices if method == "FundamentalMatrix": F_est_ab = result.atFundamentalMatrix(key_ab) F_est_ac = result.atFundamentalMatrix(key_ac) elif method == "SimpleFundamentalMatrix": - F_est_ab = result.atSimpleFundamentalMatrix(key_ab) - F_est_ac = result.atSimpleFundamentalMatrix(key_ac) + SF_est_ab = result.atSimpleFundamentalMatrix(key_ab).matrix() + SF_est_ac = result.atSimpleFundamentalMatrix(key_ac).matrix() + F_est_ab = FundamentalMatrix(SF_est_ab) + F_est_ac = FundamentalMatrix(SF_est_ac) elif method == "EssentialMatrix": - E_est_ab = result.atEssentialMatrix(key_ab) - E_est_ac = result.atEssentialMatrix(key_ac) - - # Retrieve correct calibrations from result: + # Retrieve calibrations from result: cal_a = result.atCal3f(K(a)) cal_b = result.atCal3f(K(b)) cal_c = result.atCal3f(K(c)) @@ -219,6 +226,12 @@ def compute_distances(method, result, ground_truth, num_cameras, cal): # Convert estimated EssentialMatrices to FundamentalMatrices F_est_ab = FundamentalMatrix(cal_a.K(), E_est_ab, cal_b.K()) F_est_ac = FundamentalMatrix(cal_a.K(), E_est_ac, cal_c.K()) + elif method == "CalibratedEssentialMatrix": + # Use ground truth calibration + F_est_ab = FundamentalMatrix(cal.K(), E_est_ab, cal.K()) + F_est_ac = FundamentalMatrix(cal.K(), E_est_ac, cal.K()) + else: + raise ValueError(f"Unknown method {method}") # Compute local coordinates (geodesic distance on the F-manifold) dist_ab = np.linalg.norm(F1.localCoordinates(F_est_ab)) @@ -256,7 +269,7 @@ def plot_results(results): f"{iterations[i]:.1f}", (i, distances[i]), textcoords="offset points", - xytext=(20, -5), + xytext=(0, 10), ha="center", color=color, ) @@ -309,6 +322,10 @@ def main(): # Assert that the initial error is the same for all methods: if method == methods[0]: error0 = graph.error(initial_estimate[method]) + elif method == "CalibratedEssentialMatrix": + current_error = graph.error(initial_estimate[method]) * cal.f() * cal.f() + print(error0, current_error) + assert np.allclose(error0, current_error), "Initial errors do not match among methods." else: current_error = graph.error(initial_estimate[method]) assert np.allclose(error0, current_error), "Initial errors do not match among methods." @@ -317,10 +334,12 @@ def main(): result, iterations = optimize(graph, initial_estimate[method], method) # Compute distances from ground truth - distances = compute_distances(method, result, ground_truth[method], args.num_cameras, cal) + distances = compute_distances(method, result, ground_truth, args.num_cameras, cal) # Compute final error final_error = graph.error(result) + if method == "CalibratedEssentialMatrix": + final_error *= cal.f() * cal.f() # Store results results[method]["distances"].extend(distances) From 6b96ae217f58327c66870757d32cf8a1c0b529e2 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Tue, 29 Oct 2024 10:58:08 -0700 Subject: [PATCH 30/34] Fix small issues and store scaled s_ --- gtsam/geometry/FundamentalMatrix.cpp | 30 ++++++++----------- gtsam/geometry/FundamentalMatrix.h | 16 ++++++---- .../geometry/tests/testFundamentalMatrix.cpp | 3 ++ 3 files changed, 25 insertions(+), 24 deletions(-) diff --git a/gtsam/geometry/FundamentalMatrix.cpp b/gtsam/geometry/FundamentalMatrix.cpp index 5a50b4fd2..94c72673d 100644 --- a/gtsam/geometry/FundamentalMatrix.cpp +++ b/gtsam/geometry/FundamentalMatrix.cpp @@ -2,10 +2,12 @@ * @file FundamentalMatrix.cpp * @brief FundamentalMatrix classes * @author Frank Dellaert - * @date Oct 23, 2024 + * @date October 2024 */ +#include #include +#include namespace gtsam { @@ -26,8 +28,8 @@ Point2 EpipolarTransfer(const Matrix3& Fca, const Point2& pa, // } //************************************************************************* -FundamentalMatrix::FundamentalMatrix(const Matrix& U, double s, - const Matrix& V) { +FundamentalMatrix::FundamentalMatrix(const Matrix3& U, double s, + const Matrix3& V) { initialize(U, s, V); } @@ -57,38 +59,30 @@ FundamentalMatrix::FundamentalMatrix(const Matrix3& F) { void FundamentalMatrix::initialize(const Matrix3& U, double s, const Matrix3& V) { - s_ = s; + s_ = s / kScale; sign_ = 1.0; - // Check if U is a reflection and its determinant is close to -1 or 1 - double detU = U.determinant(); - if (std::abs(std::abs(detU) - 1.0) > 1e-9) { - throw std::invalid_argument( - "Matrix U does not have a determinant close to -1 or 1."); - } + // Check if U is a reflection and flip U and sign_ if so + double detU = U.determinant(); // detU will be -1 or 1 if (detU < 0) { U_ = Rot3(-U); - sign_ = -sign_; // Flip sign if U is a reflection + sign_ = -sign_; } else { U_ = Rot3(U); } - // Check if V is a reflection and its determinant is close to -1 or 1 + // Same check for V double detV = V.determinant(); - if (std::abs(std::abs(detV) - 1.0) > 1e-9) { - throw std::invalid_argument( - "Matrix V does not have a determinant close to -1 or 1."); - } if (detV < 0) { V_ = Rot3(-V); - sign_ = -sign_; // Flip sign if V is a reflection + sign_ = -sign_; } else { V_ = Rot3(V); } } Matrix3 FundamentalMatrix::matrix() const { - return sign_ * U_.matrix() * Vector3(1.0, s_, 0).asDiagonal() * + return sign_ * U_.matrix() * Vector3(1.0, s_ * kScale, 0).asDiagonal() * V_.transpose().matrix(); } diff --git a/gtsam/geometry/FundamentalMatrix.h b/gtsam/geometry/FundamentalMatrix.h index 7e87ba62a..a36c815da 100644 --- a/gtsam/geometry/FundamentalMatrix.h +++ b/gtsam/geometry/FundamentalMatrix.h @@ -2,7 +2,7 @@ * @file FundamentalMatrix.h * @brief FundamentalMatrix classes * @author Frank Dellaert - * @date Oct 23, 2024 + * @date October 2024 */ #pragma once @@ -34,9 +34,11 @@ class GTSAM_EXPORT FundamentalMatrix { double s_; ///< Scalar parameter for S Rot3 V_; ///< Right rotation + static constexpr double kScale = 1000; // s is stored in s_ as s/kScale + public: /// Default constructor - FundamentalMatrix() : U_(Rot3()), sign_(1.0), s_(1.0), V_(Rot3()) {} + FundamentalMatrix() : U_(Rot3()), sign_(1.0), s_(1.0 / kScale), V_(Rot3()) {} /** * @brief Construct from U, V, and scalar s @@ -44,7 +46,7 @@ class GTSAM_EXPORT FundamentalMatrix { * Initializes the FundamentalMatrix From the SVD representation * U*diag(1,s,0)*V^T. It will internally convert to using SO(3). */ - FundamentalMatrix(const Matrix& U, double s, const Matrix& V); + FundamentalMatrix(const Matrix3& U, double s, const Matrix3& V); /** * @brief Construct from a 3x3 matrix using SVD @@ -60,7 +62,9 @@ class GTSAM_EXPORT FundamentalMatrix { * @brief Construct from essential matrix and calibration matrices * * Initializes the FundamentalMatrix from the given essential matrix E - * and calibration matrices Ka and Kb. + * and calibration matrices Ka and Kb, using + * F = Ka^(-T) * E * Kb^(-1) + * and then calls constructor that decomposes F via SVD. * * @param E Essential matrix * @param Ka Calibration matrix for the left camera @@ -111,8 +115,8 @@ class GTSAM_EXPORT FundamentalMatrix { /// @} private: /// Private constructor for internal use - FundamentalMatrix(const Rot3& U, double sign, double s, const Rot3& V) - : U_(U), sign_(sign), s_(s), V_(V) {} + FundamentalMatrix(const Rot3& U, double sign, double scaled_s, const Rot3& V) + : U_(U), sign_(sign), s_(scaled_s), V_(V) {} /// Initialize SO(3) matrices from general O(3) matrices void initialize(const Matrix3& U, double s, const Matrix3& V); diff --git a/gtsam/geometry/tests/testFundamentalMatrix.cpp b/gtsam/geometry/tests/testFundamentalMatrix.cpp index 00876e415..eed5da734 100644 --- a/gtsam/geometry/tests/testFundamentalMatrix.cpp +++ b/gtsam/geometry/tests/testFundamentalMatrix.cpp @@ -6,12 +6,15 @@ */ #include +#include #include #include #include #include #include +#include + using namespace std::placeholders; using namespace std; using namespace gtsam; From 005efb3f07f1e92c7aecb2d8f7e84f3795d039fa Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Tue, 29 Oct 2024 11:07:32 -0700 Subject: [PATCH 31/34] Abbreviate methods --- python/gtsam/examples/ViewGraphComparison.py | 40 ++++++++++---------- 1 file changed, 20 insertions(+), 20 deletions(-) diff --git a/python/gtsam/examples/ViewGraphComparison.py b/python/gtsam/examples/ViewGraphComparison.py index da8b06803..968bbc9df 100644 --- a/python/gtsam/examples/ViewGraphComparison.py +++ b/python/gtsam/examples/ViewGraphComparison.py @@ -32,7 +32,7 @@ from gtsam import ( K = gtsam.symbol_shorthand.K # Methods to compare -methods = ["FundamentalMatrix", "SimpleFundamentalMatrix", "EssentialMatrix", "CalibratedEssentialMatrix"] +methods = ["Fundamental", "SimpleF", "Essential+Ks", "Calibrated"] # Formatter function for printing keys @@ -82,15 +82,15 @@ def compute_ground_truth(method, poses, cal): E2 = EssentialMatrix.FromPose3(poses[0].between(poses[2])) F1 = FundamentalMatrix(cal.K(), E1, cal.K()) F2 = FundamentalMatrix(cal.K(), E2, cal.K()) - if method == "FundamentalMatrix": + if method == "Fundamental": return F1, F2 - elif method == "SimpleFundamentalMatrix": + elif method == "SimpleF": f = cal.fx() c = cal.principalPoint() SF1 = SimpleFundamentalMatrix(E1, f, f, c, c) SF2 = SimpleFundamentalMatrix(E2, f, f, c, c) return SF1, SF2 - elif method == "EssentialMatrix" or method == "CalibratedEssentialMatrix": + elif method == "Essential+Ks" or method == "Calibrated": return E1, E2 else: raise ValueError(f"Unknown method {method}") @@ -100,23 +100,23 @@ def build_factor_graph(method, num_cameras, measurements, cal): """build the factor graph""" graph = NonlinearFactorGraph() - if method == "FundamentalMatrix": + if method == "Fundamental": FactorClass = gtsam.TransferFactorFundamentalMatrix - elif method == "SimpleFundamentalMatrix": + elif method == "SimpleF": FactorClass = gtsam.TransferFactorSimpleFundamentalMatrix - elif method == "EssentialMatrix": + elif method == "Essential+Ks": FactorClass = gtsam.EssentialTransferFactorCal3f # add priors on all calibrations: for i in range(num_cameras): model = gtsam.noiseModel.Isotropic.Sigma(1, 10.0) graph.addPriorCal3f(K(i), cal, model) - elif method == "CalibratedEssentialMatrix": + elif method == "Calibrated": FactorClass = gtsam.TransferFactorEssentialMatrix # No priors on calibration needed else: raise ValueError(f"Unknown method {method}") - if method == "CalibratedEssentialMatrix": + if method == "Calibrated": # Calibrate measurements using ground truth calibration z = [[cal.calibrate(m) for m in cam_measurements] for cam_measurements in measurements] else: @@ -150,7 +150,7 @@ def get_initial_estimate(method, num_cameras, ground_truth, cal): initialEstimate = Values() total_dimension = 0 - if method in ["FundamentalMatrix", "SimpleFundamentalMatrix"]: + if method in ["Fundamental", "SimpleF"]: F1, F2 = ground_truth for a in range(num_cameras): b = (a + 1) % num_cameras # Next camera @@ -158,7 +158,7 @@ def get_initial_estimate(method, num_cameras, ground_truth, cal): initialEstimate.insert(EdgeKey(a, b).key(), F1) initialEstimate.insert(EdgeKey(a, c).key(), F2) total_dimension += F1.dim() + F2.dim() - elif method in ["EssentialMatrix", "CalibratedEssentialMatrix"]: + elif method in ["Essential+Ks", "Calibrated"]: E1, E2 = ground_truth for a in range(num_cameras): b = (a + 1) % num_cameras # Next camera @@ -169,7 +169,7 @@ def get_initial_estimate(method, num_cameras, ground_truth, cal): else: raise ValueError(f"Unknown method {method}") - if method == "EssentialMatrix": + if method == "Essential+Ks": # Insert initial calibrations for i in range(num_cameras): initialEstimate.insert(K(i), cal) @@ -196,7 +196,7 @@ def compute_distances(method, result, ground_truth, num_cameras, cal): """Compute geodesic distances from ground truth""" distances = [] - F1, F2 = ground_truth["FundamentalMatrix"] + F1, F2 = ground_truth["Fundamental"] for a in range(num_cameras): b = (a + 1) % num_cameras @@ -204,20 +204,20 @@ def compute_distances(method, result, ground_truth, num_cameras, cal): key_ab = EdgeKey(a, b).key() key_ac = EdgeKey(a, c).key() - if method in ["EssentialMatrix", "CalibratedEssentialMatrix"]: + if method in ["Essential+Ks", "Calibrated"]: E_est_ab = result.atEssentialMatrix(key_ab) E_est_ac = result.atEssentialMatrix(key_ac) # Compute estimated FundamentalMatrices - if method == "FundamentalMatrix": + if method == "Fundamental": F_est_ab = result.atFundamentalMatrix(key_ab) F_est_ac = result.atFundamentalMatrix(key_ac) - elif method == "SimpleFundamentalMatrix": + elif method == "SimpleF": SF_est_ab = result.atSimpleFundamentalMatrix(key_ab).matrix() SF_est_ac = result.atSimpleFundamentalMatrix(key_ac).matrix() F_est_ab = FundamentalMatrix(SF_est_ab) F_est_ac = FundamentalMatrix(SF_est_ac) - elif method == "EssentialMatrix": + elif method == "Essential+Ks": # Retrieve calibrations from result: cal_a = result.atCal3f(K(a)) cal_b = result.atCal3f(K(b)) @@ -226,7 +226,7 @@ def compute_distances(method, result, ground_truth, num_cameras, cal): # Convert estimated EssentialMatrices to FundamentalMatrices F_est_ab = FundamentalMatrix(cal_a.K(), E_est_ab, cal_b.K()) F_est_ac = FundamentalMatrix(cal_a.K(), E_est_ac, cal_c.K()) - elif method == "CalibratedEssentialMatrix": + elif method == "Calibrated": # Use ground truth calibration F_est_ab = FundamentalMatrix(cal.K(), E_est_ab, cal.K()) F_est_ac = FundamentalMatrix(cal.K(), E_est_ac, cal.K()) @@ -322,7 +322,7 @@ def main(): # Assert that the initial error is the same for all methods: if method == methods[0]: error0 = graph.error(initial_estimate[method]) - elif method == "CalibratedEssentialMatrix": + elif method == "Calibrated": current_error = graph.error(initial_estimate[method]) * cal.f() * cal.f() print(error0, current_error) assert np.allclose(error0, current_error), "Initial errors do not match among methods." @@ -338,7 +338,7 @@ def main(): # Compute final error final_error = graph.error(result) - if method == "CalibratedEssentialMatrix": + if method == "Calibrated": final_error *= cal.f() * cal.f() # Store results From c68858d7b672500f237f5c0b74c21384fbb96359 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Tue, 29 Oct 2024 11:55:55 -0700 Subject: [PATCH 32/34] Get rid of scale --- gtsam/geometry/FundamentalMatrix.cpp | 4 ++-- gtsam/geometry/FundamentalMatrix.h | 4 +--- 2 files changed, 3 insertions(+), 5 deletions(-) diff --git a/gtsam/geometry/FundamentalMatrix.cpp b/gtsam/geometry/FundamentalMatrix.cpp index 94c72673d..406269ff5 100644 --- a/gtsam/geometry/FundamentalMatrix.cpp +++ b/gtsam/geometry/FundamentalMatrix.cpp @@ -59,7 +59,7 @@ FundamentalMatrix::FundamentalMatrix(const Matrix3& F) { void FundamentalMatrix::initialize(const Matrix3& U, double s, const Matrix3& V) { - s_ = s / kScale; + s_ = s; sign_ = 1.0; // Check if U is a reflection and flip U and sign_ if so @@ -82,7 +82,7 @@ void FundamentalMatrix::initialize(const Matrix3& U, double s, } Matrix3 FundamentalMatrix::matrix() const { - return sign_ * U_.matrix() * Vector3(1.0, s_ * kScale, 0).asDiagonal() * + return sign_ * U_.matrix() * Vector3(1.0, s_, 0).asDiagonal() * V_.transpose().matrix(); } diff --git a/gtsam/geometry/FundamentalMatrix.h b/gtsam/geometry/FundamentalMatrix.h index a36c815da..beb2947e2 100644 --- a/gtsam/geometry/FundamentalMatrix.h +++ b/gtsam/geometry/FundamentalMatrix.h @@ -34,11 +34,9 @@ class GTSAM_EXPORT FundamentalMatrix { double s_; ///< Scalar parameter for S Rot3 V_; ///< Right rotation - static constexpr double kScale = 1000; // s is stored in s_ as s/kScale - public: /// Default constructor - FundamentalMatrix() : U_(Rot3()), sign_(1.0), s_(1.0 / kScale), V_(Rot3()) {} + FundamentalMatrix() : U_(Rot3()), sign_(1.0), s_(1.0), V_(Rot3()) {} /** * @brief Construct from U, V, and scalar s From 8af0465d92f58c2f4a02443ecb83fe8a4741e407 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Tue, 29 Oct 2024 11:56:07 -0700 Subject: [PATCH 33/34] Median/plotting/initF --- python/gtsam/examples/ViewGraphComparison.py | 49 +++++++++++--------- 1 file changed, 28 insertions(+), 21 deletions(-) diff --git a/python/gtsam/examples/ViewGraphComparison.py b/python/gtsam/examples/ViewGraphComparison.py index 968bbc9df..d8340e393 100644 --- a/python/gtsam/examples/ViewGraphComparison.py +++ b/python/gtsam/examples/ViewGraphComparison.py @@ -32,7 +32,7 @@ from gtsam import ( K = gtsam.symbol_shorthand.K # Methods to compare -methods = ["Fundamental", "SimpleF", "Essential+Ks", "Calibrated"] +methods = ["SimpleF", "Fundamental", "Essential+Ks", "Calibrated"] # Formatter function for printing keys @@ -253,13 +253,14 @@ def plot_results(results): color = "tab:red" ax1.set_xlabel("Method") - ax1.set_ylabel("Final Error", color=color) + ax1.set_ylabel("Median Error (log scale)", color=color) + ax1.set_yscale("log") ax1.bar(methods, final_errors, color=color, alpha=0.6) ax1.tick_params(axis="y", labelcolor=color) ax2 = ax1.twinx() color = "tab:blue" - ax2.set_ylabel("Mean Geodesic Distance", color=color) + ax2.set_ylabel("Median Geodesic Distance", color=color) ax2.plot(methods, distances, color=color, marker="o", linestyle="-") ax2.tick_params(axis="y", labelcolor=color) @@ -278,14 +279,13 @@ def plot_results(results): fig.tight_layout() plt.show() - # Main function def main(): # Parse command line arguments parser = argparse.ArgumentParser(description="Compare Fundamental and Essential Matrix Methods") parser.add_argument("--num_cameras", type=int, default=4, help="Number of cameras (default: 4)") parser.add_argument("--num_extra_points", type=int, default=12, help="Number of extra random points (default: 12)") - parser.add_argument("--nr_trials", type=int, default=5, help="Number of trials (default: 5)") + parser.add_argument("--num_trials", type=int, default=5, help="Number of trials (default: 5)") parser.add_argument("--seed", type=int, default=42, help="Random seed (default: 42)") parser.add_argument("--noise_std", type=float, default=0.5, help="Standard deviation of noise (default: 0.5)") args = parser.parse_args() @@ -303,12 +303,13 @@ def main(): ground_truth = {method: compute_ground_truth(method, poses, cal) for method in methods} # Get initial estimates - initial_estimate = { + initial_estimate: dict[Values] = { method: get_initial_estimate(method, args.num_cameras, ground_truth[method], cal) for method in methods } + simple_f_result: Values = Values() - for trial in range(args.nr_trials): - print(f"\nTrial {trial + 1}/{args.nr_trials}") + for trial in range(args.num_trials): + print(f"\nTrial {trial + 1}/{args.num_trials}") # Simulate data measurements = simulate_data(points, poses, cal, rng, args.noise_std) @@ -319,20 +320,26 @@ def main(): # Build the factor graph graph = build_factor_graph(method, args.num_cameras, measurements, cal) - # Assert that the initial error is the same for all methods: - if method == methods[0]: - error0 = graph.error(initial_estimate[method]) - elif method == "Calibrated": - current_error = graph.error(initial_estimate[method]) * cal.f() * cal.f() - print(error0, current_error) - assert np.allclose(error0, current_error), "Initial errors do not match among methods." - else: - current_error = graph.error(initial_estimate[method]) - assert np.allclose(error0, current_error), "Initial errors do not match among methods." + # For F, initialize from SimpleF: + if method == "Fundamental": + initial_estimate[method] = simple_f_result # Optimize the graph result, iterations = optimize(graph, initial_estimate[method], method) + # Store SimpleF result as a set of FundamentalMatrices + if method == "SimpleF": + simple_f_result = Values() + for a in range(args.num_cameras): + b = (a + 1) % args.num_cameras # Next camera + c = (a + 2) % args.num_cameras # Camera after next + key_ab = EdgeKey(a, b).key() + key_ac = EdgeKey(a, c).key() + F1 = result.atSimpleFundamentalMatrix(key_ab).matrix() + F2 = result.atSimpleFundamentalMatrix(key_ac).matrix() + simple_f_result.insert(key_ab, FundamentalMatrix(F1)) + simple_f_result.insert(key_ac, FundamentalMatrix(F2)) + # Compute distances from ground truth distances = compute_distances(method, result, ground_truth, args.num_cameras, cal) @@ -353,9 +360,9 @@ def main(): # Average results over trials for method in methods: - results[method]["final_error"] = np.mean(results[method]["final_error"]) - results[method]["distances"] = np.mean(results[method]["distances"]) - results[method]["iterations"] = np.mean(results[method]["iterations"]) + results[method]["final_error"] = np.median(results[method]["final_error"]) + results[method]["distances"] = np.median(results[method]["distances"]) + results[method]["iterations"] = np.median(results[method]["iterations"]) # Plot results plot_results(results) From be9195465ee40723683b8af3c7699f726e472aa4 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Tue, 5 Nov 2024 15:55:06 -0500 Subject: [PATCH 34/34] Differentiated EssentialTransferFactor and EssentialTransferFactorK --- examples/EssentialViewGraphExample.cpp | 4 +- gtsam/sfm/TransferFactor.h | 106 +++++++++++++++++-- gtsam/sfm/sfm.i | 18 +++- gtsam/sfm/tests/testTransferFactor.cpp | 56 +++++++++- python/gtsam/examples/ViewGraphComparison.py | 24 ++--- 5 files changed, 174 insertions(+), 34 deletions(-) diff --git a/examples/EssentialViewGraphExample.cpp b/examples/EssentialViewGraphExample.cpp index e21a36e16..205708c62 100644 --- a/examples/EssentialViewGraphExample.cpp +++ b/examples/EssentialViewGraphExample.cpp @@ -27,7 +27,7 @@ #include #include #include -#include // Contains EssentialTransferFactor +#include // Contains EssentialTransferFactorK #include @@ -63,7 +63,7 @@ int main(int argc, char* argv[]) { // Create the factor graph NonlinearFactorGraph graph; - using Factor = EssentialTransferFactor; + using Factor = EssentialTransferFactorK; for (size_t a = 0; a < 4; ++a) { size_t b = (a + 1) % 4; // Next camera diff --git a/gtsam/sfm/TransferFactor.h b/gtsam/sfm/TransferFactor.h index 2964c01ad..c8b249ddc 100644 --- a/gtsam/sfm/TransferFactor.h +++ b/gtsam/sfm/TransferFactor.h @@ -59,7 +59,7 @@ class TransferEdges { return edge1.j(); else throw std::runtime_error( - "EssentialTransferFactor: No common key in edge keys."); + "EssentialTransferFactorK: No common key in edge keys."); } /// Create Matrix3 objects based on EdgeKey configurations. @@ -85,8 +85,11 @@ class TransferEdges { */ template class TransferFactor : public NoiseModelFactorN, public TransferEdges { + public: using Base = NoiseModelFactorN; using Triplet = std::tuple; + + protected: std::vector triplets_; ///< Point triplets. public: @@ -130,6 +133,81 @@ class TransferFactor : public NoiseModelFactorN, public TransferEdges { /** * @class EssentialTransferFactor + * @brief Transfers points between views using essential matrices with a shared + * calibration. + * + * This factor is templated on the calibration class K and extends + * the TransferFactor for EssentialMatrices. It involves two essential matrices + * and a shared calibration object (K). The evaluateError function calibrates + * the image points, calls the base class's transfer method, and computes the + * error using bulk numerical differentiation. + */ +template +class EssentialTransferFactor : public TransferFactor { + using EM = EssentialMatrix; + using Triplet = std::tuple; + std::shared_ptr calibration_; ///< Shared pointer to calibration object + + public: + using Base = TransferFactor; + using This = EssentialTransferFactor; + using shared_ptr = std::shared_ptr; + + /** + * @brief Constructor that accepts a vector of point triplets and a shared + * calibration. + * + * @param edge1 First EdgeKey specifying E1: (a, c) or (c, a) + * @param edge2 Second EdgeKey specifying E2: (b, c) or (c, b) + * @param triplets A vector of triplets containing (pa, pb, pc) + * @param calibration Shared pointer to calibration object + * @param model An optional SharedNoiseModel + */ + EssentialTransferFactor(EdgeKey edge1, EdgeKey edge2, + const std::vector& triplets, + const std::shared_ptr& calibration, + const SharedNoiseModel& model = nullptr) + : Base(edge1, edge2, triplets, model), calibration_(calibration) {} + + /// Transfer points pa and pb to view c and evaluate error. + Vector2 TransferError(const Matrix3& Eca, const Point2& pa, + const Matrix3& Ecb, const Point2& pb, + const Point2& pc) const { + const Point2 pA = calibration_->calibrate(pa); + const Point2 pB = calibration_->calibrate(pb); + const Point2 pC = EpipolarTransfer(Eca, pA, Ecb, pB); + return calibration_->uncalibrate(pC) - pc; + } + + /// Evaluate error function + Vector evaluateError(const EM& E1, const EM& E2, + OptionalMatrixType H1 = nullptr, + OptionalMatrixType H2 = nullptr) const override { + std::function errorFunction = + [&](const EM& e1, const EM& e2) { + Vector errors(2 * this->triplets_.size()); + size_t idx = 0; + auto [Eca, Ecb] = this->getMatrices(e1, e2); + for (const auto& [pa, pb, pc] : this->triplets_) { + errors.segment<2>(idx) = TransferError(Eca, pa, Ecb, pb, pc); + idx += 2; + } + return errors; + }; + + // Compute error + Vector errors = errorFunction(E1, E2); + + // Compute Jacobians if requested + if (H1) *H1 = numericalDerivative21(errorFunction, E1, E2); + if (H2) *H2 = numericalDerivative22(errorFunction, E1, E2); + + return errors; + } +}; + +/** + * @class EssentialTransferFactorK * @brief Transfers points between views using essential matrices, optimizes for * calibrations of the views, as well. Note that the EssentialMatrixFactor4 does * something similar but without transfer. @@ -143,7 +221,7 @@ class TransferFactor : public NoiseModelFactorN, public TransferEdges { * and computes the error using bulk numerical differentiation. */ template -class EssentialTransferFactor +class EssentialTransferFactorK : public NoiseModelFactorN, TransferEdges { using EM = EssentialMatrix; @@ -152,7 +230,7 @@ class EssentialTransferFactor public: using Base = NoiseModelFactorN; - using This = EssentialTransferFactor; + using This = EssentialTransferFactorK; using shared_ptr = std::shared_ptr; /** @@ -163,9 +241,9 @@ class EssentialTransferFactor * @param triplets A vector of triplets containing (pa, pb, pc) * @param model An optional SharedNoiseModel */ - EssentialTransferFactor(EdgeKey edge1, EdgeKey edge2, - const std::vector& triplets, - const SharedNoiseModel& model = nullptr) + EssentialTransferFactorK(EdgeKey edge1, EdgeKey edge2, + const std::vector& triplets, + const SharedNoiseModel& model = nullptr) : Base(model, edge1, edge2, Symbol('k', ViewA(edge1, edge2)), // calibration key for view a Symbol('k', ViewB(edge1, edge2)), // calibration key for view b @@ -173,6 +251,16 @@ class EssentialTransferFactor TransferEdges(edge1, edge2), triplets_(triplets) {} + /// Transfer points pa and pb to view c and evaluate error. + Vector2 TransferError(const Matrix3& Eca, const K& Ka, const Point2& pa, + const Matrix3& Ecb, const K& Kb, const Point2& pb, + const K& Kc, const Point2& pc) const { + const Point2 pA = Ka.calibrate(pa); + const Point2 pB = Kb.calibrate(pb); + const Point2 pC = EpipolarTransfer(Eca, pA, Ecb, pB); + return Kc.uncalibrate(pC) - pc; + } + /// Evaluate error function Vector evaluateError(const EM& E1, const EM& E2, const K& Ka, const K& Kb, const K& Kc, OptionalMatrixType H1 = nullptr, @@ -187,10 +275,8 @@ class EssentialTransferFactor size_t idx = 0; auto [Eca, Ecb] = this->getMatrices(e1, e2); for (const auto& [pa, pb, pc] : triplets_) { - const Point2 pA = kA.calibrate(pa); - const Point2 pB = kB.calibrate(pb); - const Point2 pC = EpipolarTransfer(Eca, pA, Ecb, pB); - errors.segment<2>(idx) = kC.uncalibrate(pC) - pc; + errors.segment<2>(idx) = + TransferError(Eca, kA, pa, Ecb, kB, pb, kC, pc); idx += 2; } return errors; diff --git a/gtsam/sfm/sfm.i b/gtsam/sfm/sfm.i index 80479eb78..a15b73ea1 100644 --- a/gtsam/sfm/sfm.i +++ b/gtsam/sfm/sfm.i @@ -77,11 +77,11 @@ gtsam::Values initialCamerasAndPointsEstimate(const gtsam::SfmData& db); #include #include -template +template virtual class TransferFactor : gtsam::NoiseModelFactor { TransferFactor(gtsam::EdgeKey edge1, gtsam::EdgeKey edge2, - const std::vector>& triplets - ); + const std::vector>& triplets, + const gtsam::noiseModel::Base* model = nullptr); }; #include @@ -90,8 +90,16 @@ virtual class TransferFactor : gtsam::NoiseModelFactor { template virtual class EssentialTransferFactor : gtsam::NoiseModelFactor { EssentialTransferFactor(gtsam::EdgeKey edge1, gtsam::EdgeKey edge2, - const std::vector>& triplets - ); + const std::vector>& triplets, + const K* calibration, + const gtsam::noiseModel::Base* model = nullptr); +}; + +template +virtual class EssentialTransferFactorK : gtsam::NoiseModelFactor { + EssentialTransferFactorK(gtsam::EdgeKey edge1, gtsam::EdgeKey edge2, + const std::vector>& triplets, + const gtsam::noiseModel::Base* model = nullptr); }; #include diff --git a/gtsam/sfm/tests/testTransferFactor.cpp b/gtsam/sfm/tests/testTransferFactor.cpp index b1655e0a9..ced3d2ce7 100644 --- a/gtsam/sfm/tests/testTransferFactor.cpp +++ b/gtsam/sfm/tests/testTransferFactor.cpp @@ -6,10 +6,13 @@ */ #include +#include #include #include #include +#include + using namespace gtsam; using symbol_shorthand::K; @@ -173,7 +176,51 @@ TEST(EssentialTransferFactor, Jacobians) { EdgeKey key02(0, 2); // Create an EssentialTransferFactor - EssentialTransferFactor factor(key01, key02, {{p[1], p[2], p[0]}}); + auto sharedCal = std::make_shared(cal); + EssentialTransferFactor factor(key01, key02, {{p[1], p[2], p[0]}}, + sharedCal); + + // Create Values and insert variables + Values values; + values.insert(key01, E01); // Essential matrix between views 0 and 1 + values.insert(key02, E02); // Essential matrix between views 0 and 2 + + // Check error + Matrix H1, H2, H3, H4, H5; + Vector error = factor.evaluateError(E01, E02, // + &H1, &H2); + EXPECT(H1.rows() == 2 && H1.cols() == 5) + EXPECT(H2.rows() == 2 && H2.cols() == 5) + EXPECT(assert_equal(Vector::Zero(2), error, 1e-9)) + + // Check Jacobians + EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-5, 1e-7); +} + +//************************************************************************* +// Test for EssentialTransferFactorK +TEST(EssentialTransferFactorK, Jacobians) { + using namespace fixture; + + // Create EssentialMatrices between cameras + EssentialMatrix E01 = + EssentialMatrix::FromPose3(cameraPoses[0].between(cameraPoses[1])); + EssentialMatrix E02 = + EssentialMatrix::FromPose3(cameraPoses[0].between(cameraPoses[2])); + + // Project a point into the three cameras + const Point3 P(0.1, 0.2, 0.3); + std::array p; + for (size_t i = 0; i < 3; ++i) { + p[i] = cameras[i].project(P); + } + + // Create EdgeKeys + EdgeKey key01(0, 1); + EdgeKey key02(0, 2); + + // Create an EssentialTransferFactorK + EssentialTransferFactorK factor(key01, key02, {{p[1], p[2], p[0]}}); // Create Values and insert variables Values values; @@ -185,10 +232,9 @@ TEST(EssentialTransferFactor, Jacobians) { // Check error Matrix H1, H2, H3, H4, H5; - Vector error = - factor.evaluateError(E01, E02, // - fixture::cal, fixture::cal, fixture::cal, // - &H1, &H2, &H3, &H4, &H5); + Vector error = factor.evaluateError(E01, E02, // + cal, cal, cal, // + &H1, &H2, &H3, &H4, &H5); EXPECT(H1.rows() == 2 && H1.cols() == 5) EXPECT(H2.rows() == 2 && H2.cols() == 5) EXPECT(H3.rows() == 2 && H3.cols() == 5) diff --git a/python/gtsam/examples/ViewGraphComparison.py b/python/gtsam/examples/ViewGraphComparison.py index d8340e393..1eb43cec8 100644 --- a/python/gtsam/examples/ViewGraphComparison.py +++ b/python/gtsam/examples/ViewGraphComparison.py @@ -105,22 +105,18 @@ def build_factor_graph(method, num_cameras, measurements, cal): elif method == "SimpleF": FactorClass = gtsam.TransferFactorSimpleFundamentalMatrix elif method == "Essential+Ks": - FactorClass = gtsam.EssentialTransferFactorCal3f + FactorClass = gtsam.EssentialTransferFactorKCal3f # add priors on all calibrations: for i in range(num_cameras): model = gtsam.noiseModel.Isotropic.Sigma(1, 10.0) graph.addPriorCal3f(K(i), cal, model) elif method == "Calibrated": - FactorClass = gtsam.TransferFactorEssentialMatrix + FactorClass = gtsam.EssentialTransferFactorCal3f # No priors on calibration needed else: raise ValueError(f"Unknown method {method}") - if method == "Calibrated": - # Calibrate measurements using ground truth calibration - z = [[cal.calibrate(m) for m in cam_measurements] for cam_measurements in measurements] - else: - z = measurements + z = measurements # shorthand for a in range(num_cameras): b = (a + 1) % num_cameras # Next camera @@ -138,9 +134,14 @@ def build_factor_graph(method, num_cameras, measurements, cal): tuples3.append((z[c][j], z[b][j], z[a][j])) # Add transfer factors between views a, b, and c. - graph.add(FactorClass(EdgeKey(a, c), EdgeKey(b, c), tuples1)) - graph.add(FactorClass(EdgeKey(a, b), EdgeKey(b, c), tuples2)) - graph.add(FactorClass(EdgeKey(a, c), EdgeKey(a, b), tuples3)) + if method in ["Calibrated"]: + graph.add(FactorClass(EdgeKey(a, c), EdgeKey(b, c), tuples1, cal)) + graph.add(FactorClass(EdgeKey(a, b), EdgeKey(b, c), tuples2, cal)) + graph.add(FactorClass(EdgeKey(a, c), EdgeKey(a, b), tuples3, cal)) + else: + graph.add(FactorClass(EdgeKey(a, c), EdgeKey(b, c), tuples1)) + graph.add(FactorClass(EdgeKey(a, b), EdgeKey(b, c), tuples2)) + graph.add(FactorClass(EdgeKey(a, c), EdgeKey(a, b), tuples3)) return graph @@ -279,6 +280,7 @@ def plot_results(results): fig.tight_layout() plt.show() + # Main function def main(): # Parse command line arguments @@ -345,8 +347,6 @@ def main(): # Compute final error final_error = graph.error(result) - if method == "Calibrated": - final_error *= cal.f() * cal.f() # Store results results[method]["distances"].extend(distances)