From cd46bfa36311c25219251aab0a8e3370c670981f Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Mon, 30 Sep 2024 13:44:09 -0700 Subject: [PATCH 001/115] Formatting and comments --- gtsam/navigation/AttitudeFactor.h | 152 +++++++++++++++--------------- 1 file changed, 74 insertions(+), 78 deletions(-) diff --git a/gtsam/navigation/AttitudeFactor.h b/gtsam/navigation/AttitudeFactor.h index d61db21669..676e0a46d8 100644 --- a/gtsam/navigation/AttitudeFactor.h +++ b/gtsam/navigation/AttitudeFactor.h @@ -17,59 +17,61 @@ **/ #pragma once -#include #include #include +#include namespace gtsam { /** - * Base class for prior on attitude - * Example: - * - measurement is direction of gravity in body frame bF - * - reference is direction of gravity in navigation frame nG - * This factor will give zero error if nRb * bF == nG + * @class AttitudeFactor + * + * @brief Base class for an attitude factor that constrains the rotation between + * body and navigation frames. + * + * This factor enforces that when a known reference direction in the body frame + * (e.g., accelerometer axis) is rotated into the navigation frame using the + * rotation variable, it should align with a measured direction in the + * navigation frame (e.g., gravity vector). + * + * Mathematically, the error is zero when: + * nRb * bRef == nZ + * + * This is useful for incorporating absolute orientation measurements into the + * factor graph. + * * @ingroup navigation */ class AttitudeFactor { + protected: + Unit3 nZ_, bRef_; ///< Position measurement in -protected: - - Unit3 nZ_, bRef_; ///< Position measurement in - -public: - + public: /** default constructor - only use for serialization */ - AttitudeFactor() { - } + AttitudeFactor() {} /** * @brief Constructor - * @param nZ measured direction in navigation frame - * @param bRef reference direction in body frame (default Z-axis in NED frame, i.e., [0; 0; 1]) + * @param nZ Measured direction in the navigation frame (e.g., gravity). + * @param bRef Reference direction in the body frame (e.g., accelerometer + * axis). Default is Unit3(0, 0, 1). */ - AttitudeFactor(const Unit3& nZ, const Unit3& bRef = Unit3(0, 0, 1)) : - nZ_(nZ), bRef_(bRef) { - } + AttitudeFactor(const Unit3& nZ, const Unit3& bRef = Unit3(0, 0, 1)) + : nZ_(nZ), bRef_(bRef) {} /** vector of errors */ - Vector attitudeError(const Rot3& p, - OptionalJacobian<2,3> H = {}) const; + Vector attitudeError(const Rot3& p, OptionalJacobian<2, 3> H = {}) const; - const Unit3& nZ() const { - return nZ_; - } - const Unit3& bRef() const { - return bRef_; - } + const Unit3& nZ() const { return nZ_; } + const Unit3& bRef() const { return bRef_; } #ifdef GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; - template - void serialize(ARCHIVE & ar, const unsigned int /*version*/) { - ar & boost::serialization::make_nvp("nZ_", nZ_); - ar & boost::serialization::make_nvp("bRef_", bRef_); + template + void serialize(ARCHIVE& ar, const unsigned int /*version*/) { + ar& boost::serialization::make_nvp("nZ_", nZ_); + ar& boost::serialization::make_nvp("bRef_", bRef_); } #endif }; @@ -78,12 +80,11 @@ class AttitudeFactor { * Version of AttitudeFactor for Rot3 * @ingroup navigation */ -class GTSAM_EXPORT Rot3AttitudeFactor: public NoiseModelFactorN, public AttitudeFactor { - +class GTSAM_EXPORT Rot3AttitudeFactor : public NoiseModelFactorN, + public AttitudeFactor { typedef NoiseModelFactorN Base; -public: - + public: // Provide access to the Matrix& version of evaluateError: using Base::evaluateError; @@ -94,11 +95,9 @@ class GTSAM_EXPORT Rot3AttitudeFactor: public NoiseModelFactorN, public At typedef Rot3AttitudeFactor This; /** default constructor - only use for serialization */ - Rot3AttitudeFactor() { - } + Rot3AttitudeFactor() {} - ~Rot3AttitudeFactor() override { - } + ~Rot3AttitudeFactor() override {} /** * @brief Constructor @@ -108,9 +107,8 @@ class GTSAM_EXPORT Rot3AttitudeFactor: public NoiseModelFactorN, public At * @param bRef reference direction in body frame (default Z-axis) */ Rot3AttitudeFactor(Key key, const Unit3& nZ, const SharedNoiseModel& model, - const Unit3& bRef = Unit3(0, 0, 1)) : - Base(model, key), AttitudeFactor(nZ, bRef) { - } + const Unit3& bRef = Unit3(0, 0, 1)) + : Base(model, key), AttitudeFactor(nZ, bRef) {} /// @return a deep copy of this factor gtsam::NonlinearFactor::shared_ptr clone() const override { @@ -123,46 +121,46 @@ class GTSAM_EXPORT Rot3AttitudeFactor: public NoiseModelFactorN, public At DefaultKeyFormatter) const override; /** equals */ - bool equals(const NonlinearFactor& expected, double tol = 1e-9) const override; + bool equals(const NonlinearFactor& expected, + double tol = 1e-9) const override; /** vector of errors */ Vector evaluateError(const Rot3& nRb, OptionalMatrixType H) const override { return attitudeError(nRb, H); } -private: - + private: #ifdef GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; - template - void serialize(ARCHIVE & ar, const unsigned int /*version*/) { + template + void serialize(ARCHIVE& ar, const unsigned int /*version*/) { // NoiseModelFactor1 instead of NoiseModelFactorN for backward compatibility - ar & boost::serialization::make_nvp("NoiseModelFactor1", - boost::serialization::base_object(*this)); - ar & boost::serialization::make_nvp("AttitudeFactor", + ar& boost::serialization::make_nvp( + "NoiseModelFactor1", boost::serialization::base_object(*this)); + ar& boost::serialization::make_nvp( + "AttitudeFactor", boost::serialization::base_object(*this)); } #endif -public: + public: GTSAM_MAKE_ALIGNED_OPERATOR_NEW }; /// traits -template<> struct traits : public Testable {}; +template <> +struct traits : public Testable {}; /** * Version of AttitudeFactor for Pose3 * @ingroup navigation */ -class GTSAM_EXPORT Pose3AttitudeFactor: public NoiseModelFactorN, - public AttitudeFactor { - +class GTSAM_EXPORT Pose3AttitudeFactor : public NoiseModelFactorN, + public AttitudeFactor { typedef NoiseModelFactorN Base; -public: - + public: // Provide access to the Matrix& version of evaluateError: using Base::evaluateError; @@ -173,11 +171,9 @@ class GTSAM_EXPORT Pose3AttitudeFactor: public NoiseModelFactorN, typedef Pose3AttitudeFactor This; /** default constructor - only use for serialization */ - Pose3AttitudeFactor() { - } + Pose3AttitudeFactor() {} - ~Pose3AttitudeFactor() override { - } + ~Pose3AttitudeFactor() override {} /** * @brief Constructor @@ -187,9 +183,8 @@ class GTSAM_EXPORT Pose3AttitudeFactor: public NoiseModelFactorN, * @param bRef reference direction in body frame (default Z-axis) */ Pose3AttitudeFactor(Key key, const Unit3& nZ, const SharedNoiseModel& model, - const Unit3& bRef = Unit3(0, 0, 1)) : - Base(model, key), AttitudeFactor(nZ, bRef) { - } + const Unit3& bRef = Unit3(0, 0, 1)) + : Base(model, key), AttitudeFactor(nZ, bRef) {} /// @return a deep copy of this factor gtsam::NonlinearFactor::shared_ptr clone() const override { @@ -202,40 +197,41 @@ class GTSAM_EXPORT Pose3AttitudeFactor: public NoiseModelFactorN, DefaultKeyFormatter) const override; /** equals */ - bool equals(const NonlinearFactor& expected, double tol = 1e-9) const override; + bool equals(const NonlinearFactor& expected, + double tol = 1e-9) const override; /** vector of errors */ Vector evaluateError(const Pose3& nTb, OptionalMatrixType H) const override { Vector e = attitudeError(nTb.rotation(), H); if (H) { Matrix H23 = *H; - *H = Matrix::Zero(2,6); - H->block<2,3>(0,0) = H23; + *H = Matrix::Zero(2, 6); + H->block<2, 3>(0, 0) = H23; } return e; } -private: - + private: #ifdef GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; - template - void serialize(ARCHIVE & ar, const unsigned int /*version*/) { + template + void serialize(ARCHIVE& ar, const unsigned int /*version*/) { // NoiseModelFactor1 instead of NoiseModelFactorN for backward compatibility - ar & boost::serialization::make_nvp("NoiseModelFactor1", - boost::serialization::base_object(*this)); - ar & boost::serialization::make_nvp("AttitudeFactor", + ar& boost::serialization::make_nvp( + "NoiseModelFactor1", boost::serialization::base_object(*this)); + ar& boost::serialization::make_nvp( + "AttitudeFactor", boost::serialization::base_object(*this)); } #endif -public: + public: GTSAM_MAKE_ALIGNED_OPERATOR_NEW }; /// traits -template<> struct traits : public Testable {}; - -} /// namespace gtsam +template <> +struct traits : public Testable {}; +} // namespace gtsam From 81a4062edd24ee874f077d86ce3dcff9f5622e60 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Mon, 30 Sep 2024 13:44:19 -0700 Subject: [PATCH 002/115] Added documentation --- gtsam/navigation/AttitudeFactor.md | 142 +++++++++++++++++++++++++++++ 1 file changed, 142 insertions(+) create mode 100644 gtsam/navigation/AttitudeFactor.md diff --git a/gtsam/navigation/AttitudeFactor.md b/gtsam/navigation/AttitudeFactor.md new file mode 100644 index 0000000000..beb52b0b6e --- /dev/null +++ b/gtsam/navigation/AttitudeFactor.md @@ -0,0 +1,142 @@ +# AttitudeFactor in GTSAM + +[Cautionary note: this was generated from the source using ChatGPT but edited by Frank] + +## Introduction + +The `AttitudeFactor` in GTSAM is a factor that constrains the orientation (attitude) of a robot or sensor platform based on directional measurements. This is particularly useful in GPS-denied navigation contexts where orientation must be estimated from inertial sensors like accelerometers or magnetometers. + +This document explains the mathematical foundation of the `AttitudeFactor` and guides users on how to use this factor effectively in GTSAM. + +## Mathematical Foundation + +### Concept + +The `AttitudeFactor` constrains the rotation $\mathbf{R}_{nb}$ (from body frame $b$ to navigation frame $n$) such that a known reference direction in the body frame aligns with a measured direction in the navigation frame when rotated. The factor enforces that: + +$$ +\mathbf{R}_{nb} \cdot \mathbf{b}_{\text{Ref}} \approx \mathbf{nZ} +$$ + +where: + +- $\mathbf{R}_{nb}$ is the rotation matrix representing the orientation from body to navigation frame. +- $\mathbf{b}_{\text{Ref}}$ is a known reference direction in the body frame (e.g., the accelerometer's sensitive axis). +- $\mathbf{nZ}$ is the measured direction in the navigation frame (e.g., the gravity vector measured by an IMU). + +### Error Function + +The error function computes the angular difference between the rotated reference direction and the measured direction: + +$$ +\mathbf{e} = \text{error}(\mathbf{nZ}, \mathbf{R}_{nb} \cdot \mathbf{b}_{\text{Ref}}) +$$ + +This error is minimal (zero) when the rotated body reference direction aligns perfectly with the measured navigation direction. + +The error is computed internally using the `attitudeError` function: + +```cpp +Vector AttitudeFactor::attitudeError(const Rot3& nRb) const { + Unit3 nRef = nRb * bRef_; + return nZ_.error(nRef); +} +``` + +#### Explanation: +- The function computes the rotated reference direction `nRef` and then the error between `nRef` and the measured direction `nZ`. +- `nZ_.error(nRef)` is a 2D vector-valued error between two directions, defined in [Unit3.h](../geometry/Unit3.h). + +### Jacobians + +For optimization, the $2 \times 3$ Jacobian of the error function with respect to the rotation parameters is required. The Jacobian is computed using chain rule differentiation, involving the derivative of the rotated vector with respect to the rotation parameters and the derivative of the error with respect to the rotated vector. + +## Usage in GTSAM + +### Including the Header + +Include the `AttitudeFactor.h` header in your code: + +```cpp +#include +``` + +### Creating an Attitude Factor + +You can create an attitude factor for either a `Rot3` (rotation only) or a `Pose3` (position and rotation) variable. + +#### For `Rot3` Variables + +```cpp +// Define keys +gtsam::Key rotKey = ...; + +// Measured direction in navigation frame (e.g., gravity) +gtsam::Unit3 nZ(0, 0, -1); // Assuming gravity points down in navigation frame + +// Reference direction in body frame (e.g., accelerometer axis) +gtsam::Unit3 bRef(0, 0, 1); // Default is the Z-axis + +// Noise model +auto noiseModel = gtsam::noiseModel::Isotropic::Sigma(2, 0.1); // 2D error, sigma = 0.1 + +// Add to factor graph +gtsam::NonlinearFactorGraph graph; +graph.emplace_shared(rotKey, nZ, noiseModel, bRef); +``` + +#### For `Pose3` Variables + +There is also a `Pose3AttitudeFactor` that automatically extracts the rotation from the pose, taking into account the chain rule for this operation so the Jacobians with respect to pose are correct. + +```cpp +// Define keys +gtsam::Key poseKey = ...; + +// Measured direction in navigation frame +gtsam::Unit3 nZ(0, 0, -1); + +// Reference direction in body frame +gtsam::Unit3 bRef(0, 0, 1); + +// Noise model +auto noiseModel = gtsam::noiseModel::Isotropic::Sigma(2, 0.1); + +// Add to factor graph +gtsam::NonlinearFactorGraph graph; +graph.emplace_shared(poseKey, nZ, noiseModel, bRef); +``` + +### Explanation of Parameters + +- **Key**: The variable key in the factor graph corresponding to the `Rot3` or `Pose3` variable you are constraining. +- **nZ**: The measured direction in the navigation frame. This is typically obtained from sensors like accelerometers or magnetometers. +- **bRef**: The known reference direction in the body frame. By default, this is set to the Z-axis `[0; 0; 1]`, but it should match the direction your sensor measures. +- **noiseModel**: The noise model representing the uncertainty in the measurement. + +## Practical Tips + +- **Gravity Measurement**: When using an IMU, the accelerometer readings (after removing the dynamic acceleration) can provide the gravity direction in the body frame. +- **Magnetic North Measurement**: A magnetometer can provide the direction of the Earth's magnetic field, which can be used similarly. +- **Reference Direction**: Ensure that `bRef` correctly represents the sensor's measurement axis in the body frame. +- **Noise Model**: The choice of noise model affects the weight of this factor in the optimization. Adjust the standard deviation based on the confidence in your measurements. + +## Example in GPS-Denied Navigation + +In GPS-denied environments, orientation estimation relies heavily on inertial measurements. By incorporating the `AttitudeFactor`, you can: + +- Constrain the roll and pitch angles using gravity measurements from an accelerometer. +- Constrain the yaw angle using magnetic field measurements from a magnetometer (with caution due to magnetic disturbances). + +This factor helps maintain an accurate orientation estimate over time, which is crucial for applications like drone flight, underwater vehicles, or indoor robotics. + +## Conclusion + +The `AttitudeFactor` is a powerful tool in GTSAM for incorporating orientation measurements into your factor graph. By aligning a known reference direction in the body frame with a measured direction in the navigation frame, it provides a constraint that improves the estimation of the rotation variable. Correct understanding and usage of this factor enhance the performance of navigation algorithms, especially in challenging environments where GPS is unavailable. + +Remember to verify the alignment of your reference and measured directions and to adjust the noise model to reflect the reliability of your sensors. + +# References + +- [GTSAM Documentation](https://gtsam.org/) +- [Unit3 Class Reference](https://gtsam.org/doxygen/) \ No newline at end of file From 95742baccce83679b78f75b733501b33b5b4e95b Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Mon, 30 Sep 2024 14:14:46 -0700 Subject: [PATCH 003/115] Added README --- gtsam/navigation/ImuFactor.h | 14 +++++------- gtsam/navigation/README.md | 42 ++++++++++++++++++++++++++++++++++++ 2 files changed, 47 insertions(+), 9 deletions(-) create mode 100644 gtsam/navigation/README.md diff --git a/gtsam/navigation/ImuFactor.h b/gtsam/navigation/ImuFactor.h index 40bc15e111..7254838fd4 100644 --- a/gtsam/navigation/ImuFactor.h +++ b/gtsam/navigation/ImuFactor.h @@ -37,13 +37,9 @@ typedef ManifoldPreintegration PreintegrationType; /* * If you are using the factor, please cite: - * L. Carlone, Z. Kira, C. Beall, V. Indelman, F. Dellaert, "Eliminating - * conditionally independent sets in factor graphs: a unifying perspective based - * on smart factors", Int. Conf. on Robotics and Automation (ICRA), 2014. - * - * C. Forster, L. Carlone, F. Dellaert, D. Scaramuzza, "IMU Preintegration on - * Manifold for Efficient Visual-Inertial Maximum-a-Posteriori Estimation", - * Robotics: Science and Systems (RSS), 2015. + * Christian Forster, Luca Carlone, Frank Dellaert, and Davide Scaramuzza, + * "On-Manifold Preintegration for Real-Time Visual-Inertial Odometry", IEEE + * Transactions on Robotics, 2017. * * REFERENCES: * [1] G.S. Chirikjian, "Stochastic Models, Information Theory, and Lie Groups", @@ -54,8 +50,8 @@ typedef ManifoldPreintegration PreintegrationType; * [3] L. Carlone, S. Williams, R. Roberts, "Preintegrated IMU factor: * Computation of the Jacobian Matrices", Tech. Report, 2013. * Available in this repo as "PreintegratedIMUJacobians.pdf". - * [4] C. Forster, L. Carlone, F. Dellaert, D. Scaramuzza, "IMU Preintegration on - * Manifold for Efficient Visual-Inertial Maximum-a-Posteriori Estimation", + * [4] C. Forster, L. Carlone, F. Dellaert, D. Scaramuzza, "IMU Preintegration + * on Manifold for Efficient Visual-Inertial Maximum-a-Posteriori Estimation", * Robotics: Science and Systems (RSS), 2015. */ diff --git a/gtsam/navigation/README.md b/gtsam/navigation/README.md new file mode 100644 index 0000000000..fe99d8f731 --- /dev/null +++ b/gtsam/navigation/README.md @@ -0,0 +1,42 @@ +# Navigation Factors + +This directory contains factors related to navigation, including various IMU factors. + +## IMU Factor: + +![IMU Factor Diagram](https://www.mathworks.com/help/examples/shared_positioning/win64/FactorGraphPedestrianIMUGPSLocalizationExample_02.png) + +The `ImuFactor` is a 5-ways factor involving previous state (pose and velocity of +the vehicle at previous time step), current state (pose and velocity at +current time step), and the bias estimate. +Following the preintegration +scheme proposed in [2], the `ImuFactor` includes many IMU measurements, which +are "summarized" using the PreintegratedIMUMeasurements class. +The figure above, courtesy of [Mathworks' navigation toolbox](https://www.mathworks.com/help/nav/index.html), which are also using our work, shows the factor graph fragment for two time slices. + +Note that this factor does not model "temporal consistency" of the biases +(which are usually slowly varying quantities), which is up to the caller. +See also `CombinedImuFactor` for a class that does this for you. + +If you are using the factor, please cite: +> Christian Forster, Luca Carlone, Frank Dellaert, and Davide Scaramuzza, "On-Manifold Preintegration for Real-Time Visual-Inertial Odometry", IEEE Transactions on Robotics, 2017. + +## REFERENCES: +1. G.S. Chirikjian, "Stochastic Models, Information Theory, and Lie Groups", + Volume 2, 2008. +2. T. Lupton and S.Sukkarieh, "Visual-Inertial-Aided Navigation for + High-Dynamic Motion in Built Environments Without Initial Conditions", + TRO, 28(1):61-76, 2012. +3. L. Carlone, S. Williams, R. Roberts, "Preintegrated IMU factor: + Computation of the Jacobian Matrices", Tech. Report, 2013. + Available in this repo as "PreintegratedIMUJacobians.pdf". +4. C. Forster, L. Carlone, F. Dellaert, D. Scaramuzza, "IMU Preintegration on + Manifold for Efficient Visual-Inertial Maximum-a-Posteriori Estimation", + Robotics: Science and Systems (RSS), 2015. + +## The Attitude Factor + +The `AttitudeFactor` in GTSAM is a factor that constrains the orientation (attitude) of a robot or sensor platform based on directional measurements. Both `Rot3` and `Pose3` versions are available. + +Written up in detail with the help of ChatGPT [here](AttitudeFactor.md). + From 91d1dd3a9c587a833b517c5708f7bf37b0b5739d Mon Sep 17 00:00:00 2001 From: Jade Turner Date: Tue, 22 Oct 2024 12:38:31 +0800 Subject: [PATCH 004/115] Allow parent project to override options Signed-off-by: Jade Turner --- CMakeLists.txt | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/CMakeLists.txt b/CMakeLists.txt index 66b1804aff..94bbe5c6ce 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,5 +1,11 @@ cmake_minimum_required(VERSION 3.0) +# allow parent project to override options +if (POLICY CMP0077) + cmake_policy(SET CMP0077 NEW) +endif(POLICY CMP0077) + + # Set the version number for the library set (GTSAM_VERSION_MAJOR 4) set (GTSAM_VERSION_MINOR 3) From 8a268d6e13d78e62388e39275db194fce77b47ef Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sun, 27 Oct 2024 17:16:20 -0400 Subject: [PATCH 005/115] improve HybridGaussianISAM test --- gtsam/hybrid/tests/testHybridGaussianISAM.cpp | 22 ++++++++++--------- 1 file changed, 12 insertions(+), 10 deletions(-) diff --git a/gtsam/hybrid/tests/testHybridGaussianISAM.cpp b/gtsam/hybrid/tests/testHybridGaussianISAM.cpp index 11e3194f26..ce5e72dad9 100644 --- a/gtsam/hybrid/tests/testHybridGaussianISAM.cpp +++ b/gtsam/hybrid/tests/testHybridGaussianISAM.cpp @@ -259,27 +259,27 @@ TEST(HybridGaussianElimination, Approx_inference) { /* ****************************************************************************/ // Test approximate inference with an additional pruning step. -TEST(HybridGaussianElimination, IncrementalApproximate) { +TEST_DISABLED(HybridGaussianElimination, IncrementalApproximate) { Switching switching(5); HybridGaussianISAM incrementalHybrid; - HybridGaussianFactorGraph graph1; + HybridGaussianFactorGraph graph; /***** Run Round 1 *****/ // Add the 3 hybrid factors, x0-x1, x1-x2, x2-x3 for (size_t i = 1; i < 4; i++) { - graph1.push_back(switching.linearizedFactorGraph.at(i)); + graph.push_back(switching.linearizedFactorGraph.at(i)); } // Add the Gaussian factors, 1 prior on X(0), + graph.push_back(switching.linearizedFactorGraph.at(0)); // 3 measurements on X(1), X(2), X(3) - graph1.push_back(switching.linearizedFactorGraph.at(0)); for (size_t i = 5; i <= 7; i++) { - graph1.push_back(switching.linearizedFactorGraph.at(i)); + graph.push_back(switching.linearizedFactorGraph.at(i)); } // Run update with pruning size_t maxComponents = 5; - incrementalHybrid.update(graph1); + incrementalHybrid.update(graph); incrementalHybrid.prune(maxComponents); // Check if we have a bayes tree with 4 hybrid nodes, @@ -295,12 +295,14 @@ TEST(HybridGaussianElimination, IncrementalApproximate) { 5, incrementalHybrid[X(3)]->conditional()->asHybrid()->nrComponents()); /***** Run Round 2 *****/ - HybridGaussianFactorGraph graph2; - graph2.push_back(switching.linearizedFactorGraph.at(4)); - graph2.push_back(switching.linearizedFactorGraph.at(8)); + graph.resize(0); // clear all factors + // Add hybrid factor X(3)-X(4) + graph.push_back(switching.linearizedFactorGraph.at(4)); + // Add measurement factor X(4) + graph.push_back(switching.linearizedFactorGraph.at(8)); // Run update with pruning a second time. - incrementalHybrid.update(graph2); + incrementalHybrid.update(graph); incrementalHybrid.prune(maxComponents); // Check if we have a bayes tree with pruned hybrid nodes, From 26a3728d8068e50049a4d70cf5d3cc243d57c8f9 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Mon, 28 Oct 2024 08:55:49 -0700 Subject: [PATCH 006/115] Fix createPoses --- examples/SFMdata.h | 2 +- python/gtsam/examples/SFMExample.py | 2 +- python/gtsam/examples/SFMdata.py | 61 ++++++++++++++----- .../examples/TranslationAveragingExample.py | 3 +- python/gtsam/examples/VisualISAM2Example.py | 2 +- python/gtsam/examples/VisualISAMExample.py | 2 +- 6 files changed, 52 insertions(+), 20 deletions(-) diff --git a/examples/SFMdata.h b/examples/SFMdata.h index f2561b4900..2a86aa5932 100644 --- a/examples/SFMdata.h +++ b/examples/SFMdata.h @@ -56,7 +56,7 @@ std::vector createPoints() { /** * Create a set of ground-truth poses - * Default values give a circular trajectory, radius 30 at pi/4 intervals, + * Default values give a circular trajectory, radius 30 at pi/4 intervals, * always facing the circle center */ std::vector createPoses( diff --git a/python/gtsam/examples/SFMExample.py b/python/gtsam/examples/SFMExample.py index 87bb3cb871..c8d1f1271c 100644 --- a/python/gtsam/examples/SFMExample.py +++ b/python/gtsam/examples/SFMExample.py @@ -62,7 +62,7 @@ def main(): points = SFMdata.createPoints() # Create the set of ground-truth poses - poses = SFMdata.createPoses(K) + poses = SFMdata.createPoses() # Create a factor graph graph = NonlinearFactorGraph() diff --git a/python/gtsam/examples/SFMdata.py b/python/gtsam/examples/SFMdata.py index ad414a256d..1ce7430f60 100644 --- a/python/gtsam/examples/SFMdata.py +++ b/python/gtsam/examples/SFMdata.py @@ -3,14 +3,14 @@ - The landmarks form a 10 meter cube - The robot rotates around the landmarks, always facing towards the cube """ + # pylint: disable=invalid-name, E1101 from typing import List import numpy as np -import gtsam -from gtsam import Cal3_S2, Point3, Pose3 +from gtsam import Point3, Pose3, Rot3 def createPoints() -> List[Point3]: @@ -28,16 +28,49 @@ def createPoints() -> List[Point3]: return points -def createPoses(K: Cal3_S2) -> List[Pose3]: - """Generate a set of ground-truth camera poses arranged in a circle about the origin.""" - radius = 40.0 - height = 10.0 - angles = np.linspace(0, 2 * np.pi, 8, endpoint=False) - up = gtsam.Point3(0, 0, 1) - target = gtsam.Point3(0, 0, 0) - poses = [] - for theta in angles: - position = gtsam.Point3(radius * np.cos(theta), radius * np.sin(theta), height) - camera = gtsam.PinholeCameraCal3_S2.Lookat(position, target, up, K) - poses.append(camera.pose()) +_M_PI_2 = np.pi / 2 +_M_PI_4 = np.pi / 4 + + +def createPoses( + init: Pose3 = Pose3(Rot3.Ypr(_M_PI_2, 0, -_M_PI_2), Point3(30, 0, 0)), + delta: Pose3 = Pose3( + Rot3.Ypr(0, -_M_PI_4, 0), + Point3(np.sin(_M_PI_4) * 30, 0, 30 * (1 - np.sin(_M_PI_4))), + ), + steps: int = 8, +) -> List[Pose3]: + """ + Create a set of ground-truth poses + Default values give a circular trajectory, radius 30 at pi/4 intervals, + always facing the circle center + """ + poses = [init] + for _ in range(1, steps): + poses.append(poses[-1].compose(delta)) return poses + + +def posesOnCircle(num_cameras=8, R=30): + """Create regularly spaced poses with specified radius and number of cameras.""" + theta = 2 * np.pi / num_cameras + + # Initial pose at angle 0, position (R, 0, 0), facing the center with Y-axis pointing down + init_rotation = Rot3.Ypr(_M_PI_2, 0, -_M_PI_2) + init_position = np.array([R, 0, 0]) + init = Pose3(init_rotation, init_position) + + # Delta rotation: rotate by -theta around Z-axis (counterclockwise movement) + delta_rotation = Rot3.Ypr(0, -theta, 0) + + # Delta translation in world frame + delta_translation_world = np.array([R * (np.cos(theta) - 1), R * np.sin(theta), 0]) + + # Transform delta translation to local frame of the camera + delta_translation_local = init.rotation().unrotate(delta_translation_world) + + # Define delta pose + delta = Pose3(delta_rotation, delta_translation_local) + + # Generate poses + return createPoses(init, delta, num_cameras) diff --git a/python/gtsam/examples/TranslationAveragingExample.py b/python/gtsam/examples/TranslationAveragingExample.py index ea1cab88d2..e3380ce942 100644 --- a/python/gtsam/examples/TranslationAveragingExample.py +++ b/python/gtsam/examples/TranslationAveragingExample.py @@ -31,8 +31,7 @@ def get_data() -> Tuple[gtsam.Values, List[gtsam.BinaryMeasurementUnit3]]: that lie on a circle and face the center. The poses of 8 cameras are obtained from SFMdata and the unit translations directions between some camera pairs are computed from their global translations. """ - fx, fy, s, u0, v0 = 50.0, 50.0, 0.0, 50.0, 50.0 - wTc_list = SFMdata.createPoses(gtsam.Cal3_S2(fx, fy, s, u0, v0)) + wTc_list = SFMdata.createPoses() # Rotations of the cameras in the world frame. wRc_values = gtsam.Values() # Normalized translation directions from camera i to camera j diff --git a/python/gtsam/examples/VisualISAM2Example.py b/python/gtsam/examples/VisualISAM2Example.py index 60d4fb376b..03dd079f0a 100644 --- a/python/gtsam/examples/VisualISAM2Example.py +++ b/python/gtsam/examples/VisualISAM2Example.py @@ -73,7 +73,7 @@ def visual_ISAM2_example(): points = SFMdata.createPoints() # Create the set of ground-truth poses - poses = SFMdata.createPoses(K) + poses = SFMdata.createPoses() # Create an iSAM2 object. Unlike iSAM1, which performs periodic batch steps # to maintain proper linearization and efficient variable ordering, iSAM2 diff --git a/python/gtsam/examples/VisualISAMExample.py b/python/gtsam/examples/VisualISAMExample.py index 9691b3c46d..48e803919a 100644 --- a/python/gtsam/examples/VisualISAMExample.py +++ b/python/gtsam/examples/VisualISAMExample.py @@ -36,7 +36,7 @@ def main(): # Create the set of ground-truth landmarks points = SFMdata.createPoints() # Create the set of ground-truth poses - poses = SFMdata.createPoses(K) + poses = SFMdata.createPoses() # Create a NonlinearISAM object which will relinearize and reorder the variables # every "reorderInterval" updates From f206c2f1e5b6fe0041e15b939351a8f9be201d4d Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Mon, 28 Oct 2024 08:56:21 -0700 Subject: [PATCH 007/115] Fix FromPose3 --- gtsam/geometry/geometry.i | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/gtsam/geometry/geometry.i b/gtsam/geometry/geometry.i index 095a350dd9..cf86723ae2 100644 --- a/gtsam/geometry/geometry.i +++ b/gtsam/geometry/geometry.i @@ -620,10 +620,10 @@ class EssentialMatrix { EssentialMatrix(const gtsam::Rot3& aRb, const gtsam::Unit3& aTb); // Constructors from Pose3 - gtsam::EssentialMatrix FromPose3(const gtsam::Pose3& _1P2_); + static gtsam::EssentialMatrix FromPose3(const gtsam::Pose3& _1P2_); - gtsam::EssentialMatrix FromPose3(const gtsam::Pose3& _1P2_, - Eigen::Ref H); + static gtsam::EssentialMatrix FromPose3(const gtsam::Pose3& _1P2_, + Eigen::Ref H); // Testable void print(string s = "") const; From e98acff79ac5825d6fae35b2f2209a4e52ecd64a Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 26 Oct 2024 15:57:16 -0700 Subject: [PATCH 008/115] Wrap FundamentalMatrix classes --- gtsam/geometry/geometry.i | 48 +++++++++++++++++++++++++++++++++++++ gtsam/nonlinear/nonlinear.i | 3 +++ 2 files changed, 51 insertions(+) diff --git a/gtsam/geometry/geometry.i b/gtsam/geometry/geometry.i index cf86723ae2..8fd1e46b29 100644 --- a/gtsam/geometry/geometry.i +++ b/gtsam/geometry/geometry.i @@ -903,6 +903,54 @@ class Cal3Bundler { void serialize() const; }; +#include + +// FundamentalMatrix class +class FundamentalMatrix { + // Constructors + FundamentalMatrix(); + FundamentalMatrix(const gtsam::Rot3& U, double s, const gtsam::Rot3& V); + FundamentalMatrix(const gtsam::Matrix3& F); + + // Overloaded constructors for specific calibration types + FundamentalMatrix(const gtsam::Cal3_S2& Ka, const gtsam::Pose3& aPb, + const gtsam::Cal3_S2& Kb); + + // Methods + gtsam::Matrix3 matrix() const; + + // Testable + void print(const std::string& s = "") const; + bool equals(const gtsam::FundamentalMatrix& other, double tol = 1e-9) const; + + // Manifold + static size_t Dim(); + size_t dim() const; + gtsam::Vector localCoordinates(const gtsam::FundamentalMatrix& F) const; + gtsam::FundamentalMatrix retract(const gtsam::Vector& delta) const; +}; + +// SimpleFundamentalMatrix class +class SimpleFundamentalMatrix { + // Constructors + SimpleFundamentalMatrix(); + SimpleFundamentalMatrix(const gtsam::EssentialMatrix& E, double fa, double fb, + const gtsam::Point2& ca, const gtsam::Point2& cb); + + // Methods + gtsam::Matrix3 matrix() const; + + // Testable + void print(const std::string& s = "") const; + bool equals(const gtsam::SimpleFundamentalMatrix& other, double tol = 1e-9) const; + + // Manifold + static size_t Dim(); + size_t dim() const; + gtsam::Vector localCoordinates(const gtsam::SimpleFundamentalMatrix& F) const; + gtsam::SimpleFundamentalMatrix retract(const gtsam::Vector& delta) const; +}; + #include class CalibratedCamera { // Standard Constructors and Named Constructors diff --git a/gtsam/nonlinear/nonlinear.i b/gtsam/nonlinear/nonlinear.i index f40deab5aa..f493fe8f6e 100644 --- a/gtsam/nonlinear/nonlinear.i +++ b/gtsam/nonlinear/nonlinear.i @@ -11,6 +11,7 @@ namespace gtsam { #include #include #include +#include #include #include #include @@ -537,6 +538,8 @@ class ISAM2 { template , gtsam::PinholeCamera, gtsam::PinholeCamera, From 9cfca976bab21cde5f9369947eec3d2f4a104e76 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 26 Oct 2024 16:55:23 -0700 Subject: [PATCH 009/115] New constructor --- gtsam/geometry/FundamentalMatrix.h | 20 +++++++++++++++++--- gtsam/geometry/geometry.i | 2 ++ 2 files changed, 19 insertions(+), 3 deletions(-) diff --git a/gtsam/geometry/FundamentalMatrix.h b/gtsam/geometry/FundamentalMatrix.h index 6f04f45e8e..770cd711de 100644 --- a/gtsam/geometry/FundamentalMatrix.h +++ b/gtsam/geometry/FundamentalMatrix.h @@ -54,6 +54,22 @@ class GTSAM_EXPORT FundamentalMatrix { */ FundamentalMatrix(const Matrix3& F); + /** + * @brief Construct from essential matrix and calibration matrices + * + * Initializes the FundamentalMatrix from the given essential matrix E + * and calibration matrices Ka and Kb. + * + * @tparam CAL Calibration type, expected to have a matrix() method + * @param E Essential matrix + * @param Ka Calibration matrix for the left camera + * @param Kb Calibration matrix for the right camera + */ + template + FundamentalMatrix(const CAL& Ka, const EssentialMatrix& E, const CAL& Kb) + : FundamentalMatrix(Ka.K().transpose().inverse() * E.matrix() * + Kb.K().inverse()) {} + /** * @brief Construct from calibration matrices Ka, Kb, and pose aPb * @@ -67,9 +83,7 @@ class GTSAM_EXPORT FundamentalMatrix { */ template FundamentalMatrix(const CAL& Ka, const Pose3& aPb, const CAL& Kb) - : FundamentalMatrix(Ka.K().transpose().inverse() * - EssentialMatrix::FromPose3(aPb).matrix() * - Kb.K().inverse()) {} + : FundamentalMatrix(Ka, EssentialMatrix::FromPose3(aPb), Kb) {} /// Return the fundamental matrix representation Matrix3 matrix() const; diff --git a/gtsam/geometry/geometry.i b/gtsam/geometry/geometry.i index 8fd1e46b29..13cdcf70d3 100644 --- a/gtsam/geometry/geometry.i +++ b/gtsam/geometry/geometry.i @@ -913,6 +913,8 @@ class FundamentalMatrix { FundamentalMatrix(const gtsam::Matrix3& F); // Overloaded constructors for specific calibration types + FundamentalMatrix(const gtsam::Cal3_S2& Ka, const gtsam::EssentialMatrix& E, + const gtsam::Cal3_S2& Kb); FundamentalMatrix(const gtsam::Cal3_S2& Ka, const gtsam::Pose3& aPb, const gtsam::Cal3_S2& Kb); From 36539f250aee3ba4dcfc2dc5d15d4267ca10960a Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 26 Oct 2024 23:52:42 -0700 Subject: [PATCH 010/115] Small API change --- gtsam/geometry/FundamentalMatrix.h | 13 +++++-------- gtsam/geometry/geometry.i | 9 +++++---- 2 files changed, 10 insertions(+), 12 deletions(-) diff --git a/gtsam/geometry/FundamentalMatrix.h b/gtsam/geometry/FundamentalMatrix.h index 770cd711de..12cc20b3eb 100644 --- a/gtsam/geometry/FundamentalMatrix.h +++ b/gtsam/geometry/FundamentalMatrix.h @@ -60,15 +60,14 @@ class GTSAM_EXPORT FundamentalMatrix { * Initializes the FundamentalMatrix from the given essential matrix E * and calibration matrices Ka and Kb. * - * @tparam CAL Calibration type, expected to have a matrix() method * @param E Essential matrix * @param Ka Calibration matrix for the left camera * @param Kb Calibration matrix for the right camera */ - template - FundamentalMatrix(const CAL& Ka, const EssentialMatrix& E, const CAL& Kb) - : FundamentalMatrix(Ka.K().transpose().inverse() * E.matrix() * - Kb.K().inverse()) {} + FundamentalMatrix(const Matrix3& Ka, const EssentialMatrix& E, + const Matrix3& Kb) + : FundamentalMatrix(Ka.transpose().inverse() * E.matrix() * + Kb.inverse()) {} /** * @brief Construct from calibration matrices Ka, Kb, and pose aPb @@ -76,13 +75,11 @@ class GTSAM_EXPORT FundamentalMatrix { * Initializes the FundamentalMatrix from the given calibration * matrices Ka and Kb, and the pose aPb. * - * @tparam CAL Calibration type, expected to have a matrix() method * @param Ka Calibration matrix for the left camera * @param aPb Pose from the left to the right camera * @param Kb Calibration matrix for the right camera */ - template - FundamentalMatrix(const CAL& Ka, const Pose3& aPb, const CAL& Kb) + FundamentalMatrix(const Matrix3& Ka, const Pose3& aPb, const Matrix3& Kb) : FundamentalMatrix(Ka, EssentialMatrix::FromPose3(aPb), Kb) {} /// Return the fundamental matrix representation diff --git a/gtsam/geometry/geometry.i b/gtsam/geometry/geometry.i index 13cdcf70d3..671965064f 100644 --- a/gtsam/geometry/geometry.i +++ b/gtsam/geometry/geometry.i @@ -913,10 +913,10 @@ class FundamentalMatrix { FundamentalMatrix(const gtsam::Matrix3& F); // Overloaded constructors for specific calibration types - FundamentalMatrix(const gtsam::Cal3_S2& Ka, const gtsam::EssentialMatrix& E, - const gtsam::Cal3_S2& Kb); - FundamentalMatrix(const gtsam::Cal3_S2& Ka, const gtsam::Pose3& aPb, - const gtsam::Cal3_S2& Kb); + FundamentalMatrix(const gtsam::Matrix3& Ka, const gtsam::EssentialMatrix& E, + const gtsam::Matrix3& Kb); + FundamentalMatrix(const gtsam::Matrix3& Ka, const gtsam::Pose3& aPb, + const gtsam::Matrix3& Kb); // Methods gtsam::Matrix3 matrix() const; @@ -1066,6 +1066,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 56610ce5f7f8bd1987ab3cc2871547faf0c14e98 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Mon, 28 Oct 2024 09:32:04 -0700 Subject: [PATCH 011/115] Python unit tests --- gtsam/geometry/geometry.i | 6 +- python/gtsam/tests/test_FundamentalMatrix.py | 228 +++++++++++++++++++ 2 files changed, 233 insertions(+), 1 deletion(-) create mode 100644 python/gtsam/tests/test_FundamentalMatrix.py diff --git a/gtsam/geometry/geometry.i b/gtsam/geometry/geometry.i index 671965064f..3328a05bbc 100644 --- a/gtsam/geometry/geometry.i +++ b/gtsam/geometry/geometry.i @@ -578,6 +578,8 @@ class Unit3 { // Standard Constructors Unit3(); Unit3(const gtsam::Point3& pose); + Unit3(double x, double y, double z); + Unit3(const gtsam::Point2& p, double f); // Testable void print(string s = "") const; @@ -953,6 +955,9 @@ class SimpleFundamentalMatrix { gtsam::SimpleFundamentalMatrix retract(const gtsam::Vector& delta) const; }; +gtsam::Point2 EpipolarTransfer(const gtsam::Matrix3& Fca, const gtsam::Point2& pa, + const gtsam::Matrix3& Fcb, const gtsam::Point2& pb); + #include class CalibratedCamera { // Standard Constructors and Named Constructors @@ -1066,7 +1071,6 @@ 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 diff --git a/python/gtsam/tests/test_FundamentalMatrix.py b/python/gtsam/tests/test_FundamentalMatrix.py new file mode 100644 index 0000000000..4b30658b5e --- /dev/null +++ b/python/gtsam/tests/test_FundamentalMatrix.py @@ -0,0 +1,228 @@ +""" +GTSAM Copyright 2010-2019, Georgia Tech Research Corporation, +Atlanta, Georgia 30332-0415 +All Rights Reserved + +See LICENSE for the license information + +FundamentalMatrix unit tests. +Author: Frank Dellaert +""" + +# pylint: disable=no-name-in-module +import unittest + +import numpy as np +from gtsam.examples import SFMdata +from numpy.testing import assert_almost_equal + +import gtsam +from gtsam import (Cal3_S2, EssentialMatrix, FundamentalMatrix, + PinholeCameraCal3_S2, Point2, Point3, Pose3, Rot3, + SimpleFundamentalMatrix, Unit3) + + +class TestFundamentalMatrix(unittest.TestCase): + + def setUp(self): + # Create two rotations and corresponding fundamental matrix F + self.trueU = Rot3.Yaw(np.pi / 2) + self.trueV = Rot3.Yaw(np.pi / 4) + self.trueS = 0.5 + self.trueF = FundamentalMatrix(self.trueU, self.trueS, self.trueV) + + def test_localCoordinates(self): + expected = np.zeros(7) # Assuming 7 dimensions for U, V, and s + actual = self.trueF.localCoordinates(self.trueF) + assert_almost_equal(expected, actual, decimal=8) + + def test_retract(self): + actual = self.trueF.retract(np.zeros(7)) + self.assertTrue(self.trueF.equals(actual, 1e-9)) + + def test_RoundTrip(self): + d = np.array([0.1, 0.2, 0.3, 0.4, 0.5, 0.6, 0.7]) + hx = self.trueF.retract(d) + actual = self.trueF.localCoordinates(hx) + assert_almost_equal(d, actual, decimal=8) + + +class TestSimpleStereo(unittest.TestCase): + + def setUp(self): + # Create the simplest SimpleFundamentalMatrix, a stereo pair + self.defaultE = EssentialMatrix(Rot3(), Unit3(1, 0, 0)) + self.zero = Point2(0.0, 0.0) + self.stereoF = SimpleFundamentalMatrix(self.defaultE, 1.0, 1.0, self.zero, self.zero) + + def test_Conversion(self): + convertedF = FundamentalMatrix(self.stereoF.matrix()) + assert_almost_equal(self.stereoF.matrix(), convertedF.matrix(), decimal=8) + + def test_localCoordinates(self): + expected = np.zeros(7) + actual = self.stereoF.localCoordinates(self.stereoF) + assert_almost_equal(expected, actual, decimal=8) + + def test_retract(self): + actual = self.stereoF.retract(np.zeros(9)) + self.assertTrue(self.stereoF.equals(actual, 1e-9)) + + def test_RoundTrip(self): + d = np.array([0.1, 0.2, 0.3, 0.4, 0.5, 0.6, 0.7]) + hx = self.stereoF.retract(d) + actual = self.stereoF.localCoordinates(hx) + assert_almost_equal(d, actual, decimal=8) + + def test_EpipolarLine(self): + # Create a point in b + p_b = np.array([0, 2, 1]) + # Convert the point to a horizontal line in a + l_a = self.stereoF.matrix() @ p_b + # Check if the line is horizontal at height 2 + expected = np.array([0, -1, 2]) + assert_almost_equal(l_a, expected, decimal=8) + + +class TestPixelStereo(unittest.TestCase): + + def setUp(self): + # Create a stereo pair in pixels, zero principal points + self.focalLength = 1000.0 + self.defaultE = EssentialMatrix(Rot3(), Unit3(1, 0, 0)) + self.zero = Point2(0.0, 0.0) + self.pixelStereo = SimpleFundamentalMatrix( + self.defaultE, self.focalLength, self.focalLength, self.zero, self.zero + ) + + def test_Conversion(self): + expected = self.pixelStereo.matrix() + convertedF = FundamentalMatrix(self.pixelStereo.matrix()) + # Check equality of F-matrices up to a scale + actual = convertedF.matrix() + scale = expected[1, 2] / actual[1, 2] + actual *= scale + assert_almost_equal(expected, actual, decimal=5) + + def test_PointInBToHorizontalLineInA(self): + # Create a point in b + p_b = np.array([0, 300, 1]) + # Convert the point to a horizontal line in a + l_a = self.pixelStereo.matrix() @ p_b + # Check if the line is horizontal at height 0.3 + expected = np.array([0, -0.001, 0.3]) + assert_almost_equal(l_a, expected, decimal=8) + + +class TestRotatedPixelStereo(unittest.TestCase): + + def setUp(self): + # Create a stereo pair with the right camera rotated 90 degrees + self.focalLength = 1000.0 + self.zero = Point2(0.0, 0.0) + self.aRb = Rot3.Rz(np.pi / 2) # Rotate 90 degrees around the Z-axis + self.rotatedE = EssentialMatrix(self.aRb, Unit3(1, 0, 0)) + self.rotatedPixelStereo = SimpleFundamentalMatrix( + self.rotatedE, self.focalLength, self.focalLength, self.zero, self.zero + ) + + def test_Conversion(self): + expected = self.rotatedPixelStereo.matrix() + convertedF = FundamentalMatrix(self.rotatedPixelStereo.matrix()) + # Check equality of F-matrices up to a scale + actual = convertedF.matrix() + scale = expected[1, 2] / actual[1, 2] + actual *= scale + assert_almost_equal(expected, actual, decimal=4) + + def test_PointInBToHorizontalLineInA(self): + # Create a point in b + p_b = np.array([300, 0, 1]) + # Convert the point to a horizontal line in a + l_a = self.rotatedPixelStereo.matrix() @ p_b + # Check if the line is horizontal at height 0.3 + expected = np.array([0, -0.001, 0.3]) + assert_almost_equal(l_a, expected, decimal=8) + + +class TestStereoWithPrincipalPoints(unittest.TestCase): + + def setUp(self): + # Now check that principal points also survive conversion + self.focalLength = 1000.0 + self.principalPoint = Point2(640 / 2, 480 / 2) + self.aRb = Rot3.Rz(np.pi / 2) + self.rotatedE = EssentialMatrix(self.aRb, Unit3(1, 0, 0)) + self.stereoWithPrincipalPoints = SimpleFundamentalMatrix( + self.rotatedE, self.focalLength, self.focalLength, self.principalPoint, self.principalPoint + ) + + def test_Conversion(self): + expected = self.stereoWithPrincipalPoints.matrix() + convertedF = FundamentalMatrix(self.stereoWithPrincipalPoints.matrix()) + # Check equality of F-matrices up to a scale + actual = convertedF.matrix() + scale = expected[1, 2] / actual[1, 2] + actual *= scale + assert_almost_equal(expected, actual, decimal=4) + + +class TestTripleF(unittest.TestCase): + + def setUp(self): + # Generate three cameras on a circle, looking in + self.cameraPoses = SFMdata.posesOnCircle(3, 1.0) + self.focalLength = 1000.0 + self.principalPoint = Point2(640 / 2, 480 / 2) + self.triplet = self.generateTripleF(self.cameraPoses) + + def generateTripleF(self, cameraPoses): + F = [] + for i in range(3): + j = (i + 1) % 3 + iPj = cameraPoses[i].between(cameraPoses[j]) + E = EssentialMatrix(iPj.rotation(), Unit3(iPj.translation())) + F_ij = SimpleFundamentalMatrix( + E, self.focalLength, self.focalLength, self.principalPoint, self.principalPoint + ) + F.append(F_ij) + return {"Fab": F[0], "Fbc": F[1], "Fca": F[2]} + + def transferToA(self, pb, pc): + return gtsam.EpipolarTransfer(self.triplet["Fab"].matrix(), pb, self.triplet["Fca"].matrix().transpose(), pc) + + def transferToB(self, pa, pc): + return gtsam.EpipolarTransfer(self.triplet["Fab"].matrix().transpose(), pa, self.triplet["Fbc"].matrix(), pc) + + def transferToC(self, pa, pb): + return gtsam.EpipolarTransfer(self.triplet["Fca"].matrix(), pa, self.triplet["Fbc"].matrix().transpose(), pb) + + def test_Transfer(self): + triplet = self.triplet + # Check that they are all equal + self.assertTrue(triplet["Fab"].equals(triplet["Fbc"], 1e-9)) + self.assertTrue(triplet["Fbc"].equals(triplet["Fca"], 1e-9)) + self.assertTrue(triplet["Fca"].equals(triplet["Fab"], 1e-9)) + + # Now project a point into the three cameras + P = Point3(0.1, 0.2, 0.3) + K = Cal3_S2(self.focalLength, self.focalLength, 0.0, self.principalPoint[0], self.principalPoint[1]) + + p = [] + for i in range(3): + # Project the point into each camera + camera = PinholeCameraCal3_S2(self.cameraPoses[i], K) + p_i = camera.project(P) + p.append(p_i) + + # Check that transfer works + transferredA = self.transferToA(p[1], p[2]) + transferredB = self.transferToB(p[0], p[2]) + transferredC = self.transferToC(p[0], p[1]) + assert_almost_equal([p[0][0], p[0][1]], [transferredA[0], transferredA[1]], decimal=9) + assert_almost_equal([p[1][0], p[1][1]], [transferredB[0], transferredB[1]], decimal=9) + assert_almost_equal([p[2][0], p[2][1]], [transferredC[0], transferredC[1]], decimal=9) + + +if __name__ == "__main__": + unittest.main() From fba31d99f2392ba79828d63904c801b8de5cb1cd Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Mon, 28 Oct 2024 09:51:56 -0700 Subject: [PATCH 012/115] TestManyCamerasCircle --- python/gtsam/tests/test_FundamentalMatrix.py | 71 ++++++++++++++++++-- 1 file changed, 64 insertions(+), 7 deletions(-) diff --git a/python/gtsam/tests/test_FundamentalMatrix.py b/python/gtsam/tests/test_FundamentalMatrix.py index 4b30658b5e..4e6efc99d7 100644 --- a/python/gtsam/tests/test_FundamentalMatrix.py +++ b/python/gtsam/tests/test_FundamentalMatrix.py @@ -18,7 +18,7 @@ import gtsam from gtsam import (Cal3_S2, EssentialMatrix, FundamentalMatrix, - PinholeCameraCal3_S2, Point2, Point3, Pose3, Rot3, + PinholeCameraCal3_S2, Point2, Point3, Rot3, SimpleFundamentalMatrix, Unit3) @@ -216,12 +216,69 @@ def test_Transfer(self): p.append(p_i) # Check that transfer works - transferredA = self.transferToA(p[1], p[2]) - transferredB = self.transferToB(p[0], p[2]) - transferredC = self.transferToC(p[0], p[1]) - assert_almost_equal([p[0][0], p[0][1]], [transferredA[0], transferredA[1]], decimal=9) - assert_almost_equal([p[1][0], p[1][1]], [transferredB[0], transferredB[1]], decimal=9) - assert_almost_equal([p[2][0], p[2][1]], [transferredC[0], transferredC[1]], decimal=9) + assert_almost_equal(p[0], self.transferToA(p[1], p[2]), decimal=9) + assert_almost_equal(p[1], self.transferToB(p[0], p[2]), decimal=9) + assert_almost_equal(p[2], self.transferToC(p[0], p[1]), decimal=9) + + +class TestManyCamerasCircle(unittest.TestCase): + N = 6 + + def setUp(self): + # Generate six cameras on a circle, looking in + self.cameraPoses = SFMdata.posesOnCircle(self.N, 1.0) + self.focalLength = 1000.0 + self.principalPoint = Point2(640 / 2, 480 / 2) + self.manyFs = self.generateManyFs(self.cameraPoses) + + def generateManyFs(self, cameraPoses): + F = [] + for i in range(self.N): + j = (i + 1) % self.N + iPj = cameraPoses[i].between(cameraPoses[j]) + E = EssentialMatrix(iPj.rotation(), Unit3(iPj.translation())) + F_ij = SimpleFundamentalMatrix( + E, self.focalLength, self.focalLength, self.principalPoint, self.principalPoint + ) + F.append(F_ij) + return F + + def test_Conversion(self): + for i in range(self.N): + expected = self.manyFs[i].matrix() + convertedF = FundamentalMatrix(self.manyFs[i].matrix()) + # Check equality of F-matrices up to a scale + actual = convertedF.matrix() + scale = expected[1, 2] / actual[1, 2] + actual *= scale + # print(f"\n{np.round(expected, 3)}", f"\n{np.round(actual, 3)}") + assert_almost_equal(expected, actual, decimal=4) + + def test_Transfer(self): + # Now project a point into the six cameras + P = Point3(0.1, 0.2, 0.3) + K = Cal3_S2(self.focalLength, self.focalLength, 0.0, self.principalPoint[0], self.principalPoint[1]) + + p = [] + for i in range(self.N): + # Project the point into each camera + camera = PinholeCameraCal3_S2(self.cameraPoses[i], K) + p_i = camera.project(P) + p.append(p_i) + + # Check that transfer works + for a in range(self.N): + b = (a + 1) % self.N + c = (a + 2) % self.N + # We transfer from a to b and from c to b, + # and check that the result lines up with the projected point in b. + transferred = gtsam.EpipolarTransfer( + self.manyFs[a].matrix().transpose(), # need to transpose for a->b + p[a], + self.manyFs[c].matrix(), + p[c], + ) + assert_almost_equal(p[b], transferred, decimal=9) if __name__ == "__main__": From ced6d5721d50dc1415e6c76c5d4d878445b22048 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Mon, 28 Oct 2024 11:59:29 -0700 Subject: [PATCH 013/115] Fix call w new API --- examples/ViewGraphExample.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/examples/ViewGraphExample.cpp b/examples/ViewGraphExample.cpp index c23ac084c9..23393fa20c 100644 --- a/examples/ViewGraphExample.cpp +++ b/examples/ViewGraphExample.cpp @@ -38,7 +38,7 @@ 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); + Cal3_S2 cal(50.0, 50.0, 0.0, 50.0, 50.0); // Create the set of 8 ground-truth landmarks vector points = createPoints(); @@ -47,13 +47,13 @@ int main(int argc, char* argv[]) { 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); + auto F1 = FundamentalMatrix(cal.K(), poses[0].between(poses[1]), cal.K()); + auto F2 = FundamentalMatrix(cal.K(), poses[0].between(poses[2]), cal.K()); // 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], cal); for (size_t j = 0; j < 8; ++j) { p[i][j] = camera.project(points[j]); } From b45c55c9f4a98f801dfce0e6be04f5fc27ef7a4b Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Mon, 28 Oct 2024 12:00:54 -0700 Subject: [PATCH 014/115] Fix issues with SVD constructor --- gtsam/geometry/FundamentalMatrix.cpp | 57 +++++++++++++------ gtsam/geometry/FundamentalMatrix.h | 41 +++++++------ .../geometry/tests/testFundamentalMatrix.cpp | 37 ++++++++++-- 3 files changed, 96 insertions(+), 39 deletions(-) diff --git a/gtsam/geometry/FundamentalMatrix.cpp b/gtsam/geometry/FundamentalMatrix.cpp index d65053d196..5a50b4fd2d 100644 --- a/gtsam/geometry/FundamentalMatrix.cpp +++ b/gtsam/geometry/FundamentalMatrix.cpp @@ -26,6 +26,11 @@ Point2 EpipolarTransfer(const Matrix3& Fca, const Point2& pa, // } //************************************************************************* +FundamentalMatrix::FundamentalMatrix(const Matrix& U, double s, + const Matrix& V) { + initialize(U, s, V); +} + FundamentalMatrix::FundamentalMatrix(const Matrix3& F) { // Perform SVD Eigen::JacobiSVD svd(F, Eigen::ComputeFullU | Eigen::ComputeFullV); @@ -47,28 +52,44 @@ FundamentalMatrix::FundamentalMatrix(const Matrix3& F) { "The input matrix does not represent a valid fundamental matrix."); } - // Ensure the second singular value is recorded as s - s_ = singularValues(1); + initialize(U, singularValues(1), V); +} - // Check if U is a reflection - if (U.determinant() < 0) { - U = -U; - s_ = -s_; // Change sign of scalar if U is a reflection - } +void FundamentalMatrix::initialize(const Matrix3& U, double s, + const Matrix3& V) { + s_ = s; + sign_ = 1.0; - // Check if V is a reflection - if (V.determinant() < 0) { - V = -V; - s_ = -s_; // Change sign of scalar if U is a reflection + // 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."); + } + if (detU < 0) { + U_ = Rot3(-U); + sign_ = -sign_; // Flip sign if U is a reflection + } else { + U_ = Rot3(U); } - // Assign the rotations - U_ = Rot3(U); - V_ = Rot3(V); + // Check if V is a reflection and its determinant is close to -1 or 1 + 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 + } else { + V_ = Rot3(V); + } } Matrix3 FundamentalMatrix::matrix() const { - return U_.matrix() * Vector3(1, s_, 0).asDiagonal() * V_.transpose().matrix(); + return sign_ * U_.matrix() * Vector3(1.0, s_, 0).asDiagonal() * + V_.transpose().matrix(); } void FundamentalMatrix::print(const std::string& s) const { @@ -77,8 +98,8 @@ void FundamentalMatrix::print(const std::string& s) const { bool FundamentalMatrix::equals(const FundamentalMatrix& other, double tol) const { - return U_.equals(other.U_, tol) && std::abs(s_ - other.s_) < tol && - V_.equals(other.V_, tol); + return U_.equals(other.U_, tol) && sign_ == other.sign_ && + std::abs(s_ - other.s_) < tol && V_.equals(other.V_, tol); } Vector FundamentalMatrix::localCoordinates(const FundamentalMatrix& F) const { @@ -93,7 +114,7 @@ FundamentalMatrix FundamentalMatrix::retract(const Vector& delta) const { Rot3 newU = U_.retract(delta.head<3>()); double newS = s_ + delta(3); // Update scalar Rot3 newV = V_.retract(delta.tail<3>()); - return FundamentalMatrix(newU, newS, newV); + return FundamentalMatrix(newU, sign_, newS, newV); } //************************************************************************* diff --git a/gtsam/geometry/FundamentalMatrix.h b/gtsam/geometry/FundamentalMatrix.h index 12cc20b3eb..7e87ba62a7 100644 --- a/gtsam/geometry/FundamentalMatrix.h +++ b/gtsam/geometry/FundamentalMatrix.h @@ -15,34 +15,36 @@ namespace gtsam { /** * @class FundamentalMatrix - * @brief Represents a general fundamental matrix. + * @brief Represents a fundamental matrix in computer vision, which encodes the + * epipolar geometry between two views. * - * This class represents a general fundamental matrix, which is a 3x3 matrix - * that describes the relationship between two images. It is parameterized by a - * left rotation U, a scalar s, and a right rotation V. + * The FundamentalMatrix class encapsulates the fundamental matrix, which + * relates corresponding points in stereo images. It is parameterized by two + * rotation matrices (U and V), a scalar parameter (s), and a sign. + * Using these values, the fundamental matrix is represented as + * + * F = sign * U * diag(1, s, 0) * V^T + * + * We need the `sign` because we use SO(3) for U and V, not O(3). */ class GTSAM_EXPORT FundamentalMatrix { private: - Rot3 U_; ///< Left rotation - double s_; ///< Scalar parameter for S - Rot3 V_; ///< Right rotation + Rot3 U_; ///< Left rotation + double sign_; ///< Whether to flip the sign or not + double s_; ///< Scalar parameter for S + Rot3 V_; ///< Right rotation public: /// Default constructor - FundamentalMatrix() : U_(Rot3()), s_(1.0), V_(Rot3()) {} + FundamentalMatrix() : U_(Rot3()), sign_(1.0), s_(1.0), V_(Rot3()) {} /** * @brief Construct from U, V, and scalar s * - * Initializes the FundamentalMatrix with the given left rotation U, - * scalar s, and right rotation V. - * - * @param U Left rotation matrix - * @param s Scalar parameter for the fundamental matrix - * @param V Right rotation matrix + * Initializes the FundamentalMatrix From the SVD representation + * U*diag(1,s,0)*V^T. It will internally convert to using SO(3). */ - FundamentalMatrix(const Rot3& U, double s, const Rot3& V) - : U_(U), s_(s), V_(V) {} + FundamentalMatrix(const Matrix& U, double s, const Matrix& V); /** * @brief Construct from a 3x3 matrix using SVD @@ -107,6 +109,13 @@ class GTSAM_EXPORT FundamentalMatrix { /// Retract the given vector to get a new FundamentalMatrix FundamentalMatrix retract(const Vector& delta) const; /// @} + 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) {} + + /// 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 a8884e791e..3e136caa7c 100644 --- a/gtsam/geometry/tests/testFundamentalMatrix.cpp +++ b/gtsam/geometry/tests/testFundamentalMatrix.cpp @@ -24,21 +24,48 @@ GTSAM_CONCEPT_MANIFOLD_INST(FundamentalMatrix) Rot3 trueU = Rot3::Yaw(M_PI_2); Rot3 trueV = Rot3::Yaw(M_PI_4); double trueS = 0.5; -FundamentalMatrix trueF(trueU, trueS, trueV); +FundamentalMatrix trueF(trueU.matrix(), trueS, trueV.matrix()); //************************************************************************* -TEST(FundamentalMatrix, localCoordinates) { +TEST(FundamentalMatrix, LocalCoordinates) { Vector expected = Z_7x1; // Assuming 7 dimensions for U, V, and s Vector actual = trueF.localCoordinates(trueF); EXPECT(assert_equal(expected, actual, 1e-8)); } //************************************************************************* -TEST(FundamentalMatrix, retract) { +TEST(FundamentalMatrix, Retract) { FundamentalMatrix actual = trueF.retract(Z_7x1); EXPECT(assert_equal(trueF, actual)); } +//************************************************************************* +// Test conversion from an F-matrix +TEST(FundamentalMatrix, Conversion) { + const Matrix3 F = trueU.matrix() * Vector3(1, trueS, 0).asDiagonal() * + trueV.matrix().transpose(); + FundamentalMatrix actual(F); + EXPECT(assert_equal(trueF, actual)); +} + +//************************************************************************* +// Test conversion with a *non-rotation* U +TEST(FlippedFundamentalMatrix, Conversion1) { + FundamentalMatrix trueF(trueU.matrix(), trueS, -trueV.matrix()); + const Matrix3 F = trueF.matrix(); + FundamentalMatrix actual(F); + CHECK(assert_equal(F, actual.matrix())); +} + +//************************************************************************* +// Test conversion with a *non-rotation* U +TEST(FlippedFundamentalMatrix, Conversion2) { + FundamentalMatrix trueF(-trueU.matrix(), trueS, trueV.matrix()); + const Matrix3 F = trueF.matrix(); + FundamentalMatrix actual(F); + CHECK(assert_equal(F, actual.matrix())); +} + //************************************************************************* TEST(FundamentalMatrix, RoundTrip) { Vector7 d; @@ -61,14 +88,14 @@ TEST(SimpleStereo, Conversion) { } //************************************************************************* -TEST(SimpleStereo, localCoordinates) { +TEST(SimpleStereo, LocalCoordinates) { Vector expected = Z_7x1; Vector actual = stereoF.localCoordinates(stereoF); EXPECT(assert_equal(expected, actual, 1e-8)); } //************************************************************************* -TEST(SimpleStereo, retract) { +TEST(SimpleStereo, Retract) { SimpleFundamentalMatrix actual = stereoF.retract(Z_9x1); EXPECT(assert_equal(stereoF, actual)); } From 2d170e4cace4e008c9547c029488d9fb1f66be11 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Mon, 28 Oct 2024 12:15:50 -0700 Subject: [PATCH 015/115] Fix wrapper --- gtsam/geometry/geometry.i | 2 +- python/gtsam/tests/test_FundamentalMatrix.py | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/gtsam/geometry/geometry.i b/gtsam/geometry/geometry.i index 3328a05bbc..a90de48e55 100644 --- a/gtsam/geometry/geometry.i +++ b/gtsam/geometry/geometry.i @@ -911,7 +911,7 @@ class Cal3Bundler { class FundamentalMatrix { // Constructors FundamentalMatrix(); - FundamentalMatrix(const gtsam::Rot3& U, double s, const gtsam::Rot3& V); + FundamentalMatrix(const gtsam::Matrix3& U, double s, const gtsam::Matrix3& V); FundamentalMatrix(const gtsam::Matrix3& F); // Overloaded constructors for specific calibration types diff --git a/python/gtsam/tests/test_FundamentalMatrix.py b/python/gtsam/tests/test_FundamentalMatrix.py index 4e6efc99d7..9f486c2986 100644 --- a/python/gtsam/tests/test_FundamentalMatrix.py +++ b/python/gtsam/tests/test_FundamentalMatrix.py @@ -29,7 +29,7 @@ def setUp(self): self.trueU = Rot3.Yaw(np.pi / 2) self.trueV = Rot3.Yaw(np.pi / 4) self.trueS = 0.5 - self.trueF = FundamentalMatrix(self.trueU, self.trueS, self.trueV) + self.trueF = FundamentalMatrix(self.trueU.matrix(), self.trueS, self.trueV.matrix()) def test_localCoordinates(self): expected = np.zeros(7) # Assuming 7 dimensions for U, V, and s From 0836852dcb68db8fbfb3c30cd5396647c1c790a5 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Mon, 28 Oct 2024 15:14:46 -0700 Subject: [PATCH 016/115] Fix test on windows --- .../geometry/tests/testFundamentalMatrix.cpp | 36 +++++++------------ 1 file changed, 13 insertions(+), 23 deletions(-) diff --git a/gtsam/geometry/tests/testFundamentalMatrix.cpp b/gtsam/geometry/tests/testFundamentalMatrix.cpp index 3e136caa7c..00876e415e 100644 --- a/gtsam/geometry/tests/testFundamentalMatrix.cpp +++ b/gtsam/geometry/tests/testFundamentalMatrix.cpp @@ -40,30 +40,20 @@ TEST(FundamentalMatrix, Retract) { } //************************************************************************* -// Test conversion from an F-matrix +// Test conversion from F matrices, including non-rotations TEST(FundamentalMatrix, Conversion) { - const Matrix3 F = trueU.matrix() * Vector3(1, trueS, 0).asDiagonal() * - trueV.matrix().transpose(); - FundamentalMatrix actual(F); - EXPECT(assert_equal(trueF, actual)); -} - -//************************************************************************* -// Test conversion with a *non-rotation* U -TEST(FlippedFundamentalMatrix, Conversion1) { - FundamentalMatrix trueF(trueU.matrix(), trueS, -trueV.matrix()); - const Matrix3 F = trueF.matrix(); - FundamentalMatrix actual(F); - CHECK(assert_equal(F, actual.matrix())); -} - -//************************************************************************* -// Test conversion with a *non-rotation* U -TEST(FlippedFundamentalMatrix, Conversion2) { - FundamentalMatrix trueF(-trueU.matrix(), trueS, trueV.matrix()); - const Matrix3 F = trueF.matrix(); - FundamentalMatrix actual(F); - CHECK(assert_equal(F, actual.matrix())); + Matrix3 U = trueU.matrix(); + Matrix3 V = trueV.matrix(); + std::vector Fs = { + FundamentalMatrix(U, trueS, V), FundamentalMatrix(U, trueS, -V), + FundamentalMatrix(-U, trueS, V), FundamentalMatrix(-U, trueS, -V)}; + + for (const auto& trueF : Fs) { + const Matrix3 F = trueF.matrix(); + FundamentalMatrix actual(F); + // We check the matrices as the underlying representation is not unique + CHECK(assert_equal(F, actual.matrix())); + } } //************************************************************************* From 9db13c5987dcd115832282ee5d2975132038cba6 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 28 Oct 2024 18:48:47 -0400 Subject: [PATCH 017/115] better test naming --- gtsam/hybrid/tests/testHybridNonlinearISAM.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/gtsam/hybrid/tests/testHybridNonlinearISAM.cpp b/gtsam/hybrid/tests/testHybridNonlinearISAM.cpp index c5f52e3ebf..d6cff24fde 100644 --- a/gtsam/hybrid/tests/testHybridNonlinearISAM.cpp +++ b/gtsam/hybrid/tests/testHybridNonlinearISAM.cpp @@ -196,7 +196,7 @@ TEST(HybridNonlinearISAM, IncrementalInference) { /* ****************************************************************************/ // Test if we can approximately do the inference -TEST(HybridNonlinearISAM, Approx_inference) { +TEST(HybridNonlinearISAM, ApproxInference) { Switching switching(4); HybridNonlinearISAM incrementalHybrid; HybridNonlinearFactorGraph graph1; @@ -304,7 +304,7 @@ TEST(HybridNonlinearISAM, Approx_inference) { /* ****************************************************************************/ // Test approximate inference with an additional pruning step. -TEST(HybridNonlinearISAM, Incremental_approximate) { +TEST(HybridNonlinearISAM, IncrementalApproximate) { Switching switching(5); HybridNonlinearISAM incrementalHybrid; HybridNonlinearFactorGraph graph1; From 1f68a3d9b184fa76429dd98f75d56e6a00866a10 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 28 Oct 2024 18:48:57 -0400 Subject: [PATCH 018/115] fix test --- gtsam/hybrid/tests/testHybridNonlinearISAM.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/hybrid/tests/testHybridNonlinearISAM.cpp b/gtsam/hybrid/tests/testHybridNonlinearISAM.cpp index d6cff24fde..192f437424 100644 --- a/gtsam/hybrid/tests/testHybridNonlinearISAM.cpp +++ b/gtsam/hybrid/tests/testHybridNonlinearISAM.cpp @@ -83,7 +83,7 @@ TEST(HybridNonlinearISAM, IncrementalElimination) { HybridNonlinearFactorGraph graph2; initial = Values(); - graph1.push_back(switching.unaryFactors.at(1)); // P(X1) + graph2.push_back(switching.unaryFactors.at(1)); // P(X1) graph2.push_back(switching.unaryFactors.at(2)); // P(X2) graph2.push_back(switching.modeChain.at(1)); // P(M0, M1) From a7e48c0a81214cba7793630b687a22bc8e2878d0 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 28 Oct 2024 18:50:39 -0400 Subject: [PATCH 019/115] use single graph in tests --- .../hybrid/tests/testHybridNonlinearISAM.cpp | 68 +++++++++---------- 1 file changed, 34 insertions(+), 34 deletions(-) diff --git a/gtsam/hybrid/tests/testHybridNonlinearISAM.cpp b/gtsam/hybrid/tests/testHybridNonlinearISAM.cpp index 192f437424..94747ae790 100644 --- a/gtsam/hybrid/tests/testHybridNonlinearISAM.cpp +++ b/gtsam/hybrid/tests/testHybridNonlinearISAM.cpp @@ -49,7 +49,7 @@ using symbol_shorthand::Z; TEST(HybridNonlinearISAM, IncrementalElimination) { Switching switching(3); HybridNonlinearISAM isam; - HybridNonlinearFactorGraph graph1; + HybridNonlinearFactorGraph graph; Values initial; // Create initial factor graph @@ -57,17 +57,17 @@ TEST(HybridNonlinearISAM, IncrementalElimination) { // | | | // X0 -*- X1 -*- X2 // \*-M0-*/ - graph1.push_back(switching.unaryFactors.at(0)); // P(X0) - graph1.push_back(switching.binaryFactors.at(0)); // P(X0, X1 | M0) - graph1.push_back(switching.binaryFactors.at(1)); // P(X1, X2 | M1) - graph1.push_back(switching.modeChain.at(0)); // P(M0) + graph.push_back(switching.unaryFactors.at(0)); // P(X0) + graph.push_back(switching.binaryFactors.at(0)); // P(X0, X1 | M0) + graph.push_back(switching.binaryFactors.at(1)); // P(X1, X2 | M1) + graph.push_back(switching.modeChain.at(0)); // P(M0) initial.insert(X(0), 1); initial.insert(X(1), 2); initial.insert(X(2), 3); // Run update step - isam.update(graph1, initial); + isam.update(graph, initial); // Check that after update we have 3 hybrid Bayes net nodes: // P(X0 | X1, M0) and P(X1, X2 | M0, M1), P(M0, M1) @@ -80,14 +80,14 @@ TEST(HybridNonlinearISAM, IncrementalElimination) { /********************************************************/ // New factor graph for incremental update. - HybridNonlinearFactorGraph graph2; + graph = HybridNonlinearFactorGraph(); initial = Values(); - graph2.push_back(switching.unaryFactors.at(1)); // P(X1) - graph2.push_back(switching.unaryFactors.at(2)); // P(X2) - graph2.push_back(switching.modeChain.at(1)); // P(M0, M1) + graph.push_back(switching.unaryFactors.at(1)); // P(X1) + graph.push_back(switching.unaryFactors.at(2)); // P(X2) + graph.push_back(switching.modeChain.at(1)); // P(M0, M1) - isam.update(graph2, initial); + isam.update(graph, initial); bayesTree = isam.bayesTree(); // Check that after the second update we have @@ -103,7 +103,7 @@ TEST(HybridNonlinearISAM, IncrementalElimination) { TEST(HybridNonlinearISAM, IncrementalInference) { Switching switching(3); HybridNonlinearISAM isam; - HybridNonlinearFactorGraph graph1; + HybridNonlinearFactorGraph graph; Values initial; // Create initial factor graph @@ -112,16 +112,16 @@ TEST(HybridNonlinearISAM, IncrementalInference) { // X0 -*- X1 -*- X2 // | | // *-M0 - * - M1 - graph1.push_back(switching.unaryFactors.at(0)); // P(X0) - graph1.push_back(switching.binaryFactors.at(0)); // P(X0, X1 | M0) - graph1.push_back(switching.unaryFactors.at(1)); // P(X1) - graph1.push_back(switching.modeChain.at(0)); // P(M0) + graph.push_back(switching.unaryFactors.at(0)); // P(X0) + graph.push_back(switching.binaryFactors.at(0)); // P(X0, X1 | M0) + graph.push_back(switching.unaryFactors.at(1)); // P(X1) + graph.push_back(switching.modeChain.at(0)); // P(M0) initial.insert(X(0), 1); initial.insert(X(1), 2); // Run update step - isam.update(graph1, initial); + isam.update(graph, initial); HybridGaussianISAM bayesTree = isam.bayesTree(); auto discreteConditional_m0 = bayesTree[M(0)]->conditional()->asDiscrete(); @@ -129,16 +129,16 @@ TEST(HybridNonlinearISAM, IncrementalInference) { /********************************************************/ // New factor graph for incremental update. - HybridNonlinearFactorGraph graph2; + graph = HybridNonlinearFactorGraph(); initial = Values(); initial.insert(X(2), 3); - graph2.push_back(switching.binaryFactors.at(1)); // P(X1, X2 | M1) - graph2.push_back(switching.unaryFactors.at(2)); // P(X2) - graph2.push_back(switching.modeChain.at(1)); // P(M0, M1) + graph.push_back(switching.binaryFactors.at(1)); // P(X1, X2 | M1) + graph.push_back(switching.unaryFactors.at(2)); // P(X2) + graph.push_back(switching.modeChain.at(1)); // P(M0, M1) - isam.update(graph2, initial); + isam.update(graph, initial); bayesTree = isam.bayesTree(); /********************************************************/ @@ -199,18 +199,18 @@ TEST(HybridNonlinearISAM, IncrementalInference) { TEST(HybridNonlinearISAM, ApproxInference) { Switching switching(4); HybridNonlinearISAM incrementalHybrid; - HybridNonlinearFactorGraph graph1; + HybridNonlinearFactorGraph graph; Values initial; // Add the 3 hybrid factors, x0-x1, x1-x2, x2-x3 for (size_t i = 0; i < 3; i++) { - graph1.push_back(switching.binaryFactors.at(i)); + graph.push_back(switching.binaryFactors.at(i)); } // Add the Gaussian factors, 1 prior on X(0), // 3 measurements on X(1), X(2), X(3) for (size_t i = 0; i < 4; i++) { - graph1.push_back(switching.unaryFactors.at(i)); + graph.push_back(switching.unaryFactors.at(i)); initial.insert(X(i), i + 1); } @@ -228,7 +228,7 @@ TEST(HybridNonlinearISAM, ApproxInference) { .BaseEliminateable::eliminatePartialMultifrontal(ordering); size_t maxNrLeaves = 5; - incrementalHybrid.update(graph1, initial); + incrementalHybrid.update(graph, initial); HybridGaussianISAM bayesTree = incrementalHybrid.bayesTree(); bayesTree.prune(maxNrLeaves); @@ -307,19 +307,19 @@ TEST(HybridNonlinearISAM, ApproxInference) { TEST(HybridNonlinearISAM, IncrementalApproximate) { Switching switching(5); HybridNonlinearISAM incrementalHybrid; - HybridNonlinearFactorGraph graph1; + HybridNonlinearFactorGraph graph; Values initial; /***** Run Round 1 *****/ // Add the 3 hybrid factors, x0-x1, x1-x2, x2-x3 for (size_t i = 0; i < 3; i++) { - graph1.push_back(switching.binaryFactors.at(i)); + graph.push_back(switching.binaryFactors.at(i)); } // Add the Gaussian factors, 1 prior on X(0), // 3 measurements on X(1), X(2), X(3) for (size_t i = 0; i < 4; i++) { - graph1.push_back(switching.unaryFactors.at(i)); + graph.push_back(switching.unaryFactors.at(i)); initial.insert(X(i), i + 1); } @@ -328,7 +328,7 @@ TEST(HybridNonlinearISAM, IncrementalApproximate) { // Run update with pruning size_t maxComponents = 5; - incrementalHybrid.update(graph1, initial); + incrementalHybrid.update(graph, initial); incrementalHybrid.prune(maxComponents); HybridGaussianISAM bayesTree = incrementalHybrid.bayesTree(); @@ -345,14 +345,14 @@ TEST(HybridNonlinearISAM, IncrementalApproximate) { 5, bayesTree[X(3)]->conditional()->asHybrid()->nrComponents()); /***** Run Round 2 *****/ - HybridGaussianFactorGraph graph2; - graph2.push_back(switching.binaryFactors.at(3)); // x3-x4 - graph2.push_back(switching.unaryFactors.at(4)); // x4 measurement + graph = HybridGaussianFactorGraph(); + graph.push_back(switching.binaryFactors.at(3)); // x3-x4 + graph.push_back(switching.unaryFactors.at(4)); // x4 measurement initial = Values(); initial.insert(X(4), 5); // Run update with pruning a second time. - incrementalHybrid.update(graph2, initial); + incrementalHybrid.update(graph, initial); incrementalHybrid.prune(maxComponents); bayesTree = incrementalHybrid.bayesTree(); From 7c672bb91b1ad28acace110fffb4293e923d524d Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 28 Oct 2024 18:52:43 -0400 Subject: [PATCH 020/115] minor improvements --- gtsam/hybrid/tests/testHybridNonlinearISAM.cpp | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/gtsam/hybrid/tests/testHybridNonlinearISAM.cpp b/gtsam/hybrid/tests/testHybridNonlinearISAM.cpp index 94747ae790..e42adc4ecd 100644 --- a/gtsam/hybrid/tests/testHybridNonlinearISAM.cpp +++ b/gtsam/hybrid/tests/testHybridNonlinearISAM.cpp @@ -195,7 +195,7 @@ TEST(HybridNonlinearISAM, IncrementalInference) { } /* ****************************************************************************/ -// Test if we can approximately do the inference +// Test if we can approximately do the inference (using pruning) TEST(HybridNonlinearISAM, ApproxInference) { Switching switching(4); HybridNonlinearISAM incrementalHybrid; @@ -325,7 +325,6 @@ TEST(HybridNonlinearISAM, IncrementalApproximate) { // TODO(Frank): no mode chain? - // Run update with pruning size_t maxComponents = 5; incrementalHybrid.update(graph, initial); @@ -347,7 +346,7 @@ TEST(HybridNonlinearISAM, IncrementalApproximate) { /***** Run Round 2 *****/ graph = HybridGaussianFactorGraph(); graph.push_back(switching.binaryFactors.at(3)); // x3-x4 - graph.push_back(switching.unaryFactors.at(4)); // x4 measurement + graph.push_back(switching.unaryFactors.at(4)); // x4 measurement initial = Values(); initial.insert(X(4), 5); From 879bd2996312bf567269fa65c44e563b70ecf889 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Fri, 25 Oct 2024 10:17:33 -0700 Subject: [PATCH 021/115] 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 d2085f57ea..13de4eb290 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 022/115] EssentialTransferFactor --- gtsam/sfm/TransferFactor.h | 184 ++++++++++++++++++------- gtsam/sfm/tests/testTransferFactor.cpp | 76 ++++++++-- 2 files changed, 204 insertions(+), 56 deletions(-) diff --git a/gtsam/sfm/TransferFactor.h b/gtsam/sfm/TransferFactor.h index 13de4eb290..9704810dda 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); + + 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 2dca12c2ac..b8eed3b3a9 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 023/115] 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 0000000000..a7cac5dbf3 --- /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 024/115] New example now uses EssentialTransferFactor --- examples/EssentialViewGraphExample.cpp | 82 +++++++++++++------------- 1 file changed, 42 insertions(+), 40 deletions(-) diff --git a/examples/EssentialViewGraphExample.cpp b/examples/EssentialViewGraphExample.cpp index a7cac5dbf3..e6948cfab1 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)); } + + // 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, "errors: ", formatter); + graph.printErrors(initialEstimate, "Initial Errors:\n", formatter); - /* Optimize the graph and print results */ + // 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 025/115] 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 b8eed3b3a9..b1655e0a9c 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 026/115] 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 e6948cfab1..91b2bb0429 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 9704810dda..2964c01adf 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 027/115] 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 c8d1f1271c..d8d0ae1dfe 100644 --- a/python/gtsam/examples/SFMExample.py +++ b/python/gtsam/examples/SFMExample.py @@ -9,20 +9,22 @@ 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 028/115] 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 e81be74fcd..b80c952614 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 029/115] 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 bf1bf60300..3d95e4c061 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 b25e2fd628..5084355a8c 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 030/115] Copy dirty examples --- python/CMakeLists.txt | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/python/CMakeLists.txt b/python/CMakeLists.txt index 9070d191b9..f0fc3f796c 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 031/115] 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 142e65d7e1..1223ef13c8 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 032/115] 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 91b2bb0429..af254a94d8 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 0000000000..2d3df815ca --- /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 033/115] 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 7e008aec35..1520e596a2 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 725c1481f3..081688d48c 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 0000000000..319155dc91 --- /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 0000000000..e30da688c0 --- /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 a90de48e55..6421ce2641 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 020cab2f94..15e633923e 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 0000000000..d21d39f7fe --- /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 034/115] 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 1223ef13c8..80479eb789 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 2d3df815ca..a3c3136742 100644 --- a/python/gtsam/examples/EssentialViewGraphExample.py +++ b/python/gtsam/examples/EssentialViewGraphExample.py @@ -24,25 +24,20 @@ 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 0000000000..fe4d78e3c4 --- /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 035/115] 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 f493fe8f6e..80f694e976 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 3582a3249f..a75fe5ae1d 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 a3c3136742..d7d2cda388 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 fe4d78e3c4..0ba41762ee 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 036/115] 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 0000000000..f9c4335954 --- /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 037/115] Compare in F-manifold --- python/gtsam/examples/ViewGraphComparison.py | 75 +++++++++++--------- 1 file changed, 40 insertions(+), 35 deletions(-) diff --git a/python/gtsam/examples/ViewGraphComparison.py b/python/gtsam/examples/ViewGraphComparison.py index f9c4335954..fdaf028614 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() + F1, F2 = ground_truth + 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) + 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) - # 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() + elif method == "EssentialMatrix": 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}") + # 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 038/115] 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 a75fe5ae1d..99548d8ec0 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 039/115] 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 e30da688c0..f053f3d112 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 040/115] 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 af254a94d8..e21a36e167 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 23393fa20c..a33e1853d7 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 d7d2cda388..184d3f7e3e 100644 --- a/python/gtsam/examples/EssentialViewGraphExample.py +++ b/python/gtsam/examples/EssentialViewGraphExample.py @@ -18,10 +18,10 @@ 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 fdaf028614..dd9a3f3298 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 0ba41762ee..2dc41ec4bc 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 041/115] Run many trials --- python/gtsam/examples/ViewGraphComparison.py | 110 ++++++++++++------- 1 file changed, 69 insertions(+), 41 deletions(-) diff --git a/python/gtsam/examples/ViewGraphComparison.py b/python/gtsam/examples/ViewGraphComparison.py index dd9a3f3298..11048ccca2 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 the random number generator + rng = np.random.default_rng(seed=args.seed) + # Initialize results dictionary - results = {} + results = {method: {"distances": [], "final_error": []} for method in methods} - for method in methods: - print(f"Running method: {method}") + # 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 042/115] 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 11048ccca2..3f51d29a51 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 043/115] 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 6421ce2641..42d2fe5505 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 044/115] 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 3f51d29a51..e2cd77c954 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 045/115] 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 80f694e976..09c234630e 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 046/115] 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 e2cd77c954..d11fdf6f99 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 3c50f9387c4b84d3e1627de5e179e680c29279f2 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 29 Oct 2024 01:55:04 -0400 Subject: [PATCH 047/115] add pruning support for HybridNonlinearFactor --- gtsam/hybrid/HybridNonlinearFactor.cpp | 31 ++++++++++++++++++++++++++ gtsam/hybrid/HybridNonlinearFactor.h | 7 ++++++ 2 files changed, 38 insertions(+) diff --git a/gtsam/hybrid/HybridNonlinearFactor.cpp b/gtsam/hybrid/HybridNonlinearFactor.cpp index 6ffb955117..48c327156d 100644 --- a/gtsam/hybrid/HybridNonlinearFactor.cpp +++ b/gtsam/hybrid/HybridNonlinearFactor.cpp @@ -16,6 +16,7 @@ * @date Sep 12, 2024 */ +#include #include #include #include @@ -202,4 +203,34 @@ std::shared_ptr HybridNonlinearFactor::linearize( linearized_factors); } +/* *******************************************************************************/ +HybridNonlinearFactor::shared_ptr HybridNonlinearFactor::prune( + const DecisionTreeFactor& discreteProbs) const { + // Find keys in discreteProbs.keys() but not in this->keys(): + std::set mine(this->keys().begin(), this->keys().end()); + std::set theirs(discreteProbs.keys().begin(), + discreteProbs.keys().end()); + std::vector diff; + std::set_difference(theirs.begin(), theirs.end(), mine.begin(), mine.end(), + std::back_inserter(diff)); + + // Find maximum probability value for every combination of our keys. + Ordering keys(diff); + auto max = discreteProbs.max(keys); + + // Check the max value for every combination of our keys. + // If the max value is 0.0, we can prune the corresponding conditional. + auto pruner = + [&](const Assignment& choices, + const NonlinearFactorValuePair& pair) -> NonlinearFactorValuePair { + if (max->evaluate(choices) == 0.0) + return {nullptr, std::numeric_limits::infinity()}; + else + return pair; + }; + + FactorValuePairs prunedFactors = factors().apply(pruner); + return std::make_shared(discreteKeys(), prunedFactors); +} + } // namespace gtsam \ No newline at end of file diff --git a/gtsam/hybrid/HybridNonlinearFactor.h b/gtsam/hybrid/HybridNonlinearFactor.h index 325fa3eaab..e264b1d10d 100644 --- a/gtsam/hybrid/HybridNonlinearFactor.h +++ b/gtsam/hybrid/HybridNonlinearFactor.h @@ -166,6 +166,9 @@ class GTSAM_EXPORT HybridNonlinearFactor : public HybridFactor { /// @} + /// Getter for NonlinearFactor decision tree + const FactorValuePairs& factors() const { return factors_; } + /// Linearize specific nonlinear factors based on the assignment in /// discreteValues. GaussianFactor::shared_ptr linearize( @@ -176,6 +179,10 @@ class GTSAM_EXPORT HybridNonlinearFactor : public HybridFactor { std::shared_ptr linearize( const Values& continuousValues) const; + /// Prune this factor based on the discrete probabilities. + HybridNonlinearFactor::shared_ptr prune( + const DecisionTreeFactor& discreteProbs) const; + private: /// Helper struct to assist private constructor below. struct ConstructorHelper; From 3c06f0755139ec93e4b473b988c35fea8bc1adec Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 29 Oct 2024 01:56:03 -0400 Subject: [PATCH 048/115] check if NonlinearFactor is valid before linearizing --- gtsam/hybrid/HybridNonlinearFactor.cpp | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/gtsam/hybrid/HybridNonlinearFactor.cpp b/gtsam/hybrid/HybridNonlinearFactor.cpp index 48c327156d..6f52df9177 100644 --- a/gtsam/hybrid/HybridNonlinearFactor.cpp +++ b/gtsam/hybrid/HybridNonlinearFactor.cpp @@ -185,6 +185,11 @@ std::shared_ptr HybridNonlinearFactor::linearize( [continuousValues]( const std::pair& f) -> GaussianFactorValuePair { auto [factor, val] = f; + // Check if valid factor. If not, return null and infinite error. + if (!factor) { + return {nullptr, std::numeric_limits::infinity()}; + } + if (auto gaussian = std::dynamic_pointer_cast( factor->noiseModel())) { return {factor->linearize(continuousValues), From b4c7d3af81bf13f71ed19858b88869a592e7b8e3 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 29 Oct 2024 01:56:58 -0400 Subject: [PATCH 049/115] formatting --- gtsam/hybrid/HybridGaussianFactor.h | 1 + 1 file changed, 1 insertion(+) diff --git a/gtsam/hybrid/HybridGaussianFactor.h b/gtsam/hybrid/HybridGaussianFactor.h index 6c06c1c0a7..6d1064647e 100644 --- a/gtsam/hybrid/HybridGaussianFactor.h +++ b/gtsam/hybrid/HybridGaussianFactor.h @@ -148,6 +148,7 @@ class GTSAM_EXPORT HybridGaussianFactor : public HybridFactor { /// Getter for GaussianFactor decision tree const FactorValuePairs &factors() const { return factors_; } + /** * @brief Helper function to return factors and functional to create a * DecisionTree of Gaussian Factor Graphs. From 649da80c91ef68b2de2571e0fe14485dc652365c Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 29 Oct 2024 10:03:23 -0400 Subject: [PATCH 050/115] prune nonlinear factors in HybridNonlinearISAM --- gtsam/hybrid/HybridNonlinearISAM.cpp | 16 +++++++++++++++- 1 file changed, 15 insertions(+), 1 deletion(-) diff --git a/gtsam/hybrid/HybridNonlinearISAM.cpp b/gtsam/hybrid/HybridNonlinearISAM.cpp index 503afaa723..37a5560969 100644 --- a/gtsam/hybrid/HybridNonlinearISAM.cpp +++ b/gtsam/hybrid/HybridNonlinearISAM.cpp @@ -16,6 +16,7 @@ */ #include +#include #include #include @@ -39,7 +40,6 @@ void HybridNonlinearISAM::update(const HybridNonlinearFactorGraph& newFactors, if (newFactors.size() > 0) { // Reorder and relinearize every reorderInterval updates if (reorderInterval_ > 0 && ++reorderCounter_ >= reorderInterval_) { - // TODO(Varun) Re-linearization doesn't take into account pruning reorderRelinearize(); reorderCounter_ = 0; } @@ -65,8 +65,22 @@ void HybridNonlinearISAM::reorderRelinearize() { // Obtain the new linearization point const Values newLinPoint = estimate(); + auto discreteProbs = *(isam_.roots().at(0)->conditional()->asDiscrete()); + isam_.clear(); + // Prune nonlinear factors based on discrete conditional probabilities + HybridNonlinearFactorGraph pruned_factors; + for (auto&& factor : factors_) { + if (auto nf = std::dynamic_pointer_cast(factor)) { + pruned_factors.push_back(nf->prune(discreteProbs)); + } else { + pruned_factors.push_back(factor); + } + } + factors_ = pruned_factors; + factors_.print("OG factors"); + // Just recreate the whole BayesTree // TODO: allow for constrained ordering here // TODO: decouple re-linearization and reordering to avoid From 4d5f1c0f43523a0074f17f18cb9ff93860410da8 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 29 Oct 2024 10:37:17 -0400 Subject: [PATCH 051/115] formatting --- gtsam/hybrid/HybridNonlinearFactor.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/hybrid/HybridNonlinearFactor.cpp b/gtsam/hybrid/HybridNonlinearFactor.cpp index 6f52df9177..376bc66f1f 100644 --- a/gtsam/hybrid/HybridNonlinearFactor.cpp +++ b/gtsam/hybrid/HybridNonlinearFactor.cpp @@ -238,4 +238,4 @@ HybridNonlinearFactor::shared_ptr HybridNonlinearFactor::prune( return std::make_shared(discreteKeys(), prunedFactors); } -} // namespace gtsam \ No newline at end of file +} // namespace gtsam From aa0db60a52700b51d4514c665550905363f9cd84 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Tue, 29 Oct 2024 09:21:19 -0700 Subject: [PATCH 052/115] 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 d11fdf6f99..5c3c2c1761 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 @@ 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 053/115] 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 5c3c2c1761..9410240ba8 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 095a4cd1876ba9085e9d1cb74ab1eb8854721972 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 29 Oct 2024 13:09:27 -0400 Subject: [PATCH 054/115] remove print --- gtsam/hybrid/HybridNonlinearISAM.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/gtsam/hybrid/HybridNonlinearISAM.cpp b/gtsam/hybrid/HybridNonlinearISAM.cpp index 37a5560969..29e467d866 100644 --- a/gtsam/hybrid/HybridNonlinearISAM.cpp +++ b/gtsam/hybrid/HybridNonlinearISAM.cpp @@ -79,7 +79,6 @@ void HybridNonlinearISAM::reorderRelinearize() { } } factors_ = pruned_factors; - factors_.print("OG factors"); // Just recreate the whole BayesTree // TODO: allow for constrained ordering here From 3e14b7194f69734d84462f5b5e880df481d04d3a Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Tue, 29 Oct 2024 10:30:22 -0700 Subject: [PATCH 055/115] 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 9410240ba8..da8b068038 100644 --- a/python/gtsam/examples/ViewGraphComparison.py +++ b/python/gtsam/examples/ViewGraphComparison.py @@ -32,7 +32,7 @@ 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 ae95c6e84aed4377df50367f3e1859418fa7bcb3 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 29 Oct 2024 13:50:10 -0400 Subject: [PATCH 056/115] clean up tests and TODOs --- gtsam/hybrid/tests/testHybridBayesTree.cpp | 25 +++++----------------- 1 file changed, 5 insertions(+), 20 deletions(-) diff --git a/gtsam/hybrid/tests/testHybridBayesTree.cpp b/gtsam/hybrid/tests/testHybridBayesTree.cpp index 6da991173a..db298e6fcc 100644 --- a/gtsam/hybrid/tests/testHybridBayesTree.cpp +++ b/gtsam/hybrid/tests/testHybridBayesTree.cpp @@ -74,9 +74,7 @@ TEST(HybridGaussianFactorGraph, hfg.add(HybridGaussianFactor(m1, two::components(X(1)))); hfg.add(DecisionTreeFactor(m1, {2, 8})); - // TODO(Varun) Adding extra discrete variable not connected to continuous - // variable throws segfault - // hfg.add(DecisionTreeFactor({m1, m2, "1 2 3 4")); + hfg.add(DecisionTreeFactor({m1, m2}, "1 2 3 4")); HybridBayesTree::shared_ptr result = hfg.eliminateMultifrontal(); @@ -176,7 +174,7 @@ TEST(HybridGaussianFactorGraph, Switching) { // Ordering(KeyVector{X(1), X(4), X(2), X(6), X(9), X(8), X(3), X(7), // X(5), // M(1), M(4), M(2), M(6), M(8), M(3), M(7), M(5)}); - KeyVector ordering; + Ordering ordering; { std::vector naturalX(N); @@ -187,10 +185,6 @@ TEST(HybridGaussianFactorGraph, Switching) { auto [ndX, lvls] = makeBinaryOrdering(ordX); std::copy(ndX.begin(), ndX.end(), std::back_inserter(ordering)); - // TODO(dellaert): this has no effect! - for (auto& l : lvls) { - l = -l; - } } { std::vector naturalC(N - 1); @@ -199,14 +193,11 @@ TEST(HybridGaussianFactorGraph, Switching) { std::transform(naturalC.begin(), naturalC.end(), std::back_inserter(ordC), [](int x) { return M(x); }); - // std::copy(ordC.begin(), ordC.end(), std::back_inserter(ordering)); const auto [ndC, lvls] = makeBinaryOrdering(ordC); std::copy(ndC.begin(), ndC.end(), std::back_inserter(ordering)); } - auto ordering_full = Ordering(ordering); - const auto [hbt, remaining] = - hfg->eliminatePartialMultifrontal(ordering_full); + const auto [hbt, remaining] = hfg->eliminatePartialMultifrontal(ordering); // 12 cliques in the bayes tree and 0 remaining variables to eliminate. EXPECT_LONGS_EQUAL(12, hbt->size()); @@ -230,7 +221,7 @@ TEST(HybridGaussianFactorGraph, SwitchingISAM) { // Ordering(KeyVector{X(1), X(4), X(2), X(6), X(9), X(8), X(3), X(7), // X(5), // M(1), M(4), M(2), M(6), M(8), M(3), M(7), M(5)}); - KeyVector ordering; + Ordering ordering; { std::vector naturalX(N); @@ -241,10 +232,6 @@ TEST(HybridGaussianFactorGraph, SwitchingISAM) { auto [ndX, lvls] = makeBinaryOrdering(ordX); std::copy(ndX.begin(), ndX.end(), std::back_inserter(ordering)); - // TODO(dellaert): this has no effect! - for (auto& l : lvls) { - l = -l; - } } { std::vector naturalC(N - 1); @@ -257,10 +244,8 @@ TEST(HybridGaussianFactorGraph, SwitchingISAM) { const auto [ndC, lvls] = makeBinaryOrdering(ordC); std::copy(ndC.begin(), ndC.end(), std::back_inserter(ordering)); } - auto ordering_full = Ordering(ordering); - const auto [hbt, remaining] = - hfg->eliminatePartialMultifrontal(ordering_full); + const auto [hbt, remaining] = hfg->eliminatePartialMultifrontal(ordering); auto new_fg = makeSwitchingChain(12); auto isam = HybridGaussianISAM(*hbt); From 6b96ae217f58327c66870757d32cf8a1c0b529e2 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Tue, 29 Oct 2024 10:58:08 -0700 Subject: [PATCH 057/115] 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 5a50b4fd2d..94c72673d3 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 7e87ba62a7..a36c815da1 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 00876e415e..eed5da734c 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 058/115] 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 da8b068038..968bbc9dfa 100644 --- a/python/gtsam/examples/ViewGraphComparison.py +++ b/python/gtsam/examples/ViewGraphComparison.py @@ -32,7 +32,7 @@ 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 dfc91469bceb135cbec6e0f038be45c89dcde576 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 29 Oct 2024 14:45:19 -0400 Subject: [PATCH 059/115] discreteFactors method --- gtsam/hybrid/HybridGaussianFactorGraph.cpp | 11 +++++++++++ gtsam/hybrid/HybridGaussianFactorGraph.h | 8 ++++++++ gtsam/hybrid/tests/testHybridBayesTree.cpp | 7 +------ gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp | 9 ++------- 4 files changed, 22 insertions(+), 13 deletions(-) diff --git a/gtsam/hybrid/HybridGaussianFactorGraph.cpp b/gtsam/hybrid/HybridGaussianFactorGraph.cpp index ceabe0871a..049e6c38d2 100644 --- a/gtsam/hybrid/HybridGaussianFactorGraph.cpp +++ b/gtsam/hybrid/HybridGaussianFactorGraph.cpp @@ -580,4 +580,15 @@ GaussianFactorGraph HybridGaussianFactorGraph::choose( return gfg; } +/* ************************************************************************ */ +DiscreteFactorGraph HybridGaussianFactorGraph::discreteFactors() const { + DiscreteFactorGraph dfg; + for (auto &&f : factors_) { + auto discreteFactor = std::dynamic_pointer_cast(f); + assert(discreteFactor); + dfg.push_back(discreteFactor); + } + return dfg; +} + } // namespace gtsam diff --git a/gtsam/hybrid/HybridGaussianFactorGraph.h b/gtsam/hybrid/HybridGaussianFactorGraph.h index 048fd2701e..c2e50ace84 100644 --- a/gtsam/hybrid/HybridGaussianFactorGraph.h +++ b/gtsam/hybrid/HybridGaussianFactorGraph.h @@ -254,6 +254,14 @@ class GTSAM_EXPORT HybridGaussianFactorGraph GaussianFactorGraph operator()(const DiscreteValues& assignment) const { return choose(assignment); } + + /** + * @brief Helper method to get all the discrete factors + * as a DiscreteFactorGraph. + * + * @return DiscreteFactorGraph + */ + DiscreteFactorGraph discreteFactors() const; }; // traits diff --git a/gtsam/hybrid/tests/testHybridBayesTree.cpp b/gtsam/hybrid/tests/testHybridBayesTree.cpp index db298e6fcc..4f5583bf55 100644 --- a/gtsam/hybrid/tests/testHybridBayesTree.cpp +++ b/gtsam/hybrid/tests/testHybridBayesTree.cpp @@ -443,12 +443,7 @@ TEST(HybridBayesTree, Optimize) { const auto [hybridBayesNet, remainingFactorGraph] = s.linearizedFactorGraph.eliminatePartialSequential(ordering); - DiscreteFactorGraph dfg; - for (auto&& f : *remainingFactorGraph) { - auto discreteFactor = dynamic_pointer_cast(f); - assert(discreteFactor); - dfg.push_back(discreteFactor); - } + DiscreteFactorGraph dfg = remainingFactorGraph->discreteFactors(); // Add the probabilities for each branch DiscreteKeys discrete_keys = {{M(0), 2}, {M(1), 2}, {M(2), 2}}; diff --git a/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp b/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp index dd41280349..100eb024ad 100644 --- a/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp +++ b/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp @@ -479,13 +479,8 @@ TEST(HybridNonlinearFactorGraph, Full_Elimination) { const auto [hybridBayesNet_partial, remainingFactorGraph_partial] = linearizedFactorGraph.eliminatePartialSequential(ordering); - DiscreteFactorGraph discrete_fg; - // TODO(Varun) Make this a function of HybridGaussianFactorGraph? - for (auto &factor : (*remainingFactorGraph_partial)) { - auto df = dynamic_pointer_cast(factor); - assert(df); - discrete_fg.push_back(df); - } + DiscreteFactorGraph discrete_fg = + remainingFactorGraph_partial->discreteFactors(); ordering.clear(); for (size_t k = 0; k < self.K - 1; k++) ordering.push_back(M(k)); From 6734cd332ff540d8cfb92a044fe9985decf56aee Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 29 Oct 2024 14:45:43 -0400 Subject: [PATCH 060/115] formatting --- gtsam/hybrid/tests/Switching.h | 13 ++++++++----- 1 file changed, 8 insertions(+), 5 deletions(-) diff --git a/gtsam/hybrid/tests/Switching.h b/gtsam/hybrid/tests/Switching.h index fdf9092b6c..270792701d 100644 --- a/gtsam/hybrid/tests/Switching.h +++ b/gtsam/hybrid/tests/Switching.h @@ -52,7 +52,8 @@ using symbol_shorthand::X; * @return HybridGaussianFactorGraph::shared_ptr */ inline HybridGaussianFactorGraph::shared_ptr makeSwitchingChain( - size_t K, std::function x = X, std::function m = M) { + size_t K, std::function x = X, std::function m = M, + const std::string &transitionProbabilityTable = "0 1 1 3") { HybridGaussianFactorGraph hfg; hfg.add(JacobianFactor(x(1), I_3x3, Z_3x1)); @@ -68,7 +69,8 @@ inline HybridGaussianFactorGraph::shared_ptr makeSwitchingChain( hfg.add(HybridGaussianFactor({m(k), 2}, components)); if (k > 1) { - hfg.add(DecisionTreeFactor({{m(k - 1), 2}, {m(k), 2}}, "0 1 1 3")); + hfg.add(DecisionTreeFactor({{m(k - 1), 2}, {m(k), 2}}, + transitionProbabilityTable)); } } @@ -171,8 +173,8 @@ struct Switching { // Add "motion models" ϕ(X(k),X(k+1),M(k)). for (size_t k = 0; k < K - 1; k++) { auto motion_models = motionModels(k, between_sigma); - nonlinearFactorGraph_.emplace_shared(modes[k], - motion_models); + nonlinearFactorGraph_.emplace_shared( + modes[k], motion_models); binaryFactors.push_back(nonlinearFactorGraph_.back()); } @@ -193,7 +195,8 @@ struct Switching { linearizationPoint.insert(X(k), static_cast(k + 1)); } - linearizedFactorGraph = *nonlinearFactorGraph_.linearize(linearizationPoint); + linearizedFactorGraph = + *nonlinearFactorGraph_.linearize(linearizationPoint); } // Create motion models for a given time step From c68858d7b672500f237f5c0b74c21384fbb96359 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Tue, 29 Oct 2024 11:55:55 -0700 Subject: [PATCH 061/115] 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 94c72673d3..406269ff57 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 a36c815da1..beb2947e2c 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 062/115] 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 968bbc9dfa..d8340e393c 100644 --- a/python/gtsam/examples/ViewGraphComparison.py +++ b/python/gtsam/examples/ViewGraphComparison.py @@ -32,7 +32,7 @@ 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 64579373be8bd1d84536ccce93517315c9297b20 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Tue, 29 Oct 2024 10:58:08 -0700 Subject: [PATCH 063/115] 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 5a50b4fd2d..94c72673d3 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 7e87ba62a7..a36c815da1 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 00876e415e..eed5da734c 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 5a2f1f889331f004a621610a3583646221732f4a Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Tue, 29 Oct 2024 11:55:55 -0700 Subject: [PATCH 064/115] 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 94c72673d3..406269ff57 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 a36c815da1..beb2947e2c 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 8bd4738f47a4a5f81b694f4985183f59ec069e0e Mon Sep 17 00:00:00 2001 From: Jose Luis Blanco-Claraco Date: Tue, 29 Oct 2024 08:05:27 +0100 Subject: [PATCH 065/115] =?UTF-8?q?Fix=20warning:=20=E2=80=98beta=E2=80=99?= =?UTF-8?q?=20may=20be=20used=20uninitialized=20in=20this=20function?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- gtsam/nonlinear/NonlinearConjugateGradientOptimizer.h | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/gtsam/nonlinear/NonlinearConjugateGradientOptimizer.h b/gtsam/nonlinear/NonlinearConjugateGradientOptimizer.h index e3d90a5913..a36fa09ed3 100644 --- a/gtsam/nonlinear/NonlinearConjugateGradientOptimizer.h +++ b/gtsam/nonlinear/NonlinearConjugateGradientOptimizer.h @@ -21,6 +21,8 @@ #include #include +#include + namespace gtsam { /// Fletcher-Reeves formula for computing β, the direction of steepest descent. @@ -247,6 +249,9 @@ std::tuple nonlinearConjugateGradient( case DirectionMethod::DaiYuan: beta = DaiYuan(currentGradient, prevGradient, direction); break; + default: + throw std::runtime_error( + "NonlinearConjugateGradientOptimizer: Invalid directionMethod"); } direction = currentGradient + (beta * direction); From 7f07509388cf5d8041571ba44210fd15d650544a Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Wed, 30 Oct 2024 13:51:39 -0400 Subject: [PATCH 066/115] remove linearizedFactorGraph and use linearized unary and binary factors --- gtsam/hybrid/tests/Switching.h | 35 ++++++++++++++++------------------ 1 file changed, 16 insertions(+), 19 deletions(-) diff --git a/gtsam/hybrid/tests/Switching.h b/gtsam/hybrid/tests/Switching.h index fdf9092b6c..66dbc5fdc7 100644 --- a/gtsam/hybrid/tests/Switching.h +++ b/gtsam/hybrid/tests/Switching.h @@ -120,21 +120,13 @@ using MotionModel = BetweenFactor; // Test fixture with switching network. /// ϕ(X(0)) .. ϕ(X(k),X(k+1)) .. ϕ(X(k);z_k) .. ϕ(M(0)) .. ϕ(M(K-3),M(K-2)) struct Switching { - private: - HybridNonlinearFactorGraph nonlinearFactorGraph_; - public: size_t K; DiscreteKeys modes; HybridNonlinearFactorGraph unaryFactors, binaryFactors, modeChain; - HybridGaussianFactorGraph linearizedFactorGraph; + HybridGaussianFactorGraph linearUnaryFactors, linearBinaryFactors; Values linearizationPoint; - // Access the flat nonlinear factor graph. - const HybridNonlinearFactorGraph &nonlinearFactorGraph() const { - return nonlinearFactorGraph_; - } - /** * @brief Create with given number of time steps. * @@ -164,36 +156,33 @@ struct Switching { // Create hybrid factor graph. // Add a prior ϕ(X(0)) on X(0). - nonlinearFactorGraph_.emplace_shared>( + unaryFactors.emplace_shared>( X(0), measurements.at(0), Isotropic::Sigma(1, prior_sigma)); - unaryFactors.push_back(nonlinearFactorGraph_.back()); // Add "motion models" ϕ(X(k),X(k+1),M(k)). for (size_t k = 0; k < K - 1; k++) { auto motion_models = motionModels(k, between_sigma); - nonlinearFactorGraph_.emplace_shared(modes[k], - motion_models); - binaryFactors.push_back(nonlinearFactorGraph_.back()); + binaryFactors.emplace_shared(modes[k], + motion_models); } // Add measurement factors ϕ(X(k);z_k). auto measurement_noise = Isotropic::Sigma(1, prior_sigma); for (size_t k = 1; k < K; k++) { - nonlinearFactorGraph_.emplace_shared>( - X(k), measurements.at(k), measurement_noise); - unaryFactors.push_back(nonlinearFactorGraph_.back()); + unaryFactors.emplace_shared>(X(k), measurements.at(k), + measurement_noise); } // Add "mode chain" ϕ(M(0)) ϕ(M(0),M(1)) ... ϕ(M(K-3),M(K-2)) modeChain = createModeChain(transitionProbabilityTable); - nonlinearFactorGraph_ += modeChain; // Create the linearization point. for (size_t k = 0; k < K; k++) { linearizationPoint.insert(X(k), static_cast(k + 1)); } - linearizedFactorGraph = *nonlinearFactorGraph_.linearize(linearizationPoint); + linearUnaryFactors = *unaryFactors.linearize(linearizationPoint); + linearBinaryFactors = *binaryFactors.linearize(linearizationPoint); } // Create motion models for a given time step @@ -224,6 +213,14 @@ struct Switching { } return chain; } + + HybridGaussianFactorGraph linearizedFactorGraph() { + HybridGaussianFactorGraph graph; + graph.push_back(linearUnaryFactors); + graph.push_back(linearBinaryFactors); + graph.push_back(modeChain); + return graph; + } }; } // namespace gtsam From 38ffcc3e5cf3994e782913dbcd84c94fd352cb4e Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Wed, 30 Oct 2024 12:04:04 -0700 Subject: [PATCH 067/115] Switch measured/reference language. --- gtsam/navigation/AttitudeFactor.cpp | 30 +++++----- gtsam/navigation/AttitudeFactor.h | 56 ++++++++++-------- gtsam/navigation/AttitudeFactor.md | 58 +++++++++---------- gtsam/navigation/navigation.i | 28 ++++----- gtsam/navigation/tests/testAttitudeFactor.cpp | 39 ++++++------- 5 files changed, 104 insertions(+), 107 deletions(-) diff --git a/gtsam/navigation/AttitudeFactor.cpp b/gtsam/navigation/AttitudeFactor.cpp index 8c8eb5772f..1ca161ebb3 100644 --- a/gtsam/navigation/AttitudeFactor.cpp +++ b/gtsam/navigation/AttitudeFactor.cpp @@ -26,16 +26,16 @@ namespace gtsam { Vector AttitudeFactor::attitudeError(const Rot3& nRb, OptionalJacobian<2, 3> H) const { if (H) { - Matrix23 D_nRef_R; - Matrix22 D_e_nRef; - Unit3 nRef = nRb.rotate(bRef_, D_nRef_R); - Vector e = nZ_.error(nRef, D_e_nRef); + Matrix23 D_nRotated_R; + Matrix22 D_e_nRotated; + Unit3 nRotated = nRb.rotate(bMeasured_, D_nRotated_R); + Vector e = nRef_.error(nRotated, D_e_nRotated); - (*H) = D_e_nRef * D_nRef_R; + (*H) = D_e_nRotated * D_nRotated_R; return e; } else { - Unit3 nRef = nRb * bRef_; - return nZ_.error(nRef); + Unit3 nRotated = nRb * bMeasured_; + return nRef_.error(nRotated); } } @@ -44,8 +44,8 @@ void Rot3AttitudeFactor::print(const string& s, const KeyFormatter& keyFormatter) const { cout << (s.empty() ? "" : s + " ") << "Rot3AttitudeFactor on " << keyFormatter(this->key()) << "\n"; - nZ_.print(" measured direction in nav frame: "); - bRef_.print(" reference direction in body frame: "); + nRef_.print(" reference direction in nav frame: "); + bMeasured_.print(" measured direction in body frame: "); this->noiseModel_->print(" noise model: "); } @@ -53,16 +53,16 @@ void Rot3AttitudeFactor::print(const string& s, bool Rot3AttitudeFactor::equals(const NonlinearFactor& expected, double tol) const { const This* e = dynamic_cast(&expected); - return e != nullptr && Base::equals(*e, tol) && this->nZ_.equals(e->nZ_, tol) - && this->bRef_.equals(e->bRef_, tol); + return e != nullptr && Base::equals(*e, tol) && this->nRef_.equals(e->nRef_, tol) + && this->bMeasured_.equals(e->bMeasured_, tol); } //*************************************************************************** void Pose3AttitudeFactor::print(const string& s, const KeyFormatter& keyFormatter) const { cout << s << "Pose3AttitudeFactor on " << keyFormatter(this->key()) << "\n"; - nZ_.print(" measured direction in nav frame: "); - bRef_.print(" reference direction in body frame: "); + nRef_.print(" reference direction in nav frame: "); + bMeasured_.print(" measured direction in body frame: "); this->noiseModel_->print(" noise model: "); } @@ -70,8 +70,8 @@ void Pose3AttitudeFactor::print(const string& s, bool Pose3AttitudeFactor::equals(const NonlinearFactor& expected, double tol) const { const This* e = dynamic_cast(&expected); - return e != nullptr && Base::equals(*e, tol) && this->nZ_.equals(e->nZ_, tol) - && this->bRef_.equals(e->bRef_, tol); + return e != nullptr && Base::equals(*e, tol) && this->nRef_.equals(e->nRef_, tol) + && this->bMeasured_.equals(e->bMeasured_, tol); } //*************************************************************************** diff --git a/gtsam/navigation/AttitudeFactor.h b/gtsam/navigation/AttitudeFactor.h index 676e0a46d8..2fada724d1 100644 --- a/gtsam/navigation/AttitudeFactor.h +++ b/gtsam/navigation/AttitudeFactor.h @@ -29,13 +29,13 @@ namespace gtsam { * @brief Base class for an attitude factor that constrains the rotation between * body and navigation frames. * - * This factor enforces that when a known reference direction in the body frame - * (e.g., accelerometer axis) is rotated into the navigation frame using the - * rotation variable, it should align with a measured direction in the + * This factor enforces that when the measured direction in the body frame + * (e.g., from an IMU accelerometer) is rotated into the navigation frame using the + * rotation variable, it should align with a known reference direction in the * navigation frame (e.g., gravity vector). * * Mathematically, the error is zero when: - * nRb * bRef == nZ + * nRb * bMeasured == nRef * * This is useful for incorporating absolute orientation measurements into the * factor graph. @@ -44,7 +44,7 @@ namespace gtsam { */ class AttitudeFactor { protected: - Unit3 nZ_, bRef_; ///< Position measurement in + Unit3 nRef_, bMeasured_; ///< Position measurement in public: /** default constructor - only use for serialization */ @@ -52,26 +52,34 @@ class AttitudeFactor { /** * @brief Constructor - * @param nZ Measured direction in the navigation frame (e.g., gravity). - * @param bRef Reference direction in the body frame (e.g., accelerometer - * axis). Default is Unit3(0, 0, 1). + * @param nRef Reference direction in the navigation frame (e.g., gravity). + * @param bMeasured Measured direction in the body frame (e.g., from IMU + * accelerometer). Default is Unit3(0, 0, 1). */ - AttitudeFactor(const Unit3& nZ, const Unit3& bRef = Unit3(0, 0, 1)) - : nZ_(nZ), bRef_(bRef) {} + AttitudeFactor(const Unit3& nRef, const Unit3& bMeasured = Unit3(0, 0, 1)) + : nRef_(nRef), bMeasured_(bMeasured) {} /** vector of errors */ Vector attitudeError(const Rot3& p, OptionalJacobian<2, 3> H = {}) const; - const Unit3& nZ() const { return nZ_; } - const Unit3& bRef() const { return bRef_; } + const Unit3& nRef() const { return nRef_; } + const Unit3& bMeasured() const { return bMeasured_; } + +#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V43 + [[deprecated("Use nRef() instead")]] + const Unit3& nZ() const { return nRef_; } + + [[deprecated("Use bMeasured() instead")]] + const Unit3& bRef() const { return bMeasured_; } +#endif #ifdef GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template void serialize(ARCHIVE& ar, const unsigned int /*version*/) { - ar& boost::serialization::make_nvp("nZ_", nZ_); - ar& boost::serialization::make_nvp("bRef_", bRef_); + ar& boost::serialization::make_nvp("nRef_", nRef_); + ar& boost::serialization::make_nvp("bMeasured_", bMeasured_); } #endif }; @@ -102,13 +110,13 @@ class GTSAM_EXPORT Rot3AttitudeFactor : public NoiseModelFactorN, /** * @brief Constructor * @param key of the Rot3 variable that will be constrained - * @param nZ measured direction in navigation frame + * @param nRef reference direction in navigation frame * @param model Gaussian noise model - * @param bRef reference direction in body frame (default Z-axis) + * @param bMeasured measured direction in body frame (default Z-axis) */ - Rot3AttitudeFactor(Key key, const Unit3& nZ, const SharedNoiseModel& model, - const Unit3& bRef = Unit3(0, 0, 1)) - : Base(model, key), AttitudeFactor(nZ, bRef) {} + Rot3AttitudeFactor(Key key, const Unit3& nRef, const SharedNoiseModel& model, + const Unit3& bMeasured = Unit3(0, 0, 1)) + : Base(model, key), AttitudeFactor(nRef, bMeasured) {} /// @return a deep copy of this factor gtsam::NonlinearFactor::shared_ptr clone() const override { @@ -178,13 +186,13 @@ class GTSAM_EXPORT Pose3AttitudeFactor : public NoiseModelFactorN, /** * @brief Constructor * @param key of the Pose3 variable that will be constrained - * @param nZ measured direction in navigation frame + * @param nRef reference direction in navigation frame * @param model Gaussian noise model - * @param bRef reference direction in body frame (default Z-axis) + * @param bMeasured measured direction in body frame (default Z-axis) */ - Pose3AttitudeFactor(Key key, const Unit3& nZ, const SharedNoiseModel& model, - const Unit3& bRef = Unit3(0, 0, 1)) - : Base(model, key), AttitudeFactor(nZ, bRef) {} + Pose3AttitudeFactor(Key key, const Unit3& nRef, const SharedNoiseModel& model, + const Unit3& bMeasured = Unit3(0, 0, 1)) + : Base(model, key), AttitudeFactor(nRef, bMeasured) {} /// @return a deep copy of this factor gtsam::NonlinearFactor::shared_ptr clone() const override { diff --git a/gtsam/navigation/AttitudeFactor.md b/gtsam/navigation/AttitudeFactor.md index beb52b0b6e..a4d45b7835 100644 --- a/gtsam/navigation/AttitudeFactor.md +++ b/gtsam/navigation/AttitudeFactor.md @@ -12,24 +12,24 @@ This document explains the mathematical foundation of the `AttitudeFactor` and g ### Concept -The `AttitudeFactor` constrains the rotation $\mathbf{R}_{nb}$ (from body frame $b$ to navigation frame $n$) such that a known reference direction in the body frame aligns with a measured direction in the navigation frame when rotated. The factor enforces that: +The `AttitudeFactor` constrains the rotation $\mathbf{R}_{nb}$ (from body frame $b$ to navigation frame $n$) such that a known reference direction in the navigation frame aligns with a measured direction in the body frame, when rotated. The factor enforces that: $$ -\mathbf{R}_{nb} \cdot \mathbf{b}_{\text{Ref}} \approx \mathbf{nZ} +\text{nRef} \approx \mathbf{R}_{nb} \cdot \text{bMeasured} $$ where: - $\mathbf{R}_{nb}$ is the rotation matrix representing the orientation from body to navigation frame. -- $\mathbf{b}_{\text{Ref}}$ is a known reference direction in the body frame (e.g., the accelerometer's sensitive axis). -- $\mathbf{nZ}$ is the measured direction in the navigation frame (e.g., the gravity vector measured by an IMU). +- $\text{bMeasured}$ is the measured direction in the body frame (e.g., the accelerometer reading). +- $\text{nRef}$ is the known reference direction in the navigation frame (e.g., the gravity vector in nav). ### Error Function The error function computes the angular difference between the rotated reference direction and the measured direction: $$ -\mathbf{e} = \text{error}(\mathbf{nZ}, \mathbf{R}_{nb} \cdot \mathbf{b}_{\text{Ref}}) +\mathbf{e} = \text{error}(\text{nRef}, \mathbf{R}_{nb} \cdot \text{bMeasured}) $$ This error is minimal (zero) when the rotated body reference direction aligns perfectly with the measured navigation direction. @@ -38,19 +38,21 @@ The error is computed internally using the `attitudeError` function: ```cpp Vector AttitudeFactor::attitudeError(const Rot3& nRb) const { - Unit3 nRef = nRb * bRef_; - return nZ_.error(nRef); + Unit3 nRotated = nRb * bMeasured_; + return nRef_.error(nRotated); } ``` #### Explanation: -- The function computes the rotated reference direction `nRef` and then the error between `nRef` and the measured direction `nZ`. -- `nZ_.error(nRef)` is a 2D vector-valued error between two directions, defined in [Unit3.h](../geometry/Unit3.h). +- The function computes the rotated measurement `nRotated` and then the error between `nRef` and `nRotated`. +- `nRef_.error(nRotated)` is a 2D vector-valued error between two directions, defined in [Unit3.h](../geometry/Unit3.h). ### Jacobians For optimization, the $2 \times 3$ Jacobian of the error function with respect to the rotation parameters is required. The Jacobian is computed using chain rule differentiation, involving the derivative of the rotated vector with respect to the rotation parameters and the derivative of the error with respect to the rotated vector. +Note the Jacobian for this specific error function vanishes 180 degrees away from the true direction, and the factor is only expected to behave well when the `nRb` value is initialized in the correct hemisphere. + ## Usage in GTSAM ### Including the Header @@ -71,55 +73,49 @@ You can create an attitude factor for either a `Rot3` (rotation only) or a `Pose // Define keys gtsam::Key rotKey = ...; -// Measured direction in navigation frame (e.g., gravity) -gtsam::Unit3 nZ(0, 0, -1); // Assuming gravity points down in navigation frame +// Known reference direction in navigation frame (e.g., gravity) +gtsam::Unit3 nRef(0, 0, -1); // Assuming gravity points down in navigation frame -// Reference direction in body frame (e.g., accelerometer axis) -gtsam::Unit3 bRef(0, 0, 1); // Default is the Z-axis +// Measured direction in body frame (e.g., accelerometer direction) +gtsam::Unit3 bMeasured(0, 0, 9.8); // will be normalized automatically // Noise model auto noiseModel = gtsam::noiseModel::Isotropic::Sigma(2, 0.1); // 2D error, sigma = 0.1 // Add to factor graph gtsam::NonlinearFactorGraph graph; -graph.emplace_shared(rotKey, nZ, noiseModel, bRef); +graph.emplace_shared(rotKey, nRef, noiseModel, bMeasured); ``` #### For `Pose3` Variables There is also a `Pose3AttitudeFactor` that automatically extracts the rotation from the pose, taking into account the chain rule for this operation so the Jacobians with respect to pose are correct. +The same caveat about vanishing Jacobian holds. ```cpp // Define keys gtsam::Key poseKey = ...; -// Measured direction in navigation frame -gtsam::Unit3 nZ(0, 0, -1); +// Known reference direction in navigation frame (e.g., gravity) +gtsam::Unit3 nRef(0, 0, -1); // Assuming gravity points down in navigation frame -// Reference direction in body frame -gtsam::Unit3 bRef(0, 0, 1); +// Measured direction in body frame (e.g., accelerometer direction) +gtsam::Unit3 bMeasured(0, 0, 9.8); // will be normalized automatically // Noise model auto noiseModel = gtsam::noiseModel::Isotropic::Sigma(2, 0.1); // Add to factor graph gtsam::NonlinearFactorGraph graph; -graph.emplace_shared(poseKey, nZ, noiseModel, bRef); +graph.emplace_shared(poseKey, nRef, noiseModel, bMeasured); ``` ### Explanation of Parameters - **Key**: The variable key in the factor graph corresponding to the `Rot3` or `Pose3` variable you are constraining. -- **nZ**: The measured direction in the navigation frame. This is typically obtained from sensors like accelerometers or magnetometers. -- **bRef**: The known reference direction in the body frame. By default, this is set to the Z-axis `[0; 0; 1]`, but it should match the direction your sensor measures. -- **noiseModel**: The noise model representing the uncertainty in the measurement. - -## Practical Tips - -- **Gravity Measurement**: When using an IMU, the accelerometer readings (after removing the dynamic acceleration) can provide the gravity direction in the body frame. -- **Magnetic North Measurement**: A magnetometer can provide the direction of the Earth's magnetic field, which can be used similarly. -- **Reference Direction**: Ensure that `bRef` correctly represents the sensor's measurement axis in the body frame. -- **Noise Model**: The choice of noise model affects the weight of this factor in the optimization. Adjust the standard deviation based on the confidence in your measurements. +- **nRef**: The known direction in the navigation frame. This is typically obtained from sensors like accelerometers or magnetometers. +- **bMeasured**: The measured direction in the body frame. By default, this is set to the Z-axis `[0; 0; 1]`, but it should match the direction your sensor measures. When constructing a `Unit3`, will be automatically normalized. +- **noiseModel**: The noise model representing the uncertainty in the measurement. Adjust the standard deviation based on the confidence in your measurements. ## Example in GPS-Denied Navigation @@ -132,9 +128,7 @@ This factor helps maintain an accurate orientation estimate over time, which is ## Conclusion -The `AttitudeFactor` is a powerful tool in GTSAM for incorporating orientation measurements into your factor graph. By aligning a known reference direction in the body frame with a measured direction in the navigation frame, it provides a constraint that improves the estimation of the rotation variable. Correct understanding and usage of this factor enhance the performance of navigation algorithms, especially in challenging environments where GPS is unavailable. - -Remember to verify the alignment of your reference and measured directions and to adjust the noise model to reflect the reliability of your sensors. +The `AttitudeFactor` is a useful tool in GTSAM for incorporating orientation measurements into your factor graph. By aligning a measured direction in the body frame with a known reference direction in the navigation frame, it provides a constraint that improves the estimation of the rotation variable. Correct understanding and usage of this factor enhance the performance of navigation algorithms, especially in challenging environments where GPS is unavailable. # References diff --git a/gtsam/navigation/navigation.i b/gtsam/navigation/navigation.i index db47baa750..831788073f 100644 --- a/gtsam/navigation/navigation.i +++ b/gtsam/navigation/navigation.i @@ -256,34 +256,30 @@ virtual class AHRSFactor : gtsam::NonlinearFactor { }; #include -//virtual class AttitudeFactor : gtsam::NonlinearFactor { -// AttitudeFactor(const Unit3& nZ, const Unit3& bRef); -// AttitudeFactor(); -//}; -virtual class Rot3AttitudeFactor : gtsam::NonlinearFactor{ - Rot3AttitudeFactor(size_t key, const gtsam::Unit3& nZ, const gtsam::noiseModel::Diagonal* model, - const gtsam::Unit3& bRef); - Rot3AttitudeFactor(size_t key, const gtsam::Unit3& nZ, const gtsam::noiseModel::Diagonal* model); +virtual class Rot3AttitudeFactor : gtsam::NoiseModelFactor { + Rot3AttitudeFactor(size_t key, const gtsam::Unit3& nRef, const gtsam::noiseModel::Diagonal* model, + const gtsam::Unit3& bMeasured); + Rot3AttitudeFactor(size_t key, const gtsam::Unit3& nRef, const gtsam::noiseModel::Diagonal* model); Rot3AttitudeFactor(); void print(string s = "", const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter) const; bool equals(const gtsam::NonlinearFactor& expected, double tol) const; - gtsam::Unit3 nZ() const; - gtsam::Unit3 bRef() const; + gtsam::Unit3 nRef() const; + gtsam::Unit3 bMeasured() const; }; -virtual class Pose3AttitudeFactor : gtsam::NonlinearFactor { - Pose3AttitudeFactor(size_t key, const gtsam::Unit3& nZ, +virtual class Pose3AttitudeFactor : gtsam::NoiseModelFactor { + Pose3AttitudeFactor(size_t key, const gtsam::Unit3& nRef, const gtsam::noiseModel::Diagonal* model, - const gtsam::Unit3& bRef); - Pose3AttitudeFactor(size_t key, const gtsam::Unit3& nZ, + const gtsam::Unit3& bMeasured); + Pose3AttitudeFactor(size_t key, const gtsam::Unit3& nRef, const gtsam::noiseModel::Diagonal* model); Pose3AttitudeFactor(); void print(string s = "", const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter) const; bool equals(const gtsam::NonlinearFactor& expected, double tol) const; - gtsam::Unit3 nZ() const; - gtsam::Unit3 bRef() const; + gtsam::Unit3 nRef() const; + gtsam::Unit3 bMeasured() const; }; #include diff --git a/gtsam/navigation/tests/testAttitudeFactor.cpp b/gtsam/navigation/tests/testAttitudeFactor.cpp index f1fde1dca4..e01706df9f 100644 --- a/gtsam/navigation/tests/testAttitudeFactor.cpp +++ b/gtsam/navigation/tests/testAttitudeFactor.cpp @@ -11,44 +11,44 @@ /** * @file testAttitudeFactor.cpp - * @brief Unit test for Rot3AttitudeFactor + * @brief Unit test for AttitudeFactors (rot3 and Pose3 versions) * @author Frank Dellaert * @date January 22, 2014 */ -#include +#include #include #include +#include #include + #include "gtsam/base/Matrix.h" -#include using namespace std::placeholders; using namespace std; using namespace gtsam; // ************************************************************************* -TEST( Rot3AttitudeFactor, Constructor ) { - +TEST(Rot3AttitudeFactor, Constructor) { // Example: pitch and roll of aircraft in an ENU Cartesian frame. // If pitch and roll are zero for an aerospace frame, // that means Z is pointing down, i.e., direction of Z = (0,0,-1) - Unit3 bZ(0, 0, 1); // reference direction is body Z axis - Unit3 nDown(0, 0, -1); // down, in ENU navigation frame, is "measurement" + Unit3 bMeasured(0, 0, 1); // measured direction is body Z axis + Unit3 nDown(0, 0, -1); // down, in ENU navigation frame, is "reference" // Factor Key key(1); SharedNoiseModel model = noiseModel::Isotropic::Sigma(2, 0.25); Rot3AttitudeFactor factor0(key, nDown, model); - Rot3AttitudeFactor factor(key, nDown, model, bZ); - EXPECT(assert_equal(factor0,factor,1e-5)); + Rot3AttitudeFactor factor(key, nDown, model, bMeasured); + EXPECT(assert_equal(factor0, factor, 1e-5)); // Create a linearization point at the zero-error point Rot3 nRb; - EXPECT(assert_equal((Vector) Z_2x1,factor.evaluateError(nRb),1e-5)); + EXPECT(assert_equal((Vector)Z_2x1, factor.evaluateError(nRb), 1e-5)); - auto err_fn = [&factor](const Rot3& r){ + auto err_fn = [&factor](const Rot3& r) { return factor.evaluateError(r, OptionalNone); }; // Calculate numerical derivatives @@ -80,32 +80,31 @@ TEST(Rot3AttitudeFactor, CopyAndMove) { } // ************************************************************************* -TEST( Pose3AttitudeFactor, Constructor ) { - +TEST(Pose3AttitudeFactor, Constructor) { // Example: pitch and roll of aircraft in an ENU Cartesian frame. // If pitch and roll are zero for an aerospace frame, // that means Z is pointing down, i.e., direction of Z = (0,0,-1) - Unit3 bZ(0, 0, 1); // reference direction is body Z axis - Unit3 nDown(0, 0, -1); // down, in ENU navigation frame, is "measurement" + Unit3 bMeasured(0, 0, 1); // measured direction is body Z axis + Unit3 nDown(0, 0, -1); // down, in ENU navigation frame, is "reference" // Factor Key key(1); SharedNoiseModel model = noiseModel::Isotropic::Sigma(2, 0.25); Pose3AttitudeFactor factor0(key, nDown, model); - Pose3AttitudeFactor factor(key, nDown, model, bZ); - EXPECT(assert_equal(factor0,factor,1e-5)); + Pose3AttitudeFactor factor(key, nDown, model, bMeasured); + EXPECT(assert_equal(factor0, factor, 1e-5)); // Create a linearization point at the zero-error point Pose3 T(Rot3(), Point3(-5.0, 8.0, -11.0)); - EXPECT(assert_equal((Vector) Z_2x1,factor.evaluateError(T),1e-5)); + EXPECT(assert_equal((Vector)Z_2x1, factor.evaluateError(T), 1e-5)); Matrix actualH1; - auto err_fn = [&factor](const Pose3& p){ + auto err_fn = [&factor](const Pose3& p) { return factor.evaluateError(p, OptionalNone); }; // Calculate numerical derivatives - Matrix expectedH = numericalDerivative11(err_fn, T); + Matrix expectedH = numericalDerivative11(err_fn, T); // Use the factor to calculate the derivative Matrix actualH; From be9fb064f5aa8ec747b5fdef1f75f65663975086 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Wed, 30 Oct 2024 16:32:06 -0400 Subject: [PATCH 068/115] method to get the nonlinear factors --- gtsam/hybrid/tests/Switching.h | 12 +++++++++++- 1 file changed, 11 insertions(+), 1 deletion(-) diff --git a/gtsam/hybrid/tests/Switching.h b/gtsam/hybrid/tests/Switching.h index 66dbc5fdc7..49ced968ed 100644 --- a/gtsam/hybrid/tests/Switching.h +++ b/gtsam/hybrid/tests/Switching.h @@ -214,13 +214,23 @@ struct Switching { return chain; } - HybridGaussianFactorGraph linearizedFactorGraph() { + /// Get the full linear factor graph. + HybridGaussianFactorGraph linearizedFactorGraph() const { HybridGaussianFactorGraph graph; graph.push_back(linearUnaryFactors); graph.push_back(linearBinaryFactors); graph.push_back(modeChain); return graph; } + + /// Get all the nonlinear factors. + HybridNonlinearFactorGraph nonlinearFactorGraph() const { + HybridNonlinearFactorGraph graph; + graph.push_back(unaryFactors); + graph.push_back(binaryFactors); + graph.push_back(modeChain); + return graph; + } }; } // namespace gtsam From 31f0011d5f9bd8e2ff34305b560666416c2342cb Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Wed, 30 Oct 2024 16:33:28 -0400 Subject: [PATCH 069/115] update all tests --- gtsam/hybrid/tests/testHybridBayesNet.cpp | 13 +- gtsam/hybrid/tests/testHybridBayesTree.cpp | 112 +++++++------- gtsam/hybrid/tests/testHybridEstimation.cpp | 6 +- .../tests/testHybridGaussianFactorGraph.cpp | 56 +++---- gtsam/hybrid/tests/testHybridGaussianISAM.cpp | 42 +++--- .../tests/testHybridGaussianProductFactor.cpp | 2 + .../tests/testHybridNonlinearFactorGraph.cpp | 139 ++++++++++-------- .../hybrid/tests/testHybridNonlinearISAM.cpp | 9 +- .../hybrid/tests/testSerializationHybrid.cpp | 4 +- 9 files changed, 197 insertions(+), 186 deletions(-) diff --git a/gtsam/hybrid/tests/testHybridBayesNet.cpp b/gtsam/hybrid/tests/testHybridBayesNet.cpp index ad4e4e7a8d..16d0ae1a12 100644 --- a/gtsam/hybrid/tests/testHybridBayesNet.cpp +++ b/gtsam/hybrid/tests/testHybridBayesNet.cpp @@ -31,6 +31,7 @@ // Include for test suite #include + #include using namespace std; @@ -263,7 +264,7 @@ TEST(HybridBayesNet, Choose) { const Ordering ordering(s.linearizationPoint.keys()); const auto [hybridBayesNet, remainingFactorGraph] = - s.linearizedFactorGraph.eliminatePartialSequential(ordering); + s.linearizedFactorGraph().eliminatePartialSequential(ordering); DiscreteValues assignment; assignment[M(0)] = 1; @@ -292,7 +293,7 @@ TEST(HybridBayesNet, OptimizeAssignment) { const Ordering ordering(s.linearizationPoint.keys()); const auto [hybridBayesNet, remainingFactorGraph] = - s.linearizedFactorGraph.eliminatePartialSequential(ordering); + s.linearizedFactorGraph().eliminatePartialSequential(ordering); DiscreteValues assignment; assignment[M(0)] = 1; @@ -319,7 +320,7 @@ TEST(HybridBayesNet, Optimize) { Switching s(4, 1.0, 0.1, {0, 1, 2, 3}, "1/1 1/1"); HybridBayesNet::shared_ptr hybridBayesNet = - s.linearizedFactorGraph.eliminateSequential(); + s.linearizedFactorGraph().eliminateSequential(); HybridValues delta = hybridBayesNet->optimize(); @@ -347,7 +348,7 @@ TEST(HybridBayesNet, Pruning) { Switching s(3); HybridBayesNet::shared_ptr posterior = - s.linearizedFactorGraph.eliminateSequential(); + s.linearizedFactorGraph().eliminateSequential(); EXPECT_LONGS_EQUAL(5, posterior->size()); // Optimize @@ -400,7 +401,7 @@ TEST(HybridBayesNet, Prune) { Switching s(4); HybridBayesNet::shared_ptr posterior = - s.linearizedFactorGraph.eliminateSequential(); + s.linearizedFactorGraph().eliminateSequential(); EXPECT_LONGS_EQUAL(7, posterior->size()); HybridValues delta = posterior->optimize(); @@ -418,7 +419,7 @@ TEST(HybridBayesNet, UpdateDiscreteConditionals) { Switching s(4); HybridBayesNet::shared_ptr posterior = - s.linearizedFactorGraph.eliminateSequential(); + s.linearizedFactorGraph().eliminateSequential(); EXPECT_LONGS_EQUAL(7, posterior->size()); DiscreteConditional joint; diff --git a/gtsam/hybrid/tests/testHybridBayesTree.cpp b/gtsam/hybrid/tests/testHybridBayesTree.cpp index 6da991173a..9e886200a1 100644 --- a/gtsam/hybrid/tests/testHybridBayesTree.cpp +++ b/gtsam/hybrid/tests/testHybridBayesTree.cpp @@ -352,7 +352,7 @@ TEST(HybridBayesTree, OptimizeMultifrontal) { Switching s(4); HybridBayesTree::shared_ptr hybridBayesTree = - s.linearizedFactorGraph.eliminateMultifrontal(); + s.linearizedFactorGraph().eliminateMultifrontal(); HybridValues delta = hybridBayesTree->optimize(); VectorValues expectedValues; @@ -364,30 +364,40 @@ TEST(HybridBayesTree, OptimizeMultifrontal) { EXPECT(assert_equal(expectedValues, delta.continuous(), 1e-5)); } +namespace optimize_fixture { +HybridGaussianFactorGraph GetGaussianFactorGraph(size_t N) { + Switching s(N); + HybridGaussianFactorGraph graph; + + for (size_t i = 0; i < N - 1; i++) { + graph.push_back(s.linearBinaryFactors.at(i)); + } + for (size_t i = 0; i < N; i++) { + graph.push_back(s.linearUnaryFactors.at(i)); + } + for (size_t i = 0; i < N - 1; i++) { + graph.push_back(s.modeChain.at(i)); + } + + return graph; +} +} // namespace optimize_fixture + /* ****************************************************************************/ // Test for optimizing a HybridBayesTree with a given assignment. TEST(HybridBayesTree, OptimizeAssignment) { - Switching s(4); + using namespace optimize_fixture; - HybridGaussianISAM isam; - HybridGaussianFactorGraph graph1; - - // Add the 3 hybrid factors, x1-x2, x2-x3, x3-x4 - for (size_t i = 1; i < 4; i++) { - graph1.push_back(s.linearizedFactorGraph.at(i)); - } + size_t N = 4; - // Add the Gaussian factors, 1 prior on X(1), - // 3 measurements on X(2), X(3), X(4) - graph1.push_back(s.linearizedFactorGraph.at(0)); - for (size_t i = 4; i <= 7; i++) { - graph1.push_back(s.linearizedFactorGraph.at(i)); - } + HybridGaussianISAM isam; - // Add the discrete factors - for (size_t i = 7; i <= 9; i++) { - graph1.push_back(s.linearizedFactorGraph.at(i)); - } + // Add the 3 hybrid factors, x0-x1, x1-x2, x2-x3 + // Then add the Gaussian factors, 1 prior on X(0), + // 3 measurements on X(1), X(2), X(3) + // Finally add the discrete factors + // m0, m1-m0, m2-m1 + HybridGaussianFactorGraph graph1 = GetGaussianFactorGraph(N); isam.update(graph1); @@ -409,12 +419,13 @@ TEST(HybridBayesTree, OptimizeAssignment) { EXPECT(assert_equal(expected_delta, delta)); + Switching s(N); // Create ordering. Ordering ordering; for (size_t k = 0; k < s.K; k++) ordering.push_back(X(k)); const auto [hybridBayesNet, remainingFactorGraph] = - s.linearizedFactorGraph.eliminatePartialSequential(ordering); + s.linearizedFactorGraph().eliminatePartialSequential(ordering); GaussianBayesNet gbn = hybridBayesNet->choose(assignment); VectorValues expected = gbn.optimize(); @@ -425,38 +436,29 @@ TEST(HybridBayesTree, OptimizeAssignment) { /* ****************************************************************************/ // Test for optimizing a HybridBayesTree. TEST(HybridBayesTree, Optimize) { - Switching s(4); + using namespace optimize_fixture; - HybridGaussianISAM isam; - HybridGaussianFactorGraph graph1; - - // Add the 3 hybrid factors, x1-x2, x2-x3, x3-x4 - for (size_t i = 1; i < 4; i++) { - graph1.push_back(s.linearizedFactorGraph.at(i)); - } + size_t N = 4; - // Add the Gaussian factors, 1 prior on X(0), - // 3 measurements on X(2), X(3), X(4) - graph1.push_back(s.linearizedFactorGraph.at(0)); - for (size_t i = 4; i <= 6; i++) { - graph1.push_back(s.linearizedFactorGraph.at(i)); - } - - // Add the discrete factors - for (size_t i = 7; i <= 9; i++) { - graph1.push_back(s.linearizedFactorGraph.at(i)); - } + HybridGaussianISAM isam; + // Add the 3 hybrid factors, x0-x1, x1-x2, x2-x3 + // Then add the Gaussian factors, 1 prior on X(0), + // 3 measurements on X(1), X(2), X(3) + // Finally add the discrete factors + // m0, m1-m0, m2-m1 + HybridGaussianFactorGraph graph1 = GetGaussianFactorGraph(N); isam.update(graph1); HybridValues delta = isam.optimize(); + Switching s(N); // Create ordering. Ordering ordering; for (size_t k = 0; k < s.K; k++) ordering.push_back(X(k)); const auto [hybridBayesNet, remainingFactorGraph] = - s.linearizedFactorGraph.eliminatePartialSequential(ordering); + s.linearizedFactorGraph().eliminatePartialSequential(ordering); DiscreteFactorGraph dfg; for (auto&& f : *remainingFactorGraph) { @@ -481,27 +483,18 @@ TEST(HybridBayesTree, Optimize) { /* ****************************************************************************/ // Test for choosing a GaussianBayesTree from a HybridBayesTree. TEST(HybridBayesTree, Choose) { - Switching s(4); - - HybridGaussianISAM isam; - HybridGaussianFactorGraph graph1; + using namespace optimize_fixture; - // Add the 3 hybrid factors, x1-x2, x2-x3, x3-x4 - for (size_t i = 1; i < 4; i++) { - graph1.push_back(s.linearizedFactorGraph.at(i)); - } + size_t N = 4; - // Add the Gaussian factors, 1 prior on X(0), - // 3 measurements on X(2), X(3), X(4) - graph1.push_back(s.linearizedFactorGraph.at(0)); - for (size_t i = 4; i <= 6; i++) { - graph1.push_back(s.linearizedFactorGraph.at(i)); - } + HybridGaussianISAM isam; - // Add the discrete factors - for (size_t i = 7; i <= 9; i++) { - graph1.push_back(s.linearizedFactorGraph.at(i)); - } + // Add the 3 hybrid factors, x0-x1, x1-x2, x2-x3 + // Then add the Gaussian factors, 1 prior on X(0), + // 3 measurements on X(1), X(2), X(3) + // Finally add the discrete factors + // m0, m1-m0, m2-m1 + HybridGaussianFactorGraph graph1 = GetGaussianFactorGraph(N); isam.update(graph1); @@ -513,8 +506,9 @@ TEST(HybridBayesTree, Choose) { GaussianBayesTree gbt = isam.choose(assignment); // Specify ordering so it matches that of HybridGaussianISAM. + Switching s(N); Ordering ordering(KeyVector{X(0), X(1), X(2), X(3), M(0), M(1), M(2)}); - auto bayesTree = s.linearizedFactorGraph.eliminateMultifrontal(ordering); + auto bayesTree = s.linearizedFactorGraph().eliminateMultifrontal(ordering); auto expected_gbt = bayesTree->choose(assignment); diff --git a/gtsam/hybrid/tests/testHybridEstimation.cpp b/gtsam/hybrid/tests/testHybridEstimation.cpp index 9c2580a17a..5b27e2b417 100644 --- a/gtsam/hybrid/tests/testHybridEstimation.cpp +++ b/gtsam/hybrid/tests/testHybridEstimation.cpp @@ -82,7 +82,7 @@ TEST(HybridEstimation, Full) { // Switching example of robot moving in 1D // with given measurements and equal mode priors. Switching switching(K, 1.0, 0.1, measurements, "1/1 1/1"); - HybridGaussianFactorGraph graph = switching.linearizedFactorGraph; + HybridGaussianFactorGraph graph = switching.linearizedFactorGraph(); Ordering hybridOrdering; for (size_t k = 0; k < K; k++) { @@ -325,7 +325,7 @@ TEST(HybridEstimation, Probability) { // given measurements and equal mode priors. Switching switching(K, between_sigma, measurement_sigma, measurements, "1/1 1/1"); - auto graph = switching.linearizedFactorGraph; + auto graph = switching.linearizedFactorGraph(); // Continuous elimination Ordering continuous_ordering(graph.continuousKeySet()); @@ -365,7 +365,7 @@ TEST(HybridEstimation, ProbabilityMultifrontal) { // mode priors. Switching switching(K, between_sigma, measurement_sigma, measurements, "1/1 1/1"); - auto graph = switching.linearizedFactorGraph; + auto graph = switching.linearizedFactorGraph(); // Get the tree of unnormalized probabilities for each mode sequence. AlgebraicDecisionTree expected_probPrimeTree = GetProbPrimeTree(graph); diff --git a/gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp b/gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp index 4b91d091d8..13a6cd614f 100644 --- a/gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp +++ b/gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp @@ -175,7 +175,7 @@ TEST(HybridBayesNet, Switching) { Switching s(2, betweenSigma, priorSigma); // Check size of linearized factor graph - const HybridGaussianFactorGraph &graph = s.linearizedFactorGraph; + const HybridGaussianFactorGraph &graph = s.linearizedFactorGraph(); EXPECT_LONGS_EQUAL(4, graph.size()); // Create some continuous and discrete values @@ -184,7 +184,7 @@ TEST(HybridBayesNet, Switching) { const DiscreteValues modeZero{{M(0), 0}}, modeOne{{M(0), 1}}; // Get the hybrid gaussian factor and check it is as expected - auto hgf = std::dynamic_pointer_cast(graph.at(1)); + auto hgf = std::dynamic_pointer_cast(graph.at(2)); CHECK(hgf); // Get factors and scalars for both modes @@ -298,7 +298,7 @@ TEST(HybridBayesNet, Switching) { factors_x1.push_back( factor); // Use the remaining factor from previous elimination factors_x1.push_back( - graph.at(2)); // Add the factor for x1 from the original graph + graph.at(1)); // Add the factor for x1 from the original graph // Test collectProductFactor for x1 clique auto productFactor_x1 = factors_x1.collectProductFactor(); @@ -356,7 +356,7 @@ TEST(HybridGaussianFactorGraph, ErrorAndProbPrime) { Switching s(3); // Check size of linearized factor graph - const HybridGaussianFactorGraph &graph = s.linearizedFactorGraph; + const HybridGaussianFactorGraph &graph = s.linearizedFactorGraph(); EXPECT_LONGS_EQUAL(7, graph.size()); // Eliminate the graph @@ -383,16 +383,16 @@ TEST(HybridGaussianFactorGraph, ErrorAndProbPrime) { TEST(HybridGaussianFactorGraph, DiscreteSelection) { Switching s(3); - HybridGaussianFactorGraph graph = s.linearizedFactorGraph; + HybridGaussianFactorGraph graph = s.linearizedFactorGraph(); DiscreteValues dv00{{M(0), 0}, {M(1), 0}}; GaussianFactorGraph continuous_00 = graph(dv00); GaussianFactorGraph expected_00; expected_00.push_back(JacobianFactor(X(0), I_1x1 * 10, Vector1(-10))); - expected_00.push_back(JacobianFactor(X(0), -I_1x1, X(1), I_1x1, Vector1(-1))); - expected_00.push_back(JacobianFactor(X(1), -I_1x1, X(2), I_1x1, Vector1(-1))); expected_00.push_back(JacobianFactor(X(1), I_1x1 * 10, Vector1(-10))); expected_00.push_back(JacobianFactor(X(2), I_1x1 * 10, Vector1(-10))); + expected_00.push_back(JacobianFactor(X(0), -I_1x1, X(1), I_1x1, Vector1(-1))); + expected_00.push_back(JacobianFactor(X(1), -I_1x1, X(2), I_1x1, Vector1(-1))); EXPECT(assert_equal(expected_00, continuous_00)); @@ -400,10 +400,10 @@ TEST(HybridGaussianFactorGraph, DiscreteSelection) { GaussianFactorGraph continuous_01 = graph(dv01); GaussianFactorGraph expected_01; expected_01.push_back(JacobianFactor(X(0), I_1x1 * 10, Vector1(-10))); - expected_01.push_back(JacobianFactor(X(0), -I_1x1, X(1), I_1x1, Vector1(-1))); - expected_01.push_back(JacobianFactor(X(1), -I_1x1, X(2), I_1x1, Vector1(-0))); expected_01.push_back(JacobianFactor(X(1), I_1x1 * 10, Vector1(-10))); expected_01.push_back(JacobianFactor(X(2), I_1x1 * 10, Vector1(-10))); + expected_01.push_back(JacobianFactor(X(0), -I_1x1, X(1), I_1x1, Vector1(-1))); + expected_01.push_back(JacobianFactor(X(1), -I_1x1, X(2), I_1x1, Vector1(-0))); EXPECT(assert_equal(expected_01, continuous_01)); @@ -411,10 +411,10 @@ TEST(HybridGaussianFactorGraph, DiscreteSelection) { GaussianFactorGraph continuous_10 = graph(dv10); GaussianFactorGraph expected_10; expected_10.push_back(JacobianFactor(X(0), I_1x1 * 10, Vector1(-10))); - expected_10.push_back(JacobianFactor(X(0), -I_1x1, X(1), I_1x1, Vector1(-0))); - expected_10.push_back(JacobianFactor(X(1), -I_1x1, X(2), I_1x1, Vector1(-1))); expected_10.push_back(JacobianFactor(X(1), I_1x1 * 10, Vector1(-10))); expected_10.push_back(JacobianFactor(X(2), I_1x1 * 10, Vector1(-10))); + expected_10.push_back(JacobianFactor(X(0), -I_1x1, X(1), I_1x1, Vector1(-0))); + expected_10.push_back(JacobianFactor(X(1), -I_1x1, X(2), I_1x1, Vector1(-1))); EXPECT(assert_equal(expected_10, continuous_10)); @@ -422,16 +422,16 @@ TEST(HybridGaussianFactorGraph, DiscreteSelection) { GaussianFactorGraph continuous_11 = graph(dv11); GaussianFactorGraph expected_11; expected_11.push_back(JacobianFactor(X(0), I_1x1 * 10, Vector1(-10))); - expected_11.push_back(JacobianFactor(X(0), -I_1x1, X(1), I_1x1, Vector1(-0))); - expected_11.push_back(JacobianFactor(X(1), -I_1x1, X(2), I_1x1, Vector1(-0))); expected_11.push_back(JacobianFactor(X(1), I_1x1 * 10, Vector1(-10))); expected_11.push_back(JacobianFactor(X(2), I_1x1 * 10, Vector1(-10))); + expected_11.push_back(JacobianFactor(X(0), -I_1x1, X(1), I_1x1, Vector1(-0))); + expected_11.push_back(JacobianFactor(X(1), -I_1x1, X(2), I_1x1, Vector1(-0))); EXPECT(assert_equal(expected_11, continuous_11)); } /* ************************************************************************* */ -TEST(HybridGaussianFactorGraph, optimize) { +TEST(HybridGaussianFactorGraph, Optimize) { HybridGaussianFactorGraph hfg; hfg.add(JacobianFactor(X(0), I_3x3, Z_3x1)); @@ -451,16 +451,16 @@ TEST(HybridGaussianFactorGraph, Conditionals) { Switching switching(4); HybridGaussianFactorGraph hfg; - hfg.push_back(switching.linearizedFactorGraph.at(0)); // P(X0) + hfg.push_back(switching.linearUnaryFactors.at(0)); // P(X0) Ordering ordering; ordering.push_back(X(0)); HybridBayesNet::shared_ptr bayes_net = hfg.eliminateSequential(ordering); HybridGaussianFactorGraph hfg2; - hfg2.push_back(*bayes_net); // P(X0) - hfg2.push_back(switching.linearizedFactorGraph.at(1)); // P(X0, X1 | M0) - hfg2.push_back(switching.linearizedFactorGraph.at(2)); // P(X1, X2 | M1) - hfg2.push_back(switching.linearizedFactorGraph.at(5)); // P(M1) + hfg2.push_back(*bayes_net); // P(X0) + hfg2.push_back(switching.linearBinaryFactors.at(0)); // P(X0, X1 | M0) + hfg2.push_back(switching.linearBinaryFactors.at(1)); // P(X1, X2 | M1) + hfg2.push_back(switching.linearUnaryFactors.at(2)); // P(X2) ordering += X(1), X(2), M(0), M(1); // Created product of first two factors and check eliminate: @@ -510,13 +510,13 @@ TEST(HybridGaussianFactorGraph, IncrementalErrorTree) { Switching s(4); HybridGaussianFactorGraph graph; - graph.push_back(s.linearizedFactorGraph.at(0)); // f(X0) - graph.push_back(s.linearizedFactorGraph.at(1)); // f(X0, X1, M0) - graph.push_back(s.linearizedFactorGraph.at(2)); // f(X1, X2, M1) - graph.push_back(s.linearizedFactorGraph.at(4)); // f(X1) - graph.push_back(s.linearizedFactorGraph.at(5)); // f(X2) - graph.push_back(s.linearizedFactorGraph.at(7)); // f(M0) - graph.push_back(s.linearizedFactorGraph.at(8)); // f(M0, M1) + graph.push_back(s.linearUnaryFactors.at(0)); // f(X0) + graph.push_back(s.linearBinaryFactors.at(0)); // f(X0, X1, M0) + graph.push_back(s.linearBinaryFactors.at(1)); // f(X1, X2, M1) + graph.push_back(s.linearUnaryFactors.at(1)); // f(X1) + graph.push_back(s.linearUnaryFactors.at(2)); // f(X2) + graph.push_back(s.modeChain.at(0)); // f(M0) + graph.push_back(s.modeChain.at(1)); // f(M0, M1) HybridBayesNet::shared_ptr hybridBayesNet = graph.eliminateSequential(); EXPECT_LONGS_EQUAL(5, hybridBayesNet->size()); @@ -531,8 +531,8 @@ TEST(HybridGaussianFactorGraph, IncrementalErrorTree) { graph = HybridGaussianFactorGraph(); graph.push_back(*hybridBayesNet); - graph.push_back(s.linearizedFactorGraph.at(3)); // f(X2, X3, M2) - graph.push_back(s.linearizedFactorGraph.at(6)); // f(X3) + graph.push_back(s.linearBinaryFactors.at(2)); // f(X2, X3, M2) + graph.push_back(s.linearUnaryFactors.at(3)); // f(X3) hybridBayesNet = graph.eliminateSequential(); EXPECT_LONGS_EQUAL(7, hybridBayesNet->size()); diff --git a/gtsam/hybrid/tests/testHybridGaussianISAM.cpp b/gtsam/hybrid/tests/testHybridGaussianISAM.cpp index 11e3194f26..b2e388529f 100644 --- a/gtsam/hybrid/tests/testHybridGaussianISAM.cpp +++ b/gtsam/hybrid/tests/testHybridGaussianISAM.cpp @@ -42,15 +42,15 @@ using symbol_shorthand::Z; /* ****************************************************************************/ namespace switching3 { -// ϕ(x0) ϕ(x0,x1,m0) ϕ(x1,x2,m1) ϕ(x1;z1) ϕ(x2;z2) ϕ(m0) ϕ(m0,m1) +// ϕ(x0) ϕ(x1;z1) ϕ(x2;z2) ϕ(x0,x1,m0) ϕ(x1,x2,m1) ϕ(m0) ϕ(m0,m1) const Switching switching(3); -const HybridGaussianFactorGraph &lfg = switching.linearizedFactorGraph; +const HybridGaussianFactorGraph &lfg = switching.linearizedFactorGraph(); // First update graph: ϕ(x0) ϕ(x0,x1,m0) ϕ(m0) -const HybridGaussianFactorGraph graph1{lfg.at(0), lfg.at(1), lfg.at(5)}; +const HybridGaussianFactorGraph graph1{lfg.at(0), lfg.at(3), lfg.at(5)}; // Second update graph: ϕ(x1,x2,m1) ϕ(x1;z1) ϕ(x2;z2) ϕ(m0,m1) -const HybridGaussianFactorGraph graph2{lfg.at(2), lfg.at(3), lfg.at(4), +const HybridGaussianFactorGraph graph2{lfg.at(4), lfg.at(1), lfg.at(2), lfg.at(6)}; } // namespace switching3 @@ -108,7 +108,7 @@ TEST(HybridGaussianElimination, IncrementalInference) { // Now we calculate the expected factors using full elimination const auto [expectedHybridBayesTree, expectedRemainingGraph] = - switching.linearizedFactorGraph.eliminatePartialMultifrontal(ordering); + switching.linearizedFactorGraph().eliminatePartialMultifrontal(ordering); // The densities on X(0) should be the same auto x0_conditional = dynamic_pointer_cast( @@ -162,15 +162,14 @@ TEST(HybridGaussianElimination, Approx_inference) { HybridGaussianFactorGraph graph1; // Add the 3 hybrid factors, x0-x1, x1-x2, x2-x3 - for (size_t i = 1; i < 4; i++) { - graph1.push_back(switching.linearizedFactorGraph.at(i)); + for (size_t i = 0; i < 3; i++) { + graph1.push_back(switching.linearBinaryFactors.at(i)); } - // Add the Gaussian factors, 1 prior on X(0), - // 3 measurements on X(1), X(2), X(3) - graph1.push_back(switching.linearizedFactorGraph.at(0)); - for (size_t i = 4; i <= 7; i++) { - graph1.push_back(switching.linearizedFactorGraph.at(i)); + // Add the Gaussian factors, 1 prior on x0, + // 3 measurements on x1, x2, x3 + for (size_t i = 0; i <= 3; i++) { + graph1.push_back(switching.linearUnaryFactors.at(i)); } // Create ordering. @@ -181,7 +180,7 @@ TEST(HybridGaussianElimination, Approx_inference) { // Now we calculate the actual factors using full elimination const auto [unPrunedHybridBayesTree, unPrunedRemainingGraph] = - switching.linearizedFactorGraph.eliminatePartialMultifrontal(ordering); + switching.linearizedFactorGraph().eliminatePartialMultifrontal(ordering); size_t maxNrLeaves = 5; incrementalHybrid.update(graph1); @@ -266,15 +265,14 @@ TEST(HybridGaussianElimination, IncrementalApproximate) { /***** Run Round 1 *****/ // Add the 3 hybrid factors, x0-x1, x1-x2, x2-x3 - for (size_t i = 1; i < 4; i++) { - graph1.push_back(switching.linearizedFactorGraph.at(i)); + for (size_t i = 0; i < 3; i++) { + graph1.push_back(switching.linearBinaryFactors.at(i)); } - // Add the Gaussian factors, 1 prior on X(0), - // 3 measurements on X(1), X(2), X(3) - graph1.push_back(switching.linearizedFactorGraph.at(0)); - for (size_t i = 5; i <= 7; i++) { - graph1.push_back(switching.linearizedFactorGraph.at(i)); + // Add the Gaussian factors, 1 prior on x0, + // 3 measurements on x1, x2, x3 + for (size_t i = 0; i <= 3; i++) { + graph1.push_back(switching.linearUnaryFactors.at(i)); } // Run update with pruning @@ -296,8 +294,8 @@ TEST(HybridGaussianElimination, IncrementalApproximate) { /***** Run Round 2 *****/ HybridGaussianFactorGraph graph2; - graph2.push_back(switching.linearizedFactorGraph.at(4)); - graph2.push_back(switching.linearizedFactorGraph.at(8)); + graph2.push_back(switching.linearBinaryFactors.at(3)); // x3-x4 + graph2.push_back(switching.linearUnaryFactors.at(4)); // x4 // Run update with pruning a second time. incrementalHybrid.update(graph2); diff --git a/gtsam/hybrid/tests/testHybridGaussianProductFactor.cpp b/gtsam/hybrid/tests/testHybridGaussianProductFactor.cpp index 3a4a6c1f41..f1920fa060 100644 --- a/gtsam/hybrid/tests/testHybridGaussianProductFactor.cpp +++ b/gtsam/hybrid/tests/testHybridGaussianProductFactor.cpp @@ -189,8 +189,10 @@ TEST(HybridGaussianProductFactor, AddPruned) { product += prunedFactorB; EXPECT_LONGS_EQUAL(6, product.nrLeaves()); +#ifdef GTSAM_DT_MERGING auto pruned = product.removeEmpty(); EXPECT_LONGS_EQUAL(5, pruned.nrLeaves()); +#endif } /* ************************************************************************* */ diff --git a/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp b/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp index dd41280349..b22847e894 100644 --- a/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp +++ b/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp @@ -249,7 +249,7 @@ TEST(HybridNonlinearFactorGraph, Switching) { Switching self(3); EXPECT_LONGS_EQUAL(7, self.nonlinearFactorGraph().size()); - EXPECT_LONGS_EQUAL(7, self.linearizedFactorGraph.size()); + EXPECT_LONGS_EQUAL(7, self.linearizedFactorGraph().size()); } /**************************************************************************** @@ -276,7 +276,7 @@ TEST(HybridNonlinearFactorGraph, EliminationTree) { for (size_t k = 0; k < self.K; k++) ordering.push_back(X(k)); // Create elimination tree. - HybridEliminationTree etree(self.linearizedFactorGraph, ordering); + HybridEliminationTree etree(self.linearizedFactorGraph(), ordering); EXPECT_LONGS_EQUAL(1, etree.roots().size()) } @@ -286,12 +286,12 @@ TEST(HybridNonlinearFactorGraph, EliminationTree) { TEST(GaussianElimination, Eliminate_x0) { Switching self(3); - // Gather factors on x1, has a simple Gaussian and a hybrid factor. + // Gather factors on x0, has a simple Gaussian and a hybrid factor. HybridGaussianFactorGraph factors; // Add gaussian prior - factors.push_back(self.linearizedFactorGraph[0]); + factors.push_back(self.linearUnaryFactors[0]); // Add first hybrid factor - factors.push_back(self.linearizedFactorGraph[1]); + factors.push_back(self.linearBinaryFactors[0]); // Eliminate x0 const Ordering ordering{X(0)}; @@ -313,8 +313,8 @@ TEST(HybridsGaussianElimination, Eliminate_x1) { // Gather factors on x1, will be two hybrid factors (with x0 and x2, resp.). HybridGaussianFactorGraph factors; - factors.push_back(self.linearizedFactorGraph[1]); // involves m0 - factors.push_back(self.linearizedFactorGraph[2]); // involves m1 + factors.push_back(self.linearBinaryFactors[0]); // involves m0 + factors.push_back(self.linearBinaryFactors[1]); // involves m1 // Eliminate x1 const Ordering ordering{X(1)}; @@ -349,7 +349,7 @@ GaussianFactorGraph::shared_ptr batchGFG(double between, TEST(HybridGaussianElimination, EliminateHybrid_2_Variable) { Switching self(2, 1.0, 0.1); - auto factors = self.linearizedFactorGraph; + auto factors = self.linearizedFactorGraph(); // Eliminate x0 const Ordering ordering{X(0), X(1)}; @@ -380,15 +380,13 @@ TEST(HybridGaussianElimination, EliminateHybrid_2_Variable) { TEST(HybridNonlinearFactorGraph, Partial_Elimination) { Switching self(3); - auto linearizedFactorGraph = self.linearizedFactorGraph; - // Create ordering of only continuous variables. Ordering ordering; for (size_t k = 0; k < self.K; k++) ordering.push_back(X(k)); // Eliminate partially i.e. only continuous part. const auto [hybridBayesNet, remainingFactorGraph] = - linearizedFactorGraph.eliminatePartialSequential(ordering); + self.linearizedFactorGraph().eliminatePartialSequential(ordering); CHECK(hybridBayesNet); EXPECT_LONGS_EQUAL(3, hybridBayesNet->size()); @@ -444,11 +442,11 @@ TEST(HybridNonlinearFactorGraph, PrintErrors) { // Get nonlinear factor graph and add linear factors to be holistic. // TODO(Frank): ??? HybridNonlinearFactorGraph fg = self.nonlinearFactorGraph(); - fg.add(self.linearizedFactorGraph); + fg.add(self.linearizedFactorGraph()); // Optimize to get HybridValues HybridBayesNet::shared_ptr bn = - self.linearizedFactorGraph.eliminateSequential(); + self.linearizedFactorGraph().eliminateSequential(); HybridValues hv = bn->optimize(); // Print and verify @@ -465,8 +463,6 @@ TEST(HybridNonlinearFactorGraph, PrintErrors) { TEST(HybridNonlinearFactorGraph, Full_Elimination) { Switching self(3); - auto linearizedFactorGraph = self.linearizedFactorGraph; - // We first do a partial elimination DiscreteBayesNet discreteBayesNet; @@ -477,7 +473,7 @@ TEST(HybridNonlinearFactorGraph, Full_Elimination) { // Eliminate partially. const auto [hybridBayesNet_partial, remainingFactorGraph_partial] = - linearizedFactorGraph.eliminatePartialSequential(ordering); + self.linearizedFactorGraph().eliminatePartialSequential(ordering); DiscreteFactorGraph discrete_fg; // TODO(Varun) Make this a function of HybridGaussianFactorGraph? @@ -500,7 +496,7 @@ TEST(HybridNonlinearFactorGraph, Full_Elimination) { // Eliminate partially. HybridBayesNet::shared_ptr hybridBayesNet = - linearizedFactorGraph.eliminateSequential(ordering); + self.linearizedFactorGraph().eliminateSequential(ordering); CHECK(hybridBayesNet); EXPECT_LONGS_EQUAL(5, hybridBayesNet->size()); @@ -533,7 +529,7 @@ TEST(HybridNonlinearFactorGraph, Full_Elimination) { TEST(HybridNonlinearFactorGraph, Printing) { Switching self(3); - auto linearizedFactorGraph = self.linearizedFactorGraph; + auto linearizedFactorGraph = self.linearizedFactorGraph(); // Create ordering. Ordering ordering; @@ -556,6 +552,24 @@ Factor 0 No noise model Factor 1 +GaussianFactor: + + A[x1] = [ + 10 +] + b = [ -10 ] + No noise model + +Factor 2 +GaussianFactor: + + A[x2] = [ + 10 +] + b = [ -10 ] + No noise model + +Factor 3 HybridGaussianFactor: Hybrid [x0 x1; m0]{ Choice(m0) @@ -583,7 +597,7 @@ scalar: 0.918939 } -Factor 2 +Factor 4 HybridGaussianFactor: Hybrid [x1 x2; m1]{ Choice(m1) @@ -611,24 +625,6 @@ scalar: 0.918939 } -Factor 3 -GaussianFactor: - - A[x1] = [ - 10 -] - b = [ -10 ] - No noise model - -Factor 4 -GaussianFactor: - - A[x2] = [ - 10 -] - b = [ -10 ] - No noise model - Factor 5 DiscreteFactor: P( m0 ): @@ -651,16 +647,38 @@ Factor 6 #else string expected_hybridFactorGraph = R"( size: 7 -factor 0: +Factor 0 +GaussianFactor: + A[x0] = [ 10 ] b = [ -10 ] No noise model -factor 1: + +Factor 1 +GaussianFactor: + + A[x1] = [ + 10 +] + b = [ -10 ] + No noise model + +Factor 2 +GaussianFactor: + + A[x2] = [ + 10 +] + b = [ -10 ] + No noise model + +Factor 3 +HybridGaussianFactor: Hybrid [x0 x1; m0]{ Choice(m0) - 0 Leaf: + 0 Leaf : A[x0] = [ -1 ] @@ -669,8 +687,9 @@ Hybrid [x0 x1; m0]{ ] b = [ -1 ] No noise model +scalar: 0.918939 - 1 Leaf: + 1 Leaf : A[x0] = [ -1 ] @@ -679,12 +698,15 @@ Hybrid [x0 x1; m0]{ ] b = [ -0 ] No noise model +scalar: 0.918939 } -factor 2: + +Factor 4 +HybridGaussianFactor: Hybrid [x1 x2; m1]{ Choice(m1) - 0 Leaf: + 0 Leaf : A[x1] = [ -1 ] @@ -693,8 +715,9 @@ Hybrid [x1 x2; m1]{ ] b = [ -1 ] No noise model +scalar: 0.918939 - 1 Leaf: + 1 Leaf : A[x1] = [ -1 ] @@ -703,26 +726,21 @@ Hybrid [x1 x2; m1]{ ] b = [ -0 ] No noise model +scalar: 0.918939 } -factor 3: - A[x1] = [ - 10 -] - b = [ -10 ] - No noise model -factor 4: - A[x2] = [ - 10 -] - b = [ -10 ] - No noise model -factor 5: P( m0 ): + +Factor 5 +DiscreteFactor: + P( m0 ): Choice(m0) - 0 Leaf 0.5 - 1 Leaf 0.5 + 0 Leaf 0.5 + 1 Leaf 0.5 -factor 6: P( m1 | m0 ): + +Factor 6 +DiscreteFactor: + P( m1 | m0 ): Choice(m1) 0 Choice(m0) 0 0 Leaf 0.33333333 @@ -731,6 +749,7 @@ factor 6: P( m1 | m0 ): 1 0 Leaf 0.66666667 1 1 Leaf 0.4 + )"; #endif diff --git a/gtsam/hybrid/tests/testHybridNonlinearISAM.cpp b/gtsam/hybrid/tests/testHybridNonlinearISAM.cpp index c5f52e3ebf..6516ef1fba 100644 --- a/gtsam/hybrid/tests/testHybridNonlinearISAM.cpp +++ b/gtsam/hybrid/tests/testHybridNonlinearISAM.cpp @@ -147,7 +147,7 @@ TEST(HybridNonlinearISAM, IncrementalInference) { // Now we calculate the actual factors using full elimination const auto [expectedHybridBayesTree, expectedRemainingGraph] = - switching.linearizedFactorGraph + switching.linearizedFactorGraph() .BaseEliminateable::eliminatePartialMultifrontal(ordering); // The densities on X(1) should be the same @@ -214,8 +214,6 @@ TEST(HybridNonlinearISAM, Approx_inference) { initial.insert(X(i), i + 1); } - // TODO(Frank): no mode chain? - // Create ordering. Ordering ordering; for (size_t j = 0; j < 4; j++) { @@ -224,7 +222,7 @@ TEST(HybridNonlinearISAM, Approx_inference) { // Now we calculate the actual factors using full elimination const auto [unPrunedHybridBayesTree, unPrunedRemainingGraph] = - switching.linearizedFactorGraph + switching.linearizedFactorGraph() .BaseEliminateable::eliminatePartialMultifrontal(ordering); size_t maxNrLeaves = 5; @@ -325,7 +323,6 @@ TEST(HybridNonlinearISAM, Incremental_approximate) { // TODO(Frank): no mode chain? - // Run update with pruning size_t maxComponents = 5; incrementalHybrid.update(graph1, initial); @@ -347,7 +344,7 @@ TEST(HybridNonlinearISAM, Incremental_approximate) { /***** Run Round 2 *****/ HybridGaussianFactorGraph graph2; graph2.push_back(switching.binaryFactors.at(3)); // x3-x4 - graph2.push_back(switching.unaryFactors.at(4)); // x4 measurement + graph2.push_back(switching.unaryFactors.at(4)); // x4 measurement initial = Values(); initial.insert(X(4), 5); diff --git a/gtsam/hybrid/tests/testSerializationHybrid.cpp b/gtsam/hybrid/tests/testSerializationHybrid.cpp index 5b06312ba4..3be96b7512 100644 --- a/gtsam/hybrid/tests/testSerializationHybrid.cpp +++ b/gtsam/hybrid/tests/testSerializationHybrid.cpp @@ -130,7 +130,7 @@ TEST(HybridSerialization, HybridGaussianConditional) { // Test HybridBayesNet serialization. TEST(HybridSerialization, HybridBayesNet) { Switching s(2); - HybridBayesNet hbn = *(s.linearizedFactorGraph.eliminateSequential()); + HybridBayesNet hbn = *(s.linearizedFactorGraph().eliminateSequential()); EXPECT(equalsObj(hbn)); EXPECT(equalsXML(hbn)); @@ -141,7 +141,7 @@ TEST(HybridSerialization, HybridBayesNet) { // Test HybridBayesTree serialization. TEST(HybridSerialization, HybridBayesTree) { Switching s(2); - HybridBayesTree hbt = *(s.linearizedFactorGraph.eliminateMultifrontal()); + HybridBayesTree hbt = *(s.linearizedFactorGraph().eliminateMultifrontal()); EXPECT(equalsObj(hbt)); EXPECT(equalsXML(hbt)); From d985f2fc257650a3e149a53739ae1154cece352e Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Wed, 30 Oct 2024 18:21:14 -0400 Subject: [PATCH 070/115] add missing header --- gtsam/hybrid/HybridGaussianFactorGraph.h | 1 + 1 file changed, 1 insertion(+) diff --git a/gtsam/hybrid/HybridGaussianFactorGraph.h b/gtsam/hybrid/HybridGaussianFactorGraph.h index c2e50ace84..e3c1e2d557 100644 --- a/gtsam/hybrid/HybridGaussianFactorGraph.h +++ b/gtsam/hybrid/HybridGaussianFactorGraph.h @@ -18,6 +18,7 @@ #pragma once +#include #include #include #include From 56943f7be8cc22c5a44d5dd0ecc0183428068730 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Thu, 31 Oct 2024 10:24:37 -0700 Subject: [PATCH 071/115] Better SVD handling --- gtsam/geometry/FundamentalMatrix.cpp | 42 ++++++++++------------------ gtsam/geometry/FundamentalMatrix.h | 21 ++++++-------- 2 files changed, 24 insertions(+), 39 deletions(-) diff --git a/gtsam/geometry/FundamentalMatrix.cpp b/gtsam/geometry/FundamentalMatrix.cpp index 406269ff57..5e8c26e627 100644 --- a/gtsam/geometry/FundamentalMatrix.cpp +++ b/gtsam/geometry/FundamentalMatrix.cpp @@ -57,32 +57,20 @@ FundamentalMatrix::FundamentalMatrix(const Matrix3& F) { initialize(U, singularValues(1), V); } -void FundamentalMatrix::initialize(const Matrix3& U, double s, - const Matrix3& V) { - s_ = s; - sign_ = 1.0; - - // 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_; - } else { - U_ = Rot3(U); - } +void FundamentalMatrix::initialize(Matrix3 U, double s, Matrix3 V) { + // Check if U is a reflection and flip third column if so + if (U.determinant() < 0) U.col(2) *= -1; // Same check for V - double detV = V.determinant(); - if (detV < 0) { - V_ = Rot3(-V); - sign_ = -sign_; - } else { - V_ = Rot3(V); - } + if (V.determinant() < 0) V.col(2) *= -1; + + U_ = Rot3(U); + s_ = s; + V_ = Rot3(V); } Matrix3 FundamentalMatrix::matrix() const { - return sign_ * U_.matrix() * Vector3(1.0, s_, 0).asDiagonal() * + return U_.matrix() * Vector3(1.0, s_, 0).asDiagonal() * V_.transpose().matrix(); } @@ -92,8 +80,8 @@ void FundamentalMatrix::print(const std::string& s) const { bool FundamentalMatrix::equals(const FundamentalMatrix& other, double tol) const { - return U_.equals(other.U_, tol) && sign_ == other.sign_ && - std::abs(s_ - other.s_) < tol && V_.equals(other.V_, tol); + return U_.equals(other.U_, tol) && std::abs(s_ - other.s_) < tol && + V_.equals(other.V_, tol); } Vector FundamentalMatrix::localCoordinates(const FundamentalMatrix& F) const { @@ -105,10 +93,10 @@ Vector FundamentalMatrix::localCoordinates(const FundamentalMatrix& F) const { } FundamentalMatrix FundamentalMatrix::retract(const Vector& delta) const { - Rot3 newU = U_.retract(delta.head<3>()); - double newS = s_ + delta(3); // Update scalar - Rot3 newV = V_.retract(delta.tail<3>()); - return FundamentalMatrix(newU, sign_, newS, newV); + const Rot3 newU = U_.retract(delta.head<3>()); + const double newS = s_ + delta(3); + const Rot3 newV = V_.retract(delta.tail<3>()); + return FundamentalMatrix(newU, newS, newV); } //************************************************************************* diff --git a/gtsam/geometry/FundamentalMatrix.h b/gtsam/geometry/FundamentalMatrix.h index beb2947e2c..c114c2b5be 100644 --- a/gtsam/geometry/FundamentalMatrix.h +++ b/gtsam/geometry/FundamentalMatrix.h @@ -20,23 +20,20 @@ namespace gtsam { * * The FundamentalMatrix class encapsulates the fundamental matrix, which * relates corresponding points in stereo images. It is parameterized by two - * rotation matrices (U and V), a scalar parameter (s), and a sign. + * rotation matrices (U and V) and a scalar parameter (s). * Using these values, the fundamental matrix is represented as * - * F = sign * U * diag(1, s, 0) * V^T - * - * We need the `sign` because we use SO(3) for U and V, not O(3). + * F = U * diag(1, s, 0) * V^T */ class GTSAM_EXPORT FundamentalMatrix { private: - Rot3 U_; ///< Left rotation - double sign_; ///< Whether to flip the sign or not - double s_; ///< Scalar parameter for S - Rot3 V_; ///< Right rotation + Rot3 U_; ///< Left rotation + double s_; ///< Scalar parameter for S + Rot3 V_; ///< Right rotation public: /// Default constructor - FundamentalMatrix() : U_(Rot3()), sign_(1.0), s_(1.0), V_(Rot3()) {} + FundamentalMatrix() : U_(Rot3()), s_(1.0), V_(Rot3()) {} /** * @brief Construct from U, V, and scalar s @@ -113,11 +110,11 @@ class GTSAM_EXPORT FundamentalMatrix { /// @} private: /// Private constructor for internal use - FundamentalMatrix(const Rot3& U, double sign, double scaled_s, const Rot3& V) - : U_(U), sign_(sign), s_(scaled_s), V_(V) {} + FundamentalMatrix(const Rot3& U, double s, const Rot3& V) + : U_(U), s_(s), V_(V) {} /// Initialize SO(3) matrices from general O(3) matrices - void initialize(const Matrix3& U, double s, const Matrix3& V); + void initialize(Matrix3 U, double s, Matrix3 V); }; /** From f5f878e6fa0c99326155a88a5e27820cb71f9ee1 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Fri, 1 Nov 2024 14:30:31 -0400 Subject: [PATCH 072/115] update test group --- gtsam/hybrid/tests/testHybridMotionModel.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/gtsam/hybrid/tests/testHybridMotionModel.cpp b/gtsam/hybrid/tests/testHybridMotionModel.cpp index 16a2bd476d..867f22cc68 100644 --- a/gtsam/hybrid/tests/testHybridMotionModel.cpp +++ b/gtsam/hybrid/tests/testHybridMotionModel.cpp @@ -124,7 +124,7 @@ std::pair approximateDiscreteMarginal( * the posterior probability of m1 should be 0.5/0.5. * Getting a measurement on z1 gives use more information. */ -TEST(HybridGaussianFactor, TwoStateModel) { +TEST(HybridGaussianFactorGraph, TwoStateModel) { double mu0 = 1.0, mu1 = 3.0; double sigma = 0.5; auto hybridMotionModel = CreateHybridMotionModel(mu0, mu1, sigma, sigma); @@ -178,7 +178,7 @@ TEST(HybridGaussianFactor, TwoStateModel) { * the P(m1) should be 0.5/0.5. * Getting a measurement on z1 gives use more information. */ -TEST(HybridGaussianFactor, TwoStateModel2) { +TEST(HybridGaussianFactorGraph, TwoStateModel2) { double mu0 = 1.0, mu1 = 3.0; double sigma0 = 0.5, sigma1 = 2.0; auto hybridMotionModel = CreateHybridMotionModel(mu0, mu1, sigma0, sigma1); @@ -281,7 +281,7 @@ TEST(HybridGaussianFactor, TwoStateModel2) { * the p(m1) should be 0.5/0.5. * Getting a measurement on z1 gives use more information. */ -TEST(HybridGaussianFactor, TwoStateModel3) { +TEST(HybridGaussianFactorGraph, TwoStateModel3) { double mu = 1.0; double sigma0 = 0.5, sigma1 = 2.0; auto hybridMotionModel = CreateHybridMotionModel(mu, mu, sigma0, sigma1); @@ -366,7 +366,7 @@ TEST(HybridGaussianFactor, TwoStateModel3) { * measurements and vastly different motion model: either stand still or move * far. This yields a very informative posterior. */ -TEST(HybridGaussianFactor, TwoStateModel4) { +TEST(HybridGaussianFactorGraph, TwoStateModel4) { double mu0 = 0.0, mu1 = 10.0; double sigma0 = 0.2, sigma1 = 5.0; auto hybridMotionModel = CreateHybridMotionModel(mu0, mu1, sigma0, sigma1); From 01829381da5ec0fca7a2b9c382fae75eb4327f84 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Fri, 1 Nov 2024 14:31:27 -0400 Subject: [PATCH 073/115] move direct FG motion model test to testHybridMotionModel.cpp --- .../hybrid/tests/testHybridGaussianFactor.cpp | 158 ----------------- gtsam/hybrid/tests/testHybridMotionModel.cpp | 162 +++++++++++++++++- 2 files changed, 160 insertions(+), 160 deletions(-) diff --git a/gtsam/hybrid/tests/testHybridGaussianFactor.cpp b/gtsam/hybrid/tests/testHybridGaussianFactor.cpp index 05e05c79b7..9642796ab1 100644 --- a/gtsam/hybrid/tests/testHybridGaussianFactor.cpp +++ b/gtsam/hybrid/tests/testHybridGaussianFactor.cpp @@ -194,164 +194,6 @@ TEST(HybridGaussianFactor, Error) { 4.0, hybridFactor.error({continuousValues, discreteValues}), 1e-9); } -/* ************************************************************************* */ -namespace test_direct_factor_graph { -/** - * @brief Create a Factor Graph by directly specifying all - * the factors instead of creating conditionals first. - * This way we can directly provide the likelihoods and - * then perform linearization. - * - * @param values Initial values to linearize around. - * @param means The means of the HybridGaussianFactor components. - * @param sigmas The covariances of the HybridGaussianFactor components. - * @param m1 The discrete key. - * @return HybridGaussianFactorGraph - */ -static HybridGaussianFactorGraph CreateFactorGraph( - const gtsam::Values &values, const std::vector &means, - const std::vector &sigmas, DiscreteKey &m1, - double measurement_noise = 1e-3) { - auto model0 = noiseModel::Isotropic::Sigma(1, sigmas[0]); - auto model1 = noiseModel::Isotropic::Sigma(1, sigmas[1]); - auto prior_noise = noiseModel::Isotropic::Sigma(1, measurement_noise); - - auto f0 = - std::make_shared>(X(0), X(1), means[0], model0) - ->linearize(values); - auto f1 = - std::make_shared>(X(0), X(1), means[1], model1) - ->linearize(values); - - // Create HybridGaussianFactor - // We take negative since we want - // the underlying scalar to be log(\sqrt(|2πΣ|)) - std::vector factors{{f0, model0->negLogConstant()}, - {f1, model1->negLogConstant()}}; - HybridGaussianFactor motionFactor(m1, factors); - - HybridGaussianFactorGraph hfg; - hfg.push_back(motionFactor); - - hfg.push_back(PriorFactor(X(0), values.at(X(0)), prior_noise) - .linearize(values)); - - return hfg; -} -} // namespace test_direct_factor_graph - -/* ************************************************************************* */ -/** - * @brief Test components with differing means but the same covariances. - * The factor graph is - * *-X1-*-X2 - * | - * M1 - */ -TEST(HybridGaussianFactor, DifferentMeansFG) { - using namespace test_direct_factor_graph; - - DiscreteKey m1(M(1), 2); - - Values values; - double x1 = 0.0, x2 = 1.75; - values.insert(X(0), x1); - values.insert(X(1), x2); - - std::vector means = {0.0, 2.0}, sigmas = {1e-0, 1e-0}; - - HybridGaussianFactorGraph hfg = CreateFactorGraph(values, means, sigmas, m1); - - { - auto bn = hfg.eliminateSequential(); - HybridValues actual = bn->optimize(); - - HybridValues expected( - VectorValues{{X(0), Vector1(0.0)}, {X(1), Vector1(-1.75)}}, - DiscreteValues{{M(1), 0}}); - - EXPECT(assert_equal(expected, actual)); - - DiscreteValues dv0{{M(1), 0}}; - VectorValues cont0 = bn->optimize(dv0); - double error0 = bn->error(HybridValues(cont0, dv0)); - // regression - EXPECT_DOUBLES_EQUAL(0.69314718056, error0, 1e-9); - - DiscreteValues dv1{{M(1), 1}}; - VectorValues cont1 = bn->optimize(dv1); - double error1 = bn->error(HybridValues(cont1, dv1)); - EXPECT_DOUBLES_EQUAL(error0, error1, 1e-9); - } - - { - auto prior_noise = noiseModel::Isotropic::Sigma(1, 1e-3); - hfg.push_back( - PriorFactor(X(1), means[1], prior_noise).linearize(values)); - - auto bn = hfg.eliminateSequential(); - HybridValues actual = bn->optimize(); - - HybridValues expected( - VectorValues{{X(0), Vector1(0.0)}, {X(1), Vector1(0.25)}}, - DiscreteValues{{M(1), 1}}); - - EXPECT(assert_equal(expected, actual)); - - { - DiscreteValues dv{{M(1), 0}}; - VectorValues cont = bn->optimize(dv); - double error = bn->error(HybridValues(cont, dv)); - // regression - EXPECT_DOUBLES_EQUAL(2.12692448787, error, 1e-9); - } - { - DiscreteValues dv{{M(1), 1}}; - VectorValues cont = bn->optimize(dv); - double error = bn->error(HybridValues(cont, dv)); - // regression - EXPECT_DOUBLES_EQUAL(0.126928487854, error, 1e-9); - } - } -} - -/* ************************************************************************* */ -/** - * @brief Test components with differing covariances but the same means. - * The factor graph is - * *-X1-*-X2 - * | - * M1 - */ -TEST(HybridGaussianFactor, DifferentCovariancesFG) { - using namespace test_direct_factor_graph; - - DiscreteKey m1(M(1), 2); - - Values values; - double x1 = 1.0, x2 = 1.0; - values.insert(X(0), x1); - values.insert(X(1), x2); - - std::vector means = {0.0, 0.0}, sigmas = {1e2, 1e-2}; - - // Create FG with HybridGaussianFactor and prior on X1 - HybridGaussianFactorGraph fg = CreateFactorGraph(values, means, sigmas, m1); - auto hbn = fg.eliminateSequential(); - - VectorValues cv; - cv.insert(X(0), Vector1(0.0)); - cv.insert(X(1), Vector1(0.0)); - - DiscreteValues dv0{{M(1), 0}}; - DiscreteValues dv1{{M(1), 1}}; - - DiscreteConditional expected_m1(m1, "0.5/0.5"); - DiscreteConditional actual_m1 = *(hbn->at(2)->asDiscrete()); - - EXPECT(assert_equal(expected_m1, actual_m1)); -} - /* ************************************************************************* */ int main() { TestResult tr; diff --git a/gtsam/hybrid/tests/testHybridMotionModel.cpp b/gtsam/hybrid/tests/testHybridMotionModel.cpp index 867f22cc68..747a1b6883 100644 --- a/gtsam/hybrid/tests/testHybridMotionModel.cpp +++ b/gtsam/hybrid/tests/testHybridMotionModel.cpp @@ -198,7 +198,7 @@ TEST(HybridGaussianFactorGraph, TwoStateModel2) { {VectorValues{{X(0), Vector1(0.0)}, {X(1), Vector1(1.0)}}, VectorValues{{X(0), Vector1(0.5)}, {X(1), Vector1(3.0)}}}) { vv.insert(given); // add measurements for HBN - const auto& expectedDiscretePosterior = hbn.discretePosterior(vv); + const auto &expectedDiscretePosterior = hbn.discretePosterior(vv); // Equality of posteriors asserts that the factor graph is correct (same // ratios for all modes) @@ -234,7 +234,7 @@ TEST(HybridGaussianFactorGraph, TwoStateModel2) { {VectorValues{{X(0), Vector1(0.0)}, {X(1), Vector1(1.0)}}, VectorValues{{X(0), Vector1(0.5)}, {X(1), Vector1(3.0)}}}) { vv.insert(given); // add measurements for HBN - const auto& expectedDiscretePosterior = hbn.discretePosterior(vv); + const auto &expectedDiscretePosterior = hbn.discretePosterior(vv); // Equality of posteriors asserts that the factor graph is correct (same // ratios for all modes) @@ -385,6 +385,164 @@ TEST(HybridGaussianFactorGraph, TwoStateModel4) { EXPECT(assert_equal(expected, *(bn->at(2)->asDiscrete()), 0.002)); } +/* ************************************************************************* */ +namespace test_direct_factor_graph { +/** + * @brief Create a Factor Graph by directly specifying all + * the factors instead of creating conditionals first. + * This way we can directly provide the likelihoods and + * then perform linearization. + * + * @param values Initial values to linearize around. + * @param means The means of the HybridGaussianFactor components. + * @param sigmas The covariances of the HybridGaussianFactor components. + * @param m1 The discrete key. + * @return HybridGaussianFactorGraph + */ +static HybridGaussianFactorGraph CreateFactorGraph( + const gtsam::Values &values, const std::vector &means, + const std::vector &sigmas, DiscreteKey &m1, + double measurement_noise = 1e-3) { + auto model0 = noiseModel::Isotropic::Sigma(1, sigmas[0]); + auto model1 = noiseModel::Isotropic::Sigma(1, sigmas[1]); + auto prior_noise = noiseModel::Isotropic::Sigma(1, measurement_noise); + + auto f0 = + std::make_shared>(X(0), X(1), means[0], model0) + ->linearize(values); + auto f1 = + std::make_shared>(X(0), X(1), means[1], model1) + ->linearize(values); + + // Create HybridGaussianFactor + // We take negative since we want + // the underlying scalar to be log(\sqrt(|2πΣ|)) + std::vector factors{{f0, model0->negLogConstant()}, + {f1, model1->negLogConstant()}}; + HybridGaussianFactor motionFactor(m1, factors); + + HybridGaussianFactorGraph hfg; + hfg.push_back(motionFactor); + + hfg.push_back(PriorFactor(X(0), values.at(X(0)), prior_noise) + .linearize(values)); + + return hfg; +} +} // namespace test_direct_factor_graph + +/* ************************************************************************* */ +/** + * @brief Test components with differing means but the same covariances. + * The factor graph is + * *-X1-*-X2 + * | + * M1 + */ +TEST(HybridGaussianFactorGraph, DifferentMeans) { + using namespace test_direct_factor_graph; + + DiscreteKey m1(M(1), 2); + + Values values; + double x1 = 0.0, x2 = 1.75; + values.insert(X(0), x1); + values.insert(X(1), x2); + + std::vector means = {0.0, 2.0}, sigmas = {1e-0, 1e-0}; + + HybridGaussianFactorGraph hfg = CreateFactorGraph(values, means, sigmas, m1); + + { + auto bn = hfg.eliminateSequential(); + HybridValues actual = bn->optimize(); + + HybridValues expected( + VectorValues{{X(0), Vector1(0.0)}, {X(1), Vector1(-1.75)}}, + DiscreteValues{{M(1), 0}}); + + EXPECT(assert_equal(expected, actual)); + + DiscreteValues dv0{{M(1), 0}}; + VectorValues cont0 = bn->optimize(dv0); + double error0 = bn->error(HybridValues(cont0, dv0)); + // regression + EXPECT_DOUBLES_EQUAL(0.69314718056, error0, 1e-9); + + DiscreteValues dv1{{M(1), 1}}; + VectorValues cont1 = bn->optimize(dv1); + double error1 = bn->error(HybridValues(cont1, dv1)); + EXPECT_DOUBLES_EQUAL(error0, error1, 1e-9); + } + + { + auto prior_noise = noiseModel::Isotropic::Sigma(1, 1e-3); + hfg.push_back( + PriorFactor(X(1), means[1], prior_noise).linearize(values)); + + auto bn = hfg.eliminateSequential(); + HybridValues actual = bn->optimize(); + + HybridValues expected( + VectorValues{{X(0), Vector1(0.0)}, {X(1), Vector1(0.25)}}, + DiscreteValues{{M(1), 1}}); + + EXPECT(assert_equal(expected, actual)); + + { + DiscreteValues dv{{M(1), 0}}; + VectorValues cont = bn->optimize(dv); + double error = bn->error(HybridValues(cont, dv)); + // regression + EXPECT_DOUBLES_EQUAL(2.12692448787, error, 1e-9); + } + { + DiscreteValues dv{{M(1), 1}}; + VectorValues cont = bn->optimize(dv); + double error = bn->error(HybridValues(cont, dv)); + // regression + EXPECT_DOUBLES_EQUAL(0.126928487854, error, 1e-9); + } + } +} + +/* ************************************************************************* */ +/** + * @brief Test components with differing covariances but the same means. + * The factor graph is + * *-X1-*-X2 + * | + * M1 + */ +TEST(HybridGaussianFactorGraph, DifferentCovariances) { + using namespace test_direct_factor_graph; + + DiscreteKey m1(M(1), 2); + + Values values; + double x1 = 1.0, x2 = 1.0; + values.insert(X(0), x1); + values.insert(X(1), x2); + + std::vector means = {0.0, 0.0}, sigmas = {1e2, 1e-2}; + + // Create FG with HybridGaussianFactor and prior on X1 + HybridGaussianFactorGraph fg = CreateFactorGraph(values, means, sigmas, m1); + auto hbn = fg.eliminateSequential(); + + VectorValues cv; + cv.insert(X(0), Vector1(0.0)); + cv.insert(X(1), Vector1(0.0)); + + DiscreteValues dv0{{M(1), 0}}; + DiscreteValues dv1{{M(1), 1}}; + + DiscreteConditional expected_m1(m1, "0.5/0.5"); + DiscreteConditional actual_m1 = *(hbn->at(2)->asDiscrete()); + + EXPECT(assert_equal(expected_m1, actual_m1)); +} + /* ************************************************************************* */ int main() { TestResult tr; From 1b5a6ebba9f9233698bc369ad4e4323927ccdd26 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Fri, 1 Nov 2024 14:31:40 -0400 Subject: [PATCH 074/115] update test name --- gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp b/gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp index 13a6cd614f..31e36101b2 100644 --- a/gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp +++ b/gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp @@ -545,7 +545,7 @@ TEST(HybridGaussianFactorGraph, IncrementalErrorTree) { /* ****************************************************************************/ // Check that collectProductFactor works correctly. -TEST(HybridGaussianFactorGraph, collectProductFactor) { +TEST(HybridGaussianFactorGraph, CollectProductFactor) { const int num_measurements = 1; VectorValues vv{{Z(0), Vector1(5.0)}}; auto fg = tiny::createHybridGaussianFactorGraph(num_measurements, vv); From 9be3f41ca2164d77fe5d8b9cb46a580569cc2058 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Fri, 1 Nov 2024 19:58:23 -0400 Subject: [PATCH 075/115] Correct the second term in the pruner value so that the minNegLogConstant term is set correctly --- gtsam/hybrid/HybridGaussianConditional.cpp | 7 +++++-- gtsam/hybrid/tests/testHybridGaussianConditional.cpp | 8 ++++++++ 2 files changed, 13 insertions(+), 2 deletions(-) diff --git a/gtsam/hybrid/HybridGaussianConditional.cpp b/gtsam/hybrid/HybridGaussianConditional.cpp index ac03bd3a3e..1bec428107 100644 --- a/gtsam/hybrid/HybridGaussianConditional.cpp +++ b/gtsam/hybrid/HybridGaussianConditional.cpp @@ -322,8 +322,11 @@ HybridGaussianConditional::shared_ptr HybridGaussianConditional::prune( const GaussianFactorValuePair &pair) -> GaussianFactorValuePair { if (max->evaluate(choices) == 0.0) return {nullptr, std::numeric_limits::infinity()}; - else - return pair; + else { + // Add negLogConstant_ back so that the minimum negLogConstant in the + // HybridGaussianConditional is set correctly. + return {pair.first, pair.second + negLogConstant_}; + } }; FactorValuePairs prunedConditionals = factors().apply(pruner); diff --git a/gtsam/hybrid/tests/testHybridGaussianConditional.cpp b/gtsam/hybrid/tests/testHybridGaussianConditional.cpp index e29c485afd..350bc91848 100644 --- a/gtsam/hybrid/tests/testHybridGaussianConditional.cpp +++ b/gtsam/hybrid/tests/testHybridGaussianConditional.cpp @@ -275,6 +275,11 @@ TEST(HybridGaussianConditional, Prune) { // Check that the pruned HybridGaussianConditional has 2 conditionals EXPECT_LONGS_EQUAL(2, pruned->nrComponents()); + + // Check that the minimum negLogConstant is set correctly + EXPECT_DOUBLES_EQUAL( + hgc.conditionals()({{M(1), 0}, {M(2), 1}})->negLogConstant(), + pruned->negLogConstant(), 1e-9); } { const std::vector potentials{0.2, 0, 0.3, 0, // @@ -285,6 +290,9 @@ TEST(HybridGaussianConditional, Prune) { // Check that the pruned HybridGaussianConditional has 3 conditionals EXPECT_LONGS_EQUAL(3, pruned->nrComponents()); + + // Check that the minimum negLogConstant is correct + EXPECT_DOUBLES_EQUAL(hgc.negLogConstant(), pruned->negLogConstant(), 1e-9); } } From e52970aa9269c79e1b508cf6cb64b31fe50b13e0 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Fri, 1 Nov 2024 20:23:04 -0400 Subject: [PATCH 076/115] negLogConstant methods for HybridBayesNet --- gtsam/hybrid/HybridBayesNet.cpp | 29 +++++++++++++++++++++++++++++ gtsam/hybrid/HybridBayesNet.h | 17 +++++++++++++++++ 2 files changed, 46 insertions(+) diff --git a/gtsam/hybrid/HybridBayesNet.cpp b/gtsam/hybrid/HybridBayesNet.cpp index f57cda28d9..e5748366c3 100644 --- a/gtsam/hybrid/HybridBayesNet.cpp +++ b/gtsam/hybrid/HybridBayesNet.cpp @@ -197,6 +197,35 @@ AlgebraicDecisionTree HybridBayesNet::errorTree( return result; } +/* ************************************************************************* */ +double HybridBayesNet::negLogConstant() const { + double negLogNormConst = 0.0; + // Iterate over each conditional. + for (auto &&conditional : *this) { + negLogNormConst += conditional->negLogConstant(); + } + return negLogNormConst; +} + +/* ************************************************************************* */ +double HybridBayesNet::negLogConstant(const DiscreteValues &discrete) const { + double negLogNormConst = 0.0; + // Iterate over each conditional. + for (auto &&conditional : *this) { + if (auto gm = conditional->asHybrid()) { + negLogNormConst += gm->choose(discrete)->negLogConstant(); + } else if (auto gc = conditional->asGaussian()) { + negLogNormConst += gc->negLogConstant(); + } else if (auto dc = conditional->asDiscrete()) { + negLogNormConst += dc->choose(discrete)->negLogConstant(); + } else { + throw std::runtime_error( + "Unknown conditional type when computing negLogConstant"); + } + } + return negLogNormConst; +} + /* ************************************************************************* */ AlgebraicDecisionTree HybridBayesNet::discretePosterior( const VectorValues &continuousValues) const { diff --git a/gtsam/hybrid/HybridBayesNet.h b/gtsam/hybrid/HybridBayesNet.h index bba301be2f..451f7f6757 100644 --- a/gtsam/hybrid/HybridBayesNet.h +++ b/gtsam/hybrid/HybridBayesNet.h @@ -237,6 +237,23 @@ class GTSAM_EXPORT HybridBayesNet : public BayesNet { using BayesNet::logProbability; // expose HybridValues version + /** + * @brief Get the negative log of the normalization constant corresponding + * to the joint density represented by this Bayes net. + * + * @return double + */ + double negLogConstant() const; + + /** + * @brief Get the negative log of the normalization constant + * corresponding to the joint Gaussian density represented by + * this Bayes net indexed by `discrete`. + * + * @return double + */ + double negLogConstant(const DiscreteValues &discrete) const; + /** * @brief Compute normalized posterior P(M|X=x) and return as a tree. * From 44e848536033c951a809a9bb4cabc3bbf7f17fa5 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Fri, 1 Nov 2024 20:23:32 -0400 Subject: [PATCH 077/115] get failing tests in testHybridBayesNet to pass --- gtsam/hybrid/tests/testHybridBayesNet.cpp | 42 ++++++++++++++++++----- 1 file changed, 34 insertions(+), 8 deletions(-) diff --git a/gtsam/hybrid/tests/testHybridBayesNet.cpp b/gtsam/hybrid/tests/testHybridBayesNet.cpp index 16d0ae1a12..135da5dc73 100644 --- a/gtsam/hybrid/tests/testHybridBayesNet.cpp +++ b/gtsam/hybrid/tests/testHybridBayesNet.cpp @@ -363,10 +363,6 @@ TEST(HybridBayesNet, Pruning) { AlgebraicDecisionTree expected(s.modes, leaves); EXPECT(assert_equal(expected, discretePosterior, 1e-6)); - // Prune and get probabilities - auto prunedBayesNet = posterior->prune(2); - auto prunedTree = prunedBayesNet.discretePosterior(delta.continuous()); - // Verify logProbability computation and check specific logProbability value const DiscreteValues discrete_values{{M(0), 1}, {M(1), 1}}; const HybridValues hybridValues{delta.continuous(), discrete_values}; @@ -381,10 +377,21 @@ TEST(HybridBayesNet, Pruning) { EXPECT_DOUBLES_EQUAL(logProbability, posterior->logProbability(hybridValues), 1e-9); + double negLogConstant = posterior->negLogConstant(discrete_values); + + // The sum of all the mode densities + double normalizer = + AlgebraicDecisionTree(posterior->errorTree(delta.continuous()), + [](double error) { return exp(-error); }) + .sum(); + // Check agreement with discrete posterior - // double density = exp(logProbability); - // FAILS: EXPECT_DOUBLES_EQUAL(density, discretePosterior(discrete_values), - // 1e-6); + double density = exp(logProbability + negLogConstant) / normalizer; + EXPECT_DOUBLES_EQUAL(density, discretePosterior(discrete_values), 1e-6); + + // Prune and get probabilities + auto prunedBayesNet = posterior->prune(2); + auto prunedTree = prunedBayesNet.discretePosterior(delta.continuous()); // Regression test on pruned logProbability tree std::vector pruned_leaves = {0.0, 0.50758422, 0.0, 0.49241578}; @@ -392,7 +399,26 @@ TEST(HybridBayesNet, Pruning) { EXPECT(assert_equal(expected_pruned, prunedTree, 1e-6)); // Regression - // FAILS: EXPECT_DOUBLES_EQUAL(density, prunedTree(discrete_values), 1e-9); + double pruned_logProbability = 0; + pruned_logProbability += + prunedBayesNet.at(0)->asDiscrete()->logProbability(hybridValues); + pruned_logProbability += + prunedBayesNet.at(1)->asHybrid()->logProbability(hybridValues); + pruned_logProbability += + prunedBayesNet.at(2)->asHybrid()->logProbability(hybridValues); + pruned_logProbability += + prunedBayesNet.at(3)->asHybrid()->logProbability(hybridValues); + + double pruned_negLogConstant = prunedBayesNet.negLogConstant(discrete_values); + + // The sum of all the mode densities + double pruned_normalizer = + AlgebraicDecisionTree(prunedBayesNet.errorTree(delta.continuous()), + [](double error) { return exp(-error); }) + .sum(); + double pruned_density = + exp(pruned_logProbability + pruned_negLogConstant) / pruned_normalizer; + EXPECT_DOUBLES_EQUAL(pruned_density, prunedTree(discrete_values), 1e-9); } /* ****************************************************************************/ From 8aacfa95f32ab6459d7888146b406820a25bade0 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Fri, 1 Nov 2024 20:24:36 -0400 Subject: [PATCH 078/115] add docstrings for elimination results --- gtsam/hybrid/HybridGaussianFactorGraph.cpp | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/gtsam/hybrid/HybridGaussianFactorGraph.cpp b/gtsam/hybrid/HybridGaussianFactorGraph.cpp index ceabe0871a..9ca7a3938e 100644 --- a/gtsam/hybrid/HybridGaussianFactorGraph.cpp +++ b/gtsam/hybrid/HybridGaussianFactorGraph.cpp @@ -59,10 +59,11 @@ using OrphanWrapper = BayesTreeOrphanWrapper; /// Result from elimination. struct Result { + // Gaussian conditional resulting from elimination. GaussianConditional::shared_ptr conditional; - double negLogK; - GaussianFactor::shared_ptr factor; - double scalar; + double negLogK; // Negative log of the normalization constant K. + GaussianFactor::shared_ptr factor; // Leftover factor 𝜏. + double scalar; // Scalar value associated with factor 𝜏. bool operator==(const Result &other) const { return conditional == other.conditional && negLogK == other.negLogK && From d091a9d4402abe7c717015abe1c68ed4707659be Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Fri, 1 Nov 2024 20:29:24 -0400 Subject: [PATCH 079/115] combined update and pruning --- gtsam/hybrid/tests/testHybridGaussianISAM.cpp | 11 ++++------- 1 file changed, 4 insertions(+), 7 deletions(-) diff --git a/gtsam/hybrid/tests/testHybridGaussianISAM.cpp b/gtsam/hybrid/tests/testHybridGaussianISAM.cpp index 79669cfb31..0e323f5b41 100644 --- a/gtsam/hybrid/tests/testHybridGaussianISAM.cpp +++ b/gtsam/hybrid/tests/testHybridGaussianISAM.cpp @@ -258,7 +258,7 @@ TEST(HybridGaussianElimination, Approx_inference) { /* ****************************************************************************/ // Test approximate inference with an additional pruning step. -TEST_DISABLED(HybridGaussianElimination, IncrementalApproximate) { +TEST(HybridGaussianElimination, IncrementalApproximate) { Switching switching(5); HybridGaussianISAM incrementalHybrid; HybridGaussianFactorGraph graph; @@ -277,8 +277,7 @@ TEST_DISABLED(HybridGaussianElimination, IncrementalApproximate) { // Run update with pruning size_t maxComponents = 5; - incrementalHybrid.update(graph); - incrementalHybrid.prune(maxComponents); + incrementalHybrid.update(graph, maxComponents); // Check if we have a bayes tree with 4 hybrid nodes, // each with 2, 4, 8, and 5 (pruned) leaves respectively. @@ -298,8 +297,7 @@ TEST_DISABLED(HybridGaussianElimination, IncrementalApproximate) { graph.push_back(switching.linearUnaryFactors.at(4)); // x4 // Run update with pruning a second time. - incrementalHybrid.update(graph); - incrementalHybrid.prune(maxComponents); + incrementalHybrid.update(graph, maxComponents); // Check if we have a bayes tree with pruned hybrid nodes, // with 5 (pruned) leaves. @@ -470,8 +468,7 @@ TEST(HybridGaussianISAM, NonTrivial) { fg = HybridNonlinearFactorGraph(); // Keep pruning! - inc.update(gfg); - inc.prune(3); + inc.update(gfg, 3); // The final discrete graph should not be empty since we have eliminated // all continuous variables. From 6b3cb6579a6f1afb9f170ad6d7d5d3a1cd1c4735 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sat, 2 Nov 2024 21:22:38 -0400 Subject: [PATCH 080/115] update test group name --- gtsam/hybrid/tests/testHybridGaussianISAM.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/gtsam/hybrid/tests/testHybridGaussianISAM.cpp b/gtsam/hybrid/tests/testHybridGaussianISAM.cpp index 0e323f5b41..04b44f9041 100644 --- a/gtsam/hybrid/tests/testHybridGaussianISAM.cpp +++ b/gtsam/hybrid/tests/testHybridGaussianISAM.cpp @@ -56,7 +56,7 @@ const HybridGaussianFactorGraph graph2{lfg.at(4), lfg.at(1), lfg.at(2), /* ****************************************************************************/ // Test if we can perform elimination incrementally. -TEST(HybridGaussianElimination, IncrementalElimination) { +TEST(HybridGaussianISAM, IncrementalElimination) { using namespace switching3; HybridGaussianISAM isam; @@ -88,7 +88,7 @@ TEST(HybridGaussianElimination, IncrementalElimination) { /* ****************************************************************************/ // Test if we can incrementally do the inference -TEST(HybridGaussianElimination, IncrementalInference) { +TEST(HybridGaussianISAM, IncrementalInference) { using namespace switching3; HybridGaussianISAM isam; @@ -156,7 +156,7 @@ TEST(HybridGaussianElimination, IncrementalInference) { /* ****************************************************************************/ // Test if we can approximately do the inference -TEST(HybridGaussianElimination, Approx_inference) { +TEST(HybridGaussianISAM, ApproxInference) { Switching switching(4); HybridGaussianISAM incrementalHybrid; HybridGaussianFactorGraph graph1; @@ -258,7 +258,7 @@ TEST(HybridGaussianElimination, Approx_inference) { /* ****************************************************************************/ // Test approximate inference with an additional pruning step. -TEST(HybridGaussianElimination, IncrementalApproximate) { +TEST(HybridGaussianISAM, IncrementalApproximate) { Switching switching(5); HybridGaussianISAM incrementalHybrid; HybridGaussianFactorGraph graph; From 5c63ac833c56d52b1558d0be876065939394d2c6 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sun, 3 Nov 2024 15:32:21 -0500 Subject: [PATCH 081/115] use optional DiscreteValues --- gtsam/hybrid/HybridBayesNet.cpp | 33 ++++++++++++++------------------- gtsam/hybrid/HybridBayesNet.h | 15 ++++----------- 2 files changed, 18 insertions(+), 30 deletions(-) diff --git a/gtsam/hybrid/HybridBayesNet.cpp b/gtsam/hybrid/HybridBayesNet.cpp index e5748366c3..623b82eea7 100644 --- a/gtsam/hybrid/HybridBayesNet.cpp +++ b/gtsam/hybrid/HybridBayesNet.cpp @@ -198,29 +198,24 @@ AlgebraicDecisionTree HybridBayesNet::errorTree( } /* ************************************************************************* */ -double HybridBayesNet::negLogConstant() const { +double HybridBayesNet::negLogConstant( + const std::optional &discrete) const { double negLogNormConst = 0.0; // Iterate over each conditional. for (auto &&conditional : *this) { - negLogNormConst += conditional->negLogConstant(); - } - return negLogNormConst; -} - -/* ************************************************************************* */ -double HybridBayesNet::negLogConstant(const DiscreteValues &discrete) const { - double negLogNormConst = 0.0; - // Iterate over each conditional. - for (auto &&conditional : *this) { - if (auto gm = conditional->asHybrid()) { - negLogNormConst += gm->choose(discrete)->negLogConstant(); - } else if (auto gc = conditional->asGaussian()) { - negLogNormConst += gc->negLogConstant(); - } else if (auto dc = conditional->asDiscrete()) { - negLogNormConst += dc->choose(discrete)->negLogConstant(); + if (discrete.has_value()) { + if (auto gm = conditional->asHybrid()) { + negLogNormConst += gm->choose(*discrete)->negLogConstant(); + } else if (auto gc = conditional->asGaussian()) { + negLogNormConst += gc->negLogConstant(); + } else if (auto dc = conditional->asDiscrete()) { + negLogNormConst += dc->choose(*discrete)->negLogConstant(); + } else { + throw std::runtime_error( + "Unknown conditional type when computing negLogConstant"); + } } else { - throw std::runtime_error( - "Unknown conditional type when computing negLogConstant"); + negLogNormConst += conditional->negLogConstant(); } } return negLogNormConst; diff --git a/gtsam/hybrid/HybridBayesNet.h b/gtsam/hybrid/HybridBayesNet.h index 451f7f6757..96afb87d6d 100644 --- a/gtsam/hybrid/HybridBayesNet.h +++ b/gtsam/hybrid/HybridBayesNet.h @@ -237,22 +237,15 @@ class GTSAM_EXPORT HybridBayesNet : public BayesNet { using BayesNet::logProbability; // expose HybridValues version - /** - * @brief Get the negative log of the normalization constant corresponding - * to the joint density represented by this Bayes net. - * - * @return double - */ - double negLogConstant() const; - /** * @brief Get the negative log of the normalization constant - * corresponding to the joint Gaussian density represented by - * this Bayes net indexed by `discrete`. + * corresponding to the joint density represented by this Bayes net. + * Optionally index by `discrete`. * + * @param discrete Optional DiscreteValues * @return double */ - double negLogConstant(const DiscreteValues &discrete) const; + double negLogConstant(const std::optional &discrete) const; /** * @brief Compute normalized posterior P(M|X=x) and return as a tree. From a7b53aef0e5905d558560eab0fcb20db0832209e Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 4 Nov 2024 14:52:21 -0500 Subject: [PATCH 082/115] better check for discrete factors --- gtsam/hybrid/HybridGaussianFactorGraph.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/gtsam/hybrid/HybridGaussianFactorGraph.cpp b/gtsam/hybrid/HybridGaussianFactorGraph.cpp index 049e6c38d2..b66847ca7a 100644 --- a/gtsam/hybrid/HybridGaussianFactorGraph.cpp +++ b/gtsam/hybrid/HybridGaussianFactorGraph.cpp @@ -584,9 +584,9 @@ GaussianFactorGraph HybridGaussianFactorGraph::choose( DiscreteFactorGraph HybridGaussianFactorGraph::discreteFactors() const { DiscreteFactorGraph dfg; for (auto &&f : factors_) { - auto discreteFactor = std::dynamic_pointer_cast(f); - assert(discreteFactor); - dfg.push_back(discreteFactor); + if (auto discreteFactor = std::dynamic_pointer_cast(f)) { + dfg.push_back(discreteFactor); + } } return dfg; } From 4f0dcec1b6f09f7bfe13ac63fc662a4475798418 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 4 Nov 2024 14:57:35 -0500 Subject: [PATCH 083/115] update macos images to minimum 13 and add ARM64 support --- .github/workflows/build-macos.yml | 18 +++++++++++++++--- .github/workflows/build-python.yml | 8 ++++---- 2 files changed, 19 insertions(+), 7 deletions(-) diff --git a/.github/workflows/build-macos.yml b/.github/workflows/build-macos.yml index e4c78bf67c..44a29d42c4 100644 --- a/.github/workflows/build-macos.yml +++ b/.github/workflows/build-macos.yml @@ -25,15 +25,22 @@ jobs: # Github Actions requires a single row to be added to the build matrix. # See https://help.github.com/en/articles/workflow-syntax-for-github-actions. name: [ - macos-12-xcode-14.2, + macos-13-xcode-14.2, + macos-13-arm64-xcode-14.2, macos-14-xcode-15.4, + macos-14-arm64-xcode-15.4, ] build_type: [Debug, Release] build_unstable: [ON] include: - - name: macos-12-xcode-14.2 - os: macos-12 + - name: macos-13-xcode-14.2 + os: macos-13 + compiler: xcode + version: "14.2" + + - name: macos-13-arm64-xcode-14.2 + os: macos-13-xlarge compiler: xcode version: "14.2" @@ -42,6 +49,11 @@ jobs: compiler: xcode version: "15.4" + - name: macos-14-arm64-xcode-15.4 + os: macos-14-xlarge + compiler: xcode + version: "15.4" + steps: - name: Checkout uses: actions/checkout@v4 diff --git a/.github/workflows/build-python.yml b/.github/workflows/build-python.yml index f581a5974f..248b766d54 100644 --- a/.github/workflows/build-python.yml +++ b/.github/workflows/build-python.yml @@ -30,7 +30,7 @@ jobs: [ ubuntu-20.04-gcc-9, ubuntu-20.04-clang-9, - macos-12-xcode-14.2, + macos-14-arm64-xcode-15.4, macos-14-xcode-15.4, windows-2022-msbuild, ] @@ -48,10 +48,10 @@ jobs: compiler: clang version: "9" - - name: macos-12-xcode-14.2 - os: macos-12 + - name: macos-14-arm64-xcode-15.4 + os: macos-14-xlarge compiler: xcode - version: "14.2" + version: "15.4" - name: macos-14-xcode-15.4 os: macos-14 From 2fc11f386a4a3c9a9c518258cf1f8042a703fb13 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 4 Nov 2024 19:27:14 -0500 Subject: [PATCH 084/115] fix yaml file --- .github/workflows/build-macos.yml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/.github/workflows/build-macos.yml b/.github/workflows/build-macos.yml index 44a29d42c4..56b1af35c0 100644 --- a/.github/workflows/build-macos.yml +++ b/.github/workflows/build-macos.yml @@ -39,7 +39,7 @@ jobs: compiler: xcode version: "14.2" - - name: macos-13-arm64-xcode-14.2 + - name: macos-13-arm64-xcode-14.2 os: macos-13-xlarge compiler: xcode version: "14.2" @@ -49,7 +49,7 @@ jobs: compiler: xcode version: "15.4" - - name: macos-14-arm64-xcode-15.4 + - name: macos-14-arm64-xcode-15.4 os: macos-14-xlarge compiler: xcode version: "15.4" From 53e78c6b43af0cb478f0e5e87c19b3521202eb7c Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 4 Nov 2024 19:35:50 -0500 Subject: [PATCH 085/115] don't use large or xlarge runners since our payment plan doesn't support those --- .github/workflows/build-macos.yml | 12 ------------ .github/workflows/build-python.yml | 6 ------ 2 files changed, 18 deletions(-) diff --git a/.github/workflows/build-macos.yml b/.github/workflows/build-macos.yml index 56b1af35c0..e519164ec3 100644 --- a/.github/workflows/build-macos.yml +++ b/.github/workflows/build-macos.yml @@ -26,9 +26,7 @@ jobs: # See https://help.github.com/en/articles/workflow-syntax-for-github-actions. name: [ macos-13-xcode-14.2, - macos-13-arm64-xcode-14.2, macos-14-xcode-15.4, - macos-14-arm64-xcode-15.4, ] build_type: [Debug, Release] @@ -39,21 +37,11 @@ jobs: compiler: xcode version: "14.2" - - name: macos-13-arm64-xcode-14.2 - os: macos-13-xlarge - compiler: xcode - version: "14.2" - - name: macos-14-xcode-15.4 os: macos-14 compiler: xcode version: "15.4" - - name: macos-14-arm64-xcode-15.4 - os: macos-14-xlarge - compiler: xcode - version: "15.4" - steps: - name: Checkout uses: actions/checkout@v4 diff --git a/.github/workflows/build-python.yml b/.github/workflows/build-python.yml index 248b766d54..8206ffd3e9 100644 --- a/.github/workflows/build-python.yml +++ b/.github/workflows/build-python.yml @@ -30,7 +30,6 @@ jobs: [ ubuntu-20.04-gcc-9, ubuntu-20.04-clang-9, - macos-14-arm64-xcode-15.4, macos-14-xcode-15.4, windows-2022-msbuild, ] @@ -48,11 +47,6 @@ jobs: compiler: clang version: "9" - - name: macos-14-arm64-xcode-15.4 - os: macos-14-xlarge - compiler: xcode - version: "15.4" - - name: macos-14-xcode-15.4 os: macos-14 compiler: xcode From 16f9d4460dcd47be85400d67c0bdb60e9bb9c6df Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 5 Nov 2024 09:35:12 -0500 Subject: [PATCH 086/115] update macos-12 to macos-13 --- .github/workflows/build-python.yml | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/.github/workflows/build-python.yml b/.github/workflows/build-python.yml index 8206ffd3e9..24a7f6c90e 100644 --- a/.github/workflows/build-python.yml +++ b/.github/workflows/build-python.yml @@ -30,6 +30,7 @@ jobs: [ ubuntu-20.04-gcc-9, ubuntu-20.04-clang-9, + macos-13-xcode-14.2, macos-14-xcode-15.4, windows-2022-msbuild, ] @@ -47,6 +48,11 @@ jobs: compiler: clang version: "9" + - name: macos-13-xcode-14.2 + os: macos-13 + compiler: xcode + version: "14.2" + - name: macos-14-xcode-15.4 os: macos-14 compiler: xcode From be9195465ee40723683b8af3c7699f726e472aa4 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Tue, 5 Nov 2024 15:55:06 -0500 Subject: [PATCH 087/115] 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 e21a36e167..205708c62d 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 2964c01adf..c8b249ddcb 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 80479eb789..a15b73ea19 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 b1655e0a9c..ced3d2ce7c 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 d8340e393c..1eb43cec88 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) From eca2bb5d8a916043608866792c13e7cb86cae10a Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Wed, 30 Oct 2024 10:27:51 -0700 Subject: [PATCH 088/115] epipolarLine --- gtsam/geometry/FundamentalMatrix.cpp | 28 ++++++++++++++++++++++++++++ gtsam/geometry/FundamentalMatrix.h | 7 +++++++ 2 files changed, 35 insertions(+) diff --git a/gtsam/geometry/FundamentalMatrix.cpp b/gtsam/geometry/FundamentalMatrix.cpp index 5e8c26e627..837ba72632 100644 --- a/gtsam/geometry/FundamentalMatrix.cpp +++ b/gtsam/geometry/FundamentalMatrix.cpp @@ -74,6 +74,20 @@ Matrix3 FundamentalMatrix::matrix() const { V_.transpose().matrix(); } +Vector3 FundamentalMatrix::epipolarLine(const Point2& p, + OptionalJacobian<3, 7> H) { + Vector3 point(p.x(), p.y(), 1); // Create a point in homogeneous coordinates + Vector3 line = matrix() * point; // Compute the epipolar line + + if (H) { + // Compute the Jacobian if requested + throw std::runtime_error( + "FundamentalMatrix::epipolarLine: Jacobian not implemented yet."); + } + + return line; // Return the epipolar line +} + void FundamentalMatrix::print(const std::string& s) const { std::cout << s << matrix() << std::endl; } @@ -116,6 +130,20 @@ Matrix3 SimpleFundamentalMatrix::matrix() const { return Ka().transpose().inverse() * E_.matrix() * Kb().inverse(); } +Vector3 SimpleFundamentalMatrix::epipolarLine(const Point2& p, + OptionalJacobian<3, 7> H) { + Vector3 point(p.x(), p.y(), 1); // Create a point in homogeneous coordinates + Vector3 line = matrix() * point; // Compute the epipolar line + + if (H) { + // Compute the Jacobian if requested + throw std::runtime_error( + "SimpleFundamentalMatrix::epipolarLine: Jacobian not implemented yet."); + } + + return line; // Return the epipolar line +} + void SimpleFundamentalMatrix::print(const std::string& s) const { std::cout << s << " E:\n" << E_.matrix() << "\nfa: " << fa_ << "\nfb: " << fb_ diff --git a/gtsam/geometry/FundamentalMatrix.h b/gtsam/geometry/FundamentalMatrix.h index c114c2b5be..df2c2881af 100644 --- a/gtsam/geometry/FundamentalMatrix.h +++ b/gtsam/geometry/FundamentalMatrix.h @@ -7,6 +7,7 @@ #pragma once +#include #include #include #include @@ -86,6 +87,9 @@ class GTSAM_EXPORT FundamentalMatrix { /// Return the fundamental matrix representation Matrix3 matrix() const; + /// Computes the epipolar line in a (left) for a given point in b (right) + Vector3 epipolarLine(const Point2& p, OptionalJacobian<3, 7> H = {}); + /// @name Testable /// @{ /// Print the FundamentalMatrix @@ -161,6 +165,9 @@ class GTSAM_EXPORT SimpleFundamentalMatrix { /// F = Ka^(-T) * E * Kb^(-1) Matrix3 matrix() const; + /// Computes the epipolar line in a (left) for a given point in b (right) + Vector3 epipolarLine(const Point2& p, OptionalJacobian<3, 7> H = {}); + /// @name Testable /// @{ /// Print the SimpleFundamentalMatrix From 45fc039d075868c9b038b5a3d5aaebf4aba40cba Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Wed, 30 Oct 2024 14:54:57 -0700 Subject: [PATCH 089/115] EssentialMatrixFactor5 --- gtsam/slam/EssentialMatrixFactor.h | 147 +++++++++++++++--- .../slam/tests/testEssentialMatrixFactor.cpp | 33 +++- 2 files changed, 150 insertions(+), 30 deletions(-) diff --git a/gtsam/slam/EssentialMatrixFactor.h b/gtsam/slam/EssentialMatrixFactor.h index b19bb09ab6..c8f94681cf 100644 --- a/gtsam/slam/EssentialMatrixFactor.h +++ b/gtsam/slam/EssentialMatrixFactor.h @@ -38,7 +38,6 @@ class EssentialMatrixFactor : public NoiseModelFactorN { typedef EssentialMatrixFactor This; public: - // Provide access to the Matrix& version of evaluateError: using Base::evaluateError; @@ -93,8 +92,8 @@ class EssentialMatrixFactor : public NoiseModelFactorN { } /// vector of errors returns 1D vector - Vector evaluateError( - const EssentialMatrix& E, OptionalMatrixType H) const override { + Vector evaluateError(const EssentialMatrix& E, + OptionalMatrixType H) const override { Vector error(1); error << E.error(vA_, vB_, H); return error; @@ -118,7 +117,6 @@ class EssentialMatrixFactor2 typedef EssentialMatrixFactor2 This; public: - // Provide access to the Matrix& version of evaluateError: using Base::evaluateError; @@ -178,9 +176,9 @@ class EssentialMatrixFactor2 * @param E essential matrix * @param d inverse depth d */ - Vector evaluateError( - const EssentialMatrix& E, const double& d, - OptionalMatrixType DE, OptionalMatrixType Dd) const override { + Vector evaluateError(const EssentialMatrix& E, const double& d, + OptionalMatrixType DE, + OptionalMatrixType Dd) const override { // We have point x,y in image 1 // Given a depth Z, the corresponding 3D point P1 = Z*(x,y,1) = (x,y,1)/d // We then convert to second camera by P2 = 1R2'*(P1-1T2) @@ -241,7 +239,6 @@ class EssentialMatrixFactor3 : public EssentialMatrixFactor2 { Rot3 cRb_; ///< Rotation from body to camera frame public: - // Provide access to the Matrix& version of evaluateError: using Base::evaluateError; @@ -292,9 +289,9 @@ class EssentialMatrixFactor3 : public EssentialMatrixFactor2 { * @param E essential matrix * @param d inverse depth d */ - Vector evaluateError( - const EssentialMatrix& E, const double& d, - OptionalMatrixType DE, OptionalMatrixType Dd) const override { + Vector evaluateError(const EssentialMatrix& E, const double& d, + OptionalMatrixType DE, + OptionalMatrixType Dd) const override { if (!DE) { // Convert E from body to camera frame EssentialMatrix cameraE = cRb_ * E; @@ -304,8 +301,9 @@ class EssentialMatrixFactor3 : public EssentialMatrixFactor2 { // Version with derivatives Matrix D_e_cameraE, D_cameraE_E; // 2*5, 5*5 EssentialMatrix cameraE = E.rotate(cRb_, D_cameraE_E); - // Using the pointer version of evaluateError since the Base class (EssentialMatrixFactor2) - // does not have the matrix reference version of evaluateError + // Using the pointer version of evaluateError since the Base class + // (EssentialMatrixFactor2) does not have the matrix reference version of + // evaluateError Vector e = Base::evaluateError(cameraE, d, &D_e_cameraE, Dd); *DE = D_e_cameraE * D_cameraE_E; // (2*5) * (5*5) return e; @@ -327,7 +325,7 @@ class EssentialMatrixFactor3 : public EssentialMatrixFactor2 { * Even with a prior, we can only optimize 2 DoF in the calibration. So the * prior should have a noise model with very low sigma in the remaining * dimensions. This has been tested to work on Cal3_S2. With Cal3Bundler, it - * works only with a strong prior (low sigma noisemodel) on all degrees of + * works only with a strong prior (low sigma noise model) on all degrees of * freedom. */ template @@ -343,13 +341,12 @@ class EssentialMatrixFactor4 typedef Eigen::Matrix JacobianCalibration; public: - - // Provide access to the Matrix& version of evaluateError: + // Provide access to the Matrix& version of evaluateError: using Base::evaluateError; /** * Constructor - * @param keyE Essential Matrix (from camera B to A) variable key + * @param keyE Essential Matrix aEb variable key * @param keyK Calibration variable key (common for both cameras) * @param pA point in first camera, in pixel coordinates * @param pB point in second camera, in pixel coordinates @@ -385,32 +382,32 @@ class EssentialMatrixFactor4 * @param H2 optional jacobian of error w.r.t K * @return * Vector 1D vector of algebraic error */ - Vector evaluateError( - const EssentialMatrix& E, const CALIBRATION& K, - OptionalMatrixType H1, OptionalMatrixType H2) const override { + Vector evaluateError(const EssentialMatrix& E, const CALIBRATION& K, + OptionalMatrixType HE, + OptionalMatrixType HK) const override { // converting from pixel coordinates to normalized coordinates cA and cB JacobianCalibration cA_H_K; // dcA/dK JacobianCalibration cB_H_K; // dcB/dK - Point2 cA = K.calibrate(pA_, H2 ? &cA_H_K : 0, OptionalNone); - Point2 cB = K.calibrate(pB_, H2 ? &cB_H_K : 0, OptionalNone); + Point2 cA = K.calibrate(pA_, HK ? &cA_H_K : 0, OptionalNone); + Point2 cB = K.calibrate(pB_, HK ? &cB_H_K : 0, OptionalNone); // convert to homogeneous coordinates Vector3 vA = EssentialMatrix::Homogeneous(cA); Vector3 vB = EssentialMatrix::Homogeneous(cB); - if (H2) { + if (HK) { // compute the jacobian of error w.r.t K // error function f = vA.T * E * vB // H2 = df/dK = vB.T * E.T * dvA/dK + vA.T * E * dvB/dK // where dvA/dK = dvA/dcA * dcA/dK, dVB/dK = dvB/dcB * dcB/dK // and dvA/dcA = dvB/dcB = [[1, 0], [0, 1], [0, 0]] - *H2 = vB.transpose() * E.matrix().transpose().leftCols<2>() * cA_H_K + + *HK = vB.transpose() * E.matrix().transpose().leftCols<2>() * cA_H_K + vA.transpose() * E.matrix().leftCols<2>() * cB_H_K; } Vector error(1); - error << E.error(vA, vB, H1); + error << E.error(vA, vB, HE); return error; } @@ -420,4 +417,104 @@ class EssentialMatrixFactor4 }; // EssentialMatrixFactor4 +/** + * Binary factor that optimizes for E and two calibrations Ka and Kb using the + * algebraic epipolar error (Ka^-1 pA)'E (Kb^-1 pB). The calibrations are + * assumed different for the two images. Don'tt use this factor with same + * calibration unknown, as Jacobians will be incorrect... + * + * Note: even stronger caveats about having priors on calibration apply. + */ +template +class EssentialMatrixFactor5 + : public NoiseModelFactorN { + private: + Point2 pA_, pB_; ///< points in pixel coordinates + + typedef NoiseModelFactorN Base; + typedef EssentialMatrixFactor5 This; + + static constexpr int DimK = FixedDimension::value; + typedef Eigen::Matrix JacobianCalibration; + + public: + // Provide access to the Matrix& version of evaluateError: + using Base::evaluateError; + + /** + * Constructor + * @param keyE Essential Matrix aEb variable key + * @param keyKa Calibration variable key for camera A + * @param keyKb Calibration variable key for camera B + * @param pA point in first camera, in pixel coordinates + * @param pB point in second camera, in pixel coordinates + * @param model noise model is about dot product in ideal, homogeneous + * coordinates + */ + EssentialMatrixFactor5(Key keyE, Key keyKa, Key keyKb, const Point2& pA, + const Point2& pB, const SharedNoiseModel& model) + : Base(model, keyE, keyKa, keyKb), pA_(pA), pB_(pB) {} + + /// @return a deep copy of this factor + gtsam::NonlinearFactor::shared_ptr clone() const override { + return std::static_pointer_cast( + gtsam::NonlinearFactor::shared_ptr(new This(*this))); + } + + /// print + void print( + const std::string& s = "", + const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override { + Base::print(s); + std::cout << " EssentialMatrixFactor5 with measurements\n (" + << pA_.transpose() << ")' and (" << pB_.transpose() << ")'" + << std::endl; + } + + /** + * @brief Calculate the algebraic epipolar error pA' (Ka^-1)' E Kb pB. + * + * @param E essential matrix for key keyE + * @param Ka calibration for camera A for key keyKa + * @param Kb calibration for camera B for key keyKb + * @param H1 optional jacobian of error w.r.t E + * @param H2 optional jacobian of error w.r.t Ka + * @param H3 optional jacobian of error w.r.t Kb + * @return * Vector 1D vector of algebraic error + */ + Vector evaluateError(const EssentialMatrix& E, const CALIBRATION& Ka, + const CALIBRATION& Kb, OptionalMatrixType HE, + OptionalMatrixType HKa, + OptionalMatrixType HKb) const override { + // converting from pixel coordinates to normalized coordinates cA and cB + JacobianCalibration cA_H_Ka; // dcA/dKa + JacobianCalibration cB_H_Kb; // dcB/dKb + Point2 cA = Ka.calibrate(pA_, HKa ? &cA_H_Ka : 0, OptionalNone); + Point2 cB = Kb.calibrate(pB_, HKb ? &cB_H_Kb : 0, OptionalNone); + + // convert to homogeneous coordinates + Vector3 vA = EssentialMatrix::Homogeneous(cA); + Vector3 vB = EssentialMatrix::Homogeneous(cB); + + if (HKa) { + // compute the jacobian of error w.r.t Ka + *HKa = vB.transpose() * E.matrix().transpose().leftCols<2>() * cA_H_Ka; + } + + if (HKb) { + // compute the jacobian of error w.r.t Kb + *HKb = vA.transpose() * E.matrix().leftCols<2>() * cB_H_Kb; + } + + Vector error(1); + error << E.error(vA, vB, HE); + + return error; + } + + public: + GTSAM_MAKE_ALIGNED_OPERATOR_NEW +}; +// EssentialMatrixFactor5 + } // namespace gtsam diff --git a/gtsam/slam/tests/testEssentialMatrixFactor.cpp b/gtsam/slam/tests/testEssentialMatrixFactor.cpp index 0f9c2ef9f8..609f381871 100644 --- a/gtsam/slam/tests/testEssentialMatrixFactor.cpp +++ b/gtsam/slam/tests/testEssentialMatrixFactor.cpp @@ -14,8 +14,8 @@ #include #include #include -#include #include +#include #include using namespace std::placeholders; @@ -101,7 +101,8 @@ TEST(EssentialMatrixFactor, ExpressionFactor) { Key key(1); for (size_t i = 0; i < 5; i++) { std::function)> f = - std::bind(&EssentialMatrix::error, std::placeholders::_1, vA(i), vB(i), std::placeholders::_2); + std::bind(&EssentialMatrix::error, std::placeholders::_1, vA(i), vB(i), + std::placeholders::_2); Expression E_(key); // leaf expression Expression expr(f, E_); // unary expression @@ -127,10 +128,11 @@ TEST(EssentialMatrixFactor, ExpressionFactorRotationOnly) { Key key(1); for (size_t i = 0; i < 5; i++) { std::function)> f = - std::bind(&EssentialMatrix::error, std::placeholders::_1, vA(i), vB(i), std::placeholders::_2); + std::bind(&EssentialMatrix::error, std::placeholders::_1, vA(i), vB(i), + std::placeholders::_2); std::function, - OptionalJacobian<5, 2>)> + OptionalJacobian<5, 3>, + OptionalJacobian<5, 2>)> g; Expression R_(key); Expression d_(trueDirection); @@ -529,6 +531,27 @@ TEST(EssentialMatrixFactor4, minimizationWithStrongCal3BundlerPrior) { 1e-6); } +//************************************************************************* +TEST(EssentialMatrixFactor5, factor) { + Key keyE(1), keyKa(2), keyKb(3); + for (size_t i = 0; i < 5; i++) { + EssentialMatrixFactor5 factor(keyE, keyKa, keyKb, pA(i), pB(i), + model1); + + // Check evaluation + Vector1 expected; + expected << 0; + Vector actual = factor.evaluateError(trueE, trueK, trueK); + EXPECT(assert_equal(expected, actual, 1e-7)); + + Values truth; + truth.insert(keyE, trueE); + truth.insert(keyKa, trueK); + truth.insert(keyKb, trueK); + EXPECT_CORRECT_FACTOR_JACOBIANS(factor, truth, 1e-6, 1e-7); + } +} + } // namespace example1 //************************************************************************* From f8d00f82f16e16110b5d0884b56550f24cd35c74 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Wed, 30 Oct 2024 16:06:56 -0700 Subject: [PATCH 090/115] Allow for global calibration --- gtsam/sfm/TransferFactor.h | 2 ++ 1 file changed, 2 insertions(+) diff --git a/gtsam/sfm/TransferFactor.h b/gtsam/sfm/TransferFactor.h index c8b249ddcb..b73a71aa4f 100644 --- a/gtsam/sfm/TransferFactor.h +++ b/gtsam/sfm/TransferFactor.h @@ -236,6 +236,8 @@ class EssentialTransferFactorK /** * @brief Constructor that accepts a vector of point triplets. * + * @note Calibrations are assumed all different, keys are derived from edges. + * * @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) From ce196d962fbd62283cda8295cbc93f7cb6722657 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Wed, 30 Oct 2024 16:07:22 -0700 Subject: [PATCH 091/115] Wrap EMFs --- gtsam/slam/EssentialMatrixFactor.h | 4 +-- gtsam/slam/slam.i | 50 +++++++++++++++++++++++++++--- 2 files changed, 47 insertions(+), 7 deletions(-) diff --git a/gtsam/slam/EssentialMatrixFactor.h b/gtsam/slam/EssentialMatrixFactor.h index c8f94681cf..7a0b469064 100644 --- a/gtsam/slam/EssentialMatrixFactor.h +++ b/gtsam/slam/EssentialMatrixFactor.h @@ -354,7 +354,7 @@ class EssentialMatrixFactor4 * coordinates */ EssentialMatrixFactor4(Key keyE, Key keyK, const Point2& pA, const Point2& pB, - const SharedNoiseModel& model) + const SharedNoiseModel& model = nullptr) : Base(model, keyE, keyK), pA_(pA), pB_(pB) {} /// @return a deep copy of this factor @@ -452,7 +452,7 @@ class EssentialMatrixFactor5 * coordinates */ EssentialMatrixFactor5(Key keyE, Key keyKa, Key keyKb, const Point2& pA, - const Point2& pB, const SharedNoiseModel& model) + const Point2& pB, const SharedNoiseModel& model = nullptr) : Base(model, keyE, keyKa, keyKb), pA_(pA), pB_(pB) {} /// @return a deep copy of this factor diff --git a/gtsam/slam/slam.i b/gtsam/slam/slam.i index 97dcfcae7b..a226f06ec6 100644 --- a/gtsam/slam/slam.i +++ b/gtsam/slam/slam.i @@ -107,7 +107,7 @@ typedef gtsam::GeneralSFMFactor, gtsam::Point3> GeneralSFMFactorPoseCal3Unified; -template virtual class GeneralSFMFactor2 : gtsam::NoiseModelFactor { GeneralSFMFactor2(const gtsam::Point2& measured, @@ -221,15 +221,55 @@ typedef gtsam::PoseRotationPrior PoseRotationPrior3D; #include virtual class EssentialMatrixFactor : gtsam::NoiseModelFactor { - EssentialMatrixFactor(size_t key, const gtsam::Point2& pA, - const gtsam::Point2& pB, - const gtsam::noiseModel::Base* noiseModel); + EssentialMatrixFactor(size_t key, + const gtsam::Point2& pA, const gtsam::Point2& pB, + const gtsam::noiseModel::Base* model); void print(string s = "", const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter) const; - bool equals(const gtsam::EssentialMatrixFactor& other, double tol) const; gtsam::Vector evaluateError(const gtsam::EssentialMatrix& E) const; }; +virtual class EssentialMatrixFactor2 : gtsam::NoiseModelFactor { + EssentialMatrixFactor2(size_t key1, size_t key2, + const gtsam::Point2& pA, const gtsam::Point2& pB, + const gtsam::noiseModel::Base* model); + void print(string s = "", const gtsam::KeyFormatter& keyFormatter = + gtsam::DefaultKeyFormatter) const; + gtsam::Vector evaluateError(const gtsam::EssentialMatrix& E, double d) const; +}; + +virtual class EssentialMatrixFactor3 : gtsam::EssentialMatrixFactor2 { + EssentialMatrixFactor3(size_t key1, size_t key2, + const gtsam::Point2& pA, const gtsam::Point2& pB, + const gtsam::Rot3& cRb, const gtsam::noiseModel::Base* model); + void print(string s = "", const gtsam::KeyFormatter& keyFormatter = + gtsam::DefaultKeyFormatter) const; + gtsam::Vector evaluateError(const gtsam::EssentialMatrix& E, double d) const; +}; + +template +virtual class EssentialMatrixFactor4 : gtsam::NoiseModelFactor { + EssentialMatrixFactor4(size_t keyE, size_t keyK, + const gtsam::Point2& pA, const gtsam::Point2& pB, + const gtsam::noiseModel::Base* model = nullptr); + void print(string s = "", const gtsam::KeyFormatter& keyFormatter = + gtsam::DefaultKeyFormatter) const; + gtsam::Vector evaluateError(const gtsam::EssentialMatrix& E, const CALIBRATION& K) const; +}; + +template +virtual class EssentialMatrixFactor5 : gtsam::NoiseModelFactor { + EssentialMatrixFactor5(size_t keyE, size_t keyKa, size_t keyKb, + const gtsam::Point2& pA, const gtsam::Point2& pB, + const gtsam::noiseModel::Base* model = nullptr); + void print(string s = "", const gtsam::KeyFormatter& keyFormatter = + gtsam::DefaultKeyFormatter) const; + gtsam::Vector evaluateError(const gtsam::EssentialMatrix& E, + const CALIBRATION& Ka, const CALIBRATION& Kb) const; +}; + #include virtual class EssentialMatrixConstraint : gtsam::NoiseModelFactor { EssentialMatrixConstraint(size_t key1, size_t key2, const gtsam::EssentialMatrix &measuredE, From 360dc4138c9d51f2683d8bc8af51eb692b76dc7e Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Wed, 30 Oct 2024 16:07:40 -0700 Subject: [PATCH 092/115] Compare 3 more cases --- python/gtsam/examples/ViewGraphComparison.py | 112 ++++++++++++------- 1 file changed, 69 insertions(+), 43 deletions(-) diff --git a/python/gtsam/examples/ViewGraphComparison.py b/python/gtsam/examples/ViewGraphComparison.py index 1eb43cec88..37558a46c7 100644 --- a/python/gtsam/examples/ViewGraphComparison.py +++ b/python/gtsam/examples/ViewGraphComparison.py @@ -32,7 +32,7 @@ K = gtsam.symbol_shorthand.K # Methods to compare -methods = ["SimpleF", "Fundamental", "Essential+Ks", "Calibrated"] +methods = ["SimpleF", "Fundamental", "Essential+Ks", "Essential+K", "Calibrated", "Binary+Ks", "Binary+K"] # Formatter function for printing keys @@ -76,8 +76,8 @@ def simulate_data(points, poses, cal, rng, noise_std): return measurements -# Function to compute ground truth matrices def compute_ground_truth(method, poses, cal): + """Function to compute ground truth edge variables.""" E1 = EssentialMatrix.FromPose3(poses[0].between(poses[1])) E2 = EssentialMatrix.FromPose3(poses[0].between(poses[2])) F1 = FundamentalMatrix(cal.K(), E1, cal.K()) @@ -90,58 +90,80 @@ 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 == "Essential+Ks" or method == "Calibrated": - return E1, E2 else: - raise ValueError(f"Unknown method {method}") + return E1, E2 def build_factor_graph(method, num_cameras, measurements, cal): """build the factor graph""" graph = NonlinearFactorGraph() + # Determine the FactorClass based on the method if method == "Fundamental": FactorClass = gtsam.TransferFactorFundamentalMatrix elif method == "SimpleF": FactorClass = gtsam.TransferFactorSimpleFundamentalMatrix - elif method == "Essential+Ks": + elif method in ["Essential+Ks", "Essential+K"]: 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 == "Binary+K": + FactorClass = gtsam.EssentialMatrixFactor4Cal3f + elif method == "Binary+Ks": + FactorClass = gtsam.EssentialMatrixFactor5Cal3f elif method == "Calibrated": FactorClass = gtsam.EssentialTransferFactorCal3f - # No priors on calibration needed else: raise ValueError(f"Unknown method {method}") + # Add priors on calibrations if necessary + if method in ["Essential+Ks", "Binary+Ks"]: + for i in range(num_cameras): + model = gtsam.noiseModel.Isotropic.Sigma(1, 10.0) + graph.addPriorCal3f(K(i), cal, model) + elif method in ["Essential+K", "Binary+K"]: + model = gtsam.noiseModel.Isotropic.Sigma(1, 10.0) + graph.addPriorCal3f(K(0), cal, model) + z = measurements # shorthand 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((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. - 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)) + if method in ["Binary+Ks", "Binary+K"]: + # Add binary Essential Matrix factors + ab, ac = EdgeKey(a, b).key(), EdgeKey(a, c).key() + for j in range(len(measurements[0])): + if method == "Binary+Ks": + graph.add(FactorClass(ab, K(a), K(b), z[a][j], z[b][j])) + graph.add(FactorClass(ac, K(a), K(c), z[a][j], z[c][j])) + else: # Binary+K + graph.add(FactorClass(ab, K(0), z[a][j], z[b][j])) + graph.add(FactorClass(ac, K(0), z[a][j], z[c][j])) 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)) + # Add transfer factors between views a, b, and c + + # Vectors to collect tuples for each factor + tuples1 = [] + tuples2 = [] + tuples3 = [] + + for j in range(len(measurements[0])): + 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. + 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)) + elif method == "Essential+K": + graph.add(FactorClass(EdgeKey(a, c), EdgeKey(b, c), K(0), tuples1)) + graph.add(FactorClass(EdgeKey(a, b), EdgeKey(b, c), K(0), tuples2)) + graph.add(FactorClass(EdgeKey(a, c), EdgeKey(a, b), K(0), tuples3)) + 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 @@ -159,22 +181,23 @@ 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 ["Essential+Ks", "Calibrated"]: + elif method in ["Essential+Ks", "Essential+K", "Binary+Ks", "Binary+K", "Calibrated"]: 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 + b = (a + 1) % num_cameras + c = (a + 2) % num_cameras 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 == "Essential+Ks": - # Insert initial calibrations + # Insert initial calibrations + if method in ["Essential+Ks", "Binary+Ks"]: for i in range(num_cameras): initialEstimate.insert(K(i), cal) total_dimension += cal.dim() + elif method in ["Essential+K", "Binary+K"]: + initialEstimate.insert(K(0), cal) + total_dimension += cal.dim() print(f"Total dimension of the problem: {total_dimension}") return initialEstimate @@ -205,7 +228,7 @@ 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 ["Essential+Ks", "Calibrated"]: + if method in ["Essential+Ks", "Essential+K", "Binary+Ks", "Binary+K", "Calibrated"]: E_est_ab = result.atEssentialMatrix(key_ab) E_est_ac = result.atEssentialMatrix(key_ac) @@ -218,15 +241,18 @@ def compute_distances(method, result, ground_truth, num_cameras, cal): SF_est_ac = result.atSimpleFundamentalMatrix(key_ac).matrix() F_est_ab = FundamentalMatrix(SF_est_ab) F_est_ac = FundamentalMatrix(SF_est_ac) - elif method == "Essential+Ks": - # Retrieve calibrations from result: + elif method in ["Essential+Ks", "Binary+Ks"]: + # Retrieve calibrations from result for each camera 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 = 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 in ["Essential+K", "Binary+K"]: + # Use single shared calibration + cal_shared = result.atCal3f(K(0)) + F_est_ab = FundamentalMatrix(cal_shared.K(), E_est_ab, cal_shared.K()) + F_est_ac = FundamentalMatrix(cal_shared.K(), E_est_ac, cal_shared.K()) elif method == "Calibrated": # Use ground truth calibration F_est_ab = FundamentalMatrix(cal.K(), E_est_ab, cal.K()) From 183774556851cb098e58c8ab38b180b863d298ba Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Tue, 5 Nov 2024 15:06:25 -0500 Subject: [PATCH 093/115] Trying to optimize well --- python/gtsam/examples/ViewGraphComparison.py | 13 +++++++++---- 1 file changed, 9 insertions(+), 4 deletions(-) diff --git a/python/gtsam/examples/ViewGraphComparison.py b/python/gtsam/examples/ViewGraphComparison.py index 37558a46c7..7a9f77d098 100644 --- a/python/gtsam/examples/ViewGraphComparison.py +++ b/python/gtsam/examples/ViewGraphComparison.py @@ -117,10 +117,10 @@ def build_factor_graph(method, num_cameras, measurements, cal): # Add priors on calibrations if necessary if method in ["Essential+Ks", "Binary+Ks"]: for i in range(num_cameras): - model = gtsam.noiseModel.Isotropic.Sigma(1, 10.0) + model = gtsam.noiseModel.Isotropic.Sigma(1, 1000.0) graph.addPriorCal3f(K(i), cal, model) elif method in ["Essential+K", "Binary+K"]: - model = gtsam.noiseModel.Isotropic.Sigma(1, 10.0) + model = gtsam.noiseModel.Isotropic.Sigma(1, 1000.0) graph.addPriorCal3f(K(0), cal, model) z = measurements # shorthand @@ -186,6 +186,8 @@ def get_initial_estimate(method, num_cameras, ground_truth, cal): for a in range(num_cameras): b = (a + 1) % num_cameras c = (a + 2) % num_cameras + # initialEstimate.insert(EdgeKey(a, b).key(), E1.retract(0.1 * np.ones((5, 1)))) + # initialEstimate.insert(EdgeKey(a, c).key(), E2.retract(0.1 * np.ones((5, 1)))) initialEstimate.insert(EdgeKey(a, b).key(), E1) initialEstimate.insert(EdgeKey(a, c).key(), E2) total_dimension += E1.dim() + E2.dim() @@ -206,8 +208,9 @@ def get_initial_estimate(method, num_cameras, ground_truth, cal): def optimize(graph, initialEstimate, method): """optimize the graph""" params = LevenbergMarquardtParams() - params.setlambdaInitial(1e10) # Initialize lambda to a high value - params.setlambdaUpperBound(1e10) + if method not in ["Calibrated", "Binary+K", "Binary+Ks"]: + params.setlambdaInitial(1e10) # Initialize lambda to a high value + params.setlambdaUpperBound(1e10) # params.setAbsoluteErrorTol(0.1) params.setVerbosityLM("SUMMARY") optimizer = LevenbergMarquardtOptimizer(graph, initialEstimate, params) @@ -373,6 +376,8 @@ def main(): # Compute final error final_error = graph.error(result) + if method in ["Binary+K", "Binary+Ks"]: + final_error *= cal.f() * cal.f() # Store results results[method]["distances"].extend(distances) From 5f667b44255b0e6181b620833ba8097f596cd0b4 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Tue, 5 Nov 2024 17:24:13 -0500 Subject: [PATCH 094/115] Rename factor --- gtsam/sfm/TransferFactor.h | 18 ++++++++++++++++++ gtsam/sfm/sfm.i | 7 +++++-- gtsam/sfm/tests/testTransferFactor.cpp | 2 +- 3 files changed, 24 insertions(+), 3 deletions(-) diff --git a/gtsam/sfm/TransferFactor.h b/gtsam/sfm/TransferFactor.h index b73a71aa4f..0338762be2 100644 --- a/gtsam/sfm/TransferFactor.h +++ b/gtsam/sfm/TransferFactor.h @@ -253,6 +253,24 @@ class EssentialTransferFactorK TransferEdges(edge1, edge2), triplets_(triplets) {} + /** + * @brief Constructor that accepts a vector of point triplets. + * + * @note Calibrations are assumed all same, using given key `keyK`. + * + * @param edge1 First EdgeKey specifying E1: (a, c) or (c, a) + * @param edge2 Second EdgeKey specifying E2: (b, c) or (c, b) + * @param keyK Calibration key for all views. + * @param triplets A vector of triplets containing (pa, pb, pc) + * @param model An optional SharedNoiseModel + */ + EssentialTransferFactorK(EdgeKey edge1, EdgeKey edge2, Key keyK, + const std::vector& triplets, + const SharedNoiseModel& model = nullptr) + : Base(model, edge1, edge2, keyK, keyK, keyK), + 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, diff --git a/gtsam/sfm/sfm.i b/gtsam/sfm/sfm.i index a15b73ea19..24f24f1bb4 100644 --- a/gtsam/sfm/sfm.i +++ b/gtsam/sfm/sfm.i @@ -98,8 +98,11 @@ virtual class EssentialTransferFactor : gtsam::NoiseModelFactor { template virtual class EssentialTransferFactorK : gtsam::NoiseModelFactor { EssentialTransferFactorK(gtsam::EdgeKey edge1, gtsam::EdgeKey edge2, - const std::vector>& triplets, - const gtsam::noiseModel::Base* model = nullptr); + const std::vector>& triplets, + const gtsam::noiseModel::Base* model = nullptr); + EssentialTransferFactorK(gtsam::EdgeKey edge1, gtsam::EdgeKey edge2, size_t keyK, + 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 ced3d2ce7c..b34bdf8327 100644 --- a/gtsam/sfm/tests/testTransferFactor.cpp +++ b/gtsam/sfm/tests/testTransferFactor.cpp @@ -154,7 +154,7 @@ TEST(TransferFactor, MultipleTuples) { } //************************************************************************* -// Test for EssentialTransferFactor +// Test for EssentialTransferFactorK TEST(EssentialTransferFactor, Jacobians) { using namespace fixture; From 8b968c14018caab9daa5838a03d0c57829c6eb21 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Wed, 6 Nov 2024 09:08:16 -0500 Subject: [PATCH 095/115] use visitWith to not create a new tree --- gtsam/discrete/DecisionTreeFactor.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/discrete/DecisionTreeFactor.cpp b/gtsam/discrete/DecisionTreeFactor.cpp index 45574f6414..4e7a7342ef 100644 --- a/gtsam/discrete/DecisionTreeFactor.cpp +++ b/gtsam/discrete/DecisionTreeFactor.cpp @@ -239,7 +239,7 @@ namespace gtsam { }; // Go through the tree - this->apply(op); + this->visitWith(op); return probs; } From 9535ae2fc974a61fe52a0312439c64b6e71cfdad Mon Sep 17 00:00:00 2001 From: Jose Luis Blanco-Claraco Date: Wed, 6 Nov 2024 09:51:06 +0100 Subject: [PATCH 096/115] Add missing #if for clang --- gtsam/linear/VectorValues.cpp | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/gtsam/linear/VectorValues.cpp b/gtsam/linear/VectorValues.cpp index 7440c0b2b0..52e2fac227 100644 --- a/gtsam/linear/VectorValues.cpp +++ b/gtsam/linear/VectorValues.cpp @@ -22,8 +22,10 @@ #include // assert_throw needs a semicolon in Release mode. +#if defined(__clang__) #pragma clang diagnostic push #pragma clang diagnostic ignored "-Wextra-semi-stmt" +#endif namespace gtsam { @@ -417,5 +419,6 @@ namespace gtsam { } // \namespace gtsam +#if defined(__clang__) #pragma clang diagnostic pop - +#endif From d21f191219a945c91546e8d77bb0badc3f877446 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Wed, 6 Nov 2024 15:06:46 -0500 Subject: [PATCH 097/115] use a fixed size min-heap to find the pruning threshold --- gtsam/discrete/DecisionTreeFactor.cpp | 57 ++++++++++++++++++++++----- 1 file changed, 48 insertions(+), 9 deletions(-) diff --git a/gtsam/discrete/DecisionTreeFactor.cpp b/gtsam/discrete/DecisionTreeFactor.cpp index 4e7a7342ef..9b541bbf02 100644 --- a/gtsam/discrete/DecisionTreeFactor.cpp +++ b/gtsam/discrete/DecisionTreeFactor.cpp @@ -353,18 +353,57 @@ namespace gtsam { DecisionTreeFactor DecisionTreeFactor::prune(size_t maxNrAssignments) const { const size_t N = maxNrAssignments; - // Get the probabilities in the decision tree so we can threshold. - std::vector probabilities = this->probabilities(); + // Set of all keys + std::set allKeys(keys().begin(), keys().end()); + std::vector min_heap; - // The number of probabilities can be lower than max_leaves - if (probabilities.size() <= N) { - return *this; - } + auto op = [&](const Assignment& a, double p) { + // Get all the keys in the current assignment + std::set assignment_keys; + for (auto&& [k, _] : a) { + assignment_keys.insert(k); + } - std::sort(probabilities.begin(), probabilities.end(), - std::greater{}); + // Find the keys missing in the assignment + std::vector diff; + std::set_difference(allKeys.begin(), allKeys.end(), + assignment_keys.begin(), assignment_keys.end(), + std::back_inserter(diff)); + + // Compute the total number of assignments in the (pruned) subtree + size_t nrAssignments = 1; + for (auto&& k : diff) { + nrAssignments *= cardinalities_.at(k); + } + + if (min_heap.empty()) { + for (size_t i = 0; i < std::min(nrAssignments, N); ++i) { + min_heap.push_back(p); + } + std::make_heap(min_heap.begin(), min_heap.end(), + std::greater{}); + + } else { + // If p is larger than the smallest element, + // then we insert into the max heap. + if (p > min_heap.at(0)) { + for (size_t i = 0; i < std::min(nrAssignments, N); ++i) { + if (min_heap.size() == N) { + std::pop_heap(min_heap.begin(), min_heap.end(), + std::greater{}); + min_heap.pop_back(); + } + min_heap.push_back(p); + std::make_heap(min_heap.begin(), min_heap.end(), + std::greater{}); + } + } + } + return p; + }; + this->visitWith(op); - double threshold = probabilities[N - 1]; + double threshold = min_heap.at(0); // Now threshold the decision tree size_t total = 0; From 9666725473c9dfbf7b16f3447c400306ace9f783 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Wed, 6 Nov 2024 16:37:04 -0500 Subject: [PATCH 098/115] implement a min-heap to record the top N probabilities for pruning --- gtsam/discrete/DecisionTreeFactor.cpp | 74 ++++++++++++++++++++++----- 1 file changed, 60 insertions(+), 14 deletions(-) diff --git a/gtsam/discrete/DecisionTreeFactor.cpp b/gtsam/discrete/DecisionTreeFactor.cpp index 9b541bbf02..c8efc5fa5a 100644 --- a/gtsam/discrete/DecisionTreeFactor.cpp +++ b/gtsam/discrete/DecisionTreeFactor.cpp @@ -349,13 +349,67 @@ namespace gtsam { : DiscreteFactor(keys.indices(), keys.cardinalities()), AlgebraicDecisionTree(keys, table) {} + /** + * @brief Min-Heap class to help with pruning. + * The `top` element is always the smallest value. + */ + class MinHeap { + std::vector v_; + + public: + /// Default constructor + MinHeap() {} + + /// Push value onto the heap + void push(double x) { + v_.push_back(x); + std::make_heap(v_.begin(), v_.end(), std::greater{}); + } + + /// Push value `x`, `n` number of times. + void push(double x, size_t n) { + v_.insert(v_.end(), n, x); + std::make_heap(v_.begin(), v_.end(), std::greater{}); + } + + /// Pop the top value of the heap. + double pop() { + std::pop_heap(v_.begin(), v_.end(), std::greater{}); + double x = v_.back(); + v_.pop_back(); + return x; + } + + /// Return the top value of the heap without popping it. + double top() { return v_.at(0); } + + /** + * @brief Print the heap as a sequence. + * + * @param s A string to prologue the output. + */ + void print(const std::string& s = "") { + std::cout << (s.empty() ? "" : s + " "); + for (size_t i = 0; i < v_.size() - 1; i++) { + std::cout << v_.at(i) << ","; + } + std::cout << v_.at(v_.size() - 1) << std::endl; + } + + /// Return true if heap is empty. + bool empty() const { return v_.empty(); } + + /// Return the size of the heap. + size_t size() const { return v_.size(); } + }; + /* ************************************************************************ */ DecisionTreeFactor DecisionTreeFactor::prune(size_t maxNrAssignments) const { const size_t N = maxNrAssignments; // Set of all keys std::set allKeys(keys().begin(), keys().end()); - std::vector min_heap; + MinHeap min_heap; auto op = [&](const Assignment& a, double p) { // Get all the keys in the current assignment @@ -377,25 +431,17 @@ namespace gtsam { } if (min_heap.empty()) { - for (size_t i = 0; i < std::min(nrAssignments, N); ++i) { - min_heap.push_back(p); - } - std::make_heap(min_heap.begin(), min_heap.end(), - std::greater{}); + min_heap.push(p, std::min(nrAssignments, N)); } else { // If p is larger than the smallest element, // then we insert into the max heap. - if (p > min_heap.at(0)) { + if (p > min_heap.top()) { for (size_t i = 0; i < std::min(nrAssignments, N); ++i) { if (min_heap.size() == N) { - std::pop_heap(min_heap.begin(), min_heap.end(), - std::greater{}); - min_heap.pop_back(); + min_heap.pop(); } - min_heap.push_back(p); - std::make_heap(min_heap.begin(), min_heap.end(), - std::greater{}); + min_heap.push(p); } } } @@ -403,7 +449,7 @@ namespace gtsam { }; this->visitWith(op); - double threshold = min_heap.at(0); + double threshold = min_heap.top(); // Now threshold the decision tree size_t total = 0; From ae43b2ade7a6f88332146aca59a27a17f8eb17b8 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Wed, 6 Nov 2024 19:23:26 -0500 Subject: [PATCH 099/115] make MinHeap more efficient by calling push_heap instead of make_heap --- gtsam/discrete/DecisionTreeFactor.cpp | 15 +++++++++------ 1 file changed, 9 insertions(+), 6 deletions(-) diff --git a/gtsam/discrete/DecisionTreeFactor.cpp b/gtsam/discrete/DecisionTreeFactor.cpp index c8efc5fa5a..caedab713c 100644 --- a/gtsam/discrete/DecisionTreeFactor.cpp +++ b/gtsam/discrete/DecisionTreeFactor.cpp @@ -363,13 +363,15 @@ namespace gtsam { /// Push value onto the heap void push(double x) { v_.push_back(x); - std::make_heap(v_.begin(), v_.end(), std::greater{}); + std::push_heap(v_.begin(), v_.end(), std::greater{}); } /// Push value `x`, `n` number of times. void push(double x, size_t n) { - v_.insert(v_.end(), n, x); - std::make_heap(v_.begin(), v_.end(), std::greater{}); + for (size_t i = 0; i < n; ++i) { + v_.push_back(x); + std::push_heap(v_.begin(), v_.end(), std::greater{}); + } } /// Pop the top value of the heap. @@ -390,10 +392,11 @@ namespace gtsam { */ void print(const std::string& s = "") { std::cout << (s.empty() ? "" : s + " "); - for (size_t i = 0; i < v_.size() - 1; i++) { - std::cout << v_.at(i) << ","; + for (size_t i = 0; i < v_.size(); i++) { + std::cout << v_.at(i); + if (v_.size() > 1 && i < v_.size() - 1) std::cout << ", "; } - std::cout << v_.at(v_.size() - 1) << std::endl; + std::cout << std::endl; } /// Return true if heap is empty. From c4cf82668cdcdb20d0059b4f5753da89d8279dc7 Mon Sep 17 00:00:00 2001 From: Jose Luis Blanco-Claraco Date: Fri, 8 Nov 2024 00:01:30 +0100 Subject: [PATCH 100/115] Bump min required cmake version to 3.9 In the top-level cmake file. It was 3.5 and recent cmake versions complain about it becoming deprecate. Also, the wrap directory already required 3.9 anyhow... --- CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 617accc2ea..e3b462eec5 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,4 +1,4 @@ -cmake_minimum_required(VERSION 3.5) +cmake_minimum_required(VERSION 3.9) if (POLICY CMP0082) cmake_policy(SET CMP0082 NEW) # install from sub-directories immediately endif() From 733e919570159e18cc49f524598ff6a1a2ed72ef Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?S=C3=A9bastien=20Gonzalve?= Date: Sat, 9 Nov 2024 20:20:37 +0100 Subject: [PATCH 101/115] Make operator-> const Having this operator non-const is awkward as operator* is const. This sometimes leads to writing: (*obj).data; instead of obj->data; which is unexpected.... --- gtsam/nonlinear/Values.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/nonlinear/Values.h b/gtsam/nonlinear/Values.h index 21e8e281f9..2fe64d9c91 100644 --- a/gtsam/nonlinear/Values.h +++ b/gtsam/nonlinear/Values.h @@ -189,7 +189,7 @@ namespace gtsam { const_iterator_type it_; deref_iterator(const_iterator_type it) : it_(it) {} ConstKeyValuePair operator*() const { return {it_->first, *(it_->second)}; } - std::unique_ptr operator->() { + std::unique_ptr operator->() const { return std::make_unique(it_->first, *(it_->second)); } bool operator==(const deref_iterator& other) const { From baa275bf51ed63dd61f3da2c90167491a2f1806e Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Tue, 26 Nov 2024 09:53:28 -0800 Subject: [PATCH 102/115] address comments --- gtsam/slam/EssentialMatrixFactor.h | 32 +++++++++------- .../slam/tests/testEssentialMatrixFactor.cpp | 37 +++++++++++++++---- 2 files changed, 47 insertions(+), 22 deletions(-) diff --git a/gtsam/slam/EssentialMatrixFactor.h b/gtsam/slam/EssentialMatrixFactor.h index 7a0b469064..5fdf138ccd 100644 --- a/gtsam/slam/EssentialMatrixFactor.h +++ b/gtsam/slam/EssentialMatrixFactor.h @@ -420,10 +420,13 @@ class EssentialMatrixFactor4 /** * Binary factor that optimizes for E and two calibrations Ka and Kb using the * algebraic epipolar error (Ka^-1 pA)'E (Kb^-1 pB). The calibrations are - * assumed different for the two images. Don'tt use this factor with same - * calibration unknown, as Jacobians will be incorrect... + * assumed different for the two images, but if you use the same key for Ka and + * Kb, the sum of the two K Jacobians equals that of the K Jacobian for + * EssentialMatrix4. If you know there is a single global calibration, use + * that factor instead. * - * Note: even stronger caveats about having priors on calibration apply. + * Note: see the comment about priors from EssentialMatrixFactor4: even stronger + * caveats about having priors on calibration apply here. */ template class EssentialMatrixFactor5 @@ -442,17 +445,18 @@ class EssentialMatrixFactor5 using Base::evaluateError; /** - * Constructor - * @param keyE Essential Matrix aEb variable key - * @param keyKa Calibration variable key for camera A - * @param keyKb Calibration variable key for camera B - * @param pA point in first camera, in pixel coordinates - * @param pB point in second camera, in pixel coordinates - * @param model noise model is about dot product in ideal, homogeneous + * Constructor + * @param keyE Essential Matrix aEb variable key + * @param keyKa Calibration variable key for camera A + * @param keyKb Calibration variable key for camera B + * @param pA point in first camera, in pixel coordinates + * @param pB point in second camera, in pixel coordinates + * @param model noise model is about dot product in ideal, homogeneous * coordinates */ EssentialMatrixFactor5(Key keyE, Key keyKa, Key keyKb, const Point2& pA, - const Point2& pB, const SharedNoiseModel& model = nullptr) + const Point2& pB, + const SharedNoiseModel& model = nullptr) : Base(model, keyE, keyKa, keyKb), pA_(pA), pB_(pB) {} /// @return a deep copy of this factor @@ -492,17 +496,17 @@ class EssentialMatrixFactor5 Point2 cA = Ka.calibrate(pA_, HKa ? &cA_H_Ka : 0, OptionalNone); Point2 cB = Kb.calibrate(pB_, HKb ? &cB_H_Kb : 0, OptionalNone); - // convert to homogeneous coordinates + // Convert to homogeneous coordinates. Vector3 vA = EssentialMatrix::Homogeneous(cA); Vector3 vB = EssentialMatrix::Homogeneous(cB); if (HKa) { - // compute the jacobian of error w.r.t Ka + // Compute the jacobian of error w.r.t Ka. *HKa = vB.transpose() * E.matrix().transpose().leftCols<2>() * cA_H_Ka; } if (HKb) { - // compute the jacobian of error w.r.t Kb + // Compute the jacobian of error w.r.t Kb. *HKb = vA.transpose() * E.matrix().leftCols<2>() * cB_H_Kb; } diff --git a/gtsam/slam/tests/testEssentialMatrixFactor.cpp b/gtsam/slam/tests/testEssentialMatrixFactor.cpp index 609f381871..11558e38ef 100644 --- a/gtsam/slam/tests/testEssentialMatrixFactor.cpp +++ b/gtsam/slam/tests/testEssentialMatrixFactor.cpp @@ -117,8 +117,8 @@ TEST(EssentialMatrixFactor, ExpressionFactor) { // Check evaluation Vector expected(1); expected << 0; - vector Hactual(1); - Vector actual = factor.unwhitenedError(values, Hactual); + vector actualH(1); + Vector actual = factor.unwhitenedError(values, actualH); EXPECT(assert_equal(expected, actual, 1e-7)); } } @@ -151,8 +151,8 @@ TEST(EssentialMatrixFactor, ExpressionFactorRotationOnly) { // Check evaluation Vector expected(1); expected << 0; - vector Hactual(1); - Vector actual = factor.unwhitenedError(values, Hactual); + vector actualH(1); + Vector actual = factor.unwhitenedError(values, actualH); EXPECT(assert_equal(expected, actual, 1e-7)); } } @@ -213,9 +213,9 @@ TEST(EssentialMatrixFactor2, factor) { const Point2 pi = PinholeBase::Project(P2); Point2 expected(pi - pB(i)); - Matrix Hactual1, Hactual2; + Matrix actualH1, actualH2; double d(baseline / P1.z()); - Vector actual = factor.evaluateError(trueE, d, Hactual1, Hactual2); + Vector actual = factor.evaluateError(trueE, d, actualH1, actualH2); EXPECT(assert_equal(expected, actual, 1e-7)); Values val; @@ -278,9 +278,9 @@ TEST(EssentialMatrixFactor3, factor) { const Point2 pi = camera2.project(P1); Point2 expected(pi - pB(i)); - Matrix Hactual1, Hactual2; + Matrix actualH1, actualH2; double d(baseline / P1.z()); - Vector actual = factor.evaluateError(bodyE, d, Hactual1, Hactual2); + Vector actual = factor.evaluateError(bodyE, d, actualH1, actualH2); EXPECT(assert_equal(expected, actual, 1e-7)); Values val; @@ -552,6 +552,27 @@ TEST(EssentialMatrixFactor5, factor) { } } +//************************************************************************* +// Test that if we use the same keys for Ka and Kb the sum of the two K +// Jacobians equals that of the single K Jacobian for EssentialMatrix4 +TEST(EssentialMatrixFactor5, SameKeys) { + Key keyE(1), keyK(2); + for (size_t i = 0; i < 5; i++) { + EssentialMatrixFactor4 factor4(keyE, keyK, pA(i), pB(i), model1); + EssentialMatrixFactor5 factor5(keyE, keyK, keyK, pA(i), pB(i), + model1); + + Values truth; + truth.insert(keyE, trueE); + truth.insert(keyK, trueK); + + // Check Jacobians + Matrix H0, H1, H2; + factor4.evaluateError(trueE, trueK, nullptr, &H0); + factor5.evaluateError(trueE, trueK, trueK, nullptr, &H1, &H2); + EXPECT(assert_equal(H0, H1 + H2, 1e-9)); + } +} } // namespace example1 //************************************************************************* From d38b577284182e85c3285eca415ac4c622494fd5 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sun, 1 Dec 2024 18:55:55 -0500 Subject: [PATCH 103/115] update CMake for finding gperftools --- cmake/FindGooglePerfTools.cmake | 35 ++++++++++++++------------------- 1 file changed, 15 insertions(+), 20 deletions(-) diff --git a/cmake/FindGooglePerfTools.cmake b/cmake/FindGooglePerfTools.cmake index 01243257b1..d85c01389e 100644 --- a/cmake/FindGooglePerfTools.cmake +++ b/cmake/FindGooglePerfTools.cmake @@ -1,35 +1,30 @@ # -*- cmake -*- -# - Find Google perftools -# Find the Google perftools includes and libraries -# This module defines -# GOOGLE_PERFTOOLS_INCLUDE_DIR, where to find heap-profiler.h, etc. -# GOOGLE_PERFTOOLS_FOUND, If false, do not try to use Google perftools. +# - Find GPerfTools (formerly Google perftools) +# Find the GPerfTools libraries +# If false, do not try to use Google perftools. # also defined for general use are # TCMALLOC_LIBRARY, where to find the tcmalloc library. -FIND_PATH(GOOGLE_PERFTOOLS_INCLUDE_DIR google/heap-profiler.h -/usr/local/include -/usr/include -) - SET(TCMALLOC_NAMES ${TCMALLOC_NAMES} tcmalloc) -FIND_LIBRARY(TCMALLOC_LIBRARY +find_library(TCMALLOC_LIBRARY NAMES ${TCMALLOC_NAMES} PATHS /usr/lib /usr/local/lib - ) +) +find_library(GPERFTOOLS_PROFILER + NAMES profiler + PATHS /usr/lib /usr/local/lib +) -IF (TCMALLOC_LIBRARY AND GOOGLE_PERFTOOLS_INCLUDE_DIR) +IF (TCMALLOC_LIBRARY AND GPERFTOOLS_PROFILER) SET(TCMALLOC_LIBRARIES ${TCMALLOC_LIBRARY}) SET(GOOGLE_PERFTOOLS_FOUND "YES") -ELSE (TCMALLOC_LIBRARY AND GOOGLE_PERFTOOLS_INCLUDE_DIR) +ELSE (TCMALLOC_LIBRARY AND GPERFTOOLS_PROFILER) SET(GOOGLE_PERFTOOLS_FOUND "NO") -ENDIF (TCMALLOC_LIBRARY AND GOOGLE_PERFTOOLS_INCLUDE_DIR) +ENDIF (TCMALLOC_LIBRARY AND GPERFTOOLS_PROFILER) IF (GOOGLE_PERFTOOLS_FOUND) - IF (NOT GOOGLE_PERFTOOLS_FIND_QUIETLY) - MESSAGE(STATUS "Found Google perftools: ${GOOGLE_PERFTOOLS_LIBRARIES}") - ENDIF (NOT GOOGLE_PERFTOOLS_FIND_QUIETLY) + MESSAGE(STATUS "Found Google perftools: ${GPERFTOOLS_PROFILER}") ELSE (GOOGLE_PERFTOOLS_FOUND) IF (GOOGLE_PERFTOOLS_FIND_REQUIRED) MESSAGE(FATAL_ERROR "Could not find Google perftools library") @@ -38,5 +33,5 @@ ENDIF (GOOGLE_PERFTOOLS_FOUND) MARK_AS_ADVANCED( TCMALLOC_LIBRARY - GOOGLE_PERFTOOLS_INCLUDE_DIR - ) + GPERFTOOLS_PROFILER +) From be4964927c657e5e3daab55ec7b36cbe38166000 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sun, 1 Dec 2024 18:57:40 -0500 Subject: [PATCH 104/115] rename TCMALLOC_LIBRARY to GPERFTOOLS_TCMALLOC --- cmake/FindGooglePerfTools.cmake | 17 +++++++++-------- 1 file changed, 9 insertions(+), 8 deletions(-) diff --git a/cmake/FindGooglePerfTools.cmake b/cmake/FindGooglePerfTools.cmake index d85c01389e..f7caa7c1c6 100644 --- a/cmake/FindGooglePerfTools.cmake +++ b/cmake/FindGooglePerfTools.cmake @@ -3,11 +3,12 @@ # - Find GPerfTools (formerly Google perftools) # Find the GPerfTools libraries # If false, do not try to use Google perftools. -# also defined for general use are -# TCMALLOC_LIBRARY, where to find the tcmalloc library. +# Also defined for general use are +# - GPERFTOOLS_TCMALLOC: where to find the tcmalloc library +# - GPERFTOOLS_PROFILER: where to find the profiler library SET(TCMALLOC_NAMES ${TCMALLOC_NAMES} tcmalloc) -find_library(TCMALLOC_LIBRARY +find_library(GPERFTOOLS_TCMALLOC NAMES ${TCMALLOC_NAMES} PATHS /usr/lib /usr/local/lib ) @@ -16,12 +17,12 @@ find_library(GPERFTOOLS_PROFILER PATHS /usr/lib /usr/local/lib ) -IF (TCMALLOC_LIBRARY AND GPERFTOOLS_PROFILER) - SET(TCMALLOC_LIBRARIES ${TCMALLOC_LIBRARY}) +IF (GPERFTOOLS_TCMALLOC AND GPERFTOOLS_PROFILER) + SET(TCMALLOC_LIBRARIES ${GPERFTOOLS_TCMALLOC}) SET(GOOGLE_PERFTOOLS_FOUND "YES") -ELSE (TCMALLOC_LIBRARY AND GPERFTOOLS_PROFILER) +ELSE (GPERFTOOLS_TCMALLOC AND GPERFTOOLS_PROFILER) SET(GOOGLE_PERFTOOLS_FOUND "NO") -ENDIF (TCMALLOC_LIBRARY AND GPERFTOOLS_PROFILER) +ENDIF (GPERFTOOLS_TCMALLOC AND GPERFTOOLS_PROFILER) IF (GOOGLE_PERFTOOLS_FOUND) MESSAGE(STATUS "Found Google perftools: ${GPERFTOOLS_PROFILER}") @@ -32,6 +33,6 @@ ELSE (GOOGLE_PERFTOOLS_FOUND) ENDIF (GOOGLE_PERFTOOLS_FOUND) MARK_AS_ADVANCED( - TCMALLOC_LIBRARY + GPERFTOOLS_TCMALLOC GPERFTOOLS_PROFILER ) From 486feeb385345d13698e67a0ff4f20161ad8de57 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sun, 1 Dec 2024 18:58:06 -0500 Subject: [PATCH 105/115] link gperftools libraries --- cmake/HandlePerfTools.cmake | 1 + 1 file changed, 1 insertion(+) diff --git a/cmake/HandlePerfTools.cmake b/cmake/HandlePerfTools.cmake index 499caf80a1..6c455d9528 100644 --- a/cmake/HandlePerfTools.cmake +++ b/cmake/HandlePerfTools.cmake @@ -2,3 +2,4 @@ ############################################################################### # Find Google perftools find_package(GooglePerfTools) +target_link_libraries(${PROJECT_NAME} ${GPERFTOOLS_TCMALLOC} ${GPERFTOOLS_PROFILER}) \ No newline at end of file From 8473911926e8605d5733bcd34493022cf5bef497 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 2 Dec 2024 09:52:20 -0500 Subject: [PATCH 106/115] computeThreshold as an individual function --- gtsam/discrete/DecisionTreeFactor.cpp | 30 +++++++++++++++++++-------- gtsam/discrete/DecisionTreeFactor.h | 11 ++++++++++ 2 files changed, 32 insertions(+), 9 deletions(-) diff --git a/gtsam/discrete/DecisionTreeFactor.cpp b/gtsam/discrete/DecisionTreeFactor.cpp index caedab713c..9ec3b0ac53 100644 --- a/gtsam/discrete/DecisionTreeFactor.cpp +++ b/gtsam/discrete/DecisionTreeFactor.cpp @@ -407,11 +407,9 @@ namespace gtsam { }; /* ************************************************************************ */ - DecisionTreeFactor DecisionTreeFactor::prune(size_t maxNrAssignments) const { - const size_t N = maxNrAssignments; - + double DecisionTreeFactor::computeThreshold(const size_t N) const { // Set of all keys - std::set allKeys(keys().begin(), keys().end()); + std::set allKeys = this->labels(); MinHeap min_heap; auto op = [&](const Assignment& a, double p) { @@ -433,18 +431,25 @@ namespace gtsam { nrAssignments *= cardinalities_.at(k); } + // If min-heap is empty, fill it initially. + // This is because there is nothing at the top. if (min_heap.empty()) { min_heap.push(p, std::min(nrAssignments, N)); } else { - // If p is larger than the smallest element, - // then we insert into the max heap. - if (p > min_heap.top()) { - for (size_t i = 0; i < std::min(nrAssignments, N); ++i) { + for (size_t i = 0; i < std::min(nrAssignments, N); ++i) { + // If p is larger than the smallest element, + // then we insert into the min heap. + // We check against the top each time because the + // heap maintains the smallest element at the top. + if (p > min_heap.top()) { if (min_heap.size() == N) { min_heap.pop(); } min_heap.push(p); + } else { + // p is <= min value so move to the next one + break; } } } @@ -452,7 +457,14 @@ namespace gtsam { }; this->visitWith(op); - double threshold = min_heap.top(); + return min_heap.top(); + } + + /* ************************************************************************ */ + DecisionTreeFactor DecisionTreeFactor::prune(size_t maxNrAssignments) const { + const size_t N = maxNrAssignments; + + double threshold = computeThreshold(N); // Now threshold the decision tree size_t total = 0; diff --git a/gtsam/discrete/DecisionTreeFactor.h b/gtsam/discrete/DecisionTreeFactor.h index 7af729f3ef..a8ab2644f9 100644 --- a/gtsam/discrete/DecisionTreeFactor.h +++ b/gtsam/discrete/DecisionTreeFactor.h @@ -224,6 +224,17 @@ namespace gtsam { /// Get all the probabilities in order of assignment values std::vector probabilities() const; + /** + * @brief Compute the probability value which is the threshold above which + * only `N` leaves are present. + * + * This is used for pruning out the smaller probabilities. + * + * @param N The number of leaves to keep post pruning. + * @return double + */ + double computeThreshold(const size_t N) const; + /** * @brief Prune the decision tree of discrete variables. * From 1091b9cd2d03258d0f79144b92c4f432628d128a Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 2 Dec 2024 09:53:01 -0500 Subject: [PATCH 107/115] test for computeThreshold --- .../discrete/tests/testDecisionTreeFactor.cpp | 45 +++++++++++++++---- 1 file changed, 37 insertions(+), 8 deletions(-) diff --git a/gtsam/discrete/tests/testDecisionTreeFactor.cpp b/gtsam/discrete/tests/testDecisionTreeFactor.cpp index a41d06c2b3..756a0cebe8 100644 --- a/gtsam/discrete/tests/testDecisionTreeFactor.cpp +++ b/gtsam/discrete/tests/testDecisionTreeFactor.cpp @@ -140,11 +140,46 @@ TEST(DecisionTreeFactor, enumerate) { EXPECT(actual == expected); } +namespace pruning_fixture { + +DiscreteKey A(1, 2), B(2, 2), C(3, 2); +DecisionTreeFactor f(A& B& C, "1 5 3 7 2 6 4 8"); + +DiscreteKey D(4, 2); +DecisionTreeFactor factor( + D& C & B & A, + "0.0 0.0 0.0 0.60658897 0.61241912 0.61241969 0.61247685 0.61247742 0.0 " + "0.0 0.0 0.99995287 1.0 1.0 1.0 1.0"); + +} // namespace pruning_fixture + +/* ************************************************************************* */ +// Check if computing the correct threshold works. +TEST(DecisionTreeFactor, ComputeThreshold) { + using namespace pruning_fixture; + + // Only keep the leaves with the top 5 values. + double threshold = f.computeThreshold(5); + EXPECT_DOUBLES_EQUAL(4.0, threshold, 1e-9); + + // Check for more extreme pruning where we only keep the top 2 leaves + threshold = f.computeThreshold(2); + EXPECT_DOUBLES_EQUAL(7.0, threshold, 1e-9); + + threshold = factor.computeThreshold(5); + EXPECT_DOUBLES_EQUAL(0.99995287, threshold, 1e-9); + + threshold = factor.computeThreshold(3); + EXPECT_DOUBLES_EQUAL(1.0, threshold, 1e-9); + + threshold = factor.computeThreshold(6); + EXPECT_DOUBLES_EQUAL(0.61247742, threshold, 1e-9); +} + /* ************************************************************************* */ // Check pruning of the decision tree works as expected. TEST(DecisionTreeFactor, Prune) { - DiscreteKey A(1, 2), B(2, 2), C(3, 2); - DecisionTreeFactor f(A & B & C, "1 5 3 7 2 6 4 8"); + using namespace pruning_fixture; // Only keep the leaves with the top 5 values. size_t maxNrAssignments = 5; @@ -160,12 +195,6 @@ TEST(DecisionTreeFactor, Prune) { DecisionTreeFactor expected2(A & B & C, "0 0 0 7 0 0 0 8"); EXPECT(assert_equal(expected2, pruned2)); - DiscreteKey D(4, 2); - DecisionTreeFactor factor( - D & C & B & A, - "0.0 0.0 0.0 0.60658897 0.61241912 0.61241969 0.61247685 0.61247742 0.0 " - "0.0 0.0 0.99995287 1.0 1.0 1.0 1.0"); - DecisionTreeFactor expected3(D & C & B & A, "0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 " "0.999952870000 1.0 1.0 1.0 1.0"); From 3980a439c0fc655ac73a2763f60109b5e6a9270f Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 2 Dec 2024 09:53:33 -0500 Subject: [PATCH 108/115] link gperftools in the top CMakeLists.txt --- cmake/HandlePerfTools.cmake | 3 +-- gtsam/CMakeLists.txt | 4 ++++ 2 files changed, 5 insertions(+), 2 deletions(-) diff --git a/cmake/HandlePerfTools.cmake b/cmake/HandlePerfTools.cmake index 6c455d9528..4b5e5c8b32 100644 --- a/cmake/HandlePerfTools.cmake +++ b/cmake/HandlePerfTools.cmake @@ -1,5 +1,4 @@ ############################################################################### # Find Google perftools -find_package(GooglePerfTools) -target_link_libraries(${PROJECT_NAME} ${GPERFTOOLS_TCMALLOC} ${GPERFTOOLS_PROFILER}) \ No newline at end of file +find_package(GooglePerfTools) \ No newline at end of file diff --git a/gtsam/CMakeLists.txt b/gtsam/CMakeLists.txt index 1fc8e45707..dc9f40ed7e 100644 --- a/gtsam/CMakeLists.txt +++ b/gtsam/CMakeLists.txt @@ -147,6 +147,10 @@ if (GTSAM_USE_EIGEN_MKL) target_include_directories(gtsam PUBLIC ${MKL_INCLUDE_DIR}) endif() +if (GOOGLE_PERFTOOLS_FOUND) + target_link_libraries(gtsam PRIVATE ${GPERFTOOLS_TCMALLOC} ${GPERFTOOLS_PROFILER}) +endif() + # Add includes for source directories 'BEFORE' boost and any system include # paths so that the compiler uses GTSAM headers in our source directory instead # of any previously installed GTSAM headers. From de1c2d78fd2f14fdd25004edd31cae21d18f6435 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 2 Dec 2024 09:54:32 -0500 Subject: [PATCH 109/115] rename GOOGLE_PERFTOOLS to GPERFTOOLS --- cmake/FindGooglePerfTools.cmake | 12 ++++++------ cmake/HandleAllocators.cmake | 2 +- gtsam/CMakeLists.txt | 2 +- 3 files changed, 8 insertions(+), 8 deletions(-) diff --git a/cmake/FindGooglePerfTools.cmake b/cmake/FindGooglePerfTools.cmake index f7caa7c1c6..f73f449ecf 100644 --- a/cmake/FindGooglePerfTools.cmake +++ b/cmake/FindGooglePerfTools.cmake @@ -19,18 +19,18 @@ find_library(GPERFTOOLS_PROFILER IF (GPERFTOOLS_TCMALLOC AND GPERFTOOLS_PROFILER) SET(TCMALLOC_LIBRARIES ${GPERFTOOLS_TCMALLOC}) - SET(GOOGLE_PERFTOOLS_FOUND "YES") + SET(GPERFTOOLS_FOUND "YES") ELSE (GPERFTOOLS_TCMALLOC AND GPERFTOOLS_PROFILER) - SET(GOOGLE_PERFTOOLS_FOUND "NO") + SET(GPERFTOOLS_FOUND "NO") ENDIF (GPERFTOOLS_TCMALLOC AND GPERFTOOLS_PROFILER) -IF (GOOGLE_PERFTOOLS_FOUND) - MESSAGE(STATUS "Found Google perftools: ${GPERFTOOLS_PROFILER}") -ELSE (GOOGLE_PERFTOOLS_FOUND) +IF (GPERFTOOLS_FOUND) + MESSAGE(STATUS "Found Gperftools: ${GPERFTOOLS_PROFILER}") +ELSE (GPERFTOOLS_FOUND) IF (GOOGLE_PERFTOOLS_FIND_REQUIRED) MESSAGE(FATAL_ERROR "Could not find Google perftools library") ENDIF (GOOGLE_PERFTOOLS_FIND_REQUIRED) -ENDIF (GOOGLE_PERFTOOLS_FOUND) +ENDIF (GPERFTOOLS_FOUND) MARK_AS_ADVANCED( GPERFTOOLS_TCMALLOC diff --git a/cmake/HandleAllocators.cmake b/cmake/HandleAllocators.cmake index 63411b17bc..38297bbbab 100644 --- a/cmake/HandleAllocators.cmake +++ b/cmake/HandleAllocators.cmake @@ -7,7 +7,7 @@ else() list(APPEND possible_allocators BoostPool STL) set(preferred_allocator STL) endif() -if(GOOGLE_PERFTOOLS_FOUND) +if(GPERFTOOLS_FOUND) list(APPEND possible_allocators tcmalloc) endif() diff --git a/gtsam/CMakeLists.txt b/gtsam/CMakeLists.txt index dc9f40ed7e..9a4b6ac3a5 100644 --- a/gtsam/CMakeLists.txt +++ b/gtsam/CMakeLists.txt @@ -147,7 +147,7 @@ if (GTSAM_USE_EIGEN_MKL) target_include_directories(gtsam PUBLIC ${MKL_INCLUDE_DIR}) endif() -if (GOOGLE_PERFTOOLS_FOUND) +if (GPERFTOOLS_FOUND) target_link_libraries(gtsam PRIVATE ${GPERFTOOLS_TCMALLOC} ${GPERFTOOLS_PROFILER}) endif() From 94e31c99df1f9fb68eb043f5a39830694cb698df Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 2 Dec 2024 09:54:56 -0500 Subject: [PATCH 110/115] expose Rot3::expmap in wrapper --- gtsam/geometry/geometry.i | 1 + 1 file changed, 1 insertion(+) diff --git a/gtsam/geometry/geometry.i b/gtsam/geometry/geometry.i index 42d2fe5505..a8af78f2f4 100644 --- a/gtsam/geometry/geometry.i +++ b/gtsam/geometry/geometry.i @@ -360,6 +360,7 @@ class Rot3 { // Standard Interface static gtsam::Rot3 Expmap(gtsam::Vector v); static gtsam::Vector Logmap(const gtsam::Rot3& p); + gtsam::Rot3 expmap(const gtsam::Vector& v); gtsam::Vector logmap(const gtsam::Rot3& p); gtsam::Matrix matrix() const; gtsam::Matrix transpose() const; From 557d7d58278735c2d352ffe941d596be7c4b1836 Mon Sep 17 00:00:00 2001 From: Joel Truher Date: Fri, 29 Nov 2024 19:25:16 -0800 Subject: [PATCH 111/115] wrap BatchFixedLagSmoother.getFactors() --- gtsam/nonlinear/nonlinear.i | 3 +++ 1 file changed, 3 insertions(+) diff --git a/gtsam/nonlinear/nonlinear.i b/gtsam/nonlinear/nonlinear.i index 09c234630e..5d15c11634 100644 --- a/gtsam/nonlinear/nonlinear.i +++ b/gtsam/nonlinear/nonlinear.i @@ -715,6 +715,9 @@ virtual class BatchFixedLagSmoother : gtsam::FixedLagSmoother { void print(string s = "BatchFixedLagSmoother:\n") const; gtsam::LevenbergMarquardtParams params() const; + + gtsam::NonlinearFactorGraph getFactors() const; + template From a9c75d8ef4f2bfd8f188c9f02caad076331b1930 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 2 Dec 2024 12:07:30 -0500 Subject: [PATCH 112/115] add pruned flag to avoid extra pruning --- gtsam/hybrid/HybridBayesTree.cpp | 8 +++++--- gtsam/hybrid/HybridGaussianConditional.cpp | 12 +++++++----- gtsam/hybrid/HybridGaussianConditional.h | 11 +++++++++-- 3 files changed, 21 insertions(+), 10 deletions(-) diff --git a/gtsam/hybrid/HybridBayesTree.cpp b/gtsam/hybrid/HybridBayesTree.cpp index 193635a214..1b633e024d 100644 --- a/gtsam/hybrid/HybridBayesTree.cpp +++ b/gtsam/hybrid/HybridBayesTree.cpp @@ -210,9 +210,11 @@ void HybridBayesTree::prune(const size_t maxNrLeaves) { if (conditional->isHybrid()) { auto hybridGaussianCond = conditional->asHybrid(); - // Imperative - clique->conditional() = std::make_shared( - hybridGaussianCond->prune(parentData.prunedDiscreteProbs)); + if (!hybridGaussianCond->pruned()) { + // Imperative + clique->conditional() = std::make_shared( + hybridGaussianCond->prune(parentData.prunedDiscreteProbs)); + } } return parentData; } diff --git a/gtsam/hybrid/HybridGaussianConditional.cpp b/gtsam/hybrid/HybridGaussianConditional.cpp index 1bec428107..54346679ee 100644 --- a/gtsam/hybrid/HybridGaussianConditional.cpp +++ b/gtsam/hybrid/HybridGaussianConditional.cpp @@ -120,7 +120,7 @@ struct HybridGaussianConditional::Helper { /* *******************************************************************************/ HybridGaussianConditional::HybridGaussianConditional( - const DiscreteKeys &discreteParents, Helper &&helper) + const DiscreteKeys &discreteParents, Helper &&helper, bool pruned) : BaseFactor(discreteParents, FactorValuePairs( [&](const GaussianFactorValuePair @@ -130,7 +130,8 @@ HybridGaussianConditional::HybridGaussianConditional( }, std::move(helper.pairs))), BaseConditional(*helper.nrFrontals), - negLogConstant_(helper.minNegLogConstant) {} + negLogConstant_(helper.minNegLogConstant), + pruned_(pruned) {} HybridGaussianConditional::HybridGaussianConditional( const DiscreteKey &discreteParent, @@ -166,8 +167,9 @@ HybridGaussianConditional::HybridGaussianConditional( : HybridGaussianConditional(discreteParents, Helper(conditionals)) {} HybridGaussianConditional::HybridGaussianConditional( - const DiscreteKeys &discreteParents, const FactorValuePairs &pairs) - : HybridGaussianConditional(discreteParents, Helper(pairs)) {} + const DiscreteKeys &discreteParents, const FactorValuePairs &pairs, + bool pruned) + : HybridGaussianConditional(discreteParents, Helper(pairs), pruned) {} /* *******************************************************************************/ const HybridGaussianConditional::Conditionals @@ -331,7 +333,7 @@ HybridGaussianConditional::shared_ptr HybridGaussianConditional::prune( FactorValuePairs prunedConditionals = factors().apply(pruner); return std::make_shared(discreteKeys(), - prunedConditionals); + prunedConditionals, true); } /* *******************************************************************************/ diff --git a/gtsam/hybrid/HybridGaussianConditional.h b/gtsam/hybrid/HybridGaussianConditional.h index c485fafcef..0d2b1323c8 100644 --- a/gtsam/hybrid/HybridGaussianConditional.h +++ b/gtsam/hybrid/HybridGaussianConditional.h @@ -68,6 +68,9 @@ class GTSAM_EXPORT HybridGaussianConditional ///< Take advantage of the neg-log space so everything is a minimization double negLogConstant_; + /// Flag to indicate if the conditional has been pruned. + bool pruned_ = false; + public: /// @name Constructors /// @{ @@ -150,9 +153,10 @@ class GTSAM_EXPORT HybridGaussianConditional * * @param discreteParents the discrete parents. Will be placed last. * @param conditionalPairs Decision tree of GaussianFactor/scalar pairs. + * @param pruned Flag indicating if conditional has been pruned. */ HybridGaussianConditional(const DiscreteKeys &discreteParents, - const FactorValuePairs &pairs); + const FactorValuePairs &pairs, bool pruned = false); /// @} /// @name Testable @@ -233,6 +237,9 @@ class GTSAM_EXPORT HybridGaussianConditional HybridGaussianConditional::shared_ptr prune( const DecisionTreeFactor &discreteProbs) const; + /// Return true if the conditional has already been pruned. + bool pruned() const { return pruned_; } + /// @} private: @@ -241,7 +248,7 @@ class GTSAM_EXPORT HybridGaussianConditional /// Private constructor that uses helper struct above. HybridGaussianConditional(const DiscreteKeys &discreteParents, - Helper &&helper); + Helper &&helper, bool pruned = false); /// Check whether `given` has values for all frontal keys. bool allFrontalsGiven(const VectorValues &given) const; From bbdeaa400fe5305d86eb40a12a7b7b17516a6c0d Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Sun, 1 Dec 2024 12:45:26 -0500 Subject: [PATCH 113/115] Remove the asserts as they have already been checked in Eigen --- gtsam/base/Vector.h | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/gtsam/base/Vector.h b/gtsam/base/Vector.h index f9a5cf0792..4ef3742344 100644 --- a/gtsam/base/Vector.h +++ b/gtsam/base/Vector.h @@ -16,6 +16,7 @@ * @author Frank Dellaert * @author Alex Hagiopol * @author Varun Agrawal + * @author Fan Jiang */ // \callgraph @@ -193,14 +194,12 @@ GTSAM_EXPORT Vector ediv_(const Vector &a, const Vector &b); */ template inline double dot(const V1 &a, const V2& b) { - assert (b.size()==a.size()); return a.dot(b); } /** compatibility version for ublas' inner_prod() */ template inline double inner_prod(const V1 &a, const V2& b) { - assert (b.size()==a.size()); return a.dot(b); } From 2aa22005f11e8b50d5dd5332e9e59104c1996b4c Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Fri, 6 Dec 2024 11:32:12 -0500 Subject: [PATCH 114/115] add CMake switch for GPerfTools --- cmake/FindGooglePerfTools.cmake | 2 ++ 1 file changed, 2 insertions(+) diff --git a/cmake/FindGooglePerfTools.cmake b/cmake/FindGooglePerfTools.cmake index f73f449ecf..4af2685289 100644 --- a/cmake/FindGooglePerfTools.cmake +++ b/cmake/FindGooglePerfTools.cmake @@ -36,3 +36,5 @@ MARK_AS_ADVANCED( GPERFTOOLS_TCMALLOC GPERFTOOLS_PROFILER ) + +option(GTSAM_ENABLE_GPERFTOOLS "Enable/Disable Gperftools" OFF) From c00de92854fce0038bf5e5ac3b49bbf45784a77e Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Fri, 6 Dec 2024 11:32:34 -0500 Subject: [PATCH 115/115] use the switch --- gtsam/CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/CMakeLists.txt b/gtsam/CMakeLists.txt index 9a4b6ac3a5..87440d19f1 100644 --- a/gtsam/CMakeLists.txt +++ b/gtsam/CMakeLists.txt @@ -147,7 +147,7 @@ if (GTSAM_USE_EIGEN_MKL) target_include_directories(gtsam PUBLIC ${MKL_INCLUDE_DIR}) endif() -if (GPERFTOOLS_FOUND) +if (GTSAM_ENABLE_GPERFTOOLS AND GPERFTOOLS_FOUND) target_link_libraries(gtsam PRIVATE ${GPERFTOOLS_TCMALLOC} ${GPERFTOOLS_PROFILER}) endif()