From ddd95f4a51549d3c19cdca36d488da1323430676 Mon Sep 17 00:00:00 2001 From: Jade Turner Date: Sat, 7 Dec 2024 00:33:21 +0800 Subject: [PATCH 01/80] [build] Allow disabling tests and examples When you have gtsam as a dependency in CMake these would get built. This doesn't make sense and increases build times, allowing them to be disabled fixes this. Signed-off-by: Jade Turner --- CMakeLists.txt | 11 +++++++++-- 1 file changed, 9 insertions(+), 2 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index e3b462eec..ffedf56e4 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -112,10 +112,17 @@ add_subdirectory(CppUnitLite) add_subdirectory(gtsam) # Build Tests -add_subdirectory(tests) +option(BUILD_TESTS "Builds unit tests" ON) +if (BUILD_TESTS) + add_subdirectory(tests) +endif() + # Build examples -add_subdirectory(examples) +option(BUILD_EXAMPLES "Builds examples" ON) +if (BUILD_EXAMPLES) + add_subdirectory(examples) +endif() # Build timing add_subdirectory(timing) From 699757d381758cc32c5da96e4b021b42c4e283c0 Mon Sep 17 00:00:00 2001 From: Jade Turner Date: Mon, 9 Dec 2024 15:17:16 +0800 Subject: [PATCH 02/80] Revert "[build] Allow disabling tests and examples" This reverts commit ddd95f4a51549d3c19cdca36d488da1323430676. --- CMakeLists.txt | 11 ++--------- 1 file changed, 2 insertions(+), 9 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index ffedf56e4..e3b462eec 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -112,17 +112,10 @@ add_subdirectory(CppUnitLite) add_subdirectory(gtsam) # Build Tests -option(BUILD_TESTS "Builds unit tests" ON) -if (BUILD_TESTS) - add_subdirectory(tests) -endif() - +add_subdirectory(tests) # Build examples -option(BUILD_EXAMPLES "Builds examples" ON) -if (BUILD_EXAMPLES) - add_subdirectory(examples) -endif() +add_subdirectory(examples) # Build timing add_subdirectory(timing) From a425e6e3d4c80f5553d045e54d4e7d7f60a7d258 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Mon, 9 Dec 2024 16:36:43 -0500 Subject: [PATCH 03/80] better docs --- gtsam/linear/KalmanFilter.h | 226 +++++++++++++++++++++++------------- 1 file changed, 146 insertions(+), 80 deletions(-) diff --git a/gtsam/linear/KalmanFilter.h b/gtsam/linear/KalmanFilter.h index 65fac877a..7fde9975d 100644 --- a/gtsam/linear/KalmanFilter.h +++ b/gtsam/linear/KalmanFilter.h @@ -11,10 +11,10 @@ /** * @file KalmanFilter.h - * @brief Simple linear Kalman filter. Implemented using factor graphs, i.e., does Cholesky-based SRIF, really. + * @brief Simple linear Kalman filter implemented using factor graphs, i.e., + * performs Cholesky or QR-based SRIF (Square-Root Information Filter). * @date Sep 3, 2011 - * @author Stephen Williams - * @author Frank Dellaert + * @authors Stephen Williams, Frank Dellaert */ #pragma once @@ -32,120 +32,186 @@ namespace gtsam { /** * Kalman Filter class * - * Knows how to maintain a Gaussian density under linear-Gaussian motion and - * measurement models. It uses the square-root information form, as usual in GTSAM. + * Maintains a Gaussian density under linear-Gaussian motion and + * measurement models using the square-root information form. * - * The filter is functional, in that it does not have state: you call init() to create - * an initial state, then predict() and update() that create new states out of an old state. + * The filter is functional; it does not maintain internal state. Instead: + * - Use `init()` to create an initial filter state, + * - Call `predict()` and `update()` to create new states. */ class GTSAM_EXPORT KalmanFilter { - -public: - + public: /** - * This Kalman filter is a Square-root Information filter - * The type below allows you to specify the factorization variant. + * @enum Factorization + * @brief Specifies the factorization variant to use. */ - enum Factorization { - QR, CHOLESKY - }; + enum Factorization { QR, CHOLESKY }; /** - * The Kalman filter state is simply a GaussianDensity + * @typedef State + * @brief The Kalman filter state, represented as a shared pointer to a + * GaussianDensity. */ typedef GaussianDensity::shared_ptr State; -private: - - const size_t n_; /** dimensionality of state */ - const Matrix I_; /** identity matrix of size n*n */ - const GaussianFactorGraph::Eliminate function_; /** algorithm */ - - State solve(const GaussianFactorGraph& factorGraph) const; - State fuse(const State& p, GaussianFactor::shared_ptr newFactor) const; - -public: - - // Constructor - KalmanFilter(size_t n, Factorization method = - KALMANFILTER_DEFAULT_FACTORIZATION) : - n_(n), I_(Matrix::Identity(n_, n_)), function_( - method == QR ? GaussianFactorGraph::Eliminate(EliminateQR) : - GaussianFactorGraph::Eliminate(EliminateCholesky)) { - } + private: + const size_t n_; ///< Dimensionality of the state. + const Matrix I_; ///< Identity matrix of size \f$ n \times n \f$. + const GaussianFactorGraph::Eliminate + function_; ///< Elimination algorithm used. /** - * Create initial state, i.e., prior density at time k=0 - * In Kalman Filter notation, these are x_{0|0} and P_{0|0} - * @param x0 estimate at time 0 - * @param P0 covariance at time 0, given as a diagonal Gaussian 'model' + * Solve the factor graph. + * @param factorGraph The Gaussian factor graph to solve. + * @return The resulting Kalman filter state. + */ + State solve(const GaussianFactorGraph& factorGraph) const; + + /** + * Fuse two states. + * @param p The prior state. + * @param newFactor The new factor to incorporate. + * @return The resulting fused state. + */ + State fuse(const State& p, GaussianFactor::shared_ptr newFactor) const; + + public: + /** + * Constructor. + * @param n Dimensionality of the state. + * @param method Factorization method (default: QR unless compile-flag set). + */ + KalmanFilter(size_t n, + Factorization method = KALMANFILTER_DEFAULT_FACTORIZATION) + : n_(n), + I_(Matrix::Identity(n_, n_)), + function_(method == QR + ? GaussianFactorGraph::Eliminate(EliminateQR) + : GaussianFactorGraph::Eliminate(EliminateCholesky)) {} + + /** + * Create the initial state (prior density at time \f$ k=0 \f$). + * + * In Kalman Filter notation: + * - \f$ x_{0|0} \f$: Initial state estimate. + * - \f$ P_{0|0} \f$: Initial covariance matrix. + * + * @param x0 Estimate of the state at time 0 (\f$ x_{0|0} \f$). + * @param P0 Covariance matrix (\f$ P_{0|0} \f$), given as a diagonal Gaussian + * model. + * @return Initial Kalman filter state. */ State init(const Vector& x0, const SharedDiagonal& P0) const; - /// version of init with a full covariance matrix + /** + * Create the initial state with a full covariance matrix. + * @param x0 Initial state estimate. + * @param P0 Full covariance matrix. + * @return Initial Kalman filter state. + */ State init(const Vector& x0, const Matrix& P0) const; - /// print + /** + * Print the Kalman filter details. + * @param s Optional string prefix. + */ void print(const std::string& s = "") const; - /** Return step index k, starts at 0, incremented at each predict. */ - static Key step(const State& p) { - return p->firstFrontalKey(); - } + /** + * Return the step index \f$ k \f$ (starts at 0, incremented at each predict + * step). + * @param p The current state. + * @return Step index. + */ + static Key step(const State& p) { return p->firstFrontalKey(); } /** - * Predict the state P(x_{t+1}|Z^t) - * In Kalman Filter notation, this is x_{t+1|t} and P_{t+1|t} - * Details and parameters: - * In a linear Kalman Filter, the motion model is f(x_{t}) = F*x_{t} + B*u_{t} + w - * where F is the state transition model/matrix, B is the control input model, - * and w is zero-mean, Gaussian white noise with covariance Q. + * Predict the next state \f$ P(x_{k+1}|Z^k) \f$. + * + * In Kalman Filter notation: + * - \f$ x_{k+1|k} \f$: Predicted state. + * - \f$ P_{k+1|k} \f$: Predicted covariance. + * + * Motion model: + * \f[ + * x_{k+1} = F \cdot x_k + B \cdot u_k + w + * \f] + * where \f$ w \f$ is zero-mean Gaussian noise with covariance \f$ Q \f$. + * + * @param p Previous state (\f$ x_k \f$). + * @param F State transition matrix (\f$ F \f$). + * @param B Control input matrix (\f$ B \f$). + * @param u Control vector (\f$ u_k \f$). + * @param modelQ Noise model (\f$ Q \f$, diagonal Gaussian). + * @return Predicted state (\f$ x_{k+1|k} \f$). */ State predict(const State& p, const Matrix& F, const Matrix& B, - const Vector& u, const SharedDiagonal& modelQ) const; + const Vector& u, const SharedDiagonal& modelQ) const; - /* - * Version of predict with full covariance - * Q is normally derived as G*w*G^T where w models uncertainty of some - * physical property, such as velocity or acceleration, and G is derived from physics. - * This version allows more realistic models than a diagonal covariance matrix. + /** + * Predict the next state with a full covariance matrix. + * + *@note Q is normally derived as G*w*G^T where w models uncertainty of some + * physical property, such as velocity or acceleration, and G is derived from + * physics. This version allows more realistic models than a diagonal matrix. + * + * @param p Previous state. + * @param F State transition matrix. + * @param B Control input matrix. + * @param u Control vector. + * @param Q Full covariance matrix (\f$ Q \f$). + * @return Predicted state. */ State predictQ(const State& p, const Matrix& F, const Matrix& B, - const Vector& u, const Matrix& Q) const; + const Vector& u, const Matrix& Q) const; /** - * Predict the state P(x_{t+1}|Z^t) - * In Kalman Filter notation, this is x_{t+1|t} and P_{t+1|t} - * After the call, that is the density that can be queried. - * Details and parameters: - * This version of predict takes GaussianFactor motion model [A0 A1 b] - * with an optional noise model. + * Predict the next state using a GaussianFactor motion model. + * @param p Previous state. + * @param A0 Factor matrix. + * @param A1 Factor matrix. + * @param b Constant term vector. + * @param model Noise model (optional). + * @return Predicted state. */ State predict2(const State& p, const Matrix& A0, const Matrix& A1, - const Vector& b, const SharedDiagonal& model) const; + const Vector& b, const SharedDiagonal& model = nullptr) const; /** - * Update Kalman filter with a measurement - * For the Kalman Filter, the measurement function, h(x_{t}) = z_{t} - * will be of the form h(x_{t}) = H*x_{t} + v - * where H is the observation model/matrix, and v is zero-mean, - * Gaussian white noise with covariance R. + * Update the Kalman filter with a measurement. + * + * Observation model: + * \f[ + * z_k = H \cdot x_k + v + * \f] + * where \f$ v \f$ is zero-mean Gaussian noise with covariance R. * In this version, R is restricted to diagonal Gaussians (model parameter) + * + * @param p Previous state. + * @param H Observation matrix. + * @param z Measurement vector. + * @param model Noise model (diagonal Gaussian). + * @return Updated state. */ State update(const State& p, const Matrix& H, const Vector& z, - const SharedDiagonal& model) const; + const SharedDiagonal& model) const; - /* - * Version of update with full covariance - * Q is normally derived as G*w*G^T where w models uncertainty of some - * physical property, such as velocity or acceleration, and G is derived from physics. - * This version allows more realistic models than a diagonal covariance matrix. + /** + * Update the Kalman filter with a measurement using a full covariance matrix. + * @param p Previous state. + * @param H Observation matrix. + * @param z Measurement vector. + * @param R Full covariance matrix. + * @return Updated state. */ State updateQ(const State& p, const Matrix& H, const Vector& z, - const Matrix& Q) const; + const Matrix& R) const; + + /** + * Return the dimensionality of the state. + * @return Dimensionality of the state. + */ + size_t dim() const { return n_; } }; -} // \namespace gtsam - -/* ************************************************************************* */ - +} // namespace gtsam \ No newline at end of file From 1ae3fc2ad80ff27e499ea49ac942eff6ffef0910 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Mon, 9 Dec 2024 16:38:15 -0500 Subject: [PATCH 04/80] Use LLT rather than inverse --- gtsam/linear/KalmanFilter.cpp | 120 ++++++++++++++++++++-------------- 1 file changed, 70 insertions(+), 50 deletions(-) diff --git a/gtsam/linear/KalmanFilter.cpp b/gtsam/linear/KalmanFilter.cpp index ded6bc5e3..12a43093c 100644 --- a/gtsam/linear/KalmanFilter.cpp +++ b/gtsam/linear/KalmanFilter.cpp @@ -20,26 +20,35 @@ * @author Frank Dellaert */ -#include +#include #include #include -#include -#include - +#include using namespace std; namespace gtsam { -/* ************************************************************************* */ -// Auxiliary function to solve factor graph and return pointer to root conditional -KalmanFilter::State // -KalmanFilter::solve(const GaussianFactorGraph& factorGraph) const { +// In the code below we often need to multiply matrices with R, the Cholesky +// factor of the information matrix Q^{-1}, when given a covariance matrix Q. +// We can do this efficiently using Eigen, as we have: +// Q = L L^T +// => Q^{-1} = L^{-T}L^{-1} = R^T R +// => L^{-1} = R +// Rather than explicitly calculate R and do R*A, we should use L.solve(A) +// The following macro makes L available in the scope where it is used: +#define FACTORIZE_Q_INTO_L(Q) \ + Eigen::LLT llt(Q); \ + auto L = llt.matrixL(); +/* ************************************************************************* */ +// Auxiliary function to solve factor graph and return pointer to root +// conditional +KalmanFilter::State KalmanFilter::solve( + const GaussianFactorGraph& factorGraph) const { // Eliminate the graph using the provided Eliminate function Ordering ordering(factorGraph.keys()); - const auto bayesNet = // - factorGraph.eliminateSequential(ordering, function_); + const auto bayesNet = factorGraph.eliminateSequential(ordering, function_); // As this is a filter, all we need is the posterior P(x_t). // This is the last GaussianConditional in the resulting BayesNet @@ -49,9 +58,8 @@ KalmanFilter::solve(const GaussianFactorGraph& factorGraph) const { /* ************************************************************************* */ // Auxiliary function to create a small graph for predict or update and solve -KalmanFilter::State // -KalmanFilter::fuse(const State& p, GaussianFactor::shared_ptr newFactor) const { - +KalmanFilter::State KalmanFilter::fuse( + const State& p, GaussianFactor::shared_ptr newFactor) const { // Create a factor graph GaussianFactorGraph factorGraph; factorGraph.push_back(p); @@ -63,20 +71,27 @@ KalmanFilter::fuse(const State& p, GaussianFactor::shared_ptr newFactor) const { /* ************************************************************************* */ KalmanFilter::State KalmanFilter::init(const Vector& x0, - const SharedDiagonal& P0) const { - + const SharedDiagonal& P0) const { // Create a factor graph f(x0), eliminate it into P(x0) GaussianFactorGraph factorGraph; - factorGraph.emplace_shared(0, I_, x0, P0); // |x-x0|^2_diagSigma + factorGraph.emplace_shared(0, I_, x0, + P0); // |x-x0|^2_P0 return solve(factorGraph); } /* ************************************************************************* */ -KalmanFilter::State KalmanFilter::init(const Vector& x, const Matrix& P0) const { - +KalmanFilter::State KalmanFilter::init(const Vector& x0, + const Matrix& P0) const { // Create a factor graph f(x0), eliminate it into P(x0) GaussianFactorGraph factorGraph; - factorGraph.emplace_shared(0, x, P0); // 0.5*(x-x0)'*inv(Sigma)*(x-x0) + + // Perform Cholesky decomposition of P0 + FACTORIZE_Q_INTO_L(P0) + + // Premultiply I and x0 with R + const Matrix R = L.solve(I_); // = R + const Vector b = L.solve(x0); // = R*x0 + factorGraph.emplace_shared(0, R, b); return solve(factorGraph); } @@ -87,21 +102,23 @@ void KalmanFilter::print(const string& s) const { /* ************************************************************************* */ KalmanFilter::State KalmanFilter::predict(const State& p, const Matrix& F, - const Matrix& B, const Vector& u, const SharedDiagonal& model) const { - + const Matrix& B, const Vector& u, + const SharedDiagonal& model) const { // The factor related to the motion model is defined as - // f2(x_{t},x_{t+1}) = (F*x_{t} + B*u - x_{t+1}) * Q^-1 * (F*x_{t} + B*u - x_{t+1})^T + // f2(x_{k},x_{k+1}) = + // (F*x_{k} + B*u - x_{k+1}) * Q^-1 * (F*x_{k} + B*u - x_{k+1})^T Key k = step(p); return fuse(p, - std::make_shared(k, -F, k + 1, I_, B * u, model)); + std::make_shared(k, -F, k + 1, I_, B * u, model)); } /* ************************************************************************* */ KalmanFilter::State KalmanFilter::predictQ(const State& p, const Matrix& F, - const Matrix& B, const Vector& u, const Matrix& Q) const { - + const Matrix& B, const Vector& u, + const Matrix& Q) const { #ifndef NDEBUG - DenseIndex n = F.cols(); + const DenseIndex n = dim(); + assert(F.cols() == n); assert(F.rows() == n); assert(B.rows() == n); assert(B.cols() == u.size()); @@ -109,50 +126,53 @@ KalmanFilter::State KalmanFilter::predictQ(const State& p, const Matrix& F, assert(Q.cols() == n); #endif - // The factor related to the motion model is defined as - // f2(x_{t},x_{t+1}) = (F*x_{t} + B*u - x_{t+1}) * Q^-1 * (F*x_{t} + B*u - x_{t+1})^T - // See documentation in HessianFactor, we have A1 = -F, A2 = I_, b = B*u: - // TODO: starts to seem more elaborate than straight-up KF equations? - Matrix M = Q.inverse(), Ft = trans(F); - Matrix G12 = -Ft * M, G11 = -G12 * F, G22 = M; - Vector b = B * u, g2 = M * b, g1 = -Ft * g2; - double f = dot(b, g2); - Key k = step(p); - return fuse(p, - std::make_shared(k, k + 1, G11, G12, g1, G22, g2, f)); + // Perform Cholesky decomposition of Q + FACTORIZE_Q_INTO_L(Q) + + // Premultiply A1 = -F and A2 = I, b = B * u with R + const Matrix A1 = -L.solve(F); // = -R*F + const Matrix A2 = L.solve(I_); // = R + const Vector b = L.solve(B * u); // = R * (B * u) + return predict2(p, A1, A2, b); } /* ************************************************************************* */ KalmanFilter::State KalmanFilter::predict2(const State& p, const Matrix& A0, - const Matrix& A1, const Vector& b, const SharedDiagonal& model) const { + const Matrix& A1, const Vector& b, + const SharedDiagonal& model) const { // Nhe factor related to the motion model is defined as - // f2(x_{t},x_{t+1}) = |A0*x_{t} + A1*x_{t+1} - b|^2 + // f2(x_{k},x_{k+1}) = |A0*x_{k} + A1*x_{k+1} - b|^2 Key k = step(p); return fuse(p, std::make_shared(k, A0, k + 1, A1, b, model)); } /* ************************************************************************* */ KalmanFilter::State KalmanFilter::update(const State& p, const Matrix& H, - const Vector& z, const SharedDiagonal& model) const { + const Vector& z, + const SharedDiagonal& model) const { // The factor related to the measurements would be defined as - // f2 = (h(x_{t}) - z_{t}) * R^-1 * (h(x_{t}) - z_{t})^T - // = (x_{t} - z_{t}) * R^-1 * (x_{t} - z_{t})^T + // f2 = (h(x_{k}) - z_{k}) * R^-1 * (h(x_{k}) - z_{k})^T + // = (x_{k} - z_{k}) * R^-1 * (x_{k} - z_{k})^T Key k = step(p); return fuse(p, std::make_shared(k, H, z, model)); } /* ************************************************************************* */ KalmanFilter::State KalmanFilter::updateQ(const State& p, const Matrix& H, - const Vector& z, const Matrix& Q) const { + const Vector& z, + const Matrix& Q) const { Key k = step(p); - Matrix M = Q.inverse(), Ht = trans(H); - Matrix G = Ht * M * H; - Vector g = Ht * M * z; - double f = dot(z, M * z); - return fuse(p, std::make_shared(k, G, g, f)); + + // Perform Cholesky decomposition of Q + FACTORIZE_Q_INTO_L(Q) + + // Pre-multiply H and z with R, respectively + const Matrix RtH = L.solve(H); // = R * H + const Vector b = L.solve(z); // = R * z + + return fuse(p, std::make_shared(k, RtH, b)); } /* ************************************************************************* */ -} // \namespace gtsam - +} // namespace gtsam From 8ce55a221001d931a41cccfd013b9adc95dad5d9 Mon Sep 17 00:00:00 2001 From: Jade Turner Date: Thu, 12 Dec 2024 00:16:03 +0800 Subject: [PATCH 05/80] document flags Signed-off-by: Jade Turner --- INSTALL.md | 20 +++++++++++++++----- 1 file changed, 15 insertions(+), 5 deletions(-) diff --git a/INSTALL.md b/INSTALL.md index 10bee196c..51f00fd3b 100644 --- a/INSTALL.md +++ b/INSTALL.md @@ -27,7 +27,7 @@ $ make install downloaded from https://www.threadingbuildingblocks.org/ - GTSAM may be configured to use MKL by toggling `GTSAM_WITH_EIGEN_MKL` and `GTSAM_WITH_EIGEN_MKL_OPENMP` to `ON`; however, best performance is usually - achieved with MKL disabled. We therefore advise you to benchmark your problem + achieved with MKL disabled. We therefore advise you to benchmark your problem before using MKL. Tested compilers: @@ -128,12 +128,12 @@ We support several build configurations for GTSAM (case insensitive) #### CMAKE_INSTALL_PREFIX -The install folder. The default is typically `/usr/local/`. +The install folder. The default is typically `/usr/local/`. To configure to install to your home directory, you could execute: ```cmake -DCMAKE_INSTALL_PREFIX:PATH=$HOME ..``` -#### GTSAM_TOOLBOX_INSTALL_PATH +#### GTSAM_TOOLBOX_INSTALL_PATH The Matlab toolbox will be installed in a subdirectory of this folder, called 'gtsam'. @@ -142,7 +142,7 @@ of this folder, called 'gtsam'. #### GTSAM_BUILD_CONVENIENCE_LIBRARIES -This is a build option to allow for tests in subfolders to be linked against convenience libraries rather than the full libgtsam. +This is a build option to allow for tests in subfolders to be linked against convenience libraries rather than the full libgtsam. Set with the command line as follows: ```cmake -DGTSAM_BUILD_CONVENIENCE_LIBRARIES:OPTION=ON ..``` @@ -159,6 +159,16 @@ Set with the command line as follows: ON: When enabled, libgtsam_unstable will be built and installed with the same options as libgtsam. In addition, if tests are enabled, the unit tests will be built as well. The Matlab toolbox will also be generated if the matlab toolbox is enabled, installing into a folder called `gtsam_unstable`. OFF (Default) If disabled, no `gtsam_unstable` code will be included in build or install. +## Convenience Options: + +#### GTSAM_BUILD_EXAMPLES_ALWAYS + +Whether or not to force building examples, can be true or false. + +#### GTSAM_BUILD_TESTS + +Whether or not to build tests, can be true or false. + ## Check `make check` will build and run all of the tests. Note that the tests will only be @@ -179,7 +189,7 @@ Here are some tips to get the best possible performance out of GTSAM. 1. Build in `Release` mode. GTSAM will run up to 10x faster compared to `Debug` mode. 2. Enable TBB. On modern processors with multiple cores, this can easily speed up - optimization by 30-50%. Please note that this may not be true for very small + optimization by 30-50%. Please note that this may not be true for very small problems where the overhead of dispatching work to multiple threads outweighs the benefit. We recommend that you benchmark your problem with/without TBB. 3. Use `GTSAM_BUILD_WITH_MARCH_NATIVE`. A performance gain of From af9db89ea9ce9e18dbcbfbb3791501ddaf8e6724 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Wed, 11 Dec 2024 13:10:31 -0500 Subject: [PATCH 06/80] Fixed math, use functor not macro --- gtsam/linear/KalmanFilter.cpp | 74 ++++++++++++++++++----------------- gtsam/linear/KalmanFilter.h | 2 +- 2 files changed, 40 insertions(+), 36 deletions(-) diff --git a/gtsam/linear/KalmanFilter.cpp b/gtsam/linear/KalmanFilter.cpp index 12a43093c..7c6e78527 100644 --- a/gtsam/linear/KalmanFilter.cpp +++ b/gtsam/linear/KalmanFilter.cpp @@ -25,22 +25,27 @@ #include #include -using namespace std; +#include "gtsam/base/Matrix.h" + +// In the code below we often get a covariance matrix Q, and we need to create a +// JacobianFactor with cost 0.5 * |Ax - b|^T Q^{-1} |Ax - b|. Factorizing Q as +// Q = L L^T +// and hence +// Q^{-1} = L^{-T} L^{-1} = M^T M, with M = L^{-1} +// We then have +// 0.5 * |Ax - b|^T Q^{-1} |Ax - b| +// = 0.5 * |Ax - b|^T M^T M |Ax - b| +// = 0.5 * |MAx - Mb|^T |MAx - Mb| +// The functor below efficiently multiplies with M by calling L.solve(). +namespace { +using Matrix = gtsam::Matrix; +struct InverseL : public Eigen::LLT { + InverseL(const Matrix& Q) : Eigen::LLT(Q) {} + Matrix operator*(const Matrix& A) const { return matrixL().solve(A); } +}; +} // namespace namespace gtsam { - -// In the code below we often need to multiply matrices with R, the Cholesky -// factor of the information matrix Q^{-1}, when given a covariance matrix Q. -// We can do this efficiently using Eigen, as we have: -// Q = L L^T -// => Q^{-1} = L^{-T}L^{-1} = R^T R -// => L^{-1} = R -// Rather than explicitly calculate R and do R*A, we should use L.solve(A) -// The following macro makes L available in the scope where it is used: -#define FACTORIZE_Q_INTO_L(Q) \ - Eigen::LLT llt(Q); \ - auto L = llt.matrixL(); - /* ************************************************************************* */ // Auxiliary function to solve factor graph and return pointer to root // conditional @@ -85,19 +90,19 @@ KalmanFilter::State KalmanFilter::init(const Vector& x0, // Create a factor graph f(x0), eliminate it into P(x0) GaussianFactorGraph factorGraph; - // Perform Cholesky decomposition of P0 - FACTORIZE_Q_INTO_L(P0) + // Perform Cholesky decomposition of P0 = LL^T + InverseL M(P0); - // Premultiply I and x0 with R - const Matrix R = L.solve(I_); // = R - const Vector b = L.solve(x0); // = R*x0 - factorGraph.emplace_shared(0, R, b); + // Premultiply I and x0 with M=L^{-1} + const Matrix A = M * I_; // = M + const Vector b = M * x0; // = M*x0 + factorGraph.emplace_shared(0, A, b); return solve(factorGraph); } /* ************************************************************************* */ -void KalmanFilter::print(const string& s) const { - cout << "KalmanFilter " << s << ", dim = " << n_ << endl; +void KalmanFilter::print(const std::string& s) const { + std::cout << "KalmanFilter " << s << ", dim = " << n_ << std::endl; } /* ************************************************************************* */ @@ -126,13 +131,13 @@ KalmanFilter::State KalmanFilter::predictQ(const State& p, const Matrix& F, assert(Q.cols() == n); #endif - // Perform Cholesky decomposition of Q - FACTORIZE_Q_INTO_L(Q) + // Perform Cholesky decomposition of Q = LL^T + InverseL M(Q); - // Premultiply A1 = -F and A2 = I, b = B * u with R - const Matrix A1 = -L.solve(F); // = -R*F - const Matrix A2 = L.solve(I_); // = R - const Vector b = L.solve(B * u); // = R * (B * u) + // Premultiply -F, I, and B * u with M=L^{-1} + const Matrix A1 = -(M * F); + const Matrix A2 = M * I_; + const Vector b = M * (B * u); return predict2(p, A1, A2, b); } @@ -163,14 +168,13 @@ KalmanFilter::State KalmanFilter::updateQ(const State& p, const Matrix& H, const Matrix& Q) const { Key k = step(p); - // Perform Cholesky decomposition of Q - FACTORIZE_Q_INTO_L(Q) + // Perform Cholesky decomposition of Q = LL^T + InverseL M(Q); - // Pre-multiply H and z with R, respectively - const Matrix RtH = L.solve(H); // = R * H - const Vector b = L.solve(z); // = R * z - - return fuse(p, std::make_shared(k, RtH, b)); + // Pre-multiply H and z with M=L^{-1}, respectively + const Matrix A = M * H; + const Vector b = M * z; + return fuse(p, std::make_shared(k, A, b)); } /* ************************************************************************* */ diff --git a/gtsam/linear/KalmanFilter.h b/gtsam/linear/KalmanFilter.h index 7fde9975d..2bf0549fa 100644 --- a/gtsam/linear/KalmanFilter.h +++ b/gtsam/linear/KalmanFilter.h @@ -214,4 +214,4 @@ class GTSAM_EXPORT KalmanFilter { size_t dim() const { return n_; } }; -} // namespace gtsam \ No newline at end of file +} // namespace gtsam From 2d0672059cc26ec3dd9ea58ba462f2b087eca25c Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Tue, 10 Dec 2024 13:41:19 -0500 Subject: [PATCH 07/80] Update some header includes --- gtsam/basis/Chebyshev2.cpp | 8 ++++++++ gtsam/basis/Chebyshev2.h | 10 +--------- gtsam/geometry/Pose2.h | 11 +++++++---- gtsam/geometry/Rot3.h | 7 +++++-- gtsam/geometry/triangulation.h | 7 +++++-- gtsam/inference/MetisIndex.h | 7 +++++-- gtsam/linear/HessianFactor.h | 18 ++++++++++++------ gtsam/nonlinear/ExpressionFactor.h | 4 ++-- gtsam/slam/EssentialMatrixFactor.h | 5 +++-- gtsam/slam/SmartFactorBase.h | 7 +++++-- 10 files changed, 53 insertions(+), 31 deletions(-) diff --git a/gtsam/basis/Chebyshev2.cpp b/gtsam/basis/Chebyshev2.cpp index 63fca64cc..71c3db7f0 100644 --- a/gtsam/basis/Chebyshev2.cpp +++ b/gtsam/basis/Chebyshev2.cpp @@ -20,6 +20,14 @@ namespace gtsam { +double Chebyshev2::Point(size_t N, int j, double a, double b) { + assert(j >= 0 && size_t(j) < N); + const double dtheta = M_PI / (N > 1 ? (N - 1) : 1); + // We add -PI so that we get values ordered from -1 to +1 + // sin(-M_PI_2 + dtheta*j); also works + return a + (b - a) * (1. + cos(-M_PI + dtheta * j)) / 2; +} + Weights Chebyshev2::CalculateWeights(size_t N, double x, double a, double b) { // Allocate space for weights Weights weights(N); diff --git a/gtsam/basis/Chebyshev2.h b/gtsam/basis/Chebyshev2.h index 1374faeef..207f4b617 100644 --- a/gtsam/basis/Chebyshev2.h +++ b/gtsam/basis/Chebyshev2.h @@ -36,8 +36,6 @@ #include #include -#include - namespace gtsam { /** @@ -63,13 +61,7 @@ class GTSAM_EXPORT Chebyshev2 : public Basis { * @param b Upper bound of interval (default: 1) * @return double */ - static double Point(size_t N, int j, double a = -1, double b = 1) { - assert(j >= 0 && size_t(j) < N); - const double dtheta = M_PI / (N > 1 ? (N - 1) : 1); - // We add -PI so that we get values ordered from -1 to +1 - // sin(-M_PI_2 + dtheta*j); also works - return a + (b - a) * (1. + cos(-M_PI + dtheta * j)) / 2; - } + static double Point(size_t N, int j, double a = -1, double b = 1); /// All Chebyshev points static Vector Points(size_t N) { diff --git a/gtsam/geometry/Pose2.h b/gtsam/geometry/Pose2.h index d9cad37f3..ff9a6399e 100644 --- a/gtsam/geometry/Pose2.h +++ b/gtsam/geometry/Pose2.h @@ -28,7 +28,6 @@ #include #include -#include namespace gtsam { @@ -82,9 +81,13 @@ public: Pose2(const Rot2& r, const Point2& t) : r_(r), t_(t) {} /** Constructor from 3*3 matrix */ - Pose2(const Matrix &T) : - r_(Rot2::atan2(T(1, 0), T(0, 0))), t_(T(0, 2), T(1, 2)) { - assert(T.rows() == 3 && T.cols() == 3); + Pose2(const Matrix &T) + : r_(Rot2::atan2(T(1, 0), T(0, 0))), t_(T(0, 2), T(1, 2)) { +#ifndef NDEBUG + if (T.rows() != 3 || T.cols() != 3) { + throw; + } +#endif } /// @} diff --git a/gtsam/geometry/Rot3.h b/gtsam/geometry/Rot3.h index 398e8d473..d1e330438 100644 --- a/gtsam/geometry/Rot3.h +++ b/gtsam/geometry/Rot3.h @@ -30,7 +30,6 @@ #include // Get GTSAM_USE_QUATERNIONS macro #include -#include // You can override the default coordinate mode using this flag #ifndef ROT3_DEFAULT_COORDINATES_MODE @@ -160,7 +159,11 @@ class GTSAM_EXPORT Rot3 : public LieGroup { /// Rotations around Z, Y, then X axes as in http://en.wikipedia.org/wiki/Rotation_matrix, counterclockwise when looking from unchanging axis. inline static Rot3 RzRyRx(const Vector& xyz, OptionalJacobian<3, 3> H = {}) { - assert(xyz.size() == 3); +#ifndef NDEBUG + if (xyz.size() != 3) { + throw; + } +#endif Rot3 out; if (H) { Vector3 Hx, Hy, Hz; diff --git a/gtsam/geometry/triangulation.h b/gtsam/geometry/triangulation.h index 0fa9d074e..9087f01c5 100644 --- a/gtsam/geometry/triangulation.h +++ b/gtsam/geometry/triangulation.h @@ -35,7 +35,6 @@ #include #include -#include namespace gtsam { @@ -318,7 +317,11 @@ typename CAMERA::MeasurementVector undistortMeasurements( const CameraSet& cameras, const typename CAMERA::MeasurementVector& measurements) { const size_t nrMeasurements = measurements.size(); - assert(nrMeasurements == cameras.size()); +#ifndef NDEBUG + if (nrMeasurements != cameras.size()) { + throw; + } +#endif typename CAMERA::MeasurementVector undistortedMeasurements(nrMeasurements); for (size_t ii = 0; ii < nrMeasurements; ++ii) { // Calibrate with cal and uncalibrate with pinhole version of cal so that diff --git a/gtsam/inference/MetisIndex.h b/gtsam/inference/MetisIndex.h index cc058dfbd..e051b9ae9 100644 --- a/gtsam/inference/MetisIndex.h +++ b/gtsam/inference/MetisIndex.h @@ -25,7 +25,6 @@ #include #include #include -#include namespace gtsam { /** @@ -93,7 +92,11 @@ public: return nKeys_; } Key intToKey(int32_t value) const { - assert(value >= 0); +#ifndef NDEBUG + if (value < 0) { + throw; + } +#endif return intKeyBMap_.right.find(value)->second; } diff --git a/gtsam/linear/HessianFactor.h b/gtsam/linear/HessianFactor.h index 53b6d0e8b..56b3d6537 100644 --- a/gtsam/linear/HessianFactor.h +++ b/gtsam/linear/HessianFactor.h @@ -23,8 +23,6 @@ #include #include -#include - namespace gtsam { // Forward declarations @@ -242,14 +240,18 @@ namespace gtsam { * use, for example, begin() + 2 to get the 3rd variable in this factor. * @return The linear term \f$ g \f$ */ SymmetricBlockMatrix::constBlock linearTerm(const_iterator j) const { - assert(!empty()); +#ifndef NDEBUG + if(empty()) throw; +#endif return info_.aboveDiagonalBlock(j - begin(), size()); } /** Return the complete linear term \f$ g \f$ as described above. * @return The linear term \f$ g \f$ */ SymmetricBlockMatrix::constBlock linearTerm() const { - assert(!empty()); +#ifndef NDEBUG + if(empty()) throw; +#endif // get the last column (except the bottom right block) return info_.aboveDiagonalRange(0, size(), size(), size() + 1); } @@ -257,7 +259,9 @@ namespace gtsam { /** Return the complete linear term \f$ g \f$ as described above. * @return The linear term \f$ g \f$ */ SymmetricBlockMatrix::Block linearTerm() { - assert(!empty()); +#ifndef NDEBUG + if(empty()) throw; +#endif return info_.aboveDiagonalRange(0, size(), size(), size() + 1); } @@ -326,7 +330,9 @@ namespace gtsam { * @param other the HessianFactor to be updated */ void updateHessian(HessianFactor* other) const { - assert(other); +#ifndef NDEBUG + if(!other) throw; +#endif updateHessian(other->keys_, &other->info_); } diff --git a/gtsam/nonlinear/ExpressionFactor.h b/gtsam/nonlinear/ExpressionFactor.h index b3e34d079..63b2469a9 100644 --- a/gtsam/nonlinear/ExpressionFactor.h +++ b/gtsam/nonlinear/ExpressionFactor.h @@ -43,7 +43,7 @@ namespace gtsam { * */ template -class ExpressionFactor : public NoiseModelFactor { +class GTSAM_EXPORT ExpressionFactor : public NoiseModelFactor { GTSAM_CONCEPT_ASSERT(IsTestable); protected: @@ -246,7 +246,7 @@ struct traits > : public Testable > {}; * */ template -class ExpressionFactorN : public ExpressionFactor { +class GTSAM_EXPORT ExpressionFactorN : public ExpressionFactor { public: static const std::size_t NARY_EXPRESSION_SIZE = sizeof...(Args); using ArrayNKeys = std::array; diff --git a/gtsam/slam/EssentialMatrixFactor.h b/gtsam/slam/EssentialMatrixFactor.h index 3c2f105d4..601f8e94e 100644 --- a/gtsam/slam/EssentialMatrixFactor.h +++ b/gtsam/slam/EssentialMatrixFactor.h @@ -25,7 +25,6 @@ #include #include -#include namespace gtsam { @@ -71,7 +70,9 @@ class EssentialMatrixFactor : public NoiseModelFactorN { const SharedNoiseModel& model, std::shared_ptr K) : Base(model, key) { - assert(K); +#ifndef NDEBUG + if (K->empty()) throw; +#endif vA_ = EssentialMatrix::Homogeneous(K->calibrate(pA)); vB_ = EssentialMatrix::Homogeneous(K->calibrate(pB)); } diff --git a/gtsam/slam/SmartFactorBase.h b/gtsam/slam/SmartFactorBase.h index b8e789aaa..d3e879c33 100644 --- a/gtsam/slam/SmartFactorBase.h +++ b/gtsam/slam/SmartFactorBase.h @@ -34,7 +34,6 @@ #include #endif #include -#include namespace gtsam { @@ -136,7 +135,11 @@ protected: /// Add a bunch of measurements, together with the camera keys. void add(const ZVector& measurements, const KeyVector& cameraKeys) { - assert(measurements.size() == cameraKeys.size()); +#ifndef NDEBUG + if (measurements.size() != cameraKeys.size()) { + throw std::runtime_error("Number of measurements and camera keys do not match"); + } +#endif for (size_t i = 0; i < measurements.size(); i++) { this->add(measurements[i], cameraKeys[i]); } From a1ba3d77c8dcc896223bcbe4f08b7c3f207e3c3c Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Tue, 10 Dec 2024 17:25:12 -0500 Subject: [PATCH 08/80] Fix essential matrix factor --- gtsam/slam/EssentialMatrixFactor.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/slam/EssentialMatrixFactor.h b/gtsam/slam/EssentialMatrixFactor.h index 601f8e94e..5d38cb234 100644 --- a/gtsam/slam/EssentialMatrixFactor.h +++ b/gtsam/slam/EssentialMatrixFactor.h @@ -71,7 +71,7 @@ class EssentialMatrixFactor : public NoiseModelFactorN { std::shared_ptr K) : Base(model, key) { #ifndef NDEBUG - if (K->empty()) throw; + if (!K) throw; #endif vA_ = EssentialMatrix::Homogeneous(K->calibrate(pA)); vB_ = EssentialMatrix::Homogeneous(K->calibrate(pB)); From b76c6d8250270f7359884f0db98ce937f02ae5a3 Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Wed, 11 Dec 2024 12:59:13 -0500 Subject: [PATCH 09/80] Fix ExpressionFactor wrong GTSAM_EXPORT --- gtsam/nonlinear/ExpressionFactor.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/gtsam/nonlinear/ExpressionFactor.h b/gtsam/nonlinear/ExpressionFactor.h index 63b2469a9..b3e34d079 100644 --- a/gtsam/nonlinear/ExpressionFactor.h +++ b/gtsam/nonlinear/ExpressionFactor.h @@ -43,7 +43,7 @@ namespace gtsam { * */ template -class GTSAM_EXPORT ExpressionFactor : public NoiseModelFactor { +class ExpressionFactor : public NoiseModelFactor { GTSAM_CONCEPT_ASSERT(IsTestable); protected: @@ -246,7 +246,7 @@ struct traits > : public Testable > {}; * */ template -class GTSAM_EXPORT ExpressionFactorN : public ExpressionFactor { +class ExpressionFactorN : public ExpressionFactor { public: static const std::size_t NARY_EXPRESSION_SIZE = sizeof...(Args); using ArrayNKeys = std::array; From ed098eaec64d185fbf91923b89e9442383b18979 Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Wed, 4 Dec 2024 11:43:51 -0500 Subject: [PATCH 10/80] Cleanup some includes and fix cassert --- examples/CombinedImuFactorsExample.cpp | 1 + examples/ImuFactorsExample.cpp | 1 + examples/elaboratePoint2KalmanFilter.cpp | 2 ++ gtsam/base/Matrix.cpp | 1 + gtsam/base/Vector.cpp | 1 + gtsam/base/VerticalBlockMatrix.h | 2 ++ gtsam/base/cholesky.cpp | 1 + gtsam/base/tests/testNumericalDerivative.cpp | 2 ++ gtsam/basis/Chebyshev2.h | 2 ++ gtsam/discrete/DiscreteConditional.cpp | 1 + gtsam/discrete/DiscreteLookupDAG.cpp | 1 + gtsam/geometry/Cal3f.cpp | 1 + gtsam/geometry/CameraSet.h | 1 + gtsam/geometry/Pose2.cpp | 1 + gtsam/geometry/Pose2.h | 1 + gtsam/geometry/Rot3.cpp | 1 + gtsam/geometry/Rot3.h | 1 + gtsam/geometry/SOn.h | 1 + gtsam/geometry/triangulation.h | 3 ++- gtsam/hybrid/HybridJunctionTree.cpp | 1 + gtsam/inference/BayesTree-inst.h | 2 +- gtsam/inference/ClusterTree-inst.h | 1 + gtsam/inference/EliminationTree-inst.h | 1 + gtsam/inference/JunctionTree-inst.h | 2 ++ gtsam/inference/MetisIndex.h | 1 + gtsam/inference/Ordering.cpp | 1 + gtsam/linear/HessianFactor.cpp | 3 ++- gtsam/linear/HessianFactor.h | 1 + gtsam/linear/JacobianFactor.cpp | 1 + gtsam/linear/KalmanFilter.cpp | 3 +++ gtsam/linear/NoiseModel.cpp | 1 + gtsam/linear/Sampler.cpp | 3 +++ gtsam/linear/SubgraphPreconditioner.cpp | 1 + gtsam/navigation/ImuFactor.cpp | 1 + gtsam/navigation/PreintegrationBase.cpp | 4 +++- gtsam/nonlinear/DoglegOptimizerImpl.cpp | 1 + gtsam/nonlinear/DoglegOptimizerImpl.h | 1 + gtsam/nonlinear/Expression-inl.h | 1 + gtsam/nonlinear/ExtendedKalmanFilter-inl.h | 2 ++ gtsam/nonlinear/ISAM2-impl.cpp | 1 + gtsam/nonlinear/ISAM2-impl.h | 1 + gtsam/nonlinear/ISAM2.cpp | 1 + gtsam/nonlinear/ISAM2Clique.cpp | 1 + gtsam/nonlinear/NonlinearFactor.cpp | 2 ++ gtsam/nonlinear/Values.cpp | 1 + gtsam/nonlinear/internal/ExpressionNode.h | 1 + gtsam/sfm/ShonanAveraging.cpp | 1 + gtsam/slam/EssentialMatrixFactor.h | 1 + gtsam/slam/SmartFactorBase.h | 1 + gtsam_unstable/discrete/Scheduler.cpp | 1 + gtsam_unstable/dynamics/DynamicsPriors.h | 2 ++ gtsam_unstable/dynamics/FullIMUFactor.h | 2 ++ gtsam_unstable/dynamics/IMUFactor.h | 2 ++ gtsam_unstable/dynamics/PoseRTV.cpp | 2 ++ gtsam_unstable/dynamics/VelocityConstraint.h | 2 ++ gtsam_unstable/geometry/BearingS2.cpp | 2 ++ gtsam_unstable/geometry/Pose3Upright.cpp | 5 +++-- gtsam_unstable/nonlinear/ConcurrentBatchFilter.cpp | 2 ++ gtsam_unstable/nonlinear/LinearizedFactor.cpp | 1 + gtsam_unstable/partition/FindSeparator-inl.h | 1 + gtsam_unstable/slam/PartialPriorFactor.h | 2 ++ gtsam_unstable/slam/SmartStereoProjectionFactorPP.cpp | 2 ++ gtsam_unstable/slam/SmartStereoProjectionPoseFactor.cpp | 2 ++ tests/testGaussianISAM2.cpp | 1 + 64 files changed, 91 insertions(+), 6 deletions(-) diff --git a/examples/CombinedImuFactorsExample.cpp b/examples/CombinedImuFactorsExample.cpp index 6ce4f76fa..436b6e283 100644 --- a/examples/CombinedImuFactorsExample.cpp +++ b/examples/CombinedImuFactorsExample.cpp @@ -48,6 +48,7 @@ #include #include #include +#include using namespace gtsam; using namespace std; diff --git a/examples/ImuFactorsExample.cpp b/examples/ImuFactorsExample.cpp index 342f1f220..7cbc8989f 100644 --- a/examples/ImuFactorsExample.cpp +++ b/examples/ImuFactorsExample.cpp @@ -49,6 +49,7 @@ #include #include +#include #include #include diff --git a/examples/elaboratePoint2KalmanFilter.cpp b/examples/elaboratePoint2KalmanFilter.cpp index a2ad8cf0b..bc4e6e340 100644 --- a/examples/elaboratePoint2KalmanFilter.cpp +++ b/examples/elaboratePoint2KalmanFilter.cpp @@ -29,6 +29,8 @@ #include #include +#include + using namespace std; using namespace gtsam; diff --git a/gtsam/base/Matrix.cpp b/gtsam/base/Matrix.cpp index 247c53cce..1e04bc58a 100644 --- a/gtsam/base/Matrix.cpp +++ b/gtsam/base/Matrix.cpp @@ -24,6 +24,7 @@ #include #include +#include #include #include #include diff --git a/gtsam/base/Vector.cpp b/gtsam/base/Vector.cpp index ce7aeba7b..dbd48dfc8 100644 --- a/gtsam/base/Vector.cpp +++ b/gtsam/base/Vector.cpp @@ -24,6 +24,7 @@ #include #include #include +#include #include #include #include diff --git a/gtsam/base/VerticalBlockMatrix.h b/gtsam/base/VerticalBlockMatrix.h index 777441300..c8fef76e9 100644 --- a/gtsam/base/VerticalBlockMatrix.h +++ b/gtsam/base/VerticalBlockMatrix.h @@ -21,6 +21,8 @@ #include #include +#include + namespace gtsam { // Forward declarations diff --git a/gtsam/base/cholesky.cpp b/gtsam/base/cholesky.cpp index 71cef2524..9fee627fe 100644 --- a/gtsam/base/cholesky.cpp +++ b/gtsam/base/cholesky.cpp @@ -21,6 +21,7 @@ #include #include +#include using namespace std; diff --git a/gtsam/base/tests/testNumericalDerivative.cpp b/gtsam/base/tests/testNumericalDerivative.cpp index e8838a476..b14f699a3 100644 --- a/gtsam/base/tests/testNumericalDerivative.cpp +++ b/gtsam/base/tests/testNumericalDerivative.cpp @@ -18,6 +18,8 @@ #include #include +#include + using namespace std; using namespace gtsam; diff --git a/gtsam/basis/Chebyshev2.h b/gtsam/basis/Chebyshev2.h index 849a51104..1374faeef 100644 --- a/gtsam/basis/Chebyshev2.h +++ b/gtsam/basis/Chebyshev2.h @@ -36,6 +36,8 @@ #include #include +#include + namespace gtsam { /** diff --git a/gtsam/discrete/DiscreteConditional.cpp b/gtsam/discrete/DiscreteConditional.cpp index c44905d79..eeb5dca3f 100644 --- a/gtsam/discrete/DiscreteConditional.cpp +++ b/gtsam/discrete/DiscreteConditional.cpp @@ -30,6 +30,7 @@ #include #include #include +#include using namespace std; using std::pair; diff --git a/gtsam/discrete/DiscreteLookupDAG.cpp b/gtsam/discrete/DiscreteLookupDAG.cpp index 4d02e007b..d1108e7b7 100644 --- a/gtsam/discrete/DiscreteLookupDAG.cpp +++ b/gtsam/discrete/DiscreteLookupDAG.cpp @@ -23,6 +23,7 @@ #include #include #include +#include using std::pair; using std::vector; diff --git a/gtsam/geometry/Cal3f.cpp b/gtsam/geometry/Cal3f.cpp index 319155dc9..b8e365eed 100644 --- a/gtsam/geometry/Cal3f.cpp +++ b/gtsam/geometry/Cal3f.cpp @@ -28,6 +28,7 @@ #include #include +#include namespace gtsam { diff --git a/gtsam/geometry/CameraSet.h b/gtsam/geometry/CameraSet.h index 26d4952c8..e3c16f066 100644 --- a/gtsam/geometry/CameraSet.h +++ b/gtsam/geometry/CameraSet.h @@ -26,6 +26,7 @@ #include #include +#include namespace gtsam { diff --git a/gtsam/geometry/Pose2.cpp b/gtsam/geometry/Pose2.cpp index 05678dc27..640481406 100644 --- a/gtsam/geometry/Pose2.cpp +++ b/gtsam/geometry/Pose2.cpp @@ -20,6 +20,7 @@ #include #include +#include #include #include diff --git a/gtsam/geometry/Pose2.h b/gtsam/geometry/Pose2.h index aad81493f..d9cad37f3 100644 --- a/gtsam/geometry/Pose2.h +++ b/gtsam/geometry/Pose2.h @@ -28,6 +28,7 @@ #include #include +#include namespace gtsam { diff --git a/gtsam/geometry/Rot3.cpp b/gtsam/geometry/Rot3.cpp index a936bd02a..10ba5894e 100644 --- a/gtsam/geometry/Rot3.cpp +++ b/gtsam/geometry/Rot3.cpp @@ -23,6 +23,7 @@ #include #include +#include #include using namespace std; diff --git a/gtsam/geometry/Rot3.h b/gtsam/geometry/Rot3.h index 7e05ee4da..398e8d473 100644 --- a/gtsam/geometry/Rot3.h +++ b/gtsam/geometry/Rot3.h @@ -30,6 +30,7 @@ #include // Get GTSAM_USE_QUATERNIONS macro #include +#include // You can override the default coordinate mode using this flag #ifndef ROT3_DEFAULT_COORDINATES_MODE diff --git a/gtsam/geometry/SOn.h b/gtsam/geometry/SOn.h index 7e1762b3f..75ff872a8 100644 --- a/gtsam/geometry/SOn.h +++ b/gtsam/geometry/SOn.h @@ -33,6 +33,7 @@ #include #include #include +#include namespace gtsam { diff --git a/gtsam/geometry/triangulation.h b/gtsam/geometry/triangulation.h index d82da3459..0fa9d074e 100644 --- a/gtsam/geometry/triangulation.h +++ b/gtsam/geometry/triangulation.h @@ -20,7 +20,7 @@ #pragma once -#include "gtsam/geometry/Point3.h" +#include #include #include #include @@ -35,6 +35,7 @@ #include #include +#include namespace gtsam { diff --git a/gtsam/hybrid/HybridJunctionTree.cpp b/gtsam/hybrid/HybridJunctionTree.cpp index 22d3c7dd2..b9bdcbed2 100644 --- a/gtsam/hybrid/HybridJunctionTree.cpp +++ b/gtsam/hybrid/HybridJunctionTree.cpp @@ -22,6 +22,7 @@ #include #include +#include namespace gtsam { diff --git a/gtsam/inference/BayesTree-inst.h b/gtsam/inference/BayesTree-inst.h index 95d475a02..fdfa94d2b 100644 --- a/gtsam/inference/BayesTree-inst.h +++ b/gtsam/inference/BayesTree-inst.h @@ -27,7 +27,7 @@ #include #include - +#include namespace gtsam { /* ************************************************************************* */ diff --git a/gtsam/inference/ClusterTree-inst.h b/gtsam/inference/ClusterTree-inst.h index 318624608..2a5f0b455 100644 --- a/gtsam/inference/ClusterTree-inst.h +++ b/gtsam/inference/ClusterTree-inst.h @@ -19,6 +19,7 @@ #include #endif #include +#include namespace gtsam { diff --git a/gtsam/inference/EliminationTree-inst.h b/gtsam/inference/EliminationTree-inst.h index e9acc12e1..16449d0f4 100644 --- a/gtsam/inference/EliminationTree-inst.h +++ b/gtsam/inference/EliminationTree-inst.h @@ -19,6 +19,7 @@ #include #include +#include #include #include diff --git a/gtsam/inference/JunctionTree-inst.h b/gtsam/inference/JunctionTree-inst.h index 0f79c2a9a..6a8d6918a 100644 --- a/gtsam/inference/JunctionTree-inst.h +++ b/gtsam/inference/JunctionTree-inst.h @@ -25,6 +25,8 @@ #include #include +#include + namespace gtsam { template diff --git a/gtsam/inference/MetisIndex.h b/gtsam/inference/MetisIndex.h index abdf11c5f..cc058dfbd 100644 --- a/gtsam/inference/MetisIndex.h +++ b/gtsam/inference/MetisIndex.h @@ -25,6 +25,7 @@ #include #include #include +#include namespace gtsam { /** diff --git a/gtsam/inference/Ordering.cpp b/gtsam/inference/Ordering.cpp index cb2ca752d..95eb17a1c 100644 --- a/gtsam/inference/Ordering.cpp +++ b/gtsam/inference/Ordering.cpp @@ -18,6 +18,7 @@ #include #include +#include #include #include diff --git a/gtsam/linear/HessianFactor.cpp b/gtsam/linear/HessianFactor.cpp index 3c3050f90..4e8ef804c 100644 --- a/gtsam/linear/HessianFactor.cpp +++ b/gtsam/linear/HessianFactor.cpp @@ -25,13 +25,14 @@ #include #include #include +#include #include #include #include #include +#include #include -#include "gtsam/base/Vector.h" using namespace std; diff --git a/gtsam/linear/HessianFactor.h b/gtsam/linear/HessianFactor.h index 8cbabcd5d..53b6d0e8b 100644 --- a/gtsam/linear/HessianFactor.h +++ b/gtsam/linear/HessianFactor.h @@ -23,6 +23,7 @@ #include #include +#include namespace gtsam { diff --git a/gtsam/linear/JacobianFactor.cpp b/gtsam/linear/JacobianFactor.cpp index 3cfb5ce7b..51d513e33 100644 --- a/gtsam/linear/JacobianFactor.cpp +++ b/gtsam/linear/JacobianFactor.cpp @@ -32,6 +32,7 @@ #include #include +#include #include #include diff --git a/gtsam/linear/KalmanFilter.cpp b/gtsam/linear/KalmanFilter.cpp index ded6bc5e3..6291c90b6 100644 --- a/gtsam/linear/KalmanFilter.cpp +++ b/gtsam/linear/KalmanFilter.cpp @@ -26,6 +26,9 @@ #include #include +#ifndef NDEBUG +#include +#endif using namespace std; diff --git a/gtsam/linear/NoiseModel.cpp b/gtsam/linear/NoiseModel.cpp index 884c87270..39666b46b 100644 --- a/gtsam/linear/NoiseModel.cpp +++ b/gtsam/linear/NoiseModel.cpp @@ -20,6 +20,7 @@ #include #include +#include #include #include #include diff --git a/gtsam/linear/Sampler.cpp b/gtsam/linear/Sampler.cpp index 20d4c955b..68f21d723 100644 --- a/gtsam/linear/Sampler.cpp +++ b/gtsam/linear/Sampler.cpp @@ -17,6 +17,9 @@ */ #include + +#include + namespace gtsam { /* ************************************************************************* */ diff --git a/gtsam/linear/SubgraphPreconditioner.cpp b/gtsam/linear/SubgraphPreconditioner.cpp index 96f4847b5..53ea94d6e 100644 --- a/gtsam/linear/SubgraphPreconditioner.cpp +++ b/gtsam/linear/SubgraphPreconditioner.cpp @@ -25,6 +25,7 @@ #include #include +#include using std::cout; using std::endl; diff --git a/gtsam/navigation/ImuFactor.cpp b/gtsam/navigation/ImuFactor.cpp index 88e45d318..5697d54d4 100644 --- a/gtsam/navigation/ImuFactor.cpp +++ b/gtsam/navigation/ImuFactor.cpp @@ -23,6 +23,7 @@ /* External or standard includes */ #include +#include namespace gtsam { diff --git a/gtsam/navigation/PreintegrationBase.cpp b/gtsam/navigation/PreintegrationBase.cpp index 7f998987b..dd86b829c 100644 --- a/gtsam/navigation/PreintegrationBase.cpp +++ b/gtsam/navigation/PreintegrationBase.cpp @@ -20,9 +20,11 @@ * @author Varun Agrawal **/ -#include "PreintegrationBase.h" +#include #include +#include + using namespace std; namespace gtsam { diff --git a/gtsam/nonlinear/DoglegOptimizerImpl.cpp b/gtsam/nonlinear/DoglegOptimizerImpl.cpp index 4f1c6fb54..e82a513ca 100644 --- a/gtsam/nonlinear/DoglegOptimizerImpl.cpp +++ b/gtsam/nonlinear/DoglegOptimizerImpl.cpp @@ -16,6 +16,7 @@ */ #include +#include #include using namespace std; diff --git a/gtsam/nonlinear/DoglegOptimizerImpl.h b/gtsam/nonlinear/DoglegOptimizerImpl.h index 8ce9b361e..1038dd522 100644 --- a/gtsam/nonlinear/DoglegOptimizerImpl.h +++ b/gtsam/nonlinear/DoglegOptimizerImpl.h @@ -17,6 +17,7 @@ #pragma once #include +#include #include #include diff --git a/gtsam/nonlinear/Expression-inl.h b/gtsam/nonlinear/Expression-inl.h index 71ef1d840..b09bcb3a5 100644 --- a/gtsam/nonlinear/Expression-inl.h +++ b/gtsam/nonlinear/Expression-inl.h @@ -26,6 +26,7 @@ #include #include #include +#include namespace gtsam { diff --git a/gtsam/nonlinear/ExtendedKalmanFilter-inl.h b/gtsam/nonlinear/ExtendedKalmanFilter-inl.h index 5bf643c12..072f40b58 100644 --- a/gtsam/nonlinear/ExtendedKalmanFilter-inl.h +++ b/gtsam/nonlinear/ExtendedKalmanFilter-inl.h @@ -23,6 +23,8 @@ #include #include +#include + namespace gtsam { /* ************************************************************************* */ diff --git a/gtsam/nonlinear/ISAM2-impl.cpp b/gtsam/nonlinear/ISAM2-impl.cpp index 20874e2dc..d0b27f3f3 100644 --- a/gtsam/nonlinear/ISAM2-impl.cpp +++ b/gtsam/nonlinear/ISAM2-impl.cpp @@ -25,6 +25,7 @@ #include #include #include +#include using namespace std; diff --git a/gtsam/nonlinear/ISAM2-impl.h b/gtsam/nonlinear/ISAM2-impl.h index e9a9696eb..bb0210237 100644 --- a/gtsam/nonlinear/ISAM2-impl.h +++ b/gtsam/nonlinear/ISAM2-impl.h @@ -33,6 +33,7 @@ #include #include #include +#include namespace gtsam { diff --git a/gtsam/nonlinear/ISAM2.cpp b/gtsam/nonlinear/ISAM2.cpp index 47857a2a2..0eb2deef5 100644 --- a/gtsam/nonlinear/ISAM2.cpp +++ b/gtsam/nonlinear/ISAM2.cpp @@ -29,6 +29,7 @@ #include #include #include +#include using namespace std; diff --git a/gtsam/nonlinear/ISAM2Clique.cpp b/gtsam/nonlinear/ISAM2Clique.cpp index d4b050f84..64ab7c66e 100644 --- a/gtsam/nonlinear/ISAM2Clique.cpp +++ b/gtsam/nonlinear/ISAM2Clique.cpp @@ -22,6 +22,7 @@ #include #include +#include using namespace std; diff --git a/gtsam/nonlinear/NonlinearFactor.cpp b/gtsam/nonlinear/NonlinearFactor.cpp index 7312bf6d9..5eabfac91 100644 --- a/gtsam/nonlinear/NonlinearFactor.cpp +++ b/gtsam/nonlinear/NonlinearFactor.cpp @@ -19,6 +19,8 @@ #include #include +#include + namespace gtsam { /* ************************************************************************* */ diff --git a/gtsam/nonlinear/Values.cpp b/gtsam/nonlinear/Values.cpp index 6af9f2b6a..1caaa9db1 100644 --- a/gtsam/nonlinear/Values.cpp +++ b/gtsam/nonlinear/Values.cpp @@ -28,6 +28,7 @@ #include #include #include +#include using namespace std; diff --git a/gtsam/nonlinear/internal/ExpressionNode.h b/gtsam/nonlinear/internal/ExpressionNode.h index de0c9721e..25587f511 100644 --- a/gtsam/nonlinear/internal/ExpressionNode.h +++ b/gtsam/nonlinear/internal/ExpressionNode.h @@ -26,6 +26,7 @@ #include // operator typeid #include #include +#include class ExpressionFactorBinaryTest; // Forward declare for testing diff --git a/gtsam/sfm/ShonanAveraging.cpp b/gtsam/sfm/ShonanAveraging.cpp index 830f1c719..6c9f91e90 100644 --- a/gtsam/sfm/ShonanAveraging.cpp +++ b/gtsam/sfm/ShonanAveraging.cpp @@ -37,6 +37,7 @@ #include #include #include +#include namespace gtsam { diff --git a/gtsam/slam/EssentialMatrixFactor.h b/gtsam/slam/EssentialMatrixFactor.h index 5fdf138cc..3c2f105d4 100644 --- a/gtsam/slam/EssentialMatrixFactor.h +++ b/gtsam/slam/EssentialMatrixFactor.h @@ -25,6 +25,7 @@ #include #include +#include namespace gtsam { diff --git a/gtsam/slam/SmartFactorBase.h b/gtsam/slam/SmartFactorBase.h index 5d6ec4445..b8e789aaa 100644 --- a/gtsam/slam/SmartFactorBase.h +++ b/gtsam/slam/SmartFactorBase.h @@ -34,6 +34,7 @@ #include #endif #include +#include namespace gtsam { diff --git a/gtsam_unstable/discrete/Scheduler.cpp b/gtsam_unstable/discrete/Scheduler.cpp index 0de4d32c4..53be25d55 100644 --- a/gtsam_unstable/discrete/Scheduler.cpp +++ b/gtsam_unstable/discrete/Scheduler.cpp @@ -14,6 +14,7 @@ #include #include #include +#include namespace gtsam { diff --git a/gtsam_unstable/dynamics/DynamicsPriors.h b/gtsam_unstable/dynamics/DynamicsPriors.h index d4ebcba19..2dc0d552b 100644 --- a/gtsam_unstable/dynamics/DynamicsPriors.h +++ b/gtsam_unstable/dynamics/DynamicsPriors.h @@ -12,6 +12,8 @@ #include #include +#include + namespace gtsam { // Indices of relevant variables in the PoseRTV tangent vector: diff --git a/gtsam_unstable/dynamics/FullIMUFactor.h b/gtsam_unstable/dynamics/FullIMUFactor.h index 02da899b7..eea019cd2 100644 --- a/gtsam_unstable/dynamics/FullIMUFactor.h +++ b/gtsam_unstable/dynamics/FullIMUFactor.h @@ -10,6 +10,8 @@ #include #include +#include + namespace gtsam { /** diff --git a/gtsam_unstable/dynamics/IMUFactor.h b/gtsam_unstable/dynamics/IMUFactor.h index ee09600e5..241b6b497 100644 --- a/gtsam_unstable/dynamics/IMUFactor.h +++ b/gtsam_unstable/dynamics/IMUFactor.h @@ -10,6 +10,8 @@ #include #include +#include + namespace gtsam { /** diff --git a/gtsam_unstable/dynamics/PoseRTV.cpp b/gtsam_unstable/dynamics/PoseRTV.cpp index cf21c315b..9457c99e8 100644 --- a/gtsam_unstable/dynamics/PoseRTV.cpp +++ b/gtsam_unstable/dynamics/PoseRTV.cpp @@ -7,6 +7,8 @@ #include #include +#include + namespace gtsam { using namespace std; diff --git a/gtsam_unstable/dynamics/VelocityConstraint.h b/gtsam_unstable/dynamics/VelocityConstraint.h index 2bb70e1b5..dbdd27259 100644 --- a/gtsam_unstable/dynamics/VelocityConstraint.h +++ b/gtsam_unstable/dynamics/VelocityConstraint.h @@ -10,6 +10,8 @@ #include #include +#include + namespace gtsam { namespace dynamics { diff --git a/gtsam_unstable/geometry/BearingS2.cpp b/gtsam_unstable/geometry/BearingS2.cpp index 9dfed612c..23afd158b 100644 --- a/gtsam_unstable/geometry/BearingS2.cpp +++ b/gtsam_unstable/geometry/BearingS2.cpp @@ -9,6 +9,8 @@ #include +#include + namespace gtsam { using namespace std; diff --git a/gtsam_unstable/geometry/Pose3Upright.cpp b/gtsam_unstable/geometry/Pose3Upright.cpp index 9cd9f78f5..c5ef0d70f 100644 --- a/gtsam_unstable/geometry/Pose3Upright.cpp +++ b/gtsam_unstable/geometry/Pose3Upright.cpp @@ -6,8 +6,9 @@ */ #include -#include "gtsam/base/OptionalJacobian.h" -#include "gtsam/base/Vector.h" +#include +#include +#include #include diff --git a/gtsam_unstable/nonlinear/ConcurrentBatchFilter.cpp b/gtsam_unstable/nonlinear/ConcurrentBatchFilter.cpp index 36cb6165f..8dfb15aa2 100644 --- a/gtsam_unstable/nonlinear/ConcurrentBatchFilter.cpp +++ b/gtsam_unstable/nonlinear/ConcurrentBatchFilter.cpp @@ -21,6 +21,8 @@ #include #include +#include + namespace gtsam { /* ************************************************************************* */ diff --git a/gtsam_unstable/nonlinear/LinearizedFactor.cpp b/gtsam_unstable/nonlinear/LinearizedFactor.cpp index 2d404005d..375c49341 100644 --- a/gtsam_unstable/nonlinear/LinearizedFactor.cpp +++ b/gtsam_unstable/nonlinear/LinearizedFactor.cpp @@ -17,6 +17,7 @@ #include #include +#include namespace gtsam { diff --git a/gtsam_unstable/partition/FindSeparator-inl.h b/gtsam_unstable/partition/FindSeparator-inl.h index f4718f7a9..21ed91604 100644 --- a/gtsam_unstable/partition/FindSeparator-inl.h +++ b/gtsam_unstable/partition/FindSeparator-inl.h @@ -14,6 +14,7 @@ #include #include #include +#include #include #include diff --git a/gtsam_unstable/slam/PartialPriorFactor.h b/gtsam_unstable/slam/PartialPriorFactor.h index 7722e5d82..0a735a2e1 100644 --- a/gtsam_unstable/slam/PartialPriorFactor.h +++ b/gtsam_unstable/slam/PartialPriorFactor.h @@ -20,6 +20,8 @@ #include #include +#include + namespace gtsam { /** diff --git a/gtsam_unstable/slam/SmartStereoProjectionFactorPP.cpp b/gtsam_unstable/slam/SmartStereoProjectionFactorPP.cpp index c4a473154..6d78bdb7a 100644 --- a/gtsam_unstable/slam/SmartStereoProjectionFactorPP.cpp +++ b/gtsam_unstable/slam/SmartStereoProjectionFactorPP.cpp @@ -18,6 +18,8 @@ #include +#include + namespace gtsam { SmartStereoProjectionFactorPP::SmartStereoProjectionFactorPP( diff --git a/gtsam_unstable/slam/SmartStereoProjectionPoseFactor.cpp b/gtsam_unstable/slam/SmartStereoProjectionPoseFactor.cpp index 57913385a..52129daa8 100644 --- a/gtsam_unstable/slam/SmartStereoProjectionPoseFactor.cpp +++ b/gtsam_unstable/slam/SmartStereoProjectionPoseFactor.cpp @@ -21,6 +21,8 @@ #include +#include + namespace gtsam { SmartStereoProjectionPoseFactor::SmartStereoProjectionPoseFactor( diff --git a/tests/testGaussianISAM2.cpp b/tests/testGaussianISAM2.cpp index 721ed51c0..3b507d806 100644 --- a/tests/testGaussianISAM2.cpp +++ b/tests/testGaussianISAM2.cpp @@ -25,6 +25,7 @@ #include +#include using namespace std; using namespace gtsam; From 425c3ac42c10cef214839416f1427362a2f5e32c Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Wed, 11 Dec 2024 15:00:53 -0500 Subject: [PATCH 11/80] check if input size matches the total cardinality of the keys --- gtsam/discrete/TableFactor.cpp | 20 ++++++++++++++++---- gtsam/discrete/TableFactor.h | 16 ++++++++++------ 2 files changed, 26 insertions(+), 10 deletions(-) diff --git a/gtsam/discrete/TableFactor.cpp b/gtsam/discrete/TableFactor.cpp index ea51a996c..ea51fa550 100644 --- a/gtsam/discrete/TableFactor.cpp +++ b/gtsam/discrete/TableFactor.cpp @@ -98,7 +98,17 @@ TableFactor::TableFactor(const DiscreteConditional& c) /* ************************************************************************ */ Eigen::SparseVector TableFactor::Convert( - const std::vector& table) { + const DiscreteKeys& keys, const std::vector& table) { + size_t max_size = 1; + for (auto&& [_, cardinality] : keys.cardinalities()) { + max_size *= cardinality; + } + if (table.size() != max_size) { + throw std::runtime_error( + "The cardinalities of the keys don't match the number of values in the " + "input."); + } + Eigen::SparseVector sparse_table(table.size()); // Count number of nonzero elements in table and reserve the space. const uint64_t nnz = std::count_if(table.begin(), table.end(), @@ -113,13 +123,14 @@ Eigen::SparseVector TableFactor::Convert( } /* ************************************************************************ */ -Eigen::SparseVector TableFactor::Convert(const std::string& table) { +Eigen::SparseVector TableFactor::Convert(const DiscreteKeys& keys, + const std::string& table) { // Convert string to doubles. std::vector ys; std::istringstream iss(table); std::copy(std::istream_iterator(iss), std::istream_iterator(), std::back_inserter(ys)); - return Convert(ys); + return Convert(keys, ys); } /* ************************************************************************ */ @@ -250,7 +261,8 @@ void TableFactor::print(const string& s, const KeyFormatter& formatter) const { for (auto&& kv : assignment) { cout << "(" << formatter(kv.first) << ", " << kv.second << ")"; } - cout << " | " << it.value() << " | " << it.index() << endl; + cout << " | " << std::setw(10) << std::left << it.value() << " | " + << it.index() << endl; } cout << "number of nnzs: " << sparse_table_.nonZeros() << endl; } diff --git a/gtsam/discrete/TableFactor.h b/gtsam/discrete/TableFactor.h index 07564892f..ee6f456db 100644 --- a/gtsam/discrete/TableFactor.h +++ b/gtsam/discrete/TableFactor.h @@ -80,12 +80,16 @@ class GTSAM_EXPORT TableFactor : public DiscreteFactor { return DiscreteKey(keys_[i], cardinalities_.at(keys_[i])); } - /// Convert probability table given as doubles to SparseVector. - /// Example) {0, 1, 1, 0, 0, 1, 0} -> values: {1, 1, 1}, indices: {1, 2, 5} - static Eigen::SparseVector Convert(const std::vector& table); + /** + * Convert probability table given as doubles to SparseVector. + * Example: {0, 1, 1, 0, 0, 1, 0} -> values: {1, 1, 1}, indices: {1, 2, 5} + */ + static Eigen::SparseVector Convert(const DiscreteKeys& keys, + const std::vector& table); /// Convert probability table given as string to SparseVector. - static Eigen::SparseVector Convert(const std::string& table); + static Eigen::SparseVector Convert(const DiscreteKeys& keys, + const std::string& table); public: // typedefs needed to play nice with gtsam @@ -111,11 +115,11 @@ class GTSAM_EXPORT TableFactor : public DiscreteFactor { /** Constructor from doubles */ TableFactor(const DiscreteKeys& keys, const std::vector& table) - : TableFactor(keys, Convert(table)) {} + : TableFactor(keys, Convert(keys, table)) {} /** Constructor from string */ TableFactor(const DiscreteKeys& keys, const std::string& table) - : TableFactor(keys, Convert(table)) {} + : TableFactor(keys, Convert(keys, table)) {} /// Single-key specialization template From b5cd82d0b4d81ec33586f55a944be3428cdc3da3 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Wed, 11 Dec 2024 15:01:30 -0500 Subject: [PATCH 12/80] helper constructor that doesn't take separate keys --- gtsam/discrete/TableFactor.cpp | 5 +++++ gtsam/discrete/TableFactor.h | 1 + 2 files changed, 6 insertions(+) diff --git a/gtsam/discrete/TableFactor.cpp b/gtsam/discrete/TableFactor.cpp index ea51fa550..486641e3f 100644 --- a/gtsam/discrete/TableFactor.cpp +++ b/gtsam/discrete/TableFactor.cpp @@ -92,6 +92,11 @@ TableFactor::TableFactor(const DiscreteKeys& dkeys, const DecisionTreeFactor& dtf) : TableFactor(dkeys, ComputeLeafOrdering(dkeys, dtf)) {} +/* ************************************************************************ */ +TableFactor::TableFactor(const DecisionTreeFactor& dtf) + : TableFactor(dtf.discreteKeys(), + ComputeLeafOrdering(dtf.discreteKeys(), dtf)) {} + /* ************************************************************************ */ TableFactor::TableFactor(const DiscreteConditional& c) : TableFactor(c.discreteKeys(), c) {} diff --git a/gtsam/discrete/TableFactor.h b/gtsam/discrete/TableFactor.h index ee6f456db..d27c4740c 100644 --- a/gtsam/discrete/TableFactor.h +++ b/gtsam/discrete/TableFactor.h @@ -132,6 +132,7 @@ class GTSAM_EXPORT TableFactor : public DiscreteFactor { /// Constructor from DecisionTreeFactor TableFactor(const DiscreteKeys& keys, const DecisionTreeFactor& dtf); + TableFactor(const DecisionTreeFactor& dtf); /// Constructor from DecisionTree/AlgebraicDecisionTree TableFactor(const DiscreteKeys& keys, const DecisionTree& dtree); From ab3f48bbe9b2da13ebc048350bc8273219d4ac88 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Wed, 11 Dec 2024 15:36:25 -0500 Subject: [PATCH 13/80] fix equality to check for matching keys --- gtsam/discrete/DecisionTreeFactor.cpp | 2 +- gtsam/discrete/DiscreteFactor.cpp | 5 +++++ gtsam/discrete/DiscreteFactor.h | 2 +- gtsam/discrete/TableFactor.cpp | 3 ++- gtsam/discrete/tests/testTableFactor.cpp | 13 ++++++++----- 5 files changed, 17 insertions(+), 8 deletions(-) diff --git a/gtsam/discrete/DecisionTreeFactor.cpp b/gtsam/discrete/DecisionTreeFactor.cpp index f68d2ae00..1ac782b88 100644 --- a/gtsam/discrete/DecisionTreeFactor.cpp +++ b/gtsam/discrete/DecisionTreeFactor.cpp @@ -48,7 +48,7 @@ namespace gtsam { return false; } else { const auto& f(static_cast(other)); - return ADT::equals(f, tol); + return Base::equals(other, tol) && ADT::equals(f, tol); } } diff --git a/gtsam/discrete/DiscreteFactor.cpp b/gtsam/discrete/DiscreteFactor.cpp index 2b11046f4..68cc1df7d 100644 --- a/gtsam/discrete/DiscreteFactor.cpp +++ b/gtsam/discrete/DiscreteFactor.cpp @@ -28,6 +28,11 @@ using namespace std; namespace gtsam { +/* ************************************************************************* */ +bool DiscreteFactor::equals(const DiscreteFactor& lf, double tol) const { + return Base::equals(lf, tol) && cardinalities_ == lf.cardinalities_; +} + /* ************************************************************************ */ DiscreteKeys DiscreteFactor::discreteKeys() const { DiscreteKeys result; diff --git a/gtsam/discrete/DiscreteFactor.h b/gtsam/discrete/DiscreteFactor.h index fa1179c39..29981e94b 100644 --- a/gtsam/discrete/DiscreteFactor.h +++ b/gtsam/discrete/DiscreteFactor.h @@ -77,7 +77,7 @@ class GTSAM_EXPORT DiscreteFactor : public Factor { /// @{ /// equals - virtual bool equals(const DiscreteFactor& lf, double tol = 1e-9) const = 0; + virtual bool equals(const DiscreteFactor& lf, double tol = 1e-9) const; /// print void print( diff --git a/gtsam/discrete/TableFactor.cpp b/gtsam/discrete/TableFactor.cpp index 486641e3f..8e185eb3b 100644 --- a/gtsam/discrete/TableFactor.cpp +++ b/gtsam/discrete/TableFactor.cpp @@ -144,7 +144,8 @@ bool TableFactor::equals(const DiscreteFactor& other, double tol) const { return false; } else { const auto& f(static_cast(other)); - return sparse_table_.isApprox(f.sparse_table_, tol); + return Base::equals(other, tol) && + sparse_table_.isApprox(f.sparse_table_, tol); } } diff --git a/gtsam/discrete/tests/testTableFactor.cpp b/gtsam/discrete/tests/testTableFactor.cpp index 0f7d7a615..35dcaf093 100644 --- a/gtsam/discrete/tests/testTableFactor.cpp +++ b/gtsam/discrete/tests/testTableFactor.cpp @@ -134,14 +134,17 @@ TEST(TableFactor, constructors) { EXPECT(assert_equal(expected, f4)); // Test for 9=3x3 values. - DiscreteKey V(0, 3), W(1, 3); + DiscreteKey V(0, 3), W(1, 3), O(100, 3); DiscreteConditional conditional5(V | W = "1/2/3 5/6/7 9/10/11"); TableFactor f5(conditional5); - // GTSAM_PRINT(f5); - TableFactor expected_f5( - X & Y, - "0.166667 0.277778 0.3 0.333333 0.333333 0.333333 0.5 0.388889 0.366667"); + + std::string expected_values = + "0.166667 0.277778 0.3 0.333333 0.333333 0.333333 0.5 0.388889 0.366667"; + TableFactor expected_f5(V & W, expected_values); EXPECT(assert_equal(expected_f5, f5, 1e-6)); + + TableFactor f5_with_wrong_keys(V & O, expected_values); + EXPECT(!assert_equal(f5_with_wrong_keys, f5, 1e-9)); } /* ************************************************************************* */ From ed742d3cb76dd10d8b2c3e700f86dde165f37221 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Wed, 11 Dec 2024 16:26:31 -0500 Subject: [PATCH 14/80] fix assertion --- gtsam/discrete/tests/testTableFactor.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/discrete/tests/testTableFactor.cpp b/gtsam/discrete/tests/testTableFactor.cpp index 35dcaf093..212067cb3 100644 --- a/gtsam/discrete/tests/testTableFactor.cpp +++ b/gtsam/discrete/tests/testTableFactor.cpp @@ -144,7 +144,7 @@ TEST(TableFactor, constructors) { EXPECT(assert_equal(expected_f5, f5, 1e-6)); TableFactor f5_with_wrong_keys(V & O, expected_values); - EXPECT(!assert_equal(f5_with_wrong_keys, f5, 1e-9)); + EXPECT(assert_inequal(f5_with_wrong_keys, f5, 1e-9)); } /* ************************************************************************* */ From d8e412555885925b32d6f3f3f189b53c5f3a1547 Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Sun, 1 Dec 2024 12:47:56 -0500 Subject: [PATCH 15/80] Change the deprecated C++ enum math to constexprs --- gtsam/base/ProductLieGroup.h | 4 ++-- gtsam/geometry/BearingRange.h | 6 +++--- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/gtsam/base/ProductLieGroup.h b/gtsam/base/ProductLieGroup.h index beb376c19..085da712a 100644 --- a/gtsam/base/ProductLieGroup.h +++ b/gtsam/base/ProductLieGroup.h @@ -32,8 +32,8 @@ class ProductLieGroup: public std::pair { typedef std::pair Base; protected: - enum {dimension1 = traits::dimension}; - enum {dimension2 = traits::dimension}; + constexpr static const size_t dimension1 = traits::dimension; + constexpr static const size_t dimension2 = traits::dimension; public: /// Default constructor yields identity diff --git a/gtsam/geometry/BearingRange.h b/gtsam/geometry/BearingRange.h index 19194bb7a..9b8fe1f87 100644 --- a/gtsam/geometry/BearingRange.h +++ b/gtsam/geometry/BearingRange.h @@ -55,9 +55,9 @@ private: R range_; public: - enum { dimB = traits::dimension }; - enum { dimR = traits::dimension }; - enum { dimension = dimB + dimR }; + constexpr static const size_t dimB = traits::dimension; + constexpr static const size_t dimR = traits::dimension; + constexpr static const size_t dimension = dimB + dimR; /// @name Standard Constructors /// @{ From 456df093f110af74b6ee6e85ba6e44ced009a680 Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Mon, 2 Dec 2024 14:11:51 -0500 Subject: [PATCH 16/80] Change all occurances of unnamed enum (deprecated in C++20) to `constexpr` --- GTSAM-Concepts.md | 2 +- gtsam/base/GenericValue.h | 3 +-- gtsam/base/Lie.h | 4 ++-- gtsam/base/Manifold.h | 4 ++-- gtsam/base/Matrix.h | 2 +- gtsam/base/ProductLieGroup.h | 6 +++--- gtsam/base/VectorSpace.h | 10 +++++----- gtsam/basis/Basis.h | 2 +- gtsam/geometry/BearingRange.h | 4 +--- gtsam/geometry/Cal3.h | 2 +- gtsam/geometry/Cal3Bundler.h | 2 +- gtsam/geometry/Cal3DS2.h | 2 +- gtsam/geometry/Cal3DS2_Base.h | 2 +- gtsam/geometry/Cal3Fisheye.h | 2 +- gtsam/geometry/Cal3Unified.h | 2 +- gtsam/geometry/Cal3_S2.h | 2 +- gtsam/geometry/Cal3_S2Stereo.h | 2 +- gtsam/geometry/Cal3f.h | 2 +- gtsam/geometry/CalibratedCamera.h | 4 +--- gtsam/geometry/EssentialMatrix.h | 2 +- gtsam/geometry/FundamentalMatrix.h | 4 ++-- gtsam/geometry/Line3.h | 2 +- gtsam/geometry/OrientedPlane3.h | 4 +--- gtsam/geometry/PinholeCamera.h | 4 +--- gtsam/geometry/PinholePose.h | 4 +--- gtsam/geometry/Quaternion.h | 4 +--- gtsam/geometry/SOn.h | 2 +- gtsam/geometry/SphericalCamera.h | 4 ++-- gtsam/geometry/StereoCamera.h | 4 +--- gtsam/geometry/StereoPoint2.h | 2 +- gtsam/geometry/Unit3.h | 4 +--- gtsam/navigation/NavState.h | 4 +--- gtsam/nonlinear/ExpressionFactor.h | 6 +++--- gtsam/nonlinear/NonlinearFactor.h | 2 +- gtsam/nonlinear/PriorFactor.h | 2 +- gtsam/nonlinear/tests/testExpression.cpp | 2 +- gtsam/nonlinear/tests/testValues.cpp | 2 +- gtsam/slam/BetweenFactor.h | 8 ++++---- gtsam/slam/FrobeniusFactor.h | 6 +++--- gtsam/slam/KarcherMeanFactor.h | 2 +- gtsam_unstable/geometry/Event.h | 2 +- 41 files changed, 58 insertions(+), 77 deletions(-) diff --git a/GTSAM-Concepts.md b/GTSAM-Concepts.md index 9911b3764..733672fbe 100644 --- a/GTSAM-Concepts.md +++ b/GTSAM-Concepts.md @@ -22,7 +22,7 @@ In GTSAM, all properties and operations needed to use a type must be defined thr In detail, we ask that the following items are defined in the traits object (although, not all are needed for optimization): * values: - * `enum { dimension = D};`, an enum that indicates the dimensionality *n* of the manifold. In Eigen-fashion, we also support manifolds whose dimensionality is only defined at runtime, by specifying the value -1. + * `inline constexpr static auto dimension = D;`, a constexpr that indicates the dimensionality *n* of the manifold. In Eigen-fashion, we also support manifolds whose dimensionality is only defined at runtime, by specifying the value -1. * types: * `TangentVector`, type that lives in tangent space. This will almost always be an `Eigen::Matrix`. * `ChartJacobian`, a typedef for `OptionalJacobian`. diff --git a/gtsam/base/GenericValue.h b/gtsam/base/GenericValue.h index bb92b6b2e..87ce7a73d 100644 --- a/gtsam/base/GenericValue.h +++ b/gtsam/base/GenericValue.h @@ -184,9 +184,8 @@ public: } #endif - // Alignment, see https://eigen.tuxfamily.org/dox/group__TopicStructHavingEigenMembers.html - enum { NeedsToAlign = (sizeof(T) % 16) == 0 }; + constexpr static const bool NeedsToAlign = (sizeof(T) % 16) == 0; public: GTSAM_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign) }; diff --git a/gtsam/base/Lie.h b/gtsam/base/Lie.h index ce021e10e..862ae6f4d 100644 --- a/gtsam/base/Lie.h +++ b/gtsam/base/Lie.h @@ -36,7 +36,7 @@ namespace gtsam { template struct LieGroup { - enum { dimension = N }; + inline constexpr static auto dimension = N; typedef OptionalJacobian ChartJacobian; typedef Eigen::Matrix Jacobian; typedef Eigen::Matrix TangentVector; @@ -183,7 +183,7 @@ struct LieGroupTraits: GetDimensionImpl { /// @name Manifold /// @{ typedef Class ManifoldType; - enum { dimension = Class::dimension }; + inline constexpr static auto dimension = Class::dimension; typedef Eigen::Matrix TangentVector; typedef OptionalJacobian ChartJacobian; diff --git a/gtsam/base/Manifold.h b/gtsam/base/Manifold.h index cb30fa9c0..922b8dd2f 100644 --- a/gtsam/base/Manifold.h +++ b/gtsam/base/Manifold.h @@ -55,7 +55,7 @@ namespace internal { template struct HasManifoldPrereqs { - enum { dim = Class::dimension }; + inline constexpr static auto dim = Class::dimension; Class p, q; Eigen::Matrix v; @@ -95,7 +95,7 @@ struct ManifoldTraits: GetDimensionImpl { GTSAM_CONCEPT_ASSERT(HasManifoldPrereqs); // Dimension of the manifold - enum { dimension = Class::dimension }; + inline constexpr static auto dimension = Class::dimension; // Typedefs required by all manifold types. typedef Class ManifoldType; diff --git a/gtsam/base/Matrix.h b/gtsam/base/Matrix.h index 6dba9cb05..c8dc46ed5 100644 --- a/gtsam/base/Matrix.h +++ b/gtsam/base/Matrix.h @@ -479,7 +479,7 @@ struct MultiplyWithInverse { */ template struct MultiplyWithInverseFunction { - enum { M = traits::dimension }; + inline constexpr static auto M = traits::dimension; typedef Eigen::Matrix VectorN; typedef Eigen::Matrix MatrixN; diff --git a/gtsam/base/ProductLieGroup.h b/gtsam/base/ProductLieGroup.h index 085da712a..bb7ba72b8 100644 --- a/gtsam/base/ProductLieGroup.h +++ b/gtsam/base/ProductLieGroup.h @@ -67,9 +67,9 @@ public: /// @name Manifold /// @{ - enum {dimension = dimension1 + dimension2}; - inline static size_t Dim() {return dimension;} - inline size_t dim() const {return dimension;} + inline constexpr static auto dimension = dimension1 + dimension2; + inline static size_t Dim() { return dimension; } + inline size_t dim() const { return dimension; } typedef Eigen::Matrix TangentVector; typedef OptionalJacobian ChartJacobian; diff --git a/gtsam/base/VectorSpace.h b/gtsam/base/VectorSpace.h index a9e9ca106..c5c4ad622 100644 --- a/gtsam/base/VectorSpace.h +++ b/gtsam/base/VectorSpace.h @@ -163,7 +163,7 @@ struct VectorSpaceImpl { template struct HasVectorSpacePrereqs { - enum { dim = Class::dimension }; + inline constexpr static auto dim = Class::dimension; Class p, q; Vector v; @@ -197,7 +197,7 @@ GTSAM_CONCEPT_ASSERT(HasVectorSpacePrereqs); /// @name Manifold /// @{ - enum { dimension = Class::dimension}; + inline constexpr static auto dimension = Class::dimension; typedef Class ManifoldType; /// @} }; @@ -232,7 +232,7 @@ struct ScalarTraits : VectorSpaceImpl { /// @name Manifold /// @{ typedef Scalar ManifoldType; - enum { dimension = 1 }; + inline constexpr static auto dimension = 1; typedef Eigen::Matrix TangentVector; typedef OptionalJacobian<1, 1> ChartJacobian; @@ -305,7 +305,7 @@ struct traits > : /// @name Manifold /// @{ - enum { dimension = M*N}; + inline constexpr static auto dimension = M * N; typedef Fixed ManifoldType; typedef Eigen::Matrix TangentVector; typedef Eigen::Matrix Jacobian; @@ -377,7 +377,7 @@ struct DynamicTraits { /// @name Manifold /// @{ - enum { dimension = Eigen::Dynamic }; + inline constexpr static auto dimension = Eigen::Dynamic; typedef Eigen::VectorXd TangentVector; typedef Eigen::MatrixXd Jacobian; typedef OptionalJacobian ChartJacobian; diff --git a/gtsam/basis/Basis.h b/gtsam/basis/Basis.h index 1b1c44acc..960440daf 100644 --- a/gtsam/basis/Basis.h +++ b/gtsam/basis/Basis.h @@ -291,7 +291,7 @@ class Basis { */ template class ManifoldEvaluationFunctor : public VectorEvaluationFunctor { - enum { M = traits::dimension }; + inline constexpr static auto M = traits::dimension; using Base = VectorEvaluationFunctor; public: diff --git a/gtsam/geometry/BearingRange.h b/gtsam/geometry/BearingRange.h index 9b8fe1f87..07229dfca 100644 --- a/gtsam/geometry/BearingRange.h +++ b/gtsam/geometry/BearingRange.h @@ -162,9 +162,7 @@ private: /// @} // Alignment, see https://eigen.tuxfamily.org/dox/group__TopicStructHavingEigenMembers.html - enum { - NeedsToAlign = (sizeof(B) % 16) == 0 || (sizeof(R) % 16) == 0 - }; + inline constexpr static auto NeedsToAlign = (sizeof(B) % 16) == 0 || (sizeof(R) % 16) == 0; public: GTSAM_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign) }; diff --git a/gtsam/geometry/Cal3.h b/gtsam/geometry/Cal3.h index 1b66eb336..5eecee845 100644 --- a/gtsam/geometry/Cal3.h +++ b/gtsam/geometry/Cal3.h @@ -73,7 +73,7 @@ class GTSAM_EXPORT Cal3 { double u0_ = 0.0f, v0_ = 0.0f; ///< principal point public: - enum { dimension = 5 }; + inline constexpr static auto dimension = 5; ///< shared pointer to calibration object using shared_ptr = std::shared_ptr; diff --git a/gtsam/geometry/Cal3Bundler.h b/gtsam/geometry/Cal3Bundler.h index 081688d48..01eb9437a 100644 --- a/gtsam/geometry/Cal3Bundler.h +++ b/gtsam/geometry/Cal3Bundler.h @@ -37,7 +37,7 @@ class GTSAM_EXPORT Cal3Bundler : public Cal3f { // Note: u0 and v0 are constants and not optimized. public: - enum { dimension = 3 }; + inline constexpr static auto dimension = 3; using shared_ptr = std::shared_ptr; /// @name Constructors diff --git a/gtsam/geometry/Cal3DS2.h b/gtsam/geometry/Cal3DS2.h index a7ca08afc..0ecfc24ae 100644 --- a/gtsam/geometry/Cal3DS2.h +++ b/gtsam/geometry/Cal3DS2.h @@ -36,7 +36,7 @@ class GTSAM_EXPORT Cal3DS2 : public Cal3DS2_Base { using Base = Cal3DS2_Base; public: - enum { dimension = 9 }; + inline constexpr static auto dimension = 9; ///< shared pointer to stereo calibration object using shared_ptr = std::shared_ptr; diff --git a/gtsam/geometry/Cal3DS2_Base.h b/gtsam/geometry/Cal3DS2_Base.h index b0ef0368b..6eff04c10 100644 --- a/gtsam/geometry/Cal3DS2_Base.h +++ b/gtsam/geometry/Cal3DS2_Base.h @@ -46,7 +46,7 @@ class GTSAM_EXPORT Cal3DS2_Base : public Cal3 { double tol_ = 1e-5; ///< tolerance value when calibrating public: - enum { dimension = 9 }; + inline constexpr static auto dimension = 9; ///< shared pointer to stereo calibration object using shared_ptr = std::shared_ptr; diff --git a/gtsam/geometry/Cal3Fisheye.h b/gtsam/geometry/Cal3Fisheye.h index 3f1cac148..4c542c5be 100644 --- a/gtsam/geometry/Cal3Fisheye.h +++ b/gtsam/geometry/Cal3Fisheye.h @@ -55,7 +55,7 @@ class GTSAM_EXPORT Cal3Fisheye : public Cal3 { double tol_ = 1e-5; ///< tolerance value when calibrating public: - enum { dimension = 9 }; + inline constexpr static auto dimension = 9; ///< shared pointer to fisheye calibration object using shared_ptr = std::shared_ptr; diff --git a/gtsam/geometry/Cal3Unified.h b/gtsam/geometry/Cal3Unified.h index 309b6dca1..7eb4d2edb 100644 --- a/gtsam/geometry/Cal3Unified.h +++ b/gtsam/geometry/Cal3Unified.h @@ -50,7 +50,7 @@ class GTSAM_EXPORT Cal3Unified : public Cal3DS2_Base { double xi_ = 0.0f; ///< mirror parameter public: - enum { dimension = 10 }; + inline constexpr static auto dimension = 10; ///< shared pointer to stereo calibration object using shared_ptr = std::shared_ptr; diff --git a/gtsam/geometry/Cal3_S2.h b/gtsam/geometry/Cal3_S2.h index 885be614f..568cde41e 100644 --- a/gtsam/geometry/Cal3_S2.h +++ b/gtsam/geometry/Cal3_S2.h @@ -33,7 +33,7 @@ namespace gtsam { */ class GTSAM_EXPORT Cal3_S2 : public Cal3 { public: - enum { dimension = 5 }; + inline constexpr static auto dimension = 5; ///< shared pointer to calibration object using shared_ptr = std::shared_ptr; diff --git a/gtsam/geometry/Cal3_S2Stereo.h b/gtsam/geometry/Cal3_S2Stereo.h index 4fee1a022..292519cfd 100644 --- a/gtsam/geometry/Cal3_S2Stereo.h +++ b/gtsam/geometry/Cal3_S2Stereo.h @@ -32,7 +32,7 @@ class GTSAM_EXPORT Cal3_S2Stereo : public Cal3_S2 { double b_ = 1.0f; ///< Stereo baseline. public: - enum { dimension = 6 }; + inline constexpr static auto dimension = 6; ///< shared pointer to stereo calibration object using shared_ptr = std::shared_ptr; diff --git a/gtsam/geometry/Cal3f.h b/gtsam/geometry/Cal3f.h index f053f3d11..6c3c214b8 100644 --- a/gtsam/geometry/Cal3f.h +++ b/gtsam/geometry/Cal3f.h @@ -34,7 +34,7 @@ namespace gtsam { */ class GTSAM_EXPORT Cal3f : public Cal3 { public: - enum { dimension = 1 }; + inline constexpr static auto dimension = 1; using shared_ptr = std::shared_ptr; /// @name Constructors diff --git a/gtsam/geometry/CalibratedCamera.h b/gtsam/geometry/CalibratedCamera.h index dca15feb2..d3d1e1b9a 100644 --- a/gtsam/geometry/CalibratedCamera.h +++ b/gtsam/geometry/CalibratedCamera.h @@ -252,9 +252,7 @@ class GTSAM_EXPORT CalibratedCamera: public PinholeBase { public: - enum { - dimension = 6 - }; + inline constexpr static auto dimension = 6; /// @name Standard Constructors /// @{ diff --git a/gtsam/geometry/EssentialMatrix.h b/gtsam/geometry/EssentialMatrix.h index d514c4f19..3986f3063 100644 --- a/gtsam/geometry/EssentialMatrix.h +++ b/gtsam/geometry/EssentialMatrix.h @@ -82,7 +82,7 @@ class EssentialMatrix { /// @name Manifold /// @{ - enum { dimension = 5 }; + inline constexpr static auto dimension = 5; inline static size_t Dim() { return dimension;} inline size_t dim() const { return dimension;} diff --git a/gtsam/geometry/FundamentalMatrix.h b/gtsam/geometry/FundamentalMatrix.h index df2c2881a..0752ced7d 100644 --- a/gtsam/geometry/FundamentalMatrix.h +++ b/gtsam/geometry/FundamentalMatrix.h @@ -102,7 +102,7 @@ class GTSAM_EXPORT FundamentalMatrix { /// @name Manifold /// @{ - enum { dimension = 7 }; // 3 for U, 1 for s, 3 for V + inline constexpr static auto dimension = 7; // 3 for U, 1 for s, 3 for V inline static size_t Dim() { return dimension; } inline size_t dim() const { return dimension; } @@ -179,7 +179,7 @@ class GTSAM_EXPORT SimpleFundamentalMatrix { /// @name Manifold /// @{ - enum { dimension = 7 }; // 5 for E, 1 for fa, 1 for fb + inline constexpr static auto dimension = 7; // 5 for E, 1 for fa, 1 for fb inline static size_t Dim() { return dimension; } inline size_t dim() const { return dimension; } diff --git a/gtsam/geometry/Line3.h b/gtsam/geometry/Line3.h index def49d268..5ae50e138 100644 --- a/gtsam/geometry/Line3.h +++ b/gtsam/geometry/Line3.h @@ -47,7 +47,7 @@ class GTSAM_EXPORT Line3 { double a_, b_; // Intersection of line with the world x-y plane rotated by R_ // Also the closest point on line to origin public: - enum { dimension = 4 }; + inline constexpr static auto dimension = 4; /** Default constructor is the Z axis **/ Line3() : diff --git a/gtsam/geometry/OrientedPlane3.h b/gtsam/geometry/OrientedPlane3.h index 07c8445fe..3d84086ac 100644 --- a/gtsam/geometry/OrientedPlane3.h +++ b/gtsam/geometry/OrientedPlane3.h @@ -39,9 +39,7 @@ private: double d_; ///< The perpendicular distance to this plane public: - enum { - dimension = 3 - }; + inline constexpr static auto dimension = 3; /// @name Constructors /// @{ diff --git a/gtsam/geometry/PinholeCamera.h b/gtsam/geometry/PinholeCamera.h index 8329664fd..0439b2fde 100644 --- a/gtsam/geometry/PinholeCamera.h +++ b/gtsam/geometry/PinholeCamera.h @@ -51,9 +51,7 @@ private: public: - enum { - dimension = 6 + DimK - }; ///< Dimension depends on calibration + inline constexpr static auto dimension = 6 + DimK; ///< Dimension depends on calibration /// @name Standard Constructors /// @{ diff --git a/gtsam/geometry/PinholePose.h b/gtsam/geometry/PinholePose.h index df6ec5c08..f1191dbcc 100644 --- a/gtsam/geometry/PinholePose.h +++ b/gtsam/geometry/PinholePose.h @@ -245,9 +245,7 @@ private: public: - enum { - dimension = 6 - }; ///< There are 6 DOF to optimize for + inline constexpr static auto dimension = 6; ///< There are 6 DOF to optimize for /// @name Standard Constructors /// @{ diff --git a/gtsam/geometry/Quaternion.h b/gtsam/geometry/Quaternion.h index f9ed2a383..36f891505 100644 --- a/gtsam/geometry/Quaternion.h +++ b/gtsam/geometry/Quaternion.h @@ -45,9 +45,7 @@ struct traits { /// @} /// @name Basic manifold traits /// @{ - enum { - dimension = 3 - }; + inline constexpr static auto dimension = 3; typedef OptionalJacobian<3, 3> ChartJacobian; typedef Eigen::Matrix<_Scalar, 3, 1, _Options, 3, 1> TangentVector; diff --git a/gtsam/geometry/SOn.h b/gtsam/geometry/SOn.h index 75ff872a8..b0da3b213 100644 --- a/gtsam/geometry/SOn.h +++ b/gtsam/geometry/SOn.h @@ -54,7 +54,7 @@ constexpr int NSquaredSO(int N) { return (N < 0) ? Eigen::Dynamic : N * N; } template class SO : public LieGroup, internal::DimensionSO(N)> { public: - enum { dimension = internal::DimensionSO(N) }; + inline constexpr static auto dimension = internal::DimensionSO(N); using MatrixNN = Eigen::Matrix; using VectorN2 = Eigen::Matrix; using MatrixDD = Eigen::Matrix; diff --git a/gtsam/geometry/SphericalCamera.h b/gtsam/geometry/SphericalCamera.h index ef20aa7fe..1ff87cec3 100644 --- a/gtsam/geometry/SphericalCamera.h +++ b/gtsam/geometry/SphericalCamera.h @@ -41,7 +41,7 @@ namespace gtsam { */ class GTSAM_EXPORT EmptyCal { public: - enum { dimension = 0 }; + inline constexpr static auto dimension = 0; EmptyCal() {} virtual ~EmptyCal() = default; using shared_ptr = std::shared_ptr; @@ -73,7 +73,7 @@ class GTSAM_EXPORT EmptyCal { */ class GTSAM_EXPORT SphericalCamera { public: - enum { dimension = 6 }; + inline constexpr static auto dimension = 6; using Measurement = Unit3; using MeasurementVector = std::vector; diff --git a/gtsam/geometry/StereoCamera.h b/gtsam/geometry/StereoCamera.h index da71dd070..026125572 100644 --- a/gtsam/geometry/StereoCamera.h +++ b/gtsam/geometry/StereoCamera.h @@ -61,9 +61,7 @@ private: public: - enum { - dimension = 6 - }; + inline constexpr static auto dimension = 6; /// @name Standard Constructors /// @{ diff --git a/gtsam/geometry/StereoPoint2.h b/gtsam/geometry/StereoPoint2.h index 4383e212e..7a1910e42 100644 --- a/gtsam/geometry/StereoPoint2.h +++ b/gtsam/geometry/StereoPoint2.h @@ -37,7 +37,7 @@ private: double uL_, uR_, v_; public: - enum { dimension = 3 }; + inline constexpr static auto dimension = 3; /// @name Standard Constructors /// @{ diff --git a/gtsam/geometry/Unit3.h b/gtsam/geometry/Unit3.h index 18bc5d9f0..a7304280f 100644 --- a/gtsam/geometry/Unit3.h +++ b/gtsam/geometry/Unit3.h @@ -53,9 +53,7 @@ private: public: - enum { - dimension = 2 - }; + inline constexpr static auto dimension = 2; /// @name Constructors /// @{ diff --git a/gtsam/navigation/NavState.h b/gtsam/navigation/NavState.h index f778a7123..35ce435a3 100644 --- a/gtsam/navigation/NavState.h +++ b/gtsam/navigation/NavState.h @@ -42,9 +42,7 @@ private: public: - enum { - dimension = 9 - }; + inline constexpr static auto dimension = 9; typedef std::pair PositionAndVelocity; diff --git a/gtsam/nonlinear/ExpressionFactor.h b/gtsam/nonlinear/ExpressionFactor.h index b3e34d079..d9af1933c 100644 --- a/gtsam/nonlinear/ExpressionFactor.h +++ b/gtsam/nonlinear/ExpressionFactor.h @@ -224,9 +224,9 @@ private: #endif // Alignment, see https://eigen.tuxfamily.org/dox/group__TopicStructHavingEigenMembers.html - enum { NeedsToAlign = (sizeof(T) % 16) == 0 }; - public: - GTSAM_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign) + inline constexpr static auto NeedsToAlign = (sizeof(T) % 16) == 0; + public: + GTSAM_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign) }; // ExpressionFactor diff --git a/gtsam/nonlinear/NonlinearFactor.h b/gtsam/nonlinear/NonlinearFactor.h index 2d3a13766..101743b78 100644 --- a/gtsam/nonlinear/NonlinearFactor.h +++ b/gtsam/nonlinear/NonlinearFactor.h @@ -434,7 +434,7 @@ class NoiseModelFactorN public detail::NoiseModelFactorAliases { public: /// N is the number of variables (N-way factor) - enum { N = sizeof...(ValueTypes) }; + inline constexpr static auto N = sizeof...(ValueTypes); using NoiseModelFactor::unwhitenedError; diff --git a/gtsam/nonlinear/PriorFactor.h b/gtsam/nonlinear/PriorFactor.h index ab2a66a77..159373459 100644 --- a/gtsam/nonlinear/PriorFactor.h +++ b/gtsam/nonlinear/PriorFactor.h @@ -118,7 +118,7 @@ namespace gtsam { #endif // Alignment, see https://eigen.tuxfamily.org/dox/group__TopicStructHavingEigenMembers.html - enum { NeedsToAlign = (sizeof(T) % 16) == 0 }; + inline constexpr static auto NeedsToAlign = (sizeof(T) % 16) == 0; public: GTSAM_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign) }; diff --git a/gtsam/nonlinear/tests/testExpression.cpp b/gtsam/nonlinear/tests/testExpression.cpp index 6f5fc53f5..67815e262 100644 --- a/gtsam/nonlinear/tests/testExpression.cpp +++ b/gtsam/nonlinear/tests/testExpression.cpp @@ -115,7 +115,7 @@ TEST(Expression, Unary3) { // Simple test class that implements the `VectorSpace` protocol. class Class : public Point3 { public: - enum {dimension = 3}; + inline constexpr static auto dimension = 3; using Point3::Point3; const Vector3& vector() const { return *this; } inline static Class Identity() { return Class(0,0,0); } diff --git a/gtsam/nonlinear/tests/testValues.cpp b/gtsam/nonlinear/tests/testValues.cpp index a2b265a56..06eb46d4c 100644 --- a/gtsam/nonlinear/tests/testValues.cpp +++ b/gtsam/nonlinear/tests/testValues.cpp @@ -54,7 +54,7 @@ int TestValueData::DestructorCount = 0; class TestValue { TestValueData data_; public: - enum {dimension = 0}; + inline constexpr static auto dimension = 0; void print(const std::string& str = "") const {} bool equals(const TestValue& other, double tol = 1e-9) const { return true; } size_t dim() const { return 0; } diff --git a/gtsam/slam/BetweenFactor.h b/gtsam/slam/BetweenFactor.h index 6eaa5c01b..3e2478e73 100644 --- a/gtsam/slam/BetweenFactor.h +++ b/gtsam/slam/BetweenFactor.h @@ -147,10 +147,10 @@ namespace gtsam { } #endif - // Alignment, see https://eigen.tuxfamily.org/dox/group__TopicStructHavingEigenMembers.html - enum { NeedsToAlign = (sizeof(VALUE) % 16) == 0 }; - public: - GTSAM_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign) + // Alignment, see https://eigen.tuxfamily.org/dox/group__TopicStructHavingEigenMembers.html + inline constexpr static auto NeedsToAlign = (sizeof(VALUE) % 16) == 0; + public: + GTSAM_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign) }; // \class BetweenFactor /// traits diff --git a/gtsam/slam/FrobeniusFactor.h b/gtsam/slam/FrobeniusFactor.h index a000a1514..37efa5f24 100644 --- a/gtsam/slam/FrobeniusFactor.h +++ b/gtsam/slam/FrobeniusFactor.h @@ -49,7 +49,7 @@ ConvertNoiseModel(const SharedNoiseModel &model, size_t n, */ template class FrobeniusPrior : public NoiseModelFactorN { - enum { Dim = Rot::VectorN2::RowsAtCompileTime }; + inline constexpr static auto Dim = Rot::VectorN2::RowsAtCompileTime; using MatrixNN = typename Rot::MatrixNN; Eigen::Matrix vecM_; ///< vectorized matrix to approximate @@ -79,7 +79,7 @@ class FrobeniusPrior : public NoiseModelFactorN { */ template class FrobeniusFactor : public NoiseModelFactorN { - enum { Dim = Rot::VectorN2::RowsAtCompileTime }; + inline constexpr static auto Dim = Rot::VectorN2::RowsAtCompileTime; public: @@ -111,7 +111,7 @@ class FrobeniusBetweenFactor : public NoiseModelFactorN { Rot R12_; ///< measured rotation between R1 and R2 Eigen::Matrix R2hat_H_R1_; ///< fixed derivative of R2hat wrpt R1 - enum { Dim = Rot::VectorN2::RowsAtCompileTime }; + inline constexpr static auto Dim = Rot::VectorN2::RowsAtCompileTime; public: diff --git a/gtsam/slam/KarcherMeanFactor.h b/gtsam/slam/KarcherMeanFactor.h index 4e82ca1d9..6b25dd261 100644 --- a/gtsam/slam/KarcherMeanFactor.h +++ b/gtsam/slam/KarcherMeanFactor.h @@ -45,7 +45,7 @@ template T FindKarcherMean(std::initializer_list &&rotations); * */ template class KarcherMeanFactor : public NonlinearFactor { // Compile time dimension: can be -1 - enum { D = traits::dimension }; + inline constexpr static auto D = traits::dimension; // Runtime dimension: always >=0 size_t d_; diff --git a/gtsam_unstable/geometry/Event.h b/gtsam_unstable/geometry/Event.h index a3c907646..471ada35b 100644 --- a/gtsam_unstable/geometry/Event.h +++ b/gtsam_unstable/geometry/Event.h @@ -39,7 +39,7 @@ class GTSAM_UNSTABLE_EXPORT Event { Point3 location_; ///< Location at time event was generated public: - enum { dimension = 4 }; + inline constexpr static auto dimension = 4; /// Default Constructor Event() : time_(0), location_(0, 0, 0) {} From b91c470b6929bffca39bfbd36c39c3795183c79e Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Fri, 13 Dec 2024 08:07:10 -0500 Subject: [PATCH 17/80] test exposing incorrect conversion --- gtsam/discrete/tests/testTableFactor.cpp | 30 ++++++++++++++++++++++++ 1 file changed, 30 insertions(+) diff --git a/gtsam/discrete/tests/testTableFactor.cpp b/gtsam/discrete/tests/testTableFactor.cpp index 212067cb3..7df6da83e 100644 --- a/gtsam/discrete/tests/testTableFactor.cpp +++ b/gtsam/discrete/tests/testTableFactor.cpp @@ -147,6 +147,36 @@ TEST(TableFactor, constructors) { EXPECT(assert_inequal(f5_with_wrong_keys, f5, 1e-9)); } +/* ************************************************************************* */ +// Check conversion from DecisionTreeFactor. +TEST(TableFactor, Conversion) { + /* This is the DecisionTree we are using + Choice(m2) + 0 Choice(m1) + 0 0 Leaf 0 + 0 1 Choice(m0) + 0 1 0 Leaf 0 + 0 1 1 Leaf 0.14649446 // 3 + 1 Choice(m1) + 1 0 Choice(m0) + 1 0 0 Leaf 0 + 1 0 1 Leaf 0.14648756 // 5 + 1 1 Choice(m0) + 1 1 0 Leaf 0.14649446 // 6 + 1 1 1 Leaf 0.23918345 // 7 + */ + DiscreteKeys dkeys = {{0, 2}, {1, 2}, {2, 2}}; + DecisionTreeFactor dtf( + dkeys, std::vector{0, 0, 0, 0.14649446, 0, 0.14648756, 0.14649446, + 0.23918345}); + + // dtf.print(); + TableFactor tf(dtf.discreteKeys(), dtf); + // tf.print(); + // tf.toDecisionTreeFactor().print(); + EXPECT(assert_equal(dtf, tf.toDecisionTreeFactor())); +} + /* ************************************************************************* */ // Check multiplication between two TableFactors. TEST(TableFactor, multiplication) { From a8e24efdeca680f391098979ad2402dd3918153f Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Fri, 13 Dec 2024 09:34:01 -0500 Subject: [PATCH 18/80] update ComputeLeafOrdering to give a correct vector of values --- gtsam/discrete/TableFactor.cpp | 72 +++++++++++++++++++++++++++------- 1 file changed, 57 insertions(+), 15 deletions(-) diff --git a/gtsam/discrete/TableFactor.cpp b/gtsam/discrete/TableFactor.cpp index 8e185eb3b..a2d68853e 100644 --- a/gtsam/discrete/TableFactor.cpp +++ b/gtsam/discrete/TableFactor.cpp @@ -64,27 +64,69 @@ TableFactor::TableFactor(const DiscreteKeys& dkeys, /** * @brief Compute the correct ordering of the leaves in the decision tree. * - * This is done by first taking all the values which have modulo 0 value with - * the cardinality of the innermost key `n`, and we go up to modulo n. - * * @param dt The DecisionTree - * @return std::vector + * @return Eigen::SparseVector */ -std::vector ComputeLeafOrdering(const DiscreteKeys& dkeys, - const DecisionTreeFactor& dt) { - std::vector probs = dt.probabilities(); - std::vector ordered; +static Eigen::SparseVector ComputeLeafOrdering( + const DiscreteKeys& dkeys, const DecisionTreeFactor& dt) { + // SparseVector needs to know the maximum possible index, + // so we compute the product of cardinalities. + size_t prod_cardinality = 1; + for (auto&& [_, c] : dt.cardinalities()) { + prod_cardinality *= c; + } + Eigen::SparseVector sparse_table(prod_cardinality); + size_t nrValues = 0; + dt.visit([&nrValues](double x) { + if (x > 0) nrValues += 1; + }); + sparse_table.reserve(nrValues); - size_t n = dkeys[0].second; + std::set allKeys(dt.keys().begin(), dt.keys().end()); - for (size_t k = 0; k < n; ++k) { - for (size_t idx = 0; idx < probs.size(); ++idx) { - if (idx % n == k) { - ordered.push_back(probs[idx]); + auto op = [&](const Assignment& assignment, double p) { + if (p > 0) { + // Get all the keys involved in this assignment + std::set assignment_keys; + for (auto&& [k, _] : assignment) { + assignment_keys.insert(k); + } + + // 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)); + + // Generate all assignments using the missing keys + DiscreteKeys extras; + for (auto&& key : diff) { + extras.push_back({key, dt.cardinality(key)}); + } + auto&& extra_assignments = DiscreteValues::CartesianProduct(extras); + + for (auto&& extra : extra_assignments) { + // Create new assignment using the extra assignment + DiscreteValues updated_assignment(assignment); + updated_assignment.insert(extra); + + // Generate index and add to the sparse vector. + Eigen::Index idx = 0; + size_t prev_cardinality = 1; + // We go in reverse since a DecisionTree has the highest label first + for (auto&& it = updated_assignment.rbegin(); + it != updated_assignment.rend(); it++) { + idx += prev_cardinality * it->second; + prev_cardinality *= dt.cardinality(it->first); + } + sparse_table.coeffRef(idx) = p; } } - } - return ordered; + }; + + dt.visitWith(op); + + return sparse_table; } /* ************************************************************************ */ From 7d389a53007af3811a0c8195d368e8fcd3825c3f Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Fri, 13 Dec 2024 09:46:55 -0500 Subject: [PATCH 19/80] clean up test --- gtsam/discrete/tests/testTableFactor.cpp | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/gtsam/discrete/tests/testTableFactor.cpp b/gtsam/discrete/tests/testTableFactor.cpp index 7df6da83e..a455faaaa 100644 --- a/gtsam/discrete/tests/testTableFactor.cpp +++ b/gtsam/discrete/tests/testTableFactor.cpp @@ -170,10 +170,8 @@ TEST(TableFactor, Conversion) { dkeys, std::vector{0, 0, 0, 0.14649446, 0, 0.14648756, 0.14649446, 0.23918345}); - // dtf.print(); TableFactor tf(dtf.discreteKeys(), dtf); - // tf.print(); - // tf.toDecisionTreeFactor().print(); + EXPECT(assert_equal(dtf, tf.toDecisionTreeFactor())); } From 9830981351fd5e5b714c6a5dc58b6e67ba8ec7cc Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Fri, 13 Dec 2024 13:58:19 -0500 Subject: [PATCH 20/80] address review comments --- gtsam/discrete/TableFactor.cpp | 61 ++++++++++++++++++++++------------ 1 file changed, 39 insertions(+), 22 deletions(-) diff --git a/gtsam/discrete/TableFactor.cpp b/gtsam/discrete/TableFactor.cpp index a2d68853e..de1e1f867 100644 --- a/gtsam/discrete/TableFactor.cpp +++ b/gtsam/discrete/TableFactor.cpp @@ -62,40 +62,55 @@ TableFactor::TableFactor(const DiscreteKeys& dkeys, : TableFactor(dkeys, DecisionTreeFactor(dkeys, dtree)) {} /** - * @brief Compute the correct ordering of the leaves in the decision tree. + * @brief Compute the indexing of the leaves in the decision tree based on the + * assignment and add the (index, leaf) pair to a SparseVector. + * + * We visit each leaf in the tree, and using the cardinalities of the keys, + * compute the correct index to add the leaf to a SparseVector which + * is then used to create the TableFactor. * * @param dt The DecisionTree * @return Eigen::SparseVector */ -static Eigen::SparseVector ComputeLeafOrdering( +static Eigen::SparseVector ComputeSparseTable( const DiscreteKeys& dkeys, const DecisionTreeFactor& dt) { // SparseVector needs to know the maximum possible index, // so we compute the product of cardinalities. - size_t prod_cardinality = 1; + size_t cardinalityProduct = 1; for (auto&& [_, c] : dt.cardinalities()) { - prod_cardinality *= c; + cardinalityProduct *= c; } - Eigen::SparseVector sparse_table(prod_cardinality); + Eigen::SparseVector sparseTable(cardinalityProduct); size_t nrValues = 0; dt.visit([&nrValues](double x) { if (x > 0) nrValues += 1; }); - sparse_table.reserve(nrValues); + sparseTable.reserve(nrValues); std::set allKeys(dt.keys().begin(), dt.keys().end()); + /** + * @brief Functor which is called by the DecisionTree for each leaf. + * For each leaf value, we use the corresponding assignment to compute a + * corresponding index into a SparseVector. We then populate sparseTable with + * the value at the computed index. + * + * Takes advantage of the sparsity of the DecisionTree to be efficient. When + * merged branches are encountered, we enumerate over the missing keys. + * + */ auto op = [&](const Assignment& assignment, double p) { if (p > 0) { // Get all the keys involved in this assignment - std::set assignment_keys; + std::set assignmentKeys; for (auto&& [k, _] : assignment) { - assignment_keys.insert(k); + assignmentKeys.insert(k); } // Find the keys missing in the assignment std::vector diff; std::set_difference(allKeys.begin(), allKeys.end(), - assignment_keys.begin(), assignment_keys.end(), + assignmentKeys.begin(), assignmentKeys.end(), std::back_inserter(diff)); // Generate all assignments using the missing keys @@ -103,41 +118,43 @@ static Eigen::SparseVector ComputeLeafOrdering( for (auto&& key : diff) { extras.push_back({key, dt.cardinality(key)}); } - auto&& extra_assignments = DiscreteValues::CartesianProduct(extras); + auto&& extraAssignments = DiscreteValues::CartesianProduct(extras); - for (auto&& extra : extra_assignments) { + for (auto&& extra : extraAssignments) { // Create new assignment using the extra assignment - DiscreteValues updated_assignment(assignment); - updated_assignment.insert(extra); + DiscreteValues updatedAssignment(assignment); + updatedAssignment.insert(extra); // Generate index and add to the sparse vector. Eigen::Index idx = 0; - size_t prev_cardinality = 1; + size_t previousCardinality = 1; // We go in reverse since a DecisionTree has the highest label first - for (auto&& it = updated_assignment.rbegin(); - it != updated_assignment.rend(); it++) { - idx += prev_cardinality * it->second; - prev_cardinality *= dt.cardinality(it->first); + for (auto&& it = updatedAssignment.rbegin(); + it != updatedAssignment.rend(); it++) { + idx += previousCardinality * it->second; + previousCardinality *= dt.cardinality(it->first); } - sparse_table.coeffRef(idx) = p; + sparseTable.coeffRef(idx) = p; } } }; + // Visit each leaf in `dt` to get the Assignment and leaf value + // to populate the sparseTable. dt.visitWith(op); - return sparse_table; + return sparseTable; } /* ************************************************************************ */ TableFactor::TableFactor(const DiscreteKeys& dkeys, const DecisionTreeFactor& dtf) - : TableFactor(dkeys, ComputeLeafOrdering(dkeys, dtf)) {} + : TableFactor(dkeys, ComputeSparseTable(dkeys, dtf)) {} /* ************************************************************************ */ TableFactor::TableFactor(const DecisionTreeFactor& dtf) : TableFactor(dtf.discreteKeys(), - ComputeLeafOrdering(dtf.discreteKeys(), dtf)) {} + ComputeSparseTable(dtf.discreteKeys(), dtf)) {} /* ************************************************************************ */ TableFactor::TableFactor(const DiscreteConditional& c) From 923afba4d81c083a750be9b9f355310012590b1a Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Fri, 13 Dec 2024 23:36:27 -0500 Subject: [PATCH 21/80] Endow NavState with group operations --- gtsam/navigation/NavState.cpp | 255 +++++++++++++- gtsam/navigation/NavState.h | 126 ++++++- gtsam/navigation/tests/testNavState.cpp | 443 +++++++++++++++++++++++- 3 files changed, 779 insertions(+), 45 deletions(-) diff --git a/gtsam/navigation/NavState.cpp b/gtsam/navigation/NavState.cpp index 3e2817752..9f91ae415 100644 --- a/gtsam/navigation/NavState.cpp +++ b/gtsam/navigation/NavState.cpp @@ -77,10 +77,13 @@ Vector3 NavState::bodyVelocity(OptionalJacobian<3, 9> H) const { } //------------------------------------------------------------------------------ -Matrix7 NavState::matrix() const { +Matrix5 NavState::matrix() const { Matrix3 R = this->R(); - Matrix7 T; - T << R, Z_3x3, t(), Z_3x3, R, v(), Vector6::Zero().transpose(), 1.0; + + Matrix5 T = Matrix5::Identity(); + T.block<3, 3>(0, 0) = R; + T.block<3, 1>(0, 3) = t_; + T.block<3, 1>(0, 4) = v_; return T; } @@ -103,9 +106,215 @@ bool NavState::equals(const NavState& other, double tol) const { && equal_with_abs_tol(v_, other.v_, tol); } +//------------------------------------------------------------------------------ +NavState NavState::inverse() const { + Rot3 Rt = R_.inverse(); + return NavState(Rt, Rt * (-t_), Rt * -(v_)); +} + +//------------------------------------------------------------------------------ +NavState NavState::Expmap(const Vector9& xi, OptionalJacobian<9, 9> Hxi) { + if (Hxi) *Hxi = ExpmapDerivative(xi); + + // Order is rotation, position, velocity, represented by ω,ρ,ν + Vector3 omega(xi(0), xi(1), xi(2)), rho(xi(3), xi(4), xi(5)), + nu(xi(6), xi(7), xi(8)); + + Rot3 R = Rot3::Expmap(omega); + + double omega_norm = omega.norm(); + + if (omega_norm < 1e-8) { + return NavState(Rot3(), Point3(rho), Point3(nu)); + + } else { + Matrix W = skewSymmetric(omega); + double omega_norm2 = omega_norm * omega_norm; + double omega_norm3 = omega_norm2 * omega_norm; + Matrix A = I_3x3 + ((1 - cos(omega_norm)) / omega_norm2) * W + + ((omega_norm - sin(omega_norm)) / omega_norm3) * (W * W); + + return NavState(Rot3::Expmap(omega), Point3(A * rho), Point3(A * nu)); + } +} + +//------------------------------------------------------------------------------ +Vector9 NavState::Logmap(const NavState& state, OptionalJacobian<9, 9> Hstate) { + if (Hstate) *Hstate = LogmapDerivative(state); + + const Vector3 phi = Rot3::Logmap(state.rotation()); + const Vector3& p = state.position(); + const Vector3& v = state.velocity(); + const double t = phi.norm(); + if (t < 1e-8) { + Vector9 log; + log << phi, p, v; + return log; + + } else { + const Matrix3 W = skewSymmetric(phi / t); + + const double Tan = tan(0.5 * t); + const Vector3 Wp = W * p; + const Vector3 Wv = W * v; + const Vector3 rho = p - (0.5 * t) * Wp + (1 - t / (2. * Tan)) * (W * Wp); + const Vector3 nu = v - (0.5 * t) * Wv + (1 - t / (2. * Tan)) * (W * Wv); + Vector9 log; + // Order is ω, p, v + log << phi, rho, nu; + return log; + } +} + +//------------------------------------------------------------------------------ +Matrix9 NavState::AdjointMap() const { + const Matrix3 R = R_.matrix(); + Matrix3 A = skewSymmetric(t_.x(), t_.y(), t_.z()) * R; + Matrix3 B = skewSymmetric(v_.x(), v_.y(), v_.z()) * R; + // Eqn 2 in Barrau20icra + Matrix9 adj; + adj << R, Z_3x3, Z_3x3, A, R, Z_3x3, B, Z_3x3, R; + return adj; +} + +//------------------------------------------------------------------------------ +Vector9 NavState::Adjoint(const Vector9& xi_b, OptionalJacobian<9, 9> H_state, + OptionalJacobian<9, 9> H_xib) const { + const Matrix9 Ad = AdjointMap(); + + // Jacobians + if (H_state) *H_state = -Ad * adjointMap(xi_b); + if (H_xib) *H_xib = Ad; + + return Ad * xi_b; +} + +//------------------------------------------------------------------------------ +Matrix9 NavState::adjointMap(const Vector9& xi) { + Matrix3 w_hat = skewSymmetric(xi(0), xi(1), xi(2)); + Matrix3 v_hat = skewSymmetric(xi(3), xi(4), xi(5)); + Matrix3 a_hat = skewSymmetric(xi(6), xi(7), xi(8)); + Matrix9 adj; + adj << w_hat, Z_3x3, Z_3x3, v_hat, w_hat, Z_3x3, a_hat, Z_3x3, w_hat; + return adj; +} + +//------------------------------------------------------------------------------ +Vector9 NavState::adjoint(const Vector9& xi, const Vector9& y, + OptionalJacobian<9, 9> Hxi, + OptionalJacobian<9, 9> H_y) { + if (Hxi) { + Hxi->setZero(); + for (int i = 0; i < 9; ++i) { + Vector9 dxi; + dxi.setZero(); + dxi(i) = 1.0; + Matrix9 Gi = adjointMap(dxi); + Hxi->col(i) = Gi * y; + } + } + + const Matrix9& ad_xi = adjointMap(xi); + if (H_y) *H_y = ad_xi; + + return ad_xi * y; +} + +/* ************************************************************************* */ +Matrix63 NavState::ComputeQforExpmapDerivative(const Vector9& xi, + double nearZeroThreshold) { + const auto omega = xi.head<3>(); + const auto nu = xi.segment<3>(3); + const auto rho = xi.tail<3>(); + const Matrix3 V = skewSymmetric(nu); + const Matrix3 P = skewSymmetric(rho); + const Matrix3 W = skewSymmetric(omega); + + Matrix3 Qv, Qp; + Matrix63 Q; + + // The closed-form formula in Barfoot14tro eq. (102) + double phi = omega.norm(); + if (std::abs(phi) > 1e-5) { + const double sinPhi = sin(phi), cosPhi = cos(phi); + const double phi2 = phi * phi, phi3 = phi2 * phi, phi4 = phi3 * phi, + phi5 = phi4 * phi; + // Invert the sign of odd-order terms to have the right Jacobian + Qv = -0.5 * V + (phi - sinPhi) / phi3 * (W * V + V * W - W * V * W) + + (1 - phi2 / 2 - cosPhi) / phi4 * + (W * W * V + V * W * W - 3 * W * V * W) - + 0.5 * + ((1 - phi2 / 2 - cosPhi) / phi4 - + 3 * (phi - sinPhi - phi3 / 6.) / phi5) * + (W * V * W * W + W * W * V * W); + Qp = -0.5 * P + (phi - sinPhi) / phi3 * (W * P + P * W - W * P * W) + + (1 - phi2 / 2 - cosPhi) / phi4 * + (W * W * P + P * W * W - 3 * W * P * W) - + 0.5 * + ((1 - phi2 / 2 - cosPhi) / phi4 - + 3 * (phi - sinPhi - phi3 / 6.) / phi5) * + (W * P * W * W + W * W * P * W); + } else { + Qv = -0.5 * V + 1. / 6. * (W * V + V * W - W * V * W) + + 1. / 24. * (W * W * V + V * W * W - 3 * W * V * W) - + 0.5 * (1. / 24. + 3. / 120.) * (W * V * W * W + W * W * V * W); + Qp = -0.5 * P + 1. / 6. * (W * P + P * W - W * P * W) + + 1. / 24. * (W * W * P + P * W * W - 3 * W * P * W) - + 0.5 * (1. / 24. + 3. / 120.) * (W * P * W * W + W * W * P * W); + } + + Q << Qv, Qp; + return Q; +} + +//------------------------------------------------------------------------------ +Matrix9 NavState::ExpmapDerivative(const Vector9& xi) { + const Vector3 w = xi.head<3>(); + const Matrix3 Jw = Rot3::ExpmapDerivative(w); + const Matrix63 Q = ComputeQforExpmapDerivative(xi); + const Matrix3 Qv = Q.topRows<3>(); + const Matrix3 Qp = Q.bottomRows<3>(); + + Matrix9 J; + J << Jw, Z_3x3, Z_3x3, Qv, Jw, Z_3x3, Qp, Z_3x3, Jw; + + return J; +} + +//------------------------------------------------------------------------------ +Matrix9 NavState::LogmapDerivative(const NavState& state) { + const Vector9 xi = Logmap(state); + const Vector3 w = xi.head<3>(); + const Matrix3 Jw = Rot3::LogmapDerivative(w); + const Matrix63 Q = ComputeQforExpmapDerivative(xi); + const Matrix3 Qv = Q.topRows<3>(); + const Matrix3 Qp = Q.bottomRows<3>(); + const Matrix3 Qv2 = -Jw * Qv * Jw; + const Matrix3 Qp2 = -Jw * Qp * Jw; + + Matrix9 J; + J << Jw, Z_3x3, Z_3x3, Qv2, Jw, Z_3x3, Qp2, Z_3x3, Jw; + return J; +} + + +//------------------------------------------------------------------------------ +NavState NavState::ChartAtOrigin::Retract(const Vector9& xi, + ChartJacobian Hxi) { + return Expmap(xi, Hxi); +} + +//------------------------------------------------------------------------------ +Vector9 NavState::ChartAtOrigin::Local(const NavState& state, + ChartJacobian Hstate) { + return Logmap(state, Hstate); +} + //------------------------------------------------------------------------------ NavState NavState::retract(const Vector9& xi, // OptionalJacobian<9, 9> H1, OptionalJacobian<9, 9> H2) const { + // return LieGroup::retract(xi, H1, H2); + Rot3 nRb = R_; Point3 n_t = t_, n_v = v_; Matrix3 D_bRc_xi, D_R_nRb, D_t_nRb, D_v_nRb; @@ -133,6 +342,30 @@ NavState NavState::retract(const Vector9& xi, // //------------------------------------------------------------------------------ Vector9 NavState::localCoordinates(const NavState& g, // OptionalJacobian<9, 9> H1, OptionalJacobian<9, 9> H2) const { + // return LieGroup::localCoordinates(g, H1, H2); + + //TODO(Varun) Fix so that test on L680 passes + + // Matrix3 D_dR_R, D_dt_R, D_dv_R; + // const Rot3 dR = R_.between(g.R_, H1 ? &D_dR_R : 0); + // const Point3 dP = R_.unrotate(g.t_ - t_, H1 ? &D_dt_R : 0); + // const Vector dV = R_.unrotate(g.v_ - v_, H1 ? &D_dv_R : 0); + + // Vector9 xi; + // Matrix3 D_xi_R; + // xi << Rot3::Logmap(dR, (H1 || H2) ? &D_xi_R : 0), dP, dV; + // if (H1) { + // *H1 << D_xi_R * D_dR_R, Z_3x3, Z_3x3, // + // D_dt_R, -I_3x3, Z_3x3, // + // D_dv_R, Z_3x3, -I_3x3; + // } + // if (H2) { + // *H2 << D_xi_R, Z_3x3, Z_3x3, // + // Z_3x3, dR.matrix(), Z_3x3, // + // Z_3x3, Z_3x3, dR.matrix(); + // } + // return xi; + Matrix3 D_dR_R, D_dt_R, D_dv_R; const Rot3 dR = R_.between(g.R_, H1 ? &D_dR_R : 0); const Point3 dP = R_.unrotate(g.t_ - t_, H1 ? &D_dt_R : 0); @@ -142,15 +375,16 @@ Vector9 NavState::localCoordinates(const NavState& g, // Matrix3 D_xi_R; xi << Rot3::Logmap(dR, (H1 || H2) ? &D_xi_R : 0), dP, dV; if (H1) { - *H1 << D_xi_R * D_dR_R, Z_3x3, Z_3x3, // - D_dt_R, -I_3x3, Z_3x3, // - D_dv_R, Z_3x3, -I_3x3; + *H1 << D_xi_R * D_dR_R, Z_3x3, Z_3x3, // + D_dt_R, -I_3x3, Z_3x3, // + D_dv_R, Z_3x3, -I_3x3; } if (H2) { - *H2 << D_xi_R, Z_3x3, Z_3x3, // - Z_3x3, dR.matrix(), Z_3x3, // - Z_3x3, Z_3x3, dR.matrix(); + *H2 << D_xi_R, Z_3x3, Z_3x3, // + Z_3x3, dR.matrix(), Z_3x3, // + Z_3x3, Z_3x3, dR.matrix(); } + return xi; } @@ -213,7 +447,8 @@ NavState NavState::update(const Vector3& b_acceleration, const Vector3& b_omega, //------------------------------------------------------------------------------ Vector9 NavState::coriolis(double dt, const Vector3& omega, bool secondOrder, OptionalJacobian<9, 9> H) const { - auto [nRb, n_t, n_v] = (*this); + Rot3 nRb = R_; + Point3 n_t = t_, n_v = v_; const double dt2 = dt * dt; const Vector3 omega_cross_vel = omega.cross(n_v); diff --git a/gtsam/navigation/NavState.h b/gtsam/navigation/NavState.h index 35ce435a3..2f037a795 100644 --- a/gtsam/navigation/NavState.h +++ b/gtsam/navigation/NavState.h @@ -12,7 +12,7 @@ /** * @file NavState.h * @brief Navigation state composing of attitude, position, and velocity - * @author Frank Dellaert + * @authors Frank Dellaert, Varun Agrawal, Fan Jiang * @date July 2015 **/ @@ -25,14 +25,18 @@ namespace gtsam { /// Velocity is currently typedef'd to Vector3 -typedef Vector3 Velocity3; +using Velocity3 = Vector3; /** * Navigation state: Pose (rotation, translation) + velocity - * NOTE(frank): it does not make sense to make this a Lie group, but it is a 9D manifold + * Following Barrau20icra, this class belongs to the Lie group SE_2(3). + * This group is also called "double direct isometries”. + * + * NOTE: While Barrau20icra follow a R,v,t order, + * we use a R,t,v order to maintain backwards compatibility. */ -class GTSAM_EXPORT NavState { -private: +class GTSAM_EXPORT NavState : public LieGroup { + private: // TODO(frank): // - should we rename t_ to p_? if not, we should rename dP do dT @@ -44,7 +48,6 @@ public: inline constexpr static auto dimension = 9; - typedef std::pair PositionAndVelocity; /// @name Constructors /// @{ @@ -67,11 +70,14 @@ public: } /// Named constructor with derivatives static NavState Create(const Rot3& R, const Point3& t, const Velocity3& v, - OptionalJacobian<9, 3> H1, OptionalJacobian<9, 3> H2, - OptionalJacobian<9, 3> H3); + OptionalJacobian<9, 3> H1 = {}, + OptionalJacobian<9, 3> H2 = {}, + OptionalJacobian<9, 3> H3 = {}); + /// Named constructor with derivatives static NavState FromPoseVelocity(const Pose3& pose, const Vector3& vel, - OptionalJacobian<9, 6> H1, OptionalJacobian<9, 3> H2); + OptionalJacobian<9, 6> H1 = {}, + OptionalJacobian<9, 3> H2 = {}); /// @} /// @name Component Access @@ -109,9 +115,8 @@ public: Velocity3 bodyVelocity(OptionalJacobian<3, 9> H = {}) const; /// Return matrix group representation, in MATLAB notation: - /// nTb = [nRb 0 n_t; 0 nRb n_v; 0 0 1] - /// With this embedding in GL(3), matrix product agrees with compose - Matrix7 matrix() const; + /// nTb = [nRb n_t n_v; 0_1x3 1 0; 0_1x3 0 1] + Matrix5 matrix() const; /// @} /// @name Testable @@ -128,7 +133,29 @@ public: bool equals(const NavState& other, double tol = 1e-8) const; /// @} - /// @name Manifold + /// @name Group + /// @{ + + /// identity for group operation + static NavState Identity() { + return NavState(); + } + + /// inverse transformation with derivatives + NavState inverse() const; + + using LieGroup::inverse; // version with derivative + + /// compose syntactic sugar + NavState operator*(const NavState& T) const { + return NavState(R_ * T.R_, t_ + R_ * T.t_, v_ + R_ * T.v_); + } + + /// Syntactic sugar + const Rot3& rotation() const { return attitude(); }; + + /// @} + /// @name Lie Group /// @{ // Tangent space sugar. @@ -162,6 +189,66 @@ public: OptionalJacobian<9, 9> H1 = {}, OptionalJacobian<9, 9> H2 = {}) const; + /** + * Exponential map at identity - create a NavState from canonical coordinates + * \f$ [R_x,R_y,R_z,T_x,T_y,T_z,V_x,V_y,V_z] \f$ + */ + static NavState Expmap(const Vector9& xi, OptionalJacobian<9, 9> Hxi = {}); + + /** + * Log map at identity - return the canonical coordinates \f$ + * [R_x,R_y,R_z,T_x,T_y,T_z,V_x,V_y,V_z] \f$ of this NavState + */ + static Vector9 Logmap(const NavState& pose, OptionalJacobian<9, 9> Hpose = {}); + + /** + * Calculate Adjoint map, transforming a twist in this pose's (i.e, body) + * frame to the world spatial frame. + */ + Matrix9 AdjointMap() const; + + /** + * Apply this NavState's AdjointMap Ad_g to a twist \f$ \xi_b \f$, i.e. a + * body-fixed velocity, transforming it to the spatial frame + * \f$ \xi^s = g*\xi^b*g^{-1} = Ad_g * \xi^b \f$ + * Note that H_xib = AdjointMap() + */ + Vector9 Adjoint(const Vector9& xi_b, + OptionalJacobian<9, 9> H_this = {}, + OptionalJacobian<9, 9> H_xib = {}) const; + + /** + * Compute the [ad(w,v)] operator as defined in [Kobilarov09siggraph], pg 11 + * but for the NavState [ad(w,v)] = [w^, zero3; v^, w^] + */ + static Matrix9 adjointMap(const Vector9& xi); + + /** + * Action of the adjointMap on a Lie-algebra vector y, with optional derivatives + */ + static Vector9 adjoint(const Vector9& xi, const Vector9& y, + OptionalJacobian<9, 9> Hxi = {}, + OptionalJacobian<9, 9> H_y = {}); + + /// Derivative of Expmap + static Matrix9 ExpmapDerivative(const Vector9& xi); + + /// Derivative of Logmap + static Matrix9 LogmapDerivative(const NavState& xi); + + // Chart at origin, depends on compile-time flag GTSAM_POSE3_EXPMAP + struct GTSAM_EXPORT ChartAtOrigin { + static NavState Retract(const Vector9& xi, ChartJacobian Hxi = {}); + static Vector9 Local(const NavState& state, ChartJacobian Hstate = {}); + }; + + /** + * Compute the 6x3 bottom-left block Qs of the SE_2(3) Expmap derivative + * matrix + */ + static Matrix63 ComputeQforExpmapDerivative(const Vector9& xi, + double nearZeroThreshold = 1e-5); + /// @} /// @name Dynamics /// @{ @@ -169,8 +256,9 @@ public: /// Integrate forward in time given angular velocity and acceleration in body frame /// Uses second order integration for position, returns derivatives except dt. NavState update(const Vector3& b_acceleration, const Vector3& b_omega, - const double dt, OptionalJacobian<9, 9> F, OptionalJacobian<9, 3> G1, - OptionalJacobian<9, 3> G2) const; + const double dt, OptionalJacobian<9, 9> F = {}, + OptionalJacobian<9, 3> G1 = {}, + OptionalJacobian<9, 3> G2 = {}) const; /// Compute tangent space contribution due to Coriolis forces Vector9 coriolis(double dt, const Vector3& omega, bool secondOrder = false, @@ -201,8 +289,10 @@ private: }; // Specialize NavState traits to use a Retract/Local that agrees with IMUFactors -template<> -struct traits : internal::Manifold { -}; +template <> +struct traits : public internal::LieGroup {}; + +template <> +struct traits : public internal::LieGroup {}; } // namespace gtsam diff --git a/gtsam/navigation/tests/testNavState.cpp b/gtsam/navigation/tests/testNavState.cpp index b8f081518..8934129ee 100644 --- a/gtsam/navigation/tests/testNavState.cpp +++ b/gtsam/navigation/tests/testNavState.cpp @@ -12,13 +12,16 @@ /** * @file testNavState.cpp * @brief Unit tests for NavState - * @author Frank Dellaert + * @authors Frank Dellaert, Varun Agrawal, Fan Jiang * @date July 2015 */ #include -#include + +#include #include +#include +#include #include @@ -26,6 +29,9 @@ using namespace std::placeholders; using namespace std; using namespace gtsam; +GTSAM_CONCEPT_TESTABLE_INST(NavState) +GTSAM_CONCEPT_LIE_INST(NavState) + static const Rot3 kAttitude = Rot3::RzRyRx(0.1, 0.2, 0.3); static const Point3 kPosition(1.0, 2.0, 3.0); static const Pose3 kPose(kAttitude, kPosition); @@ -36,6 +42,16 @@ static const Vector3 kOmegaCoriolis(0.02, 0.03, 0.04); static const Vector3 kGravity(0, 0, 9.81); static const Vector9 kZeroXi = Vector9::Zero(); +static const Point3 V(3, 0.4, -2.2); +static const Point3 P(0.2, 0.7, -2); +static const Rot3 R = Rot3::Rodrigues(0.3, 0, 0); +static const Point3 V2(-6.5, 3.5, 6.2); +static const Point3 P2(3.5, -8.2, 4.2); +static const NavState T(R, P2, V2); +static const NavState T2(Rot3::Rodrigues(0.3, 0.2, 0.1), P2, V2); +static const NavState T3(Rot3::Rodrigues(-90, 0, 0), Point3(5, 6, 7), + Point3(1, 2, 3)); + /* ************************************************************************* */ TEST(NavState, Constructor) { std::function create = @@ -46,14 +62,14 @@ TEST(NavState, Constructor) { assert_equal(kState1, NavState::Create(kAttitude, kPosition, kVelocity, aH1, aH2, aH3))); EXPECT( - assert_equal( - numericalDerivative31(create, kAttitude, kPosition, kVelocity), aH1)); +assert_equal( + numericalDerivative31(create, kAttitude, kPosition, kVelocity), aH1)); EXPECT( - assert_equal( - numericalDerivative32(create, kAttitude, kPosition, kVelocity), aH2)); +assert_equal( + numericalDerivative32(create, kAttitude, kPosition, kVelocity), aH2)); EXPECT( - assert_equal( - numericalDerivative32(create, kAttitude, kPosition, kVelocity), aH2)); +assert_equal( + numericalDerivative32(create, kAttitude, kPosition, kVelocity), aH2)); } /* ************************************************************************* */ @@ -64,7 +80,7 @@ TEST(NavState, Constructor2) { Matrix aH1, aH2; EXPECT( assert_equal(kState1, - NavState::FromPoseVelocity(kPose, kVelocity, aH1, aH2))); + NavState::FromPoseVelocity(kPose, kVelocity, aH1, aH2))); EXPECT(assert_equal(numericalDerivative21(construct, kPose, kVelocity), aH1)); EXPECT(assert_equal(numericalDerivative22(construct, kPose, kVelocity), aH2)); } @@ -127,8 +143,8 @@ TEST( NavState, Manifold ) { Point3 dt = Point3(xi.segment<3>(3)); Velocity3 dvel = Velocity3(-0.1, -0.2, -0.3); NavState state2 = NavState(kState1.attitude() * drot, - kState1.position() + kState1.attitude() * dt, - kState1.velocity() + kState1.attitude() * dvel); + kState1.position() + kState1.attitude() * dt, + kState1.velocity() + kState1.attitude() * dvel); EXPECT(assert_equal(state2, kState1.retract(xi))); EXPECT(assert_equal(xi, kState1.localCoordinates(state2))); @@ -169,6 +185,143 @@ TEST( NavState, Manifold ) { EXPECT(assert_equal(numericalDerivative22(local, state2, kIdentity), aH2)); } +/* ************************************************************************* */ +TEST(NavState, Equals) { + NavState T3(Rot3::Rodrigues(-90, 0, 0), Point3(5, 6, 7), Point3(1, 2, 3)); + NavState pose2 = T3; + EXPECT(T3.equals(pose2)); + NavState origin; + EXPECT(!T3.equals(origin)); +} + +/* ************************************************************************* */ +TEST(NavState, Compose) { + NavState nav_state_a(Rot3::Identity(), {0.0, 1.0, 2.0}, {1.0, -1.0, 1.0}); + NavState nav_state_b(Rot3::Rx(M_PI_4), {0.0, 1.0, 3.0}, {1.0, -1.0, 2.0}); + NavState nav_state_c(Rot3::Ry(M_PI / 180.0), {1.0, 1.0, 2.0}, + {3.0, -1.0, 1.0}); + + auto ab_c = (nav_state_a * nav_state_b) * nav_state_c; + auto a_bc = nav_state_a * (nav_state_b * nav_state_c); + CHECK(assert_equal(ab_c, a_bc)); + + Matrix actual = (T2 * T2).matrix(); + + Matrix expected = T2.matrix() * T2.matrix(); + EXPECT(assert_equal(actual, expected, 1e-8)); + + Matrix actualDcompose1, actualDcompose2; + T2.compose(T2, actualDcompose1, actualDcompose2); + + Matrix numericalH1 = + numericalDerivative21(testing::compose, T2, T2); + + EXPECT(assert_equal(numericalH1, actualDcompose1, 5e-3)); + EXPECT(assert_equal(T2.inverse().AdjointMap(), actualDcompose1, 5e-3)); + + Matrix numericalH2 = + numericalDerivative22(testing::compose, T2, T2); + EXPECT(assert_equal(numericalH2, actualDcompose2, 1e-4)); +} + +/* ************************************************************************* */ +// Check compose and its push-forward, another case +TEST(NavState, Compose2) { + const NavState& T1 = T; + Matrix actual = (T1 * T2).matrix(); + Matrix expected = T1.matrix() * T2.matrix(); + EXPECT(assert_equal(actual, expected, 1e-8)); + + Matrix actualDcompose1, actualDcompose2; + T1.compose(T2, actualDcompose1, actualDcompose2); + + Matrix numericalH1 = + numericalDerivative21(testing::compose, T1, T2); + EXPECT(assert_equal(numericalH1, actualDcompose1, 5e-3)); + EXPECT(assert_equal(T2.inverse().AdjointMap(), actualDcompose1, 5e-3)); + + Matrix numericalH2 = + numericalDerivative22(testing::compose, T1, T2); + EXPECT(assert_equal(numericalH2, actualDcompose2, 1e-5)); +} + +/* ************************************************************************* */ +TEST(NavState, Inverse) { + NavState nav_state_a(Rot3::Identity(), {0.0, 1.0, 2.0}, {1.0, -1.0, 1.0}); + NavState nav_state_b(Rot3::Rx(M_PI_4), {0.0, 1.0, 3.0}, {1.0, -1.0, 2.0}); + NavState nav_state_c(Rot3::Ry(M_PI / 180.0), {1.0, 1.0, 2.0}, + {3.0, -1.0, 1.0}); + + auto a_inv = nav_state_a.inverse(); + auto a_a_inv = nav_state_a * a_inv; + CHECK(assert_equal(a_a_inv, NavState())); + + auto b_inv = nav_state_b.inverse(); + auto b_b_inv = nav_state_b * b_inv; + CHECK(assert_equal(b_b_inv, NavState())); + + Matrix actualDinverse; + Matrix actual = T.inverse(actualDinverse).matrix(); + Matrix expected = T.matrix().inverse(); + EXPECT(assert_equal(actual, expected, 1e-8)); + + Matrix numericalH = numericalDerivative11(testing::inverse, T); + EXPECT(assert_equal(numericalH, actualDinverse, 5e-3)); + EXPECT(assert_equal(-T.AdjointMap(), actualDinverse, 5e-3)); +} + +/* ************************************************************************* */ +TEST(NavState, InverseDerivatives) { + Rot3 R = Rot3::Rodrigues(0.3, 0.4, -0.5); + Vector3 v(3.5, -8.2, 4.2); + Point3 p(3.5, -8.2, 4.2); + NavState T(R, p, v); + + Matrix numericalH = numericalDerivative11(testing::inverse, T); + Matrix actualDinverse; + T.inverse(actualDinverse); + EXPECT(assert_equal(numericalH, actualDinverse, 5e-3)); + EXPECT(assert_equal(-T.AdjointMap(), actualDinverse, 5e-3)); +} + +/* ************************************************************************* */ +TEST(NavState, Compose_Inverse) { + NavState actual = T * T.inverse(); + NavState expected; + EXPECT(assert_equal(actual, expected, 1e-8)); +} + +/* ************************************************************************* */ +TEST(NavState, Between) { + NavState s1, s2(Rot3(), Point3(1, 2, 3), Velocity3(0, 0, 0)); + + NavState actual = s1.compose(s2); + EXPECT(assert_equal(s2, actual)); + + NavState between = s2.between(s1); + NavState expected_between(Rot3(), Point3(-1, -2, -3), Velocity3(0, 0, 0)); + EXPECT(assert_equal(expected_between, between)); + + NavState expected = T2.inverse() * T3; + Matrix actualDBetween1, actualDBetween2; + actual = T2.between(T3, actualDBetween1, actualDBetween2); + EXPECT(assert_equal(expected, actual)); + + Matrix numericalH1 = + numericalDerivative21(testing::between, T2, T3); + EXPECT(assert_equal(numericalH1, actualDBetween1, 5e-3)); + + Matrix numericalH2 = + numericalDerivative22(testing::between, T2, T3); + EXPECT(assert_equal(numericalH2, actualDBetween2, 1e-5)); +} + +/* ************************************************************************* */ +TEST(NavState, interpolate) { + EXPECT(assert_equal(T2, interpolate(T2, T3, 0.0))); + EXPECT(assert_equal(T3, interpolate(T2, T3, 1.0))); +} + /* ************************************************************************* */ static const double dt = 2.0; std::function coriolis = @@ -189,7 +342,7 @@ TEST(NavState, Coriolis) { TEST(NavState, Coriolis2) { Matrix9 aH; NavState state2(Rot3::RzRyRx(M_PI / 12.0, M_PI / 6.0, M_PI / 4.0), - Point3(5.0, 1.0, -50.0), Vector3(0.5, 0.0, 0.0)); + Point3(5.0, 1.0, -50.0), Vector3(0.5, 0.0, 0.0)); // first-order state2.coriolis(dt, kOmegaCoriolis, false, aH); @@ -200,10 +353,10 @@ TEST(NavState, Coriolis2) { } TEST(NavState, Coriolis3) { - /** Consider a massless planet with an attached nav frame at - * n_omega = [0 0 1]', and a body at position n_t = [1 0 0]', travelling with + /** Consider a massless planet with an attached nav frame at + * n_omega = [0 0 1]', and a body at position n_t = [1 0 0]', travelling with * velocity n_v = [0 1 0]'. Orient the body so that it is not instantaneously - * aligned with the nav frame (i.e., nRb != I_3x3). Test that first and + * aligned with the nav frame (i.e., nRb != I_3x3). Test that first and * second order Coriolis corrections are as expected. */ @@ -216,9 +369,9 @@ TEST(NavState, Coriolis3) { bRn = nRb.inverse(); // Get expected first and second order corrections in the nav frame - Vector3 n_dP1e = 0.5 * dt2 * n_aCorr1, + Vector3 n_dP1e = 0.5 * dt2 * n_aCorr1, n_dP2e = 0.5 * dt2 * (n_aCorr1 + n_aCorr2), - n_dV1e = dt * n_aCorr1, + n_dV1e = dt * n_aCorr1, n_dV2e = dt * (n_aCorr1 + n_aCorr2); // Get expected first and second order corrections in the body frame @@ -271,6 +424,262 @@ TEST(NavState, Stream) EXPECT(os.str() == expected); } +/* ************************************************************************* */ +TEST(NavState, Print) { + NavState state(Rot3::Identity(), Point3(1, 2, 3), Vector3(1, 2, 3)); + + // Generate the expected output + std::string R = "R: [\n\t1, 0, 0;\n\t0, 1, 0;\n\t0, 0, 1\n]\n"; + std::string p = "p: 1 2 3\n"; + std::string v = "v: 1 2 3\n"; + std::string expected = R + p + v; + + EXPECT(assert_print_equal(expected, state)); +} + +/* ************************************************************************* */ +#ifndef GTSAM_POSE3_EXPMAP +TEST(NavState, Retract_first_order) { + NavState id; + Vector v = Z_9x1; + v(0) = 0.3; + EXPECT(assert_equal(NavState(R, Point3(0, 0, 0), Vector3(0, 0, 0)), + id.retract(v), 1e-2)); + v(3) = 0.2; + v(4) = 0.7; + v(5) = -2; + v(6) = 3; + v(7) = 0.4; + v(8) = -2.2; + EXPECT(assert_equal(NavState(R, P, V), id.retract(v), 1e-2)); +} +#endif + +/* ************************************************************************* */ +TEST(NavState, RetractExpmap) { + Vector xi = Z_9x1; + xi(0) = 0.3; + NavState pose = NavState::Expmap(xi), + expected(R, Point3(0, 0, 0), Point3(0, 0, 0)); + EXPECT(assert_equal(expected, pose, 1e-2)); + EXPECT(assert_equal(xi, NavState::Logmap(pose), 1e-2)); +} + +/* ************************************************************************* */ +TEST(NavState, Expmap_A_Full) { + NavState id; + Vector xi = Z_9x1; + xi(0) = 0.3; + EXPECT(assert_equal(expmap_default(id, xi), + NavState(R, Point3(0, 0, 0), Point3(0, 0, 0)))); + xi(3) = -0.2; + xi(4) = -0.394742; + xi(5) = 2.08998; + xi(6) = 0.2; + xi(7) = 0.394742; + xi(8) = -2.08998; + + NavState expected(R, -P, P); + EXPECT(assert_equal(expected, expmap_default(id, xi), 1e-5)); +} + +/* ************************************************************************* */ +TEST(NavState, Expmap_b) { + NavState p1(Rot3(), Point3(-100, 0, 0), Point3(100, 0, 0)); + NavState p2 = p1.retract( + (Vector(9) << 0.0, 0.0, 0.1, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0).finished()); + NavState expected(Rot3::Rodrigues(0.0, 0.0, 0.1), Point3(-100.0, 0.0, 0.0), + Point3(100.0, 0.0, 0.0)); + EXPECT(assert_equal(expected, p2, 1e-2)); +} + +/* ************************************************************************* */ +// test case for screw motion in the plane +namespace screwNavState { +double a = 0.3, c = cos(a), s = sin(a), w = 0.3; +Vector xi = (Vector(9) << 0.0, 0.0, w, w, 0.0, 1.0, w, 0.0, 1.0).finished(); +Rot3 expectedR(c, -s, 0, s, c, 0, 0, 0, 1); +Point3 expectedV(0.29552, 0.0446635, 1); +Point3 expectedP(0.29552, 0.0446635, 1); +NavState expected(expectedR, expectedV, expectedP); +} // namespace screwNavState + +/* ************************************************************************* */ +// assert that T*exp(xi)*T^-1 is equal to exp(Ad_T(xi)) +TEST(NavState, Adjoint_full) { + NavState expected = T * NavState::Expmap(screwNavState::xi) * T.inverse(); + Vector xiPrime = T.Adjoint(screwNavState::xi); + EXPECT(assert_equal(expected, NavState::Expmap(xiPrime), 1e-6)); + + NavState expected2 = T2 * NavState::Expmap(screwNavState::xi) * T2.inverse(); + Vector xiPrime2 = T2.Adjoint(screwNavState::xi); + EXPECT(assert_equal(expected2, NavState::Expmap(xiPrime2), 1e-6)); + + NavState expected3 = T3 * NavState::Expmap(screwNavState::xi) * T3.inverse(); + Vector xiPrime3 = T3.Adjoint(screwNavState::xi); + EXPECT(assert_equal(expected3, NavState::Expmap(xiPrime3), 1e-6)); +} + +/* ************************************************************************* */ +TEST(NavState, Adjoint_compose_full) { + // To debug derivatives of compose, assert that + // T1*T2*exp(Adjoint(inv(T2),x) = T1*exp(x)*T2 + const NavState& T1 = T; + Vector x = + (Vector(9) << 0.1, 0.1, 0.1, 0.4, 0.2, 0.8, 0.4, 0.2, 0.8).finished(); + NavState expected = T1 * NavState::Expmap(x) * T2; + Vector y = T2.inverse().Adjoint(x); + NavState actual = T1 * T2 * NavState::Expmap(y); + EXPECT(assert_equal(expected, actual, 1e-6)); +} + +/* ************************************************************************* */ +TEST(NavState, Retract_LocalCoordinates) { + Vector9 d; + d << 1, 2, 3, 4, 5, 6, 7, 8, 9; + d /= 10; + const Rot3 R = Rot3::Retract(d.head<3>()); + NavState t = NavState::Retract(d); + EXPECT(assert_equal(d, NavState::LocalCoordinates(t))); +} +/* ************************************************************************* */ +TEST(NavState, retract_localCoordinates) { + Vector9 d12; + d12 << 1, 2, 3, 4, 5, 6, 7, 8, 9; + d12 /= 10; + NavState t1 = T, t2 = t1.retract(d12); + EXPECT(assert_equal(d12, t1.localCoordinates(t2))); +} +/* ************************************************************************* */ +TEST(NavState, expmap_logmap) { + Vector d12 = Vector9::Constant(0.1); + NavState t1 = T, t2 = t1.expmap(d12); + EXPECT(assert_equal(d12, t1.logmap(t2))); +} + +/* ************************************************************************* */ +TEST(NavState, retract_localCoordinates2) { + NavState t1 = T; + NavState t2 = T3; + NavState origin; + Vector d12 = t1.localCoordinates(t2); + EXPECT(assert_equal(t2, t1.retract(d12))); + Vector d21 = t2.localCoordinates(t1); + EXPECT(assert_equal(t1, t2.retract(d21))); + // EXPECT(assert_equal(d12, -d21)); +} +/* ************************************************************************* */ +TEST(NavState, manifold_expmap) { + NavState t1 = T; + NavState t2 = T3; + NavState origin; + Vector d12 = t1.logmap(t2); + EXPECT(assert_equal(t2, t1.expmap(d12))); + Vector d21 = t2.logmap(t1); + EXPECT(assert_equal(t1, t2.expmap(d21))); + + // Check that log(t1,t2)=-log(t2,t1) + EXPECT(assert_equal(d12, -d21)); +} + +/* ************************************************************************* */ +TEST(NavState, subgroups) { + // Frank - Below only works for correct "Agrawal06iros style expmap + // lines in canonical coordinates correspond to Abelian subgroups in SE(3) + Vector d = + (Vector(9) << 0.1, 0.2, 0.3, 0.4, 0.5, 0.6, 0.7, 0.8, 0.9).finished(); + // exp(-d)=inverse(exp(d)) + EXPECT(assert_equal(NavState::Expmap(-d), NavState::Expmap(d).inverse())); + // exp(5d)=exp(2*d+3*d)=exp(2*d)exp(3*d)=exp(3*d)exp(2*d) + NavState T2 = NavState::Expmap(2 * d); + NavState T3 = NavState::Expmap(3 * d); + NavState T5 = NavState::Expmap(5 * d); + EXPECT(assert_equal(T5, T2 * T3)); + EXPECT(assert_equal(T5, T3 * T2)); +} + +/* ************************************************************************* */ +TEST(NavState, adjointMap) { + Matrix res = NavState::adjointMap(screwNavState::xi); + Matrix wh = skewSymmetric(screwNavState::xi(0), screwNavState::xi(1), + screwNavState::xi(2)); + Matrix vh = skewSymmetric(screwNavState::xi(3), screwNavState::xi(4), + screwNavState::xi(5)); + Matrix rh = skewSymmetric(screwNavState::xi(6), screwNavState::xi(7), + screwNavState::xi(8)); + Matrix9 expected; + expected << wh, Z_3x3, Z_3x3, vh, wh, Z_3x3, rh, Z_3x3, wh; + EXPECT(assert_equal(expected, res, 1e-5)); +} + +/* ************************************************************************* */ +TEST(NavState, ExpmapDerivative1) { + Matrix9 actualH; + Vector9 w; + w << 0.1, 0.2, 0.3, 4.0, 5.0, 6.0, 7.0, 8.0, 9.0; + NavState::Expmap(w, actualH); + + std::function f = [](const Vector9& w) { + return NavState::Expmap(w); + }; + Matrix expectedH = + numericalDerivative21 >( + &NavState::Expmap, w, {}); + EXPECT(assert_equal(expectedH, actualH)); +} + +/* ************************************************************************* */ +TEST(NavState, LogmapDerivative) { + Matrix9 actualH; + Vector9 w; + w << 0.1, 0.2, 0.3, 4.0, 5.0, 6.0, 7.0, 8.0, 9.0; + NavState p = NavState::Expmap(w); + EXPECT(assert_equal(w, NavState::Logmap(p, actualH), 1e-5)); + + std::function f = [](const NavState& p) { + return NavState::Logmap(p); + }; + Matrix expectedH = + numericalDerivative21 >( + &NavState::Logmap, p, {}); + EXPECT(assert_equal(expectedH, actualH)); +} + +//****************************************************************************** +TEST(NavState, Invariants) { + NavState id; + + EXPECT(check_group_invariants(id, id)); + EXPECT(check_group_invariants(id, T3)); + EXPECT(check_group_invariants(T2, id)); + EXPECT(check_group_invariants(T2, T3)); + + EXPECT(check_manifold_invariants(id, id)); + EXPECT(check_manifold_invariants(id, T3)); + EXPECT(check_manifold_invariants(T2, id)); + EXPECT(check_manifold_invariants(T2, T3)); +} + +//****************************************************************************** +TEST(NavState, LieGroupDerivatives) { + NavState id; + + CHECK_LIE_GROUP_DERIVATIVES(id, id); + CHECK_LIE_GROUP_DERIVATIVES(id, T2); + CHECK_LIE_GROUP_DERIVATIVES(T2, id); + CHECK_LIE_GROUP_DERIVATIVES(T2, T3); +} + +//****************************************************************************** +TEST(NavState, ChartDerivatives) { + NavState id; + if (ROT3_DEFAULT_COORDINATES_MODE == Rot3::EXPMAP) { + CHECK_CHART_DERIVATIVES(id, id); + // CHECK_CHART_DERIVATIVES(id,T2); + // CHECK_CHART_DERIVATIVES(T2,id); + // CHECK_CHART_DERIVATIVES(T2,T3); + } +} /* ************************************************************************* */ int main() { From d0c1854e63999f0b8e394e8ae20ad4af271f22a5 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 14 Dec 2024 01:04:05 -0500 Subject: [PATCH 22/80] ExpmapTranslation --- gtsam/geometry/Pose3.cpp | 120 +++++++++++++++++++++------------------ gtsam/geometry/Pose3.h | 15 +++++ 2 files changed, 80 insertions(+), 55 deletions(-) diff --git a/gtsam/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp index 894314491..15eddf7a7 100644 --- a/gtsam/geometry/Pose3.cpp +++ b/gtsam/geometry/Pose3.cpp @@ -26,8 +26,6 @@ namespace gtsam { -using std::vector; - /** instantiate concept checks */ GTSAM_CONCEPT_POSE_INST(Pose3) @@ -167,21 +165,23 @@ Pose3 Pose3::interpolateRt(const Pose3& T, double t) const { /* ************************************************************************* */ /** Modified from Murray94book version (which assumes w and v normalized?) */ Pose3 Pose3::Expmap(const Vector6& xi, OptionalJacobian<6, 6> Hxi) { - if (Hxi) *Hxi = ExpmapDerivative(xi); + // Get angular velocity omega and translational velocity v from twist xi + const Vector3 w = xi.head<3>(), v = xi.tail<3>(); - // get angular velocity omega and translational velocity v from twist xi - Vector3 omega(xi(0), xi(1), xi(2)), v(xi(3), xi(4), xi(5)); + // Compute rotation using Expmap + Rot3 R = Rot3::Expmap(w); - Rot3 R = Rot3::Expmap(omega); - double theta2 = omega.dot(omega); - if (theta2 > std::numeric_limits::epsilon()) { - Vector3 t_parallel = omega * omega.dot(v); // translation parallel to axis - Vector3 omega_cross_v = omega.cross(v); // points towards axis - Vector3 t = (omega_cross_v - R * omega_cross_v + t_parallel) / theta2; - return Pose3(R, t); - } else { - return Pose3(R, v); + // Compute translation and optionally its Jacobian in w + Matrix3 Q; + const Vector3 t = ExpmapTranslation(w, v, Hxi ? &Q : nullptr, R); + + if (Hxi) { + const Matrix3 Jw = Rot3::ExpmapDerivative(w); + *Hxi << Jw, Z_3x3, + Q, Jw; } + + return Pose3(R, t); } /* ************************************************************************* */ @@ -240,55 +240,65 @@ Vector6 Pose3::ChartAtOrigin::Local(const Pose3& pose, ChartJacobian Hpose) { } /* ************************************************************************* */ -Matrix3 Pose3::ComputeQforExpmapDerivative(const Vector6& xi, double nearZeroThreshold) { +Matrix3 Pose3::ComputeQforExpmapDerivative(const Vector6& xi, + double nearZeroThreshold) { + Matrix3 Q; const auto w = xi.head<3>(); const auto v = xi.tail<3>(); - const Matrix3 V = skewSymmetric(v); - const Matrix3 W = skewSymmetric(w); - - Matrix3 Q; - -#ifdef NUMERICAL_EXPMAP_DERIV - Matrix3 Qj = Z_3x3; - double invFac = 1.0; - Q = Z_3x3; - Matrix3 Wj = I_3x3; - for (size_t j=1; j<10; ++j) { - Qj = Qj*W + Wj*V; - invFac = -invFac/(j+1); - Q = Q + invFac*Qj; - Wj = Wj*W; - } -#else - // The closed-form formula in Barfoot14tro eq. (102) - double phi = w.norm(); - const Matrix3 WVW = W * V * W; - if (std::abs(phi) > nearZeroThreshold) { - const double s = sin(phi), c = cos(phi); - const double phi2 = phi * phi, phi3 = phi2 * phi, phi4 = phi3 * phi, - phi5 = phi4 * phi; - // Invert the sign of odd-order terms to have the right Jacobian - Q = -0.5 * V + (phi - s) / phi3 * (W * V + V * W - WVW) + - (1 - phi2 / 2 - c) / phi4 * (W * W * V + V * W * W - 3 * WVW) - - 0.5 * ((1 - phi2 / 2 - c) / phi4 - 3 * (phi - s - phi3 / 6.) / phi5) * - (WVW * W + W * WVW); - } else { - Q = -0.5 * V + 1. / 6. * (W * V + V * W - WVW) - - 1. / 24. * (W * W * V + V * W * W - 3 * WVW) + - 1. / 120. * (WVW * W + W * WVW); - } -#endif - + ExpmapTranslation(w, v, Q, {}, nearZeroThreshold); return Q; } +/* ************************************************************************* */ +Vector3 Pose3::ExpmapTranslation(const Vector3& w, const Vector3& v, + OptionalJacobian<3, 3> Q, + const std::optional& R, + double nearZeroThreshold) { + double phi2 = w.dot(w); + + if (Q) { + const Matrix3 V = skewSymmetric(v); + const Matrix3 W = skewSymmetric(w); + const Matrix3 WVW = W * V * W; + const double phi = w.norm(); + + if (std::abs(phi) > nearZeroThreshold) { + const double s = sin(phi), c = cos(phi); + const double phi3 = phi2 * phi, phi4 = phi3 * phi, phi5 = phi4 * phi; + + // Invert the sign of odd-order terms to have the right Jacobian + *Q = -0.5 * V + (phi - s) / phi3 * (W * V + V * W - WVW) + + (1 - phi2 / 2 - c) / phi4 * (W * W * V + V * W * W - 3 * WVW) - + 0.5 * + ((1 - phi2 / 2 - c) / phi4 - 3 * (phi - s - phi3 / 6.) / phi5) * + (WVW * W + W * WVW); + } else { + constexpr double one_sixth = 1. / 6.; + constexpr double one_twenty_fourth = 1. / 24.; + constexpr double one_one_hundred_twentieth = 1. / 120.; + + *Q = -0.5 * V + one_sixth * (W * V + V * W - WVW) - + one_twenty_fourth * (W * W * V + V * W * W - 3 * WVW) + + one_one_hundred_twentieth * (WVW * W + W * WVW); + } + } + + // TODO(Frank): this threshold is *different*. Why? + if (phi2 > std::numeric_limits::epsilon()) { + Vector3 t_parallel = w * w.dot(v); // translation parallel to axis + Vector3 w_cross_v = w.cross(v); // translation orthogonal to axis + Rot3 rotation = R.value_or(Rot3::Expmap(w)); + Vector3 t = (w_cross_v - rotation * w_cross_v + t_parallel) / phi2; + return t; + } else { + return v; + } +} + /* ************************************************************************* */ Matrix6 Pose3::ExpmapDerivative(const Vector6& xi) { - const Vector3 w = xi.head<3>(); - const Matrix3 Jw = Rot3::ExpmapDerivative(w); - const Matrix3 Q = ComputeQforExpmapDerivative(xi); Matrix6 J; - J << Jw, Z_3x3, Q, Jw; + Expmap(xi, J); return J; } diff --git a/gtsam/geometry/Pose3.h b/gtsam/geometry/Pose3.h index 8a807cc23..d7c280c80 100644 --- a/gtsam/geometry/Pose3.h +++ b/gtsam/geometry/Pose3.h @@ -217,10 +217,25 @@ public: * (see Chirikjian11book2, pg 44, eq 10.95. * The closed-form formula is identical to formula 102 in Barfoot14tro where * Q_l of the SE3 Expmap left derivative matrix is given. + * This is the Jacobian of ExpmapTranslation and computed there. */ static Matrix3 ComputeQforExpmapDerivative( const Vector6& xi, double nearZeroThreshold = 1e-5); + /** + * Compute the translation part of the exponential map, with derivative. + * @param w 3D angular velocity + * @param v 3D velocity + * @param Q Optionally, compute 3x3 Jacobian wrpt w + * @param R Optionally, precomputed as Rot3::Expmap(w) + * @param nearZeroThreshold threshold for small values + * Note Q is 3x3 bottom-left block of SE3 Expmap right derivative matrix + */ + static Vector3 ExpmapTranslation(const Vector3& w, const Vector3& v, + OptionalJacobian<3, 3> Q = {}, + const std::optional& R = {}, + double nearZeroThreshold = 1e-5); + using LieGroup::inverse; // version with derivative /** From 693eafb0747b6fbadeaf9be4c3a5b48ccfbc1dae Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 14 Dec 2024 01:42:35 -0500 Subject: [PATCH 23/80] Use ExpmapTranslation --- gtsam/navigation/NavState.cpp | 108 ++++++++++------------------------ gtsam/navigation/NavState.h | 7 --- 2 files changed, 30 insertions(+), 85 deletions(-) diff --git a/gtsam/navigation/NavState.cpp b/gtsam/navigation/NavState.cpp index 9f91ae415..f7e0d48be 100644 --- a/gtsam/navigation/NavState.cpp +++ b/gtsam/navigation/NavState.cpp @@ -19,6 +19,7 @@ #include #include +#include "gtsam/geometry/Rot3.h" namespace gtsam { @@ -114,28 +115,27 @@ NavState NavState::inverse() const { //------------------------------------------------------------------------------ NavState NavState::Expmap(const Vector9& xi, OptionalJacobian<9, 9> Hxi) { - if (Hxi) *Hxi = ExpmapDerivative(xi); + // Get angular velocity w, translational velocity v, and acceleration a + Vector3 w = xi.head<3>(); + Vector3 rho = xi.segment<3>(3); + Vector3 nu = xi.tail<3>(); - // Order is rotation, position, velocity, represented by ω,ρ,ν - Vector3 omega(xi(0), xi(1), xi(2)), rho(xi(3), xi(4), xi(5)), - nu(xi(6), xi(7), xi(8)); + // Compute rotation using Expmap + Rot3 R = Rot3::Expmap(w); - Rot3 R = Rot3::Expmap(omega); + // Compute translations and optionally their Jacobians + Matrix3 Qt, Qv; + Vector3 t = Pose3::ExpmapTranslation(w, rho, Hxi ? &Qt : nullptr, R); + Vector3 v = Pose3::ExpmapTranslation(w, nu, Hxi ? &Qv : nullptr, R); - double omega_norm = omega.norm(); - - if (omega_norm < 1e-8) { - return NavState(Rot3(), Point3(rho), Point3(nu)); - - } else { - Matrix W = skewSymmetric(omega); - double omega_norm2 = omega_norm * omega_norm; - double omega_norm3 = omega_norm2 * omega_norm; - Matrix A = I_3x3 + ((1 - cos(omega_norm)) / omega_norm2) * W + - ((omega_norm - sin(omega_norm)) / omega_norm3) * (W * W); - - return NavState(Rot3::Expmap(omega), Point3(A * rho), Point3(A * nu)); + if (Hxi) { + const Matrix3 Jw = Rot3::ExpmapDerivative(w); + *Hxi << Jw, Z_3x3, Z_3x3, + Qt, Jw, Z_3x3, + Qv, Z_3x3, Jw; } + + return NavState(R, t, v); } //------------------------------------------------------------------------------ @@ -220,64 +220,10 @@ Vector9 NavState::adjoint(const Vector9& xi, const Vector9& y, return ad_xi * y; } -/* ************************************************************************* */ -Matrix63 NavState::ComputeQforExpmapDerivative(const Vector9& xi, - double nearZeroThreshold) { - const auto omega = xi.head<3>(); - const auto nu = xi.segment<3>(3); - const auto rho = xi.tail<3>(); - const Matrix3 V = skewSymmetric(nu); - const Matrix3 P = skewSymmetric(rho); - const Matrix3 W = skewSymmetric(omega); - - Matrix3 Qv, Qp; - Matrix63 Q; - - // The closed-form formula in Barfoot14tro eq. (102) - double phi = omega.norm(); - if (std::abs(phi) > 1e-5) { - const double sinPhi = sin(phi), cosPhi = cos(phi); - const double phi2 = phi * phi, phi3 = phi2 * phi, phi4 = phi3 * phi, - phi5 = phi4 * phi; - // Invert the sign of odd-order terms to have the right Jacobian - Qv = -0.5 * V + (phi - sinPhi) / phi3 * (W * V + V * W - W * V * W) + - (1 - phi2 / 2 - cosPhi) / phi4 * - (W * W * V + V * W * W - 3 * W * V * W) - - 0.5 * - ((1 - phi2 / 2 - cosPhi) / phi4 - - 3 * (phi - sinPhi - phi3 / 6.) / phi5) * - (W * V * W * W + W * W * V * W); - Qp = -0.5 * P + (phi - sinPhi) / phi3 * (W * P + P * W - W * P * W) + - (1 - phi2 / 2 - cosPhi) / phi4 * - (W * W * P + P * W * W - 3 * W * P * W) - - 0.5 * - ((1 - phi2 / 2 - cosPhi) / phi4 - - 3 * (phi - sinPhi - phi3 / 6.) / phi5) * - (W * P * W * W + W * W * P * W); - } else { - Qv = -0.5 * V + 1. / 6. * (W * V + V * W - W * V * W) + - 1. / 24. * (W * W * V + V * W * W - 3 * W * V * W) - - 0.5 * (1. / 24. + 3. / 120.) * (W * V * W * W + W * W * V * W); - Qp = -0.5 * P + 1. / 6. * (W * P + P * W - W * P * W) + - 1. / 24. * (W * W * P + P * W * W - 3 * W * P * W) - - 0.5 * (1. / 24. + 3. / 120.) * (W * P * W * W + W * W * P * W); - } - - Q << Qv, Qp; - return Q; -} - //------------------------------------------------------------------------------ Matrix9 NavState::ExpmapDerivative(const Vector9& xi) { - const Vector3 w = xi.head<3>(); - const Matrix3 Jw = Rot3::ExpmapDerivative(w); - const Matrix63 Q = ComputeQforExpmapDerivative(xi); - const Matrix3 Qv = Q.topRows<3>(); - const Matrix3 Qp = Q.bottomRows<3>(); - Matrix9 J; - J << Jw, Z_3x3, Z_3x3, Qv, Jw, Z_3x3, Qp, Z_3x3, Jw; - + Expmap(xi, J); return J; } @@ -285,15 +231,21 @@ Matrix9 NavState::ExpmapDerivative(const Vector9& xi) { Matrix9 NavState::LogmapDerivative(const NavState& state) { const Vector9 xi = Logmap(state); const Vector3 w = xi.head<3>(); + Vector3 rho = xi.segment<3>(3); + Vector3 nu = xi.tail<3>(); + + Matrix3 Qt, Qv; + const Rot3 R = Rot3::Expmap(w); + Pose3::ExpmapTranslation(w, rho, Qt, R); + Pose3::ExpmapTranslation(w, nu, Qv, R); const Matrix3 Jw = Rot3::LogmapDerivative(w); - const Matrix63 Q = ComputeQforExpmapDerivative(xi); - const Matrix3 Qv = Q.topRows<3>(); - const Matrix3 Qp = Q.bottomRows<3>(); + const Matrix3 Qt2 = -Jw * Qt * Jw; const Matrix3 Qv2 = -Jw * Qv * Jw; - const Matrix3 Qp2 = -Jw * Qp * Jw; Matrix9 J; - J << Jw, Z_3x3, Z_3x3, Qv2, Jw, Z_3x3, Qp2, Z_3x3, Jw; + J << Jw, Z_3x3, Z_3x3, + Qt2, Jw, Z_3x3, + Qv2, Z_3x3, Jw; return J; } diff --git a/gtsam/navigation/NavState.h b/gtsam/navigation/NavState.h index 2f037a795..95ee71fe9 100644 --- a/gtsam/navigation/NavState.h +++ b/gtsam/navigation/NavState.h @@ -242,13 +242,6 @@ public: static Vector9 Local(const NavState& state, ChartJacobian Hstate = {}); }; - /** - * Compute the 6x3 bottom-left block Qs of the SE_2(3) Expmap derivative - * matrix - */ - static Matrix63 ComputeQforExpmapDerivative(const Vector9& xi, - double nearZeroThreshold = 1e-5); - /// @} /// @name Dynamics /// @{ From 3eba54c05cf703f3a105d11214345afe4f269dbb Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 14 Dec 2024 01:54:29 -0500 Subject: [PATCH 24/80] Remove TODO --- gtsam/navigation/NavState.cpp | 24 ------------------------ 1 file changed, 24 deletions(-) diff --git a/gtsam/navigation/NavState.cpp b/gtsam/navigation/NavState.cpp index f7e0d48be..6234b9c27 100644 --- a/gtsam/navigation/NavState.cpp +++ b/gtsam/navigation/NavState.cpp @@ -294,30 +294,6 @@ NavState NavState::retract(const Vector9& xi, // //------------------------------------------------------------------------------ Vector9 NavState::localCoordinates(const NavState& g, // OptionalJacobian<9, 9> H1, OptionalJacobian<9, 9> H2) const { - // return LieGroup::localCoordinates(g, H1, H2); - - //TODO(Varun) Fix so that test on L680 passes - - // Matrix3 D_dR_R, D_dt_R, D_dv_R; - // const Rot3 dR = R_.between(g.R_, H1 ? &D_dR_R : 0); - // const Point3 dP = R_.unrotate(g.t_ - t_, H1 ? &D_dt_R : 0); - // const Vector dV = R_.unrotate(g.v_ - v_, H1 ? &D_dv_R : 0); - - // Vector9 xi; - // Matrix3 D_xi_R; - // xi << Rot3::Logmap(dR, (H1 || H2) ? &D_xi_R : 0), dP, dV; - // if (H1) { - // *H1 << D_xi_R * D_dR_R, Z_3x3, Z_3x3, // - // D_dt_R, -I_3x3, Z_3x3, // - // D_dv_R, Z_3x3, -I_3x3; - // } - // if (H2) { - // *H2 << D_xi_R, Z_3x3, Z_3x3, // - // Z_3x3, dR.matrix(), Z_3x3, // - // Z_3x3, Z_3x3, dR.matrix(); - // } - // return xi; - Matrix3 D_dR_R, D_dt_R, D_dv_R; const Rot3 dR = R_.between(g.R_, H1 ? &D_dR_R : 0); const Point3 dP = R_.unrotate(g.t_ - t_, H1 ? &D_dt_R : 0); From dddb6acde08a31d8df02577110c67625f88df384 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 14 Dec 2024 01:55:16 -0500 Subject: [PATCH 25/80] Remove extra include --- gtsam/navigation/NavState.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/gtsam/navigation/NavState.cpp b/gtsam/navigation/NavState.cpp index 6234b9c27..7c7aa373b 100644 --- a/gtsam/navigation/NavState.cpp +++ b/gtsam/navigation/NavState.cpp @@ -19,7 +19,6 @@ #include #include -#include "gtsam/geometry/Rot3.h" namespace gtsam { From 8193bdbec31158c4c18566aa64e96ac5c5d7b5c7 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 15 Dec 2024 12:11:48 -0500 Subject: [PATCH 26/80] Fix nearZero of SE(3) and SE_2(3) --- gtsam/geometry/Pose3.cpp | 44 +++++++++++++++++++++------------------- 1 file changed, 23 insertions(+), 21 deletions(-) diff --git a/gtsam/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp index 15eddf7a7..bb1483432 100644 --- a/gtsam/geometry/Pose3.cpp +++ b/gtsam/geometry/Pose3.cpp @@ -21,7 +21,6 @@ #include #include -#include #include namespace gtsam { @@ -254,44 +253,47 @@ Vector3 Pose3::ExpmapTranslation(const Vector3& w, const Vector3& v, OptionalJacobian<3, 3> Q, const std::optional& R, double nearZeroThreshold) { - double phi2 = w.dot(w); + const double theta2 = w.dot(w); + bool nearZero = (theta2 <= nearZeroThreshold); if (Q) { const Matrix3 V = skewSymmetric(v); const Matrix3 W = skewSymmetric(w); const Matrix3 WVW = W * V * W; - const double phi = w.norm(); + const double theta = w.norm(); - if (std::abs(phi) > nearZeroThreshold) { - const double s = sin(phi), c = cos(phi); - const double phi3 = phi2 * phi, phi4 = phi3 * phi, phi5 = phi4 * phi; - - // Invert the sign of odd-order terms to have the right Jacobian - *Q = -0.5 * V + (phi - s) / phi3 * (W * V + V * W - WVW) + - (1 - phi2 / 2 - c) / phi4 * (W * W * V + V * W * W - 3 * WVW) - - 0.5 * - ((1 - phi2 / 2 - c) / phi4 - 3 * (phi - s - phi3 / 6.) / phi5) * - (WVW * W + W * WVW); - } else { - constexpr double one_sixth = 1. / 6.; - constexpr double one_twenty_fourth = 1. / 24.; - constexpr double one_one_hundred_twentieth = 1. / 120.; + if (nearZero) { + static constexpr double one_sixth = 1. / 6.; + static constexpr double one_twenty_fourth = 1. / 24.; + static constexpr double one_one_hundred_twentieth = 1. / 120.; *Q = -0.5 * V + one_sixth * (W * V + V * W - WVW) - one_twenty_fourth * (W * W * V + V * W * W - 3 * WVW) + one_one_hundred_twentieth * (WVW * W + W * WVW); + } else { + const double s = sin(theta), c = cos(theta); + const double theta3 = theta2 * theta, theta4 = theta3 * theta, + theta5 = theta4 * theta; + + // Invert the sign of odd-order terms to have the right Jacobian + *Q = -0.5 * V + (theta - s) / theta3 * (W * V + V * W - WVW) + + (1 - theta2 / 2 - c) / theta4 * (W * W * V + V * W * W - 3 * WVW) - + 0.5 * + ((1 - theta2 / 2 - c) / theta4 - + 3 * (theta - s - theta3 / 6.) / theta5) * + (WVW * W + W * WVW); } } // TODO(Frank): this threshold is *different*. Why? - if (phi2 > std::numeric_limits::epsilon()) { + if (nearZero) { + return v + 0.5 * w.cross(v); + } else { Vector3 t_parallel = w * w.dot(v); // translation parallel to axis Vector3 w_cross_v = w.cross(v); // translation orthogonal to axis Rot3 rotation = R.value_or(Rot3::Expmap(w)); - Vector3 t = (w_cross_v - rotation * w_cross_v + t_parallel) / phi2; + Vector3 t = (w_cross_v - rotation * w_cross_v + t_parallel) / theta2; return t; - } else { - return v; } } From f7a678e54e551b66b8aec8325f5c5085e903d69a Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 15 Dec 2024 12:11:56 -0500 Subject: [PATCH 27/80] Address comments --- gtsam/navigation/NavState.cpp | 2 -- gtsam/navigation/tests/testNavState.cpp | 10 +++++----- 2 files changed, 5 insertions(+), 7 deletions(-) diff --git a/gtsam/navigation/NavState.cpp b/gtsam/navigation/NavState.cpp index 7c7aa373b..b9d48a3cb 100644 --- a/gtsam/navigation/NavState.cpp +++ b/gtsam/navigation/NavState.cpp @@ -264,8 +264,6 @@ Vector9 NavState::ChartAtOrigin::Local(const NavState& state, //------------------------------------------------------------------------------ NavState NavState::retract(const Vector9& xi, // OptionalJacobian<9, 9> H1, OptionalJacobian<9, 9> H2) const { - // return LieGroup::retract(xi, H1, H2); - Rot3 nRb = R_; Point3 n_t = t_, n_v = v_; Matrix3 D_bRc_xi, D_R_nRb, D_t_nRb, D_v_nRb; diff --git a/gtsam/navigation/tests/testNavState.cpp b/gtsam/navigation/tests/testNavState.cpp index 8934129ee..427531415 100644 --- a/gtsam/navigation/tests/testNavState.cpp +++ b/gtsam/navigation/tests/testNavState.cpp @@ -490,7 +490,7 @@ TEST(NavState, Expmap_b) { (Vector(9) << 0.0, 0.0, 0.1, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0).finished()); NavState expected(Rot3::Rodrigues(0.0, 0.0, 0.1), Point3(-100.0, 0.0, 0.0), Point3(100.0, 0.0, 0.0)); - EXPECT(assert_equal(expected, p2, 1e-2)); + EXPECT(assert_equal(expected, p2)); } /* ************************************************************************* */ @@ -566,7 +566,7 @@ TEST(NavState, retract_localCoordinates2) { EXPECT(assert_equal(t2, t1.retract(d12))); Vector d21 = t2.localCoordinates(t1); EXPECT(assert_equal(t1, t2.retract(d21))); - // EXPECT(assert_equal(d12, -d21)); + // NOTE(FRANK): d12 !== -d21 for arbitrary retractions. } /* ************************************************************************* */ TEST(NavState, manifold_expmap) { @@ -675,9 +675,9 @@ TEST(NavState, ChartDerivatives) { NavState id; if (ROT3_DEFAULT_COORDINATES_MODE == Rot3::EXPMAP) { CHECK_CHART_DERIVATIVES(id, id); - // CHECK_CHART_DERIVATIVES(id,T2); - // CHECK_CHART_DERIVATIVES(T2,id); - // CHECK_CHART_DERIVATIVES(T2,T3); + CHECK_CHART_DERIVATIVES(id,T2); + CHECK_CHART_DERIVATIVES(T2,id); + CHECK_CHART_DERIVATIVES(T2,T3); } } From 440c3ea64bbcd750c692bca7d8cd348db43bb7f4 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 14 Dec 2024 14:28:44 -0500 Subject: [PATCH 28/80] Simplify functor according to Eade --- gtsam/geometry/SO3.cpp | 47 +++++++++++++++++++++++------------------- gtsam/geometry/SO3.h | 27 ++++++++++++++++-------- 2 files changed, 44 insertions(+), 30 deletions(-) diff --git a/gtsam/geometry/SO3.cpp b/gtsam/geometry/SO3.cpp index 2585c37be..d4c30119e 100644 --- a/gtsam/geometry/SO3.cpp +++ b/gtsam/geometry/SO3.cpp @@ -48,12 +48,15 @@ GTSAM_EXPORT Matrix3 compose(const Matrix3& M, const SO3& R, OptionalJacobian<9, } void ExpmapFunctor::init(bool nearZeroApprox) { + WW = W * W; nearZero = nearZeroApprox || (theta2 <= std::numeric_limits::epsilon()); if (!nearZero) { sin_theta = std::sin(theta); const double s2 = std::sin(theta / 2.0); one_minus_cos = 2.0 * s2 * s2; // numerically better than [1 - cos(theta)] + A = sin_theta / theta; + B = one_minus_cos / theta2; } } @@ -62,39 +65,39 @@ ExpmapFunctor::ExpmapFunctor(const Vector3& omega, bool nearZeroApprox) const double wx = omega.x(), wy = omega.y(), wz = omega.z(); W << 0.0, -wz, +wy, +wz, 0.0, -wx, -wy, +wx, 0.0; init(nearZeroApprox); - if (!nearZero) { - K = W / theta; - KK = K * K; - } } ExpmapFunctor::ExpmapFunctor(const Vector3& axis, double angle, bool nearZeroApprox) : theta2(angle * angle), theta(angle) { const double ax = axis.x(), ay = axis.y(), az = axis.z(); - K << 0.0, -az, +ay, +az, 0.0, -ax, -ay, +ax, 0.0; - W = K * angle; + W << 0.0, -az, +ay, +az, 0.0, -ax, -ay, +ax, 0.0; + W *= angle; init(nearZeroApprox); - if (!nearZero) { - KK = K * K; - } } SO3 ExpmapFunctor::expmap() const { if (nearZero) - return SO3(I_3x3 + W); + return SO3(I_3x3 + W + 0.5 * WW); else - return SO3(I_3x3 + sin_theta * K + one_minus_cos * KK); + return SO3(I_3x3 + A * W + B * WW); +} + +Matrix3 DexpFunctor::leftJacobian() const { + if (nearZero) { + return I_3x3 + 0.5 * W + one_sixth * WW; + } else { + return I_3x3 + B * W + C * WW; + } } DexpFunctor::DexpFunctor(const Vector3& omega, bool nearZeroApprox) : ExpmapFunctor(omega, nearZeroApprox), omega(omega) { if (nearZero) { - dexp_ = I_3x3 - 0.5 * W; + rightJacobian_ = I_3x3 - 0.5 * W + one_sixth * WW; } else { - a = one_minus_cos / theta; - b = 1.0 - sin_theta / theta; - dexp_ = I_3x3 - a * K + b * KK; + C = (1 - A) / theta2; + rightJacobian_ = I_3x3 - B * W + C * WW; } } @@ -105,21 +108,23 @@ Vector3 DexpFunctor::applyDexp(const Vector3& v, OptionalJacobian<3, 3> H1, *H1 = 0.5 * skewSymmetric(v); } else { // TODO(frank): Iserles hints that there should be a form I + c*K + d*KK - const Vector3 Kv = K * v; + double a = B * theta; + double b = C * theta2; + const Vector3 Kv = W * v / theta; const double Da = (sin_theta - 2.0 * a) / theta2; const double Db = (one_minus_cos - 3.0 * b) / theta2; - *H1 = (Db * K - Da * I_3x3) * Kv * omega.transpose() - + *H1 = (Db * W / theta - Da * I_3x3) * Kv * omega.transpose() - skewSymmetric(Kv * b / theta) + - (a * I_3x3 - b * K) * skewSymmetric(v / theta); + (a * I_3x3 - b * W / theta) * skewSymmetric(v / theta); } } - if (H2) *H2 = dexp_; - return dexp_ * v; + if (H2) *H2 = rightJacobian_; + return rightJacobian_ * v; } Vector3 DexpFunctor::applyInvDexp(const Vector3& v, OptionalJacobian<3, 3> H1, OptionalJacobian<3, 3> H2) const { - const Matrix3 invDexp = dexp_.inverse(); + const Matrix3 invDexp = rightJacobian_.inverse(); const Vector3 c = invDexp * v; if (H1) { Matrix3 D_dexpv_omega; diff --git a/gtsam/geometry/SO3.h b/gtsam/geometry/SO3.h index cd67bfb8c..8d4401c31 100644 --- a/gtsam/geometry/SO3.h +++ b/gtsam/geometry/SO3.h @@ -136,7 +136,8 @@ GTSAM_EXPORT Matrix99 Dcompose(const SO3& R); class GTSAM_EXPORT ExpmapFunctor { protected: const double theta2; - Matrix3 W, K, KK; + Matrix3 W, WW; + double A, B; // Ethan Eade's constants bool nearZero; double theta, sin_theta, one_minus_cos; // only defined if !nearZero @@ -155,13 +156,16 @@ class GTSAM_EXPORT ExpmapFunctor { /// Functor that implements Exponential map *and* its derivatives class DexpFunctor : public ExpmapFunctor { + protected: + static constexpr double one_sixth = 1.0 / 6.0; const Vector3 omega; - double a, b; - Matrix3 dexp_; + double C; // Ethan Eade's C constant + Matrix3 rightJacobian_; public: /// Constructor with element of Lie algebra so(3) - GTSAM_EXPORT explicit DexpFunctor(const Vector3& omega, bool nearZeroApprox = false); + GTSAM_EXPORT explicit DexpFunctor(const Vector3& omega, + bool nearZeroApprox = false); // NOTE(luca): Right Jacobian for Exponential map in SO(3) - equation // (10.86) and following equations in G.S. Chirikjian, "Stochastic Models, @@ -169,16 +173,21 @@ class DexpFunctor : public ExpmapFunctor { // expmap(omega + v) \approx expmap(omega) * expmap(dexp * v) // This maps a perturbation v in the tangent space to // a perturbation on the manifold Expmap(dexp * v) */ - const Matrix3& dexp() const { return dexp_; } + const Matrix3& dexp() const { return rightJacobian_; } /// Multiplies with dexp(), with optional derivatives - GTSAM_EXPORT Vector3 applyDexp(const Vector3& v, OptionalJacobian<3, 3> H1 = {}, - OptionalJacobian<3, 3> H2 = {}) const; + GTSAM_EXPORT Vector3 applyDexp(const Vector3& v, + OptionalJacobian<3, 3> H1 = {}, + OptionalJacobian<3, 3> H2 = {}) const; /// Multiplies with dexp().inverse(), with optional derivatives GTSAM_EXPORT Vector3 applyInvDexp(const Vector3& v, - OptionalJacobian<3, 3> H1 = {}, - OptionalJacobian<3, 3> H2 = {}) const; + OptionalJacobian<3, 3> H1 = {}, + OptionalJacobian<3, 3> H2 = {}) const; + + // Compute the left Jacobian for Exponential map in SO(3) + // Note precomputed, as not used as much + Matrix3 leftJacobian() const; }; } // namespace so3 From 7f1e101c6b1f934e23cbd3914a40396b6d282efb Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 14 Dec 2024 14:28:56 -0500 Subject: [PATCH 29/80] Use functor as in SO3 --- gtsam/geometry/Pose3.cpp | 73 +++++++++++++++++------------- gtsam/geometry/tests/testPose3.cpp | 23 +++++++++- 2 files changed, 63 insertions(+), 33 deletions(-) diff --git a/gtsam/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp index bb1483432..f867b3088 100644 --- a/gtsam/geometry/Pose3.cpp +++ b/gtsam/geometry/Pose3.cpp @@ -238,14 +238,50 @@ Vector6 Pose3::ChartAtOrigin::Local(const Pose3& pose, ChartJacobian Hpose) { #endif } +/* ************************************************************************* */ +namespace pose3 { +class ExpmapFunctor : public so3::DexpFunctor { + private: + static constexpr double one_twenty_fourth = 1.0 / 24.0; + static constexpr double one_one_hundred_twentieth = 1.0 / 120.0; + + public: + ExpmapFunctor(const Vector3& omega, bool nearZeroApprox = false) + : so3::DexpFunctor(omega, nearZeroApprox) {} + + // Compute the Jacobian Q with respect to w + Matrix3 computeQ(const Vector3& v) const { + const Matrix3 V = skewSymmetric(v); + const Matrix3 WVW = W * V * W; + + if (!nearZero) { + const double theta3 = theta2 * theta; + const double theta4 = theta2 * theta2; + const double theta5 = theta4 * theta; + const double s = sin_theta; + const double a = one_minus_cos - theta2 / 2; + + // The closed-form formula in Barfoot14tro eq. (102) + return -0.5 * V + (theta - s) / theta3 * (W * V + V * W - WVW) + + a / theta4 * (WW * V + V * WW - 3 * WVW) - + 0.5 * + (a / theta4 - 3 * (theta - s - theta3 * one_sixth) / theta5) * + (WVW * W + W * WVW); + } else { + return -0.5 * V + one_sixth * (W * V + V * W - WVW) - + one_twenty_fourth * (WW * V + V * WW - 3 * WVW) + + one_one_hundred_twentieth * (WVW * W + W * WVW); + } + } +}; +} // namespace pose3 + /* ************************************************************************* */ Matrix3 Pose3::ComputeQforExpmapDerivative(const Vector6& xi, double nearZeroThreshold) { - Matrix3 Q; const auto w = xi.head<3>(); const auto v = xi.tail<3>(); - ExpmapTranslation(w, v, Q, {}, nearZeroThreshold); - return Q; + return pose3::ExpmapFunctor(w).computeQ(v); } /* ************************************************************************* */ @@ -256,39 +292,12 @@ Vector3 Pose3::ExpmapTranslation(const Vector3& w, const Vector3& v, const double theta2 = w.dot(w); bool nearZero = (theta2 <= nearZeroThreshold); - if (Q) { - const Matrix3 V = skewSymmetric(v); - const Matrix3 W = skewSymmetric(w); - const Matrix3 WVW = W * V * W; - const double theta = w.norm(); + if (Q) *Q = pose3::ExpmapFunctor(w, nearZero).computeQ(v); - if (nearZero) { - static constexpr double one_sixth = 1. / 6.; - static constexpr double one_twenty_fourth = 1. / 24.; - static constexpr double one_one_hundred_twentieth = 1. / 120.; - - *Q = -0.5 * V + one_sixth * (W * V + V * W - WVW) - - one_twenty_fourth * (W * W * V + V * W * W - 3 * WVW) + - one_one_hundred_twentieth * (WVW * W + W * WVW); - } else { - const double s = sin(theta), c = cos(theta); - const double theta3 = theta2 * theta, theta4 = theta3 * theta, - theta5 = theta4 * theta; - - // Invert the sign of odd-order terms to have the right Jacobian - *Q = -0.5 * V + (theta - s) / theta3 * (W * V + V * W - WVW) + - (1 - theta2 / 2 - c) / theta4 * (W * W * V + V * W * W - 3 * WVW) - - 0.5 * - ((1 - theta2 / 2 - c) / theta4 - - 3 * (theta - s - theta3 / 6.) / theta5) * - (WVW * W + W * WVW); - } - } - - // TODO(Frank): this threshold is *different*. Why? if (nearZero) { return v + 0.5 * w.cross(v); } else { + // Geometric intuition and faster than using functor. Vector3 t_parallel = w * w.dot(v); // translation parallel to axis Vector3 w_cross_v = w.cross(v); // translation orthogonal to axis Rot3 rotation = R.value_or(Rot3::Expmap(w)); diff --git a/gtsam/geometry/tests/testPose3.cpp b/gtsam/geometry/tests/testPose3.cpp index c9851f761..4ff4f9a85 100644 --- a/gtsam/geometry/tests/testPose3.cpp +++ b/gtsam/geometry/tests/testPose3.cpp @@ -845,7 +845,28 @@ TEST( Pose3, ExpmapDerivative1) { } /* ************************************************************************* */ -TEST(Pose3, ExpmapDerivative2) { +TEST( Pose3, ExpmapDerivative2) { + Matrix6 actualH; + Vector6 w; w << 1.0, -2.0, 3.0, -10.0, -20.0, 30.0; + Pose3::Expmap(w,actualH); + Matrix expectedH = numericalDerivative21 >(&Pose3::Expmap, w, {}); + EXPECT(assert_equal(expectedH, actualH)); +} + +/* ************************************************************************* */ +TEST( Pose3, ExpmapDerivative3) { + Matrix6 actualH; + Vector6 w; w << 0.0, 0.0, 0.0, -10.0, -20.0, 30.0; + Pose3::Expmap(w,actualH); + Matrix expectedH = numericalDerivative21 >(&Pose3::Expmap, w, {}); + // Small angle approximation is not as precise as numerical derivative? + EXPECT(assert_equal(expectedH, actualH, 1e-5)); +} + +/* ************************************************************************* */ +TEST(Pose3, ExpmapDerivative4) { // Iserles05an (Lie-group Methods) says: // scalar is easy: d exp(a(t)) / dt = exp(a(t)) a'(t) // matrix is hard: d exp(A(t)) / dt = exp(A(t)) dexp[-A(t)] A'(t) From d1634c033520920496c117548013339e2c5cddbe Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 14 Dec 2024 15:48:55 -0500 Subject: [PATCH 30/80] Simplified Jacobians with ABC --- gtsam/geometry/Pose3.cpp | 37 +++++++++++++++++++------------------ gtsam/geometry/SO3.cpp | 19 ++++++++----------- 2 files changed, 27 insertions(+), 29 deletions(-) diff --git a/gtsam/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp index f867b3088..16ea55665 100644 --- a/gtsam/geometry/Pose3.cpp +++ b/gtsam/geometry/Pose3.cpp @@ -246,33 +246,34 @@ class ExpmapFunctor : public so3::DexpFunctor { static constexpr double one_one_hundred_twentieth = 1.0 / 120.0; public: - ExpmapFunctor(const Vector3& omega, bool nearZeroApprox = false) - : so3::DexpFunctor(omega, nearZeroApprox) {} + ExpmapFunctor(const Vector3& omega, bool nearZeroApprox = false, + bool includeHigherOrder = false) + : so3::DexpFunctor(omega, nearZeroApprox), + includeHigherOrder(includeHigherOrder) {} - // Compute the Jacobian Q with respect to w Matrix3 computeQ(const Vector3& v) const { const Matrix3 V = skewSymmetric(v); const Matrix3 WVW = W * V * W; if (!nearZero) { - const double theta3 = theta2 * theta; - const double theta4 = theta2 * theta2; - const double theta5 = theta4 * theta; - const double s = sin_theta; - const double a = one_minus_cos - theta2 / 2; - - // The closed-form formula in Barfoot14tro eq. (102) - return -0.5 * V + (theta - s) / theta3 * (W * V + V * W - WVW) + - a / theta4 * (WW * V + V * WW - 3 * WVW) - - 0.5 * - (a / theta4 - 3 * (theta - s - theta3 * one_sixth) / theta5) * - (WVW * W + W * WVW); + // Simplified from closed-form formula in Barfoot14tro eq. (102) + // Note dexp = I_3x3 - B * W + C * WW and t = dexp * v + return -0.5 * V + C * (W * V + V * W - WVW) + + (B - 0.5) / theta2 * (WW * V + V * WW - 3 * WVW) - + 0.5 * (B - 3 * C) / theta2 * (WVW * W + W * WVW); } else { - return -0.5 * V + one_sixth * (W * V + V * W - WVW) - - one_twenty_fourth * (WW * V + V * WW - 3 * WVW) + - one_one_hundred_twentieth * (WVW * W + W * WVW); + Matrix3 Q = -0.5 * V + one_sixth * (W * V + V * W - WVW); + Q -= one_twenty_fourth * (WW * V + V * WW - 3 * WVW); + + if (includeHigherOrder) { + Q += one_one_hundred_twentieth * (WVW * W + W * WVW); + } + return Q; } } + + private: + bool includeHigherOrder; }; } // namespace pose3 diff --git a/gtsam/geometry/SO3.cpp b/gtsam/geometry/SO3.cpp index d4c30119e..626599abf 100644 --- a/gtsam/geometry/SO3.cpp +++ b/gtsam/geometry/SO3.cpp @@ -85,7 +85,7 @@ SO3 ExpmapFunctor::expmap() const { Matrix3 DexpFunctor::leftJacobian() const { if (nearZero) { - return I_3x3 + 0.5 * W + one_sixth * WW; + return I_3x3 + 0.5 * W; // + one_sixth * WW; } else { return I_3x3 + B * W + C * WW; } @@ -94,7 +94,7 @@ Matrix3 DexpFunctor::leftJacobian() const { DexpFunctor::DexpFunctor(const Vector3& omega, bool nearZeroApprox) : ExpmapFunctor(omega, nearZeroApprox), omega(omega) { if (nearZero) { - rightJacobian_ = I_3x3 - 0.5 * W + one_sixth * WW; + rightJacobian_ = I_3x3 - 0.5 * W; // + one_sixth * WW; } else { C = (1 - A) / theta2; rightJacobian_ = I_3x3 - B * W + C * WW; @@ -104,18 +104,15 @@ DexpFunctor::DexpFunctor(const Vector3& omega, bool nearZeroApprox) Vector3 DexpFunctor::applyDexp(const Vector3& v, OptionalJacobian<3, 3> H1, OptionalJacobian<3, 3> H2) const { if (H1) { + const Matrix3 V = skewSymmetric(v); if (nearZero) { - *H1 = 0.5 * skewSymmetric(v); + *H1 = 0.5 * V; } else { // TODO(frank): Iserles hints that there should be a form I + c*K + d*KK - double a = B * theta; - double b = C * theta2; - const Vector3 Kv = W * v / theta; - const double Da = (sin_theta - 2.0 * a) / theta2; - const double Db = (one_minus_cos - 3.0 * b) / theta2; - *H1 = (Db * W / theta - Da * I_3x3) * Kv * omega.transpose() - - skewSymmetric(Kv * b / theta) + - (a * I_3x3 - b * W / theta) * skewSymmetric(v / theta); + const double Da = (A - 2.0 * B) / theta2; + const double Db = (B - 3.0 * C) / theta2; + *H1 = (Db * WW - Da * W) * v * omega.transpose() - + C * skewSymmetric(W * v) + B * V - C * W * V; } } if (H2) *H2 = rightJacobian_; From 8040a0a31e188d99583a747de4be2dd6887a37f4 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 14 Dec 2024 17:22:09 -0500 Subject: [PATCH 31/80] doubleCross, tested --- gtsam/geometry/Point3.cpp | 10 ++++++++++ gtsam/geometry/Point3.h | 7 ++++++- gtsam/geometry/tests/testPoint3.cpp | 13 +++++++++++++ 3 files changed, 29 insertions(+), 1 deletion(-) diff --git a/gtsam/geometry/Point3.cpp b/gtsam/geometry/Point3.cpp index ef91108eb..79fd6b9bf 100644 --- a/gtsam/geometry/Point3.cpp +++ b/gtsam/geometry/Point3.cpp @@ -69,6 +69,16 @@ Point3 cross(const Point3 &p, const Point3 &q, OptionalJacobian<3, 3> H1, p.x() * q.y() - p.y() * q.x()); } +Point3 doubleCross(const Point3 &p, const Point3 &q, // + OptionalJacobian<3, 3> H1, OptionalJacobian<3, 3> H2) { + if (H1) *H1 = q.dot(p) * I_3x3 + p * q.transpose() - 2 * q * p.transpose(); + if (H2) { + const Matrix3 W = skewSymmetric(p); + *H2 = W * W; + } + return gtsam::cross(p, gtsam::cross(p, q)); +} + double dot(const Point3 &p, const Point3 &q, OptionalJacobian<1, 3> H1, OptionalJacobian<1, 3> H2) { if (H1) *H1 << q.x(), q.y(), q.z(); diff --git a/gtsam/geometry/Point3.h b/gtsam/geometry/Point3.h index 8da83817d..0fa53b1b2 100644 --- a/gtsam/geometry/Point3.h +++ b/gtsam/geometry/Point3.h @@ -55,11 +55,16 @@ GTSAM_EXPORT double norm3(const Point3& p, OptionalJacobian<1, 3> H = {}); /// normalize, with optional Jacobian GTSAM_EXPORT Point3 normalize(const Point3& p, OptionalJacobian<3, 3> H = {}); -/// cross product @return this x q +/// cross product @return p x q GTSAM_EXPORT Point3 cross(const Point3& p, const Point3& q, OptionalJacobian<3, 3> H_p = {}, OptionalJacobian<3, 3> H_q = {}); +/// double cross product @return p x (p x q) +Point3 doubleCross(const Point3& p, const Point3& q, + OptionalJacobian<3, 3> H1 = {}, + OptionalJacobian<3, 3> H2 = {}); + /// dot product GTSAM_EXPORT double dot(const Point3& p, const Point3& q, OptionalJacobian<1, 3> H_p = {}, diff --git a/gtsam/geometry/tests/testPoint3.cpp b/gtsam/geometry/tests/testPoint3.cpp index dcfb99105..bb49486a8 100644 --- a/gtsam/geometry/tests/testPoint3.cpp +++ b/gtsam/geometry/tests/testPoint3.cpp @@ -18,6 +18,8 @@ #include #include #include +#include "gtsam/base/Matrix.h" +#include "gtsam/base/OptionalJacobian.h" using namespace std::placeholders; using namespace gtsam; @@ -154,6 +156,17 @@ TEST( Point3, cross2) { } } +/* ************************************************************************* */ +TEST(Point3, doubleCross) { + Matrix aH1, aH2; + std::function f = + [](const Point3& p, const Point3& q) { return doubleCross(p, q); }; + const Point3 omega(1, 2, 3), theta(4, 5, 6); + doubleCross(omega, theta, aH1, aH2); + EXPECT(assert_equal(numericalDerivative21(f, omega, theta), aH1)); + EXPECT(assert_equal(numericalDerivative22(f, omega, theta), aH2)); +} + //************************************************************************* TEST (Point3, normalize) { Matrix actualH; From 78f17aabc466b81554b513ff22d7d3df42b643ed Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 14 Dec 2024 23:53:51 -0500 Subject: [PATCH 32/80] Simplified applyDexp --- gtsam/geometry/SO3.cpp | 82 ++++++++++++++++++++++++++++-------------- gtsam/geometry/SO3.h | 17 +++++---- 2 files changed, 66 insertions(+), 33 deletions(-) diff --git a/gtsam/geometry/SO3.cpp b/gtsam/geometry/SO3.cpp index 626599abf..bc2e1e6ec 100644 --- a/gtsam/geometry/SO3.cpp +++ b/gtsam/geometry/SO3.cpp @@ -18,13 +18,14 @@ * @date December 2014 */ +#include +#include #include +#include #include #include - #include -#include #include namespace gtsam { @@ -41,7 +42,8 @@ GTSAM_EXPORT Matrix99 Dcompose(const SO3& Q) { return H; } -GTSAM_EXPORT Matrix3 compose(const Matrix3& M, const SO3& R, OptionalJacobian<9, 9> H) { +GTSAM_EXPORT Matrix3 compose(const Matrix3& M, const SO3& R, + OptionalJacobian<9, 9> H) { Matrix3 MR = M * R.matrix(); if (H) *H = Dcompose(R); return MR; @@ -83,45 +85,54 @@ SO3 ExpmapFunctor::expmap() const { return SO3(I_3x3 + A * W + B * WW); } -Matrix3 DexpFunctor::leftJacobian() const { - if (nearZero) { - return I_3x3 + 0.5 * W; // + one_sixth * WW; - } else { - return I_3x3 + B * W + C * WW; - } -} - DexpFunctor::DexpFunctor(const Vector3& omega, bool nearZeroApprox) : ExpmapFunctor(omega, nearZeroApprox), omega(omega) { + C = (1 - A) / theta2; +} + +Matrix3 DexpFunctor::rightJacobian() const { if (nearZero) { - rightJacobian_ = I_3x3 - 0.5 * W; // + one_sixth * WW; + return I_3x3 - 0.5 * W; // + one_sixth * WW; } else { - C = (1 - A) / theta2; - rightJacobian_ = I_3x3 - B * W + C * WW; + return I_3x3 - B * W + C * WW; } } +// Derivative of w x w x v in w: +static Matrix3 doubleCrossJacobian(const Vector3& w, const Vector3& v) { + return v.dot(w) * I_3x3 + w * v.transpose() - 2 * v * w.transpose(); +} + +// Multiplies v with left Jacobian through vector operations only. +// When Jacobian H1 wrt omega is requested, product rule is invoked twice, once +// for (B * Wv) and (C * WWv). The Jacobian H2 wrt v is just the right Jacobian. Vector3 DexpFunctor::applyDexp(const Vector3& v, OptionalJacobian<3, 3> H1, OptionalJacobian<3, 3> H2) const { - if (H1) { - const Matrix3 V = skewSymmetric(v); - if (nearZero) { - *H1 = 0.5 * V; - } else { - // TODO(frank): Iserles hints that there should be a form I + c*K + d*KK - const double Da = (A - 2.0 * B) / theta2; - const double Db = (B - 3.0 * C) / theta2; - *H1 = (Db * WW - Da * W) * v * omega.transpose() - - C * skewSymmetric(W * v) + B * V - C * W * V; + // Wv = omega x * v, with Jacobian -V in w + const Vector3 Wv = gtsam::cross(omega, v); + + if (nearZero) { + if (H1) *H1 = 0.5 * skewSymmetric(v); + if (H2) *H2 = I_3x3 - 0.5 * W; + return v - 0.5 * Wv; + } else { + // WWv = omega x (omega x * v), with Jacobian doubleCrossJacobian + const Vector3 WWv = gtsam::cross(omega, Wv); + if (H1) { + // 1x3 Jacobians of B and C with respect to theta + const Matrix13 HB = (A - 2.0 * B) / theta2 * omega.transpose(); + const Matrix13 HC = (B - 3.0 * C) / theta2 * omega.transpose(); + *H1 = -Wv * HB + B * skewSymmetric(v) + + C * doubleCrossJacobian(omega, v) + WWv * HC; } + if (H2) *H2 = rightJacobian(); + return v - B * Wv + C * WWv; } - if (H2) *H2 = rightJacobian_; - return rightJacobian_ * v; } Vector3 DexpFunctor::applyInvDexp(const Vector3& v, OptionalJacobian<3, 3> H1, OptionalJacobian<3, 3> H2) const { - const Matrix3 invDexp = rightJacobian_.inverse(); + const Matrix3 invDexp = rightJacobian().inverse(); const Vector3 c = invDexp * v; if (H1) { Matrix3 D_dexpv_omega; @@ -132,6 +143,23 @@ Vector3 DexpFunctor::applyInvDexp(const Vector3& v, OptionalJacobian<3, 3> H1, return c; } +Matrix3 DexpFunctor::leftJacobian() const { + if (nearZero) { + return I_3x3 + 0.5 * W; // + one_sixth * WW; + } else { + return I_3x3 + B * W + C * WW; + } +} + +Vector3 DexpFunctor::applyLeftJacobian(const Vector3& v, + OptionalJacobian<3, 3> H1, + OptionalJacobian<3, 3> H2) const { + const Matrix3 Jw = leftJacobian(); + if (H1) H1->setZero(); + if (H2) *H2 = Jw; + return Jw * v; +} + } // namespace so3 //****************************************************************************** diff --git a/gtsam/geometry/SO3.h b/gtsam/geometry/SO3.h index 8d4401c31..bd54f0cc4 100644 --- a/gtsam/geometry/SO3.h +++ b/gtsam/geometry/SO3.h @@ -26,7 +26,6 @@ #include #include -#include #include namespace gtsam { @@ -160,7 +159,6 @@ class DexpFunctor : public ExpmapFunctor { static constexpr double one_sixth = 1.0 / 6.0; const Vector3 omega; double C; // Ethan Eade's C constant - Matrix3 rightJacobian_; public: /// Constructor with element of Lie algebra so(3) @@ -172,8 +170,11 @@ class DexpFunctor : public ExpmapFunctor { // Information Theory, and Lie Groups", Volume 2, 2008. // expmap(omega + v) \approx expmap(omega) * expmap(dexp * v) // This maps a perturbation v in the tangent space to - // a perturbation on the manifold Expmap(dexp * v) */ - const Matrix3& dexp() const { return rightJacobian_; } + // a perturbation on the manifold Expmap(dexp * v) + GTSAM_EXPORT Matrix3 rightJacobian() const; + + /// differential of expmap == right Jacobian + GTSAM_EXPORT Matrix3 dexp() const { return rightJacobian(); } /// Multiplies with dexp(), with optional derivatives GTSAM_EXPORT Vector3 applyDexp(const Vector3& v, @@ -186,8 +187,12 @@ class DexpFunctor : public ExpmapFunctor { OptionalJacobian<3, 3> H2 = {}) const; // Compute the left Jacobian for Exponential map in SO(3) - // Note precomputed, as not used as much - Matrix3 leftJacobian() const; + GTSAM_EXPORT Matrix3 leftJacobian() const; + + /// Multiplies with leftJacobian(), with optional derivatives + GTSAM_EXPORT Vector3 applyLeftJacobian(const Vector3& v, + OptionalJacobian<3, 3> H1 = {}, + OptionalJacobian<3, 3> H2 = {}) const; }; } // namespace so3 From c7f651d98db1f0a7d3bea7b1a047cd1e4f93c7e1 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 15 Dec 2024 00:50:38 -0500 Subject: [PATCH 33/80] applyLeftJacobian --- gtsam/geometry/SO3.cpp | 67 +++++++++++++++++++++----------- gtsam/geometry/SO3.h | 6 +++ gtsam/geometry/tests/testSO3.cpp | 24 ++++++++++++ 3 files changed, 74 insertions(+), 23 deletions(-) diff --git a/gtsam/geometry/SO3.cpp b/gtsam/geometry/SO3.cpp index bc2e1e6ec..13600f884 100644 --- a/gtsam/geometry/SO3.cpp +++ b/gtsam/geometry/SO3.cpp @@ -26,6 +26,7 @@ #include #include +#include #include namespace gtsam { @@ -98,35 +99,47 @@ Matrix3 DexpFunctor::rightJacobian() const { } } -// Derivative of w x w x v in w: -static Matrix3 doubleCrossJacobian(const Vector3& w, const Vector3& v) { - return v.dot(w) * I_3x3 + w * v.transpose() - 2 * v * w.transpose(); +Vector3 DexpFunctor::cross(const Vector3& v, OptionalJacobian<3, 3> H) const { + // Wv = omega x * v + const Vector3 Wv = gtsam::cross(omega, v); + if (H) { + // 1x3 Jacobian of B with respect to omega + const Matrix13 HB = (A - 2.0 * B) / theta2 * omega.transpose(); + // Apply product rule: + *H = Wv * HB - B * skewSymmetric(v); + } + return B * Wv; +} + +Vector3 DexpFunctor::doubleCross(const Vector3& v, + OptionalJacobian<3, 3> H) const { + // WWv = omega x (omega x * v) + Matrix3 doubleCrossJacobian; + const Vector3 WWv = + gtsam::doubleCross(omega, v, H ? &doubleCrossJacobian : nullptr); + if (H) { + // 1x3 Jacobian of C with respect to omega + const Matrix13 HC = (B - 3.0 * C) / theta2 * omega.transpose(); + // Apply product rule: + *H = WWv * HC + C * doubleCrossJacobian; + } + return C * WWv; } // Multiplies v with left Jacobian through vector operations only. -// When Jacobian H1 wrt omega is requested, product rule is invoked twice, once -// for (B * Wv) and (C * WWv). The Jacobian H2 wrt v is just the right Jacobian. Vector3 DexpFunctor::applyDexp(const Vector3& v, OptionalJacobian<3, 3> H1, OptionalJacobian<3, 3> H2) const { - // Wv = omega x * v, with Jacobian -V in w - const Vector3 Wv = gtsam::cross(omega, v); - if (nearZero) { if (H1) *H1 = 0.5 * skewSymmetric(v); if (H2) *H2 = I_3x3 - 0.5 * W; - return v - 0.5 * Wv; + return v - 0.5 * gtsam::cross(omega, v); } else { - // WWv = omega x (omega x * v), with Jacobian doubleCrossJacobian - const Vector3 WWv = gtsam::cross(omega, Wv); - if (H1) { - // 1x3 Jacobians of B and C with respect to theta - const Matrix13 HB = (A - 2.0 * B) / theta2 * omega.transpose(); - const Matrix13 HC = (B - 3.0 * C) / theta2 * omega.transpose(); - *H1 = -Wv * HB + B * skewSymmetric(v) + - C * doubleCrossJacobian(omega, v) + WWv * HC; - } + Matrix3 D_BWv_omega, D_CWWv_omega; + const Vector3 BWv = cross(v, D_BWv_omega); + const Vector3 CWWv = doubleCross(v, D_CWWv_omega); + if (H1) *H1 = - D_BWv_omega + D_CWWv_omega; if (H2) *H2 = rightJacobian(); - return v - B * Wv + C * WWv; + return v - BWv + CWWv; } } @@ -154,10 +167,18 @@ Matrix3 DexpFunctor::leftJacobian() const { Vector3 DexpFunctor::applyLeftJacobian(const Vector3& v, OptionalJacobian<3, 3> H1, OptionalJacobian<3, 3> H2) const { - const Matrix3 Jw = leftJacobian(); - if (H1) H1->setZero(); - if (H2) *H2 = Jw; - return Jw * v; + if (nearZero) { + if (H1) *H1 = - 0.5 * skewSymmetric(v); + if (H2) *H2 = I_3x3 + 0.5 * W; + return v + 0.5 * gtsam::cross(omega, v); + } else { + Matrix3 D_BWv_omega, D_CWWv_omega; + const Vector3 BWv = cross(v, D_BWv_omega); + const Vector3 CWWv = doubleCross(v, D_CWWv_omega); + if (H1) *H1 = D_BWv_omega + D_CWWv_omega; + if (H2) *H2 = leftJacobian(); + return v + BWv + CWWv; + } } } // namespace so3 diff --git a/gtsam/geometry/SO3.h b/gtsam/geometry/SO3.h index bd54f0cc4..8766fecbe 100644 --- a/gtsam/geometry/SO3.h +++ b/gtsam/geometry/SO3.h @@ -160,6 +160,12 @@ class DexpFunctor : public ExpmapFunctor { const Vector3 omega; double C; // Ethan Eade's C constant + /// Computes B * (omega x v). + Vector3 cross(const Vector3& v, OptionalJacobian<3, 3> H = {}) const; + + /// Computes C * (omega x (omega x v)). + Vector3 doubleCross(const Vector3& v, OptionalJacobian<3, 3> H = {}) const; + public: /// Constructor with element of Lie algebra so(3) GTSAM_EXPORT explicit DexpFunctor(const Vector3& omega, diff --git a/gtsam/geometry/tests/testSO3.cpp b/gtsam/geometry/tests/testSO3.cpp index 27c143d31..8773e287a 100644 --- a/gtsam/geometry/tests/testSO3.cpp +++ b/gtsam/geometry/tests/testSO3.cpp @@ -19,6 +19,7 @@ #include #include #include +#include using namespace std::placeholders; using namespace std; @@ -326,6 +327,29 @@ TEST(SO3, ApplyDexp) { } } +//****************************************************************************** +TEST(SO3, ApplyLeftJacobian) { + Matrix aH1, aH2; + for (bool nearZeroApprox : {false, true}) { + std::function f = + [=](const Vector3& omega, const Vector3& v) { + return so3::DexpFunctor(omega, nearZeroApprox).applyLeftJacobian(v); + }; + for (Vector3 omega : {Vector3(0, 0, 0), Vector3(1, 0, 0), Vector3(0, 1, 0), + Vector3(0, 0, 1), Vector3(0.1, 0.2, 0.3)}) { + so3::DexpFunctor local(omega, nearZeroApprox); + for (Vector3 v : {Vector3(1, 0, 0), Vector3(0, 1, 0), Vector3(0, 0, 1), + Vector3(0.4, 0.3, 0.2)}) { + CHECK(assert_equal(Vector3(local.leftJacobian() * v), + local.applyLeftJacobian(v, aH1, aH2))); + CHECK(assert_equal(numericalDerivative21(f, omega, v), aH1)); + CHECK(assert_equal(numericalDerivative22(f, omega, v), aH2)); + CHECK(assert_equal(local.leftJacobian(), aH2)); + } + } + } +} + //****************************************************************************** TEST(SO3, ApplyInvDexp) { Matrix aH1, aH2; From 82e13806035327385d66b13bbbe36bb1d91b87dd Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 15 Dec 2024 01:53:02 -0500 Subject: [PATCH 34/80] Cleanup --- gtsam/geometry/Pose3.cpp | 30 ++++++++++++----------------- gtsam/geometry/tests/testPoint3.cpp | 2 -- gtsam/geometry/tests/testSO3.cpp | 1 - 3 files changed, 12 insertions(+), 21 deletions(-) diff --git a/gtsam/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp index 16ea55665..d87e3164c 100644 --- a/gtsam/geometry/Pose3.cpp +++ b/gtsam/geometry/Pose3.cpp @@ -168,14 +168,14 @@ Pose3 Pose3::Expmap(const Vector6& xi, OptionalJacobian<6, 6> Hxi) { const Vector3 w = xi.head<3>(), v = xi.tail<3>(); // Compute rotation using Expmap - Rot3 R = Rot3::Expmap(w); + Matrix3 Jw; + Rot3 R = Rot3::Expmap(w, Hxi ? &Jw : nullptr); // Compute translation and optionally its Jacobian in w Matrix3 Q; const Vector3 t = ExpmapTranslation(w, v, Hxi ? &Q : nullptr, R); if (Hxi) { - const Matrix3 Jw = Rot3::ExpmapDerivative(w); *Hxi << Jw, Z_3x3, Q, Jw; } @@ -246,34 +246,27 @@ class ExpmapFunctor : public so3::DexpFunctor { static constexpr double one_one_hundred_twentieth = 1.0 / 120.0; public: - ExpmapFunctor(const Vector3& omega, bool nearZeroApprox = false, - bool includeHigherOrder = false) - : so3::DexpFunctor(omega, nearZeroApprox), - includeHigherOrder(includeHigherOrder) {} + ExpmapFunctor(const Vector3& omega, bool nearZeroApprox = false) + : so3::DexpFunctor(omega, nearZeroApprox) {} + // Compute the bottom-left 3x3 block of the SE(3) Expmap derivative + // TODO(Frank): t = applyLeftJacobian(v), it would be nice to understand + // how to compute mess below from its derivatives in w and v. Matrix3 computeQ(const Vector3& v) const { const Matrix3 V = skewSymmetric(v); const Matrix3 WVW = W * V * W; if (!nearZero) { // Simplified from closed-form formula in Barfoot14tro eq. (102) - // Note dexp = I_3x3 - B * W + C * WW and t = dexp * v return -0.5 * V + C * (W * V + V * W - WVW) + (B - 0.5) / theta2 * (WW * V + V * WW - 3 * WVW) - 0.5 * (B - 3 * C) / theta2 * (WVW * W + W * WVW); } else { - Matrix3 Q = -0.5 * V + one_sixth * (W * V + V * W - WVW); - Q -= one_twenty_fourth * (WW * V + V * WW - 3 * WVW); - - if (includeHigherOrder) { - Q += one_one_hundred_twentieth * (WVW * W + W * WVW); - } - return Q; + return -0.5 * V + one_sixth * (W * V + V * W - WVW) - + one_twenty_fourth * (WW * V + V * WW - 3 * WVW) + + one_one_hundred_twentieth * (WVW * W + W * WVW); } } - - private: - bool includeHigherOrder; }; } // namespace pose3 @@ -298,7 +291,8 @@ Vector3 Pose3::ExpmapTranslation(const Vector3& w, const Vector3& v, if (nearZero) { return v + 0.5 * w.cross(v); } else { - // Geometric intuition and faster than using functor. + // NOTE(Frank): t can also be computed by calling applyLeftJacobian(v), but + // formulas below convey geometric insight and creating functor is not free. Vector3 t_parallel = w * w.dot(v); // translation parallel to axis Vector3 w_cross_v = w.cross(v); // translation orthogonal to axis Rot3 rotation = R.value_or(Rot3::Expmap(w)); diff --git a/gtsam/geometry/tests/testPoint3.cpp b/gtsam/geometry/tests/testPoint3.cpp index bb49486a8..b3b751ea1 100644 --- a/gtsam/geometry/tests/testPoint3.cpp +++ b/gtsam/geometry/tests/testPoint3.cpp @@ -18,8 +18,6 @@ #include #include #include -#include "gtsam/base/Matrix.h" -#include "gtsam/base/OptionalJacobian.h" using namespace std::placeholders; using namespace gtsam; diff --git a/gtsam/geometry/tests/testSO3.cpp b/gtsam/geometry/tests/testSO3.cpp index 8773e287a..1e105ceca 100644 --- a/gtsam/geometry/tests/testSO3.cpp +++ b/gtsam/geometry/tests/testSO3.cpp @@ -19,7 +19,6 @@ #include #include #include -#include using namespace std::placeholders; using namespace std; From 039c9b15426287e3669c5e1ffaaf50a2ce505e29 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sun, 15 Dec 2024 12:29:31 -0500 Subject: [PATCH 35/80] add getter for sparse_table_ --- gtsam/discrete/TableFactor.h | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/gtsam/discrete/TableFactor.h b/gtsam/discrete/TableFactor.h index d27c4740c..5ddb4ab43 100644 --- a/gtsam/discrete/TableFactor.h +++ b/gtsam/discrete/TableFactor.h @@ -99,7 +99,6 @@ class GTSAM_EXPORT TableFactor : public DiscreteFactor { typedef Eigen::SparseVector::InnerIterator SparseIt; typedef std::vector> AssignValList; - public: /// @name Standard Constructors /// @{ @@ -156,6 +155,9 @@ class GTSAM_EXPORT TableFactor : public DiscreteFactor { // /// @name Standard Interface // /// @{ + /// Getter for the underlying sparse vector + Eigen::SparseVector sparseTable() const { return sparse_table_; } + /// Evaluate probability distribution, is just look up in TableFactor. double evaluate(const Assignment& values) const override; From 293c29ebf8f2f706a1dd6857fc5b30593eed5e36 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sun, 15 Dec 2024 12:30:11 -0500 Subject: [PATCH 36/80] update toDecisionTreeFactor to use reverse key format so it's faster --- gtsam/discrete/TableFactor.cpp | 29 +++++++++++++++++++++++++---- 1 file changed, 25 insertions(+), 4 deletions(-) diff --git a/gtsam/discrete/TableFactor.cpp b/gtsam/discrete/TableFactor.cpp index de1e1f867..521ca4d47 100644 --- a/gtsam/discrete/TableFactor.cpp +++ b/gtsam/discrete/TableFactor.cpp @@ -252,12 +252,33 @@ DecisionTreeFactor TableFactor::operator*(const DecisionTreeFactor& f) const { /* ************************************************************************ */ DecisionTreeFactor TableFactor::toDecisionTreeFactor() const { DiscreteKeys dkeys = discreteKeys(); - std::vector table; + + DiscreteKeys rdkeys(dkeys.rbegin(), dkeys.rend()); + std::vector> pair_table; for (auto i = 0; i < sparse_table_.size(); i++) { - table.push_back(sparse_table_.coeff(i)); + std::stringstream ss; + for (auto&& [key, _] : rdkeys) { + ss << keyValueForIndex(key, i); + } + // k will be in reverse key order already + uint64_t k = std::strtoull(ss.str().c_str(), NULL, 10); + pair_table.push_back(std::make_pair(k, sparse_table_.coeff(i))); } - // NOTE(Varun): This constructor is really expensive!! - DecisionTreeFactor f(dkeys, table); + + // Sort based on key so we get values in reverse key order. + std::sort( + pair_table.begin(), pair_table.end(), + [](const std::pair& a, + const std::pair& b) { return a.first <= b.first; }); + + // Create the table vector + std::vector table; + std::for_each(pair_table.begin(), pair_table.end(), + [&table](const std::pair& pair) { + table.push_back(pair.second); + }); + + DecisionTreeFactor f(rdkeys, table); return f; } From 8fefbbf06a751bdd9e637f72571229145be77b13 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sun, 15 Dec 2024 16:03:10 -0500 Subject: [PATCH 37/80] fix toDecisionTreeFactor so the keys are ordered correctly --- gtsam/discrete/TableFactor.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/gtsam/discrete/TableFactor.cpp b/gtsam/discrete/TableFactor.cpp index 521ca4d47..63e9e5d6b 100644 --- a/gtsam/discrete/TableFactor.cpp +++ b/gtsam/discrete/TableFactor.cpp @@ -153,8 +153,7 @@ TableFactor::TableFactor(const DiscreteKeys& dkeys, /* ************************************************************************ */ TableFactor::TableFactor(const DecisionTreeFactor& dtf) - : TableFactor(dtf.discreteKeys(), - ComputeSparseTable(dtf.discreteKeys(), dtf)) {} + : TableFactor(dtf.discreteKeys(), dtf) {} /* ************************************************************************ */ TableFactor::TableFactor(const DiscreteConditional& c) @@ -278,7 +277,8 @@ DecisionTreeFactor TableFactor::toDecisionTreeFactor() const { table.push_back(pair.second); }); - DecisionTreeFactor f(rdkeys, table); + AlgebraicDecisionTree tree(rdkeys, table); + DecisionTreeFactor f(dkeys, tree); return f; } From fa1249bf14dc815402061bc826b7adbfc2db620f Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 15 Dec 2024 17:53:42 -0500 Subject: [PATCH 38/80] Add export --- gtsam/geometry/Point3.h | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/gtsam/geometry/Point3.h b/gtsam/geometry/Point3.h index 0fa53b1b2..d5c32816e 100644 --- a/gtsam/geometry/Point3.h +++ b/gtsam/geometry/Point3.h @@ -61,9 +61,9 @@ GTSAM_EXPORT Point3 cross(const Point3& p, const Point3& q, OptionalJacobian<3, 3> H_q = {}); /// double cross product @return p x (p x q) -Point3 doubleCross(const Point3& p, const Point3& q, - OptionalJacobian<3, 3> H1 = {}, - OptionalJacobian<3, 3> H2 = {}); +GTSAM_EXPORT Point3 doubleCross(const Point3& p, const Point3& q, + OptionalJacobian<3, 3> H1 = {}, + OptionalJacobian<3, 3> H2 = {}); /// dot product GTSAM_EXPORT double dot(const Point3& p, const Point3& q, From e96d8487e493d9cce4f60a09ab7ec20c2412210e Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 15 Dec 2024 17:54:30 -0500 Subject: [PATCH 39/80] New constants for cross and doubleCross --- gtsam/geometry/SO3.cpp | 65 ++++++++++++------------ gtsam/geometry/SO3.h | 22 ++++++--- gtsam/geometry/tests/testSO3.cpp | 84 +++++++++++++++++++++++--------- 3 files changed, 108 insertions(+), 63 deletions(-) diff --git a/gtsam/geometry/SO3.cpp b/gtsam/geometry/SO3.cpp index 13600f884..e341911b9 100644 --- a/gtsam/geometry/SO3.cpp +++ b/gtsam/geometry/SO3.cpp @@ -51,49 +51,51 @@ GTSAM_EXPORT Matrix3 compose(const Matrix3& M, const SO3& R, } void ExpmapFunctor::init(bool nearZeroApprox) { - WW = W * W; nearZero = nearZeroApprox || (theta2 <= std::numeric_limits::epsilon()); if (!nearZero) { - sin_theta = std::sin(theta); - const double s2 = std::sin(theta / 2.0); - one_minus_cos = 2.0 * s2 * s2; // numerically better than [1 - cos(theta)] + const double sin_theta = std::sin(theta); A = sin_theta / theta; + const double s2 = std::sin(theta / 2.0); + const double one_minus_cos = + 2.0 * s2 * s2; // numerically better than [1 - cos(theta)] B = one_minus_cos / theta2; + } else { + // Limits as theta -> 0: + A = 1.0; + B = 0.5; } } ExpmapFunctor::ExpmapFunctor(const Vector3& omega, bool nearZeroApprox) - : theta2(omega.dot(omega)), theta(std::sqrt(theta2)) { - const double wx = omega.x(), wy = omega.y(), wz = omega.z(); - W << 0.0, -wz, +wy, +wz, 0.0, -wx, -wy, +wx, 0.0; + : theta2(omega.dot(omega)), + theta(std::sqrt(theta2)), + W(skewSymmetric(omega)), + WW(W * W) { init(nearZeroApprox); } ExpmapFunctor::ExpmapFunctor(const Vector3& axis, double angle, bool nearZeroApprox) - : theta2(angle * angle), theta(angle) { - const double ax = axis.x(), ay = axis.y(), az = axis.z(); - W << 0.0, -az, +ay, +az, 0.0, -ax, -ay, +ax, 0.0; - W *= angle; + : theta2(angle * angle), + theta(angle), + W(skewSymmetric(axis * angle)), + WW(W * W) { init(nearZeroApprox); } -SO3 ExpmapFunctor::expmap() const { - if (nearZero) - return SO3(I_3x3 + W + 0.5 * WW); - else - return SO3(I_3x3 + A * W + B * WW); -} +SO3 ExpmapFunctor::expmap() const { return SO3(I_3x3 + A * W + B * WW); } DexpFunctor::DexpFunctor(const Vector3& omega, bool nearZeroApprox) : ExpmapFunctor(omega, nearZeroApprox), omega(omega) { - C = (1 - A) / theta2; + C = nearZero ? one_sixth : (1 - A) / theta2; + D = nearZero ? _one_twelfth : (A - 2.0 * B) / theta2; + E = nearZero ? _one_sixtieth : (B - 3.0 * C) / theta2; } Matrix3 DexpFunctor::rightJacobian() const { if (nearZero) { - return I_3x3 - 0.5 * W; // + one_sixth * WW; + return I_3x3 - B * W; // + C * WW; } else { return I_3x3 - B * W + C * WW; } @@ -103,10 +105,10 @@ Vector3 DexpFunctor::cross(const Vector3& v, OptionalJacobian<3, 3> H) const { // Wv = omega x * v const Vector3 Wv = gtsam::cross(omega, v); if (H) { - // 1x3 Jacobian of B with respect to omega - const Matrix13 HB = (A - 2.0 * B) / theta2 * omega.transpose(); // Apply product rule: - *H = Wv * HB - B * skewSymmetric(v); + // D * omega.transpose() is 1x3 Jacobian of B with respect to omega + // - skewSymmetric(v) is 3x3 Jacobian of B gtsam::cross(omega, v) + *H = Wv * D * omega.transpose() - B * skewSymmetric(v); } return B * Wv; } @@ -118,10 +120,10 @@ Vector3 DexpFunctor::doubleCross(const Vector3& v, const Vector3 WWv = gtsam::doubleCross(omega, v, H ? &doubleCrossJacobian : nullptr); if (H) { - // 1x3 Jacobian of C with respect to omega - const Matrix13 HC = (B - 3.0 * C) / theta2 * omega.transpose(); // Apply product rule: - *H = WWv * HC + C * doubleCrossJacobian; + // E * omega.transpose() is 1x3 Jacobian of C with respect to omega + // doubleCrossJacobian is 3x3 Jacobian of C gtsam::doubleCross(omega, v) + *H = WWv * E * omega.transpose() + C * doubleCrossJacobian; } return C * WWv; } @@ -134,12 +136,12 @@ Vector3 DexpFunctor::applyDexp(const Vector3& v, OptionalJacobian<3, 3> H1, if (H2) *H2 = I_3x3 - 0.5 * W; return v - 0.5 * gtsam::cross(omega, v); } else { - Matrix3 D_BWv_omega, D_CWWv_omega; + Matrix3 D_BWv_omega, D_CWWv_omega; const Vector3 BWv = cross(v, D_BWv_omega); const Vector3 CWWv = doubleCross(v, D_CWWv_omega); if (H1) *H1 = - D_BWv_omega + D_CWWv_omega; - if (H2) *H2 = rightJacobian(); - return v - BWv + CWWv; + if (H2) *H2 = rightJacobian(); + return v - BWv + CWWv; } } @@ -219,12 +221,7 @@ SO3 SO3::ChordalMean(const std::vector& rotations) { template <> GTSAM_EXPORT Matrix3 SO3::Hat(const Vector3& xi) { - // skew symmetric matrix X = xi^ - Matrix3 Y = Z_3x3; - Y(0, 1) = -xi(2); - Y(0, 2) = +xi(1); - Y(1, 2) = -xi(0); - return Y - Y.transpose(); + return skewSymmetric(xi); } //****************************************************************************** diff --git a/gtsam/geometry/SO3.h b/gtsam/geometry/SO3.h index 8766fecbe..6403c3ae0 100644 --- a/gtsam/geometry/SO3.h +++ b/gtsam/geometry/SO3.h @@ -134,11 +134,12 @@ GTSAM_EXPORT Matrix99 Dcompose(const SO3& R); /// Functor implementing Exponential map class GTSAM_EXPORT ExpmapFunctor { protected: - const double theta2; - Matrix3 W, WW; - double A, B; // Ethan Eade's constants + const double theta2, theta; + const Matrix3 W, WW; bool nearZero; - double theta, sin_theta, one_minus_cos; // only defined if !nearZero + // Ethan Eade's constants: + double A; // A = sin(theta) / theta or 1 for theta->0 + double B; // B = (1 - cos(theta)) / theta^2 or 0.5 for theta->0 void init(bool nearZeroApprox = false); @@ -156,17 +157,19 @@ class GTSAM_EXPORT ExpmapFunctor { /// Functor that implements Exponential map *and* its derivatives class DexpFunctor : public ExpmapFunctor { protected: - static constexpr double one_sixth = 1.0 / 6.0; const Vector3 omega; - double C; // Ethan Eade's C constant + double C; // Ethan's C constant: (1 - A) / theta^2 or 1/6 for theta->0 + // Constants used in cross and doubleCross + double D; // (A - 2.0 * B) / theta2 or -1/12 for theta->0 + double E; // (B - 3.0 * C) / theta2 or -1/60 for theta->0 + public: /// Computes B * (omega x v). Vector3 cross(const Vector3& v, OptionalJacobian<3, 3> H = {}) const; /// Computes C * (omega x (omega x v)). Vector3 doubleCross(const Vector3& v, OptionalJacobian<3, 3> H = {}) const; - public: /// Constructor with element of Lie algebra so(3) GTSAM_EXPORT explicit DexpFunctor(const Vector3& omega, bool nearZeroApprox = false); @@ -199,6 +202,11 @@ class DexpFunctor : public ExpmapFunctor { GTSAM_EXPORT Vector3 applyLeftJacobian(const Vector3& v, OptionalJacobian<3, 3> H1 = {}, OptionalJacobian<3, 3> H2 = {}) const; + + protected: + static constexpr double one_sixth = 1.0 / 6.0; + static constexpr double _one_twelfth = -1.0 / 12.0; + static constexpr double _one_sixtieth = -1.0 / 60.0; }; } // namespace so3 diff --git a/gtsam/geometry/tests/testSO3.cpp b/gtsam/geometry/tests/testSO3.cpp index 1e105ceca..e8de2e93d 100644 --- a/gtsam/geometry/tests/testSO3.cpp +++ b/gtsam/geometry/tests/testSO3.cpp @@ -303,19 +303,63 @@ TEST(SO3, JacobianLogmap) { EXPECT(assert_equal(Jexpected, Jactual)); } +namespace test_cases { +std::vector nearZeros{ + {0, 0, 0}, {1e-5, 0, 0}, {0, 1e-5, 0}, {0, 0, 1e-5}}; +std::vector omegas{ + {0, 0, 0}, {1, 0, 0}, {0, 1, 0}, {0, 0, 1}, {0.1, 0.2, 0.3}}; +std::vector vs{{1, 0, 0}, {0, 1, 0}, {0, 0, 1}, {0.4, 0.3, 0.2}}; +} // namespace test_cases + +//****************************************************************************** +TEST(SO3, Cross) { + Matrix aH1; + for (bool nearZero : {true, false}) { + std::function f = + [=](const Vector3& omega, const Vector3& v) { + return so3::DexpFunctor(omega, nearZero).cross(v); + }; + const auto& omegas = nearZero ? test_cases::nearZeros : test_cases::omegas; + for (Vector3 omega : omegas) { + so3::DexpFunctor local(omega, nearZero); + for (Vector3 v : test_cases::vs) { + local.cross(v, aH1); + EXPECT(assert_equal(numericalDerivative21(f, omega, v), aH1)); + } + } + } +} + +//****************************************************************************** +TEST(SO3, DoubleCross) { + Matrix aH1; + for (bool nearZero : {true, false}) { + std::function f = + [=](const Vector3& omega, const Vector3& v) { + return so3::DexpFunctor(omega, nearZero).doubleCross(v); + }; + const auto& omegas = nearZero ? test_cases::nearZeros : test_cases::omegas; + for (Vector3 omega : omegas) { + so3::DexpFunctor local(omega, nearZero); + for (Vector3 v : test_cases::vs) { + local.doubleCross(v, aH1); + EXPECT(assert_equal(numericalDerivative21(f, omega, v), aH1)); + } + } + } +} + //****************************************************************************** TEST(SO3, ApplyDexp) { Matrix aH1, aH2; - for (bool nearZeroApprox : {true, false}) { + for (bool nearZero : {true, false}) { std::function f = [=](const Vector3& omega, const Vector3& v) { - return so3::DexpFunctor(omega, nearZeroApprox).applyDexp(v); + return so3::DexpFunctor(omega, nearZero).applyDexp(v); }; - for (Vector3 omega : {Vector3(0, 0, 0), Vector3(1, 0, 0), Vector3(0, 1, 0), - Vector3(0, 0, 1), Vector3(0.1, 0.2, 0.3)}) { - so3::DexpFunctor local(omega, nearZeroApprox); - for (Vector3 v : {Vector3(1, 0, 0), Vector3(0, 1, 0), Vector3(0, 0, 1), - Vector3(0.4, 0.3, 0.2)}) { + for (Vector3 omega : test_cases::omegas) { + so3::DexpFunctor local(omega, nearZero); + for (Vector3 v : test_cases::vs) { EXPECT(assert_equal(Vector3(local.dexp() * v), local.applyDexp(v, aH1, aH2))); EXPECT(assert_equal(numericalDerivative21(f, omega, v), aH1)); @@ -329,18 +373,16 @@ TEST(SO3, ApplyDexp) { //****************************************************************************** TEST(SO3, ApplyLeftJacobian) { Matrix aH1, aH2; - for (bool nearZeroApprox : {false, true}) { + for (bool nearZero : {true, false}) { std::function f = [=](const Vector3& omega, const Vector3& v) { - return so3::DexpFunctor(omega, nearZeroApprox).applyLeftJacobian(v); + return so3::DexpFunctor(omega, nearZero).applyLeftJacobian(v); }; - for (Vector3 omega : {Vector3(0, 0, 0), Vector3(1, 0, 0), Vector3(0, 1, 0), - Vector3(0, 0, 1), Vector3(0.1, 0.2, 0.3)}) { - so3::DexpFunctor local(omega, nearZeroApprox); - for (Vector3 v : {Vector3(1, 0, 0), Vector3(0, 1, 0), Vector3(0, 0, 1), - Vector3(0.4, 0.3, 0.2)}) { + for (Vector3 omega : test_cases::omegas) { + so3::DexpFunctor local(omega, nearZero); + for (Vector3 v : test_cases::vs) { CHECK(assert_equal(Vector3(local.leftJacobian() * v), - local.applyLeftJacobian(v, aH1, aH2))); + local.applyLeftJacobian(v, aH1, aH2))); CHECK(assert_equal(numericalDerivative21(f, omega, v), aH1)); CHECK(assert_equal(numericalDerivative22(f, omega, v), aH2)); CHECK(assert_equal(local.leftJacobian(), aH2)); @@ -352,17 +394,15 @@ TEST(SO3, ApplyLeftJacobian) { //****************************************************************************** TEST(SO3, ApplyInvDexp) { Matrix aH1, aH2; - for (bool nearZeroApprox : {true, false}) { + for (bool nearZero : {true, false}) { std::function f = [=](const Vector3& omega, const Vector3& v) { - return so3::DexpFunctor(omega, nearZeroApprox).applyInvDexp(v); + return so3::DexpFunctor(omega, nearZero).applyInvDexp(v); }; - for (Vector3 omega : {Vector3(0, 0, 0), Vector3(1, 0, 0), Vector3(0, 1, 0), - Vector3(0, 0, 1), Vector3(0.1, 0.2, 0.3)}) { - so3::DexpFunctor local(omega, nearZeroApprox); + for (Vector3 omega : test_cases::omegas) { + so3::DexpFunctor local(omega, nearZero); Matrix invDexp = local.dexp().inverse(); - for (Vector3 v : {Vector3(1, 0, 0), Vector3(0, 1, 0), Vector3(0, 0, 1), - Vector3(0.4, 0.3, 0.2)}) { + for (Vector3 v : test_cases::vs) { EXPECT(assert_equal(Vector3(invDexp * v), local.applyInvDexp(v, aH1, aH2))); EXPECT(assert_equal(numericalDerivative21(f, omega, v), aH1)); From 9b0f3c3b38f9eaaa2cc2e9be24a0fa94a3e5c04b Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 15 Dec 2024 17:55:22 -0500 Subject: [PATCH 40/80] Incredible simplification using E and F --- gtsam/geometry/Pose3.cpp | 21 ++++++++------------- 1 file changed, 8 insertions(+), 13 deletions(-) diff --git a/gtsam/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp index d87e3164c..7faf433bb 100644 --- a/gtsam/geometry/Pose3.cpp +++ b/gtsam/geometry/Pose3.cpp @@ -243,11 +243,15 @@ namespace pose3 { class ExpmapFunctor : public so3::DexpFunctor { private: static constexpr double one_twenty_fourth = 1.0 / 24.0; - static constexpr double one_one_hundred_twentieth = 1.0 / 120.0; + + // Constant used in computeQ + double F; // (B - 0.5) / theta2 or -1/24 for theta->0 public: ExpmapFunctor(const Vector3& omega, bool nearZeroApprox = false) - : so3::DexpFunctor(omega, nearZeroApprox) {} + : so3::DexpFunctor(omega, nearZeroApprox) { + F = nearZero ? - one_twenty_fourth : (B - 0.5) / theta2; + } // Compute the bottom-left 3x3 block of the SE(3) Expmap derivative // TODO(Frank): t = applyLeftJacobian(v), it would be nice to understand @@ -255,17 +259,8 @@ class ExpmapFunctor : public so3::DexpFunctor { Matrix3 computeQ(const Vector3& v) const { const Matrix3 V = skewSymmetric(v); const Matrix3 WVW = W * V * W; - - if (!nearZero) { - // Simplified from closed-form formula in Barfoot14tro eq. (102) - return -0.5 * V + C * (W * V + V * W - WVW) + - (B - 0.5) / theta2 * (WW * V + V * WW - 3 * WVW) - - 0.5 * (B - 3 * C) / theta2 * (WVW * W + W * WVW); - } else { - return -0.5 * V + one_sixth * (W * V + V * W - WVW) - - one_twenty_fourth * (WW * V + V * WW - 3 * WVW) + - one_one_hundred_twentieth * (WVW * W + W * WVW); - } + return -0.5 * V + C * (W * V + V * W - WVW) + + F * (WW * V + V * WW - 3 * WVW) - 0.5 * E * (WVW * W + W * WVW); } }; } // namespace pose3 From e3906162e7604cb52f2fced9ed4bd2f57f17b942 Mon Sep 17 00:00:00 2001 From: Joel Truher Date: Sun, 15 Dec 2024 15:44:45 -0800 Subject: [PATCH 41/80] projection and SFM for 2d poses --- gtsam/slam/PlanarProjectionFactor.h | 174 +++++++++++++ gtsam/slam/PlanarSFMFactor.h | 195 ++++++++++++++ gtsam/slam/slam.i | 24 ++ .../slam/tests/testPlanarProjectionFactor.cpp | 156 +++++++++++ gtsam/slam/tests/testPlanarSFMFactor.cpp | 243 ++++++++++++++++++ 5 files changed, 792 insertions(+) create mode 100644 gtsam/slam/PlanarProjectionFactor.h create mode 100644 gtsam/slam/PlanarSFMFactor.h create mode 100644 gtsam/slam/tests/testPlanarProjectionFactor.cpp create mode 100644 gtsam/slam/tests/testPlanarSFMFactor.cpp diff --git a/gtsam/slam/PlanarProjectionFactor.h b/gtsam/slam/PlanarProjectionFactor.h new file mode 100644 index 000000000..9589c36d5 --- /dev/null +++ b/gtsam/slam/PlanarProjectionFactor.h @@ -0,0 +1,174 @@ +/** + * Similar to GenericProjectionFactor, but: + * + * * 2d pose variable (robot on the floor) + * * constant landmarks + * * batched input + * * numeric differentiation + * + * This factor is useful for high-school robotics competitions, + * which run robots on the floor, with use fixed maps and fiducial + * markers. The camera offset and calibration are fixed, perhaps + * found using PlanarSFMFactor. + * + * The python version of this factor uses batches, to save on calls + * across the C++/python boundary, but here the only extra cost + * is instantiating the camera, so there's no batch. + * + * @see https://www.firstinspires.org/ + * @see PlanarSFMFactor.h + * + * @file PlanarSFMFactor.h + * @brief for planar smoothing + * @date Dec 2, 2024 + * @author joel@truher.org + */ +#pragma once + +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include + + +namespace gtsam { + /** + * @class PlanarProjectionFactor + * @brief Camera projection for robot on the floor. + */ + class PlanarProjectionFactor : public NoiseModelFactorN { + typedef NoiseModelFactorN Base; + static const Pose3 CAM_COORD; + + protected: + + Point3 landmark_; // landmark + Point2 measured_; // pixel measurement + Pose3 offset_; // camera offset to robot pose + Cal3DS2 calib_; // camera calibration + + public: + // Provide access to the Matrix& version of evaluateError: + using Base::evaluateError; + + PlanarProjectionFactor() {} + + /** + * @param landmarks point in the world + * @param measured corresponding point in the camera frame + * @param offset constant camera offset from pose + * @param calib constant camera calibration + * @param model stddev of the measurements, ~one pixel? + * @param poseKey index of the robot pose in the z=0 plane + */ + PlanarProjectionFactor( + const Point3& landmark, + const Point2& measured, + const Pose3& offset, + const Cal3DS2& calib, + const SharedNoiseModel& model, + Key poseKey) + : NoiseModelFactorN(model, poseKey), + landmark_(landmark), + measured_(measured), + offset_(offset), + calib_(calib) + { + assert(2 == model->dim()); + } + + ~PlanarProjectionFactor() override {} + + /// @return a deep copy of this factor + gtsam::NonlinearFactor::shared_ptr clone() const override { + return std::static_pointer_cast( + gtsam::NonlinearFactor::shared_ptr(new PlanarProjectionFactor(*this))); + } + + Point2 h2(const Pose2& pose, OptionalMatrixType H1) const { + // this is x-forward z-up + gtsam::Matrix H0; + Pose3 offset_pose = Pose3(pose).compose(offset_, H0); + // std::cout << "\nH0\n" << H0 << "\n"; + // this is z-forward y-down + gtsam::Matrix H00; + Pose3 camera_pose = offset_pose.compose(CAM_COORD, H00); + // std::cout << "\nH00\n" << H00 << "\n"; + PinholeCamera camera = PinholeCamera(camera_pose, calib_); + if (H1) { + // Dpose is 2x6 (R,t) + // H1 is 2x3 (x, y, theta) + gtsam::Matrix Dpose; + Point2 result = camera.project(landmark_, Dpose); + // std::cout << "\nDpose 1\n" << Dpose << "\n"; + Dpose = Dpose * H00 * H0; + // std::cout << "\nDpose 2\n" << Dpose << "\n"; + *H1 = Matrix::Zero(2,3); + (*H1)(0,0) = Dpose(0,3); // du/dx + (*H1)(1,0) = Dpose(1,3); // dv/dx + (*H1)(0,1) = Dpose(0,4); // du/dy + (*H1)(1,1) = Dpose(1,4); // dv/dy + (*H1)(0,2) = Dpose(0,2); // du/dyaw + (*H1)(1,2) = Dpose(1,2); // dv/dyaw + return result; + } else { + return camera.project(landmark_, {}, {}, {}); + } + } + + Point2 h(const Pose2& pose) const { + // this is x-forward z-up + Pose3 offset_pose = Pose3(pose).compose(offset_); + // this is z-forward y-down + Pose3 camera_pose = offset_pose.compose(CAM_COORD); + PinholeCamera camera = PinholeCamera(camera_pose, calib_); + return camera.project2(landmark_); + } + + Vector evaluateError( + const Pose2& pose, + OptionalMatrixType H1 = OptionalNone) const override { + try { + return h2(pose, H1) - measured_; + + // Point2 result = h(pose) - measured_; + // if (H1) *H1 = numericalDerivative11( + // [&](const Pose2& p) {return h(p);}, + // pose); + // return result; + } + catch (CheiralityException& e) { + std::cout << "****** CHIRALITY EXCEPTION ******\n"; + std::cout << "landmark " << landmark_ << "\n"; + std::cout << "pose " << pose << "\n"; + std::cout << "offset " << offset_ << "\n"; + std::cout << "calib " << calib_ << "\n"; + // TODO: check the size here + if (H1) *H1 = Matrix::Zero(2, 3); + // return a large error + return Matrix::Constant(2, 1, 2.0 * calib_.fx()); + } + } + }; + + // camera "zero" is facing +z; this turns it to face +x + const Pose3 PlanarProjectionFactor::CAM_COORD = Pose3( + Rot3(0, 0, 1,// + -1, 0, 0, // + 0, -1, 0), + Vector3(0, 0, 0) + ); + + template<> + struct traits : + public Testable { + }; + +} // namespace gtsam diff --git a/gtsam/slam/PlanarSFMFactor.h b/gtsam/slam/PlanarSFMFactor.h new file mode 100644 index 000000000..c39b40fef --- /dev/null +++ b/gtsam/slam/PlanarSFMFactor.h @@ -0,0 +1,195 @@ +/** + * Similar to GeneralSFMFactor, but: + * + * * 2d pose variable (robot on the floor) + * * camera offset variable + * * constant landmarks + * * batched input + * * numeric differentiation + * + * This factor is useful to find camera calibration and placement, in + * a sort of "autocalibrate" mode. Once a satisfactory solution is + * found, the PlanarProjectionFactor should be used for localization. + * + * The python version of this factor uses batches, to save on calls + * across the C++/python boundary, but here the only extra cost + * is instantiating the camera, so there's no batch. + * + * @see https://www.firstinspires.org/ + * @see PlanarProjectionFactor.h + * + * @file PlanarSFMFactor.h + * @brief for planar smoothing with unknown calibration + * @date Dec 2, 2024 + * @author joel@truher.org + */ +#pragma once + +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include + + +namespace gtsam { + /** + * @class PlanarSFMFactor + * @brief Camera calibration for robot on the floor. + */ + class PlanarSFMFactor : public NoiseModelFactorN { + private: + typedef NoiseModelFactorN Base; + static const Pose3 CAM_COORD; + + protected: + + Point3 landmark_; // landmark + Point2 measured_; // pixel measurement + + public: + // Provide access to the Matrix& version of evaluateError: + using Base::evaluateError; + + PlanarSFMFactor() {} + /** + * @param landmarks point in the world + * @param measured corresponding point in the camera frame + * @param model stddev of the measurements, ~one pixel? + * @param poseKey index of the robot pose2 in the z=0 plane + * @param offsetKey index of the 3d camera offset from the robot pose + * @param calibKey index of the camera calibration + */ + PlanarSFMFactor( + const Point3& landmark, + const Point2& measured, + const SharedNoiseModel& model, + Key poseKey, + Key offsetKey, + Key calibKey) + : NoiseModelFactorN(model, poseKey, offsetKey, calibKey), + landmark_(landmark), measured_(measured) + { + assert(2 == model->dim()); + } + + ~PlanarSFMFactor() override {} + + /// @return a deep copy of this factor + gtsam::NonlinearFactor::shared_ptr clone() const override { + return std::static_pointer_cast( + gtsam::NonlinearFactor::shared_ptr(new PlanarSFMFactor(*this))); + } + + Point2 h2(const Pose2& pose, + const Pose3& offset, + const Cal3DS2& calib, + OptionalMatrixType H1, + OptionalMatrixType H2, + OptionalMatrixType H3 + ) const { + // std::cout << "POSE: " << pose << "\n"; + // std::cout << "OFFSET: " << offset << "\n"; + // std::cout << "CALIB: " << calib << "\n"; + // this is x-forward z-up + gtsam::Matrix H0; + Pose3 offset_pose = Pose3(pose).compose(offset, H0); + // this is z-forward y-down + gtsam::Matrix H00; + Pose3 camera_pose = offset_pose.compose(CAM_COORD, H00); + PinholeCamera camera = PinholeCamera(camera_pose, calib); + if (H1 || H2) { + gtsam::Matrix Dpose; + Point2 result = camera.project(landmark_, Dpose, {}, H3); + gtsam::Matrix DposeOffset = Dpose * H00; + if (H2) + *H2 = DposeOffset; // a deep copy + if (H1) { + gtsam::Matrix DposeOffsetFwd = DposeOffset * H0; + *H1 = Matrix::Zero(2,3); + (*H1)(0,0) = DposeOffsetFwd(0,3); // du/dx + (*H1)(1,0) = DposeOffsetFwd(1,3); // dv/dx + (*H1)(0,1) = DposeOffsetFwd(0,4); // du/dy + (*H1)(1,1) = DposeOffsetFwd(1,4); // dv/dy + (*H1)(0,2) = DposeOffsetFwd(0,2); // du/dyaw + (*H1)(1,2) = DposeOffsetFwd(1,2); // dv/dyaw + } + return result; + } else { + return camera.project(landmark_, {}, {}, {}); + } + } + + Point2 h(const Pose2& pose, + const Pose3& offset, + const Cal3DS2& calib) const { + // std::cout << "POSE: " << pose << "\n"; + // std::cout << "OFFSET: " << offset << "\n"; + // std::cout << "CALIB: " << calib << "\n"; + // this is x-forward z-up + Pose3 offset_pose = Pose3(pose).compose(offset); + // this is z-forward y-down + Pose3 camera_pose = offset_pose.compose(CAM_COORD); + PinholeCamera camera = PinholeCamera(camera_pose, calib); + return camera.project2(landmark_); + } + + Vector evaluateError( + const Pose2& pose, + const Pose3& offset, + const Cal3DS2& calib, + OptionalMatrixType H1 = OptionalNone, + OptionalMatrixType H2 = OptionalNone, + OptionalMatrixType H3 = OptionalNone + ) const override { + try { + return h2(pose,offset,calib,H1,H2,H3) - measured_; + + // Point2 result = h(pose, offset, calib) - measured_; + // if (H1) *H1 = numericalDerivative31( + // [&](const Pose2& p, const Pose3& o, const Cal3DS2& c) {return h(p, o, c);}, + // pose, offset, calib); + // if (H2) *H2 = numericalDerivative32( + // [&](const Pose2& p, const Pose3& o, const Cal3DS2& c) {return h(p, o, c);}, + // pose, offset, calib); + // if (H3) *H3 = numericalDerivative33( + // [&](const Pose2& p, const Pose3& o, const Cal3DS2& c) {return h(p, o, c);}, + // pose, offset, calib); + // return result; + } + catch (CheiralityException& e) { + std::cout << "****** CHIRALITY EXCEPTION ******\n"; + std::cout << "landmark " << landmark_ << "\n"; + std::cout << "pose " << pose << "\n"; + std::cout << "offset " << offset << "\n"; + std::cout << "calib " << calib << "\n"; + // TODO: what should these sizes be? + if (H1) *H1 = Matrix::Zero(2, 3); + if (H2) *H2 = Matrix::Zero(2, 6); + if (H3) *H3 = Matrix::Zero(2, 9); + // return a large error + return Matrix::Constant(2, 1, 2.0 * calib.fx()); + } + } + }; + + // camera "zero" is facing +z; this turns it to face +x + const Pose3 PlanarSFMFactor::CAM_COORD = Pose3( + Rot3(0, 0, 1,// + -1, 0, 0, // + 0, -1, 0), + Vector3(0, 0, 0) + ); + + template<> + struct traits : + public Testable { + }; + +} // namespace gtsam diff --git a/gtsam/slam/slam.i b/gtsam/slam/slam.i index a226f06ec..d69b9890e 100644 --- a/gtsam/slam/slam.i +++ b/gtsam/slam/slam.i @@ -24,6 +24,18 @@ virtual class BetweenFactor : gtsam::NoiseModelFactor { void serialize() const; }; +#include +virtual class PlanarProjectionFactor : gtsam::NoiseModelFactor { + PlanarProjectionFactor( + const gtsam::Point3& landmark, + const gtsam::Point2& measured, + const gtsam::Pose3& offset, + const gtsam::Cal3DS2& calib, + const gtsam::noiseModel::Base* model, + size_t poseKey); + void serialize() const; +}; + #include template virtual class GenericProjectionFactor : gtsam::NoiseModelFactor { @@ -67,6 +79,18 @@ typedef gtsam::GenericProjectionFactor GenericProjectionFactorCal3Unified; +#include +virtual class PlanarSFMFactor : gtsam::NoiseModelFactor { + PlanarSFMFactor( + const gtsam::Point3& landmark, + const gtsam::Point2& measured, + const gtsam::noiseModel::Base* model, + size_t poseKey, + size_t offsetKey, + size_t calibKey); + void serialize() const; +}; + #include template virtual class GeneralSFMFactor : gtsam::NoiseModelFactor { diff --git a/gtsam/slam/tests/testPlanarProjectionFactor.cpp b/gtsam/slam/tests/testPlanarProjectionFactor.cpp new file mode 100644 index 000000000..eafe2041f --- /dev/null +++ b/gtsam/slam/tests/testPlanarProjectionFactor.cpp @@ -0,0 +1,156 @@ +/** + * @file testPlanarProjectionFactor.cpp + * @date Dec 3, 2024 + * @author joel@truher.org + * @brief unit tests for PlanarProjectionFactor + */ + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +using namespace std; +using namespace gtsam; +using symbol_shorthand::X; + +TEST(PlanarProjectionFactor, error1) { + // landmark is on the camera bore (which faces +x) + Point3 landmark(1, 0, 0); + // so pixel measurement is (cx, cy) + Point2 measured(200, 200); + Pose3 offset; + Cal3DS2 calib(200, 200, 0, 200, 200, 0, 0); + SharedNoiseModel model = noiseModel::Diagonal::Sigmas(Vector2(1, 1)); + Values values; + Pose2 pose(0, 0, 0); + values.insert(X(0), pose); + + PlanarProjectionFactor factor(landmark, measured, offset, calib, model, X(0)); + + CHECK_EQUAL(2, factor.dim()); + CHECK(factor.active(values)); + std::vector actualHs(1); + gtsam::Vector actual = factor.unwhitenedError(values, actualHs); + + CHECK(assert_equal(Vector2(0, 0), actual)); + + const Matrix& H1Actual = actualHs.at(0); + CHECK_EQUAL(2, H1Actual.rows()); + CHECK_EQUAL(3, H1Actual.cols()); + const Matrix23 H1Expected = (Matrix23() << // + 0, 200, 200,// + 0, 0, 0).finished(); + CHECK(assert_equal(H1Expected, H1Actual, 1e-6)); +} + +TEST(PlanarProjectionFactor, error2) { + // landmark is in the upper left corner + Point3 landmark(1, 1, 1); + // upper left corner in pixels + Point2 measured(0, 0); + Pose3 offset; + Cal3DS2 calib(200, 200, 0, 200, 200, 0, 0); + SharedNoiseModel model = noiseModel::Diagonal::Sigmas(Vector2(1, 1)); + PlanarProjectionFactor factor(landmark, measured, offset, calib, model, X(0)); + Values values; + Pose2 pose(0, 0, 0); + + values.insert(X(0), pose); + + CHECK_EQUAL(2, factor.dim()); + CHECK(factor.active(values)); + std::vector actualHs(1); + gtsam::Vector actual = factor.unwhitenedError(values, actualHs); + + CHECK(assert_equal(Vector2(0, 0), actual)); + + const Matrix& H1Actual = actualHs.at(0); + CHECK_EQUAL(2, H1Actual.rows()); + CHECK_EQUAL(3, H1Actual.cols()); + Matrix23 H1Expected = (Matrix23() << // + -200, 200, 400, // + -200, 0, 200).finished(); + CHECK(assert_equal(H1Expected, H1Actual, 1e-6)); +} + +TEST(PlanarProjectionFactor, error3) { + // landmark is in the upper left corner + Point3 landmark(1, 1, 1); + // upper left corner in pixels + Point2 measured(0, 0); + Pose3 offset; + // distortion + Cal3DS2 calib(200, 200, 0, 200, 200, -0.2, 0.1); + SharedNoiseModel model = noiseModel::Diagonal::Sigmas(Vector2(1, 1)); + PlanarProjectionFactor factor(landmark, measured, offset, calib, model, X(0)); + Values values; + Pose2 pose(0, 0, 0); + + values.insert(X(0), pose); + + CHECK_EQUAL(2, factor.dim()); + CHECK(factor.active(values)); + std::vector actualHs(1); + gtsam::Vector actual = factor.unwhitenedError(values, actualHs); + + CHECK(assert_equal(Vector2(0, 0), actual)); + + const Matrix& H1Actual = actualHs.at(0); + CHECK_EQUAL(2, H1Actual.rows()); + CHECK_EQUAL(3, H1Actual.cols()); + Matrix23 H1Expected = (Matrix23() << // + -360, 280, 640, // + -360, 80, 440).finished(); + CHECK(assert_equal(H1Expected, H1Actual, 1e-6)); +} + +TEST(PlanarProjectionFactor, jacobian) { + // test many jacobians with many randoms + + std::default_random_engine g; + std::uniform_real_distribution s(-0.3, 0.3); + SharedNoiseModel model = noiseModel::Diagonal::Sigmas(Vector2(1, 1)); + + for (int i = 0; i < 1000; ++i) { + Point3 landmark(2 + s(g), s(g), s(g)); + Point2 measured(200 + 100*s(g), 200 + 100*s(g)); + Pose3 offset(Rot3::Ypr(s(g),s(g),s(g)), Point3(s(g),s(g),s(g))); + Cal3DS2 calib(200, 200, 0, 200, 200, -0.2, 0.1); + + PlanarProjectionFactor factor(landmark, measured, offset, calib, model, X(0)); + + Pose2 pose(s(g), s(g), s(g)); + + // actual H + Matrix H1; + factor.evaluateError(pose, H1); + + Matrix expectedH1 = numericalDerivative11( + [&factor](const Pose2& p) { + return factor.evaluateError(p);}, + pose); + CHECK(assert_equal(expectedH1, H1, 1e-6)); + } + + +} + +/* ************************************************************************* */ +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +/* ************************************************************************* */ + diff --git a/gtsam/slam/tests/testPlanarSFMFactor.cpp b/gtsam/slam/tests/testPlanarSFMFactor.cpp new file mode 100644 index 000000000..22ec67561 --- /dev/null +++ b/gtsam/slam/tests/testPlanarSFMFactor.cpp @@ -0,0 +1,243 @@ +/** + * @file testPlanarSFMFactor.cpp + * @date Dec 3, 2024 + * @author joel@truher.org + * @brief unit tests for PlanarSFMFactor + */ + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +using namespace std; +using namespace gtsam; +using symbol_shorthand::X; +using symbol_shorthand::C; +using symbol_shorthand::K; + +TEST(PlanarSFMFactor, error1) { + // landmark is on the camera bore (facing +x) + Point3 landmark(1, 0, 0); + // so px is (cx, cy) + Point2 measured(200, 200); + // offset is identity + Pose3 offset; + Cal3DS2 calib(200, 200, 0, 200, 200, 0, 0); + SharedNoiseModel model = noiseModel::Diagonal::Sigmas(Vector2(1, 1)); + Values values; + Pose2 pose(0, 0, 0); + values.insert(X(0), pose); + values.insert(C(0), offset); + values.insert(K(0), calib); + + PlanarSFMFactor factor(landmark, measured, model, X(0), C(0), K(0)); + + CHECK_EQUAL(2, factor.dim()); + CHECK(factor.active(values)); + std::vector actualHs(3); + + gtsam::Vector actual = factor.unwhitenedError(values, actualHs); + CHECK(assert_equal(Vector2(0, 0), actual)); + + const Matrix& H1Actual = actualHs.at(0); + const Matrix& H2Actual = actualHs.at(1); + const Matrix& H3Actual = actualHs.at(2); + + CHECK_EQUAL(2, H1Actual.rows()); + CHECK_EQUAL(3, H1Actual.cols()); + CHECK_EQUAL(2, H2Actual.rows()); + CHECK_EQUAL(6, H2Actual.cols()); + CHECK_EQUAL(2, H3Actual.rows()); + CHECK_EQUAL(9, H3Actual.cols()); + + // du/dx etc for the pose2d + Matrix23 H1Expected = (Matrix23() <dim()); + CHECK_EQUAL(2, factor.dim()); + CHECK(factor.active(values)); + std::vector actualHs(3); + gtsam::Vector actual = factor.unwhitenedError(values, actualHs); + + CHECK(assert_equal(Vector2(0, 0), actual)); + + const Matrix& H1Actual = actualHs.at(0); + const Matrix& H2Actual = actualHs.at(1); + const Matrix& H3Actual = actualHs.at(2); + + CHECK_EQUAL(2, H1Actual.rows()); + CHECK_EQUAL(3, H1Actual.cols()); + CHECK_EQUAL(2, H2Actual.rows()); + CHECK_EQUAL(6, H2Actual.cols()); + CHECK_EQUAL(2, H3Actual.rows()); + CHECK_EQUAL(9, H3Actual.cols()); + + Matrix23 H1Expected = (Matrix23() <dim()); + CHECK_EQUAL(2, factor.dim()); + CHECK(factor.active(values)); + std::vector actualHs(3); + gtsam::Vector actual = factor.unwhitenedError(values, actualHs); + + CHECK(assert_equal(Vector2(0, 0), actual)); + + const Matrix& H1Actual = actualHs.at(0); + const Matrix& H2Actual = actualHs.at(1); + const Matrix& H3Actual = actualHs.at(2); + + CHECK_EQUAL(2, H1Actual.rows()); + CHECK_EQUAL(3, H1Actual.cols()); + CHECK_EQUAL(2, H2Actual.rows()); + CHECK_EQUAL(6, H2Actual.cols()); + CHECK_EQUAL(2, H3Actual.rows()); + CHECK_EQUAL(9, H3Actual.cols()); + + Matrix23 H1Expected = (Matrix23() < s(-0.3, 0.3); + SharedNoiseModel model = noiseModel::Diagonal::Sigmas(Vector2(1, 1)); + + for (int i = 0; i < 1000; ++i) { + Point3 landmark(2 + s(g), s(g), s(g)); + Point2 measured(200 + 100*s(g), 200 + 100*s(g)); + Pose3 offset(Rot3::Ypr(s(g),s(g),s(g)), Point3(s(g),s(g),s(g))); + Cal3DS2 calib(200, 200, 0, 200, 200, -0.2, 0.1); + + PlanarSFMFactor factor(landmark, measured, model, X(0), C(0), K(0)); + + Pose2 pose(s(g), s(g), s(g)); + + // actual H + Matrix H1, H2, H3; + factor.evaluateError(pose, offset, calib, H1, H2, H3); + + Matrix expectedH1 = numericalDerivative31( + [&factor](const Pose2& p, const Pose3& o, const Cal3DS2& c) { + return factor.evaluateError(p, o, c);}, + pose, offset, calib); + Matrix expectedH2 = numericalDerivative32( + [&factor](const Pose2& p, const Pose3& o, const Cal3DS2& c) { + return factor.evaluateError(p, o, c);}, + pose, offset, calib); + Matrix expectedH3 = numericalDerivative33( + [&factor](const Pose2& p, const Pose3& o, const Cal3DS2& c) { + return factor.evaluateError(p, o, c);}, + pose, offset, calib); + CHECK(assert_equal(expectedH1, H1, 1e-6)); + CHECK(assert_equal(expectedH2, H2, 1e-6)); + CHECK(assert_equal(expectedH3, H3, 1e-6)); + } +} + + +/* ************************************************************************* */ +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +/* ************************************************************************* */ + From 06d45c416e2934e9c97ccf598df6820ffea7248a Mon Sep 17 00:00:00 2001 From: Joel Truher Date: Sun, 15 Dec 2024 15:44:45 -0800 Subject: [PATCH 42/80] projection and SFM for 2d poses --- gtsam/slam/PlanarProjectionFactor.h | 154 +++++++++++ gtsam/slam/PlanarSFMFactor.h | 165 ++++++++++++ gtsam/slam/slam.i | 24 ++ .../slam/tests/testPlanarProjectionFactor.cpp | 156 +++++++++++ gtsam/slam/tests/testPlanarSFMFactor.cpp | 243 ++++++++++++++++++ 5 files changed, 742 insertions(+) create mode 100644 gtsam/slam/PlanarProjectionFactor.h create mode 100644 gtsam/slam/PlanarSFMFactor.h create mode 100644 gtsam/slam/tests/testPlanarProjectionFactor.cpp create mode 100644 gtsam/slam/tests/testPlanarSFMFactor.cpp diff --git a/gtsam/slam/PlanarProjectionFactor.h b/gtsam/slam/PlanarProjectionFactor.h new file mode 100644 index 000000000..49d1f7cac --- /dev/null +++ b/gtsam/slam/PlanarProjectionFactor.h @@ -0,0 +1,154 @@ +/** + * Similar to GenericProjectionFactor, but: + * + * * 2d pose variable (robot on the floor) + * * constant landmarks + * * batched input + * * numeric differentiation + * + * This factor is useful for high-school robotics competitions, + * which run robots on the floor, with use fixed maps and fiducial + * markers. The camera offset and calibration are fixed, perhaps + * found using PlanarSFMFactor. + * + * The python version of this factor uses batches, to save on calls + * across the C++/python boundary, but here the only extra cost + * is instantiating the camera, so there's no batch. + * + * @see https://www.firstinspires.org/ + * @see PlanarSFMFactor.h + * + * @file PlanarSFMFactor.h + * @brief for planar smoothing + * @date Dec 2, 2024 + * @author joel@truher.org + */ +#pragma once + +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include + + +namespace gtsam { + /** + * @class PlanarProjectionFactor + * @brief Camera projection for robot on the floor. + */ + class PlanarProjectionFactor : public NoiseModelFactorN { + typedef NoiseModelFactorN Base; + static const Pose3 CAM_COORD; + + protected: + + Point3 landmark_; // landmark + Point2 measured_; // pixel measurement + Pose3 offset_; // camera offset to robot pose + Cal3DS2 calib_; // camera calibration + + public: + // Provide access to the Matrix& version of evaluateError: + using Base::evaluateError; + + PlanarProjectionFactor() {} + + /** + * @param landmarks point in the world + * @param measured corresponding point in the camera frame + * @param offset constant camera offset from pose + * @param calib constant camera calibration + * @param model stddev of the measurements, ~one pixel? + * @param poseKey index of the robot pose in the z=0 plane + */ + PlanarProjectionFactor( + const Point3& landmark, + const Point2& measured, + const Pose3& offset, + const Cal3DS2& calib, + const SharedNoiseModel& model, + Key poseKey) + : NoiseModelFactorN(model, poseKey), + landmark_(landmark), + measured_(measured), + offset_(offset), + calib_(calib) + { + assert(2 == model->dim()); + } + + ~PlanarProjectionFactor() override {} + + /// @return a deep copy of this factor + gtsam::NonlinearFactor::shared_ptr clone() const override { + return std::static_pointer_cast( + gtsam::NonlinearFactor::shared_ptr(new PlanarProjectionFactor(*this))); + } + + Point2 h2(const Pose2& pose, OptionalMatrixType H1) const { + // this is x-forward z-up + gtsam::Matrix H0; + Pose3 offset_pose = Pose3(pose).compose(offset_, H0); + // this is z-forward y-down + gtsam::Matrix H00; + Pose3 camera_pose = offset_pose.compose(CAM_COORD, H00); + PinholeCamera camera = PinholeCamera(camera_pose, calib_); + if (H1) { + // Dpose is 2x6 (R,t) + // H1 is 2x3 (x, y, theta) + gtsam::Matrix Dpose; + Point2 result = camera.project(landmark_, Dpose); + Dpose = Dpose * H00 * H0; + *H1 = Matrix::Zero(2, 3); + (*H1)(0, 0) = Dpose(0, 3); // du/dx + (*H1)(1, 0) = Dpose(1, 3); // dv/dx + (*H1)(0, 1) = Dpose(0, 4); // du/dy + (*H1)(1, 1) = Dpose(1, 4); // dv/dy + (*H1)(0, 2) = Dpose(0, 2); // du/dyaw + (*H1)(1, 2) = Dpose(1, 2); // dv/dyaw + return result; + } + else { + return camera.project(landmark_, {}, {}, {}); + } + } + + Vector evaluateError( + const Pose2& pose, + OptionalMatrixType H1 = OptionalNone) const override { + try { + return h2(pose, H1) - measured_; + } catch (CheiralityException& e) { + std::cout << "****** CHIRALITY EXCEPTION ******\n"; + std::cout << "landmark " << landmark_ << "\n"; + std::cout << "pose " << pose << "\n"; + std::cout << "offset " << offset_ << "\n"; + std::cout << "calib " << calib_ << "\n"; + if (H1) *H1 = Matrix::Zero(2, 3); + // return a large error + return Matrix::Constant(2, 1, 2.0 * calib_.fx()); + } + } + }; + + // camera "zero" is facing +z; this turns it to face +x + const Pose3 PlanarProjectionFactor::CAM_COORD = Pose3( + Rot3(0, 0, 1,// + -1, 0, 0, // + 0, -1, 0), + Vector3(0, 0, 0) + ); + + template<> + struct traits : + public Testable { + }; + +} // namespace gtsam diff --git a/gtsam/slam/PlanarSFMFactor.h b/gtsam/slam/PlanarSFMFactor.h new file mode 100644 index 000000000..bdcbb6ee2 --- /dev/null +++ b/gtsam/slam/PlanarSFMFactor.h @@ -0,0 +1,165 @@ +/** + * Similar to GeneralSFMFactor, but: + * + * * 2d pose variable (robot on the floor) + * * camera offset variable + * * constant landmarks + * * batched input + * * numeric differentiation + * + * This factor is useful to find camera calibration and placement, in + * a sort of "autocalibrate" mode. Once a satisfactory solution is + * found, the PlanarProjectionFactor should be used for localization. + * + * The python version of this factor uses batches, to save on calls + * across the C++/python boundary, but here the only extra cost + * is instantiating the camera, so there's no batch. + * + * @see https://www.firstinspires.org/ + * @see PlanarProjectionFactor.h + * + * @file PlanarSFMFactor.h + * @brief for planar smoothing with unknown calibration + * @date Dec 2, 2024 + * @author joel@truher.org + */ +#pragma once + +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include + + +namespace gtsam { + /** + * @class PlanarSFMFactor + * @brief Camera calibration for robot on the floor. + */ + class PlanarSFMFactor : public NoiseModelFactorN { + private: + typedef NoiseModelFactorN Base; + static const Pose3 CAM_COORD; + + protected: + + Point3 landmark_; // landmark + Point2 measured_; // pixel measurement + + public: + // Provide access to the Matrix& version of evaluateError: + using Base::evaluateError; + + PlanarSFMFactor() {} + /** + * @param landmarks point in the world + * @param measured corresponding point in the camera frame + * @param model stddev of the measurements, ~one pixel? + * @param poseKey index of the robot pose2 in the z=0 plane + * @param offsetKey index of the 3d camera offset from the robot pose + * @param calibKey index of the camera calibration + */ + PlanarSFMFactor( + const Point3& landmark, + const Point2& measured, + const SharedNoiseModel& model, + Key poseKey, + Key offsetKey, + Key calibKey) + : NoiseModelFactorN(model, poseKey, offsetKey, calibKey), + landmark_(landmark), measured_(measured) + { + assert(2 == model->dim()); + } + + ~PlanarSFMFactor() override {} + + /// @return a deep copy of this factor + gtsam::NonlinearFactor::shared_ptr clone() const override { + return std::static_pointer_cast( + gtsam::NonlinearFactor::shared_ptr(new PlanarSFMFactor(*this))); + } + + Point2 h2(const Pose2& pose, + const Pose3& offset, + const Cal3DS2& calib, + OptionalMatrixType H1, + OptionalMatrixType H2, + OptionalMatrixType H3 + ) const { + // this is x-forward z-up + gtsam::Matrix H0; + Pose3 offset_pose = Pose3(pose).compose(offset, H0); + // this is z-forward y-down + gtsam::Matrix H00; + Pose3 camera_pose = offset_pose.compose(CAM_COORD, H00); + PinholeCamera camera = PinholeCamera(camera_pose, calib); + if (H1 || H2) { + gtsam::Matrix Dpose; + Point2 result = camera.project(landmark_, Dpose, {}, H3); + gtsam::Matrix DposeOffset = Dpose * H00; + if (H2) + *H2 = DposeOffset; // a deep copy + if (H1) { + gtsam::Matrix DposeOffsetFwd = DposeOffset * H0; + *H1 = Matrix::Zero(2,3); + (*H1)(0,0) = DposeOffsetFwd(0,3); // du/dx + (*H1)(1,0) = DposeOffsetFwd(1,3); // dv/dx + (*H1)(0,1) = DposeOffsetFwd(0,4); // du/dy + (*H1)(1,1) = DposeOffsetFwd(1,4); // dv/dy + (*H1)(0,2) = DposeOffsetFwd(0,2); // du/dyaw + (*H1)(1,2) = DposeOffsetFwd(1,2); // dv/dyaw + } + return result; + } else { + return camera.project(landmark_, {}, {}, {}); + } + } + + Vector evaluateError( + const Pose2& pose, + const Pose3& offset, + const Cal3DS2& calib, + OptionalMatrixType H1 = OptionalNone, + OptionalMatrixType H2 = OptionalNone, + OptionalMatrixType H3 = OptionalNone + ) const override { + try { + return h2(pose,offset,calib,H1,H2,H3) - measured_; + } + catch (CheiralityException& e) { + std::cout << "****** CHIRALITY EXCEPTION ******\n"; + std::cout << "landmark " << landmark_ << "\n"; + std::cout << "pose " << pose << "\n"; + std::cout << "offset " << offset << "\n"; + std::cout << "calib " << calib << "\n"; + if (H1) *H1 = Matrix::Zero(2, 3); + if (H2) *H2 = Matrix::Zero(2, 6); + if (H3) *H3 = Matrix::Zero(2, 9); + // return a large error + return Matrix::Constant(2, 1, 2.0 * calib.fx()); + } + } + }; + + // camera "zero" is facing +z; this turns it to face +x + const Pose3 PlanarSFMFactor::CAM_COORD = Pose3( + Rot3(0, 0, 1,// + -1, 0, 0, // + 0, -1, 0), + Vector3(0, 0, 0) + ); + + template<> + struct traits : + public Testable { + }; + +} // namespace gtsam diff --git a/gtsam/slam/slam.i b/gtsam/slam/slam.i index a226f06ec..d69b9890e 100644 --- a/gtsam/slam/slam.i +++ b/gtsam/slam/slam.i @@ -24,6 +24,18 @@ virtual class BetweenFactor : gtsam::NoiseModelFactor { void serialize() const; }; +#include +virtual class PlanarProjectionFactor : gtsam::NoiseModelFactor { + PlanarProjectionFactor( + const gtsam::Point3& landmark, + const gtsam::Point2& measured, + const gtsam::Pose3& offset, + const gtsam::Cal3DS2& calib, + const gtsam::noiseModel::Base* model, + size_t poseKey); + void serialize() const; +}; + #include template virtual class GenericProjectionFactor : gtsam::NoiseModelFactor { @@ -67,6 +79,18 @@ typedef gtsam::GenericProjectionFactor GenericProjectionFactorCal3Unified; +#include +virtual class PlanarSFMFactor : gtsam::NoiseModelFactor { + PlanarSFMFactor( + const gtsam::Point3& landmark, + const gtsam::Point2& measured, + const gtsam::noiseModel::Base* model, + size_t poseKey, + size_t offsetKey, + size_t calibKey); + void serialize() const; +}; + #include template virtual class GeneralSFMFactor : gtsam::NoiseModelFactor { diff --git a/gtsam/slam/tests/testPlanarProjectionFactor.cpp b/gtsam/slam/tests/testPlanarProjectionFactor.cpp new file mode 100644 index 000000000..eafe2041f --- /dev/null +++ b/gtsam/slam/tests/testPlanarProjectionFactor.cpp @@ -0,0 +1,156 @@ +/** + * @file testPlanarProjectionFactor.cpp + * @date Dec 3, 2024 + * @author joel@truher.org + * @brief unit tests for PlanarProjectionFactor + */ + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +using namespace std; +using namespace gtsam; +using symbol_shorthand::X; + +TEST(PlanarProjectionFactor, error1) { + // landmark is on the camera bore (which faces +x) + Point3 landmark(1, 0, 0); + // so pixel measurement is (cx, cy) + Point2 measured(200, 200); + Pose3 offset; + Cal3DS2 calib(200, 200, 0, 200, 200, 0, 0); + SharedNoiseModel model = noiseModel::Diagonal::Sigmas(Vector2(1, 1)); + Values values; + Pose2 pose(0, 0, 0); + values.insert(X(0), pose); + + PlanarProjectionFactor factor(landmark, measured, offset, calib, model, X(0)); + + CHECK_EQUAL(2, factor.dim()); + CHECK(factor.active(values)); + std::vector actualHs(1); + gtsam::Vector actual = factor.unwhitenedError(values, actualHs); + + CHECK(assert_equal(Vector2(0, 0), actual)); + + const Matrix& H1Actual = actualHs.at(0); + CHECK_EQUAL(2, H1Actual.rows()); + CHECK_EQUAL(3, H1Actual.cols()); + const Matrix23 H1Expected = (Matrix23() << // + 0, 200, 200,// + 0, 0, 0).finished(); + CHECK(assert_equal(H1Expected, H1Actual, 1e-6)); +} + +TEST(PlanarProjectionFactor, error2) { + // landmark is in the upper left corner + Point3 landmark(1, 1, 1); + // upper left corner in pixels + Point2 measured(0, 0); + Pose3 offset; + Cal3DS2 calib(200, 200, 0, 200, 200, 0, 0); + SharedNoiseModel model = noiseModel::Diagonal::Sigmas(Vector2(1, 1)); + PlanarProjectionFactor factor(landmark, measured, offset, calib, model, X(0)); + Values values; + Pose2 pose(0, 0, 0); + + values.insert(X(0), pose); + + CHECK_EQUAL(2, factor.dim()); + CHECK(factor.active(values)); + std::vector actualHs(1); + gtsam::Vector actual = factor.unwhitenedError(values, actualHs); + + CHECK(assert_equal(Vector2(0, 0), actual)); + + const Matrix& H1Actual = actualHs.at(0); + CHECK_EQUAL(2, H1Actual.rows()); + CHECK_EQUAL(3, H1Actual.cols()); + Matrix23 H1Expected = (Matrix23() << // + -200, 200, 400, // + -200, 0, 200).finished(); + CHECK(assert_equal(H1Expected, H1Actual, 1e-6)); +} + +TEST(PlanarProjectionFactor, error3) { + // landmark is in the upper left corner + Point3 landmark(1, 1, 1); + // upper left corner in pixels + Point2 measured(0, 0); + Pose3 offset; + // distortion + Cal3DS2 calib(200, 200, 0, 200, 200, -0.2, 0.1); + SharedNoiseModel model = noiseModel::Diagonal::Sigmas(Vector2(1, 1)); + PlanarProjectionFactor factor(landmark, measured, offset, calib, model, X(0)); + Values values; + Pose2 pose(0, 0, 0); + + values.insert(X(0), pose); + + CHECK_EQUAL(2, factor.dim()); + CHECK(factor.active(values)); + std::vector actualHs(1); + gtsam::Vector actual = factor.unwhitenedError(values, actualHs); + + CHECK(assert_equal(Vector2(0, 0), actual)); + + const Matrix& H1Actual = actualHs.at(0); + CHECK_EQUAL(2, H1Actual.rows()); + CHECK_EQUAL(3, H1Actual.cols()); + Matrix23 H1Expected = (Matrix23() << // + -360, 280, 640, // + -360, 80, 440).finished(); + CHECK(assert_equal(H1Expected, H1Actual, 1e-6)); +} + +TEST(PlanarProjectionFactor, jacobian) { + // test many jacobians with many randoms + + std::default_random_engine g; + std::uniform_real_distribution s(-0.3, 0.3); + SharedNoiseModel model = noiseModel::Diagonal::Sigmas(Vector2(1, 1)); + + for (int i = 0; i < 1000; ++i) { + Point3 landmark(2 + s(g), s(g), s(g)); + Point2 measured(200 + 100*s(g), 200 + 100*s(g)); + Pose3 offset(Rot3::Ypr(s(g),s(g),s(g)), Point3(s(g),s(g),s(g))); + Cal3DS2 calib(200, 200, 0, 200, 200, -0.2, 0.1); + + PlanarProjectionFactor factor(landmark, measured, offset, calib, model, X(0)); + + Pose2 pose(s(g), s(g), s(g)); + + // actual H + Matrix H1; + factor.evaluateError(pose, H1); + + Matrix expectedH1 = numericalDerivative11( + [&factor](const Pose2& p) { + return factor.evaluateError(p);}, + pose); + CHECK(assert_equal(expectedH1, H1, 1e-6)); + } + + +} + +/* ************************************************************************* */ +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +/* ************************************************************************* */ + diff --git a/gtsam/slam/tests/testPlanarSFMFactor.cpp b/gtsam/slam/tests/testPlanarSFMFactor.cpp new file mode 100644 index 000000000..22ec67561 --- /dev/null +++ b/gtsam/slam/tests/testPlanarSFMFactor.cpp @@ -0,0 +1,243 @@ +/** + * @file testPlanarSFMFactor.cpp + * @date Dec 3, 2024 + * @author joel@truher.org + * @brief unit tests for PlanarSFMFactor + */ + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +using namespace std; +using namespace gtsam; +using symbol_shorthand::X; +using symbol_shorthand::C; +using symbol_shorthand::K; + +TEST(PlanarSFMFactor, error1) { + // landmark is on the camera bore (facing +x) + Point3 landmark(1, 0, 0); + // so px is (cx, cy) + Point2 measured(200, 200); + // offset is identity + Pose3 offset; + Cal3DS2 calib(200, 200, 0, 200, 200, 0, 0); + SharedNoiseModel model = noiseModel::Diagonal::Sigmas(Vector2(1, 1)); + Values values; + Pose2 pose(0, 0, 0); + values.insert(X(0), pose); + values.insert(C(0), offset); + values.insert(K(0), calib); + + PlanarSFMFactor factor(landmark, measured, model, X(0), C(0), K(0)); + + CHECK_EQUAL(2, factor.dim()); + CHECK(factor.active(values)); + std::vector actualHs(3); + + gtsam::Vector actual = factor.unwhitenedError(values, actualHs); + CHECK(assert_equal(Vector2(0, 0), actual)); + + const Matrix& H1Actual = actualHs.at(0); + const Matrix& H2Actual = actualHs.at(1); + const Matrix& H3Actual = actualHs.at(2); + + CHECK_EQUAL(2, H1Actual.rows()); + CHECK_EQUAL(3, H1Actual.cols()); + CHECK_EQUAL(2, H2Actual.rows()); + CHECK_EQUAL(6, H2Actual.cols()); + CHECK_EQUAL(2, H3Actual.rows()); + CHECK_EQUAL(9, H3Actual.cols()); + + // du/dx etc for the pose2d + Matrix23 H1Expected = (Matrix23() <dim()); + CHECK_EQUAL(2, factor.dim()); + CHECK(factor.active(values)); + std::vector actualHs(3); + gtsam::Vector actual = factor.unwhitenedError(values, actualHs); + + CHECK(assert_equal(Vector2(0, 0), actual)); + + const Matrix& H1Actual = actualHs.at(0); + const Matrix& H2Actual = actualHs.at(1); + const Matrix& H3Actual = actualHs.at(2); + + CHECK_EQUAL(2, H1Actual.rows()); + CHECK_EQUAL(3, H1Actual.cols()); + CHECK_EQUAL(2, H2Actual.rows()); + CHECK_EQUAL(6, H2Actual.cols()); + CHECK_EQUAL(2, H3Actual.rows()); + CHECK_EQUAL(9, H3Actual.cols()); + + Matrix23 H1Expected = (Matrix23() <dim()); + CHECK_EQUAL(2, factor.dim()); + CHECK(factor.active(values)); + std::vector actualHs(3); + gtsam::Vector actual = factor.unwhitenedError(values, actualHs); + + CHECK(assert_equal(Vector2(0, 0), actual)); + + const Matrix& H1Actual = actualHs.at(0); + const Matrix& H2Actual = actualHs.at(1); + const Matrix& H3Actual = actualHs.at(2); + + CHECK_EQUAL(2, H1Actual.rows()); + CHECK_EQUAL(3, H1Actual.cols()); + CHECK_EQUAL(2, H2Actual.rows()); + CHECK_EQUAL(6, H2Actual.cols()); + CHECK_EQUAL(2, H3Actual.rows()); + CHECK_EQUAL(9, H3Actual.cols()); + + Matrix23 H1Expected = (Matrix23() < s(-0.3, 0.3); + SharedNoiseModel model = noiseModel::Diagonal::Sigmas(Vector2(1, 1)); + + for (int i = 0; i < 1000; ++i) { + Point3 landmark(2 + s(g), s(g), s(g)); + Point2 measured(200 + 100*s(g), 200 + 100*s(g)); + Pose3 offset(Rot3::Ypr(s(g),s(g),s(g)), Point3(s(g),s(g),s(g))); + Cal3DS2 calib(200, 200, 0, 200, 200, -0.2, 0.1); + + PlanarSFMFactor factor(landmark, measured, model, X(0), C(0), K(0)); + + Pose2 pose(s(g), s(g), s(g)); + + // actual H + Matrix H1, H2, H3; + factor.evaluateError(pose, offset, calib, H1, H2, H3); + + Matrix expectedH1 = numericalDerivative31( + [&factor](const Pose2& p, const Pose3& o, const Cal3DS2& c) { + return factor.evaluateError(p, o, c);}, + pose, offset, calib); + Matrix expectedH2 = numericalDerivative32( + [&factor](const Pose2& p, const Pose3& o, const Cal3DS2& c) { + return factor.evaluateError(p, o, c);}, + pose, offset, calib); + Matrix expectedH3 = numericalDerivative33( + [&factor](const Pose2& p, const Pose3& o, const Cal3DS2& c) { + return factor.evaluateError(p, o, c);}, + pose, offset, calib); + CHECK(assert_equal(expectedH1, H1, 1e-6)); + CHECK(assert_equal(expectedH2, H2, 1e-6)); + CHECK(assert_equal(expectedH3, H3, 1e-6)); + } +} + + +/* ************************************************************************* */ +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +/* ************************************************************************* */ + From a32bb7bf096d6ccd7432563de28b2fdfd96deb39 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 15 Dec 2024 23:39:45 -0500 Subject: [PATCH 43/80] Export --- gtsam/geometry/Pose3.cpp | 13 +++++++------ 1 file changed, 7 insertions(+), 6 deletions(-) diff --git a/gtsam/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp index 7faf433bb..f317cd4e8 100644 --- a/gtsam/geometry/Pose3.cpp +++ b/gtsam/geometry/Pose3.cpp @@ -240,18 +240,16 @@ Vector6 Pose3::ChartAtOrigin::Local(const Pose3& pose, ChartJacobian Hpose) { /* ************************************************************************* */ namespace pose3 { -class ExpmapFunctor : public so3::DexpFunctor { - private: - static constexpr double one_twenty_fourth = 1.0 / 24.0; - +class GTSAM_EXPORT ExpmapFunctor : public so3::DexpFunctor { + protected: // Constant used in computeQ double F; // (B - 0.5) / theta2 or -1/24 for theta->0 public: ExpmapFunctor(const Vector3& omega, bool nearZeroApprox = false) : so3::DexpFunctor(omega, nearZeroApprox) { - F = nearZero ? - one_twenty_fourth : (B - 0.5) / theta2; - } + F = nearZero ? _one_twenty_fourth : (B - 0.5) / theta2; + } // Compute the bottom-left 3x3 block of the SE(3) Expmap derivative // TODO(Frank): t = applyLeftJacobian(v), it would be nice to understand @@ -262,6 +260,9 @@ class ExpmapFunctor : public so3::DexpFunctor { return -0.5 * V + C * (W * V + V * W - WVW) + F * (WW * V + V * WW - 3 * WVW) - 0.5 * E * (WVW * W + W * WVW); } + + protected: + static constexpr double _one_twenty_fourth = - 1.0 / 24.0; }; } // namespace pose3 From bcfb7d8444ae4fd1f8883c27e7f81a6b559c2a27 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 15 Dec 2024 23:42:59 -0500 Subject: [PATCH 44/80] Fix test cases --- gtsam/geometry/tests/testSO3.cpp | 50 +++++++++++++++----------------- 1 file changed, 24 insertions(+), 26 deletions(-) diff --git a/gtsam/geometry/tests/testSO3.cpp b/gtsam/geometry/tests/testSO3.cpp index e8de2e93d..3ecabe84b 100644 --- a/gtsam/geometry/tests/testSO3.cpp +++ b/gtsam/geometry/tests/testSO3.cpp @@ -304,26 +304,25 @@ TEST(SO3, JacobianLogmap) { } namespace test_cases { -std::vector nearZeros{ - {0, 0, 0}, {1e-5, 0, 0}, {0, 1e-5, 0}, {0, 0, 1e-5}}; -std::vector omegas{ +std::vector small{{0, 0, 0}, {1e-5, 0, 0}, {0, 1e-5, 0}, {0, 0, 1e-5}}; +std::vector large{ {0, 0, 0}, {1, 0, 0}, {0, 1, 0}, {0, 0, 1}, {0.1, 0.2, 0.3}}; +auto omegas = [](bool nearZero) { return nearZero ? small : large; }; std::vector vs{{1, 0, 0}, {0, 1, 0}, {0, 0, 1}, {0.4, 0.3, 0.2}}; } // namespace test_cases //****************************************************************************** -TEST(SO3, Cross) { +TEST(SO3, CrossB) { Matrix aH1; for (bool nearZero : {true, false}) { std::function f = [=](const Vector3& omega, const Vector3& v) { - return so3::DexpFunctor(omega, nearZero).cross(v); + return so3::DexpFunctor(omega, nearZero).crossB(v); }; - const auto& omegas = nearZero ? test_cases::nearZeros : test_cases::omegas; - for (Vector3 omega : omegas) { + for (const Vector3& omega : test_cases::omegas(nearZero)) { so3::DexpFunctor local(omega, nearZero); - for (Vector3 v : test_cases::vs) { - local.cross(v, aH1); + for (const Vector3& v : test_cases::vs) { + local.crossB(v, aH1); EXPECT(assert_equal(numericalDerivative21(f, omega, v), aH1)); } } @@ -331,18 +330,17 @@ TEST(SO3, Cross) { } //****************************************************************************** -TEST(SO3, DoubleCross) { +TEST(SO3, DoubleCrossC) { Matrix aH1; for (bool nearZero : {true, false}) { std::function f = [=](const Vector3& omega, const Vector3& v) { - return so3::DexpFunctor(omega, nearZero).doubleCross(v); + return so3::DexpFunctor(omega, nearZero).doubleCrossC(v); }; - const auto& omegas = nearZero ? test_cases::nearZeros : test_cases::omegas; - for (Vector3 omega : omegas) { + for (const Vector3& omega : test_cases::omegas(nearZero)) { so3::DexpFunctor local(omega, nearZero); - for (Vector3 v : test_cases::vs) { - local.doubleCross(v, aH1); + for (const Vector3& v : test_cases::vs) { + local.doubleCrossC(v, aH1); EXPECT(assert_equal(numericalDerivative21(f, omega, v), aH1)); } } @@ -357,9 +355,9 @@ TEST(SO3, ApplyDexp) { [=](const Vector3& omega, const Vector3& v) { return so3::DexpFunctor(omega, nearZero).applyDexp(v); }; - for (Vector3 omega : test_cases::omegas) { + for (const Vector3& omega : test_cases::omegas(nearZero)) { so3::DexpFunctor local(omega, nearZero); - for (Vector3 v : test_cases::vs) { + for (const Vector3& v : test_cases::vs) { EXPECT(assert_equal(Vector3(local.dexp() * v), local.applyDexp(v, aH1, aH2))); EXPECT(assert_equal(numericalDerivative21(f, omega, v), aH1)); @@ -378,14 +376,14 @@ TEST(SO3, ApplyLeftJacobian) { [=](const Vector3& omega, const Vector3& v) { return so3::DexpFunctor(omega, nearZero).applyLeftJacobian(v); }; - for (Vector3 omega : test_cases::omegas) { + for (const Vector3& omega : test_cases::omegas(nearZero)) { so3::DexpFunctor local(omega, nearZero); - for (Vector3 v : test_cases::vs) { - CHECK(assert_equal(Vector3(local.leftJacobian() * v), - local.applyLeftJacobian(v, aH1, aH2))); - CHECK(assert_equal(numericalDerivative21(f, omega, v), aH1)); - CHECK(assert_equal(numericalDerivative22(f, omega, v), aH2)); - CHECK(assert_equal(local.leftJacobian(), aH2)); + for (const Vector3& v : test_cases::vs) { + EXPECT(assert_equal(Vector3(local.leftJacobian() * v), + local.applyLeftJacobian(v, aH1, aH2))); + EXPECT(assert_equal(numericalDerivative21(f, omega, v), aH1)); + EXPECT(assert_equal(numericalDerivative22(f, omega, v), aH2)); + EXPECT(assert_equal(local.leftJacobian(), aH2)); } } } @@ -399,10 +397,10 @@ TEST(SO3, ApplyInvDexp) { [=](const Vector3& omega, const Vector3& v) { return so3::DexpFunctor(omega, nearZero).applyInvDexp(v); }; - for (Vector3 omega : test_cases::omegas) { + for (const Vector3& omega : test_cases::omegas(nearZero)) { so3::DexpFunctor local(omega, nearZero); Matrix invDexp = local.dexp().inverse(); - for (Vector3 v : test_cases::vs) { + for (const Vector3& v : test_cases::vs) { EXPECT(assert_equal(Vector3(invDexp * v), local.applyInvDexp(v, aH1, aH2))); EXPECT(assert_equal(numericalDerivative21(f, omega, v), aH1)); From d547fe2ec137ce3f6727d84bd9f1e5b9d7508867 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 15 Dec 2024 23:43:59 -0500 Subject: [PATCH 45/80] Remove all nearZero paths --- gtsam/geometry/SO3.cpp | 52 ++++++++++-------------------------------- gtsam/geometry/SO3.h | 50 +++++++++++++++++++--------------------- 2 files changed, 35 insertions(+), 67 deletions(-) diff --git a/gtsam/geometry/SO3.cpp b/gtsam/geometry/SO3.cpp index e341911b9..ef1fa374b 100644 --- a/gtsam/geometry/SO3.cpp +++ b/gtsam/geometry/SO3.cpp @@ -93,15 +93,7 @@ DexpFunctor::DexpFunctor(const Vector3& omega, bool nearZeroApprox) E = nearZero ? _one_sixtieth : (B - 3.0 * C) / theta2; } -Matrix3 DexpFunctor::rightJacobian() const { - if (nearZero) { - return I_3x3 - B * W; // + C * WW; - } else { - return I_3x3 - B * W + C * WW; - } -} - -Vector3 DexpFunctor::cross(const Vector3& v, OptionalJacobian<3, 3> H) const { +Vector3 DexpFunctor::crossB(const Vector3& v, OptionalJacobian<3, 3> H) const { // Wv = omega x * v const Vector3 Wv = gtsam::cross(omega, v); if (H) { @@ -113,8 +105,8 @@ Vector3 DexpFunctor::cross(const Vector3& v, OptionalJacobian<3, 3> H) const { return B * Wv; } -Vector3 DexpFunctor::doubleCross(const Vector3& v, - OptionalJacobian<3, 3> H) const { +Vector3 DexpFunctor::doubleCrossC(const Vector3& v, + OptionalJacobian<3, 3> H) const { // WWv = omega x (omega x * v) Matrix3 doubleCrossJacobian; const Vector3 WWv = @@ -131,18 +123,12 @@ Vector3 DexpFunctor::doubleCross(const Vector3& v, // Multiplies v with left Jacobian through vector operations only. Vector3 DexpFunctor::applyDexp(const Vector3& v, OptionalJacobian<3, 3> H1, OptionalJacobian<3, 3> H2) const { - if (nearZero) { - if (H1) *H1 = 0.5 * skewSymmetric(v); - if (H2) *H2 = I_3x3 - 0.5 * W; - return v - 0.5 * gtsam::cross(omega, v); - } else { Matrix3 D_BWv_omega, D_CWWv_omega; - const Vector3 BWv = cross(v, D_BWv_omega); - const Vector3 CWWv = doubleCross(v, D_CWWv_omega); - if (H1) *H1 = - D_BWv_omega + D_CWWv_omega; + const Vector3 BWv = crossB(v, D_BWv_omega); + const Vector3 CWWv = doubleCrossC(v, D_CWWv_omega); + if (H1) *H1 = -D_BWv_omega + D_CWWv_omega; if (H2) *H2 = rightJacobian(); return v - BWv + CWWv; - } } Vector3 DexpFunctor::applyInvDexp(const Vector3& v, OptionalJacobian<3, 3> H1, @@ -158,29 +144,15 @@ Vector3 DexpFunctor::applyInvDexp(const Vector3& v, OptionalJacobian<3, 3> H1, return c; } -Matrix3 DexpFunctor::leftJacobian() const { - if (nearZero) { - return I_3x3 + 0.5 * W; // + one_sixth * WW; - } else { - return I_3x3 + B * W + C * WW; - } -} - Vector3 DexpFunctor::applyLeftJacobian(const Vector3& v, OptionalJacobian<3, 3> H1, OptionalJacobian<3, 3> H2) const { - if (nearZero) { - if (H1) *H1 = - 0.5 * skewSymmetric(v); - if (H2) *H2 = I_3x3 + 0.5 * W; - return v + 0.5 * gtsam::cross(omega, v); - } else { - Matrix3 D_BWv_omega, D_CWWv_omega; - const Vector3 BWv = cross(v, D_BWv_omega); - const Vector3 CWWv = doubleCross(v, D_CWWv_omega); - if (H1) *H1 = D_BWv_omega + D_CWWv_omega; - if (H2) *H2 = leftJacobian(); - return v + BWv + CWWv; - } + Matrix3 D_BWv_omega, D_CWWv_omega; + const Vector3 BWv = crossB(v, D_BWv_omega); + const Vector3 CWWv = doubleCrossC(v, D_CWWv_omega); + if (H1) *H1 = D_BWv_omega + D_CWWv_omega; + if (H2) *H2 = leftJacobian(); + return v + BWv + CWWv; } } // namespace so3 diff --git a/gtsam/geometry/SO3.h b/gtsam/geometry/SO3.h index 6403c3ae0..efb947e73 100644 --- a/gtsam/geometry/SO3.h +++ b/gtsam/geometry/SO3.h @@ -155,7 +155,7 @@ class GTSAM_EXPORT ExpmapFunctor { }; /// Functor that implements Exponential map *and* its derivatives -class DexpFunctor : public ExpmapFunctor { +class GTSAM_EXPORT DexpFunctor : public ExpmapFunctor { protected: const Vector3 omega; double C; // Ethan's C constant: (1 - A) / theta^2 or 1/6 for theta->0 @@ -164,15 +164,8 @@ class DexpFunctor : public ExpmapFunctor { double E; // (B - 3.0 * C) / theta2 or -1/60 for theta->0 public: - /// Computes B * (omega x v). - Vector3 cross(const Vector3& v, OptionalJacobian<3, 3> H = {}) const; - - /// Computes C * (omega x (omega x v)). - Vector3 doubleCross(const Vector3& v, OptionalJacobian<3, 3> H = {}) const; - /// Constructor with element of Lie algebra so(3) - GTSAM_EXPORT explicit DexpFunctor(const Vector3& omega, - bool nearZeroApprox = false); + explicit DexpFunctor(const Vector3& omega, bool nearZeroApprox = false); // NOTE(luca): Right Jacobian for Exponential map in SO(3) - equation // (10.86) and following equations in G.S. Chirikjian, "Stochastic Models, @@ -180,28 +173,31 @@ class DexpFunctor : public ExpmapFunctor { // expmap(omega + v) \approx expmap(omega) * expmap(dexp * v) // This maps a perturbation v in the tangent space to // a perturbation on the manifold Expmap(dexp * v) - GTSAM_EXPORT Matrix3 rightJacobian() const; - - /// differential of expmap == right Jacobian - GTSAM_EXPORT Matrix3 dexp() const { return rightJacobian(); } - - /// Multiplies with dexp(), with optional derivatives - GTSAM_EXPORT Vector3 applyDexp(const Vector3& v, - OptionalJacobian<3, 3> H1 = {}, - OptionalJacobian<3, 3> H2 = {}) const; - - /// Multiplies with dexp().inverse(), with optional derivatives - GTSAM_EXPORT Vector3 applyInvDexp(const Vector3& v, - OptionalJacobian<3, 3> H1 = {}, - OptionalJacobian<3, 3> H2 = {}) const; + Matrix3 rightJacobian() const { return I_3x3 - B * W + C * WW; } // Compute the left Jacobian for Exponential map in SO(3) - GTSAM_EXPORT Matrix3 leftJacobian() const; + Matrix3 leftJacobian() const { return I_3x3 + B * W + C * WW; } + + /// differential of expmap == right Jacobian + inline Matrix3 dexp() const { return rightJacobian(); } + + /// Computes B * (omega x v). + Vector3 crossB(const Vector3& v, OptionalJacobian<3, 3> H = {}) const; + + /// Computes C * (omega x (omega x v)). + Vector3 doubleCrossC(const Vector3& v, OptionalJacobian<3, 3> H = {}) const; + + /// Multiplies with dexp(), with optional derivatives + Vector3 applyDexp(const Vector3& v, OptionalJacobian<3, 3> H1 = {}, + OptionalJacobian<3, 3> H2 = {}) const; + + /// Multiplies with dexp().inverse(), with optional derivatives + Vector3 applyInvDexp(const Vector3& v, OptionalJacobian<3, 3> H1 = {}, + OptionalJacobian<3, 3> H2 = {}) const; /// Multiplies with leftJacobian(), with optional derivatives - GTSAM_EXPORT Vector3 applyLeftJacobian(const Vector3& v, - OptionalJacobian<3, 3> H1 = {}, - OptionalJacobian<3, 3> H2 = {}) const; + Vector3 applyLeftJacobian(const Vector3& v, OptionalJacobian<3, 3> H1 = {}, + OptionalJacobian<3, 3> H2 = {}) const; protected: static constexpr double one_sixth = 1.0 / 6.0; From 0c1f087dba7374e13440461e260ee47086d39c31 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Mon, 16 Dec 2024 00:10:26 -0500 Subject: [PATCH 46/80] Final touches --- gtsam/geometry/Pose3.cpp | 7 ++----- gtsam/geometry/SO3.cpp | 25 ++++++++++++------------- gtsam/geometry/SO3.h | 27 ++++++++++++--------------- 3 files changed, 26 insertions(+), 33 deletions(-) diff --git a/gtsam/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp index f317cd4e8..5f0e8e3ce 100644 --- a/gtsam/geometry/Pose3.cpp +++ b/gtsam/geometry/Pose3.cpp @@ -240,12 +240,10 @@ Vector6 Pose3::ChartAtOrigin::Local(const Pose3& pose, ChartJacobian Hpose) { /* ************************************************************************* */ namespace pose3 { -class GTSAM_EXPORT ExpmapFunctor : public so3::DexpFunctor { - protected: +struct GTSAM_EXPORT ExpmapFunctor : public so3::DexpFunctor { // Constant used in computeQ double F; // (B - 0.5) / theta2 or -1/24 for theta->0 - public: ExpmapFunctor(const Vector3& omega, bool nearZeroApprox = false) : so3::DexpFunctor(omega, nearZeroApprox) { F = nearZero ? _one_twenty_fourth : (B - 0.5) / theta2; @@ -253,7 +251,7 @@ class GTSAM_EXPORT ExpmapFunctor : public so3::DexpFunctor { // Compute the bottom-left 3x3 block of the SE(3) Expmap derivative // TODO(Frank): t = applyLeftJacobian(v), it would be nice to understand - // how to compute mess below from its derivatives in w and v. + // how to compute mess below from applyLeftJacobian derivatives in w and v. Matrix3 computeQ(const Vector3& v) const { const Matrix3 V = skewSymmetric(v); const Matrix3 WVW = W * V * W; @@ -261,7 +259,6 @@ class GTSAM_EXPORT ExpmapFunctor : public so3::DexpFunctor { F * (WW * V + V * WW - 3 * WVW) - 0.5 * E * (WVW * W + W * WVW); } - protected: static constexpr double _one_twenty_fourth = - 1.0 / 24.0; }; } // namespace pose3 diff --git a/gtsam/geometry/SO3.cpp b/gtsam/geometry/SO3.cpp index ef1fa374b..c54306ae5 100644 --- a/gtsam/geometry/SO3.cpp +++ b/gtsam/geometry/SO3.cpp @@ -26,7 +26,6 @@ #include #include -#include #include namespace gtsam { @@ -94,7 +93,7 @@ DexpFunctor::DexpFunctor(const Vector3& omega, bool nearZeroApprox) } Vector3 DexpFunctor::crossB(const Vector3& v, OptionalJacobian<3, 3> H) const { - // Wv = omega x * v + // Wv = omega x v const Vector3 Wv = gtsam::cross(omega, v); if (H) { // Apply product rule: @@ -123,10 +122,10 @@ Vector3 DexpFunctor::doubleCrossC(const Vector3& v, // Multiplies v with left Jacobian through vector operations only. Vector3 DexpFunctor::applyDexp(const Vector3& v, OptionalJacobian<3, 3> H1, OptionalJacobian<3, 3> H2) const { - Matrix3 D_BWv_omega, D_CWWv_omega; - const Vector3 BWv = crossB(v, D_BWv_omega); - const Vector3 CWWv = doubleCrossC(v, D_CWWv_omega); - if (H1) *H1 = -D_BWv_omega + D_CWWv_omega; + Matrix3 D_BWv_w, D_CWWv_w; + const Vector3 BWv = crossB(v, H1 ? &D_BWv_w : nullptr); + const Vector3 CWWv = doubleCrossC(v, H1 ? &D_CWWv_w : nullptr); + if (H1) *H1 = -D_BWv_w + D_CWWv_w; if (H2) *H2 = rightJacobian(); return v - BWv + CWWv; } @@ -136,9 +135,9 @@ Vector3 DexpFunctor::applyInvDexp(const Vector3& v, OptionalJacobian<3, 3> H1, const Matrix3 invDexp = rightJacobian().inverse(); const Vector3 c = invDexp * v; if (H1) { - Matrix3 D_dexpv_omega; - applyDexp(c, D_dexpv_omega); // get derivative H of forward mapping - *H1 = -invDexp * D_dexpv_omega; + Matrix3 D_dexpv_w; + applyDexp(c, D_dexpv_w); // get derivative H of forward mapping + *H1 = -invDexp * D_dexpv_w; } if (H2) *H2 = invDexp; return c; @@ -147,10 +146,10 @@ Vector3 DexpFunctor::applyInvDexp(const Vector3& v, OptionalJacobian<3, 3> H1, Vector3 DexpFunctor::applyLeftJacobian(const Vector3& v, OptionalJacobian<3, 3> H1, OptionalJacobian<3, 3> H2) const { - Matrix3 D_BWv_omega, D_CWWv_omega; - const Vector3 BWv = crossB(v, D_BWv_omega); - const Vector3 CWWv = doubleCrossC(v, D_CWWv_omega); - if (H1) *H1 = D_BWv_omega + D_CWWv_omega; + Matrix3 D_BWv_w, D_CWWv_w; + const Vector3 BWv = crossB(v, H1 ? &D_BWv_w : nullptr); + const Vector3 CWWv = doubleCrossC(v, H1 ? &D_CWWv_w : nullptr); + if (H1) *H1 = D_BWv_w + D_CWWv_w; if (H2) *H2 = leftJacobian(); return v + BWv + CWWv; } diff --git a/gtsam/geometry/SO3.h b/gtsam/geometry/SO3.h index efb947e73..f831aa426 100644 --- a/gtsam/geometry/SO3.h +++ b/gtsam/geometry/SO3.h @@ -132,18 +132,15 @@ GTSAM_EXPORT Matrix99 Dcompose(const SO3& R); // functor also implements dedicated methods to apply dexp and/or inv(dexp). /// Functor implementing Exponential map -class GTSAM_EXPORT ExpmapFunctor { - protected: +struct GTSAM_EXPORT ExpmapFunctor { const double theta2, theta; const Matrix3 W, WW; bool nearZero; + // Ethan Eade's constants: - double A; // A = sin(theta) / theta or 1 for theta->0 - double B; // B = (1 - cos(theta)) / theta^2 or 0.5 for theta->0 + double A; // A = sin(theta) / theta or 1 for theta->0 + double B; // B = (1 - cos(theta)) / theta^2 or 0.5 for theta->0 - void init(bool nearZeroApprox = false); - - public: /// Constructor with element of Lie algebra so(3) explicit ExpmapFunctor(const Vector3& omega, bool nearZeroApprox = false); @@ -152,33 +149,34 @@ class GTSAM_EXPORT ExpmapFunctor { /// Rodrigues formula SO3 expmap() const; + + protected: + void init(bool nearZeroApprox = false); }; /// Functor that implements Exponential map *and* its derivatives -class GTSAM_EXPORT DexpFunctor : public ExpmapFunctor { - protected: +struct GTSAM_EXPORT DexpFunctor : public ExpmapFunctor { const Vector3 omega; double C; // Ethan's C constant: (1 - A) / theta^2 or 1/6 for theta->0 // Constants used in cross and doubleCross double D; // (A - 2.0 * B) / theta2 or -1/12 for theta->0 double E; // (B - 3.0 * C) / theta2 or -1/60 for theta->0 - public: /// Constructor with element of Lie algebra so(3) explicit DexpFunctor(const Vector3& omega, bool nearZeroApprox = false); // NOTE(luca): Right Jacobian for Exponential map in SO(3) - equation // (10.86) and following equations in G.S. Chirikjian, "Stochastic Models, // Information Theory, and Lie Groups", Volume 2, 2008. - // expmap(omega + v) \approx expmap(omega) * expmap(dexp * v) - // This maps a perturbation v in the tangent space to - // a perturbation on the manifold Expmap(dexp * v) + // Expmap(xi + dxi) \approx Expmap(xi) * Expmap(dexp * dxi) + // This maps a perturbation dxi=(w,v) in the tangent space to + // a perturbation on the manifold Expmap(dexp * xi) Matrix3 rightJacobian() const { return I_3x3 - B * W + C * WW; } // Compute the left Jacobian for Exponential map in SO(3) Matrix3 leftJacobian() const { return I_3x3 + B * W + C * WW; } - /// differential of expmap == right Jacobian + /// Differential of expmap == right Jacobian inline Matrix3 dexp() const { return rightJacobian(); } /// Computes B * (omega x v). @@ -199,7 +197,6 @@ class GTSAM_EXPORT DexpFunctor : public ExpmapFunctor { Vector3 applyLeftJacobian(const Vector3& v, OptionalJacobian<3, 3> H1 = {}, OptionalJacobian<3, 3> H2 = {}) const; - protected: static constexpr double one_sixth = 1.0 / 6.0; static constexpr double _one_twelfth = -1.0 / 12.0; static constexpr double _one_sixtieth = -1.0 / 60.0; From 37f6de744df609a6cbc6f347d9d82bd690cf6358 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 16 Dec 2024 10:28:55 -0500 Subject: [PATCH 47/80] use c++11 method for string to int --- gtsam/discrete/TableFactor.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/discrete/TableFactor.cpp b/gtsam/discrete/TableFactor.cpp index 63e9e5d6b..3cfae56ec 100644 --- a/gtsam/discrete/TableFactor.cpp +++ b/gtsam/discrete/TableFactor.cpp @@ -260,7 +260,7 @@ DecisionTreeFactor TableFactor::toDecisionTreeFactor() const { ss << keyValueForIndex(key, i); } // k will be in reverse key order already - uint64_t k = std::strtoull(ss.str().c_str(), NULL, 10); + uint64_t k = std::stoll(ss.str().c_str()); pair_table.push_back(std::make_pair(k, sparse_table_.coeff(i))); } From 76c953784704c084ffba34e4d3fdc233a67b2ba4 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Mon, 16 Dec 2024 12:48:34 -0500 Subject: [PATCH 48/80] inverse Jacobians --- gtsam/geometry/SO3.cpp | 32 +++++++++++++++++++++----------- gtsam/geometry/SO3.h | 25 ++++++++++++++++++++----- gtsam/geometry/tests/testSO3.cpp | 19 ++++++++++++++++++- 3 files changed, 59 insertions(+), 17 deletions(-) diff --git a/gtsam/geometry/SO3.cpp b/gtsam/geometry/SO3.cpp index c54306ae5..6499d4139 100644 --- a/gtsam/geometry/SO3.cpp +++ b/gtsam/geometry/SO3.cpp @@ -87,19 +87,29 @@ SO3 ExpmapFunctor::expmap() const { return SO3(I_3x3 + A * W + B * WW); } DexpFunctor::DexpFunctor(const Vector3& omega, bool nearZeroApprox) : ExpmapFunctor(omega, nearZeroApprox), omega(omega) { - C = nearZero ? one_sixth : (1 - A) / theta2; - D = nearZero ? _one_twelfth : (A - 2.0 * B) / theta2; - E = nearZero ? _one_sixtieth : (B - 3.0 * C) / theta2; + if (!nearZero) { + C = (1 - A) / theta2; + D = (1.0 - A / (2.0 * B)) / theta2; + E = (2.0 * B - A) / theta2; + F = (3.0 * C - B) / theta2; + } else { + // Limit as theta -> 0 + // TODO(Frank): flipping signs here does not trigger any tests: harden! + C = one_sixth; + D = one_twelfth; + E = one_twelfth; + F = one_sixtieth; + } } Vector3 DexpFunctor::crossB(const Vector3& v, OptionalJacobian<3, 3> H) const { // Wv = omega x v const Vector3 Wv = gtsam::cross(omega, v); if (H) { - // Apply product rule: - // D * omega.transpose() is 1x3 Jacobian of B with respect to omega - // - skewSymmetric(v) is 3x3 Jacobian of B gtsam::cross(omega, v) - *H = Wv * D * omega.transpose() - B * skewSymmetric(v); + // Apply product rule to (B Wv) + // - E * omega.transpose() is 1x3 Jacobian of B with respect to omega + // - skewSymmetric(v) is 3x3 Jacobian of Wv = gtsam::cross(omega, v) + *H = - Wv * E * omega.transpose() - B * skewSymmetric(v); } return B * Wv; } @@ -111,10 +121,10 @@ Vector3 DexpFunctor::doubleCrossC(const Vector3& v, const Vector3 WWv = gtsam::doubleCross(omega, v, H ? &doubleCrossJacobian : nullptr); if (H) { - // Apply product rule: - // E * omega.transpose() is 1x3 Jacobian of C with respect to omega - // doubleCrossJacobian is 3x3 Jacobian of C gtsam::doubleCross(omega, v) - *H = WWv * E * omega.transpose() + C * doubleCrossJacobian; + // Apply product rule to (C WWv) + // - F * omega.transpose() is 1x3 Jacobian of C with respect to omega + // doubleCrossJacobian is 3x3 Jacobian of WWv = gtsam::doubleCross(omega, v) + *H = - WWv * F * omega.transpose() + C * doubleCrossJacobian; } return C * WWv; } diff --git a/gtsam/geometry/SO3.h b/gtsam/geometry/SO3.h index f831aa426..488dea12e 100644 --- a/gtsam/geometry/SO3.h +++ b/gtsam/geometry/SO3.h @@ -157,10 +157,16 @@ struct GTSAM_EXPORT ExpmapFunctor { /// Functor that implements Exponential map *and* its derivatives struct GTSAM_EXPORT DexpFunctor : public ExpmapFunctor { const Vector3 omega; - double C; // Ethan's C constant: (1 - A) / theta^2 or 1/6 for theta->0 + + // Ethan's C constant used in Jacobians + double C; // (1 - A) / theta^2 or 1/6 for theta->0 + + // Constant used in inverse Jacobians + double D; // 1.0 - A / (2.0 * B)) / theta2 or 1/12 for theta->0 + // Constants used in cross and doubleCross - double D; // (A - 2.0 * B) / theta2 or -1/12 for theta->0 - double E; // (B - 3.0 * C) / theta2 or -1/60 for theta->0 + double E; // (A - 2.0 * B) / theta2 or -1/12 for theta->0 + double F; // (B - 3.0 * C) / theta2 or -1/60 for theta->0 /// Constructor with element of Lie algebra so(3) explicit DexpFunctor(const Vector3& omega, bool nearZeroApprox = false); @@ -179,6 +185,15 @@ struct GTSAM_EXPORT DexpFunctor : public ExpmapFunctor { /// Differential of expmap == right Jacobian inline Matrix3 dexp() const { return rightJacobian(); } + /// Inverse of right Jacobian + Matrix3 rightJacobianInverse() const { return I_3x3 + 0.5 * W + D * WW; } + + // Inverse of left Jacobian + Matrix3 leftJacobianInverse() const { return I_3x3 - 0.5 * W + D * WW; } + + /// Synonym for rightJacobianInverse + inline Matrix3 invDexp() const { return rightJacobianInverse(); } + /// Computes B * (omega x v). Vector3 crossB(const Vector3& v, OptionalJacobian<3, 3> H = {}) const; @@ -198,8 +213,8 @@ struct GTSAM_EXPORT DexpFunctor : public ExpmapFunctor { OptionalJacobian<3, 3> H2 = {}) const; static constexpr double one_sixth = 1.0 / 6.0; - static constexpr double _one_twelfth = -1.0 / 12.0; - static constexpr double _one_sixtieth = -1.0 / 60.0; + static constexpr double one_twelfth = 1.0 / 12.0; + static constexpr double one_sixtieth = 1.0 / 60.0; }; } // namespace so3 diff --git a/gtsam/geometry/tests/testSO3.cpp b/gtsam/geometry/tests/testSO3.cpp index 3ecabe84b..96872eae9 100644 --- a/gtsam/geometry/tests/testSO3.cpp +++ b/gtsam/geometry/tests/testSO3.cpp @@ -19,6 +19,7 @@ #include #include #include +#include using namespace std::placeholders; using namespace std; @@ -304,13 +305,29 @@ TEST(SO3, JacobianLogmap) { } namespace test_cases { -std::vector small{{0, 0, 0}, {1e-5, 0, 0}, {0, 1e-5, 0}, {0, 0, 1e-5}}; +std::vector small{{0, 0, 0}, // + {1e-5, 0, 0}, {0, 1e-5, 0}, {0, 0, 1e-5}, //, + {1e-4, 0, 0}, {0, 1e-4, 0}, {0, 0, 1e-4}}; std::vector large{ {0, 0, 0}, {1, 0, 0}, {0, 1, 0}, {0, 0, 1}, {0.1, 0.2, 0.3}}; auto omegas = [](bool nearZero) { return nearZero ? small : large; }; std::vector vs{{1, 0, 0}, {0, 1, 0}, {0, 0, 1}, {0.4, 0.3, 0.2}}; } // namespace test_cases +//****************************************************************************** +TEST(SO3, JacobianInverses) { + Matrix HR, HL; + for (bool nearZero : {true, false}) { + for (const Vector3& omega : test_cases::omegas(nearZero)) { + so3::DexpFunctor local(omega, nearZero); + EXPECT(assert_equal(local.rightJacobian().inverse(), + local.rightJacobianInverse())); + EXPECT(assert_equal(local.leftJacobian().inverse(), + local.leftJacobianInverse())); + } + } +} + //****************************************************************************** TEST(SO3, CrossB) { Matrix aH1; From 6c84a2b539c144ed06c9a3aec5fa2ba697a29b5d Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Mon, 16 Dec 2024 12:48:52 -0500 Subject: [PATCH 49/80] Use X to map left to right --- gtsam/geometry/Pose3.cpp | 20 +++++++------------- 1 file changed, 7 insertions(+), 13 deletions(-) diff --git a/gtsam/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp index 5f0e8e3ce..cb8de111e 100644 --- a/gtsam/geometry/Pose3.cpp +++ b/gtsam/geometry/Pose3.cpp @@ -22,6 +22,7 @@ #include #include #include +#include "gtsam/geometry/Rot3.h" namespace gtsam { @@ -241,25 +242,18 @@ Vector6 Pose3::ChartAtOrigin::Local(const Pose3& pose, ChartJacobian Hpose) { /* ************************************************************************* */ namespace pose3 { struct GTSAM_EXPORT ExpmapFunctor : public so3::DexpFunctor { - // Constant used in computeQ - double F; // (B - 0.5) / theta2 or -1/24 for theta->0 - ExpmapFunctor(const Vector3& omega, bool nearZeroApprox = false) : so3::DexpFunctor(omega, nearZeroApprox) { - F = nearZero ? _one_twenty_fourth : (B - 0.5) / theta2; } - // Compute the bottom-left 3x3 block of the SE(3) Expmap derivative - // TODO(Frank): t = applyLeftJacobian(v), it would be nice to understand - // how to compute mess below from applyLeftJacobian derivatives in w and v. + // Compute the bottom-left 3x3 block of the SE(3) Expmap derivative. Matrix3 computeQ(const Vector3& v) const { - const Matrix3 V = skewSymmetric(v); - const Matrix3 WVW = W * V * W; - return -0.5 * V + C * (W * V + V * W - WVW) + - F * (WW * V + V * WW - 3 * WVW) - 0.5 * E * (WVW * W + W * WVW); + // X translate from left to right for our right expmap convention: + Matrix X = rightJacobian() * leftJacobianInverse(); + Matrix3 H; + applyLeftJacobian(v, H); + return X * H; } - - static constexpr double _one_twenty_fourth = - 1.0 / 24.0; }; } // namespace pose3 From 98697251bd9d8558258493f18c266c032b2c1dc3 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Mon, 16 Dec 2024 12:57:27 -0500 Subject: [PATCH 50/80] applyLeftJacobianInverse --- gtsam/geometry/SO3.cpp | 26 ++++++++++++++++++------ gtsam/geometry/SO3.h | 7 ++++++- gtsam/geometry/tests/testSO3.cpp | 34 ++++++++++++++++++++++++++------ 3 files changed, 54 insertions(+), 13 deletions(-) diff --git a/gtsam/geometry/SO3.cpp b/gtsam/geometry/SO3.cpp index 6499d4139..93bf072c5 100644 --- a/gtsam/geometry/SO3.cpp +++ b/gtsam/geometry/SO3.cpp @@ -142,14 +142,14 @@ Vector3 DexpFunctor::applyDexp(const Vector3& v, OptionalJacobian<3, 3> H1, Vector3 DexpFunctor::applyInvDexp(const Vector3& v, OptionalJacobian<3, 3> H1, OptionalJacobian<3, 3> H2) const { - const Matrix3 invDexp = rightJacobian().inverse(); - const Vector3 c = invDexp * v; + const Matrix3 invJr = rightJacobianInverse(); + const Vector3 c = invJr * v; if (H1) { - Matrix3 D_dexpv_w; - applyDexp(c, D_dexpv_w); // get derivative H of forward mapping - *H1 = -invDexp * D_dexpv_w; + Matrix3 H; + applyDexp(c, H); // get derivative H of forward mapping + *H1 = -invJr * H; } - if (H2) *H2 = invDexp; + if (H2) *H2 = invJr; return c; } @@ -164,6 +164,20 @@ Vector3 DexpFunctor::applyLeftJacobian(const Vector3& v, return v + BWv + CWWv; } +Vector3 DexpFunctor::applyLeftJacobianInverse(const Vector3& v, + OptionalJacobian<3, 3> H1, + OptionalJacobian<3, 3> H2) const { + const Matrix3 invJl = leftJacobianInverse(); + const Vector3 c = invJl * v; + if (H1) { + Matrix3 H; + applyLeftJacobian(c, H); // get derivative H of forward mapping + *H1 = -invJl * H; + } + if (H2) *H2 = invJl; + return c; +} + } // namespace so3 //****************************************************************************** diff --git a/gtsam/geometry/SO3.h b/gtsam/geometry/SO3.h index 488dea12e..2845fcce7 100644 --- a/gtsam/geometry/SO3.h +++ b/gtsam/geometry/SO3.h @@ -162,7 +162,7 @@ struct GTSAM_EXPORT DexpFunctor : public ExpmapFunctor { double C; // (1 - A) / theta^2 or 1/6 for theta->0 // Constant used in inverse Jacobians - double D; // 1.0 - A / (2.0 * B)) / theta2 or 1/12 for theta->0 + double D; // (1 - A/2B) / theta2 or 1/12 for theta->0 // Constants used in cross and doubleCross double E; // (A - 2.0 * B) / theta2 or -1/12 for theta->0 @@ -212,6 +212,11 @@ struct GTSAM_EXPORT DexpFunctor : public ExpmapFunctor { Vector3 applyLeftJacobian(const Vector3& v, OptionalJacobian<3, 3> H1 = {}, OptionalJacobian<3, 3> H2 = {}) const; + /// Multiplies with leftJacobianInverse(), with optional derivatives + Vector3 applyLeftJacobianInverse(const Vector3& v, + OptionalJacobian<3, 3> H1 = {}, + OptionalJacobian<3, 3> H2 = {}) const; + static constexpr double one_sixth = 1.0 / 6.0; static constexpr double one_twelfth = 1.0 / 12.0; static constexpr double one_sixtieth = 1.0 / 60.0; diff --git a/gtsam/geometry/tests/testSO3.cpp b/gtsam/geometry/tests/testSO3.cpp index 96872eae9..d3313b7f1 100644 --- a/gtsam/geometry/tests/testSO3.cpp +++ b/gtsam/geometry/tests/testSO3.cpp @@ -385,6 +385,28 @@ TEST(SO3, ApplyDexp) { } } +//****************************************************************************** +TEST(SO3, ApplyInvDexp) { + Matrix aH1, aH2; + for (bool nearZero : {true, false}) { + std::function f = + [=](const Vector3& omega, const Vector3& v) { + return so3::DexpFunctor(omega, nearZero).applyInvDexp(v); + }; + for (const Vector3& omega : test_cases::omegas(nearZero)) { + so3::DexpFunctor local(omega, nearZero); + Matrix invJr = local.rightJacobianInverse(); + for (const Vector3& v : test_cases::vs) { + EXPECT( + assert_equal(Vector3(invJr * v), local.applyInvDexp(v, aH1, aH2))); + EXPECT(assert_equal(numericalDerivative21(f, omega, v), aH1)); + EXPECT(assert_equal(numericalDerivative22(f, omega, v), aH2)); + EXPECT(assert_equal(invJr, aH2)); + } + } + } +} + //****************************************************************************** TEST(SO3, ApplyLeftJacobian) { Matrix aH1, aH2; @@ -407,22 +429,22 @@ TEST(SO3, ApplyLeftJacobian) { } //****************************************************************************** -TEST(SO3, ApplyInvDexp) { +TEST(SO3, ApplyLeftJacobianInverse) { Matrix aH1, aH2; for (bool nearZero : {true, false}) { std::function f = [=](const Vector3& omega, const Vector3& v) { - return so3::DexpFunctor(omega, nearZero).applyInvDexp(v); + return so3::DexpFunctor(omega, nearZero).applyLeftJacobianInverse(v); }; for (const Vector3& omega : test_cases::omegas(nearZero)) { so3::DexpFunctor local(omega, nearZero); - Matrix invDexp = local.dexp().inverse(); + Matrix invJl = local.leftJacobianInverse(); for (const Vector3& v : test_cases::vs) { - EXPECT(assert_equal(Vector3(invDexp * v), - local.applyInvDexp(v, aH1, aH2))); + EXPECT(assert_equal(Vector3(invJl * v), + local.applyLeftJacobianInverse(v, aH1, aH2))); EXPECT(assert_equal(numericalDerivative21(f, omega, v), aH1)); EXPECT(assert_equal(numericalDerivative22(f, omega, v), aH2)); - EXPECT(assert_equal(invDexp, aH2)); + EXPECT(assert_equal(invJl, aH2)); } } } From b11387167d9d8988e6a35c920518863c9d14fe44 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Mon, 16 Dec 2024 13:28:19 -0500 Subject: [PATCH 51/80] No more functor for Q --- gtsam/geometry/Pose3.cpp | 59 +++++++++++++----------------- gtsam/geometry/tests/testPose3.cpp | 11 ------ 2 files changed, 26 insertions(+), 44 deletions(-) diff --git a/gtsam/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp index cb8de111e..9f81204d6 100644 --- a/gtsam/geometry/Pose3.cpp +++ b/gtsam/geometry/Pose3.cpp @@ -22,6 +22,7 @@ #include #include #include + #include "gtsam/geometry/Rot3.h" namespace gtsam { @@ -112,7 +113,7 @@ Matrix6 Pose3::adjointMap(const Vector6& xi) { /* ************************************************************************* */ Vector6 Pose3::adjoint(const Vector6& xi, const Vector6& y, - OptionalJacobian<6, 6> Hxi, OptionalJacobian<6, 6> H_y) { + OptionalJacobian<6, 6> Hxi, OptionalJacobian<6, 6> H_y) { if (Hxi) { Hxi->setZero(); for (int i = 0; i < 6; ++i) { @@ -159,7 +160,7 @@ bool Pose3::equals(const Pose3& pose, double tol) const { /* ************************************************************************* */ Pose3 Pose3::interpolateRt(const Pose3& T, double t) const { return Pose3(interpolate(R_, T.R_, t), - interpolate(t_, T.t_, t)); + interpolate(t_, T.t_, t)); } /* ************************************************************************* */ @@ -239,30 +240,14 @@ Vector6 Pose3::ChartAtOrigin::Local(const Pose3& pose, ChartJacobian Hpose) { #endif } -/* ************************************************************************* */ -namespace pose3 { -struct GTSAM_EXPORT ExpmapFunctor : public so3::DexpFunctor { - ExpmapFunctor(const Vector3& omega, bool nearZeroApprox = false) - : so3::DexpFunctor(omega, nearZeroApprox) { - } - - // Compute the bottom-left 3x3 block of the SE(3) Expmap derivative. - Matrix3 computeQ(const Vector3& v) const { - // X translate from left to right for our right expmap convention: - Matrix X = rightJacobian() * leftJacobianInverse(); - Matrix3 H; - applyLeftJacobian(v, H); - return X * H; - } -}; -} // namespace pose3 - /* ************************************************************************* */ Matrix3 Pose3::ComputeQforExpmapDerivative(const Vector6& xi, double nearZeroThreshold) { const auto w = xi.head<3>(); const auto v = xi.tail<3>(); - return pose3::ExpmapFunctor(w).computeQ(v); + Matrix3 Q; + ExpmapTranslation(w, v, Q, {}, nearZeroThreshold); + return Q; } /* ************************************************************************* */ @@ -273,13 +258,22 @@ Vector3 Pose3::ExpmapTranslation(const Vector3& w, const Vector3& v, const double theta2 = w.dot(w); bool nearZero = (theta2 <= nearZeroThreshold); - if (Q) *Q = pose3::ExpmapFunctor(w, nearZero).computeQ(v); - - if (nearZero) { - return v + 0.5 * w.cross(v); + if (Q) { + // Instantiate functor for Dexp-related operations: + so3::DexpFunctor local(w, nearZero); + // X translate from left to right for our right expmap convention: + Matrix3 X = local.rightJacobian() * local.leftJacobianInverse(); + Matrix3 H; + Vector t = local.applyLeftJacobian(v, H); + *Q = X * H; + return t; + } else if (nearZero) { + // (I_3x3 + B * W + C * WW) * v with B->0.5, C->1/6 + Vector3 Wv = w.cross(v); + return v + 0.5 * Wv + w.cross(Wv) * so3::DexpFunctor::one_sixth; } else { - // NOTE(Frank): t can also be computed by calling applyLeftJacobian(v), but - // formulas below convey geometric insight and creating functor is not free. + // NOTE(Frank): if Q is not asked we use formulas below instead of calling + // applyLeftJacobian(v), as they convey geometric insight and are faster. Vector3 t_parallel = w * w.dot(v); // translation parallel to axis Vector3 w_cross_v = w.cross(v); // translation orthogonal to axis Rot3 rotation = R.value_or(Rot3::Expmap(w)); @@ -314,7 +308,6 @@ const Point3& Pose3::translation(OptionalJacobian<3, 6> Hself) const { } /* ************************************************************************* */ - const Rot3& Pose3::rotation(OptionalJacobian<3, 6> Hself) const { if (Hself) { *Hself << I_3x3, Z_3x3; @@ -332,14 +325,14 @@ Matrix4 Pose3::matrix() const { /* ************************************************************************* */ Pose3 Pose3::transformPoseFrom(const Pose3& aTb, OptionalJacobian<6, 6> Hself, - OptionalJacobian<6, 6> HaTb) const { + OptionalJacobian<6, 6> HaTb) const { const Pose3& wTa = *this; return wTa.compose(aTb, Hself, HaTb); } /* ************************************************************************* */ Pose3 Pose3::transformPoseTo(const Pose3& wTb, OptionalJacobian<6, 6> Hself, - OptionalJacobian<6, 6> HwTb) const { + OptionalJacobian<6, 6> HwTb) const { if (Hself) *Hself = -wTb.inverse().AdjointMap() * AdjointMap(); if (HwTb) *HwTb = I_6x6; const Pose3& wTa = *this; @@ -348,7 +341,7 @@ Pose3 Pose3::transformPoseTo(const Pose3& wTb, OptionalJacobian<6, 6> Hself, /* ************************************************************************* */ Point3 Pose3::transformFrom(const Point3& point, OptionalJacobian<3, 6> Hself, - OptionalJacobian<3, 3> Hpoint) const { + OptionalJacobian<3, 3> Hpoint) const { // Only get matrix once, to avoid multiple allocations, // as well as multiple conversions in the Quaternion case const Matrix3 R = R_.matrix(); @@ -372,7 +365,7 @@ Matrix Pose3::transformFrom(const Matrix& points) const { /* ************************************************************************* */ Point3 Pose3::transformTo(const Point3& point, OptionalJacobian<3, 6> Hself, - OptionalJacobian<3, 3> Hpoint) const { + OptionalJacobian<3, 3> Hpoint) const { // Only get transpose once, to avoid multiple allocations, // as well as multiple conversions in the Quaternion case const Matrix3 Rt = R_.transpose(); @@ -478,7 +471,7 @@ std::optional Pose3::Align(const Point3Pairs &abPointPairs) { std::optional Pose3::Align(const Matrix& a, const Matrix& b) { if (a.rows() != 3 || b.rows() != 3 || a.cols() != b.cols()) { throw std::invalid_argument( - "Pose3:Align expects 3*N matrices of equal shape."); + "Pose3:Align expects 3*N matrices of equal shape."); } Point3Pairs abPointPairs; for (Eigen::Index j = 0; j < a.cols(); j++) { diff --git a/gtsam/geometry/tests/testPose3.cpp b/gtsam/geometry/tests/testPose3.cpp index 4ff4f9a85..3b3de6792 100644 --- a/gtsam/geometry/tests/testPose3.cpp +++ b/gtsam/geometry/tests/testPose3.cpp @@ -900,17 +900,6 @@ TEST(Pose3, ExpmapDerivative4) { } } -TEST( Pose3, ExpmapDerivativeQr) { - Vector6 w = Vector6::Random(); - w.head<3>().normalize(); - w.head<3>() = w.head<3>() * 0.9e-2; - Matrix3 actualQr = Pose3::ComputeQforExpmapDerivative(w, 0.01); - Matrix expectedH = numericalDerivative21 >(&Pose3::Expmap, w, {}); - Matrix3 expectedQr = expectedH.bottomLeftCorner<3, 3>(); - EXPECT(assert_equal(expectedQr, actualQr, 1e-6)); -} - /* ************************************************************************* */ TEST( Pose3, LogmapDerivative) { Matrix6 actualH; From 2aa36d4f7ab89e59138fbaa2c9914643069b77ef Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Mon, 16 Dec 2024 14:08:02 -0500 Subject: [PATCH 52/80] Modernize ExmapTranslation --- gtsam/geometry/Pose3.cpp | 56 +++++++++++++++++--------------- gtsam/geometry/Pose3.h | 10 +++--- gtsam/geometry/tests/testSO3.cpp | 1 + gtsam/navigation/NavState.cpp | 27 ++++++++------- 4 files changed, 50 insertions(+), 44 deletions(-) diff --git a/gtsam/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp index 9f81204d6..6fbd2468f 100644 --- a/gtsam/geometry/Pose3.cpp +++ b/gtsam/geometry/Pose3.cpp @@ -164,22 +164,22 @@ Pose3 Pose3::interpolateRt(const Pose3& T, double t) const { } /* ************************************************************************* */ -/** Modified from Murray94book version (which assumes w and v normalized?) */ Pose3 Pose3::Expmap(const Vector6& xi, OptionalJacobian<6, 6> Hxi) { // Get angular velocity omega and translational velocity v from twist xi const Vector3 w = xi.head<3>(), v = xi.tail<3>(); // Compute rotation using Expmap - Matrix3 Jw; - Rot3 R = Rot3::Expmap(w, Hxi ? &Jw : nullptr); + Matrix3 Jr; + Rot3 R = Rot3::Expmap(w, Hxi ? &Jr : nullptr); - // Compute translation and optionally its Jacobian in w + // Compute translation and optionally its Jacobian Q in w + // The Jacobian in v is the right Jacobian Jr of SO(3), which we already have. Matrix3 Q; - const Vector3 t = ExpmapTranslation(w, v, Hxi ? &Q : nullptr, R); + const Vector3 t = ExpmapTranslation(w, v, Hxi ? &Q : nullptr); if (Hxi) { - *Hxi << Jw, Z_3x3, - Q, Jw; + *Hxi << Jr, Z_3x3, // + Q, Jr; } return Pose3(R, t); @@ -251,35 +251,39 @@ Matrix3 Pose3::ComputeQforExpmapDerivative(const Vector6& xi, } /* ************************************************************************* */ +// NOTE(Frank): t = applyLeftJacobian(v) does the same as the intuitive formulas +// t_parallel = w * w.dot(v); // translation parallel to axis +// w_cross_v = w.cross(v); // translation orthogonal to axis +// t = (w_cross_v - Rot3::Expmap(w) * w_cross_v + t_parallel) / theta2; +// but functor does not need R, deals automatically with the case where theta2 +// is near zero, and also gives us the machinery for the Jacobians. Vector3 Pose3::ExpmapTranslation(const Vector3& w, const Vector3& v, OptionalJacobian<3, 3> Q, - const std::optional& R, + OptionalJacobian<3, 3> J, double nearZeroThreshold) { const double theta2 = w.dot(w); bool nearZero = (theta2 <= nearZeroThreshold); + // Instantiate functor for Dexp-related operations: + so3::DexpFunctor local(w, nearZero); + + // Call applyLeftJacobian which is faster than local.leftJacobian() * v if you + // don't need Jacobians, and returns Jacobian of t with respect to w if asked. + Matrix3 H; + Vector t = local.applyLeftJacobian(v, Q ? &H : nullptr); + + // We return Jacobians for use in Expmap, so we multiply with X, that + // translates from left to right for our right expmap convention: if (Q) { - // Instantiate functor for Dexp-related operations: - so3::DexpFunctor local(w, nearZero); - // X translate from left to right for our right expmap convention: Matrix3 X = local.rightJacobian() * local.leftJacobianInverse(); - Matrix3 H; - Vector t = local.applyLeftJacobian(v, H); *Q = X * H; - return t; - } else if (nearZero) { - // (I_3x3 + B * W + C * WW) * v with B->0.5, C->1/6 - Vector3 Wv = w.cross(v); - return v + 0.5 * Wv + w.cross(Wv) * so3::DexpFunctor::one_sixth; - } else { - // NOTE(Frank): if Q is not asked we use formulas below instead of calling - // applyLeftJacobian(v), as they convey geometric insight and are faster. - Vector3 t_parallel = w * w.dot(v); // translation parallel to axis - Vector3 w_cross_v = w.cross(v); // translation orthogonal to axis - Rot3 rotation = R.value_or(Rot3::Expmap(w)); - Vector3 t = (w_cross_v - rotation * w_cross_v + t_parallel) / theta2; - return t; } + + if (J) { + *J = local.rightJacobian(); // = X * local.leftJacobian(); + } + + return t; } /* ************************************************************************* */ diff --git a/gtsam/geometry/Pose3.h b/gtsam/geometry/Pose3.h index d7c280c80..04504de65 100644 --- a/gtsam/geometry/Pose3.h +++ b/gtsam/geometry/Pose3.h @@ -223,17 +223,19 @@ public: const Vector6& xi, double nearZeroThreshold = 1e-5); /** - * Compute the translation part of the exponential map, with derivative. + * Compute the translation part of the exponential map, with Jacobians. * @param w 3D angular velocity * @param v 3D velocity * @param Q Optionally, compute 3x3 Jacobian wrpt w - * @param R Optionally, precomputed as Rot3::Expmap(w) + * @param J Optionally, compute 3x3 Jacobian wrpt v = right Jacobian of SO(3) * @param nearZeroThreshold threshold for small values - * Note Q is 3x3 bottom-left block of SE3 Expmap right derivative matrix + * @note This function returns Jacobians Q and J corresponding to the bottom + * blocks of the SE(3) exponential, and translated from left to right from the + * applyLeftJacobian Jacobians. */ static Vector3 ExpmapTranslation(const Vector3& w, const Vector3& v, OptionalJacobian<3, 3> Q = {}, - const std::optional& R = {}, + OptionalJacobian<3, 3> J = {}, double nearZeroThreshold = 1e-5); using LieGroup::inverse; // version with derivative diff --git a/gtsam/geometry/tests/testSO3.cpp b/gtsam/geometry/tests/testSO3.cpp index d3313b7f1..200642456 100644 --- a/gtsam/geometry/tests/testSO3.cpp +++ b/gtsam/geometry/tests/testSO3.cpp @@ -304,6 +304,7 @@ TEST(SO3, JacobianLogmap) { EXPECT(assert_equal(Jexpected, Jactual)); } +//****************************************************************************** namespace test_cases { std::vector small{{0, 0, 0}, // {1e-5, 0, 0}, {0, 1e-5, 0}, {0, 0, 1e-5}, //, diff --git a/gtsam/navigation/NavState.cpp b/gtsam/navigation/NavState.cpp index b9d48a3cb..c9cd244d3 100644 --- a/gtsam/navigation/NavState.cpp +++ b/gtsam/navigation/NavState.cpp @@ -114,24 +114,23 @@ NavState NavState::inverse() const { //------------------------------------------------------------------------------ NavState NavState::Expmap(const Vector9& xi, OptionalJacobian<9, 9> Hxi) { - // Get angular velocity w, translational velocity v, and acceleration a - Vector3 w = xi.head<3>(); - Vector3 rho = xi.segment<3>(3); - Vector3 nu = xi.tail<3>(); + // Get angular velocity w and components rho and nu from xi + Vector3 w = xi.head<3>(), rho = xi.segment<3>(3), nu = xi.tail<3>(); // Compute rotation using Expmap - Rot3 R = Rot3::Expmap(w); + Matrix3 Jr; + Rot3 R = Rot3::Expmap(w, Hxi ? &Jr : nullptr); - // Compute translations and optionally their Jacobians + // Compute translations and optionally their Jacobians Q in w + // The Jacobians with respect to rho and nu are equal to Jr Matrix3 Qt, Qv; - Vector3 t = Pose3::ExpmapTranslation(w, rho, Hxi ? &Qt : nullptr, R); - Vector3 v = Pose3::ExpmapTranslation(w, nu, Hxi ? &Qv : nullptr, R); + Vector3 t = Pose3::ExpmapTranslation(w, rho, Hxi ? &Qt : nullptr); + Vector3 v = Pose3::ExpmapTranslation(w, nu, Hxi ? &Qv : nullptr); if (Hxi) { - const Matrix3 Jw = Rot3::ExpmapDerivative(w); - *Hxi << Jw, Z_3x3, Z_3x3, - Qt, Jw, Z_3x3, - Qv, Z_3x3, Jw; + *Hxi << Jr, Z_3x3, Z_3x3, // + Qt, Jr, Z_3x3, // + Qv, Z_3x3, Jr; } return NavState(R, t, v); @@ -235,8 +234,8 @@ Matrix9 NavState::LogmapDerivative(const NavState& state) { Matrix3 Qt, Qv; const Rot3 R = Rot3::Expmap(w); - Pose3::ExpmapTranslation(w, rho, Qt, R); - Pose3::ExpmapTranslation(w, nu, Qv, R); + Pose3::ExpmapTranslation(w, rho, Qt); + Pose3::ExpmapTranslation(w, nu, Qv); const Matrix3 Jw = Rot3::LogmapDerivative(w); const Matrix3 Qt2 = -Jw * Qt * Jw; const Matrix3 Qv2 = -Jw * Qv * Jw; From df0f597ed763d028b170690846f6eddcecf0b46f Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 16 Dec 2024 14:28:27 -0500 Subject: [PATCH 53/80] debug conversion --- gtsam/discrete/TableFactor.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/gtsam/discrete/TableFactor.cpp b/gtsam/discrete/TableFactor.cpp index 3cfae56ec..cb60a9818 100644 --- a/gtsam/discrete/TableFactor.cpp +++ b/gtsam/discrete/TableFactor.cpp @@ -260,7 +260,9 @@ DecisionTreeFactor TableFactor::toDecisionTreeFactor() const { ss << keyValueForIndex(key, i); } // k will be in reverse key order already - uint64_t k = std::stoll(ss.str().c_str()); + uint64_t k; + ss >> k; + std::cout << "ss: " << ss.str() << ", k=" << k << std::endl; pair_table.push_back(std::make_pair(k, sparse_table_.coeff(i))); } From 17cae8c45336cdb62bf2bfcbc7c6fe6f7cdc1d39 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 16 Dec 2024 15:22:25 -0500 Subject: [PATCH 54/80] print more to debug --- gtsam/discrete/TableFactor.cpp | 9 +++++++-- 1 file changed, 7 insertions(+), 2 deletions(-) diff --git a/gtsam/discrete/TableFactor.cpp b/gtsam/discrete/TableFactor.cpp index cb60a9818..38b4ddd30 100644 --- a/gtsam/discrete/TableFactor.cpp +++ b/gtsam/discrete/TableFactor.cpp @@ -262,16 +262,21 @@ DecisionTreeFactor TableFactor::toDecisionTreeFactor() const { // k will be in reverse key order already uint64_t k; ss >> k; - std::cout << "ss: " << ss.str() << ", k=" << k << std::endl; + std::cout << "ss: " << ss.str() << ", k=" << k + << ", v=" << sparse_table_.coeff(i) << std::endl; pair_table.push_back(std::make_pair(k, sparse_table_.coeff(i))); } - // Sort based on key so we get values in reverse key order. + // Sort based on key assignment so we get values in reverse key order. std::sort( pair_table.begin(), pair_table.end(), [](const std::pair& a, const std::pair& b) { return a.first <= b.first; }); + std::cout << "Sorted pair_table:" << std::endl; + for (auto&& [k, v] : pair_table) { + std::cout << "k=" << k << ", v=" << v << std::endl; + } // Create the table vector std::vector table; std::for_each(pair_table.begin(), pair_table.end(), From 72306efe9830f24ed741db8b614d0c665ccf9452 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 16 Dec 2024 18:20:16 -0500 Subject: [PATCH 55/80] strict less than check --- gtsam/discrete/TableFactor.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/discrete/TableFactor.cpp b/gtsam/discrete/TableFactor.cpp index 38b4ddd30..0a93e7b5d 100644 --- a/gtsam/discrete/TableFactor.cpp +++ b/gtsam/discrete/TableFactor.cpp @@ -271,7 +271,7 @@ DecisionTreeFactor TableFactor::toDecisionTreeFactor() const { std::sort( pair_table.begin(), pair_table.end(), [](const std::pair& a, - const std::pair& b) { return a.first <= b.first; }); + const std::pair& b) { return a.first < b.first; }); std::cout << "Sorted pair_table:" << std::endl; for (auto&& [k, v] : pair_table) { From db5b9dee65442245a8de1708fb1a21d72ee16e55 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Mon, 16 Dec 2024 18:35:17 -0500 Subject: [PATCH 56/80] Taylor expansion --- gtsam/geometry/SO3.cpp | 25 ++++--- gtsam/geometry/SO3.h | 16 ++--- gtsam/geometry/tests/testPose3.cpp | 101 +++++++++++++++++------------ 3 files changed, 83 insertions(+), 59 deletions(-) diff --git a/gtsam/geometry/SO3.cpp b/gtsam/geometry/SO3.cpp index 93bf072c5..41f7ce810 100644 --- a/gtsam/geometry/SO3.cpp +++ b/gtsam/geometry/SO3.cpp @@ -33,6 +33,15 @@ namespace gtsam { //****************************************************************************** namespace so3 { +static constexpr double one_6th = 1.0 / 6.0; +static constexpr double one_12th = 1.0 / 12.0; +static constexpr double one_24th = 1.0 / 24.0; +static constexpr double one_60th = 1.0 / 60.0; +static constexpr double one_120th = 1.0 / 120.0; +static constexpr double one_180th = 1.0 / 180.0; +static constexpr double one_720th = 1.0 / 720.0; +static constexpr double one_1260th = 1.0 / 1260.0; + GTSAM_EXPORT Matrix99 Dcompose(const SO3& Q) { Matrix99 H; auto R = Q.matrix(); @@ -60,9 +69,9 @@ void ExpmapFunctor::init(bool nearZeroApprox) { 2.0 * s2 * s2; // numerically better than [1 - cos(theta)] B = one_minus_cos / theta2; } else { - // Limits as theta -> 0: - A = 1.0; - B = 0.5; + // Taylor expansion at 0 + A = 1.0 - theta2 * one_6th; + B = 0.5 - theta2 * one_24th; } } @@ -93,12 +102,12 @@ DexpFunctor::DexpFunctor(const Vector3& omega, bool nearZeroApprox) E = (2.0 * B - A) / theta2; F = (3.0 * C - B) / theta2; } else { - // Limit as theta -> 0 + // Taylor expansion at 0 // TODO(Frank): flipping signs here does not trigger any tests: harden! - C = one_sixth; - D = one_twelfth; - E = one_twelfth; - F = one_sixtieth; + C = one_6th - theta2 * one_120th; + D = one_12th + theta2 * one_720th; + E = one_12th - theta2 * one_180th; + F = one_60th - theta2 * one_1260th; } } diff --git a/gtsam/geometry/SO3.h b/gtsam/geometry/SO3.h index 2845fcce7..6e2e38101 100644 --- a/gtsam/geometry/SO3.h +++ b/gtsam/geometry/SO3.h @@ -138,8 +138,8 @@ struct GTSAM_EXPORT ExpmapFunctor { bool nearZero; // Ethan Eade's constants: - double A; // A = sin(theta) / theta or 1 for theta->0 - double B; // B = (1 - cos(theta)) / theta^2 or 0.5 for theta->0 + double A; // A = sin(theta) / theta + double B; // B = (1 - cos(theta)) /// Constructor with element of Lie algebra so(3) explicit ExpmapFunctor(const Vector3& omega, bool nearZeroApprox = false); @@ -159,14 +159,14 @@ struct GTSAM_EXPORT DexpFunctor : public ExpmapFunctor { const Vector3 omega; // Ethan's C constant used in Jacobians - double C; // (1 - A) / theta^2 or 1/6 for theta->0 + double C; // (1 - A) / theta^2 // Constant used in inverse Jacobians - double D; // (1 - A/2B) / theta2 or 1/12 for theta->0 + double D; // (1 - A/2B) / theta2 // Constants used in cross and doubleCross - double E; // (A - 2.0 * B) / theta2 or -1/12 for theta->0 - double F; // (B - 3.0 * C) / theta2 or -1/60 for theta->0 + double E; // (2B - A) / theta2 + double F; // (3C - B) / theta2 /// Constructor with element of Lie algebra so(3) explicit DexpFunctor(const Vector3& omega, bool nearZeroApprox = false); @@ -216,10 +216,6 @@ struct GTSAM_EXPORT DexpFunctor : public ExpmapFunctor { Vector3 applyLeftJacobianInverse(const Vector3& v, OptionalJacobian<3, 3> H1 = {}, OptionalJacobian<3, 3> H2 = {}) const; - - static constexpr double one_sixth = 1.0 / 6.0; - static constexpr double one_twelfth = 1.0 / 12.0; - static constexpr double one_sixtieth = 1.0 / 60.0; }; } // namespace so3 diff --git a/gtsam/geometry/tests/testPose3.cpp b/gtsam/geometry/tests/testPose3.cpp index 3b3de6792..ce74091f0 100644 --- a/gtsam/geometry/tests/testPose3.cpp +++ b/gtsam/geometry/tests/testPose3.cpp @@ -835,38 +835,7 @@ TEST(Pose3, Align2) { } /* ************************************************************************* */ -TEST( Pose3, ExpmapDerivative1) { - Matrix6 actualH; - Vector6 w; w << 0.1, 0.2, 0.3, 4.0, 5.0, 6.0; - Pose3::Expmap(w,actualH); - Matrix expectedH = numericalDerivative21 >(&Pose3::Expmap, w, {}); - EXPECT(assert_equal(expectedH, actualH)); -} - -/* ************************************************************************* */ -TEST( Pose3, ExpmapDerivative2) { - Matrix6 actualH; - Vector6 w; w << 1.0, -2.0, 3.0, -10.0, -20.0, 30.0; - Pose3::Expmap(w,actualH); - Matrix expectedH = numericalDerivative21 >(&Pose3::Expmap, w, {}); - EXPECT(assert_equal(expectedH, actualH)); -} - -/* ************************************************************************* */ -TEST( Pose3, ExpmapDerivative3) { - Matrix6 actualH; - Vector6 w; w << 0.0, 0.0, 0.0, -10.0, -20.0, 30.0; - Pose3::Expmap(w,actualH); - Matrix expectedH = numericalDerivative21 >(&Pose3::Expmap, w, {}); - // Small angle approximation is not as precise as numerical derivative? - EXPECT(assert_equal(expectedH, actualH, 1e-5)); -} - -/* ************************************************************************* */ -TEST(Pose3, ExpmapDerivative4) { +TEST(Pose3, ExpmapDerivative) { // Iserles05an (Lie-group Methods) says: // scalar is easy: d exp(a(t)) / dt = exp(a(t)) a'(t) // matrix is hard: d exp(A(t)) / dt = exp(A(t)) dexp[-A(t)] A'(t) @@ -900,15 +869,65 @@ TEST(Pose3, ExpmapDerivative4) { } } -/* ************************************************************************* */ -TEST( Pose3, LogmapDerivative) { - Matrix6 actualH; - Vector6 w; w << 0.1, 0.2, 0.3, 4.0, 5.0, 6.0; - Pose3 p = Pose3::Expmap(w); - EXPECT(assert_equal(w, Pose3::Logmap(p,actualH), 1e-5)); - Matrix expectedH = numericalDerivative21 >(&Pose3::Logmap, p, {}); - EXPECT(assert_equal(expectedH, actualH)); +//****************************************************************************** +namespace test_cases { +std::vector small{{0, 0, 0}, // + {1e-5, 0, 0}, {0, 1e-5, 0}, {0, 0, 1e-5}, //, + {1e-4, 0, 0}, {0, 1e-4, 0}, {0, 0, 1e-4}}; +std::vector large{{0, 0, 0}, {1, 0, 0}, {0, 1, 0}, + {0, 0, 1}, {.1, .2, .3}, {1, -2, 3}}; +auto omegas = [](bool nearZero) { return nearZero ? small : large; }; +std::vector vs{{1, 0, 0}, {0, 1, 0}, {0, 0, 1}, + {.4, .3, .2}, {4, 5, 6}, {-10, -20, 30}}; +} // namespace test_cases + +//****************************************************************************** +TEST(Pose3, ExpmapDerivatives) { + for (bool nearZero : {true, false}) { + for (const Vector3& w : test_cases::omegas(nearZero)) { + for (Vector3 v : test_cases::vs) { + const Vector6 xi = (Vector6() << w, v).finished(); + const Matrix6 expectedH = + numericalDerivative21 >( + &Pose3::Expmap, xi, {}); + Matrix actualH; + Pose3::Expmap(xi, actualH); + EXPECT(assert_equal(expectedH, actualH)); + } + } + } +} + +//****************************************************************************** +// Check logmap for all small values, as we don't want wrapping. +TEST(Pose3, Logmap) { + static constexpr bool nearZero = true; + for (const Vector3& w : test_cases::omegas(nearZero)) { + for (Vector3 v : test_cases::vs) { + const Vector6 xi = (Vector6() << w, v).finished(); + Pose3 pose = Pose3::Expmap(xi); + EXPECT(assert_equal(xi, Pose3::Logmap(pose), 1e-5)); + } + } +} + +//****************************************************************************** +// Check logmap derivatives for all values +TEST(Pose3, LogmapDerivatives) { + for (bool nearZero : {true, false}) { + for (const Vector3& w : test_cases::omegas(nearZero)) { + for (Vector3 v : test_cases::vs) { + const Vector6 xi = (Vector6() << w, v).finished(); + Pose3 pose = Pose3::Expmap(xi); + const Matrix6 expectedH = + numericalDerivative21 >( + &Pose3::Logmap, pose, {}); + Matrix actualH; + Pose3::Logmap(pose, actualH); + EXPECT(assert_equal(expectedH, actualH)); + } + } + } } /* ************************************************************************* */ From 49c2a040095a4d4d682a2c846f8a79bb8f87584c Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Mon, 16 Dec 2024 18:51:29 -0500 Subject: [PATCH 57/80] Address review comments --- gtsam/geometry/Pose3.cpp | 13 +++++++------ gtsam/geometry/Pose3.h | 2 +- gtsam/geometry/SO3.h | 4 ++++ gtsam/geometry/tests/testSO3.cpp | 1 - gtsam/navigation/NavState.cpp | 2 +- 5 files changed, 13 insertions(+), 9 deletions(-) diff --git a/gtsam/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp index 6fbd2468f..16dda3d1e 100644 --- a/gtsam/geometry/Pose3.cpp +++ b/gtsam/geometry/Pose3.cpp @@ -11,20 +11,19 @@ /** * @file Pose3.cpp - * @brief 3D Pose + * @brief 3D Pose manifold SO(3) x R^3 and group SE(3) */ -#include -#include -#include #include +#include +#include +#include +#include #include #include #include -#include "gtsam/geometry/Rot3.h" - namespace gtsam { /** instantiate concept checks */ @@ -164,6 +163,8 @@ Pose3 Pose3::interpolateRt(const Pose3& T, double t) const { } /* ************************************************************************* */ +// Expmap is implemented in so3::ExpmapFunctor::expmap, based on Ethan Eade's +// elegant Lie group document, at https://www.ethaneade.org/lie.pdf. Pose3 Pose3::Expmap(const Vector6& xi, OptionalJacobian<6, 6> Hxi) { // Get angular velocity omega and translational velocity v from twist xi const Vector3 w = xi.head<3>(), v = xi.tail<3>(); diff --git a/gtsam/geometry/Pose3.h b/gtsam/geometry/Pose3.h index 04504de65..81a9848e2 100644 --- a/gtsam/geometry/Pose3.h +++ b/gtsam/geometry/Pose3.h @@ -11,7 +11,7 @@ /** *@file Pose3.h - *@brief 3D Pose + * @brief 3D Pose manifold SO(3) x R^3 and group SE(3) */ // \callgraph diff --git a/gtsam/geometry/SO3.h b/gtsam/geometry/SO3.h index 6e2e38101..163ae6623 100644 --- a/gtsam/geometry/SO3.h +++ b/gtsam/geometry/SO3.h @@ -132,6 +132,8 @@ GTSAM_EXPORT Matrix99 Dcompose(const SO3& R); // functor also implements dedicated methods to apply dexp and/or inv(dexp). /// Functor implementing Exponential map +/// Math is based on Ethan Eade's elegant Lie group document, at +/// https://www.ethaneade.org/lie.pdf. struct GTSAM_EXPORT ExpmapFunctor { const double theta2, theta; const Matrix3 W, WW; @@ -155,6 +157,8 @@ struct GTSAM_EXPORT ExpmapFunctor { }; /// Functor that implements Exponential map *and* its derivatives +/// Math extends Ethan theme of elegant I + aW + bWW expressions. +/// See https://www.ethaneade.org/lie.pdf expmap (82) and left Jacobian (83). struct GTSAM_EXPORT DexpFunctor : public ExpmapFunctor { const Vector3 omega; diff --git a/gtsam/geometry/tests/testSO3.cpp b/gtsam/geometry/tests/testSO3.cpp index 200642456..c6fd52161 100644 --- a/gtsam/geometry/tests/testSO3.cpp +++ b/gtsam/geometry/tests/testSO3.cpp @@ -19,7 +19,6 @@ #include #include #include -#include using namespace std::placeholders; using namespace std; diff --git a/gtsam/navigation/NavState.cpp b/gtsam/navigation/NavState.cpp index c9cd244d3..bd482132e 100644 --- a/gtsam/navigation/NavState.cpp +++ b/gtsam/navigation/NavState.cpp @@ -114,7 +114,7 @@ NavState NavState::inverse() const { //------------------------------------------------------------------------------ NavState NavState::Expmap(const Vector9& xi, OptionalJacobian<9, 9> Hxi) { - // Get angular velocity w and components rho and nu from xi + // Get angular velocity w and components rho (for t) and nu (for v) from xi Vector3 w = xi.head<3>(), rho = xi.segment<3>(3), nu = xi.tail<3>(); // Compute rotation using Expmap From 14c79986d3416320e110f567b0def090b9ae9c1f Mon Sep 17 00:00:00 2001 From: Joel Truher Date: Mon, 16 Dec 2024 18:04:40 -0800 Subject: [PATCH 58/80] consolidate factors in one file --- gtsam/slam/PlanarProjectionFactor.h | 360 +++++++++++++----- gtsam/slam/PlanarSFMFactor.h | 165 -------- gtsam/slam/slam.i | 36 +- .../slam/tests/testPlanarProjectionFactor.cpp | 224 ++++++++++- gtsam/slam/tests/testPlanarSFMFactor.cpp | 243 ------------ 5 files changed, 499 insertions(+), 529 deletions(-) delete mode 100644 gtsam/slam/PlanarSFMFactor.h delete mode 100644 gtsam/slam/tests/testPlanarSFMFactor.cpp diff --git a/gtsam/slam/PlanarProjectionFactor.h b/gtsam/slam/PlanarProjectionFactor.h index 49d1f7cac..285b64900 100644 --- a/gtsam/slam/PlanarProjectionFactor.h +++ b/gtsam/slam/PlanarProjectionFactor.h @@ -1,24 +1,13 @@ /** - * Similar to GenericProjectionFactor, but: - * - * * 2d pose variable (robot on the floor) - * * constant landmarks - * * batched input - * * numeric differentiation + * ProjectionFactor, but with pose2 (robot on the floor) * * This factor is useful for high-school robotics competitions, * which run robots on the floor, with use fixed maps and fiducial - * markers. The camera offset and calibration are fixed, perhaps - * found using PlanarSFMFactor. - * - * The python version of this factor uses batches, to save on calls - * across the C++/python boundary, but here the only extra cost - * is instantiating the camera, so there's no batch. + * markers. * * @see https://www.firstinspires.org/ - * @see PlanarSFMFactor.h * - * @file PlanarSFMFactor.h + * @file PlanarProjectionFactor.h * @brief for planar smoothing * @date Dec 2, 2024 * @author joel@truher.org @@ -39,116 +28,291 @@ namespace gtsam { + /** - * @class PlanarProjectionFactor - * @brief Camera projection for robot on the floor. + * @class PlanarProjectionFactorBase + * @brief Camera projection for robot on the floor. Measurement is camera pixels. */ - class PlanarProjectionFactor : public NoiseModelFactorN { - typedef NoiseModelFactorN Base; - static const Pose3 CAM_COORD; - + class PlanarProjectionFactorBase { protected: - - Point3 landmark_; // landmark - Point2 measured_; // pixel measurement - Pose3 offset_; // camera offset to robot pose - Cal3DS2 calib_; // camera calibration - - public: - // Provide access to the Matrix& version of evaluateError: - using Base::evaluateError; - - PlanarProjectionFactor() {} + PlanarProjectionFactorBase() {} /** - * @param landmarks point in the world * @param measured corresponding point in the camera frame - * @param offset constant camera offset from pose - * @param calib constant camera calibration - * @param model stddev of the measurements, ~one pixel? * @param poseKey index of the robot pose in the z=0 plane */ - PlanarProjectionFactor( + PlanarProjectionFactorBase(const Point2& measured) : measured_(measured) {} + + /** + * @param landmark point3 + * @param pose + * @param offset oriented parallel to pose2 zero i.e. +x + * @param calib + * @param H1 landmark jacobian + * @param H2 pose jacobian + * @param H3 offset jacobian + * @param H4 calib jacobian + */ + Point2 h( const Point3& landmark, - const Point2& measured, + const Pose2& pose, const Pose3& offset, const Cal3DS2& calib, - const SharedNoiseModel& model, - Key poseKey) - : NoiseModelFactorN(model, poseKey), - landmark_(landmark), - measured_(measured), - offset_(offset), - calib_(calib) - { - assert(2 == model->dim()); - } - - ~PlanarProjectionFactor() override {} - - /// @return a deep copy of this factor - gtsam::NonlinearFactor::shared_ptr clone() const override { - return std::static_pointer_cast( - gtsam::NonlinearFactor::shared_ptr(new PlanarProjectionFactor(*this))); - } - - Point2 h2(const Pose2& pose, OptionalMatrixType H1) const { - // this is x-forward z-up - gtsam::Matrix H0; - Pose3 offset_pose = Pose3(pose).compose(offset_, H0); - // this is z-forward y-down - gtsam::Matrix H00; - Pose3 camera_pose = offset_pose.compose(CAM_COORD, H00); - PinholeCamera camera = PinholeCamera(camera_pose, calib_); - if (H1) { - // Dpose is 2x6 (R,t) - // H1 is 2x3 (x, y, theta) - gtsam::Matrix Dpose; - Point2 result = camera.project(landmark_, Dpose); - Dpose = Dpose * H00 * H0; - *H1 = Matrix::Zero(2, 3); - (*H1)(0, 0) = Dpose(0, 3); // du/dx - (*H1)(1, 0) = Dpose(1, 3); // dv/dx - (*H1)(0, 1) = Dpose(0, 4); // du/dy - (*H1)(1, 1) = Dpose(1, 4); // dv/dy - (*H1)(0, 2) = Dpose(0, 2); // du/dyaw - (*H1)(1, 2) = Dpose(1, 2); // dv/dyaw - return result; - } - else { - return camera.project(landmark_, {}, {}, {}); - } - } - - Vector evaluateError( - const Pose2& pose, - OptionalMatrixType H1 = OptionalNone) const override { + OptionalMatrixType H1, // 2x3 (x, y, z) + OptionalMatrixType H2, // 2x3 (x, y, theta) + OptionalMatrixType H3, // 2x6 (rx, ry, rz, x, y, theta) + OptionalMatrixType H4 // 2x9 + ) const { try { - return h2(pose, H1) - measured_; + // this is x-forward z-up + gtsam::Matrix H0; // 6x6 + Pose3 offset_pose = Pose3(pose).compose(offset, H0); + // this is z-forward y-down + gtsam::Matrix H00; // 6x6 + Pose3 camera_pose = offset_pose.compose(CAM_COORD, H00); + PinholeCamera camera = PinholeCamera(camera_pose, calib); + if (H2 || H3) { + // Dpose is for pose3, 2x6 (R,t) + gtsam::Matrix Dpose; + Point2 result = camera.project(landmark, Dpose, H1, H4); + gtsam::Matrix DposeOffset = Dpose * H00; // 2x6 + if (H3) + *H3 = DposeOffset; // with Eigen this is a deep copy (!) + if (H2) { + gtsam::Matrix DposeOffsetFwd = DposeOffset * H0; + *H2 = Matrix::Zero(2, 3); + (*H2)(0, 0) = DposeOffsetFwd(0, 3); // du/dx + (*H2)(1, 0) = DposeOffsetFwd(1, 3); // dv/dx + (*H2)(0, 1) = DposeOffsetFwd(0, 4); // du/dy + (*H2)(1, 1) = DposeOffsetFwd(1, 4); // dv/dy + (*H2)(0, 2) = DposeOffsetFwd(0, 2); // du/dyaw + (*H2)(1, 2) = DposeOffsetFwd(1, 2); // dv/dyaw + } + return result; + } else { + return camera.project(landmark, {}, {}, {}); + } } catch (CheiralityException& e) { std::cout << "****** CHIRALITY EXCEPTION ******\n"; - std::cout << "landmark " << landmark_ << "\n"; - std::cout << "pose " << pose << "\n"; - std::cout << "offset " << offset_ << "\n"; - std::cout << "calib " << calib_ << "\n"; if (H1) *H1 = Matrix::Zero(2, 3); + if (H2) *H2 = Matrix::Zero(2, 3); + if (H3) *H3 = Matrix::Zero(2, 6); + if (H4) *H4 = Matrix::Zero(2, 9); // return a large error - return Matrix::Constant(2, 1, 2.0 * calib_.fx()); + return Matrix::Constant(2, 1, 2.0 * calib.fx()); } } + + Point2 measured_; // pixel measurement + + private: + static const Pose3 CAM_COORD; }; // camera "zero" is facing +z; this turns it to face +x - const Pose3 PlanarProjectionFactor::CAM_COORD = Pose3( + const Pose3 PlanarProjectionFactorBase::CAM_COORD = Pose3( Rot3(0, 0, 1,// -1, 0, 0, // 0, -1, 0), Vector3(0, 0, 0) ); - template<> - struct traits : - public Testable { + /** + * @class PlanarProjectionFactor1 + * @brief One variable: the pose. + * Landmark, camera offset, camera calibration are constant. + * This is intended for localization within a known map. + */ + class PlanarProjectionFactor1 + : public PlanarProjectionFactorBase, public NoiseModelFactorN { + public: + typedef NoiseModelFactorN Base; + using Base::evaluateError; + PlanarProjectionFactor1() {} + + ~PlanarProjectionFactor1() override {} + + /// @return a deep copy of this factor + gtsam::NonlinearFactor::shared_ptr clone() const override { + return std::static_pointer_cast( + gtsam::NonlinearFactor::shared_ptr(new PlanarProjectionFactor1(*this))); + } + + + /** + * @param landmark point3 in the world + * @param measured corresponding point2 in the camera frame + * @param offset constant camera offset from pose + * @param calib constant camera calibration + * @param model stddev of the measurements, ~one pixel? + * @param poseKey index of the robot pose2 in the z=0 plane + */ + PlanarProjectionFactor1( + const Point3& landmark, + const Point2& measured, + const Pose3& offset, + const Cal3DS2& calib, + const SharedNoiseModel& model, + Key poseKey) + : PlanarProjectionFactorBase(measured), + NoiseModelFactorN(model, poseKey), + landmark_(landmark), + offset_(offset), + calib_(calib) { + assert(2 == model->dim()); + } + + /** + * @param pose estimated pose2 + * @param H1 pose jacobian + */ + Vector evaluateError( + const Pose2& pose, + OptionalMatrixType H1 = OptionalNone) const override { + return h(landmark_, pose, offset_, calib_, {}, H1, {}, {}) - measured_; + } + + private: + Point3 landmark_; // landmark + Pose3 offset_; // camera offset to robot pose + Cal3DS2 calib_; // camera calibration }; -} // namespace gtsam + template<> + struct traits : + public Testable {}; + + /** + * @class PlanarProjectionFactor2 + * @brief Two unknowns: the pose and the landmark. + * Camera offset and calibration are constant. + * This is similar to GeneralSFMFactor, used for SLAM. + */ + class PlanarProjectionFactor2 : public PlanarProjectionFactorBase, public NoiseModelFactorN { + public: + typedef NoiseModelFactorN Base; + using Base::evaluateError; + + PlanarProjectionFactor2() {} + + ~PlanarProjectionFactor2() override {} + + /// @return a deep copy of this factor + gtsam::NonlinearFactor::shared_ptr clone() const override { + return std::static_pointer_cast( + gtsam::NonlinearFactor::shared_ptr(new PlanarProjectionFactor2(*this))); + } + + /** + * @param measured corresponding point in the camera frame + * @param offset constant camera offset from pose + * @param calib constant camera calibration + * @param model stddev of the measurements, ~one pixel? + * @param poseKey index of the robot pose2 in the z=0 plane + * @param landmarkKey index of the landmark point3 + */ + PlanarProjectionFactor2( + const Point2& measured, + const Pose3& offset, + const Cal3DS2& calib, + const SharedNoiseModel& model, + Key landmarkKey, + Key poseKey) + : PlanarProjectionFactorBase(measured), + NoiseModelFactorN(model, landmarkKey, poseKey), + offset_(offset), + calib_(calib) { + assert(2 == model->dim()); + } + + /** + * @param landmark estimated landmark + * @param pose estimated pose2 + * @param H1 landmark jacobian + * @param H2 pose jacobian + */ + Vector evaluateError( + const Point3& landmark, + const Pose2& pose, + OptionalMatrixType H1 = OptionalNone, + OptionalMatrixType H2 = OptionalNone) const override { + return h(landmark, pose, offset_, calib_, H1, H2, {}, {}) - measured_; + } + + private: + Pose3 offset_; // camera offset to robot pose + Cal3DS2 calib_; // camera calibration + }; + + template<> + struct traits : + public Testable {}; + + /** + * @class PlanarProjectionFactor3 + * @brief Three unknowns: the pose, the camera offset, and the camera calibration. + * Landmark is constant. + * This is intended to be used for camera calibration. + */ + class PlanarProjectionFactor3 : public PlanarProjectionFactorBase, public NoiseModelFactorN { + public: + typedef NoiseModelFactorN Base; + using Base::evaluateError; + + PlanarProjectionFactor3() {} + + ~PlanarProjectionFactor3() override {} + + /// @return a deep copy of this factor + gtsam::NonlinearFactor::shared_ptr clone() const override { + return std::static_pointer_cast( + gtsam::NonlinearFactor::shared_ptr(new PlanarProjectionFactor3(*this))); + } + + /** + * @param landmark point3 in the world + * @param measured corresponding point2 in the camera frame + * @param model stddev of the measurements, ~one pixel? + * @param poseKey index of the robot pose2 in the z=0 plane + * @param offsetKey index of camera offset from pose + * @param calibKey index of camera calibration */ + PlanarProjectionFactor3( + const Point3& landmark, + const Point2& measured, + const SharedNoiseModel& model, + Key poseKey, + Key offsetKey, + Key calibKey) + : PlanarProjectionFactorBase(measured), + NoiseModelFactorN(model, poseKey, offsetKey, calibKey), + landmark_(landmark) { + assert(2 == model->dim()); + } + + /** + * @param pose estimated pose2 + * @param offset pose3 offset from pose2 +x + * @param calib calibration + * @param H1 pose jacobian + * @param H2 offset jacobian + * @param H3 calibration jacobian + */ + Vector evaluateError( + const Pose2& pose, + const Pose3& offset, + const Cal3DS2& calib, + OptionalMatrixType H1 = OptionalNone, + OptionalMatrixType H2 = OptionalNone, + OptionalMatrixType H3 = OptionalNone) const override { + return h(landmark_, pose, offset, calib, {}, H1, H2, H3) - measured_; + } + + private: + Point3 landmark_; // landmark + }; + + template<> + struct traits : + public Testable {}; + +} // namespace gtsam \ No newline at end of file diff --git a/gtsam/slam/PlanarSFMFactor.h b/gtsam/slam/PlanarSFMFactor.h deleted file mode 100644 index bdcbb6ee2..000000000 --- a/gtsam/slam/PlanarSFMFactor.h +++ /dev/null @@ -1,165 +0,0 @@ -/** - * Similar to GeneralSFMFactor, but: - * - * * 2d pose variable (robot on the floor) - * * camera offset variable - * * constant landmarks - * * batched input - * * numeric differentiation - * - * This factor is useful to find camera calibration and placement, in - * a sort of "autocalibrate" mode. Once a satisfactory solution is - * found, the PlanarProjectionFactor should be used for localization. - * - * The python version of this factor uses batches, to save on calls - * across the C++/python boundary, but here the only extra cost - * is instantiating the camera, so there's no batch. - * - * @see https://www.firstinspires.org/ - * @see PlanarProjectionFactor.h - * - * @file PlanarSFMFactor.h - * @brief for planar smoothing with unknown calibration - * @date Dec 2, 2024 - * @author joel@truher.org - */ -#pragma once - -#include -#include - -#include -#include -#include -#include -#include -#include -#include -#include - - -namespace gtsam { - /** - * @class PlanarSFMFactor - * @brief Camera calibration for robot on the floor. - */ - class PlanarSFMFactor : public NoiseModelFactorN { - private: - typedef NoiseModelFactorN Base; - static const Pose3 CAM_COORD; - - protected: - - Point3 landmark_; // landmark - Point2 measured_; // pixel measurement - - public: - // Provide access to the Matrix& version of evaluateError: - using Base::evaluateError; - - PlanarSFMFactor() {} - /** - * @param landmarks point in the world - * @param measured corresponding point in the camera frame - * @param model stddev of the measurements, ~one pixel? - * @param poseKey index of the robot pose2 in the z=0 plane - * @param offsetKey index of the 3d camera offset from the robot pose - * @param calibKey index of the camera calibration - */ - PlanarSFMFactor( - const Point3& landmark, - const Point2& measured, - const SharedNoiseModel& model, - Key poseKey, - Key offsetKey, - Key calibKey) - : NoiseModelFactorN(model, poseKey, offsetKey, calibKey), - landmark_(landmark), measured_(measured) - { - assert(2 == model->dim()); - } - - ~PlanarSFMFactor() override {} - - /// @return a deep copy of this factor - gtsam::NonlinearFactor::shared_ptr clone() const override { - return std::static_pointer_cast( - gtsam::NonlinearFactor::shared_ptr(new PlanarSFMFactor(*this))); - } - - Point2 h2(const Pose2& pose, - const Pose3& offset, - const Cal3DS2& calib, - OptionalMatrixType H1, - OptionalMatrixType H2, - OptionalMatrixType H3 - ) const { - // this is x-forward z-up - gtsam::Matrix H0; - Pose3 offset_pose = Pose3(pose).compose(offset, H0); - // this is z-forward y-down - gtsam::Matrix H00; - Pose3 camera_pose = offset_pose.compose(CAM_COORD, H00); - PinholeCamera camera = PinholeCamera(camera_pose, calib); - if (H1 || H2) { - gtsam::Matrix Dpose; - Point2 result = camera.project(landmark_, Dpose, {}, H3); - gtsam::Matrix DposeOffset = Dpose * H00; - if (H2) - *H2 = DposeOffset; // a deep copy - if (H1) { - gtsam::Matrix DposeOffsetFwd = DposeOffset * H0; - *H1 = Matrix::Zero(2,3); - (*H1)(0,0) = DposeOffsetFwd(0,3); // du/dx - (*H1)(1,0) = DposeOffsetFwd(1,3); // dv/dx - (*H1)(0,1) = DposeOffsetFwd(0,4); // du/dy - (*H1)(1,1) = DposeOffsetFwd(1,4); // dv/dy - (*H1)(0,2) = DposeOffsetFwd(0,2); // du/dyaw - (*H1)(1,2) = DposeOffsetFwd(1,2); // dv/dyaw - } - return result; - } else { - return camera.project(landmark_, {}, {}, {}); - } - } - - Vector evaluateError( - const Pose2& pose, - const Pose3& offset, - const Cal3DS2& calib, - OptionalMatrixType H1 = OptionalNone, - OptionalMatrixType H2 = OptionalNone, - OptionalMatrixType H3 = OptionalNone - ) const override { - try { - return h2(pose,offset,calib,H1,H2,H3) - measured_; - } - catch (CheiralityException& e) { - std::cout << "****** CHIRALITY EXCEPTION ******\n"; - std::cout << "landmark " << landmark_ << "\n"; - std::cout << "pose " << pose << "\n"; - std::cout << "offset " << offset << "\n"; - std::cout << "calib " << calib << "\n"; - if (H1) *H1 = Matrix::Zero(2, 3); - if (H2) *H2 = Matrix::Zero(2, 6); - if (H3) *H3 = Matrix::Zero(2, 9); - // return a large error - return Matrix::Constant(2, 1, 2.0 * calib.fx()); - } - } - }; - - // camera "zero" is facing +z; this turns it to face +x - const Pose3 PlanarSFMFactor::CAM_COORD = Pose3( - Rot3(0, 0, 1,// - -1, 0, 0, // - 0, -1, 0), - Vector3(0, 0, 0) - ); - - template<> - struct traits : - public Testable { - }; - -} // namespace gtsam diff --git a/gtsam/slam/slam.i b/gtsam/slam/slam.i index d69b9890e..992c9aa6f 100644 --- a/gtsam/slam/slam.i +++ b/gtsam/slam/slam.i @@ -25,8 +25,8 @@ virtual class BetweenFactor : gtsam::NoiseModelFactor { }; #include -virtual class PlanarProjectionFactor : gtsam::NoiseModelFactor { - PlanarProjectionFactor( +virtual class PlanarProjectionFactor1 : gtsam::NoiseModelFactor { + PlanarProjectionFactor1( const gtsam::Point3& landmark, const gtsam::Point2& measured, const gtsam::Pose3& offset, @@ -35,6 +35,26 @@ virtual class PlanarProjectionFactor : gtsam::NoiseModelFactor { size_t poseKey); void serialize() const; }; +virtual class PlanarProjectionFactor2 : gtsam::NoiseModelFactor { + PlanarProjectionFactor2( + const gtsam::Point2& measured, + const gtsam::Pose3& offset, + const gtsam::Cal3DS2& calib, + const gtsam::noiseModel::Base* model, + size_t landmarkKey, + size_t poseKey); + void serialize() const; +}; +virtual class PlanarProjectionFactor3 : gtsam::NoiseModelFactor { + PlanarProjectionFactor3( + const gtsam::Point3& landmark, + const gtsam::Point2& measured, + const gtsam::noiseModel::Base* model, + size_t poseKey, + size_t offsetKey, + size_t calibKey)); + void serialize() const; +}; #include template @@ -79,18 +99,6 @@ typedef gtsam::GenericProjectionFactor GenericProjectionFactorCal3Unified; -#include -virtual class PlanarSFMFactor : gtsam::NoiseModelFactor { - PlanarSFMFactor( - const gtsam::Point3& landmark, - const gtsam::Point2& measured, - const gtsam::noiseModel::Base* model, - size_t poseKey, - size_t offsetKey, - size_t calibKey); - void serialize() const; -}; - #include template virtual class GeneralSFMFactor : gtsam::NoiseModelFactor { diff --git a/gtsam/slam/tests/testPlanarProjectionFactor.cpp b/gtsam/slam/tests/testPlanarProjectionFactor.cpp index eafe2041f..21961cdad 100644 --- a/gtsam/slam/tests/testPlanarProjectionFactor.cpp +++ b/gtsam/slam/tests/testPlanarProjectionFactor.cpp @@ -24,8 +24,10 @@ using namespace std; using namespace gtsam; using symbol_shorthand::X; +using symbol_shorthand::C; +using symbol_shorthand::K; -TEST(PlanarProjectionFactor, error1) { +TEST(PlanarProjectionFactor1, error1) { // landmark is on the camera bore (which faces +x) Point3 landmark(1, 0, 0); // so pixel measurement is (cx, cy) @@ -37,7 +39,7 @@ TEST(PlanarProjectionFactor, error1) { Pose2 pose(0, 0, 0); values.insert(X(0), pose); - PlanarProjectionFactor factor(landmark, measured, offset, calib, model, X(0)); + PlanarProjectionFactor1 factor(landmark, measured, offset, calib, model, X(0)); CHECK_EQUAL(2, factor.dim()); CHECK(factor.active(values)); @@ -55,7 +57,7 @@ TEST(PlanarProjectionFactor, error1) { CHECK(assert_equal(H1Expected, H1Actual, 1e-6)); } -TEST(PlanarProjectionFactor, error2) { +TEST(PlanarProjectionFactor1, error2) { // landmark is in the upper left corner Point3 landmark(1, 1, 1); // upper left corner in pixels @@ -63,7 +65,7 @@ TEST(PlanarProjectionFactor, error2) { Pose3 offset; Cal3DS2 calib(200, 200, 0, 200, 200, 0, 0); SharedNoiseModel model = noiseModel::Diagonal::Sigmas(Vector2(1, 1)); - PlanarProjectionFactor factor(landmark, measured, offset, calib, model, X(0)); + PlanarProjectionFactor1 factor(landmark, measured, offset, calib, model, X(0)); Values values; Pose2 pose(0, 0, 0); @@ -85,7 +87,7 @@ TEST(PlanarProjectionFactor, error2) { CHECK(assert_equal(H1Expected, H1Actual, 1e-6)); } -TEST(PlanarProjectionFactor, error3) { +TEST(PlanarProjectionFactor1, error3) { // landmark is in the upper left corner Point3 landmark(1, 1, 1); // upper left corner in pixels @@ -94,7 +96,7 @@ TEST(PlanarProjectionFactor, error3) { // distortion Cal3DS2 calib(200, 200, 0, 200, 200, -0.2, 0.1); SharedNoiseModel model = noiseModel::Diagonal::Sigmas(Vector2(1, 1)); - PlanarProjectionFactor factor(landmark, measured, offset, calib, model, X(0)); + PlanarProjectionFactor1 factor(landmark, measured, offset, calib, model, X(0)); Values values; Pose2 pose(0, 0, 0); @@ -116,7 +118,7 @@ TEST(PlanarProjectionFactor, error3) { CHECK(assert_equal(H1Expected, H1Actual, 1e-6)); } -TEST(PlanarProjectionFactor, jacobian) { +TEST(PlanarProjectionFactor1, jacobian) { // test many jacobians with many randoms std::default_random_engine g; @@ -129,7 +131,7 @@ TEST(PlanarProjectionFactor, jacobian) { Pose3 offset(Rot3::Ypr(s(g),s(g),s(g)), Point3(s(g),s(g),s(g))); Cal3DS2 calib(200, 200, 0, 200, 200, -0.2, 0.1); - PlanarProjectionFactor factor(landmark, measured, offset, calib, model, X(0)); + PlanarProjectionFactor1 factor(landmark, measured, offset, calib, model, X(0)); Pose2 pose(s(g), s(g), s(g)); @@ -143,8 +145,213 @@ TEST(PlanarProjectionFactor, jacobian) { pose); CHECK(assert_equal(expectedH1, H1, 1e-6)); } +} + +TEST(PlanarProjectionFactor3, error1) { + // landmark is on the camera bore (facing +x) + Point3 landmark(1, 0, 0); + // so px is (cx, cy) + Point2 measured(200, 200); + // offset is identity + Pose3 offset; + Cal3DS2 calib(200, 200, 0, 200, 200, 0, 0); + SharedNoiseModel model = noiseModel::Diagonal::Sigmas(Vector2(1, 1)); + Values values; + Pose2 pose(0, 0, 0); + values.insert(X(0), pose); + values.insert(C(0), offset); + values.insert(K(0), calib); + + PlanarProjectionFactor3 factor(landmark, measured, model, X(0), C(0), K(0)); + + CHECK_EQUAL(2, factor.dim()); + CHECK(factor.active(values)); + std::vector actualHs(3); + + gtsam::Vector actual = factor.unwhitenedError(values, actualHs); + CHECK(assert_equal(Vector2(0, 0), actual)); + + const Matrix& H1Actual = actualHs.at(0); + const Matrix& H2Actual = actualHs.at(1); + const Matrix& H3Actual = actualHs.at(2); + + CHECK_EQUAL(2, H1Actual.rows()); + CHECK_EQUAL(3, H1Actual.cols()); + CHECK_EQUAL(2, H2Actual.rows()); + CHECK_EQUAL(6, H2Actual.cols()); + CHECK_EQUAL(2, H3Actual.rows()); + CHECK_EQUAL(9, H3Actual.cols()); + + // du/dx etc for the pose2d + Matrix23 H1Expected = (Matrix23() <dim()); + CHECK_EQUAL(2, factor.dim()); + CHECK(factor.active(values)); + std::vector actualHs(3); + gtsam::Vector actual = factor.unwhitenedError(values, actualHs); + + CHECK(assert_equal(Vector2(0, 0), actual)); + + const Matrix& H1Actual = actualHs.at(0); + const Matrix& H2Actual = actualHs.at(1); + const Matrix& H3Actual = actualHs.at(2); + + CHECK_EQUAL(2, H1Actual.rows()); + CHECK_EQUAL(3, H1Actual.cols()); + CHECK_EQUAL(2, H2Actual.rows()); + CHECK_EQUAL(6, H2Actual.cols()); + CHECK_EQUAL(2, H3Actual.rows()); + CHECK_EQUAL(9, H3Actual.cols()); + + Matrix23 H1Expected = (Matrix23() <dim()); + CHECK_EQUAL(2, factor.dim()); + CHECK(factor.active(values)); + std::vector actualHs(3); + gtsam::Vector actual = factor.unwhitenedError(values, actualHs); + + CHECK(assert_equal(Vector2(0, 0), actual)); + + const Matrix& H1Actual = actualHs.at(0); + const Matrix& H2Actual = actualHs.at(1); + const Matrix& H3Actual = actualHs.at(2); + + CHECK_EQUAL(2, H1Actual.rows()); + CHECK_EQUAL(3, H1Actual.cols()); + CHECK_EQUAL(2, H2Actual.rows()); + CHECK_EQUAL(6, H2Actual.cols()); + CHECK_EQUAL(2, H3Actual.rows()); + CHECK_EQUAL(9, H3Actual.cols()); + + Matrix23 H1Expected = (Matrix23() < s(-0.3, 0.3); + SharedNoiseModel model = noiseModel::Diagonal::Sigmas(Vector2(1, 1)); + + for (int i = 0; i < 1000; ++i) { + Point3 landmark(2 + s(g), s(g), s(g)); + Point2 measured(200 + 100*s(g), 200 + 100*s(g)); + Pose3 offset(Rot3::Ypr(s(g),s(g),s(g)), Point3(s(g),s(g),s(g))); + Cal3DS2 calib(200, 200, 0, 200, 200, -0.2, 0.1); + + PlanarProjectionFactor3 factor(landmark, measured, model, X(0), C(0), K(0)); + + Pose2 pose(s(g), s(g), s(g)); + + // actual H + Matrix H1, H2, H3; + factor.evaluateError(pose, offset, calib, H1, H2, H3); + + Matrix expectedH1 = numericalDerivative31( + [&factor](const Pose2& p, const Pose3& o, const Cal3DS2& c) { + return factor.evaluateError(p, o, c);}, + pose, offset, calib); + Matrix expectedH2 = numericalDerivative32( + [&factor](const Pose2& p, const Pose3& o, const Cal3DS2& c) { + return factor.evaluateError(p, o, c);}, + pose, offset, calib); + Matrix expectedH3 = numericalDerivative33( + [&factor](const Pose2& p, const Pose3& o, const Cal3DS2& c) { + return factor.evaluateError(p, o, c);}, + pose, offset, calib); + CHECK(assert_equal(expectedH1, H1, 1e-6)); + CHECK(assert_equal(expectedH2, H2, 1e-6)); + CHECK(assert_equal(expectedH3, H3, 1e-6)); + } } /* ************************************************************************* */ @@ -153,4 +360,3 @@ int main() { return TestRegistry::runAllTests(tr); } /* ************************************************************************* */ - diff --git a/gtsam/slam/tests/testPlanarSFMFactor.cpp b/gtsam/slam/tests/testPlanarSFMFactor.cpp deleted file mode 100644 index 22ec67561..000000000 --- a/gtsam/slam/tests/testPlanarSFMFactor.cpp +++ /dev/null @@ -1,243 +0,0 @@ -/** - * @file testPlanarSFMFactor.cpp - * @date Dec 3, 2024 - * @author joel@truher.org - * @brief unit tests for PlanarSFMFactor - */ - -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include - -using namespace std; -using namespace gtsam; -using symbol_shorthand::X; -using symbol_shorthand::C; -using symbol_shorthand::K; - -TEST(PlanarSFMFactor, error1) { - // landmark is on the camera bore (facing +x) - Point3 landmark(1, 0, 0); - // so px is (cx, cy) - Point2 measured(200, 200); - // offset is identity - Pose3 offset; - Cal3DS2 calib(200, 200, 0, 200, 200, 0, 0); - SharedNoiseModel model = noiseModel::Diagonal::Sigmas(Vector2(1, 1)); - Values values; - Pose2 pose(0, 0, 0); - values.insert(X(0), pose); - values.insert(C(0), offset); - values.insert(K(0), calib); - - PlanarSFMFactor factor(landmark, measured, model, X(0), C(0), K(0)); - - CHECK_EQUAL(2, factor.dim()); - CHECK(factor.active(values)); - std::vector actualHs(3); - - gtsam::Vector actual = factor.unwhitenedError(values, actualHs); - CHECK(assert_equal(Vector2(0, 0), actual)); - - const Matrix& H1Actual = actualHs.at(0); - const Matrix& H2Actual = actualHs.at(1); - const Matrix& H3Actual = actualHs.at(2); - - CHECK_EQUAL(2, H1Actual.rows()); - CHECK_EQUAL(3, H1Actual.cols()); - CHECK_EQUAL(2, H2Actual.rows()); - CHECK_EQUAL(6, H2Actual.cols()); - CHECK_EQUAL(2, H3Actual.rows()); - CHECK_EQUAL(9, H3Actual.cols()); - - // du/dx etc for the pose2d - Matrix23 H1Expected = (Matrix23() <dim()); - CHECK_EQUAL(2, factor.dim()); - CHECK(factor.active(values)); - std::vector actualHs(3); - gtsam::Vector actual = factor.unwhitenedError(values, actualHs); - - CHECK(assert_equal(Vector2(0, 0), actual)); - - const Matrix& H1Actual = actualHs.at(0); - const Matrix& H2Actual = actualHs.at(1); - const Matrix& H3Actual = actualHs.at(2); - - CHECK_EQUAL(2, H1Actual.rows()); - CHECK_EQUAL(3, H1Actual.cols()); - CHECK_EQUAL(2, H2Actual.rows()); - CHECK_EQUAL(6, H2Actual.cols()); - CHECK_EQUAL(2, H3Actual.rows()); - CHECK_EQUAL(9, H3Actual.cols()); - - Matrix23 H1Expected = (Matrix23() <dim()); - CHECK_EQUAL(2, factor.dim()); - CHECK(factor.active(values)); - std::vector actualHs(3); - gtsam::Vector actual = factor.unwhitenedError(values, actualHs); - - CHECK(assert_equal(Vector2(0, 0), actual)); - - const Matrix& H1Actual = actualHs.at(0); - const Matrix& H2Actual = actualHs.at(1); - const Matrix& H3Actual = actualHs.at(2); - - CHECK_EQUAL(2, H1Actual.rows()); - CHECK_EQUAL(3, H1Actual.cols()); - CHECK_EQUAL(2, H2Actual.rows()); - CHECK_EQUAL(6, H2Actual.cols()); - CHECK_EQUAL(2, H3Actual.rows()); - CHECK_EQUAL(9, H3Actual.cols()); - - Matrix23 H1Expected = (Matrix23() < s(-0.3, 0.3); - SharedNoiseModel model = noiseModel::Diagonal::Sigmas(Vector2(1, 1)); - - for (int i = 0; i < 1000; ++i) { - Point3 landmark(2 + s(g), s(g), s(g)); - Point2 measured(200 + 100*s(g), 200 + 100*s(g)); - Pose3 offset(Rot3::Ypr(s(g),s(g),s(g)), Point3(s(g),s(g),s(g))); - Cal3DS2 calib(200, 200, 0, 200, 200, -0.2, 0.1); - - PlanarSFMFactor factor(landmark, measured, model, X(0), C(0), K(0)); - - Pose2 pose(s(g), s(g), s(g)); - - // actual H - Matrix H1, H2, H3; - factor.evaluateError(pose, offset, calib, H1, H2, H3); - - Matrix expectedH1 = numericalDerivative31( - [&factor](const Pose2& p, const Pose3& o, const Cal3DS2& c) { - return factor.evaluateError(p, o, c);}, - pose, offset, calib); - Matrix expectedH2 = numericalDerivative32( - [&factor](const Pose2& p, const Pose3& o, const Cal3DS2& c) { - return factor.evaluateError(p, o, c);}, - pose, offset, calib); - Matrix expectedH3 = numericalDerivative33( - [&factor](const Pose2& p, const Pose3& o, const Cal3DS2& c) { - return factor.evaluateError(p, o, c);}, - pose, offset, calib); - CHECK(assert_equal(expectedH1, H1, 1e-6)); - CHECK(assert_equal(expectedH2, H2, 1e-6)); - CHECK(assert_equal(expectedH3, H3, 1e-6)); - } -} - - -/* ************************************************************************* */ -int main() { - TestResult tr; - return TestRegistry::runAllTests(tr); -} -/* ************************************************************************* */ - From ca958ccbc70eda61d5d56d101fa0101cac2e099c Mon Sep 17 00:00:00 2001 From: Joel Truher Date: Mon, 16 Dec 2024 19:06:23 -0800 Subject: [PATCH 59/80] maybe fix clang? --- gtsam/slam/tests/testPlanarProjectionFactor.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/gtsam/slam/tests/testPlanarProjectionFactor.cpp b/gtsam/slam/tests/testPlanarProjectionFactor.cpp index 21961cdad..03a0b7bed 100644 --- a/gtsam/slam/tests/testPlanarProjectionFactor.cpp +++ b/gtsam/slam/tests/testPlanarProjectionFactor.cpp @@ -141,7 +141,7 @@ TEST(PlanarProjectionFactor1, jacobian) { Matrix expectedH1 = numericalDerivative11( [&factor](const Pose2& p) { - return factor.evaluateError(p);}, + return factor.evaluateError(p, {});}, pose); CHECK(assert_equal(expectedH1, H1, 1e-6)); } @@ -338,15 +338,15 @@ TEST(PlanarProjectionFactor3, jacobian) { Matrix expectedH1 = numericalDerivative31( [&factor](const Pose2& p, const Pose3& o, const Cal3DS2& c) { - return factor.evaluateError(p, o, c);}, + return factor.evaluateError(p, o, c, {}, {}, {});}, pose, offset, calib); Matrix expectedH2 = numericalDerivative32( [&factor](const Pose2& p, const Pose3& o, const Cal3DS2& c) { - return factor.evaluateError(p, o, c);}, + return factor.evaluateError(p, o, c, {}, {}, {});}, pose, offset, calib); Matrix expectedH3 = numericalDerivative33( [&factor](const Pose2& p, const Pose3& o, const Cal3DS2& c) { - return factor.evaluateError(p, o, c);}, + return factor.evaluateError(p, o, c, {}, {}, {});}, pose, offset, calib); CHECK(assert_equal(expectedH1, H1, 1e-6)); CHECK(assert_equal(expectedH2, H2, 1e-6)); From 70288bc32a573cb2040d2f6f0fc115b1e5a094b2 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 16 Dec 2024 22:18:12 -0500 Subject: [PATCH 60/80] remove print statements --- gtsam/discrete/TableFactor.cpp | 6 ------ 1 file changed, 6 deletions(-) diff --git a/gtsam/discrete/TableFactor.cpp b/gtsam/discrete/TableFactor.cpp index 0a93e7b5d..d9bfc42c2 100644 --- a/gtsam/discrete/TableFactor.cpp +++ b/gtsam/discrete/TableFactor.cpp @@ -262,8 +262,6 @@ DecisionTreeFactor TableFactor::toDecisionTreeFactor() const { // k will be in reverse key order already uint64_t k; ss >> k; - std::cout << "ss: " << ss.str() << ", k=" << k - << ", v=" << sparse_table_.coeff(i) << std::endl; pair_table.push_back(std::make_pair(k, sparse_table_.coeff(i))); } @@ -273,10 +271,6 @@ DecisionTreeFactor TableFactor::toDecisionTreeFactor() const { [](const std::pair& a, const std::pair& b) { return a.first < b.first; }); - std::cout << "Sorted pair_table:" << std::endl; - for (auto&& [k, v] : pair_table) { - std::cout << "k=" << k << ", v=" << v << std::endl; - } // Create the table vector std::vector table; std::for_each(pair_table.begin(), pair_table.end(), From 8e473c04e82eacbef373cc8efdc8be59eb01e4e1 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Mon, 16 Dec 2024 22:46:16 -0500 Subject: [PATCH 61/80] Address Quaternion on Ubuntu accuracy --- gtsam/geometry/tests/testPose3.cpp | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) diff --git a/gtsam/geometry/tests/testPose3.cpp b/gtsam/geometry/tests/testPose3.cpp index ce74091f0..b04fb1982 100644 --- a/gtsam/geometry/tests/testPose3.cpp +++ b/gtsam/geometry/tests/testPose3.cpp @@ -906,7 +906,7 @@ TEST(Pose3, Logmap) { for (Vector3 v : test_cases::vs) { const Vector6 xi = (Vector6() << w, v).finished(); Pose3 pose = Pose3::Expmap(xi); - EXPECT(assert_equal(xi, Pose3::Logmap(pose), 1e-5)); + EXPECT(assert_equal(xi, Pose3::Logmap(pose))); } } } @@ -924,7 +924,13 @@ TEST(Pose3, LogmapDerivatives) { &Pose3::Logmap, pose, {}); Matrix actualH; Pose3::Logmap(pose, actualH); +#ifdef GTSAM_USE_QUATERNIONS + // TODO(Frank): Figure out why quaternions are not as accurate. + // Hint: 6 cases fail on Ubuntu 22.04, but none on MacOS. + EXPECT(assert_equal(expectedH, actualH, 1e-7)); +#else EXPECT(assert_equal(expectedH, actualH)); +#endif } } } From 11f7eb642295d997012ca942695a5be7e1e79cb4 Mon Sep 17 00:00:00 2001 From: Joel Truher Date: Mon, 16 Dec 2024 19:47:42 -0800 Subject: [PATCH 62/80] fix typo, fix python CI? --- gtsam/slam/slam.i | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/slam/slam.i b/gtsam/slam/slam.i index 992c9aa6f..e141cb50c 100644 --- a/gtsam/slam/slam.i +++ b/gtsam/slam/slam.i @@ -52,7 +52,7 @@ virtual class PlanarProjectionFactor3 : gtsam::NoiseModelFactor { const gtsam::noiseModel::Base* model, size_t poseKey, size_t offsetKey, - size_t calibKey)); + size_t calibKey); void serialize() const; }; From 004cab1542c807e16218bc8cf85c4feb759ca8cb Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 17 Dec 2024 00:14:17 -0500 Subject: [PATCH 63/80] add serialize() to various preintegration classes --- gtsam/navigation/navigation.i | 28 +++++++++++++++++++--------- 1 file changed, 19 insertions(+), 9 deletions(-) diff --git a/gtsam/navigation/navigation.i b/gtsam/navigation/navigation.i index 831788073..3e7aa0fd6 100644 --- a/gtsam/navigation/navigation.i +++ b/gtsam/navigation/navigation.i @@ -77,6 +77,9 @@ virtual class PreintegratedRotationParams { std::optional getOmegaCoriolis() const; std::optional getBodyPSensor() const; + + // enabling serialization functionality + void serialize() const; }; #include @@ -170,22 +173,26 @@ virtual class PreintegrationCombinedParams : gtsam::PreintegrationParams { gtsam::Matrix getBiasAccCovariance() const ; gtsam::Matrix getBiasOmegaCovariance() const ; gtsam::Matrix getBiasAccOmegaInit() const; - + + // enabling serialization functionality + void serialize() const; }; class PreintegratedCombinedMeasurements { -// Constructors - PreintegratedCombinedMeasurements(const gtsam::PreintegrationCombinedParams* params); - PreintegratedCombinedMeasurements(const gtsam::PreintegrationCombinedParams* params, - const gtsam::imuBias::ConstantBias& bias); + // Constructors + PreintegratedCombinedMeasurements( + const gtsam::PreintegrationCombinedParams* params); + PreintegratedCombinedMeasurements( + const gtsam::PreintegrationCombinedParams* params, + const gtsam::imuBias::ConstantBias& bias); // Testable void print(string s = "Preintegrated Measurements:") const; bool equals(const gtsam::PreintegratedCombinedMeasurements& expected, - double tol); + double tol); // Standard Interface - void integrateMeasurement(gtsam::Vector measuredAcc, gtsam::Vector measuredOmega, - double deltaT); + void integrateMeasurement(gtsam::Vector measuredAcc, + gtsam::Vector measuredOmega, double deltaT); void resetIntegration(); void resetIntegrationAndSetBias(const gtsam::imuBias::ConstantBias& biasHat); @@ -197,7 +204,10 @@ class PreintegratedCombinedMeasurements { gtsam::imuBias::ConstantBias biasHat() const; gtsam::Vector biasHatVector() const; gtsam::NavState predict(const gtsam::NavState& state_i, - const gtsam::imuBias::ConstantBias& bias) const; + const gtsam::imuBias::ConstantBias& bias) const; + + // enabling serialization functionality + void serialize() const; }; virtual class CombinedImuFactor: gtsam::NoiseModelFactor { From c5554c836f79ec52bebadf31659dde5baf48d004 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 17 Dec 2024 00:15:09 -0500 Subject: [PATCH 64/80] python tests --- python/gtsam/tests/test_Serialization.py | 47 ++++++++++++++++++++++++ 1 file changed, 47 insertions(+) create mode 100644 python/gtsam/tests/test_Serialization.py diff --git a/python/gtsam/tests/test_Serialization.py b/python/gtsam/tests/test_Serialization.py new file mode 100644 index 000000000..be8dd7bec --- /dev/null +++ b/python/gtsam/tests/test_Serialization.py @@ -0,0 +1,47 @@ +""" +GTSAM Copyright 2010-2019, Georgia Tech Research Corporation, +Atlanta, Georgia 30332-0415 +All Rights Reserved + +See LICENSE for the license information + +KalmanFilter unit tests. +Author: Frank Dellaert & Duy Nguyen Ta (Python) +""" +import unittest +from copy import deepcopy + +import numpy as np +from gtsam.utils.test_case import GtsamTestCase + +import gtsam + + +class TestSerialization(GtsamTestCase): + """Tests for serialization of various GTSAM objects.""" + + def test_PreintegratedImuMeasurements(self): + """ + Test the serialization of `PreintegratedImuMeasurements` by performing a deepcopy. + """ + params = gtsam.PreintegrationParams(np.asarray([0, 0, -9.81])) + pim = gtsam.PreintegratedImuMeasurements(params) + + # If serialization failed, then this will throw an error + pim2 = deepcopy(pim) + self.assertEqual(pim, pim2) + + def test_PreintegratedCombinedMeasurements(self): + """ + Test the serialization of `PreintegratedCombinedMeasurements` by performing a deepcopy. + """ + params = gtsam.PreintegrationCombinedParams(np.asarray([0, 0, -9.81])) + pim = gtsam.PreintegratedCombinedMeasurements(params) + + # If serialization failed, then this will throw an error + pim2 = deepcopy(pim) + self.assertEqual(pim, pim2) + + +if __name__ == "__main__": + unittest.main() From dfcbba822e673fa60527409cf33db7b8731cf08b Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 17 Dec 2024 09:20:33 -0500 Subject: [PATCH 65/80] move default AHRSFactor constructor to public for wrapper --- gtsam/navigation/AHRSFactor.h | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/gtsam/navigation/AHRSFactor.h b/gtsam/navigation/AHRSFactor.h index 05439bafc..4289b4969 100644 --- a/gtsam/navigation/AHRSFactor.h +++ b/gtsam/navigation/AHRSFactor.h @@ -139,9 +139,6 @@ class GTSAM_EXPORT AHRSFactor: public NoiseModelFactorN { PreintegratedAhrsMeasurements _PIM_; - /** Default constructor - only use for serialization */ - AHRSFactor() {} - public: // Provide access to the Matrix& version of evaluateError: @@ -154,6 +151,9 @@ public: typedef std::shared_ptr shared_ptr; #endif + /** Default constructor - only use for serialization */ + AHRSFactor() {} + /** * Constructor * @param rot_i previous rot key From 8f77bfa13b9688358abc476f467afdb3c9940493 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 17 Dec 2024 09:22:47 -0500 Subject: [PATCH 66/80] serialize navigation factors --- gtsam/navigation/navigation.i | 29 +++++++++++++++++++++++- python/gtsam/tests/test_Serialization.py | 13 +++++++++++ 2 files changed, 41 insertions(+), 1 deletion(-) diff --git a/gtsam/navigation/navigation.i b/gtsam/navigation/navigation.i index 3e7aa0fd6..adb8bf2bb 100644 --- a/gtsam/navigation/navigation.i +++ b/gtsam/navigation/navigation.i @@ -151,6 +151,9 @@ virtual class ImuFactor: gtsam::NonlinearFactor { gtsam::Vector evaluateError(const gtsam::Pose3& pose_i, gtsam::Vector vel_i, const gtsam::Pose3& pose_j, gtsam::Vector vel_j, const gtsam::imuBias::ConstantBias& bias); + + // enable serialization functionality + void serialize() const; }; #include @@ -206,7 +209,7 @@ class PreintegratedCombinedMeasurements { gtsam::NavState predict(const gtsam::NavState& state_i, const gtsam::imuBias::ConstantBias& bias) const; - // enabling serialization functionality + // enable serialization functionality void serialize() const; }; @@ -221,6 +224,9 @@ virtual class CombinedImuFactor: gtsam::NoiseModelFactor { const gtsam::Pose3& pose_j, gtsam::Vector vel_j, const gtsam::imuBias::ConstantBias& bias_i, const gtsam::imuBias::ConstantBias& bias_j); + + // enable serialization functionality + void serialize() const; }; #include @@ -247,6 +253,9 @@ class PreintegratedAhrsMeasurements { // Standard Interface void integrateMeasurement(gtsam::Vector measuredOmega, double deltaT); void resetIntegration() ; + + // enable serialization functionality + void serialize() const; }; virtual class AHRSFactor : gtsam::NonlinearFactor { @@ -263,6 +272,9 @@ virtual class AHRSFactor : gtsam::NonlinearFactor { gtsam::Rot3 predict(const gtsam::Rot3& rot_i, gtsam::Vector bias, const gtsam::PreintegratedAhrsMeasurements& preintegratedMeasurements, gtsam::Vector omegaCoriolis) const; + + // enable serialization functionality + void serialize() const; }; #include @@ -276,6 +288,9 @@ virtual class Rot3AttitudeFactor : gtsam::NoiseModelFactor { bool equals(const gtsam::NonlinearFactor& expected, double tol) const; gtsam::Unit3 nRef() const; gtsam::Unit3 bMeasured() const; + + // enable serialization functionality + void serialize() const; }; virtual class Pose3AttitudeFactor : gtsam::NoiseModelFactor { @@ -290,6 +305,9 @@ virtual class Pose3AttitudeFactor : gtsam::NoiseModelFactor { bool equals(const gtsam::NonlinearFactor& expected, double tol) const; gtsam::Unit3 nRef() const; gtsam::Unit3 bMeasured() const; + + // enable serialization functionality + void serialize() const; }; #include @@ -304,6 +322,9 @@ virtual class GPSFactor : gtsam::NonlinearFactor{ // Standard Interface gtsam::Point3 measurementIn() const; + + // enable serialization functionality + void serialize() const; }; virtual class GPSFactor2 : gtsam::NonlinearFactor { @@ -317,6 +338,9 @@ virtual class GPSFactor2 : gtsam::NonlinearFactor { // Standard Interface gtsam::Point3 measurementIn() const; + + // enable serialization functionality + void serialize() const; }; #include @@ -334,6 +358,9 @@ virtual class BarometricFactor : gtsam::NonlinearFactor { const double& measurementIn() const; double heightOut(double n) const; double baroOut(const double& meters) const; + + // enable serialization functionality + void serialize() const; }; #include diff --git a/python/gtsam/tests/test_Serialization.py b/python/gtsam/tests/test_Serialization.py index be8dd7bec..3e22e6603 100644 --- a/python/gtsam/tests/test_Serialization.py +++ b/python/gtsam/tests/test_Serialization.py @@ -12,6 +12,7 @@ import unittest from copy import deepcopy import numpy as np +from gtsam.symbol_shorthand import B, V, X from gtsam.utils.test_case import GtsamTestCase import gtsam @@ -31,6 +32,18 @@ class TestSerialization(GtsamTestCase): pim2 = deepcopy(pim) self.assertEqual(pim, pim2) + def test_ImuFactor(self): + """ + Test the serialization of `ImuFactor` by performing a deepcopy. + """ + params = gtsam.PreintegrationParams(np.asarray([0, 0, -9.81])) + pim = gtsam.PreintegratedImuMeasurements(params) + imu_odom = gtsam.ImuFactor(X(0), V(0), X(1), V(1), B(0), pim) + + # If serialization failed, then this will throw an error + imu_odom2 = deepcopy(imu_odom) + self.assertEqual(imu_odom, imu_odom2) + def test_PreintegratedCombinedMeasurements(self): """ Test the serialization of `PreintegratedCombinedMeasurements` by performing a deepcopy. From 60b00ebdda6da38bf26c35d112cee6d964bd5da8 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 17 Dec 2024 10:08:19 -0500 Subject: [PATCH 67/80] improve testing for deep copy --- python/gtsam/tests/test_Serialization.py | 30 ++++++++++-------------- python/gtsam/tests/test_pickle.py | 10 ++++---- python/gtsam/utils/test_case.py | 16 +++++++++++-- 3 files changed, 32 insertions(+), 24 deletions(-) diff --git a/python/gtsam/tests/test_Serialization.py b/python/gtsam/tests/test_Serialization.py index 3e22e6603..e935f1f62 100644 --- a/python/gtsam/tests/test_Serialization.py +++ b/python/gtsam/tests/test_Serialization.py @@ -5,11 +5,11 @@ All Rights Reserved See LICENSE for the license information -KalmanFilter unit tests. -Author: Frank Dellaert & Duy Nguyen Ta (Python) +Serialization and deep copy tests. + +Author: Varun Agrawal """ import unittest -from copy import deepcopy import numpy as np from gtsam.symbol_shorthand import B, V, X @@ -18,42 +18,36 @@ from gtsam.utils.test_case import GtsamTestCase import gtsam -class TestSerialization(GtsamTestCase): - """Tests for serialization of various GTSAM objects.""" +class TestDeepCopy(GtsamTestCase): + """Tests for deep copy of various GTSAM objects.""" def test_PreintegratedImuMeasurements(self): """ - Test the serialization of `PreintegratedImuMeasurements` by performing a deepcopy. + Test the deep copy of `PreintegratedImuMeasurements` by performing a deepcopy. """ params = gtsam.PreintegrationParams(np.asarray([0, 0, -9.81])) pim = gtsam.PreintegratedImuMeasurements(params) - # If serialization failed, then this will throw an error - pim2 = deepcopy(pim) - self.assertEqual(pim, pim2) + self.assertDeepCopyEquality(pim) def test_ImuFactor(self): """ - Test the serialization of `ImuFactor` by performing a deepcopy. + Test the deep copy of `ImuFactor` by performing a deepcopy. """ params = gtsam.PreintegrationParams(np.asarray([0, 0, -9.81])) pim = gtsam.PreintegratedImuMeasurements(params) - imu_odom = gtsam.ImuFactor(X(0), V(0), X(1), V(1), B(0), pim) + imu_factor = gtsam.ImuFactor(X(0), V(0), X(1), V(1), B(0), pim) - # If serialization failed, then this will throw an error - imu_odom2 = deepcopy(imu_odom) - self.assertEqual(imu_odom, imu_odom2) + self.assertDeepCopyEquality(imu_factor) def test_PreintegratedCombinedMeasurements(self): """ - Test the serialization of `PreintegratedCombinedMeasurements` by performing a deepcopy. + Test the deep copy of `PreintegratedCombinedMeasurements` by performing a deepcopy. """ params = gtsam.PreintegrationCombinedParams(np.asarray([0, 0, -9.81])) pim = gtsam.PreintegratedCombinedMeasurements(params) - # If serialization failed, then this will throw an error - pim2 = deepcopy(pim) - self.assertEqual(pim, pim2) + self.assertDeepCopyEquality(pim) if __name__ == "__main__": diff --git a/python/gtsam/tests/test_pickle.py b/python/gtsam/tests/test_pickle.py index a6a5745bc..e51617b00 100644 --- a/python/gtsam/tests/test_pickle.py +++ b/python/gtsam/tests/test_pickle.py @@ -9,24 +9,26 @@ Unit tests to check pickling. Author: Ayush Baid """ -from gtsam import Cal3Bundler, PinholeCameraCal3Bundler, Point2, Point3, Pose3, Rot3, SfmTrack, Unit3 - from gtsam.utils.test_case import GtsamTestCase +from gtsam import (Cal3Bundler, PinholeCameraCal3Bundler, Point2, Point3, + Pose3, Rot3, SfmTrack, Unit3) + + class TestPickle(GtsamTestCase): """Tests pickling on some of the classes.""" def test_cal3Bundler_roundtrip(self): obj = Cal3Bundler(fx=100, k1=0.1, k2=0.2, u0=100, v0=70) self.assertEqualityOnPickleRoundtrip(obj) - + def test_pinholeCameraCal3Bundler_roundtrip(self): obj = PinholeCameraCal3Bundler( Pose3(Rot3.RzRyRx(0, 0.1, -0.05), Point3(1, 1, 0)), Cal3Bundler(fx=100, k1=0.1, k2=0.2, u0=100, v0=70), ) self.assertEqualityOnPickleRoundtrip(obj) - + def test_rot3_roundtrip(self): obj = Rot3.RzRyRx(0, 0.05, 0.1) self.assertEqualityOnPickleRoundtrip(obj) diff --git a/python/gtsam/utils/test_case.py b/python/gtsam/utils/test_case.py index 50af004f4..74eaff1db 100644 --- a/python/gtsam/utils/test_case.py +++ b/python/gtsam/utils/test_case.py @@ -10,6 +10,7 @@ Author: Frank Dellaert """ import pickle import unittest +from copy import deepcopy class GtsamTestCase(unittest.TestCase): @@ -28,8 +29,8 @@ class GtsamTestCase(unittest.TestCase): else: equal = actual.equals(expected, tol) if not equal: - raise self.failureException( - "Values are not equal:\n{}!={}".format(actual, expected)) + raise self.failureException("Values are not equal:\n{}!={}".format( + actual, expected)) def assertEqualityOnPickleRoundtrip(self, obj: object, tol=1e-9) -> None: """ Performs a round-trip using pickle and asserts equality. @@ -41,3 +42,14 @@ class GtsamTestCase(unittest.TestCase): """ roundTripObj = pickle.loads(pickle.dumps(obj)) self.gtsamAssertEquals(roundTripObj, obj) + + def assertDeepCopyEquality(self, obj): + """Perform assertion by checking if a + deep copied version of `obj` is equal to itself. + + Args: + obj: The object to check is deep-copyable. + """ + # If deep copy failed, then this will throw an error + obj2 = deepcopy(obj) + self.gtsamAssertEquals(obj, obj2) From e4538d5b3e4ecddfabc8c8d81dc09553772f7147 Mon Sep 17 00:00:00 2001 From: Joel Truher Date: Tue, 17 Dec 2024 22:39:13 -0800 Subject: [PATCH 68/80] address some comments --- gtsam/geometry/Pose3.cpp | 11 ++ gtsam/geometry/Pose3.h | 2 + gtsam/slam/PlanarProjectionFactor.h | 168 +++++++++--------- gtsam/slam/slam.i | 36 ++-- .../slam/tests/testPlanarProjectionFactor.cpp | 24 ++- 5 files changed, 132 insertions(+), 109 deletions(-) diff --git a/gtsam/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp index bb1483432..5d7732947 100644 --- a/gtsam/geometry/Pose3.cpp +++ b/gtsam/geometry/Pose3.cpp @@ -42,6 +42,17 @@ Pose3 Pose3::Create(const Rot3& R, const Point3& t, OptionalJacobian<6, 3> HR, return Pose3(R, t); } +Pose3 Pose3::FromPose2(const Pose2& p, OptionalJacobian<6, 3> H) { + if (H) *H << (gtsam::Matrix(6, 3) << // + 0., 0., 0., // + 0., 0., 0.,// + 0., 0., 1.,// + 1., 0., 0.,// + 0., 1., 0.,// + 0., 0., 0.).finished(); + return Pose3(p); +} + /* ************************************************************************* */ Pose3 Pose3::inverse() const { Rot3 Rt = R_.inverse(); diff --git a/gtsam/geometry/Pose3.h b/gtsam/geometry/Pose3.h index d7c280c80..baac21ccb 100644 --- a/gtsam/geometry/Pose3.h +++ b/gtsam/geometry/Pose3.h @@ -78,6 +78,8 @@ public: OptionalJacobian<6, 3> HR = {}, OptionalJacobian<6, 3> Ht = {}); + static Pose3 FromPose2(const Pose2& p, OptionalJacobian<6,3> H = {}); + /** * Create Pose3 by aligning two point pairs * A pose aTb is estimated between pairs (a_point, b_point) such that a_point = aTb * b_point diff --git a/gtsam/slam/PlanarProjectionFactor.h b/gtsam/slam/PlanarProjectionFactor.h index 285b64900..21f24da93 100644 --- a/gtsam/slam/PlanarProjectionFactor.h +++ b/gtsam/slam/PlanarProjectionFactor.h @@ -38,69 +38,67 @@ namespace gtsam { PlanarProjectionFactorBase() {} /** - * @param measured corresponding point in the camera frame - * @param poseKey index of the robot pose in the z=0 plane + * @param measured pixels in the camera frame */ PlanarProjectionFactorBase(const Point2& measured) : measured_(measured) {} /** - * @param landmark point3 - * @param pose - * @param offset oriented parallel to pose2 zero i.e. +x - * @param calib - * @param H1 landmark jacobian - * @param H2 pose jacobian - * @param H3 offset jacobian - * @param H4 calib jacobian + * Predict the projection of the landmark in camera pixels. + * + * @param landmark point3 of the target + * @param wTb "world to body": planar pose2 of vehicle body frame in world frame + * @param bTc "body to camera": camera pose in body frame, oriented parallel to pose2 zero i.e. +x + * @param calib camera calibration with distortion + * @param Hlandmark jacobian + * @param HwTb jacobian + * @param HbTc jacobian + * @param Hcalib jacobian */ - Point2 h( + Point2 predict( const Point3& landmark, - const Pose2& pose, - const Pose3& offset, + const Pose2& wTb, + const Pose3& bTc, const Cal3DS2& calib, - OptionalMatrixType H1, // 2x3 (x, y, z) - OptionalMatrixType H2, // 2x3 (x, y, theta) - OptionalMatrixType H3, // 2x6 (rx, ry, rz, x, y, theta) - OptionalMatrixType H4 // 2x9 + OptionalMatrixType Hlandmark = nullptr, // 2x3 (x, y, z) + OptionalMatrixType HwTb = nullptr, // 2x3 (x, y, theta) + OptionalMatrixType HbTc = nullptr, // 2x6 (rx, ry, rz, x, y, theta) + OptionalMatrixType Hcalib = nullptr // 2x9 ) const { +#ifndef GTSAM_THROW_CHEIRALITY_EXCEPTION try { - // this is x-forward z-up +#endif + gtsam::Matrix Hp; // 6x3 gtsam::Matrix H0; // 6x6 - Pose3 offset_pose = Pose3(pose).compose(offset, H0); + // this is x-forward z-up + Pose3 wTc = Pose3::FromPose2(wTb, Hp).compose(bTc, HwTb ? &H0 : nullptr); // this is z-forward y-down gtsam::Matrix H00; // 6x6 - Pose3 camera_pose = offset_pose.compose(CAM_COORD, H00); + Pose3 camera_pose = wTc.compose(CAM_COORD, H00); PinholeCamera camera = PinholeCamera(camera_pose, calib); - if (H2 || H3) { + if (HwTb || HbTc) { // Dpose is for pose3, 2x6 (R,t) gtsam::Matrix Dpose; - Point2 result = camera.project(landmark, Dpose, H1, H4); + Point2 result = camera.project(landmark, Dpose, Hlandmark, Hcalib); gtsam::Matrix DposeOffset = Dpose * H00; // 2x6 - if (H3) - *H3 = DposeOffset; // with Eigen this is a deep copy (!) - if (H2) { - gtsam::Matrix DposeOffsetFwd = DposeOffset * H0; - *H2 = Matrix::Zero(2, 3); - (*H2)(0, 0) = DposeOffsetFwd(0, 3); // du/dx - (*H2)(1, 0) = DposeOffsetFwd(1, 3); // dv/dx - (*H2)(0, 1) = DposeOffsetFwd(0, 4); // du/dy - (*H2)(1, 1) = DposeOffsetFwd(1, 4); // dv/dy - (*H2)(0, 2) = DposeOffsetFwd(0, 2); // du/dyaw - (*H2)(1, 2) = DposeOffsetFwd(1, 2); // dv/dyaw - } + if (HbTc) + *HbTc = DposeOffset; // with Eigen this is a deep copy (!) + if (HwTb) + *HwTb = DposeOffset * H0 * Hp; return result; } else { return camera.project(landmark, {}, {}, {}); } +#ifndef GTSAM_THROW_CHEIRALITY_EXCEPTION } catch (CheiralityException& e) { std::cout << "****** CHIRALITY EXCEPTION ******\n"; - if (H1) *H1 = Matrix::Zero(2, 3); - if (H2) *H2 = Matrix::Zero(2, 3); - if (H3) *H3 = Matrix::Zero(2, 6); - if (H4) *H4 = Matrix::Zero(2, 9); + if (Hlandmark) *Hlandmark = Matrix::Zero(2, 3); + if (HwTb) *HwTb = Matrix::Zero(2, 3); + if (HbTc) *HbTc = Matrix::Zero(2, 6); + if (Hcalib) *Hcalib = Matrix::Zero(2, 9); // return a large error return Matrix::Constant(2, 1, 2.0 * calib.fx()); } +#endif } Point2 measured_; // pixel measurement @@ -140,41 +138,42 @@ namespace gtsam { /** + * @brief constructor for known landmark, offset, and calibration + * @param poseKey index of the robot pose2 in the z=0 plane * @param landmark point3 in the world * @param measured corresponding point2 in the camera frame - * @param offset constant camera offset from pose + * @param bTc "body to camera": constant camera offset from pose * @param calib constant camera calibration * @param model stddev of the measurements, ~one pixel? - * @param poseKey index of the robot pose2 in the z=0 plane */ PlanarProjectionFactor1( + Key poseKey, const Point3& landmark, const Point2& measured, - const Pose3& offset, + const Pose3& bTc, const Cal3DS2& calib, - const SharedNoiseModel& model, - Key poseKey) + const SharedNoiseModel& model) : PlanarProjectionFactorBase(measured), NoiseModelFactorN(model, poseKey), landmark_(landmark), - offset_(offset), + bTc_(bTc), calib_(calib) { assert(2 == model->dim()); } /** - * @param pose estimated pose2 - * @param H1 pose jacobian + * @param wTb "world to body": estimated pose2 + * @param HwTb jacobian */ Vector evaluateError( - const Pose2& pose, - OptionalMatrixType H1 = OptionalNone) const override { - return h(landmark_, pose, offset_, calib_, {}, H1, {}, {}) - measured_; + const Pose2& wTb, + OptionalMatrixType HwTb = OptionalNone) const override { + return predict(landmark_, wTb, bTc_, calib_, {}, HwTb, {}, {}) - measured_; } private: Point3 landmark_; // landmark - Pose3 offset_; // camera offset to robot pose + Pose3 bTc_; // "body to camera": camera offset to robot pose Cal3DS2 calib_; // camera calibration }; @@ -188,9 +187,10 @@ namespace gtsam { * Camera offset and calibration are constant. * This is similar to GeneralSFMFactor, used for SLAM. */ - class PlanarProjectionFactor2 : public PlanarProjectionFactorBase, public NoiseModelFactorN { + class PlanarProjectionFactor2 + : public PlanarProjectionFactorBase, public NoiseModelFactorN { public: - typedef NoiseModelFactorN Base; + typedef NoiseModelFactorN Base; using Base::evaluateError; PlanarProjectionFactor2() {} @@ -204,43 +204,44 @@ namespace gtsam { } /** - * @param measured corresponding point in the camera frame - * @param offset constant camera offset from pose - * @param calib constant camera calibration - * @param model stddev of the measurements, ~one pixel? + * @brief constructor for variable landmark, known offset and calibration * @param poseKey index of the robot pose2 in the z=0 plane * @param landmarkKey index of the landmark point3 + * @param measured corresponding point in the camera frame + * @param bTc "body to camera": constant camera offset from pose + * @param calib constant camera calibration + * @param model stddev of the measurements, ~one pixel? */ PlanarProjectionFactor2( - const Point2& measured, - const Pose3& offset, - const Cal3DS2& calib, - const SharedNoiseModel& model, + Key poseKey, Key landmarkKey, - Key poseKey) + const Point2& measured, + const Pose3& bTc, + const Cal3DS2& calib, + const SharedNoiseModel& model) : PlanarProjectionFactorBase(measured), NoiseModelFactorN(model, landmarkKey, poseKey), - offset_(offset), + bTc_(bTc), calib_(calib) { assert(2 == model->dim()); } /** + * @param wTb "world to body": estimated pose2 * @param landmark estimated landmark - * @param pose estimated pose2 - * @param H1 landmark jacobian - * @param H2 pose jacobian + * @param HwTb jacobian + * @param Hlandmark jacobian */ Vector evaluateError( + const Pose2& wTb, const Point3& landmark, - const Pose2& pose, - OptionalMatrixType H1 = OptionalNone, - OptionalMatrixType H2 = OptionalNone) const override { - return h(landmark, pose, offset_, calib_, H1, H2, {}, {}) - measured_; + OptionalMatrixType HwTb = OptionalNone, + OptionalMatrixType Hlandmark = OptionalNone) const override { + return predict(landmark, wTb, bTc_, calib_, Hlandmark, HwTb, {}, {}) - measured_; } private: - Pose3 offset_; // camera offset to robot pose + Pose3 bTc_; // "body to camera": camera offset to robot pose Cal3DS2 calib_; // camera calibration }; @@ -270,6 +271,7 @@ namespace gtsam { } /** + * @brief constructor for variable pose, offset, and calibration, known landmark. * @param landmark point3 in the world * @param measured corresponding point2 in the camera frame * @param model stddev of the measurements, ~one pixel? @@ -277,12 +279,12 @@ namespace gtsam { * @param offsetKey index of camera offset from pose * @param calibKey index of camera calibration */ PlanarProjectionFactor3( - const Point3& landmark, - const Point2& measured, - const SharedNoiseModel& model, Key poseKey, Key offsetKey, - Key calibKey) + Key calibKey, + const Point3& landmark, + const Point2& measured, + const SharedNoiseModel& model) : PlanarProjectionFactorBase(measured), NoiseModelFactorN(model, poseKey, offsetKey, calibKey), landmark_(landmark) { @@ -290,21 +292,21 @@ namespace gtsam { } /** - * @param pose estimated pose2 - * @param offset pose3 offset from pose2 +x + * @param wTb "world to body": estimated pose2 + * @param bTc "body to camera": pose3 offset from pose2 +x * @param calib calibration - * @param H1 pose jacobian + * @param HwTb pose jacobian * @param H2 offset jacobian * @param H3 calibration jacobian */ Vector evaluateError( - const Pose2& pose, - const Pose3& offset, + const Pose2& wTb, + const Pose3& bTc, const Cal3DS2& calib, - OptionalMatrixType H1 = OptionalNone, - OptionalMatrixType H2 = OptionalNone, - OptionalMatrixType H3 = OptionalNone) const override { - return h(landmark_, pose, offset, calib, {}, H1, H2, H3) - measured_; + OptionalMatrixType HwTb = OptionalNone, + OptionalMatrixType HbTc = OptionalNone, + OptionalMatrixType Hcalib = OptionalNone) const override { + return predict(landmark_, wTb, bTc, calib, {}, HwTb, HbTc, Hcalib) - measured_; } private: diff --git a/gtsam/slam/slam.i b/gtsam/slam/slam.i index e141cb50c..1761a6cc3 100644 --- a/gtsam/slam/slam.i +++ b/gtsam/slam/slam.i @@ -27,32 +27,32 @@ virtual class BetweenFactor : gtsam::NoiseModelFactor { #include virtual class PlanarProjectionFactor1 : gtsam::NoiseModelFactor { PlanarProjectionFactor1( - const gtsam::Point3& landmark, - const gtsam::Point2& measured, - const gtsam::Pose3& offset, - const gtsam::Cal3DS2& calib, - const gtsam::noiseModel::Base* model, - size_t poseKey); + size_t poseKey, + const gtsam::Point3& landmark, + const gtsam::Point2& measured, + const gtsam::Pose3& bTc, + const gtsam::Cal3DS2& calib, + const gtsam::noiseModel::Base* model); void serialize() const; }; virtual class PlanarProjectionFactor2 : gtsam::NoiseModelFactor { PlanarProjectionFactor2( - const gtsam::Point2& measured, - const gtsam::Pose3& offset, - const gtsam::Cal3DS2& calib, - const gtsam::noiseModel::Base* model, - size_t landmarkKey, - size_t poseKey); + size_t poseKey, + size_t landmarkKey, + const gtsam::Point2& measured, + const gtsam::Pose3& bTc, + const gtsam::Cal3DS2& calib, + const gtsam::noiseModel::Base* model); void serialize() const; }; virtual class PlanarProjectionFactor3 : gtsam::NoiseModelFactor { PlanarProjectionFactor3( - const gtsam::Point3& landmark, - const gtsam::Point2& measured, - const gtsam::noiseModel::Base* model, - size_t poseKey, - size_t offsetKey, - size_t calibKey); + size_t poseKey, + size_t offsetKey, + size_t calibKey, + const gtsam::Point3& landmark, + const gtsam::Point2& measured, + const gtsam::noiseModel::Base* model); void serialize() const; }; diff --git a/gtsam/slam/tests/testPlanarProjectionFactor.cpp b/gtsam/slam/tests/testPlanarProjectionFactor.cpp index 03a0b7bed..67ebd5c25 100644 --- a/gtsam/slam/tests/testPlanarProjectionFactor.cpp +++ b/gtsam/slam/tests/testPlanarProjectionFactor.cpp @@ -39,7 +39,8 @@ TEST(PlanarProjectionFactor1, error1) { Pose2 pose(0, 0, 0); values.insert(X(0), pose); - PlanarProjectionFactor1 factor(landmark, measured, offset, calib, model, X(0)); + PlanarProjectionFactor1 factor( + X(0), landmark, measured, offset, calib, model); CHECK_EQUAL(2, factor.dim()); CHECK(factor.active(values)); @@ -65,7 +66,8 @@ TEST(PlanarProjectionFactor1, error2) { Pose3 offset; Cal3DS2 calib(200, 200, 0, 200, 200, 0, 0); SharedNoiseModel model = noiseModel::Diagonal::Sigmas(Vector2(1, 1)); - PlanarProjectionFactor1 factor(landmark, measured, offset, calib, model, X(0)); + PlanarProjectionFactor1 factor( + X(0), landmark, measured, offset, calib, model); Values values; Pose2 pose(0, 0, 0); @@ -96,7 +98,8 @@ TEST(PlanarProjectionFactor1, error3) { // distortion Cal3DS2 calib(200, 200, 0, 200, 200, -0.2, 0.1); SharedNoiseModel model = noiseModel::Diagonal::Sigmas(Vector2(1, 1)); - PlanarProjectionFactor1 factor(landmark, measured, offset, calib, model, X(0)); + PlanarProjectionFactor1 factor( + X(0), landmark, measured, offset, calib, model); Values values; Pose2 pose(0, 0, 0); @@ -131,7 +134,8 @@ TEST(PlanarProjectionFactor1, jacobian) { Pose3 offset(Rot3::Ypr(s(g),s(g),s(g)), Point3(s(g),s(g),s(g))); Cal3DS2 calib(200, 200, 0, 200, 200, -0.2, 0.1); - PlanarProjectionFactor1 factor(landmark, measured, offset, calib, model, X(0)); + PlanarProjectionFactor1 factor( + X(0), landmark, measured, offset, calib, model); Pose2 pose(s(g), s(g), s(g)); @@ -164,7 +168,8 @@ TEST(PlanarProjectionFactor3, error1) { values.insert(C(0), offset); values.insert(K(0), calib); - PlanarProjectionFactor3 factor(landmark, measured, model, X(0), C(0), K(0)); + PlanarProjectionFactor3 factor( + X(0), C(0), K(0), landmark, measured, model); CHECK_EQUAL(2, factor.dim()); CHECK(factor.active(values)); @@ -213,7 +218,8 @@ TEST(PlanarProjectionFactor3, error2) { Cal3DS2 calib(200, 200, 0, 200, 200, 0, 0); SharedNoiseModel model = noiseModel::Diagonal::Sigmas(Vector2(1, 1)); - PlanarProjectionFactor3 factor(landmark, measured, model, X(0), C(0), K(0)); + PlanarProjectionFactor3 factor( + X(0), C(0), K(0), landmark, measured, model); Values values; Pose2 pose(0, 0, 0); @@ -267,7 +273,8 @@ TEST(PlanarProjectionFactor3, error3) { Cal3DS2 calib(200, 200, 0, 200, 200, -0.2, 0.1); SharedNoiseModel model = noiseModel::Diagonal::Sigmas(Vector2(1, 1)); - PlanarProjectionFactor3 factor(landmark, measured, model, X(0), C(0), K(0)); + PlanarProjectionFactor3 factor( + X(0), C(0), K(0), landmark, measured, model); Values values; Pose2 pose(0, 0, 0); @@ -328,7 +335,8 @@ TEST(PlanarProjectionFactor3, jacobian) { Pose3 offset(Rot3::Ypr(s(g),s(g),s(g)), Point3(s(g),s(g),s(g))); Cal3DS2 calib(200, 200, 0, 200, 200, -0.2, 0.1); - PlanarProjectionFactor3 factor(landmark, measured, model, X(0), C(0), K(0)); + PlanarProjectionFactor3 factor( + X(0), C(0), K(0), landmark, measured, model); Pose2 pose(s(g), s(g), s(g)); From 3690937159aa79de1570f4cac8db0ac53f8235c0 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Wed, 18 Dec 2024 08:11:42 -0500 Subject: [PATCH 69/80] add comments --- gtsam/discrete/TableFactor.cpp | 12 ++++++++++-- 1 file changed, 10 insertions(+), 2 deletions(-) diff --git a/gtsam/discrete/TableFactor.cpp b/gtsam/discrete/TableFactor.cpp index d9bfc42c2..22548da07 100644 --- a/gtsam/discrete/TableFactor.cpp +++ b/gtsam/discrete/TableFactor.cpp @@ -252,6 +252,11 @@ DecisionTreeFactor TableFactor::operator*(const DecisionTreeFactor& f) const { DecisionTreeFactor TableFactor::toDecisionTreeFactor() const { DiscreteKeys dkeys = discreteKeys(); + // Record key assignment and value pairs in pair_table. + // The assignments are stored in descending order of keys so that the order of + // the values matches what is expected by a DecisionTree. + // This is why we reverse the keys and then + // query for the key value/assignment. DiscreteKeys rdkeys(dkeys.rbegin(), dkeys.rend()); std::vector> pair_table; for (auto i = 0; i < sparse_table_.size(); i++) { @@ -265,13 +270,16 @@ DecisionTreeFactor TableFactor::toDecisionTreeFactor() const { pair_table.push_back(std::make_pair(k, sparse_table_.coeff(i))); } - // Sort based on key assignment so we get values in reverse key order. + // Sort the pair_table (of assignment-value pairs) based on assignment so we + // get values in reverse key order. std::sort( pair_table.begin(), pair_table.end(), [](const std::pair& a, const std::pair& b) { return a.first < b.first; }); - // Create the table vector + // Create the table vector by extracting the values from pair_table. + // The pair_table has already been sorted in the desired order, + // so the values will be in descending key order. std::vector table; std::for_each(pair_table.begin(), pair_table.end(), [&table](const std::pair& pair) { From 8a30aacf6ca997df0073d633cb3eaa4ff0a5510b Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sat, 21 Dec 2024 13:10:29 -0500 Subject: [PATCH 70/80] fix serialization of ImuFactor --- python/gtsam/tests/test_Serialization.py | 26 ++++++++++++++++-------- 1 file changed, 18 insertions(+), 8 deletions(-) diff --git a/python/gtsam/tests/test_Serialization.py b/python/gtsam/tests/test_Serialization.py index e935f1f62..4f5c57b0a 100644 --- a/python/gtsam/tests/test_Serialization.py +++ b/python/gtsam/tests/test_Serialization.py @@ -23,26 +23,36 @@ class TestDeepCopy(GtsamTestCase): def test_PreintegratedImuMeasurements(self): """ - Test the deep copy of `PreintegratedImuMeasurements` by performing a deepcopy. + Test the deep copy of `PreintegratedImuMeasurements`. """ - params = gtsam.PreintegrationParams(np.asarray([0, 0, -9.81])) + params = gtsam.PreintegrationParams.MakeSharedD(9.81) pim = gtsam.PreintegratedImuMeasurements(params) self.assertDeepCopyEquality(pim) def test_ImuFactor(self): """ - Test the deep copy of `ImuFactor` by performing a deepcopy. + Test the deep copy of `ImuFactor`. """ - params = gtsam.PreintegrationParams(np.asarray([0, 0, -9.81])) - pim = gtsam.PreintegratedImuMeasurements(params) - imu_factor = gtsam.ImuFactor(X(0), V(0), X(1), V(1), B(0), pim) + params = gtsam.PreintegrationParams.MakeSharedD(9.81) + params.setAccelerometerCovariance(1e-7 * np.eye(3)) + params.setGyroscopeCovariance(1e-8 * np.eye(3)) + params.setIntegrationCovariance(1e-9 * np.eye(3)) + priorBias = gtsam.imuBias.ConstantBias(np.zeros(3), np.zeros(3)) + pim = gtsam.PreintegratedImuMeasurements(params, priorBias) - self.assertDeepCopyEquality(imu_factor) + # Preintegrate a single measurement for serialization to work. + pim.integrateMeasurement(measuredAcc=np.zeros(3), + measuredOmega=np.zeros(3), + deltaT=0.005) + + factor = gtsam.ImuFactor(0, 1, 2, 3, 4, pim) + + self.assertDeepCopyEquality(factor) def test_PreintegratedCombinedMeasurements(self): """ - Test the deep copy of `PreintegratedCombinedMeasurements` by performing a deepcopy. + Test the deep copy of `PreintegratedCombinedMeasurements`. """ params = gtsam.PreintegrationCombinedParams(np.asarray([0, 0, -9.81])) pim = gtsam.PreintegratedCombinedMeasurements(params) From 60506e2440e825fb7f41ef2331c1993df512ccd3 Mon Sep 17 00:00:00 2001 From: Matt Date: Sun, 22 Dec 2024 09:25:06 -0800 Subject: [PATCH 71/80] [cephes] use GTSAM_LIBRARY_TYPE --- gtsam/3rdparty/cephes/CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/3rdparty/cephes/CMakeLists.txt b/gtsam/3rdparty/cephes/CMakeLists.txt index 190b73db9..aeac7b403 100644 --- a/gtsam/3rdparty/cephes/CMakeLists.txt +++ b/gtsam/3rdparty/cephes/CMakeLists.txt @@ -89,7 +89,7 @@ set(CEPHES_SOURCES cephes/zetac.c) # Add library source files -add_library(cephes-gtsam SHARED ${CEPHES_SOURCES}) +add_library(cephes-gtsam ${GTSAM_LIBRARY_TYPE} ${CEPHES_SOURCES}) # Add include directory (aka headers) target_include_directories( From 3fb1b214524e22c000df0ed7155890c6f1c07a76 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sun, 22 Dec 2024 23:48:10 -0500 Subject: [PATCH 72/80] remove add_definitions since it is deprecated and suboptimal --- CMakeLists.txt | 7 ------- 1 file changed, 7 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index e3b462eec..90162fe29 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -71,14 +71,7 @@ include(GtsamPrinting) ############### Decide on BOOST ###################################### # Enable or disable serialization with GTSAM_ENABLE_BOOST_SERIALIZATION option(GTSAM_ENABLE_BOOST_SERIALIZATION "Enable Boost serialization" ON) -if(GTSAM_ENABLE_BOOST_SERIALIZATION) - add_definitions(-DGTSAM_ENABLE_BOOST_SERIALIZATION) -endif() - option(GTSAM_USE_BOOST_FEATURES "Enable Features that use Boost" ON) -if(GTSAM_USE_BOOST_FEATURES) - add_definitions(-DGTSAM_USE_BOOST_FEATURES) -endif() if(GTSAM_ENABLE_BOOST_SERIALIZATION OR GTSAM_USE_BOOST_FEATURES) include(cmake/HandleBoost.cmake) From 51f3c6dea3d76896897bdf1ca50d8ca97e322b65 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sun, 22 Dec 2024 23:48:50 -0500 Subject: [PATCH 73/80] Optimize build time by adding Boost definitions in config.h --- gtsam/base/FastSet.h | 2 ++ gtsam/base/serialization.h | 5 ++++- gtsam/base/serializationTestHelpers.h | 5 ++++- gtsam/config.h.in | 4 ++++ 4 files changed, 14 insertions(+), 2 deletions(-) diff --git a/gtsam/base/FastSet.h b/gtsam/base/FastSet.h index a1be08d80..58ddd13a0 100644 --- a/gtsam/base/FastSet.h +++ b/gtsam/base/FastSet.h @@ -18,6 +18,8 @@ #pragma once +#include + #ifdef GTSAM_ENABLE_BOOST_SERIALIZATION #include #if BOOST_VERSION >= 107400 diff --git a/gtsam/base/serialization.h b/gtsam/base/serialization.h index 18612bc22..e829014ae 100644 --- a/gtsam/base/serialization.h +++ b/gtsam/base/serialization.h @@ -17,9 +17,12 @@ * @date Feb 7, 2012 */ -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION #pragma once +#include + +#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION + #include #include #include diff --git a/gtsam/base/serializationTestHelpers.h b/gtsam/base/serializationTestHelpers.h index 09b36cdc8..fb0701d89 100644 --- a/gtsam/base/serializationTestHelpers.h +++ b/gtsam/base/serializationTestHelpers.h @@ -17,9 +17,12 @@ * @date Feb 7, 2012 */ -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION #pragma once +#include + +#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION + #include #include #include diff --git a/gtsam/config.h.in b/gtsam/config.h.in index 719cbc6cb..8b4903d3a 100644 --- a/gtsam/config.h.in +++ b/gtsam/config.h.in @@ -88,3 +88,7 @@ #cmakedefine GTSAM_SLOW_BUT_CORRECT_BETWEENFACTOR #cmakedefine GTSAM_SLOW_BUT_CORRECT_EXPMAP + +// Boost flags +#cmakedefine01 GTSAM_ENABLE_BOOST_SERIALIZATION +#cmakedefine01 GTSAM_USE_BOOST_FEATURES From 427d54dce8d03c89e1f40a26bd9dad346eb4a5b6 Mon Sep 17 00:00:00 2001 From: "dependabot[bot]" <49699333+dependabot[bot]@users.noreply.github.com> Date: Tue, 24 Dec 2024 00:30:15 +0000 Subject: [PATCH 74/80] Bump jinja2 from 3.1.4 to 3.1.5 in /wrap/pybind11/docs Bumps [jinja2](https://github.com/pallets/jinja) from 3.1.4 to 3.1.5. - [Release notes](https://github.com/pallets/jinja/releases) - [Changelog](https://github.com/pallets/jinja/blob/main/CHANGES.rst) - [Commits](https://github.com/pallets/jinja/compare/3.1.4...3.1.5) --- updated-dependencies: - dependency-name: jinja2 dependency-type: indirect ... Signed-off-by: dependabot[bot] --- wrap/pybind11/docs/requirements.txt | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/wrap/pybind11/docs/requirements.txt b/wrap/pybind11/docs/requirements.txt index 293db6a06..8dffd36b5 100644 --- a/wrap/pybind11/docs/requirements.txt +++ b/wrap/pybind11/docs/requirements.txt @@ -130,9 +130,9 @@ imagesize==1.4.1 \ --hash=sha256:0d8d18d08f840c19d0ee7ca1fd82490fdc3729b7ac93f49870406ddde8ef8d8b \ --hash=sha256:69150444affb9cb0d5cc5a92b3676f0b2fb7cd9ae39e947a5e11a36b4497cd4a # via sphinx -jinja2==3.1.4 \ - --hash=sha256:4a3aee7acbbe7303aede8e9648d13b8bf88a429282aa6122a993f0ac800cb369 \ - --hash=sha256:bc5dd2abb727a5319567b7a813e6a2e7318c39f4f487cfe6c89c6f9c7d25197d +jinja2==3.1.5 \ + --hash=sha256:8fefff8dc3034e27bb80d67c671eb8a9bc424c0ef4c0826edbff304cceff43bb \ + --hash=sha256:aba0f4dc9ed8013c424088f68a5c226f7d6097ed89b246d7749c2ec4175c6adb # via sphinx markupsafe==2.1.5 \ --hash=sha256:00e046b6dd71aa03a41079792f8473dc494d564611a8f89bbbd7cb93295ebdcf \ From 0f8fe15e3118fd235ecddd10756a72410a0603bf Mon Sep 17 00:00:00 2001 From: Joel Truher Date: Tue, 24 Dec 2024 09:06:25 -0800 Subject: [PATCH 75/80] remove "cam coord" idea --- gtsam/geometry/Pose3.cpp | 9 +- gtsam/geometry/Pose3.h | 3 +- gtsam/slam/PlanarProjectionFactor.h | 114 ++-- .../slam/tests/testPlanarProjectionFactor.cpp | 535 ++++++++++-------- 4 files changed, 355 insertions(+), 306 deletions(-) diff --git a/gtsam/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp index 5d7732947..03464b0db 100644 --- a/gtsam/geometry/Pose3.cpp +++ b/gtsam/geometry/Pose3.cpp @@ -42,14 +42,17 @@ Pose3 Pose3::Create(const Rot3& R, const Point3& t, OptionalJacobian<6, 3> HR, return Pose3(R, t); } -Pose3 Pose3::FromPose2(const Pose2& p, OptionalJacobian<6, 3> H) { - if (H) *H << (gtsam::Matrix(6, 3) << // +// Pose2 constructor Jacobian is always the same. +static const Matrix63 Hpose2 = (Matrix63() << // 0., 0., 0., // 0., 0., 0.,// 0., 0., 1.,// 1., 0., 0.,// 0., 1., 0.,// 0., 0., 0.).finished(); + +Pose3 Pose3::FromPose2(const Pose2& p, OptionalJacobian<6, 3> H) { + if (H) *H << Hpose2; return Pose3(p); } @@ -520,4 +523,4 @@ std::ostream &operator<<(std::ostream &os, const Pose3& pose) { return os; } -} // namespace gtsam +} // namespace gtsam \ No newline at end of file diff --git a/gtsam/geometry/Pose3.h b/gtsam/geometry/Pose3.h index baac21ccb..ba00c1863 100644 --- a/gtsam/geometry/Pose3.h +++ b/gtsam/geometry/Pose3.h @@ -78,6 +78,7 @@ public: OptionalJacobian<6, 3> HR = {}, OptionalJacobian<6, 3> Ht = {}); + /** Construct from Pose2 in the xy plane, with derivative. */ static Pose3 FromPose2(const Pose2& p, OptionalJacobian<6,3> H = {}); /** @@ -460,4 +461,4 @@ struct Bearing : HasBearing {}; template struct Range : HasRange {}; -} // namespace gtsam +} // namespace gtsam \ No newline at end of file diff --git a/gtsam/slam/PlanarProjectionFactor.h b/gtsam/slam/PlanarProjectionFactor.h index 21f24da93..22cea2214 100644 --- a/gtsam/slam/PlanarProjectionFactor.h +++ b/gtsam/slam/PlanarProjectionFactor.h @@ -59,31 +59,26 @@ namespace gtsam { const Pose2& wTb, const Pose3& bTc, const Cal3DS2& calib, - OptionalMatrixType Hlandmark = nullptr, // 2x3 (x, y, z) - OptionalMatrixType HwTb = nullptr, // 2x3 (x, y, theta) - OptionalMatrixType HbTc = nullptr, // 2x6 (rx, ry, rz, x, y, theta) - OptionalMatrixType Hcalib = nullptr // 2x9 + OptionalJacobian<2, 3> Hlandmark = {}, // (x, y, z) + OptionalJacobian<2, 3> HwTb = {}, // (x, y, theta) + OptionalJacobian<2, 6> HbTc = {}, // (rx, ry, rz, x, y, theta) + OptionalJacobian<2, 9> Hcalib = {} ) const { #ifndef GTSAM_THROW_CHEIRALITY_EXCEPTION try { #endif - gtsam::Matrix Hp; // 6x3 - gtsam::Matrix H0; // 6x6 - // this is x-forward z-up - Pose3 wTc = Pose3::FromPose2(wTb, Hp).compose(bTc, HwTb ? &H0 : nullptr); - // this is z-forward y-down - gtsam::Matrix H00; // 6x6 - Pose3 camera_pose = wTc.compose(CAM_COORD, H00); - PinholeCamera camera = PinholeCamera(camera_pose, calib); + Matrix63 Hp; // 6x3 + Matrix66 H0; // 6x6 + Pose3 wTc = Pose3::FromPose2(wTb, HwTb ? &Hp : nullptr).compose(bTc, HwTb ? &H0 : nullptr); + PinholeCamera camera = PinholeCamera(wTc, calib); if (HwTb || HbTc) { - // Dpose is for pose3, 2x6 (R,t) - gtsam::Matrix Dpose; + // Dpose is for pose3 (R,t) + Matrix26 Dpose; Point2 result = camera.project(landmark, Dpose, Hlandmark, Hcalib); - gtsam::Matrix DposeOffset = Dpose * H00; // 2x6 if (HbTc) - *HbTc = DposeOffset; // with Eigen this is a deep copy (!) - if (HwTb) - *HwTb = DposeOffset * H0 * Hp; + *HbTc = Dpose; + if (HwTb) + *HwTb = Dpose * H0 * Hp; return result; } else { return camera.project(landmark, {}, {}, {}); @@ -91,10 +86,10 @@ namespace gtsam { #ifndef GTSAM_THROW_CHEIRALITY_EXCEPTION } catch (CheiralityException& e) { std::cout << "****** CHIRALITY EXCEPTION ******\n"; - if (Hlandmark) *Hlandmark = Matrix::Zero(2, 3); - if (HwTb) *HwTb = Matrix::Zero(2, 3); - if (HbTc) *HbTc = Matrix::Zero(2, 6); - if (Hcalib) *Hcalib = Matrix::Zero(2, 9); + if (Hlandmark) Hlandmark->setZero(); + if (HwTb) HwTb->setZero(); + if (HbTc) HbTc->setZero(); + if (Hcalib) Hcalib->setZero(); // return a large error return Matrix::Constant(2, 1, 2.0 * calib.fx()); } @@ -102,18 +97,8 @@ namespace gtsam { } Point2 measured_; // pixel measurement - - private: - static const Pose3 CAM_COORD; }; - // camera "zero" is facing +z; this turns it to face +x - const Pose3 PlanarProjectionFactorBase::CAM_COORD = Pose3( - Rot3(0, 0, 1,// - -1, 0, 0, // - 0, -1, 0), - Vector3(0, 0, 0) - ); /** * @class PlanarProjectionFactor1 @@ -131,9 +116,9 @@ namespace gtsam { ~PlanarProjectionFactor1() override {} /// @return a deep copy of this factor - gtsam::NonlinearFactor::shared_ptr clone() const override { - return std::static_pointer_cast( - gtsam::NonlinearFactor::shared_ptr(new PlanarProjectionFactor1(*this))); + NonlinearFactor::shared_ptr clone() const override { + return std::static_pointer_cast( + NonlinearFactor::shared_ptr(new PlanarProjectionFactor1(*this))); } @@ -152,22 +137,18 @@ namespace gtsam { const Point2& measured, const Pose3& bTc, const Cal3DS2& calib, - const SharedNoiseModel& model) + const SharedNoiseModel& model = {}) : PlanarProjectionFactorBase(measured), NoiseModelFactorN(model, poseKey), landmark_(landmark), bTc_(bTc), - calib_(calib) { - assert(2 == model->dim()); - } + calib_(calib) {} /** * @param wTb "world to body": estimated pose2 * @param HwTb jacobian */ - Vector evaluateError( - const Pose2& wTb, - OptionalMatrixType HwTb = OptionalNone) const override { + Vector evaluateError(const Pose2& wTb, OptionalMatrixType HwTb) const override { return predict(landmark_, wTb, bTc_, calib_, {}, HwTb, {}, {}) - measured_; } @@ -187,8 +168,8 @@ namespace gtsam { * Camera offset and calibration are constant. * This is similar to GeneralSFMFactor, used for SLAM. */ - class PlanarProjectionFactor2 - : public PlanarProjectionFactorBase, public NoiseModelFactorN { + class PlanarProjectionFactor2 + : public PlanarProjectionFactorBase, public NoiseModelFactorN { public: typedef NoiseModelFactorN Base; using Base::evaluateError; @@ -198,9 +179,9 @@ namespace gtsam { ~PlanarProjectionFactor2() override {} /// @return a deep copy of this factor - gtsam::NonlinearFactor::shared_ptr clone() const override { - return std::static_pointer_cast( - gtsam::NonlinearFactor::shared_ptr(new PlanarProjectionFactor2(*this))); + NonlinearFactor::shared_ptr clone() const override { + return std::static_pointer_cast( + NonlinearFactor::shared_ptr(new PlanarProjectionFactor2(*this))); } /** @@ -218,13 +199,11 @@ namespace gtsam { const Point2& measured, const Pose3& bTc, const Cal3DS2& calib, - const SharedNoiseModel& model) + const SharedNoiseModel& model = {}) : PlanarProjectionFactorBase(measured), NoiseModelFactorN(model, landmarkKey, poseKey), bTc_(bTc), - calib_(calib) { - assert(2 == model->dim()); - } + calib_(calib) {} /** * @param wTb "world to body": estimated pose2 @@ -235,8 +214,8 @@ namespace gtsam { Vector evaluateError( const Pose2& wTb, const Point3& landmark, - OptionalMatrixType HwTb = OptionalNone, - OptionalMatrixType Hlandmark = OptionalNone) const override { + OptionalMatrixType HwTb, + OptionalMatrixType Hlandmark) const override { return predict(landmark, wTb, bTc_, calib_, Hlandmark, HwTb, {}, {}) - measured_; } @@ -265,47 +244,46 @@ namespace gtsam { ~PlanarProjectionFactor3() override {} /// @return a deep copy of this factor - gtsam::NonlinearFactor::shared_ptr clone() const override { - return std::static_pointer_cast( - gtsam::NonlinearFactor::shared_ptr(new PlanarProjectionFactor3(*this))); + NonlinearFactor::shared_ptr clone() const override { + return std::static_pointer_cast( + NonlinearFactor::shared_ptr(new PlanarProjectionFactor3(*this))); } /** * @brief constructor for variable pose, offset, and calibration, known landmark. + * @param poseKey index of the robot pose2 in the z=0 plane + * @param offsetKey index of camera offset from pose + * @param calibKey index of camera calibration * @param landmark point3 in the world * @param measured corresponding point2 in the camera frame * @param model stddev of the measurements, ~one pixel? - * @param poseKey index of the robot pose2 in the z=0 plane - * @param offsetKey index of camera offset from pose - * @param calibKey index of camera calibration */ + */ PlanarProjectionFactor3( Key poseKey, Key offsetKey, Key calibKey, const Point3& landmark, const Point2& measured, - const SharedNoiseModel& model) + const SharedNoiseModel& model = {}) : PlanarProjectionFactorBase(measured), NoiseModelFactorN(model, poseKey, offsetKey, calibKey), - landmark_(landmark) { - assert(2 == model->dim()); - } + landmark_(landmark) {} /** * @param wTb "world to body": estimated pose2 * @param bTc "body to camera": pose3 offset from pose2 +x * @param calib calibration * @param HwTb pose jacobian - * @param H2 offset jacobian - * @param H3 calibration jacobian + * @param HbTc offset jacobian + * @param Hcalib calibration jacobian */ Vector evaluateError( const Pose2& wTb, const Pose3& bTc, const Cal3DS2& calib, - OptionalMatrixType HwTb = OptionalNone, - OptionalMatrixType HbTc = OptionalNone, - OptionalMatrixType Hcalib = OptionalNone) const override { + OptionalMatrixType HwTb, + OptionalMatrixType HbTc, + OptionalMatrixType Hcalib) const override { return predict(landmark_, wTb, bTc, calib, {}, HwTb, HbTc, Hcalib) - measured_; } diff --git a/gtsam/slam/tests/testPlanarProjectionFactor.cpp b/gtsam/slam/tests/testPlanarProjectionFactor.cpp index 67ebd5c25..1cc002797 100644 --- a/gtsam/slam/tests/testPlanarProjectionFactor.cpp +++ b/gtsam/slam/tests/testPlanarProjectionFactor.cpp @@ -16,6 +16,10 @@ #include #include #include +#include +#include +#include +#include #include #include @@ -26,124 +30,101 @@ using namespace gtsam; using symbol_shorthand::X; using symbol_shorthand::C; using symbol_shorthand::K; +using symbol_shorthand::L; +/* ************************************************************************* */ TEST(PlanarProjectionFactor1, error1) { - // landmark is on the camera bore (which faces +x) + // Example: center projection and Jacobian Point3 landmark(1, 0, 0); - // so pixel measurement is (cx, cy) Point2 measured(200, 200); - Pose3 offset; + Pose3 offset( + Rot3(0, 0, 1,// + -1, 0, 0, // + 0, -1, 0), + Vector3(0, 0, 0) + ); Cal3DS2 calib(200, 200, 0, 200, 200, 0, 0); SharedNoiseModel model = noiseModel::Diagonal::Sigmas(Vector2(1, 1)); - Values values; + PlanarProjectionFactor1 factor(X(0), landmark, measured, offset, calib, model); Pose2 pose(0, 0, 0); - values.insert(X(0), pose); - - PlanarProjectionFactor1 factor( - X(0), landmark, measured, offset, calib, model); - - CHECK_EQUAL(2, factor.dim()); - CHECK(factor.active(values)); - std::vector actualHs(1); - gtsam::Vector actual = factor.unwhitenedError(values, actualHs); - - CHECK(assert_equal(Vector2(0, 0), actual)); - - const Matrix& H1Actual = actualHs.at(0); - CHECK_EQUAL(2, H1Actual.rows()); - CHECK_EQUAL(3, H1Actual.cols()); - const Matrix23 H1Expected = (Matrix23() << // - 0, 200, 200,// - 0, 0, 0).finished(); - CHECK(assert_equal(H1Expected, H1Actual, 1e-6)); + Matrix H; + CHECK(assert_equal(Vector2(0, 0), factor.evaluateError(pose, H), 1e-6)); + CHECK(assert_equal((Matrix23() << // + 0, 200, 200, // + 0, 0, 0).finished(), H, 1e-6)); } +/* ************************************************************************* */ TEST(PlanarProjectionFactor1, error2) { - // landmark is in the upper left corner + // Example: upper left corner projection and Jacobian Point3 landmark(1, 1, 1); - // upper left corner in pixels Point2 measured(0, 0); - Pose3 offset; + Pose3 offset( + Rot3(0, 0, 1,// + -1, 0, 0, // + 0, -1, 0), + Vector3(0, 0, 0) + ); Cal3DS2 calib(200, 200, 0, 200, 200, 0, 0); SharedNoiseModel model = noiseModel::Diagonal::Sigmas(Vector2(1, 1)); - PlanarProjectionFactor1 factor( - X(0), landmark, measured, offset, calib, model); - Values values; + PlanarProjectionFactor1 factor(X(0), landmark, measured, offset, calib, model); Pose2 pose(0, 0, 0); - - values.insert(X(0), pose); - - CHECK_EQUAL(2, factor.dim()); - CHECK(factor.active(values)); - std::vector actualHs(1); - gtsam::Vector actual = factor.unwhitenedError(values, actualHs); - - CHECK(assert_equal(Vector2(0, 0), actual)); - - const Matrix& H1Actual = actualHs.at(0); - CHECK_EQUAL(2, H1Actual.rows()); - CHECK_EQUAL(3, H1Actual.cols()); - Matrix23 H1Expected = (Matrix23() << // + Matrix H; + CHECK(assert_equal(Vector2(0, 0), factor.evaluateError(pose, H), 1e-6)); + CHECK(assert_equal((Matrix23() << // -200, 200, 400, // - -200, 0, 200).finished(); - CHECK(assert_equal(H1Expected, H1Actual, 1e-6)); + -200, 0, 200).finished(), H, 1e-6)); } +/* ************************************************************************* */ TEST(PlanarProjectionFactor1, error3) { - // landmark is in the upper left corner + // Example: upper left corner projection and Jacobian with distortion Point3 landmark(1, 1, 1); - // upper left corner in pixels Point2 measured(0, 0); - Pose3 offset; - // distortion - Cal3DS2 calib(200, 200, 0, 200, 200, -0.2, 0.1); + Pose3 offset( + Rot3(0, 0, 1,// + -1, 0, 0, // + 0, -1, 0), + Vector3(0, 0, 0) + ); + Cal3DS2 calib(200, 200, 0, 200, 200, -0.2, 0.1); // note distortion SharedNoiseModel model = noiseModel::Diagonal::Sigmas(Vector2(1, 1)); - PlanarProjectionFactor1 factor( - X(0), landmark, measured, offset, calib, model); - Values values; + PlanarProjectionFactor1 factor(X(0), landmark, measured, offset, calib, model); Pose2 pose(0, 0, 0); - - values.insert(X(0), pose); - - CHECK_EQUAL(2, factor.dim()); - CHECK(factor.active(values)); - std::vector actualHs(1); - gtsam::Vector actual = factor.unwhitenedError(values, actualHs); - - CHECK(assert_equal(Vector2(0, 0), actual)); - - const Matrix& H1Actual = actualHs.at(0); - CHECK_EQUAL(2, H1Actual.rows()); - CHECK_EQUAL(3, H1Actual.cols()); - Matrix23 H1Expected = (Matrix23() << // + Matrix H; + CHECK(assert_equal(Vector2(0, 0), factor.evaluateError(pose, H), 1e-6)); + CHECK(assert_equal((Matrix23() << // -360, 280, 640, // - -360, 80, 440).finished(); - CHECK(assert_equal(H1Expected, H1Actual, 1e-6)); + -360, 80, 440).finished(), H, 1e-6)); } +/* ************************************************************************* */ TEST(PlanarProjectionFactor1, jacobian) { - // test many jacobians with many randoms - - std::default_random_engine g; - std::uniform_real_distribution s(-0.3, 0.3); + // Verify Jacobians with numeric derivative + std::default_random_engine rng(42); + std::uniform_real_distribution dist(-0.3, 0.3); SharedNoiseModel model = noiseModel::Diagonal::Sigmas(Vector2(1, 1)); + // center of the random camera poses + Pose3 centerOffset( + Rot3(0, 0, 1,// + -1, 0, 0, // + 0, -1, 0), + Vector3(0, 0, 0) + ); for (int i = 0; i < 1000; ++i) { - Point3 landmark(2 + s(g), s(g), s(g)); - Point2 measured(200 + 100*s(g), 200 + 100*s(g)); - Pose3 offset(Rot3::Ypr(s(g),s(g),s(g)), Point3(s(g),s(g),s(g))); + Point3 landmark(2 + dist(rng), dist(rng), dist(rng)); + Point2 measured(200 + 100 * dist(rng), 200 + 100 * dist(rng)); + Pose3 offset = centerOffset.compose( + Pose3( + Rot3::Ypr(dist(rng), dist(rng), dist(rng)), + Point3(dist(rng), dist(rng), dist(rng)))); Cal3DS2 calib(200, 200, 0, 200, 200, -0.2, 0.1); - - PlanarProjectionFactor1 factor( - X(0), landmark, measured, offset, calib, model); - - Pose2 pose(s(g), s(g), s(g)); - - // actual H + PlanarProjectionFactor1 factor(X(0), landmark, measured, offset, calib, model); + Pose2 pose(dist(rng), dist(rng), dist(rng)); Matrix H1; factor.evaluateError(pose, H1); - - Matrix expectedH1 = numericalDerivative11( + auto expectedH1 = numericalDerivative11( [&factor](const Pose2& p) { return factor.evaluateError(p, {});}, pose); @@ -151,194 +132,182 @@ TEST(PlanarProjectionFactor1, jacobian) { } } +/* ************************************************************************* */ +TEST(PlanarProjectionFactor1, solve) { + // Example localization + SharedNoiseModel pxModel = noiseModel::Diagonal::Sigmas(Vector2(1, 1)); + // pose model is wide, so the solver finds the right answer. + SharedNoiseModel xNoise = noiseModel::Diagonal::Sigmas(Vector3(10, 10, 10)); -TEST(PlanarProjectionFactor3, error1) { - // landmark is on the camera bore (facing +x) - Point3 landmark(1, 0, 0); - // so px is (cx, cy) - Point2 measured(200, 200); - // offset is identity - Pose3 offset; + // landmarks + Point3 l0(1, 0.1, 1); + Point3 l1(1, -0.1, 1); + + // camera pixels + Point2 p0(180, 0); + Point2 p1(220, 0); + + // body + Pose2 x0(0, 0, 0); + + // camera z looking at +x with (xy) antiparallel to (yz) + Pose3 c0( + Rot3(0, 0, 1, // + -1, 0, 0, // + 0, -1, 0), // + Vector3(0, 0, 0)); Cal3DS2 calib(200, 200, 0, 200, 200, 0, 0); - SharedNoiseModel model = noiseModel::Diagonal::Sigmas(Vector2(1, 1)); - Values values; - Pose2 pose(0, 0, 0); - values.insert(X(0), pose); - values.insert(C(0), offset); - values.insert(K(0), calib); - PlanarProjectionFactor3 factor( - X(0), C(0), K(0), landmark, measured, model); + NonlinearFactorGraph graph; + graph.add(PlanarProjectionFactor1(X(0), l0, p0, c0, calib, pxModel)); + graph.add(PlanarProjectionFactor1(X(0), l1, p1, c0, calib, pxModel)); + graph.add(PriorFactor(X(0), x0, xNoise)); - CHECK_EQUAL(2, factor.dim()); - CHECK(factor.active(values)); - std::vector actualHs(3); + Values initialEstimate; + initialEstimate.insert(X(0), x0); - gtsam::Vector actual = factor.unwhitenedError(values, actualHs); - CHECK(assert_equal(Vector2(0, 0), actual)); + // run the optimizer + LevenbergMarquardtOptimizer optimizer(graph, initialEstimate); + Values result = optimizer.optimize(); - const Matrix& H1Actual = actualHs.at(0); - const Matrix& H2Actual = actualHs.at(1); - const Matrix& H3Actual = actualHs.at(2); + // verify that the optimizer found the right pose. + CHECK(assert_equal(x0, result.at(X(0)), 2e-3)); - CHECK_EQUAL(2, H1Actual.rows()); - CHECK_EQUAL(3, H1Actual.cols()); - CHECK_EQUAL(2, H2Actual.rows()); - CHECK_EQUAL(6, H2Actual.cols()); - CHECK_EQUAL(2, H3Actual.rows()); - CHECK_EQUAL(9, H3Actual.cols()); + // covariance + Marginals marginals(graph, result); + Matrix cov = marginals.marginalCovariance(X(0)); + CHECK(assert_equal((Matrix33() << // + 0.000012, 0.000000, 0.000000, // + 0.000000, 0.001287, -.001262, // + 0.000000, -.001262, 0.001250).finished(), cov, 3e-6)); - // du/dx etc for the pose2d - Matrix23 H1Expected = (Matrix23() <dim()); - CHECK_EQUAL(2, factor.dim()); - CHECK(factor.active(values)); - std::vector actualHs(3); - gtsam::Vector actual = factor.unwhitenedError(values, actualHs); - + Pose3 offset( + Rot3(0, 0, 1,// + -1, 0, 0, // + 0, -1, 0), + Vector3(0, 0, 0) + ); + Cal3DS2 calib(200, 200, 0, 200, 200, 0, 0); + Matrix H1; + Matrix H2; + Matrix H3; + gtsam::Vector actual = factor.evaluateError(pose, offset, calib, H1, H2, H3); CHECK(assert_equal(Vector2(0, 0), actual)); - - const Matrix& H1Actual = actualHs.at(0); - const Matrix& H2Actual = actualHs.at(1); - const Matrix& H3Actual = actualHs.at(2); - - CHECK_EQUAL(2, H1Actual.rows()); - CHECK_EQUAL(3, H1Actual.cols()); - CHECK_EQUAL(2, H2Actual.rows()); - CHECK_EQUAL(6, H2Actual.cols()); - CHECK_EQUAL(2, H3Actual.rows()); - CHECK_EQUAL(9, H3Actual.cols()); - - Matrix23 H1Expected = (Matrix23() <dim()); - CHECK_EQUAL(2, factor.dim()); - CHECK(factor.active(values)); - std::vector actualHs(3); - gtsam::Vector actual = factor.unwhitenedError(values, actualHs); - - CHECK(assert_equal(Vector2(0, 0), actual)); - - const Matrix& H1Actual = actualHs.at(0); - const Matrix& H2Actual = actualHs.at(1); - const Matrix& H3Actual = actualHs.at(2); - - CHECK_EQUAL(2, H1Actual.rows()); - CHECK_EQUAL(3, H1Actual.cols()); - CHECK_EQUAL(2, H2Actual.rows()); - CHECK_EQUAL(6, H2Actual.cols()); - CHECK_EQUAL(2, H3Actual.rows()); - CHECK_EQUAL(9, H3Actual.cols()); - - Matrix23 H1Expected = (Matrix23() < s(-0.3, 0.3); + std::default_random_engine rng(42); + std::uniform_real_distribution dist(-0.3, 0.3); SharedNoiseModel model = noiseModel::Diagonal::Sigmas(Vector2(1, 1)); + // center of the random camera poses + Pose3 centerOffset( + Rot3(0, 0, 1,// + -1, 0, 0, // + 0, -1, 0), + Vector3(0, 0, 0) + ); for (int i = 0; i < 1000; ++i) { - Point3 landmark(2 + s(g), s(g), s(g)); - Point2 measured(200 + 100*s(g), 200 + 100*s(g)); - Pose3 offset(Rot3::Ypr(s(g),s(g),s(g)), Point3(s(g),s(g),s(g))); + Point3 landmark(2 + dist(rng), dist(rng), dist(rng)); + Point2 measured(200 + 100 * dist(rng), 200 + 100 * dist(rng)); + Pose3 offset = centerOffset.compose( + Pose3( + Rot3::Ypr(dist(rng), dist(rng), dist(rng)), + Point3(dist(rng), dist(rng), dist(rng)))); Cal3DS2 calib(200, 200, 0, 200, 200, -0.2, 0.1); - PlanarProjectionFactor3 factor( - X(0), C(0), K(0), landmark, measured, model); + PlanarProjectionFactor3 factor(X(0), C(0), K(0), landmark, measured, model); - Pose2 pose(s(g), s(g), s(g)); + Pose2 pose(dist(rng), dist(rng), dist(rng)); // actual H Matrix H1, H2, H3; @@ -362,6 +331,104 @@ TEST(PlanarProjectionFactor3, jacobian) { } } +/* ************************************************************************* */ +TEST(PlanarProjectionFactor3, solveOffset) { + // Example localization + SharedNoiseModel pxModel = noiseModel::Diagonal::Sigmas(Vector2(1, 1)); + SharedNoiseModel xNoise = noiseModel::Diagonal::Sigmas(Vector3(0.01, 0.01, 0.01)); + // offset model is wide, so the solver finds the right answer. + SharedNoiseModel cNoise = noiseModel::Diagonal::Sigmas(Vector6(10, 10, 10, 10, 10, 10)); + SharedNoiseModel kNoise = noiseModel::Diagonal::Sigmas(Vector9(0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001)); + + // landmarks + Point3 l0(1, 0, 1); + Point3 l1(1, 0, 0); + Point3 l2(1, -1, 1); + Point3 l3(2, 2, 1); + + // camera pixels + Point2 p0(200, 200); + Point2 p1(200, 400); + Point2 p2(400, 200); + Point2 p3(0, 200); + + // body + Pose2 x0(0, 0, 0); + + // camera z looking at +x with (xy) antiparallel to (yz) + Pose3 c0( + Rot3(0, 0, 1, // + -1, 0, 0, // + 0, -1, 0), // + Vector3(0, 0, 1)); // note z offset + Cal3DS2 calib(200, 200, 0, 200, 200, 0, 0); + + NonlinearFactorGraph graph; + graph.add(PlanarProjectionFactor3(X(0), C(0), K(0), l0, p0, pxModel)); + graph.add(PlanarProjectionFactor3(X(0), C(0), K(0), l1, p1, pxModel)); + graph.add(PlanarProjectionFactor3(X(0), C(0), K(0), l2, p2, pxModel)); + graph.add(PlanarProjectionFactor3(X(0), C(0), K(0), l3, p3, pxModel)); + graph.add(PriorFactor(X(0), x0, xNoise)); + graph.add(PriorFactor(C(0), c0, cNoise)); + graph.add(PriorFactor(K(0), calib, kNoise)); + + Values initialEstimate; + initialEstimate.insert(X(0), x0); + initialEstimate.insert(C(0), c0); + initialEstimate.insert(K(0), calib); + + // run the optimizer + LevenbergMarquardtOptimizer optimizer(graph, initialEstimate); + Values result = optimizer.optimize(); + + // verify that the optimizer found the right pose. + CHECK(assert_equal(x0, result.at(X(0)), 2e-3)); + + // verify the camera is pointing at +x + Pose3 cc0 = result.at(C(0)); + CHECK(assert_equal(c0, cc0, 5e-3)); + + // verify the calibration + CHECK(assert_equal(calib, result.at(K(0)), 2e-3)); + + Marginals marginals(graph, result); + Matrix x0cov = marginals.marginalCovariance(X(0)); + + // narrow prior => ~zero cov + CHECK(assert_equal(Matrix33::Zero(), x0cov, 1e-4)); + + Matrix c0cov = marginals.marginalCovariance(C(0)); + + // invert the camera offset to get covariance in body coordinates + Matrix66 HcTb = cc0.inverse().AdjointMap().inverse(); + Matrix c0cov2 = HcTb * c0cov * HcTb.transpose(); + + // camera-frame stddev + Vector6 c0sigma = c0cov.diagonal().cwiseSqrt(); + CHECK(assert_equal((Vector6() << // + 0.009, + 0.011, + 0.004, + 0.012, + 0.012, + 0.011 + ).finished(), c0sigma, 1e-3)); + + // body frame stddev + Vector6 bTcSigma = c0cov2.diagonal().cwiseSqrt(); + CHECK(assert_equal((Vector6() << // + 0.004, + 0.009, + 0.011, + 0.012, + 0.012, + 0.012 + ).finished(), bTcSigma, 1e-3)); + + // narrow prior => ~zero cov + CHECK(assert_equal(Matrix99::Zero(), marginals.marginalCovariance(K(0)), 3e-3)); +} + /* ************************************************************************* */ int main() { TestResult tr; From 69765496c5935edaa71791ac2a69b41a407b2acb Mon Sep 17 00:00:00 2001 From: Joel Truher Date: Tue, 24 Dec 2024 14:27:11 -0800 Subject: [PATCH 76/80] fix windows test --- gtsam/slam/tests/testPlanarProjectionFactor.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/slam/tests/testPlanarProjectionFactor.cpp b/gtsam/slam/tests/testPlanarProjectionFactor.cpp index 1cc002797..603eb555a 100644 --- a/gtsam/slam/tests/testPlanarProjectionFactor.cpp +++ b/gtsam/slam/tests/testPlanarProjectionFactor.cpp @@ -128,7 +128,7 @@ TEST(PlanarProjectionFactor1, jacobian) { [&factor](const Pose2& p) { return factor.evaluateError(p, {});}, pose); - CHECK(assert_equal(expectedH1, H1, 1e-6)); + CHECK(assert_equal(expectedH1, H1, 2e-6)); } } From 2827a1e04167707c74f0e51816d1e658b6e8eb58 Mon Sep 17 00:00:00 2001 From: Joel Truher Date: Tue, 24 Dec 2024 14:30:41 -0800 Subject: [PATCH 77/80] fix test names --- .../slam/tests/testPlanarProjectionFactor.cpp | 20 +++++++++---------- 1 file changed, 10 insertions(+), 10 deletions(-) diff --git a/gtsam/slam/tests/testPlanarProjectionFactor.cpp b/gtsam/slam/tests/testPlanarProjectionFactor.cpp index 603eb555a..acfa95618 100644 --- a/gtsam/slam/tests/testPlanarProjectionFactor.cpp +++ b/gtsam/slam/tests/testPlanarProjectionFactor.cpp @@ -33,7 +33,7 @@ using symbol_shorthand::K; using symbol_shorthand::L; /* ************************************************************************* */ -TEST(PlanarProjectionFactor1, error1) { +TEST(PlanarProjectionFactor1, Error1) { // Example: center projection and Jacobian Point3 landmark(1, 0, 0); Point2 measured(200, 200); @@ -55,7 +55,7 @@ TEST(PlanarProjectionFactor1, error1) { } /* ************************************************************************* */ -TEST(PlanarProjectionFactor1, error2) { +TEST(PlanarProjectionFactor1, Error2) { // Example: upper left corner projection and Jacobian Point3 landmark(1, 1, 1); Point2 measured(0, 0); @@ -77,7 +77,7 @@ TEST(PlanarProjectionFactor1, error2) { } /* ************************************************************************* */ -TEST(PlanarProjectionFactor1, error3) { +TEST(PlanarProjectionFactor1, Error3) { // Example: upper left corner projection and Jacobian with distortion Point3 landmark(1, 1, 1); Point2 measured(0, 0); @@ -99,7 +99,7 @@ TEST(PlanarProjectionFactor1, error3) { } /* ************************************************************************* */ -TEST(PlanarProjectionFactor1, jacobian) { +TEST(PlanarProjectionFactor1, Jacobian) { // Verify Jacobians with numeric derivative std::default_random_engine rng(42); std::uniform_real_distribution dist(-0.3, 0.3); @@ -133,7 +133,7 @@ TEST(PlanarProjectionFactor1, jacobian) { } /* ************************************************************************* */ -TEST(PlanarProjectionFactor1, solve) { +TEST(PlanarProjectionFactor1, Solve) { // Example localization SharedNoiseModel pxModel = noiseModel::Diagonal::Sigmas(Vector2(1, 1)); @@ -193,7 +193,7 @@ TEST(PlanarProjectionFactor1, solve) { } /* ************************************************************************* */ -TEST(PlanarProjectionFactor3, error1) { +TEST(PlanarProjectionFactor3, Error1) { // Example: center projection and Jacobian Point3 landmark(1, 0, 0); Point2 measured(200, 200); @@ -223,7 +223,7 @@ TEST(PlanarProjectionFactor3, error1) { } /* ************************************************************************* */ -TEST(PlanarProjectionFactor3, error2) { +TEST(PlanarProjectionFactor3, Error2) { Point3 landmark(1, 1, 1); Point2 measured(0, 0); SharedNoiseModel model = noiseModel::Diagonal::Sigmas(Vector2(1, 1)); @@ -253,7 +253,7 @@ TEST(PlanarProjectionFactor3, error2) { } /* ************************************************************************* */ -TEST(PlanarProjectionFactor3, error3) { +TEST(PlanarProjectionFactor3, Error3) { Point3 landmark(1, 1, 1); Point2 measured(0, 0); SharedNoiseModel model = noiseModel::Diagonal::Sigmas(Vector2(1, 1)); @@ -282,7 +282,7 @@ TEST(PlanarProjectionFactor3, error3) { } /* ************************************************************************* */ -TEST(PlanarProjectionFactor3, jacobian) { +TEST(PlanarProjectionFactor3, Jacobian) { // Verify Jacobians with numeric derivative std::default_random_engine rng(42); @@ -332,7 +332,7 @@ TEST(PlanarProjectionFactor3, jacobian) { } /* ************************************************************************* */ -TEST(PlanarProjectionFactor3, solveOffset) { +TEST(PlanarProjectionFactor3, SolveOffset) { // Example localization SharedNoiseModel pxModel = noiseModel::Diagonal::Sigmas(Vector2(1, 1)); SharedNoiseModel xNoise = noiseModel::Diagonal::Sigmas(Vector3(0.01, 0.01, 0.01)); From 1203f0c7024a197207c39f0995c61eaa860c06d5 Mon Sep 17 00:00:00 2001 From: Joel Truher Date: Tue, 24 Dec 2024 14:32:44 -0800 Subject: [PATCH 78/80] add newlines --- gtsam/geometry/Pose3.cpp | 2 +- gtsam/geometry/Pose3.h | 2 +- gtsam/slam/PlanarProjectionFactor.h | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) diff --git a/gtsam/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp index 03464b0db..f3dacb163 100644 --- a/gtsam/geometry/Pose3.cpp +++ b/gtsam/geometry/Pose3.cpp @@ -523,4 +523,4 @@ std::ostream &operator<<(std::ostream &os, const Pose3& pose) { return os; } -} // namespace gtsam \ No newline at end of file +} // namespace gtsam diff --git a/gtsam/geometry/Pose3.h b/gtsam/geometry/Pose3.h index ba00c1863..edb6094fa 100644 --- a/gtsam/geometry/Pose3.h +++ b/gtsam/geometry/Pose3.h @@ -461,4 +461,4 @@ struct Bearing : HasBearing {}; template struct Range : HasRange {}; -} // namespace gtsam \ No newline at end of file +} // namespace gtsam diff --git a/gtsam/slam/PlanarProjectionFactor.h b/gtsam/slam/PlanarProjectionFactor.h index 22cea2214..d53cd0ae1 100644 --- a/gtsam/slam/PlanarProjectionFactor.h +++ b/gtsam/slam/PlanarProjectionFactor.h @@ -295,4 +295,4 @@ namespace gtsam { struct traits : public Testable {}; -} // namespace gtsam \ No newline at end of file +} // namespace gtsam From 993ac90e43012f8b2008ad5e74144a2d26622a91 Mon Sep 17 00:00:00 2001 From: Joel Truher Date: Tue, 24 Dec 2024 15:27:49 -0800 Subject: [PATCH 79/80] fix windows test --- gtsam/slam/tests/testPlanarProjectionFactor.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/gtsam/slam/tests/testPlanarProjectionFactor.cpp b/gtsam/slam/tests/testPlanarProjectionFactor.cpp index acfa95618..5f65c9b5e 100644 --- a/gtsam/slam/tests/testPlanarProjectionFactor.cpp +++ b/gtsam/slam/tests/testPlanarProjectionFactor.cpp @@ -128,7 +128,7 @@ TEST(PlanarProjectionFactor1, Jacobian) { [&factor](const Pose2& p) { return factor.evaluateError(p, {});}, pose); - CHECK(assert_equal(expectedH1, H1, 2e-6)); + CHECK(assert_equal(expectedH1, H1, 5e-6)); } } @@ -325,9 +325,9 @@ TEST(PlanarProjectionFactor3, Jacobian) { [&factor](const Pose2& p, const Pose3& o, const Cal3DS2& c) { return factor.evaluateError(p, o, c, {}, {}, {});}, pose, offset, calib); - CHECK(assert_equal(expectedH1, H1, 1e-6)); - CHECK(assert_equal(expectedH2, H2, 1e-6)); - CHECK(assert_equal(expectedH3, H3, 1e-6)); + CHECK(assert_equal(expectedH1, H1, 5e-6)); + CHECK(assert_equal(expectedH2, H2, 5e-6)); + CHECK(assert_equal(expectedH3, H3, 5e-6)); } } From ae213dd46477fd39e8399f466ff6b21d11909713 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 26 Dec 2024 23:44:00 -0500 Subject: [PATCH 80/80] replace #ifdef with #if for GTSAM_ENABLE_BOOST_SERIALIZATION --- gtsam/base/ConcurrentMap.h | 4 +-- gtsam/base/FastList.h | 4 +-- gtsam/base/FastMap.h | 4 +-- gtsam/base/FastSet.h | 4 +-- gtsam/base/GenericValue.h | 2 +- gtsam/base/MatrixSerialization.h | 2 +- gtsam/base/SymmetricBlockMatrix.h | 4 +-- gtsam/base/Value.h | 6 ++-- gtsam/base/VectorSerialization.h | 2 +- gtsam/base/VerticalBlockMatrix.h | 2 +- gtsam/base/serialization.h | 2 +- gtsam/base/serializationTestHelpers.h | 2 +- gtsam/base/std_optional_serialization.h | 2 +- gtsam/discrete/DecisionTree-inl.h | 4 +-- gtsam/discrete/DecisionTree.h | 6 ++-- gtsam/discrete/DecisionTreeFactor.h | 2 +- gtsam/discrete/DiscreteBayesNet.h | 2 +- gtsam/discrete/DiscreteConditional.h | 2 +- gtsam/discrete/DiscreteFactor.h | 2 +- gtsam/discrete/DiscreteKey.h | 4 +-- gtsam/discrete/DiscreteLookupDAG.h | 2 +- gtsam/geometry/BearingRange.h | 4 +-- gtsam/geometry/Cal3.h | 2 +- gtsam/geometry/Cal3Bundler.h | 2 +- gtsam/geometry/Cal3DS2.h | 2 +- gtsam/geometry/Cal3DS2_Base.h | 2 +- gtsam/geometry/Cal3Fisheye.h | 2 +- gtsam/geometry/Cal3Unified.h | 2 +- gtsam/geometry/Cal3_S2.h | 2 +- gtsam/geometry/Cal3_S2Stereo.h | 2 +- gtsam/geometry/Cal3f.h | 2 +- gtsam/geometry/CalibratedCamera.h | 6 ++-- gtsam/geometry/CameraSet.h | 2 +- gtsam/geometry/EssentialMatrix.h | 2 +- gtsam/geometry/PinholeCamera.h | 2 +- gtsam/geometry/PinholePose.h | 4 +-- gtsam/geometry/PinholeSet.h | 2 +- gtsam/geometry/Point2.h | 2 +- gtsam/geometry/Point3.h | 2 +- gtsam/geometry/Pose2.h | 2 +- gtsam/geometry/Pose3.h | 2 +- gtsam/geometry/Rot2.h | 2 +- gtsam/geometry/Rot3.h | 2 +- gtsam/geometry/SO3.h | 2 +- gtsam/geometry/SO4.h | 2 +- gtsam/geometry/SOn.h | 6 ++-- gtsam/geometry/Similarity3.h | 2 +- gtsam/geometry/SphericalCamera.h | 6 ++-- gtsam/geometry/StereoCamera.h | 2 +- gtsam/geometry/StereoPoint2.h | 4 +-- gtsam/geometry/Unit3.h | 2 +- gtsam/geometry/tests/testBearingRange.cpp | 2 +- gtsam/geometry/tests/testUnit3.cpp | 2 +- gtsam/geometry/triangulation.h | 4 +-- gtsam/hybrid/HybridBayesNet.h | 2 +- gtsam/hybrid/HybridBayesTree.h | 2 +- gtsam/hybrid/HybridConditional.h | 2 +- gtsam/hybrid/HybridFactor.h | 2 +- gtsam/hybrid/HybridGaussianConditional.h | 2 +- gtsam/hybrid/HybridGaussianFactor.h | 2 +- gtsam/hybrid/HybridGaussianProductFactor.h | 2 +- gtsam/inference/BayesTree.h | 2 +- gtsam/inference/BayesTreeCliqueBase.h | 2 +- gtsam/inference/Conditional.h | 2 +- gtsam/inference/Factor.h | 4 +-- gtsam/inference/FactorGraph.h | 4 +-- gtsam/inference/LabeledSymbol.h | 2 +- gtsam/inference/Ordering.h | 2 +- gtsam/inference/Symbol.h | 4 +-- gtsam/inference/VariableIndex.h | 2 +- gtsam/linear/GaussianBayesNet.h | 2 +- gtsam/linear/GaussianConditional.h | 2 +- gtsam/linear/GaussianFactor.h | 2 +- gtsam/linear/GaussianFactorGraph.h | 2 +- gtsam/linear/HessianFactor.h | 2 +- gtsam/linear/JacobianFactor.h | 6 ++-- gtsam/linear/LossFunctions.h | 28 +++++++++---------- gtsam/linear/NoiseModel.h | 16 +++++------ gtsam/linear/SubgraphBuilder.cpp | 4 +-- gtsam/linear/SubgraphBuilder.h | 6 ++-- gtsam/linear/VectorValues.h | 2 +- gtsam/navigation/AHRSFactor.h | 4 +-- gtsam/navigation/AttitudeFactor.h | 6 ++-- gtsam/navigation/BarometricFactor.h | 2 +- gtsam/navigation/CombinedImuFactor.cpp | 2 +- gtsam/navigation/CombinedImuFactor.h | 4 +-- gtsam/navigation/GPSFactor.h | 4 +-- gtsam/navigation/ImuBias.h | 4 +-- gtsam/navigation/ImuFactor.h | 6 ++-- gtsam/navigation/MagPoseFactor.h | 2 +- gtsam/navigation/ManifoldPreintegration.h | 2 +- gtsam/navigation/NavState.h | 2 +- gtsam/navigation/PreintegratedRotation.h | 4 +-- gtsam/navigation/PreintegrationBase.h | 2 +- .../navigation/PreintegrationCombinedParams.h | 2 +- gtsam/navigation/PreintegrationParams.h | 2 +- gtsam/navigation/TangentPreintegration.h | 2 +- gtsam/nonlinear/CustomFactor.h | 2 +- gtsam/nonlinear/ExpressionFactor.h | 4 +-- gtsam/nonlinear/FunctorizedFactor.h | 4 +-- gtsam/nonlinear/ISAM2.h | 2 +- gtsam/nonlinear/ISAM2Clique.h | 2 +- gtsam/nonlinear/LinearContainerFactor.h | 2 +- gtsam/nonlinear/NonlinearEquality.h | 6 ++-- gtsam/nonlinear/NonlinearFactor.h | 6 ++-- gtsam/nonlinear/NonlinearFactorGraph.h | 2 +- gtsam/nonlinear/PriorFactor.h | 2 +- gtsam/nonlinear/Values.h | 4 +-- gtsam/sam/BearingFactor.h | 2 +- gtsam/sam/BearingRangeFactor.h | 2 +- gtsam/sam/RangeFactor.h | 4 +-- gtsam/sfm/SfmData.h | 2 +- gtsam/sfm/SfmTrack.h | 2 +- gtsam/sfm/TranslationFactor.h | 4 +-- gtsam/slam/AntiFactor.h | 2 +- gtsam/slam/BetweenFactor.h | 4 +-- gtsam/slam/BoundingConstraint.h | 4 +-- gtsam/slam/EssentialMatrixConstraint.h | 2 +- gtsam/slam/GeneralSFMFactor.h | 6 ++-- gtsam/slam/PoseRotationPrior.h | 2 +- gtsam/slam/PoseTranslationPrior.h | 2 +- gtsam/slam/ProjectionFactor.h | 2 +- gtsam/slam/ReferenceFrameFactor.h | 2 +- gtsam/slam/SmartFactorBase.h | 4 +-- gtsam/slam/SmartFactorParams.h | 2 +- gtsam/slam/SmartProjectionFactor.h | 2 +- gtsam/slam/SmartProjectionPoseFactor.h | 2 +- gtsam/slam/SmartProjectionRigFactor.h | 2 +- gtsam/slam/StereoFactor.h | 2 +- gtsam/slam/TriangulationFactor.h | 2 +- gtsam/symbolic/SymbolicBayesNet.h | 2 +- gtsam/symbolic/SymbolicBayesTree.h | 2 +- gtsam/symbolic/SymbolicConditional.h | 2 +- gtsam/symbolic/SymbolicFactor.h | 2 +- gtsam/symbolic/SymbolicFactorGraph.h | 2 +- gtsam_unstable/dynamics/PoseRTV.h | 2 +- gtsam_unstable/dynamics/VelocityConstraint3.h | 2 +- gtsam_unstable/geometry/BearingS2.h | 2 +- gtsam_unstable/geometry/InvDepthCamera3.h | 4 +-- gtsam_unstable/geometry/Pose3Upright.h | 2 +- gtsam_unstable/nonlinear/LinearizedFactor.h | 6 ++-- gtsam_unstable/slam/BetweenFactorEM.h | 2 +- gtsam_unstable/slam/BiasedGPSFactor.h | 2 +- .../slam/EquivInertialNavFactor_GlobalVel.h | 2 +- .../EquivInertialNavFactor_GlobalVel_NoBias.h | 2 +- .../slam/GaussMarkov1stOrderFactor.h | 2 +- .../slam/InertialNavFactor_GlobalVelocity.h | 2 +- gtsam_unstable/slam/InvDepthFactor3.h | 2 +- gtsam_unstable/slam/InvDepthFactorVariant1.h | 2 +- gtsam_unstable/slam/InvDepthFactorVariant2.h | 2 +- gtsam_unstable/slam/InvDepthFactorVariant3.h | 4 +-- gtsam_unstable/slam/MultiProjectionFactor.h | 2 +- gtsam_unstable/slam/PartialPriorFactor.h | 2 +- gtsam_unstable/slam/PoseBetweenFactor.h | 2 +- gtsam_unstable/slam/PosePriorFactor.h | 2 +- gtsam_unstable/slam/PoseToPointFactor.h | 2 +- gtsam_unstable/slam/ProjectionFactorPPP.h | 2 +- gtsam_unstable/slam/ProjectionFactorPPPC.h | 2 +- .../slam/ProjectionFactorRollingShutter.h | 2 +- gtsam_unstable/slam/RelativeElevationFactor.h | 2 +- .../SmartProjectionPoseFactorRollingShutter.h | 2 +- .../slam/SmartStereoProjectionFactor.h | 4 +-- .../slam/SmartStereoProjectionFactorPP.h | 2 +- .../slam/SmartStereoProjectionPoseFactor.h | 2 +- .../slam/TransformBtwRobotsUnaryFactor.h | 2 +- .../slam/TransformBtwRobotsUnaryFactorEM.h | 2 +- tests/simulated2D.h | 6 ++-- tests/testExpressionFactor.cpp | 2 +- 168 files changed, 247 insertions(+), 247 deletions(-) diff --git a/gtsam/base/ConcurrentMap.h b/gtsam/base/ConcurrentMap.h index 372f02d06..03687f562 100644 --- a/gtsam/base/ConcurrentMap.h +++ b/gtsam/base/ConcurrentMap.h @@ -48,7 +48,7 @@ using ConcurrentMapBase = gtsam::FastMap; #endif -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION #include #include #endif @@ -101,7 +101,7 @@ public: #endif private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template diff --git a/gtsam/base/FastList.h b/gtsam/base/FastList.h index 414c1e83f..1a3b9ed3f 100644 --- a/gtsam/base/FastList.h +++ b/gtsam/base/FastList.h @@ -20,7 +20,7 @@ #include #include -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION #include #include #if BOOST_VERSION >= 107400 @@ -79,7 +79,7 @@ public: } private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template diff --git a/gtsam/base/FastMap.h b/gtsam/base/FastMap.h index e8ef3fc4f..75998fd2f 100644 --- a/gtsam/base/FastMap.h +++ b/gtsam/base/FastMap.h @@ -19,7 +19,7 @@ #pragma once #include -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION #include #include #endif @@ -69,7 +69,7 @@ public: bool exists(const KEY& e) const { return this->find(e) != this->end(); } private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template diff --git a/gtsam/base/FastSet.h b/gtsam/base/FastSet.h index 58ddd13a0..9a2f62497 100644 --- a/gtsam/base/FastSet.h +++ b/gtsam/base/FastSet.h @@ -20,7 +20,7 @@ #include -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION #include #if BOOST_VERSION >= 107400 #include @@ -125,7 +125,7 @@ public: } private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template diff --git a/gtsam/base/GenericValue.h b/gtsam/base/GenericValue.h index 87ce7a73d..62ea8ec39 100644 --- a/gtsam/base/GenericValue.h +++ b/gtsam/base/GenericValue.h @@ -173,7 +173,7 @@ public: private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template diff --git a/gtsam/base/MatrixSerialization.h b/gtsam/base/MatrixSerialization.h index 9cf730230..11b6a417a 100644 --- a/gtsam/base/MatrixSerialization.h +++ b/gtsam/base/MatrixSerialization.h @@ -19,7 +19,7 @@ // \callgraph // Defined only if boost serialization is enabled -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION #pragma once #include diff --git a/gtsam/base/SymmetricBlockMatrix.h b/gtsam/base/SymmetricBlockMatrix.h index 1783857cb..378e91144 100644 --- a/gtsam/base/SymmetricBlockMatrix.h +++ b/gtsam/base/SymmetricBlockMatrix.h @@ -21,7 +21,7 @@ #include #include #include -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION #include #endif #include @@ -386,7 +386,7 @@ namespace gtsam { template friend class SymmetricBlockMatrixBlockExpr; private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template diff --git a/gtsam/base/Value.h b/gtsam/base/Value.h index c36de42ab..09a3f4878 100644 --- a/gtsam/base/Value.h +++ b/gtsam/base/Value.h @@ -21,7 +21,7 @@ #include // Configuration from CMake #include -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION #include #include #endif @@ -121,7 +121,7 @@ namespace gtsam { * The last two links explain why these export lines have to be in the same source module that includes * any of the archive class headers. * */ -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION friend class boost::serialization::access; template void serialize(ARCHIVE & /*ar*/, const unsigned int /*version*/) { @@ -132,6 +132,6 @@ namespace gtsam { } /* namespace gtsam */ -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION BOOST_SERIALIZATION_ASSUME_ABSTRACT(gtsam::Value) #endif diff --git a/gtsam/base/VectorSerialization.h b/gtsam/base/VectorSerialization.h index 7acf108f4..39d7e0299 100644 --- a/gtsam/base/VectorSerialization.h +++ b/gtsam/base/VectorSerialization.h @@ -17,7 +17,7 @@ */ // Defined only if boost serialization is enabled -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION #pragma once #include diff --git a/gtsam/base/VerticalBlockMatrix.h b/gtsam/base/VerticalBlockMatrix.h index c8fef76e9..0ff8a8ae2 100644 --- a/gtsam/base/VerticalBlockMatrix.h +++ b/gtsam/base/VerticalBlockMatrix.h @@ -221,7 +221,7 @@ namespace gtsam { friend class SymmetricBlockMatrix; private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template diff --git a/gtsam/base/serialization.h b/gtsam/base/serialization.h index e829014ae..ef7ed497f 100644 --- a/gtsam/base/serialization.h +++ b/gtsam/base/serialization.h @@ -21,7 +21,7 @@ #include -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION #include #include diff --git a/gtsam/base/serializationTestHelpers.h b/gtsam/base/serializationTestHelpers.h index fb0701d89..cc6dadafd 100644 --- a/gtsam/base/serializationTestHelpers.h +++ b/gtsam/base/serializationTestHelpers.h @@ -21,7 +21,7 @@ #include -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION #include #include diff --git a/gtsam/base/std_optional_serialization.h b/gtsam/base/std_optional_serialization.h index 7e0f2e844..93a5c8dba 100644 --- a/gtsam/base/std_optional_serialization.h +++ b/gtsam/base/std_optional_serialization.h @@ -11,7 +11,7 @@ #pragma once // Defined only if boost serialization is enabled -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION #include #include diff --git a/gtsam/discrete/DecisionTree-inl.h b/gtsam/discrete/DecisionTree-inl.h index bda44bb9d..efc19d9ee 100644 --- a/gtsam/discrete/DecisionTree-inl.h +++ b/gtsam/discrete/DecisionTree-inl.h @@ -144,7 +144,7 @@ namespace gtsam { private: using Base = DecisionTree::Node; -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template @@ -471,7 +471,7 @@ namespace gtsam { private: using Base = DecisionTree::Node; -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template diff --git a/gtsam/discrete/DecisionTree.h b/gtsam/discrete/DecisionTree.h index 486f798e9..7c8d2742d 100644 --- a/gtsam/discrete/DecisionTree.h +++ b/gtsam/discrete/DecisionTree.h @@ -23,7 +23,7 @@ #include #include -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION #include #endif #include @@ -132,7 +132,7 @@ namespace gtsam { virtual bool isLeaf() const = 0; private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template @@ -440,7 +440,7 @@ namespace gtsam { /// @} private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template diff --git a/gtsam/discrete/DecisionTreeFactor.h b/gtsam/discrete/DecisionTreeFactor.h index e1eb2a453..80ee10a7b 100644 --- a/gtsam/discrete/DecisionTreeFactor.h +++ b/gtsam/discrete/DecisionTreeFactor.h @@ -310,7 +310,7 @@ namespace gtsam { /// @} private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template diff --git a/gtsam/discrete/DiscreteBayesNet.h b/gtsam/discrete/DiscreteBayesNet.h index a5a4621aa..738b91aa5 100644 --- a/gtsam/discrete/DiscreteBayesNet.h +++ b/gtsam/discrete/DiscreteBayesNet.h @@ -147,7 +147,7 @@ class GTSAM_EXPORT DiscreteBayesNet: public BayesNet { /// @} private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template diff --git a/gtsam/discrete/DiscreteConditional.h b/gtsam/discrete/DiscreteConditional.h index 8cba6dbe7..3ec9ae590 100644 --- a/gtsam/discrete/DiscreteConditional.h +++ b/gtsam/discrete/DiscreteConditional.h @@ -275,7 +275,7 @@ class GTSAM_EXPORT DiscreteConditional bool forceComplete) const; private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template diff --git a/gtsam/discrete/DiscreteFactor.h b/gtsam/discrete/DiscreteFactor.h index 29981e94b..a1fde0f86 100644 --- a/gtsam/discrete/DiscreteFactor.h +++ b/gtsam/discrete/DiscreteFactor.h @@ -163,7 +163,7 @@ class GTSAM_EXPORT DiscreteFactor : public Factor { /// @} private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template diff --git a/gtsam/discrete/DiscreteKey.h b/gtsam/discrete/DiscreteKey.h index 44cc192ef..d200792c8 100644 --- a/gtsam/discrete/DiscreteKey.h +++ b/gtsam/discrete/DiscreteKey.h @@ -21,7 +21,7 @@ #include #include -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION #include #endif #include @@ -87,7 +87,7 @@ namespace gtsam { /// Check equality to another DiscreteKeys object. bool equals(const DiscreteKeys& other, double tol = 0) const; -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template diff --git a/gtsam/discrete/DiscreteLookupDAG.h b/gtsam/discrete/DiscreteLookupDAG.h index f077a13d9..afb4d4208 100644 --- a/gtsam/discrete/DiscreteLookupDAG.h +++ b/gtsam/discrete/DiscreteLookupDAG.h @@ -123,7 +123,7 @@ class GTSAM_EXPORT DiscreteLookupDAG : public BayesNet { /// @} private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template diff --git a/gtsam/geometry/BearingRange.h b/gtsam/geometry/BearingRange.h index 07229dfca..9c4e42edf 100644 --- a/gtsam/geometry/BearingRange.h +++ b/gtsam/geometry/BearingRange.h @@ -21,7 +21,7 @@ #include #include #include -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION #include #endif #include @@ -148,7 +148,7 @@ public: /// @{ private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /// Serialization function template void serialize(ARCHIVE& ar, const unsigned int /*version*/) { diff --git a/gtsam/geometry/Cal3.h b/gtsam/geometry/Cal3.h index 5eecee845..63efb0d49 100644 --- a/gtsam/geometry/Cal3.h +++ b/gtsam/geometry/Cal3.h @@ -184,7 +184,7 @@ class GTSAM_EXPORT Cal3 { /// @{ private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION /// +#if GTSAM_ENABLE_BOOST_SERIALIZATION /// /// Serialization function friend class boost::serialization::access; template diff --git a/gtsam/geometry/Cal3Bundler.h b/gtsam/geometry/Cal3Bundler.h index 01eb9437a..71b5fba1d 100644 --- a/gtsam/geometry/Cal3Bundler.h +++ b/gtsam/geometry/Cal3Bundler.h @@ -145,7 +145,7 @@ class GTSAM_EXPORT Cal3Bundler : public Cal3f { /// @name Advanced Interface /// @{ -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template diff --git a/gtsam/geometry/Cal3DS2.h b/gtsam/geometry/Cal3DS2.h index 0ecfc24ae..8c1bbe302 100644 --- a/gtsam/geometry/Cal3DS2.h +++ b/gtsam/geometry/Cal3DS2.h @@ -104,7 +104,7 @@ class GTSAM_EXPORT Cal3DS2 : public Cal3DS2_Base { /// @name Advanced Interface /// @{ -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template diff --git a/gtsam/geometry/Cal3DS2_Base.h b/gtsam/geometry/Cal3DS2_Base.h index 6eff04c10..986c9842b 100644 --- a/gtsam/geometry/Cal3DS2_Base.h +++ b/gtsam/geometry/Cal3DS2_Base.h @@ -156,7 +156,7 @@ class GTSAM_EXPORT Cal3DS2_Base : public Cal3 { /// @name Advanced Interface /// @{ -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template diff --git a/gtsam/geometry/Cal3Fisheye.h b/gtsam/geometry/Cal3Fisheye.h index 4c542c5be..f10a31b65 100644 --- a/gtsam/geometry/Cal3Fisheye.h +++ b/gtsam/geometry/Cal3Fisheye.h @@ -184,7 +184,7 @@ class GTSAM_EXPORT Cal3Fisheye : public Cal3 { /// @name Advanced Interface /// @{ -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template diff --git a/gtsam/geometry/Cal3Unified.h b/gtsam/geometry/Cal3Unified.h index 7eb4d2edb..cef2b8f9b 100644 --- a/gtsam/geometry/Cal3Unified.h +++ b/gtsam/geometry/Cal3Unified.h @@ -138,7 +138,7 @@ class GTSAM_EXPORT Cal3Unified : public Cal3DS2_Base { /// @} private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template diff --git a/gtsam/geometry/Cal3_S2.h b/gtsam/geometry/Cal3_S2.h index 568cde41e..8a587af9b 100644 --- a/gtsam/geometry/Cal3_S2.h +++ b/gtsam/geometry/Cal3_S2.h @@ -132,7 +132,7 @@ class GTSAM_EXPORT Cal3_S2 : public Cal3 { /// @{ private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION /// +#if GTSAM_ENABLE_BOOST_SERIALIZATION /// /// Serialization function friend class boost::serialization::access; template diff --git a/gtsam/geometry/Cal3_S2Stereo.h b/gtsam/geometry/Cal3_S2Stereo.h index 292519cfd..58c7dd37a 100644 --- a/gtsam/geometry/Cal3_S2Stereo.h +++ b/gtsam/geometry/Cal3_S2Stereo.h @@ -143,7 +143,7 @@ class GTSAM_EXPORT Cal3_S2Stereo : public Cal3_S2 { /// @{ private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template diff --git a/gtsam/geometry/Cal3f.h b/gtsam/geometry/Cal3f.h index 6c3c214b8..b6cf4435b 100644 --- a/gtsam/geometry/Cal3f.h +++ b/gtsam/geometry/Cal3f.h @@ -118,7 +118,7 @@ class GTSAM_EXPORT Cal3f : public Cal3 { /// @} private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template diff --git a/gtsam/geometry/CalibratedCamera.h b/gtsam/geometry/CalibratedCamera.h index d3d1e1b9a..d364318c9 100644 --- a/gtsam/geometry/CalibratedCamera.h +++ b/gtsam/geometry/CalibratedCamera.h @@ -25,7 +25,7 @@ #include #include #include -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION #include #endif @@ -230,7 +230,7 @@ public: private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template @@ -406,7 +406,7 @@ private: /// @name Advanced Interface /// @{ -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template diff --git a/gtsam/geometry/CameraSet.h b/gtsam/geometry/CameraSet.h index e3c16f066..36c1e1f54 100644 --- a/gtsam/geometry/CameraSet.h +++ b/gtsam/geometry/CameraSet.h @@ -472,7 +472,7 @@ class CameraSet : public std::vector> { } private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION /// +#if GTSAM_ENABLE_BOOST_SERIALIZATION /// /// Serialization function friend class boost::serialization::access; template diff --git a/gtsam/geometry/EssentialMatrix.h b/gtsam/geometry/EssentialMatrix.h index 3986f3063..74b736685 100644 --- a/gtsam/geometry/EssentialMatrix.h +++ b/gtsam/geometry/EssentialMatrix.h @@ -180,7 +180,7 @@ class EssentialMatrix { /// @name Advanced Interface /// @{ -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template diff --git a/gtsam/geometry/PinholeCamera.h b/gtsam/geometry/PinholeCamera.h index 0439b2fde..f8599572d 100644 --- a/gtsam/geometry/PinholeCamera.h +++ b/gtsam/geometry/PinholeCamera.h @@ -324,7 +324,7 @@ public: private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template diff --git a/gtsam/geometry/PinholePose.h b/gtsam/geometry/PinholePose.h index f1191dbcc..252d8bc6f 100644 --- a/gtsam/geometry/PinholePose.h +++ b/gtsam/geometry/PinholePose.h @@ -212,7 +212,7 @@ public: private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template @@ -425,7 +425,7 @@ public: private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template diff --git a/gtsam/geometry/PinholeSet.h b/gtsam/geometry/PinholeSet.h index 55269f9d6..45cdb11e4 100644 --- a/gtsam/geometry/PinholeSet.h +++ b/gtsam/geometry/PinholeSet.h @@ -64,7 +64,7 @@ public: private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION /// +#if GTSAM_ENABLE_BOOST_SERIALIZATION /// /// Serialization function friend class boost::serialization::access; template diff --git a/gtsam/geometry/Point2.h b/gtsam/geometry/Point2.h index 76beea11a..66d57f785 100644 --- a/gtsam/geometry/Point2.h +++ b/gtsam/geometry/Point2.h @@ -19,7 +19,7 @@ #include #include -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION #include #endif diff --git a/gtsam/geometry/Point3.h b/gtsam/geometry/Point3.h index d5c32816e..9bc943a43 100644 --- a/gtsam/geometry/Point3.h +++ b/gtsam/geometry/Point3.h @@ -26,7 +26,7 @@ #include #include #include -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION #include #endif #include diff --git a/gtsam/geometry/Pose2.h b/gtsam/geometry/Pose2.h index ff9a6399e..3f85a7f42 100644 --- a/gtsam/geometry/Pose2.h +++ b/gtsam/geometry/Pose2.h @@ -339,7 +339,7 @@ public: private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION // +#if GTSAM_ENABLE_BOOST_SERIALIZATION // // Serialization function friend class boost::serialization::access; template diff --git a/gtsam/geometry/Pose3.h b/gtsam/geometry/Pose3.h index 81a9848e2..eefbd7654 100644 --- a/gtsam/geometry/Pose3.h +++ b/gtsam/geometry/Pose3.h @@ -406,7 +406,7 @@ public: friend std::ostream &operator<<(std::ostream &os, const Pose3& p); private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template diff --git a/gtsam/geometry/Rot2.h b/gtsam/geometry/Rot2.h index 6bb97ff6c..07f328b96 100644 --- a/gtsam/geometry/Rot2.h +++ b/gtsam/geometry/Rot2.h @@ -213,7 +213,7 @@ namespace gtsam { static Rot2 ClosestTo(const Matrix2& M); private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template diff --git a/gtsam/geometry/Rot3.h b/gtsam/geometry/Rot3.h index d1e330438..71f4cbacd 100644 --- a/gtsam/geometry/Rot3.h +++ b/gtsam/geometry/Rot3.h @@ -532,7 +532,7 @@ class GTSAM_EXPORT Rot3 : public LieGroup { /// @} private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template diff --git a/gtsam/geometry/SO3.h b/gtsam/geometry/SO3.h index 163ae6623..36329a19d 100644 --- a/gtsam/geometry/SO3.h +++ b/gtsam/geometry/SO3.h @@ -98,7 +98,7 @@ template <> GTSAM_EXPORT Vector9 SO3::vec(OptionalJacobian<9, 3> H) const; -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION template /** Serialization function */ void serialize(Archive& ar, SO3& R, const unsigned int /*version*/) { diff --git a/gtsam/geometry/SO4.h b/gtsam/geometry/SO4.h index 834cab746..43b74f7aa 100644 --- a/gtsam/geometry/SO4.h +++ b/gtsam/geometry/SO4.h @@ -78,7 +78,7 @@ GTSAM_EXPORT Matrix3 topLeft(const SO4 &Q, OptionalJacobian<9, 6> H = {}); */ GTSAM_EXPORT Matrix43 stiefel(const SO4 &Q, OptionalJacobian<12, 6> H = {}); -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION template /** Serialization function */ void serialize(Archive &ar, SO4 &Q, const unsigned int /*version*/) { diff --git a/gtsam/geometry/SOn.h b/gtsam/geometry/SOn.h index b0da3b213..e8e1f641d 100644 --- a/gtsam/geometry/SOn.h +++ b/gtsam/geometry/SOn.h @@ -24,7 +24,7 @@ #include #include -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION #include #endif @@ -326,7 +326,7 @@ class SO : public LieGroup, internal::DimensionSO(N)> { /// @name Serialization /// @{ -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION template friend void save(Archive&, SO&, const unsigned int); template @@ -380,7 +380,7 @@ template <> GTSAM_EXPORT typename SOn::VectorN2 SOn::vec(DynamicJacobian H) const; -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ template void serialize( diff --git a/gtsam/geometry/Similarity3.h b/gtsam/geometry/Similarity3.h index 05bd0431e..8f7f30be8 100644 --- a/gtsam/geometry/Similarity3.h +++ b/gtsam/geometry/Similarity3.h @@ -203,7 +203,7 @@ class GTSAM_EXPORT Similarity3 : public LieGroup { private: - #ifdef GTSAM_ENABLE_BOOST_SERIALIZATION + #if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template diff --git a/gtsam/geometry/SphericalCamera.h b/gtsam/geometry/SphericalCamera.h index 1ff87cec3..786295d7b 100644 --- a/gtsam/geometry/SphericalCamera.h +++ b/gtsam/geometry/SphericalCamera.h @@ -26,7 +26,7 @@ #include #include -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION #include #endif @@ -54,7 +54,7 @@ class GTSAM_EXPORT EmptyCal { } private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION /// +#if GTSAM_ENABLE_BOOST_SERIALIZATION /// /// Serialization function friend class boost::serialization::access; template @@ -223,7 +223,7 @@ class GTSAM_EXPORT SphericalCamera { static size_t Dim() { return 6; } private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template diff --git a/gtsam/geometry/StereoCamera.h b/gtsam/geometry/StereoCamera.h index 026125572..315e4d267 100644 --- a/gtsam/geometry/StereoCamera.h +++ b/gtsam/geometry/StereoCamera.h @@ -177,7 +177,7 @@ public: private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION friend class boost::serialization::access; template void serialize(Archive & ar, const unsigned int /*version*/) { diff --git a/gtsam/geometry/StereoPoint2.h b/gtsam/geometry/StereoPoint2.h index 7a1910e42..26c14750b 100644 --- a/gtsam/geometry/StereoPoint2.h +++ b/gtsam/geometry/StereoPoint2.h @@ -20,7 +20,7 @@ #include #include -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION #include #endif @@ -150,7 +150,7 @@ private: /// @name Advanced Interface /// @{ -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template diff --git a/gtsam/geometry/Unit3.h b/gtsam/geometry/Unit3.h index a7304280f..65a304d62 100644 --- a/gtsam/geometry/Unit3.h +++ b/gtsam/geometry/Unit3.h @@ -198,7 +198,7 @@ private: /// @name Advanced Interface /// @{ -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template diff --git a/gtsam/geometry/tests/testBearingRange.cpp b/gtsam/geometry/tests/testBearingRange.cpp index ff2a9a6a4..df4a4dc51 100644 --- a/gtsam/geometry/tests/testBearingRange.cpp +++ b/gtsam/geometry/tests/testBearingRange.cpp @@ -56,7 +56,7 @@ TEST(BearingRange, 3D) { EXPECT(assert_equal(expected, actual)); } -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION using namespace serializationTestHelpers; /* ************************************************************************* */ TEST(BearingRange, Serialization2D) { diff --git a/gtsam/geometry/tests/testUnit3.cpp b/gtsam/geometry/tests/testUnit3.cpp index 0c4c2c725..855e50a42 100644 --- a/gtsam/geometry/tests/testUnit3.cpp +++ b/gtsam/geometry/tests/testUnit3.cpp @@ -499,7 +499,7 @@ TEST(Unit3, CopyAssign) { } /* ************************************************************************* */ -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION TEST(actualH, Serialization) { Unit3 p(0, 1, 0); EXPECT(serializationTestHelpers::equalsObj(p)); diff --git a/gtsam/geometry/triangulation.h b/gtsam/geometry/triangulation.h index 9087f01c5..749824845 100644 --- a/gtsam/geometry/triangulation.h +++ b/gtsam/geometry/triangulation.h @@ -622,7 +622,7 @@ struct GTSAM_EXPORT TriangulationParameters { private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION /// +#if GTSAM_ENABLE_BOOST_SERIALIZATION /// /// Serialization function friend class boost::serialization::access; template @@ -687,7 +687,7 @@ class TriangulationResult : public std::optional { } private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION /// +#if GTSAM_ENABLE_BOOST_SERIALIZATION /// /// Serialization function friend class boost::serialization::access; template diff --git a/gtsam/hybrid/HybridBayesNet.h b/gtsam/hybrid/HybridBayesNet.h index 96afb87d6..3e07c71ce 100644 --- a/gtsam/hybrid/HybridBayesNet.h +++ b/gtsam/hybrid/HybridBayesNet.h @@ -268,7 +268,7 @@ class GTSAM_EXPORT HybridBayesNet : public BayesNet { /// @} private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template diff --git a/gtsam/hybrid/HybridBayesTree.h b/gtsam/hybrid/HybridBayesTree.h index 0fe9ca6ea..06d880f02 100644 --- a/gtsam/hybrid/HybridBayesTree.h +++ b/gtsam/hybrid/HybridBayesTree.h @@ -115,7 +115,7 @@ class GTSAM_EXPORT HybridBayesTree : public BayesTree { /// @} private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template diff --git a/gtsam/hybrid/HybridConditional.h b/gtsam/hybrid/HybridConditional.h index f3df725ef..a08b3a6ee 100644 --- a/gtsam/hybrid/HybridConditional.h +++ b/gtsam/hybrid/HybridConditional.h @@ -217,7 +217,7 @@ class GTSAM_EXPORT HybridConditional /// @} private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template diff --git a/gtsam/hybrid/HybridFactor.h b/gtsam/hybrid/HybridFactor.h index 8d6fef3f4..9fc280322 100644 --- a/gtsam/hybrid/HybridFactor.h +++ b/gtsam/hybrid/HybridFactor.h @@ -140,7 +140,7 @@ class GTSAM_EXPORT HybridFactor : public Factor { /// @} private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template diff --git a/gtsam/hybrid/HybridGaussianConditional.h b/gtsam/hybrid/HybridGaussianConditional.h index 0d2b1323c..e769662ed 100644 --- a/gtsam/hybrid/HybridGaussianConditional.h +++ b/gtsam/hybrid/HybridGaussianConditional.h @@ -253,7 +253,7 @@ class GTSAM_EXPORT HybridGaussianConditional /// Check whether `given` has values for all frontal keys. bool allFrontalsGiven(const VectorValues &given) const; -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template diff --git a/gtsam/hybrid/HybridGaussianFactor.h b/gtsam/hybrid/HybridGaussianFactor.h index 6d1064647..efbba9e51 100644 --- a/gtsam/hybrid/HybridGaussianFactor.h +++ b/gtsam/hybrid/HybridGaussianFactor.h @@ -176,7 +176,7 @@ class GTSAM_EXPORT HybridGaussianFactor : public HybridFactor { // Private constructor using ConstructorHelper above. HybridGaussianFactor(const ConstructorHelper &helper); -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template diff --git a/gtsam/hybrid/HybridGaussianProductFactor.h b/gtsam/hybrid/HybridGaussianProductFactor.h index 60a58a3a5..f5e75aa86 100644 --- a/gtsam/hybrid/HybridGaussianProductFactor.h +++ b/gtsam/hybrid/HybridGaussianProductFactor.h @@ -119,7 +119,7 @@ class GTSAM_EXPORT HybridGaussianProductFactor ///@} private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template diff --git a/gtsam/inference/BayesTree.h b/gtsam/inference/BayesTree.h index d0ebd916e..03f79c8cf 100644 --- a/gtsam/inference/BayesTree.h +++ b/gtsam/inference/BayesTree.h @@ -262,7 +262,7 @@ namespace gtsam { template friend class EliminatableClusterTree; private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template diff --git a/gtsam/inference/BayesTreeCliqueBase.h b/gtsam/inference/BayesTreeCliqueBase.h index adffa2f14..0ccb04e90 100644 --- a/gtsam/inference/BayesTreeCliqueBase.h +++ b/gtsam/inference/BayesTreeCliqueBase.h @@ -208,7 +208,7 @@ namespace gtsam { private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template diff --git a/gtsam/inference/Conditional.h b/gtsam/inference/Conditional.h index f9da36b7b..31e451714 100644 --- a/gtsam/inference/Conditional.h +++ b/gtsam/inference/Conditional.h @@ -233,7 +233,7 @@ namespace gtsam { // Cast to derived type (const) (casts down to derived conditional type, then up to factor type) const FACTOR& asFactor() const { return static_cast(static_cast(*this)); } -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template diff --git a/gtsam/inference/Factor.h b/gtsam/inference/Factor.h index 074151e16..19741b22e 100644 --- a/gtsam/inference/Factor.h +++ b/gtsam/inference/Factor.h @@ -21,7 +21,7 @@ #pragma once -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION #include #endif #include @@ -193,7 +193,7 @@ namespace gtsam { /// @} private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /// @name Serialization /// @{ /** Serialization function */ diff --git a/gtsam/inference/FactorGraph.h b/gtsam/inference/FactorGraph.h index b6046d0bb..ca7321359 100644 --- a/gtsam/inference/FactorGraph.h +++ b/gtsam/inference/FactorGraph.h @@ -29,7 +29,7 @@ #include // for Eigen::aligned_allocator -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION #include #include #endif @@ -420,7 +420,7 @@ class FactorGraph { inline bool exists(size_t idx) const { return idx < size() && at(idx); } private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template diff --git a/gtsam/inference/LabeledSymbol.h b/gtsam/inference/LabeledSymbol.h index 927fb7669..16ac7bccc 100644 --- a/gtsam/inference/LabeledSymbol.h +++ b/gtsam/inference/LabeledSymbol.h @@ -142,7 +142,7 @@ class GTSAM_EXPORT LabeledSymbol { /// @} private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /// Serialization function friend class boost::serialization::access; template diff --git a/gtsam/inference/Ordering.h b/gtsam/inference/Ordering.h index c3df1b8a0..fced5d4f2 100644 --- a/gtsam/inference/Ordering.h +++ b/gtsam/inference/Ordering.h @@ -252,7 +252,7 @@ private: static Ordering ColamdConstrained( const VariableIndex& variableIndex, std::vector& cmember); -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template diff --git a/gtsam/inference/Symbol.h b/gtsam/inference/Symbol.h index 9f2133707..a62c22fac 100644 --- a/gtsam/inference/Symbol.h +++ b/gtsam/inference/Symbol.h @@ -21,7 +21,7 @@ #include #include -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION #include #endif #include @@ -124,7 +124,7 @@ public: private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template diff --git a/gtsam/inference/VariableIndex.h b/gtsam/inference/VariableIndex.h index 110c0bba4..2f872b25d 100644 --- a/gtsam/inference/VariableIndex.h +++ b/gtsam/inference/VariableIndex.h @@ -192,7 +192,7 @@ protected: } private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template diff --git a/gtsam/linear/GaussianBayesNet.h b/gtsam/linear/GaussianBayesNet.h index e858bbe43..538d1532e 100644 --- a/gtsam/linear/GaussianBayesNet.h +++ b/gtsam/linear/GaussianBayesNet.h @@ -267,7 +267,7 @@ namespace gtsam { /// @} private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template diff --git a/gtsam/linear/GaussianConditional.h b/gtsam/linear/GaussianConditional.h index 27270fece..14b1ce87f 100644 --- a/gtsam/linear/GaussianConditional.h +++ b/gtsam/linear/GaussianConditional.h @@ -278,7 +278,7 @@ namespace gtsam { /// @} private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template diff --git a/gtsam/linear/GaussianFactor.h b/gtsam/linear/GaussianFactor.h index c5133c6b2..253bbff0f 100644 --- a/gtsam/linear/GaussianFactor.h +++ b/gtsam/linear/GaussianFactor.h @@ -179,7 +179,7 @@ namespace gtsam { /// @} private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template diff --git a/gtsam/linear/GaussianFactorGraph.h b/gtsam/linear/GaussianFactorGraph.h index 6a7848f51..67313c086 100644 --- a/gtsam/linear/GaussianFactorGraph.h +++ b/gtsam/linear/GaussianFactorGraph.h @@ -400,7 +400,7 @@ namespace gtsam { /// @} private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template diff --git a/gtsam/linear/HessianFactor.h b/gtsam/linear/HessianFactor.h index 56b3d6537..189020712 100644 --- a/gtsam/linear/HessianFactor.h +++ b/gtsam/linear/HessianFactor.h @@ -370,7 +370,7 @@ namespace gtsam { friend class NonlinearFactorGraph; friend class NonlinearClusterTree; -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template diff --git a/gtsam/linear/JacobianFactor.h b/gtsam/linear/JacobianFactor.h index 407ed1e27..4a5b6d5d1 100644 --- a/gtsam/linear/JacobianFactor.h +++ b/gtsam/linear/JacobianFactor.h @@ -24,7 +24,7 @@ #include #include -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION #include #include #endif @@ -421,7 +421,7 @@ namespace gtsam { // be very selective on who can access these private methods: template friend class ExpressionFactor; -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template @@ -468,7 +468,7 @@ struct traits : public Testable { } // \ namespace gtsam -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION BOOST_CLASS_VERSION(gtsam::JacobianFactor, 1) #endif diff --git a/gtsam/linear/LossFunctions.h b/gtsam/linear/LossFunctions.h index a8902e11b..989557d87 100644 --- a/gtsam/linear/LossFunctions.h +++ b/gtsam/linear/LossFunctions.h @@ -24,7 +24,7 @@ #include #include -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION #include #include #include @@ -129,7 +129,7 @@ class GTSAM_EXPORT Base { void reweight(Matrix &A1, Matrix &A2, Matrix &A3, Vector &error) const; private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template @@ -161,7 +161,7 @@ class GTSAM_EXPORT Null : public Base { static shared_ptr Create(); private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template @@ -195,7 +195,7 @@ class GTSAM_EXPORT Fair : public Base { double modelParameter() const { return c_; } private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template @@ -230,7 +230,7 @@ class GTSAM_EXPORT Huber : public Base { double modelParameter() const { return k_; } private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template @@ -270,7 +270,7 @@ class GTSAM_EXPORT Cauchy : public Base { double modelParameter() const { return k_; } private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template @@ -306,7 +306,7 @@ class GTSAM_EXPORT Tukey : public Base { double modelParameter() const { return c_; } private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template @@ -341,7 +341,7 @@ class GTSAM_EXPORT Welsch : public Base { double modelParameter() const { return c_; } private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template @@ -380,7 +380,7 @@ class GTSAM_EXPORT GemanMcClure : public Base { double c_; private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template @@ -420,7 +420,7 @@ class GTSAM_EXPORT DCS : public Base { double c_; private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template @@ -460,7 +460,7 @@ class GTSAM_EXPORT L2WithDeadZone : public Base { double modelParameter() const { return k_; } private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template @@ -496,7 +496,7 @@ class GTSAM_EXPORT AsymmetricTukey : public Base { double modelParameter() const { return c_; } private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template @@ -532,7 +532,7 @@ class GTSAM_EXPORT AsymmetricCauchy : public Base { double modelParameter() const { return k_; } private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template @@ -577,7 +577,7 @@ class GTSAM_EXPORT Custom : public Base { inline Custom() = default; private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template diff --git a/gtsam/linear/NoiseModel.h b/gtsam/linear/NoiseModel.h index 34fa63e4c..47a84654a 100644 --- a/gtsam/linear/NoiseModel.h +++ b/gtsam/linear/NoiseModel.h @@ -24,7 +24,7 @@ #include #include -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION #include #include #include @@ -141,7 +141,7 @@ namespace gtsam { virtual double weight(const Vector& v) const { return 1.0; } private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template @@ -280,7 +280,7 @@ namespace gtsam { double negLogConstant() const; private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template @@ -375,7 +375,7 @@ namespace gtsam { } private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template @@ -520,7 +520,7 @@ namespace gtsam { shared_ptr unit() const; private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template @@ -593,7 +593,7 @@ namespace gtsam { inline double sigma() const { return sigma_; } private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template @@ -648,7 +648,7 @@ namespace gtsam { void unwhitenInPlace(Eigen::Block& /*v*/) const override {} private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template @@ -739,7 +739,7 @@ namespace gtsam { const RobustModel::shared_ptr &robust, const NoiseModel::shared_ptr noise); private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template diff --git a/gtsam/linear/SubgraphBuilder.cpp b/gtsam/linear/SubgraphBuilder.cpp index 834fc8d12..9520b826b 100644 --- a/gtsam/linear/SubgraphBuilder.cpp +++ b/gtsam/linear/SubgraphBuilder.cpp @@ -25,7 +25,7 @@ #include #include -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION #include #include #include @@ -71,7 +71,7 @@ vector Subgraph::edgeIndices() const { return eid; } -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /****************************************************************************/ void Subgraph::save(const std::string &fn) const { std::ofstream os(fn.c_str()); diff --git a/gtsam/linear/SubgraphBuilder.h b/gtsam/linear/SubgraphBuilder.h index f9ddd4c9a..df77ea9de 100644 --- a/gtsam/linear/SubgraphBuilder.h +++ b/gtsam/linear/SubgraphBuilder.h @@ -21,7 +21,7 @@ #include #include -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION #include #include #endif @@ -51,7 +51,7 @@ class GTSAM_EXPORT Subgraph { friend std::ostream &operator<<(std::ostream &os, const Edge &edge); private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION friend class boost::serialization::access; template void serialize(Archive &ar, const unsigned int /*version*/) { @@ -87,7 +87,7 @@ class GTSAM_EXPORT Subgraph { friend std::ostream &operator<<(std::ostream &os, const Subgraph &subgraph); private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION friend class boost::serialization::access; template void serialize(Archive &ar, const unsigned int /*version*/) { diff --git a/gtsam/linear/VectorValues.h b/gtsam/linear/VectorValues.h index 7fbd43ffc..e60209a83 100644 --- a/gtsam/linear/VectorValues.h +++ b/gtsam/linear/VectorValues.h @@ -371,7 +371,7 @@ namespace gtsam { /// @} private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template diff --git a/gtsam/navigation/AHRSFactor.h b/gtsam/navigation/AHRSFactor.h index 4289b4969..496fcde3d 100644 --- a/gtsam/navigation/AHRSFactor.h +++ b/gtsam/navigation/AHRSFactor.h @@ -120,7 +120,7 @@ class GTSAM_EXPORT PreintegratedAhrsMeasurements : public PreintegratedRotation private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template @@ -208,7 +208,7 @@ public: private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template diff --git a/gtsam/navigation/AttitudeFactor.h b/gtsam/navigation/AttitudeFactor.h index 2fada724d..e342e451e 100644 --- a/gtsam/navigation/AttitudeFactor.h +++ b/gtsam/navigation/AttitudeFactor.h @@ -73,7 +73,7 @@ class AttitudeFactor { const Unit3& bRef() const { return bMeasured_; } #endif -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template @@ -138,7 +138,7 @@ class GTSAM_EXPORT Rot3AttitudeFactor : public NoiseModelFactorN, } private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template @@ -220,7 +220,7 @@ class GTSAM_EXPORT Pose3AttitudeFactor : public NoiseModelFactorN, } private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template diff --git a/gtsam/navigation/BarometricFactor.h b/gtsam/navigation/BarometricFactor.h index 70cae8d36..545928ca9 100644 --- a/gtsam/navigation/BarometricFactor.h +++ b/gtsam/navigation/BarometricFactor.h @@ -97,7 +97,7 @@ class GTSAM_EXPORT BarometricFactor : public NoiseModelFactorN { } private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION /// +#if GTSAM_ENABLE_BOOST_SERIALIZATION /// /// Serialization function friend class boost::serialization::access; template diff --git a/gtsam/navigation/CombinedImuFactor.cpp b/gtsam/navigation/CombinedImuFactor.cpp index a2eb9e0c0..64ee86be9 100644 --- a/gtsam/navigation/CombinedImuFactor.cpp +++ b/gtsam/navigation/CombinedImuFactor.cpp @@ -21,7 +21,7 @@ **/ #include -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION #include #endif diff --git a/gtsam/navigation/CombinedImuFactor.h b/gtsam/navigation/CombinedImuFactor.h index 0ffb386a0..b4c3ae8dc 100644 --- a/gtsam/navigation/CombinedImuFactor.h +++ b/gtsam/navigation/CombinedImuFactor.h @@ -169,7 +169,7 @@ class GTSAM_EXPORT PreintegratedCombinedMeasurements /// @} private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION /// +#if GTSAM_ENABLE_BOOST_SERIALIZATION /// /// Serialization function friend class boost::serialization::access; template @@ -281,7 +281,7 @@ class GTSAM_EXPORT CombinedImuFactor OptionalMatrixType H6) const override; private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template diff --git a/gtsam/navigation/GPSFactor.h b/gtsam/navigation/GPSFactor.h index cd89dd464..6af184360 100644 --- a/gtsam/navigation/GPSFactor.h +++ b/gtsam/navigation/GPSFactor.h @@ -97,7 +97,7 @@ public: private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION /// +#if GTSAM_ENABLE_BOOST_SERIALIZATION /// /// Serialization function friend class boost::serialization::access; template @@ -166,7 +166,7 @@ public: private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /// Serialization function friend class boost::serialization::access; template diff --git a/gtsam/navigation/ImuBias.h b/gtsam/navigation/ImuBias.h index 6df87eda6..02a020c42 100644 --- a/gtsam/navigation/ImuBias.h +++ b/gtsam/navigation/ImuBias.h @@ -20,7 +20,7 @@ #include #include #include -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION #include #endif @@ -142,7 +142,7 @@ private: /// @name Advanced Interface /// @{ -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template diff --git a/gtsam/navigation/ImuFactor.h b/gtsam/navigation/ImuFactor.h index 7254838fd..b19989a77 100644 --- a/gtsam/navigation/ImuFactor.h +++ b/gtsam/navigation/ImuFactor.h @@ -142,7 +142,7 @@ public: #endif private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION /// +#if GTSAM_ENABLE_BOOST_SERIALIZATION /// /// Serialization function friend class boost::serialization::access; template @@ -244,7 +244,7 @@ public: private: /** Serialization function */ -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION friend class boost::serialization::access; template void serialize(ARCHIVE & ar, const unsigned int /*version*/) { @@ -316,7 +316,7 @@ public: private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template diff --git a/gtsam/navigation/MagPoseFactor.h b/gtsam/navigation/MagPoseFactor.h index ba9a08d78..ea0b30c75 100644 --- a/gtsam/navigation/MagPoseFactor.h +++ b/gtsam/navigation/MagPoseFactor.h @@ -132,7 +132,7 @@ class MagPoseFactor: public NoiseModelFactorN { } private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION /// +#if GTSAM_ENABLE_BOOST_SERIALIZATION /// /// Serialization function. friend class boost::serialization::access; template diff --git a/gtsam/navigation/ManifoldPreintegration.h b/gtsam/navigation/ManifoldPreintegration.h index 40691c445..43228044a 100644 --- a/gtsam/navigation/ManifoldPreintegration.h +++ b/gtsam/navigation/ManifoldPreintegration.h @@ -113,7 +113,7 @@ public: /// @} private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template diff --git a/gtsam/navigation/NavState.h b/gtsam/navigation/NavState.h index 95ee71fe9..e246cbe27 100644 --- a/gtsam/navigation/NavState.h +++ b/gtsam/navigation/NavState.h @@ -269,7 +269,7 @@ public: private: /// @{ /// serialization -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION /// +#if GTSAM_ENABLE_BOOST_SERIALIZATION /// friend class boost::serialization::access; template void serialize(ARCHIVE & ar, const unsigned int /*version*/) { diff --git a/gtsam/navigation/PreintegratedRotation.h b/gtsam/navigation/PreintegratedRotation.h index 49b1aa4c1..a887ef321 100644 --- a/gtsam/navigation/PreintegratedRotation.h +++ b/gtsam/navigation/PreintegratedRotation.h @@ -85,7 +85,7 @@ struct GTSAM_EXPORT PreintegratedRotationParams { std::optional getBodyPSensor() const { return body_P_sensor; } private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template @@ -227,7 +227,7 @@ class GTSAM_EXPORT PreintegratedRotation { /// @} private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template diff --git a/gtsam/navigation/PreintegrationBase.h b/gtsam/navigation/PreintegrationBase.h index e4a790b9d..de3a380cf 100644 --- a/gtsam/navigation/PreintegrationBase.h +++ b/gtsam/navigation/PreintegrationBase.h @@ -173,7 +173,7 @@ class GTSAM_EXPORT PreintegrationBase { OptionalJacobian<9, 6> H5 = {}) const; private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template diff --git a/gtsam/navigation/PreintegrationCombinedParams.h b/gtsam/navigation/PreintegrationCombinedParams.h index 6be05c082..b384a36e7 100644 --- a/gtsam/navigation/PreintegrationCombinedParams.h +++ b/gtsam/navigation/PreintegrationCombinedParams.h @@ -84,7 +84,7 @@ struct GTSAM_EXPORT PreintegrationCombinedParams : PreintegrationParams { const Matrix6& getBiasAccOmegaInit() const { return biasAccOmegaInt; } private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template diff --git a/gtsam/navigation/PreintegrationParams.h b/gtsam/navigation/PreintegrationParams.h index 8435d0bad..6ea9e73b3 100644 --- a/gtsam/navigation/PreintegrationParams.h +++ b/gtsam/navigation/PreintegrationParams.h @@ -71,7 +71,7 @@ struct GTSAM_EXPORT PreintegrationParams: PreintegratedRotationParams { protected: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template diff --git a/gtsam/navigation/TangentPreintegration.h b/gtsam/navigation/TangentPreintegration.h index e16ad9310..7f5c63d50 100644 --- a/gtsam/navigation/TangentPreintegration.h +++ b/gtsam/navigation/TangentPreintegration.h @@ -127,7 +127,7 @@ public: /// @} private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template diff --git a/gtsam/nonlinear/CustomFactor.h b/gtsam/nonlinear/CustomFactor.h index c4015db37..5f8a19ff0 100644 --- a/gtsam/nonlinear/CustomFactor.h +++ b/gtsam/nonlinear/CustomFactor.h @@ -88,7 +88,7 @@ public: private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template diff --git a/gtsam/nonlinear/ExpressionFactor.h b/gtsam/nonlinear/ExpressionFactor.h index d9af1933c..4f6648849 100644 --- a/gtsam/nonlinear/ExpressionFactor.h +++ b/gtsam/nonlinear/ExpressionFactor.h @@ -200,7 +200,7 @@ protected: } private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /// Save to an archive: just saves the base class template void save(Archive& ar, const unsigned int /*version*/) const { @@ -287,7 +287,7 @@ private: return expression(keys); } -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION friend class boost::serialization::access; template void serialize(ARCHIVE &ar, const unsigned int /*version*/) { diff --git a/gtsam/nonlinear/FunctorizedFactor.h b/gtsam/nonlinear/FunctorizedFactor.h index 128de5847..ca253179b 100644 --- a/gtsam/nonlinear/FunctorizedFactor.h +++ b/gtsam/nonlinear/FunctorizedFactor.h @@ -119,7 +119,7 @@ class FunctorizedFactor : public NoiseModelFactorN { /// @} private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template @@ -231,7 +231,7 @@ class FunctorizedFactor2 : public NoiseModelFactorN { /// @} private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template diff --git a/gtsam/nonlinear/ISAM2.h b/gtsam/nonlinear/ISAM2.h index d3abd95fd..f71b4b7da 100644 --- a/gtsam/nonlinear/ISAM2.h +++ b/gtsam/nonlinear/ISAM2.h @@ -340,7 +340,7 @@ class GTSAM_EXPORT ISAM2 : public BayesTree { void updateDelta(bool forceFullSolve = false) const; private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template diff --git a/gtsam/nonlinear/ISAM2Clique.h b/gtsam/nonlinear/ISAM2Clique.h index fde9dd3f2..39bce432a 100644 --- a/gtsam/nonlinear/ISAM2Clique.h +++ b/gtsam/nonlinear/ISAM2Clique.h @@ -146,7 +146,7 @@ class GTSAM_EXPORT ISAM2Clique void restoreFromOriginals(const Vector& originalValues, VectorValues* delta) const; -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template diff --git a/gtsam/nonlinear/LinearContainerFactor.h b/gtsam/nonlinear/LinearContainerFactor.h index dfbf221c3..f63ecb0c6 100644 --- a/gtsam/nonlinear/LinearContainerFactor.h +++ b/gtsam/nonlinear/LinearContainerFactor.h @@ -163,7 +163,7 @@ public: void initializeLinearizationPoint(const Values& linearizationPoint); private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template diff --git a/gtsam/nonlinear/NonlinearEquality.h b/gtsam/nonlinear/NonlinearEquality.h index f81486b30..538a95208 100644 --- a/gtsam/nonlinear/NonlinearEquality.h +++ b/gtsam/nonlinear/NonlinearEquality.h @@ -180,7 +180,7 @@ public: private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION /// +#if GTSAM_ENABLE_BOOST_SERIALIZATION /// /// Serialization function friend class boost::serialization::access; template @@ -273,7 +273,7 @@ public: private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION /// +#if GTSAM_ENABLE_BOOST_SERIALIZATION /// /// Serialization function friend class boost::serialization::access; template @@ -344,7 +344,7 @@ class NonlinearEquality2 : public NoiseModelFactorN { GTSAM_MAKE_ALIGNED_OPERATOR_NEW private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /// Serialization function friend class boost::serialization::access; template diff --git a/gtsam/nonlinear/NonlinearFactor.h b/gtsam/nonlinear/NonlinearFactor.h index 101743b78..1af01f60d 100644 --- a/gtsam/nonlinear/NonlinearFactor.h +++ b/gtsam/nonlinear/NonlinearFactor.h @@ -29,7 +29,7 @@ #include #include -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION #include #endif #include @@ -303,7 +303,7 @@ public: shared_ptr cloneWithNewNoiseModel(const SharedNoiseModel newNoise) const; private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template @@ -715,7 +715,7 @@ protected: } } -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template diff --git a/gtsam/nonlinear/NonlinearFactorGraph.h b/gtsam/nonlinear/NonlinearFactorGraph.h index bf56d835f..8e3443dc7 100644 --- a/gtsam/nonlinear/NonlinearFactorGraph.h +++ b/gtsam/nonlinear/NonlinearFactorGraph.h @@ -250,7 +250,7 @@ namespace gtsam { std::shared_ptr linearizeToHessianFactor( const Values& values, const Scatter& scatter, const Dampen& dampen = nullptr) const; -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template diff --git a/gtsam/nonlinear/PriorFactor.h b/gtsam/nonlinear/PriorFactor.h index 159373459..0979be3df 100644 --- a/gtsam/nonlinear/PriorFactor.h +++ b/gtsam/nonlinear/PriorFactor.h @@ -105,7 +105,7 @@ namespace gtsam { private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template diff --git a/gtsam/nonlinear/Values.h b/gtsam/nonlinear/Values.h index 2fe64d9c9..7e950479a 100644 --- a/gtsam/nonlinear/Values.h +++ b/gtsam/nonlinear/Values.h @@ -29,7 +29,7 @@ #include #include -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION #include #endif @@ -393,7 +393,7 @@ namespace gtsam { return filter(key_value.key) && (dynamic_cast*>(&key_value.value)); } -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template diff --git a/gtsam/sam/BearingFactor.h b/gtsam/sam/BearingFactor.h index 01b2baf18..97f23ed8f 100644 --- a/gtsam/sam/BearingFactor.h +++ b/gtsam/sam/BearingFactor.h @@ -75,7 +75,7 @@ struct BearingFactor : public ExpressionFactorN { private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION friend class boost::serialization::access; template void serialize(ARCHIVE& ar, const unsigned int /*version*/) { diff --git a/gtsam/sam/BearingRangeFactor.h b/gtsam/sam/BearingRangeFactor.h index d5467be47..aa08fea17 100644 --- a/gtsam/sam/BearingRangeFactor.h +++ b/gtsam/sam/BearingRangeFactor.h @@ -92,7 +92,7 @@ class BearingRangeFactor private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION friend class boost::serialization::access; template void serialize(ARCHIVE& ar, const unsigned int /*version*/) { diff --git a/gtsam/sam/RangeFactor.h b/gtsam/sam/RangeFactor.h index 193a89a5a..0ccc54afd 100644 --- a/gtsam/sam/RangeFactor.h +++ b/gtsam/sam/RangeFactor.h @@ -80,7 +80,7 @@ class RangeFactor : public ExpressionFactorN { } private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION friend class boost::serialization::access; template void serialize(ARCHIVE& ar, const unsigned int /*version*/) { @@ -164,7 +164,7 @@ class RangeFactorWithTransform : public ExpressionFactorN { } private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION friend class boost::serialization::access; /** Serialization function */ template diff --git a/gtsam/sfm/SfmData.h b/gtsam/sfm/SfmData.h index 7161ab2f9..dbe336979 100644 --- a/gtsam/sfm/SfmData.h +++ b/gtsam/sfm/SfmData.h @@ -125,7 +125,7 @@ struct GTSAM_EXPORT SfmData { /// @name Serialization /// @{ -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION friend class boost::serialization::access; /** Serialization function */ template diff --git a/gtsam/sfm/SfmTrack.h b/gtsam/sfm/SfmTrack.h index 2b494ed7c..bdfa79cef 100644 --- a/gtsam/sfm/SfmTrack.h +++ b/gtsam/sfm/SfmTrack.h @@ -160,7 +160,7 @@ struct GTSAM_EXPORT SfmTrack : SfmTrack2d { /// @name Serialization /// @{ -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template diff --git a/gtsam/sfm/TranslationFactor.h b/gtsam/sfm/TranslationFactor.h index cfb9cff20..20bf005c1 100644 --- a/gtsam/sfm/TranslationFactor.h +++ b/gtsam/sfm/TranslationFactor.h @@ -78,7 +78,7 @@ class TranslationFactor : public NoiseModelFactorN { } private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION friend class boost::serialization::access; template void serialize(ARCHIVE& ar, const unsigned int /*version*/) { @@ -149,7 +149,7 @@ class BilinearAngleTranslationFactor } private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION friend class boost::serialization::access; template void serialize(ARCHIVE& ar, const unsigned int /*version*/) { diff --git a/gtsam/slam/AntiFactor.h b/gtsam/slam/AntiFactor.h index 3b031b334..11a0b1530 100644 --- a/gtsam/slam/AntiFactor.h +++ b/gtsam/slam/AntiFactor.h @@ -106,7 +106,7 @@ namespace gtsam { private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template diff --git a/gtsam/slam/BetweenFactor.h b/gtsam/slam/BetweenFactor.h index 3e2478e73..297703d4a 100644 --- a/gtsam/slam/BetweenFactor.h +++ b/gtsam/slam/BetweenFactor.h @@ -135,7 +135,7 @@ namespace gtsam { private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template @@ -176,7 +176,7 @@ namespace gtsam { private: /** Serialization function */ -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION friend class boost::serialization::access; template void serialize(ARCHIVE & ar, const unsigned int /*version*/) { diff --git a/gtsam/slam/BoundingConstraint.h b/gtsam/slam/BoundingConstraint.h index a496971e2..85d1f6255 100644 --- a/gtsam/slam/BoundingConstraint.h +++ b/gtsam/slam/BoundingConstraint.h @@ -83,7 +83,7 @@ struct BoundingConstraint1: public NoiseModelFactorN { private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template @@ -161,7 +161,7 @@ struct BoundingConstraint2: public NoiseModelFactorN { private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template diff --git a/gtsam/slam/EssentialMatrixConstraint.h b/gtsam/slam/EssentialMatrixConstraint.h index 45e54aa5d..6b57a8f27 100644 --- a/gtsam/slam/EssentialMatrixConstraint.h +++ b/gtsam/slam/EssentialMatrixConstraint.h @@ -91,7 +91,7 @@ public: private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template diff --git a/gtsam/slam/GeneralSFMFactor.h b/gtsam/slam/GeneralSFMFactor.h index 581624b38..06c832eb4 100644 --- a/gtsam/slam/GeneralSFMFactor.h +++ b/gtsam/slam/GeneralSFMFactor.h @@ -36,7 +36,7 @@ #include #include -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION #include #endif #include @@ -182,7 +182,7 @@ public: } private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template @@ -283,7 +283,7 @@ public: } private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template diff --git a/gtsam/slam/PoseRotationPrior.h b/gtsam/slam/PoseRotationPrior.h index 8ce90988b..47ea4c69f 100644 --- a/gtsam/slam/PoseRotationPrior.h +++ b/gtsam/slam/PoseRotationPrior.h @@ -91,7 +91,7 @@ public: private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template diff --git a/gtsam/slam/PoseTranslationPrior.h b/gtsam/slam/PoseTranslationPrior.h index c5257f45c..378610825 100644 --- a/gtsam/slam/PoseTranslationPrior.h +++ b/gtsam/slam/PoseTranslationPrior.h @@ -90,7 +90,7 @@ public: private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template diff --git a/gtsam/slam/ProjectionFactor.h b/gtsam/slam/ProjectionFactor.h index f40d8f5bb..baea29eed 100644 --- a/gtsam/slam/ProjectionFactor.h +++ b/gtsam/slam/ProjectionFactor.h @@ -188,7 +188,7 @@ namespace gtsam { private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION /// +#if GTSAM_ENABLE_BOOST_SERIALIZATION /// /// Serialization function friend class boost::serialization::access; template diff --git a/gtsam/slam/ReferenceFrameFactor.h b/gtsam/slam/ReferenceFrameFactor.h index 0ae6ea88a..21617e27a 100644 --- a/gtsam/slam/ReferenceFrameFactor.h +++ b/gtsam/slam/ReferenceFrameFactor.h @@ -122,7 +122,7 @@ public: Key local_key() const { return this->key3(); } private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template diff --git a/gtsam/slam/SmartFactorBase.h b/gtsam/slam/SmartFactorBase.h index d3e879c33..e641bf5d1 100644 --- a/gtsam/slam/SmartFactorBase.h +++ b/gtsam/slam/SmartFactorBase.h @@ -30,7 +30,7 @@ #include #include -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION #include #endif #include @@ -450,7 +450,7 @@ protected: private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION/// +#if GTSAM_ENABLE_BOOST_SERIALIZATION/// /// Serialization function friend class boost::serialization::access; template diff --git a/gtsam/slam/SmartFactorParams.h b/gtsam/slam/SmartFactorParams.h index 4523d0e0e..de3245038 100644 --- a/gtsam/slam/SmartFactorParams.h +++ b/gtsam/slam/SmartFactorParams.h @@ -118,7 +118,7 @@ struct SmartProjectionParams { private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION /// +#if GTSAM_ENABLE_BOOST_SERIALIZATION /// /// Serialization function friend class boost::serialization::access; template diff --git a/gtsam/slam/SmartProjectionFactor.h b/gtsam/slam/SmartProjectionFactor.h index c54efecc4..3c8843b39 100644 --- a/gtsam/slam/SmartProjectionFactor.h +++ b/gtsam/slam/SmartProjectionFactor.h @@ -466,7 +466,7 @@ protected: private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION /// +#if GTSAM_ENABLE_BOOST_SERIALIZATION /// /// Serialization function friend class boost::serialization::access; template diff --git a/gtsam/slam/SmartProjectionPoseFactor.h b/gtsam/slam/SmartProjectionPoseFactor.h index b88c1c628..855ae8c08 100644 --- a/gtsam/slam/SmartProjectionPoseFactor.h +++ b/gtsam/slam/SmartProjectionPoseFactor.h @@ -148,7 +148,7 @@ public: private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION /// +#if GTSAM_ENABLE_BOOST_SERIALIZATION /// /// Serialization function friend class boost::serialization::access; template diff --git a/gtsam/slam/SmartProjectionRigFactor.h b/gtsam/slam/SmartProjectionRigFactor.h index 6007c84e2..dd4237299 100644 --- a/gtsam/slam/SmartProjectionRigFactor.h +++ b/gtsam/slam/SmartProjectionRigFactor.h @@ -349,7 +349,7 @@ class SmartProjectionRigFactor : public SmartProjectionFactor { } private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION /// +#if GTSAM_ENABLE_BOOST_SERIALIZATION /// /// Serialization function friend class boost::serialization::access; template diff --git a/gtsam/slam/StereoFactor.h b/gtsam/slam/StereoFactor.h index 5adafcf3a..5fd171851 100644 --- a/gtsam/slam/StereoFactor.h +++ b/gtsam/slam/StereoFactor.h @@ -175,7 +175,7 @@ public: } private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template diff --git a/gtsam/slam/TriangulationFactor.h b/gtsam/slam/TriangulationFactor.h index 3b8486a59..f36e9b385 100644 --- a/gtsam/slam/TriangulationFactor.h +++ b/gtsam/slam/TriangulationFactor.h @@ -187,7 +187,7 @@ public: private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION /// +#if GTSAM_ENABLE_BOOST_SERIALIZATION /// /// Serialization function friend class boost::serialization::access; template diff --git a/gtsam/symbolic/SymbolicBayesNet.h b/gtsam/symbolic/SymbolicBayesNet.h index 3102191dc..4bba7cb69 100644 --- a/gtsam/symbolic/SymbolicBayesNet.h +++ b/gtsam/symbolic/SymbolicBayesNet.h @@ -101,7 +101,7 @@ namespace gtsam { /// @} private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template diff --git a/gtsam/symbolic/SymbolicBayesTree.h b/gtsam/symbolic/SymbolicBayesTree.h index 275b9560d..2b9e2f7ad 100644 --- a/gtsam/symbolic/SymbolicBayesTree.h +++ b/gtsam/symbolic/SymbolicBayesTree.h @@ -62,7 +62,7 @@ namespace gtsam { bool equals(const This& other, double tol = 1e-9) const; private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template diff --git a/gtsam/symbolic/SymbolicConditional.h b/gtsam/symbolic/SymbolicConditional.h index 5cc4e9cfc..3cc3e60cc 100644 --- a/gtsam/symbolic/SymbolicConditional.h +++ b/gtsam/symbolic/SymbolicConditional.h @@ -126,7 +126,7 @@ namespace gtsam { /// @} private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template diff --git a/gtsam/symbolic/SymbolicFactor.h b/gtsam/symbolic/SymbolicFactor.h index 190609ecb..fd19740de 100644 --- a/gtsam/symbolic/SymbolicFactor.h +++ b/gtsam/symbolic/SymbolicFactor.h @@ -151,7 +151,7 @@ namespace gtsam { /// @} private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template diff --git a/gtsam/symbolic/SymbolicFactorGraph.h b/gtsam/symbolic/SymbolicFactorGraph.h index 4122031d0..bbcd59f5e 100644 --- a/gtsam/symbolic/SymbolicFactorGraph.h +++ b/gtsam/symbolic/SymbolicFactorGraph.h @@ -145,7 +145,7 @@ namespace gtsam { /// @} private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template diff --git a/gtsam_unstable/dynamics/PoseRTV.h b/gtsam_unstable/dynamics/PoseRTV.h index 0e0d9ae27..95b7cfff3 100644 --- a/gtsam_unstable/dynamics/PoseRTV.h +++ b/gtsam_unstable/dynamics/PoseRTV.h @@ -157,7 +157,7 @@ public: /// @} private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template diff --git a/gtsam_unstable/dynamics/VelocityConstraint3.h b/gtsam_unstable/dynamics/VelocityConstraint3.h index 9f2ec89d2..9751b30a7 100644 --- a/gtsam_unstable/dynamics/VelocityConstraint3.h +++ b/gtsam_unstable/dynamics/VelocityConstraint3.h @@ -51,7 +51,7 @@ public: private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template diff --git a/gtsam_unstable/geometry/BearingS2.h b/gtsam_unstable/geometry/BearingS2.h index cb1d055df..0c3f40df8 100644 --- a/gtsam_unstable/geometry/BearingS2.h +++ b/gtsam_unstable/geometry/BearingS2.h @@ -90,7 +90,7 @@ private: /// @name Advanced Interface /// @{ -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION // +#if GTSAM_ENABLE_BOOST_SERIALIZATION // // Serialization function friend class boost::serialization::access; template diff --git a/gtsam_unstable/geometry/InvDepthCamera3.h b/gtsam_unstable/geometry/InvDepthCamera3.h index 45b27efe3..d1486209e 100644 --- a/gtsam_unstable/geometry/InvDepthCamera3.h +++ b/gtsam_unstable/geometry/InvDepthCamera3.h @@ -19,7 +19,7 @@ #include #include -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION #include #endif @@ -175,7 +175,7 @@ private: /// @name Advanced Interface /// @{ -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template diff --git a/gtsam_unstable/geometry/Pose3Upright.h b/gtsam_unstable/geometry/Pose3Upright.h index a77715fc6..20e0fde5d 100644 --- a/gtsam_unstable/geometry/Pose3Upright.h +++ b/gtsam_unstable/geometry/Pose3Upright.h @@ -130,7 +130,7 @@ public: private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION // +#if GTSAM_ENABLE_BOOST_SERIALIZATION // // Serialization function friend class boost::serialization::access; template diff --git a/gtsam_unstable/nonlinear/LinearizedFactor.h b/gtsam_unstable/nonlinear/LinearizedFactor.h index 1ff45ef5f..7aa596be1 100644 --- a/gtsam_unstable/nonlinear/LinearizedFactor.h +++ b/gtsam_unstable/nonlinear/LinearizedFactor.h @@ -59,7 +59,7 @@ public: const Values& linearizationPoint() const { return lin_points_; } private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template @@ -148,7 +148,7 @@ public: Vector error_vector(const Values& c) const; private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION friend class boost::serialization::access; /** Serialization function */ template @@ -279,7 +279,7 @@ public: private: /** Serialization function */ -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION friend class boost::serialization::access; template void serialize(ARCHIVE & ar, const unsigned int /*version*/) { diff --git a/gtsam_unstable/slam/BetweenFactorEM.h b/gtsam_unstable/slam/BetweenFactorEM.h index f1337c0e8..6cdce415e 100644 --- a/gtsam_unstable/slam/BetweenFactorEM.h +++ b/gtsam_unstable/slam/BetweenFactorEM.h @@ -415,7 +415,7 @@ public: private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template diff --git a/gtsam_unstable/slam/BiasedGPSFactor.h b/gtsam_unstable/slam/BiasedGPSFactor.h index f2b669f4f..1c278b111 100644 --- a/gtsam_unstable/slam/BiasedGPSFactor.h +++ b/gtsam_unstable/slam/BiasedGPSFactor.h @@ -94,7 +94,7 @@ namespace gtsam { private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template diff --git a/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel.h b/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel.h index 0accb8f42..b759ceb56 100644 --- a/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel.h +++ b/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel.h @@ -704,7 +704,7 @@ public: } private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template diff --git a/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel_NoBias.h b/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel_NoBias.h index a94c95335..56f2e2cee 100644 --- a/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel_NoBias.h +++ b/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel_NoBias.h @@ -573,7 +573,7 @@ public: } private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template diff --git a/gtsam_unstable/slam/GaussMarkov1stOrderFactor.h b/gtsam_unstable/slam/GaussMarkov1stOrderFactor.h index b91ed2afe..c9c4141b1 100644 --- a/gtsam_unstable/slam/GaussMarkov1stOrderFactor.h +++ b/gtsam_unstable/slam/GaussMarkov1stOrderFactor.h @@ -113,7 +113,7 @@ public: private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template diff --git a/gtsam_unstable/slam/InertialNavFactor_GlobalVelocity.h b/gtsam_unstable/slam/InertialNavFactor_GlobalVelocity.h index 8fda5456b..95fde2f75 100644 --- a/gtsam_unstable/slam/InertialNavFactor_GlobalVelocity.h +++ b/gtsam_unstable/slam/InertialNavFactor_GlobalVelocity.h @@ -411,7 +411,7 @@ public: private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template diff --git a/gtsam_unstable/slam/InvDepthFactor3.h b/gtsam_unstable/slam/InvDepthFactor3.h index 64a1366a2..f47c56bc9 100644 --- a/gtsam_unstable/slam/InvDepthFactor3.h +++ b/gtsam_unstable/slam/InvDepthFactor3.h @@ -113,7 +113,7 @@ public: private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION /// +#if GTSAM_ENABLE_BOOST_SERIALIZATION /// /// Serialization function friend class boost::serialization::access; template diff --git a/gtsam_unstable/slam/InvDepthFactorVariant1.h b/gtsam_unstable/slam/InvDepthFactorVariant1.h index a49b2090c..638b77440 100644 --- a/gtsam_unstable/slam/InvDepthFactorVariant1.h +++ b/gtsam_unstable/slam/InvDepthFactorVariant1.h @@ -133,7 +133,7 @@ public: private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION /// +#if GTSAM_ENABLE_BOOST_SERIALIZATION /// /// Serialization function friend class boost::serialization::access; template diff --git a/gtsam_unstable/slam/InvDepthFactorVariant2.h b/gtsam_unstable/slam/InvDepthFactorVariant2.h index 7abbbe89a..01f087df0 100644 --- a/gtsam_unstable/slam/InvDepthFactorVariant2.h +++ b/gtsam_unstable/slam/InvDepthFactorVariant2.h @@ -140,7 +140,7 @@ public: private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION /// +#if GTSAM_ENABLE_BOOST_SERIALIZATION /// /// Serialization function friend class boost::serialization::access; template diff --git a/gtsam_unstable/slam/InvDepthFactorVariant3.h b/gtsam_unstable/slam/InvDepthFactorVariant3.h index 8664002e8..f71df8425 100644 --- a/gtsam_unstable/slam/InvDepthFactorVariant3.h +++ b/gtsam_unstable/slam/InvDepthFactorVariant3.h @@ -137,7 +137,7 @@ public: private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION /// +#if GTSAM_ENABLE_BOOST_SERIALIZATION /// /// Serialization function friend class boost::serialization::access; template @@ -269,7 +269,7 @@ public: private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION friend class boost::serialization::access; /// Serialization function template diff --git a/gtsam_unstable/slam/MultiProjectionFactor.h b/gtsam_unstable/slam/MultiProjectionFactor.h index 8e2ea0cf7..28e9c0215 100644 --- a/gtsam_unstable/slam/MultiProjectionFactor.h +++ b/gtsam_unstable/slam/MultiProjectionFactor.h @@ -213,7 +213,7 @@ namespace gtsam { private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION /// +#if GTSAM_ENABLE_BOOST_SERIALIZATION /// /// Serialization function friend class boost::serialization::access; template diff --git a/gtsam_unstable/slam/PartialPriorFactor.h b/gtsam_unstable/slam/PartialPriorFactor.h index 0a735a2e1..645d0305a 100644 --- a/gtsam_unstable/slam/PartialPriorFactor.h +++ b/gtsam_unstable/slam/PartialPriorFactor.h @@ -142,7 +142,7 @@ namespace gtsam { const std::vector& indices() const { return indices_; } private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template diff --git a/gtsam_unstable/slam/PoseBetweenFactor.h b/gtsam_unstable/slam/PoseBetweenFactor.h index aa383967d..1e0526e22 100644 --- a/gtsam_unstable/slam/PoseBetweenFactor.h +++ b/gtsam_unstable/slam/PoseBetweenFactor.h @@ -119,7 +119,7 @@ namespace gtsam { private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template diff --git a/gtsam_unstable/slam/PosePriorFactor.h b/gtsam_unstable/slam/PosePriorFactor.h index 4a201ad89..60df7e948 100644 --- a/gtsam_unstable/slam/PosePriorFactor.h +++ b/gtsam_unstable/slam/PosePriorFactor.h @@ -101,7 +101,7 @@ namespace gtsam { private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template diff --git a/gtsam_unstable/slam/PoseToPointFactor.h b/gtsam_unstable/slam/PoseToPointFactor.h index be454cb70..911445fa5 100644 --- a/gtsam_unstable/slam/PoseToPointFactor.h +++ b/gtsam_unstable/slam/PoseToPointFactor.h @@ -92,7 +92,7 @@ class PoseToPointFactor : public NoiseModelFactorN { const POINT& measured() const { return measured_; } private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template diff --git a/gtsam_unstable/slam/ProjectionFactorPPP.h b/gtsam_unstable/slam/ProjectionFactorPPP.h index 38467c6cc..126900bc2 100644 --- a/gtsam_unstable/slam/ProjectionFactorPPP.h +++ b/gtsam_unstable/slam/ProjectionFactorPPP.h @@ -171,7 +171,7 @@ namespace gtsam { private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION /// +#if GTSAM_ENABLE_BOOST_SERIALIZATION /// /// Serialization function friend class boost::serialization::access; template diff --git a/gtsam_unstable/slam/ProjectionFactorPPPC.h b/gtsam_unstable/slam/ProjectionFactorPPPC.h index df63330df..cea74842f 100644 --- a/gtsam_unstable/slam/ProjectionFactorPPPC.h +++ b/gtsam_unstable/slam/ProjectionFactorPPPC.h @@ -151,7 +151,7 @@ class ProjectionFactorPPPC private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION /// +#if GTSAM_ENABLE_BOOST_SERIALIZATION /// /// Serialization function friend class boost::serialization::access; template diff --git a/gtsam_unstable/slam/ProjectionFactorRollingShutter.h b/gtsam_unstable/slam/ProjectionFactorRollingShutter.h index c5eb58f14..778713733 100644 --- a/gtsam_unstable/slam/ProjectionFactorRollingShutter.h +++ b/gtsam_unstable/slam/ProjectionFactorRollingShutter.h @@ -194,7 +194,7 @@ class GTSAM_UNSTABLE_EXPORT ProjectionFactorRollingShutter inline bool throwCheirality() const { return throwCheirality_; } private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION /// +#if GTSAM_ENABLE_BOOST_SERIALIZATION /// /// Serialization function friend class boost::serialization::access; template diff --git a/gtsam_unstable/slam/RelativeElevationFactor.h b/gtsam_unstable/slam/RelativeElevationFactor.h index d24415c3b..13bd5eba0 100644 --- a/gtsam_unstable/slam/RelativeElevationFactor.h +++ b/gtsam_unstable/slam/RelativeElevationFactor.h @@ -65,7 +65,7 @@ public: private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template diff --git a/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h b/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h index 4a2047ee1..f28ce642b 100644 --- a/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h +++ b/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h @@ -453,7 +453,7 @@ class SmartProjectionPoseFactorRollingShutter } private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION /// +#if GTSAM_ENABLE_BOOST_SERIALIZATION /// /// Serialization function friend class boost::serialization::access; template diff --git a/gtsam_unstable/slam/SmartStereoProjectionFactor.h b/gtsam_unstable/slam/SmartStereoProjectionFactor.h index 52d6eda05..7b06e8827 100644 --- a/gtsam_unstable/slam/SmartStereoProjectionFactor.h +++ b/gtsam_unstable/slam/SmartStereoProjectionFactor.h @@ -30,7 +30,7 @@ #include #include -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION #include #endif @@ -504,7 +504,7 @@ public: private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /// Serialization function friend class boost::serialization::access; template diff --git a/gtsam_unstable/slam/SmartStereoProjectionFactorPP.h b/gtsam_unstable/slam/SmartStereoProjectionFactorPP.h index 189c501bb..7482682fa 100644 --- a/gtsam_unstable/slam/SmartStereoProjectionFactorPP.h +++ b/gtsam_unstable/slam/SmartStereoProjectionFactorPP.h @@ -285,7 +285,7 @@ class GTSAM_UNSTABLE_EXPORT SmartStereoProjectionFactorPP } private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION /// +#if GTSAM_ENABLE_BOOST_SERIALIZATION /// /// Serialization function friend class boost::serialization::access; template diff --git a/gtsam_unstable/slam/SmartStereoProjectionPoseFactor.h b/gtsam_unstable/slam/SmartStereoProjectionPoseFactor.h index 77a15c246..93d8877a7 100644 --- a/gtsam_unstable/slam/SmartStereoProjectionPoseFactor.h +++ b/gtsam_unstable/slam/SmartStereoProjectionPoseFactor.h @@ -138,7 +138,7 @@ class GTSAM_UNSTABLE_EXPORT SmartStereoProjectionPoseFactor Base::Cameras cameras(const Values& values) const override; private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION /// +#if GTSAM_ENABLE_BOOST_SERIALIZATION /// /// Serialization function friend class boost::serialization::access; template diff --git a/gtsam_unstable/slam/TransformBtwRobotsUnaryFactor.h b/gtsam_unstable/slam/TransformBtwRobotsUnaryFactor.h index 2a257ff57..cbc8eba52 100644 --- a/gtsam_unstable/slam/TransformBtwRobotsUnaryFactor.h +++ b/gtsam_unstable/slam/TransformBtwRobotsUnaryFactor.h @@ -216,7 +216,7 @@ namespace gtsam { private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template diff --git a/gtsam_unstable/slam/TransformBtwRobotsUnaryFactorEM.h b/gtsam_unstable/slam/TransformBtwRobotsUnaryFactorEM.h index c47cf3b7d..352a9ff37 100644 --- a/gtsam_unstable/slam/TransformBtwRobotsUnaryFactorEM.h +++ b/gtsam_unstable/slam/TransformBtwRobotsUnaryFactorEM.h @@ -414,7 +414,7 @@ namespace gtsam { private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template diff --git a/tests/simulated2D.h b/tests/simulated2D.h index 612dd1891..78fc26708 100644 --- a/tests/simulated2D.h +++ b/tests/simulated2D.h @@ -160,7 +160,7 @@ namespace simulated2D { /// Default constructor GenericPrior() { } -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION /// +#if GTSAM_ENABLE_BOOST_SERIALIZATION /// /// Serialization function friend class boost::serialization::access; template @@ -210,7 +210,7 @@ namespace simulated2D { /// Default constructor GenericOdometry() { } -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION /// +#if GTSAM_ENABLE_BOOST_SERIALIZATION /// /// Serialization function friend class boost::serialization::access; template @@ -261,7 +261,7 @@ namespace simulated2D { /// Default constructor GenericMeasurement() { } -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION /// +#if GTSAM_ENABLE_BOOST_SERIALIZATION /// /// Serialization function friend class boost::serialization::access; template diff --git a/tests/testExpressionFactor.cpp b/tests/testExpressionFactor.cpp index 4c2ac99b1..845c28e5a 100644 --- a/tests/testExpressionFactor.cpp +++ b/tests/testExpressionFactor.cpp @@ -688,7 +688,7 @@ public: } private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template