From 9318861ac29740d481b04ca50a37a3ae22fcd1e3 Mon Sep 17 00:00:00 2001 From: Jose Luis Blanco-Claraco Date: Thu, 28 Sep 2023 17:28:11 +0200 Subject: [PATCH] Port to gtsam 4.2 --- README.md | 2 +- .../main.cpp | 28 +++++++++---------- .../main.cpp | 18 ++++++------ .../include/mbse/factors/FactorConstraints.h | 6 ++-- .../mbse/factors/FactorConstraintsAccIndep.h | 13 +++++---- .../mbse/factors/FactorConstraintsIndep.h | 8 ++++-- .../mbse/factors/FactorConstraintsVel.h | 8 ++++-- .../mbse/factors/FactorConstraintsVelIndep.h | 10 ++++--- libmbse/include/mbse/factors/FactorDynamics.h | 10 ++++--- .../mbse/factors/FactorDynamicsIndep.h | 10 ++++--- libmbse/include/mbse/factors/FactorEulerInt.h | 10 ++++--- .../include/mbse/factors/FactorGyroscope.h | 8 ++++-- .../mbse/factors/FactorInverseDynamics.h | 6 ++-- libmbse/include/mbse/factors/FactorTrapInt.h | 13 +++++---- libmbse/src/factors/FactorConstraints.cpp | 6 ++-- .../src/factors/FactorConstraintsAccIndep.cpp | 17 ++++++----- .../src/factors/FactorConstraintsIndep.cpp | 11 ++++---- libmbse/src/factors/FactorConstraintsVel.cpp | 10 +++---- .../src/factors/FactorConstraintsVelIndep.cpp | 13 ++++----- libmbse/src/factors/FactorDynamics.cpp | 12 ++++---- libmbse/src/factors/FactorDynamicsIndep.cpp | 13 ++++----- libmbse/src/factors/FactorEulerInt.cpp | 12 ++++---- libmbse/src/factors/FactorGyroscope.cpp | 10 +++---- libmbse/src/factors/FactorInverseDynamics.cpp | 6 ++-- libmbse/src/factors/FactorTrapInt.cpp | 16 +++++------ .../factor-dynamics-icoords-jacobian.cpp | 4 +-- libmbse/tests/factor-dynamics-jacobian.cpp | 4 +-- libmbse/tests/factor-gyroscope-jacobian.cpp | 4 +-- 28 files changed, 153 insertions(+), 135 deletions(-) diff --git a/README.md b/README.md index 0dc7905..b191a35 100644 --- a/README.md +++ b/README.md @@ -39,7 +39,7 @@ Dependencies: * SuiteSparse * [MRPT](https://github.com/mrpt/mrpt/) (>=2.0.0) * Ubuntu: Use [this PPA](https://launchpad.net/~joseluisblancoc/+archive/ubuntu/mrpt) - * [GTSAM](https://github.com/borglab/gtsam) + * [GTSAM](https://github.com/borglab/gtsam) >=4.2 * Ubuntu: Use [this PPA](https://launchpad.net/~borglab/+archive/ubuntu/gtsam-release-4.1) In Ubuntu, install dependencies with: diff --git a/apps/mbse-fg-smoother-forward-dynamics-icoords/main.cpp b/apps/mbse-fg-smoother-forward-dynamics-icoords/main.cpp index 0a904e6..8cd816a 100644 --- a/apps/mbse-fg-smoother-forward-dynamics-icoords/main.cpp +++ b/apps/mbse-fg-smoother-forward-dynamics-icoords/main.cpp @@ -25,7 +25,7 @@ #include #include #include -#include +#include #include #include #include @@ -186,10 +186,10 @@ void test_smoother() // Create Prior factors: factorsByTime.emplace( - 0.0, boost::make_shared>( + 0.0, std::make_shared>( sZ(0), z_0, noise_prior_z_0)); factorsByTime.emplace( - 0.0, boost::make_shared>( + 0.0, std::make_shared>( sZp(0), zeros_z, noise_prior_dz_0)); const double lag = arg_lag_time.getValue(); // seconds @@ -278,47 +278,47 @@ void test_smoother() // Create Trapezoidal Integrator factors: factorsByTime.emplace( - t, boost::make_shared( + t, std::make_shared( dt, noise_vel_z, sZ(timeStep), sZ(timeStep + 1), sZp(timeStep), sZp(timeStep + 1))); factorsByTime.emplace( - t, boost::make_shared( + t, std::make_shared( dt, noise_acc_z, sZp(timeStep), sZp(timeStep + 1), sZpp(timeStep), sZpp(timeStep + 1))); // Create Dynamics factors: factorsByTime.emplace( t + dt, - boost::make_shared( + std::make_shared( &dynSimul, noise_dyn_z, sZ(timeStep + 1), sZp(timeStep + 1), sZpp(timeStep + 1), sQ(timeStep + 1), wholeValues)); if (timeStep == 0) factorsByTime.emplace( - t, boost::make_shared( + t, std::make_shared( &dynSimul, noise_dyn_z, sZ(timeStep), sZp(timeStep), sZpp(timeStep), sQ(timeStep), wholeValues)); // "Soft equality" constraints between q_i and q_{i+1} to solve // configuration/branches ambiguities: factorsByTime.emplace( - t, boost::make_shared>( + t, std::make_shared>( sQ(timeStep), sQ(timeStep + 1), zeros_q, softBetweenNoise)); // Add dependent-coordinates constraint factor: if (timeStep == 0) { factorsByTime.emplace( - t, boost::make_shared( + t, std::make_shared( aMBS, indepCoordIndices, noise_constr_z, sZ(timeStep), sQ(timeStep))); factorsByTime.emplace( - t, boost::make_shared( + t, std::make_shared( aMBS, indepCoordIndices, noise_constr_dz, sQ(timeStep), sQp(timeStep), sZp(timeStep))); factorsByTime.emplace( - t, boost::make_shared( + t, std::make_shared( aMBS, indepCoordIndices, noise_constr_dz, sQ(timeStep), sQp(timeStep), sQpp(timeStep), sZpp(timeStep))); } @@ -326,19 +326,19 @@ void test_smoother() // if (timeStep < N - 1) { factorsByTime.emplace( - t + dt, boost::make_shared( + t + dt, std::make_shared( aMBS, indepCoordIndices, noise_constr_z, sZ(timeStep + 1), sQ(timeStep + 1))); factorsByTime.emplace( t + dt, - boost::make_shared( + std::make_shared( aMBS, indepCoordIndices, noise_constr_dz, sQ(timeStep + 1), sQp(timeStep + 1), sZp(timeStep + 1))); factorsByTime.emplace( t + dt, - boost::make_shared( + std::make_shared( aMBS, indepCoordIndices, noise_constr_dz, sQ(timeStep + 1), sQp(timeStep + 1), sQpp(timeStep + 1), sZpp(timeStep + 1))); } diff --git a/apps/mbse-fg-smoother-forward-dynamics/main.cpp b/apps/mbse-fg-smoother-forward-dynamics/main.cpp index 7269fa6..9e2a868 100644 --- a/apps/mbse-fg-smoother-forward-dynamics/main.cpp +++ b/apps/mbse-fg-smoother-forward-dynamics/main.cpp @@ -14,7 +14,7 @@ #include #include #include -#include +#include #include #include #include @@ -164,9 +164,9 @@ void test_smoother() // Create Prior factors: factorsByTime.emplace( - 0.0, boost::make_shared>(Q(0), q_0)); + 0.0, std::make_shared>(Q(0), q_0)); factorsByTime.emplace( - 0.0, boost::make_shared>( + 0.0, std::make_shared>( V(0), zeros, noise_prior_dq_0)); const double lag = arg_lag_time.getValue(); // seconds @@ -255,34 +255,34 @@ void test_smoother() // Create Trapezoidal Integrator factors: factorsByTime.emplace( - t, boost::make_shared( + t, std::make_shared( dt, noise_vel, Q(timeStep), Q(timeStep + 1), V(timeStep), V(timeStep + 1))); factorsByTime.emplace( - t, boost::make_shared( + t, std::make_shared( dt, noise_acc, V(timeStep), V(timeStep + 1), A(timeStep), A(timeStep + 1))); // Create Dynamics factors: factorsByTime.emplace( - t + dt, boost::make_shared( + t + dt, std::make_shared( &dynSimul, noise_dyn, Q(timeStep + 1), V(timeStep + 1), A(timeStep + 1))); if (timeStep == 0) factorsByTime.emplace( - t, boost::make_shared( + t, std::make_shared( &dynSimul, noise_dyn, Q(timeStep), V(timeStep), A(timeStep))); // Add dependent-coordinates constraint factor: if (!arg_dont_add_q_constraints.isSet()) factorsByTime.emplace( - t, boost::make_shared( + t, std::make_shared( aMBS, noise_constr_q, Q(timeStep))); if (!arg_dont_add_dq_constraints.isSet()) factorsByTime.emplace( - t, boost::make_shared( + t, std::make_shared( aMBS, noise_constr_dq, Q(timeStep), V(timeStep))); // Create initial estimates: diff --git a/libmbse/include/mbse/factors/FactorConstraints.h b/libmbse/include/mbse/factors/FactorConstraints.h index cc4aa9d..c71e5a0 100644 --- a/libmbse/include/mbse/factors/FactorConstraints.h +++ b/libmbse/include/mbse/factors/FactorConstraints.h @@ -29,7 +29,7 @@ class FactorConstraints : public gtsam::NoiseModelFactor1 public: // shorthand for a smart pointer to a factor - using shared_ptr = boost::shared_ptr; + using shared_ptr = std::shared_ptr; /** default constructor - only use for serialization */ FactorConstraints() = default; @@ -61,7 +61,7 @@ class FactorConstraints : public gtsam::NoiseModelFactor1 /** vector of errors */ gtsam::Vector evaluateError( const state_t& q_k, - boost::optional H1 = boost::none) const override; + gtsam::OptionalMatrixType H1 = OptionalNone) const override; /** numberof variable attached to this factor */ std::size_t size() const { return 1; } @@ -72,9 +72,11 @@ class FactorConstraints : public gtsam::NoiseModelFactor1 template void serialize(ARCHIVE& ar, const unsigned int /*version*/) { +#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION ar& boost::serialization::make_nvp( "FactorConstraints", boost::serialization::base_object(*this)); +#endif } }; diff --git a/libmbse/include/mbse/factors/FactorConstraintsAccIndep.h b/libmbse/include/mbse/factors/FactorConstraintsAccIndep.h index 23ea55d..6d5c440 100644 --- a/libmbse/include/mbse/factors/FactorConstraintsAccIndep.h +++ b/libmbse/include/mbse/factors/FactorConstraintsAccIndep.h @@ -36,7 +36,7 @@ class FactorConstraintsAccIndep public: // shorthand for a smart pointer to a factor - using shared_ptr = boost::shared_ptr; + using shared_ptr = std::shared_ptr; /** default constructor - only use for serialization */ FactorConstraintsAccIndep() = default; @@ -67,11 +67,10 @@ class FactorConstraintsAccIndep /** vector of errors */ gtsam::Vector evaluateError( const state_t& q_k, const state_t& dotq_k, const state_t& ddotq_k, - const state_t& ddotz_k, - boost::optional de_dq = boost::none, - boost::optional de_dqp = boost::none, - boost::optional de_dqpp = boost::none, - boost::optional de_dzpp = boost::none) const override; + const state_t& ddotz_k, gtsam::OptionalMatrixType de_dq = OptionalNone, + gtsam::OptionalMatrixType de_dqp = OptionalNone, + gtsam::OptionalMatrixType de_dqpp = OptionalNone, + gtsam::OptionalMatrixType de_dzpp = OptionalNone) const override; /** numberof variable attached to this factor */ std::size_t size() const { return 4; } @@ -82,9 +81,11 @@ class FactorConstraintsAccIndep template void serialize(ARCHIVE& ar, const unsigned int /*version*/) { +#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION ar& boost::serialization::make_nvp( "FactorConstraintsAccIndep", boost::serialization::base_object(*this)); +#endif } }; diff --git a/libmbse/include/mbse/factors/FactorConstraintsIndep.h b/libmbse/include/mbse/factors/FactorConstraintsIndep.h index 317be72..31cd9a9 100644 --- a/libmbse/include/mbse/factors/FactorConstraintsIndep.h +++ b/libmbse/include/mbse/factors/FactorConstraintsIndep.h @@ -32,7 +32,7 @@ class FactorConstraintsIndep public: // shorthand for a smart pointer to a factor - using shared_ptr = boost::shared_ptr; + using shared_ptr = std::shared_ptr; /** default constructor - only use for serialization */ FactorConstraintsIndep() = default; @@ -63,8 +63,8 @@ class FactorConstraintsIndep /** vector of errors */ gtsam::Vector evaluateError( const state_t& z_k, const state_t& q_k, - boost::optional de_dz = boost::none, - boost::optional de_dq = boost::none) const override; + gtsam::OptionalMatrixType de_dz = OptionalNone, + gtsam::OptionalMatrixType de_dq = OptionalNone) const override; /** numberof variable attached to this factor */ std::size_t size() const { return 2; } @@ -75,9 +75,11 @@ class FactorConstraintsIndep template void serialize(ARCHIVE& ar, const unsigned int /*version*/) { +#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION ar& boost::serialization::make_nvp( "FactorConstraintsIndep", boost::serialization::base_object(*this)); +#endif } }; diff --git a/libmbse/include/mbse/factors/FactorConstraintsVel.h b/libmbse/include/mbse/factors/FactorConstraintsVel.h index 04155ac..9b6e4b1 100644 --- a/libmbse/include/mbse/factors/FactorConstraintsVel.h +++ b/libmbse/include/mbse/factors/FactorConstraintsVel.h @@ -31,7 +31,7 @@ class FactorConstraintsVel : public gtsam::NoiseModelFactor2 public: // shorthand for a smart pointer to a factor - using shared_ptr = boost::shared_ptr; + using shared_ptr = std::shared_ptr; /** default constructor - only use for serialization */ FactorConstraintsVel() = default; @@ -64,8 +64,8 @@ class FactorConstraintsVel : public gtsam::NoiseModelFactor2 /** vector of errors */ gtsam::Vector evaluateError( const state_t& q_k, const state_t& dotq_k, - boost::optional H1 = boost::none, - boost::optional H2 = boost::none) const override; + gtsam::OptionalMatrixType H1 = OptionalNone, + gtsam::OptionalMatrixType H2 = OptionalNone) const override; /** numberof variable attached to this factor */ std::size_t size() const { return 2; } @@ -76,9 +76,11 @@ class FactorConstraintsVel : public gtsam::NoiseModelFactor2 template void serialize(ARCHIVE& ar, const unsigned int /*version*/) { +#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION ar& boost::serialization::make_nvp( "FactorConstraintsVel", boost::serialization::base_object(*this)); +#endif } }; diff --git a/libmbse/include/mbse/factors/FactorConstraintsVelIndep.h b/libmbse/include/mbse/factors/FactorConstraintsVelIndep.h index f48849a..70bd257 100644 --- a/libmbse/include/mbse/factors/FactorConstraintsVelIndep.h +++ b/libmbse/include/mbse/factors/FactorConstraintsVelIndep.h @@ -36,7 +36,7 @@ class FactorConstraintsVelIndep public: // shorthand for a smart pointer to a factor - using shared_ptr = boost::shared_ptr; + using shared_ptr = std::shared_ptr; /** default constructor - only use for serialization */ FactorConstraintsVelIndep() = default; @@ -67,9 +67,9 @@ class FactorConstraintsVelIndep /** vector of errors */ gtsam::Vector evaluateError( const state_t& q_k, const state_t& dotq_k, const state_t& dotz_k, - boost::optional de_dq = boost::none, - boost::optional de_dqp = boost::none, - boost::optional de_dzp = boost::none) const override; + gtsam::OptionalMatrixType de_dq = OptionalNone, + gtsam::OptionalMatrixType de_dqp = OptionalNone, + gtsam::OptionalMatrixType de_dzp = OptionalNone) const override; /** numberof variable attached to this factor */ std::size_t size() const { return 3; } @@ -80,9 +80,11 @@ class FactorConstraintsVelIndep template void serialize(ARCHIVE& ar, const unsigned int /*version*/) { +#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION ar& boost::serialization::make_nvp( "FactorConstraintsVelIndep", boost::serialization::base_object(*this)); +#endif } }; diff --git a/libmbse/include/mbse/factors/FactorDynamics.h b/libmbse/include/mbse/factors/FactorDynamics.h index 263d9c6..f44764d 100644 --- a/libmbse/include/mbse/factors/FactorDynamics.h +++ b/libmbse/include/mbse/factors/FactorDynamics.h @@ -38,7 +38,7 @@ class FactorDynamics public: // shorthand for a smart pointer to a factor - using shared_ptr = boost::shared_ptr; + using shared_ptr = std::shared_ptr; /** default constructor - only use for serialization */ FactorDynamics() = default; @@ -75,9 +75,9 @@ class FactorDynamics /** vector of errors */ gtsam::Vector evaluateError( const state_t& q_k, const state_t& dq_k, const state_t& ddq_k, - boost::optional H1 = boost::none, - boost::optional H2 = boost::none, - boost::optional H3 = boost::none) const override; + gtsam::OptionalMatrixType H1 = OptionalNone, + gtsam::OptionalMatrixType H2 = OptionalNone, + gtsam::OptionalMatrixType H3 = OptionalNone) const override; /** number of variables attached to this factor */ std::size_t size() const { return 3; } @@ -88,8 +88,10 @@ class FactorDynamics template void serialize(ARCHIVE& ar, const unsigned int /*version*/) { +#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION ar& boost::serialization::make_nvp( "FactorDynamics", boost::serialization::base_object(*this)); +#endif } }; diff --git a/libmbse/include/mbse/factors/FactorDynamicsIndep.h b/libmbse/include/mbse/factors/FactorDynamicsIndep.h index e9c4353..8494a39 100644 --- a/libmbse/include/mbse/factors/FactorDynamicsIndep.h +++ b/libmbse/include/mbse/factors/FactorDynamicsIndep.h @@ -44,7 +44,7 @@ class FactorDynamicsIndep public: // shorthand for a smart pointer to a factor - using shared_ptr = boost::shared_ptr; + using shared_ptr = std::shared_ptr; /** default constructor - only use for serialization */ FactorDynamicsIndep() = default; @@ -90,9 +90,9 @@ class FactorDynamicsIndep /** vector of errors */ gtsam::Vector evaluateError( const state_t& z_k, const state_t& dz_k, const state_t& ddz_k, - boost::optional de_dz = boost::none, - boost::optional de_dzp = boost::none, - boost::optional de_dzpp = boost::none) const override; + gtsam::OptionalMatrixType de_dz = OptionalNone, + gtsam::OptionalMatrixType de_dzp = OptionalNone, + gtsam::OptionalMatrixType de_dzpp = OptionalNone) const override; /** number of variables attached to this factor */ std::size_t size() const { return 3; } @@ -103,9 +103,11 @@ class FactorDynamicsIndep template void serialize(ARCHIVE& ar, const unsigned int /*version*/) { +#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION ar& boost::serialization::make_nvp( "FactorDynamicsIndep", boost::serialization::base_object(*this)); +#endif } }; diff --git a/libmbse/include/mbse/factors/FactorEulerInt.h b/libmbse/include/mbse/factors/FactorEulerInt.h index e9decd2..1467b5d 100644 --- a/libmbse/include/mbse/factors/FactorEulerInt.h +++ b/libmbse/include/mbse/factors/FactorEulerInt.h @@ -36,7 +36,7 @@ class FactorEulerInt public: // shorthand for a smart pointer to a factor - using shared_ptr = boost::shared_ptr; + using shared_ptr = std::shared_ptr; /** default constructor - only use for serialization */ FactorEulerInt() = default; @@ -71,9 +71,9 @@ class FactorEulerInt /** vector of errors */ gtsam::Vector evaluateError( const state_t& x_k, const state_t& x_kp1, const state_t& v_k, - boost::optional H1 = boost::none, - boost::optional H2 = boost::none, - boost::optional H3 = boost::none) const override; + gtsam::OptionalMatrixType H1 = OptionalNone, + gtsam::OptionalMatrixType H2 = OptionalNone, + gtsam::OptionalMatrixType H3 = OptionalNone) const override; /** number of variables attached to this factor */ std::size_t size() const { return 3; } @@ -84,9 +84,11 @@ class FactorEulerInt template void serialize(ARCHIVE& ar, const unsigned int /*version*/) { +#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION ar& boost::serialization::make_nvp( "FactorEulerInt", boost::serialization::base_object(*this)); ar& BOOST_SERIALIZATION_NVP(timestep_); +#endif } }; diff --git a/libmbse/include/mbse/factors/FactorGyroscope.h b/libmbse/include/mbse/factors/FactorGyroscope.h index 61b1557..0f6cfa6 100644 --- a/libmbse/include/mbse/factors/FactorGyroscope.h +++ b/libmbse/include/mbse/factors/FactorGyroscope.h @@ -33,7 +33,7 @@ class FactorGyroscope : public gtsam::NoiseModelFactor2 public: // shorthand for a smart pointer to a factor - using shared_ptr = boost::shared_ptr; + using shared_ptr = std::shared_ptr; /** default constructor - only use for serialization */ FactorGyroscope() = default; @@ -71,8 +71,8 @@ class FactorGyroscope : public gtsam::NoiseModelFactor2 /** vector of errors */ gtsam::Vector evaluateError( const state_t& q_k, const state_t& dq_k, - boost::optional H1 = boost::none, - boost::optional H2 = boost::none) const override; + gtsam::OptionalMatrixType H1 = OptionalNone, + gtsam::OptionalMatrixType H2 = OptionalNone) const override; /** number of variables attached to this factor */ std::size_t size() const { return 2; } @@ -83,8 +83,10 @@ class FactorGyroscope : public gtsam::NoiseModelFactor2 template void serialize(ARCHIVE& ar, const unsigned int /*version*/) { +#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION ar& boost::serialization::make_nvp( "FactorGyroscope", boost::serialization::base_object(*this)); +#endif } }; diff --git a/libmbse/include/mbse/factors/FactorInverseDynamics.h b/libmbse/include/mbse/factors/FactorInverseDynamics.h index eeaf4c1..624b1e8 100644 --- a/libmbse/include/mbse/factors/FactorInverseDynamics.h +++ b/libmbse/include/mbse/factors/FactorInverseDynamics.h @@ -51,7 +51,7 @@ class FactorInverseDynamics : public gtsam::NoiseModelFactor1 public: // shorthand for a smart pointer to a factor - using shared_ptr = boost::shared_ptr; + using shared_ptr = std::shared_ptr; /** default constructor - only use for serialization */ FactorInverseDynamics() = default; @@ -95,7 +95,7 @@ class FactorInverseDynamics : public gtsam::NoiseModelFactor1 /** vector of errors */ gtsam::Vector evaluateError( const state_t& Q_k, - boost::optional d_e_Q = boost::none) const override; + gtsam::OptionalMatrixType d_e_Q = OptionalNone) const override; /** number of variables attached to this factor */ std::size_t size() const { return 2; } @@ -106,9 +106,11 @@ class FactorInverseDynamics : public gtsam::NoiseModelFactor1 template void serialize(ARCHIVE& ar, const unsigned int /*version*/) { +#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION ar& boost::serialization::make_nvp( "FactorInverseDynamics", boost::serialization::base_object(*this)); +#endif } }; diff --git a/libmbse/include/mbse/factors/FactorTrapInt.h b/libmbse/include/mbse/factors/FactorTrapInt.h index f46b330..17b950e 100644 --- a/libmbse/include/mbse/factors/FactorTrapInt.h +++ b/libmbse/include/mbse/factors/FactorTrapInt.h @@ -38,7 +38,7 @@ class FactorTrapInt public: // shorthand for a smart pointer to a factor - using shared_ptr = boost::shared_ptr; + using shared_ptr = std::shared_ptr; /** default constructor - only use for serialization */ FactorTrapInt() = default; @@ -75,10 +75,11 @@ class FactorTrapInt /** vector of errors */ gtsam::Vector evaluateError( const state_t& x_k, const state_t& x_kp1, const state_t& v_k, - const state_t& v_kp1, boost::optional H1 = boost::none, - boost::optional H2 = boost::none, - boost::optional H3 = boost::none, - boost::optional H4 = boost::none) const override; + const state_t& v_kp1, + gtsam::OptionalMatrixType H1 = OptionalNone, + gtsam::OptionalMatrixType H2 = OptionalNone, + gtsam::OptionalMatrixType H3 = OptionalNone, + gtsam::OptionalMatrixType H4 = OptionalNone) const override; /** number of variables attached to this factor */ std::size_t size() const { return 4; } @@ -89,9 +90,11 @@ class FactorTrapInt template void serialize(ARCHIVE& ar, const unsigned int /*version*/) { +#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION ar& boost::serialization::make_nvp( "FactorEulerInt", boost::serialization::base_object(*this)); ar& BOOST_SERIALIZATION_NVP(timestep_); +#endif } }; diff --git a/libmbse/src/factors/FactorConstraints.cpp b/libmbse/src/factors/FactorConstraints.cpp index 934e0bb..8dc26d9 100644 --- a/libmbse/src/factors/FactorConstraints.cpp +++ b/libmbse/src/factors/FactorConstraints.cpp @@ -17,7 +17,7 @@ FactorConstraints::~FactorConstraints() = default; gtsam::NonlinearFactor::shared_ptr FactorConstraints::clone() const { - return boost::static_pointer_cast( + return std::static_pointer_cast( gtsam::NonlinearFactor::shared_ptr(new This(*this))); } @@ -37,7 +37,7 @@ bool FactorConstraints::equals( } gtsam::Vector FactorConstraints::evaluateError( - const state_t& q_k, boost::optional H1) const + const state_t& q_k, gtsam::OptionalMatrixType H1) const { MRPT_START @@ -58,7 +58,7 @@ gtsam::Vector FactorConstraints::evaluateError( // d err / d q_k if (H1) { - auto& Hv = H1.value(); + auto& Hv = *H1; Hv = arm_->Phi_q_.asDense(); } diff --git a/libmbse/src/factors/FactorConstraintsAccIndep.cpp b/libmbse/src/factors/FactorConstraintsAccIndep.cpp index 0b64ddd..11dbc30 100644 --- a/libmbse/src/factors/FactorConstraintsAccIndep.cpp +++ b/libmbse/src/factors/FactorConstraintsAccIndep.cpp @@ -31,7 +31,7 @@ FactorConstraintsAccIndep::~FactorConstraintsAccIndep() = default; gtsam::NonlinearFactor::shared_ptr FactorConstraintsAccIndep::clone() const { - return boost::static_pointer_cast( + return std::static_pointer_cast( gtsam::NonlinearFactor::shared_ptr(new This(*this))); } @@ -53,10 +53,9 @@ bool FactorConstraintsAccIndep::equals( gtsam::Vector FactorConstraintsAccIndep::evaluateError( const state_t& q_k, const state_t& dotq_k, const state_t& ddotq_k, - const state_t& ddotz_k, boost::optional de_dq, - boost::optional de_dqp, - boost::optional de_dqpp, - boost::optional de_dzpp) const + const state_t& ddotz_k, gtsam::OptionalMatrixType de_dq, + gtsam::OptionalMatrixType de_dqp, gtsam::OptionalMatrixType de_dqpp, + gtsam::OptionalMatrixType de_dzpp) const { MRPT_START @@ -95,7 +94,7 @@ gtsam::Vector FactorConstraintsAccIndep::evaluateError( // d err / d q_k if (de_dq) { - auto& Hv = de_dq.value(); + auto& Hv = *de_dq; Hv.resize(m + d, n); // first block = \dotPhiqq(\q_t) \dq_t + \Phiqq(\q_t) \ddq_t const Eigen::MatrixXd t1 = arm_->Phiqq_times_ddq_.asDense(); @@ -106,7 +105,7 @@ gtsam::Vector FactorConstraintsAccIndep::evaluateError( if (de_dqp) { - auto& Hv = de_dqp.value(); + auto& Hv = *de_dqp; Hv.resize(m + d, n); Hv.block(0, 0, m, n) = 2 * arm_->dotPhi_q_.asDense(); Hv.block(m, 0, d, n).setZero(); @@ -114,7 +113,7 @@ gtsam::Vector FactorConstraintsAccIndep::evaluateError( if (de_dqpp) { - auto& Hv = de_dqpp.value(); + auto& Hv = *de_dqpp; Hv.resize(m + d, n); Hv.block(0, 0, m, n) = arm_->Phi_q_.asDense(); Hv.block(m, 0, d, n) = matrix_Iidx_; @@ -122,7 +121,7 @@ gtsam::Vector FactorConstraintsAccIndep::evaluateError( if (de_dzpp) { - auto& Hv = de_dzpp.value(); + auto& Hv = *de_dzpp; Hv.setZero(m + d, d); Hv.block(m, 0, d, d) = -gtsam::Matrix::Identity(d, d); } diff --git a/libmbse/src/factors/FactorConstraintsIndep.cpp b/libmbse/src/factors/FactorConstraintsIndep.cpp index c4aa0d1..1bc3c73 100644 --- a/libmbse/src/factors/FactorConstraintsIndep.cpp +++ b/libmbse/src/factors/FactorConstraintsIndep.cpp @@ -30,7 +30,7 @@ FactorConstraintsIndep::~FactorConstraintsIndep() = default; gtsam::NonlinearFactor::shared_ptr FactorConstraintsIndep::clone() const { - return boost::static_pointer_cast( + return std::static_pointer_cast( gtsam::NonlinearFactor::shared_ptr(new This(*this))); } @@ -51,9 +51,8 @@ bool FactorConstraintsIndep::equals( } gtsam::Vector FactorConstraintsIndep::evaluateError( - const state_t& z_k, const state_t& q_k, - boost::optional de_dz, - boost::optional de_dq) const + const state_t& z_k, const state_t& q_k, gtsam::OptionalMatrixType de_dz, + gtsam::OptionalMatrixType de_dq) const { MRPT_START @@ -81,7 +80,7 @@ gtsam::Vector FactorConstraintsIndep::evaluateError( // d err / d z_k if (de_dz) { - auto& Hv = de_dz.value(); + auto& Hv = *de_dz; Hv.setZero(m + d, d); Hv.block(m, 0, d, d) = -gtsam::Matrix::Identity(d, d); } @@ -89,7 +88,7 @@ gtsam::Vector FactorConstraintsIndep::evaluateError( // d err / d q_k if (de_dq) { - auto& Hv = de_dq.value(); + auto& Hv = *de_dq; Hv.resize(m + d, n); Hv.block(0, 0, m, n) = arm_->Phi_q_.asDense(); // Build "I_idx", as called in the paper (sect. 6.7) diff --git a/libmbse/src/factors/FactorConstraintsVel.cpp b/libmbse/src/factors/FactorConstraintsVel.cpp index 18f6f5e..49a60c0 100644 --- a/libmbse/src/factors/FactorConstraintsVel.cpp +++ b/libmbse/src/factors/FactorConstraintsVel.cpp @@ -27,7 +27,7 @@ FactorConstraintsVel::~FactorConstraintsVel() = default; gtsam::NonlinearFactor::shared_ptr FactorConstraintsVel::clone() const { - return boost::static_pointer_cast( + return std::static_pointer_cast( gtsam::NonlinearFactor::shared_ptr(new This(*this))); } @@ -86,8 +86,8 @@ static void num_err_wrt_dq( gtsam::Vector FactorConstraintsVel::evaluateError( const state_t& q_k, const state_t& dotq_k, - boost::optional H1, - boost::optional H2) const + gtsam::OptionalMatrixType H1, + gtsam::OptionalMatrixType H2) const { MRPT_START @@ -115,7 +115,7 @@ gtsam::Vector FactorConstraintsVel::evaluateError( // d err / d q_k if (H1) { - auto& Hv = H1.value(); + auto& Hv = *H1; #if USE_NUMERIC_JACOBIAN NumericJacobParams p; p.arm = arm_.get(); @@ -140,7 +140,7 @@ gtsam::Vector FactorConstraintsVel::evaluateError( if (H2) { - auto& Hv = H2.value(); + auto& Hv = *H2; #if USE_NUMERIC_JACOBIAN NumericJacobParams p; p.arm = arm_.get(); diff --git a/libmbse/src/factors/FactorConstraintsVelIndep.cpp b/libmbse/src/factors/FactorConstraintsVelIndep.cpp index 206ad5f..7a818ca 100644 --- a/libmbse/src/factors/FactorConstraintsVelIndep.cpp +++ b/libmbse/src/factors/FactorConstraintsVelIndep.cpp @@ -31,7 +31,7 @@ FactorConstraintsVelIndep::~FactorConstraintsVelIndep() = default; gtsam::NonlinearFactor::shared_ptr FactorConstraintsVelIndep::clone() const { - return boost::static_pointer_cast( + return std::static_pointer_cast( gtsam::NonlinearFactor::shared_ptr(new This(*this))); } @@ -53,9 +53,8 @@ bool FactorConstraintsVelIndep::equals( gtsam::Vector FactorConstraintsVelIndep::evaluateError( const state_t& q_k, const state_t& dotq_k, const state_t& dotz_k, - boost::optional de_dq, - boost::optional de_dqp, - boost::optional de_dzp) const + gtsam::OptionalMatrixType de_dq, gtsam::OptionalMatrixType de_dqp, + gtsam::OptionalMatrixType de_dzp) const { MRPT_START @@ -91,7 +90,7 @@ gtsam::Vector FactorConstraintsVelIndep::evaluateError( // d err / d q_k if (de_dq) { - auto& Hv = de_dq.value(); + auto& Hv = *de_dq; Hv.resize(m + d, n); // Phi_qq*dq = \dot{Phi_q} Hv.block(0, 0, m, n) = arm_->dotPhi_q_.asDense(); @@ -100,7 +99,7 @@ gtsam::Vector FactorConstraintsVelIndep::evaluateError( if (de_dqp) { - auto& Hv = de_dqp.value(); + auto& Hv = *de_dqp; Hv.resize(m + d, n); Hv.block(0, 0, m, n) = arm_->Phi_q_.asDense(); Hv.block(m, 0, d, n) = matrix_Iidx_; @@ -108,7 +107,7 @@ gtsam::Vector FactorConstraintsVelIndep::evaluateError( if (de_dzp) { - auto& Hv = de_dzp.value(); + auto& Hv = *de_dzp; Hv.setZero(m + d, d); Hv.block(m, 0, d, d) = -gtsam::Matrix::Identity(d, d); } diff --git a/libmbse/src/factors/FactorDynamics.cpp b/libmbse/src/factors/FactorDynamics.cpp index 758843e..3027109 100644 --- a/libmbse/src/factors/FactorDynamics.cpp +++ b/libmbse/src/factors/FactorDynamics.cpp @@ -33,7 +33,7 @@ FactorDynamics::~FactorDynamics() = default; gtsam::NonlinearFactor::shared_ptr FactorDynamics::clone() const { - return boost::static_pointer_cast( + return std::static_pointer_cast( gtsam::NonlinearFactor::shared_ptr(new This(*this))); } @@ -97,8 +97,8 @@ bool FactorDynamics::equals( gtsam::Vector FactorDynamics::evaluateError( const state_t& q_k, const state_t& dq_k, const state_t& ddq_k, - boost::optional H1, boost::optional H2, - boost::optional H3) const + gtsam::OptionalMatrixType H1, gtsam::OptionalMatrixType H2, + gtsam::OptionalMatrixType H3) const { MRPT_START @@ -126,7 +126,7 @@ gtsam::Vector FactorDynamics::evaluateError( // d err / d q_k if (H1) { - auto& Hv = H1.value(); + auto& Hv = *H1; #if USE_NUMERIC_JACOBIAN NumericJacobParams p; p.arm = &arm; @@ -192,7 +192,7 @@ Jacc_qt = ddq_I+ddq_II+ddq_III+ddq_IV+ddq_V; // d err / d dq_k if (H2) { - auto& Hv = H2.value(); + auto& Hv = *H2; #if USE_NUMERIC_JACOBIAN NumericJacobParams p; p.arm = &arm; @@ -233,7 +233,7 @@ Jacc_dqt = -2*invM*B; // d err / d ddq_k if (H3) { - auto& Hv = H3.value(); + auto& Hv = *H3; Hv = -Eigen::MatrixXd::Identity(n, n); } return err; diff --git a/libmbse/src/factors/FactorDynamicsIndep.cpp b/libmbse/src/factors/FactorDynamicsIndep.cpp index dbf4514..a48438b 100644 --- a/libmbse/src/factors/FactorDynamicsIndep.cpp +++ b/libmbse/src/factors/FactorDynamicsIndep.cpp @@ -26,7 +26,7 @@ FactorDynamicsIndep::~FactorDynamicsIndep() = default; gtsam::NonlinearFactor::shared_ptr FactorDynamicsIndep::clone() const { - return boost::static_pointer_cast( + return std::static_pointer_cast( gtsam::NonlinearFactor::shared_ptr(new This(*this))); } @@ -117,9 +117,8 @@ bool FactorDynamicsIndep::equals( gtsam::Vector FactorDynamicsIndep::evaluateError( const state_t& z_k, const state_t& dz_k, const state_t& ddz_k, - boost::optional de_dz, - boost::optional de_dzp, - boost::optional de_dzpp) const + gtsam::OptionalMatrixType de_dz, gtsam::OptionalMatrixType de_dzp, + gtsam::OptionalMatrixType de_dzpp) const { MRPT_START @@ -164,7 +163,7 @@ gtsam::Vector FactorDynamicsIndep::evaluateError( // d err / d z_k if (de_dz) { - auto& Hv = de_dz.value(); + auto& Hv = *de_dz; #if USE_NUMERIC_JACOBIAN NumericJacobParams p; p.arm = &arm; @@ -191,7 +190,7 @@ gtsam::Vector FactorDynamicsIndep::evaluateError( // d err / d dz_k if (de_dzp) { - auto& Hv = de_dzp.value(); + auto& Hv = *de_dzp; #if USE_NUMERIC_JACOBIAN NumericJacobParams p; p.arm = &arm; @@ -218,7 +217,7 @@ gtsam::Vector FactorDynamicsIndep::evaluateError( // d err / d ddz_k if (de_dzpp) { - auto& Hv = de_dzpp.value(); + auto& Hv = *de_dzpp; Hv = -Eigen::MatrixXd::Identity(d, d); } return err; diff --git a/libmbse/src/factors/FactorEulerInt.cpp b/libmbse/src/factors/FactorEulerInt.cpp index 69fe2c8..385573f 100644 --- a/libmbse/src/factors/FactorEulerInt.cpp +++ b/libmbse/src/factors/FactorEulerInt.cpp @@ -17,7 +17,7 @@ FactorEulerInt::~FactorEulerInt() = default; gtsam::NonlinearFactor::shared_ptr FactorEulerInt::clone() const { - return boost::static_pointer_cast( + return std::static_pointer_cast( gtsam::NonlinearFactor::shared_ptr(new This(*this))); } @@ -41,8 +41,8 @@ bool FactorEulerInt::equals( gtsam::Vector FactorEulerInt::evaluateError( const state_t& x_k, const state_t& x_kp1, const state_t& v_k, - boost::optional H1, boost::optional H2, - boost::optional H3) const + gtsam::OptionalMatrixType H1, gtsam::OptionalMatrixType H2, + gtsam::OptionalMatrixType H3) const { const auto n = x_k.size(); @@ -53,17 +53,17 @@ gtsam::Vector FactorEulerInt::evaluateError( if (H1) { - auto& H1v = H1.value(); + auto& H1v = *H1; H1v = -Eigen::MatrixXd::Identity(n, n); } if (H2) { - auto& H2v = H2.value(); + auto& H2v = *H2; H2v = Eigen::MatrixXd::Identity(n, n); } if (H3) { - auto& H3v = H3.value(); + auto& H3v = *H3; H3v = -timestep_ * Eigen::MatrixXd::Identity(n, n); } diff --git a/libmbse/src/factors/FactorGyroscope.cpp b/libmbse/src/factors/FactorGyroscope.cpp index b37f31b..d24788d 100644 --- a/libmbse/src/factors/FactorGyroscope.cpp +++ b/libmbse/src/factors/FactorGyroscope.cpp @@ -22,7 +22,7 @@ using namespace mrpt::math; gtsam::NonlinearFactor::shared_ptr FactorGyroscope::clone() const { - return boost::static_pointer_cast( + return std::static_pointer_cast( gtsam::NonlinearFactor::shared_ptr(new This(*this))); } @@ -44,8 +44,8 @@ bool FactorGyroscope::equals( } gtsam::Vector FactorGyroscope::evaluateError( - const state_t& q_k, const state_t& dq_k, boost::optional H1, - boost::optional H2) const + const state_t& q_k, const state_t& dq_k, gtsam::OptionalMatrixType H1, + gtsam::OptionalMatrixType H2) const { const auto n = q_k.size(); if (dq_k.size() != n) @@ -99,7 +99,7 @@ gtsam::Vector FactorGyroscope::evaluateError( // d err / d q_k if (H1) { - auto& Hv = H1.value(); + auto& Hv = *H1; Hv.setZero(1, n); // point0 & point1: @@ -151,7 +151,7 @@ gtsam::Vector FactorGyroscope::evaluateError( // d err / d dq_k if (H2) { - auto& Hv = H2.value(); + auto& Hv = *H2; Hv.setZero(1, n); // point0 & point1: diff --git a/libmbse/src/factors/FactorInverseDynamics.cpp b/libmbse/src/factors/FactorInverseDynamics.cpp index 0c64eba..e2a0906 100644 --- a/libmbse/src/factors/FactorInverseDynamics.cpp +++ b/libmbse/src/factors/FactorInverseDynamics.cpp @@ -24,7 +24,7 @@ FactorInverseDynamics::~FactorInverseDynamics() = default; gtsam::NonlinearFactor::shared_ptr FactorInverseDynamics::clone() const { - return boost::static_pointer_cast( + return std::static_pointer_cast( gtsam::NonlinearFactor::shared_ptr(new This(*this))); } @@ -110,7 +110,7 @@ bool FactorInverseDynamics::equals( } gtsam::Vector FactorInverseDynamics::evaluateError( - const state_t& Q_k, boost::optional d_e_Q) const + const state_t& Q_k, gtsam::OptionalMatrixType d_e_Q) const { MRPT_START @@ -146,7 +146,7 @@ gtsam::Vector FactorInverseDynamics::evaluateError( if (d_e_Q) { - auto& Hv = d_e_Q.value(); + auto& Hv = *d_e_Q; const size_t& g = actuatedDegreesOfFreedomInQ_.size(); diff --git a/libmbse/src/factors/FactorTrapInt.cpp b/libmbse/src/factors/FactorTrapInt.cpp index e34b63c..ca05911 100644 --- a/libmbse/src/factors/FactorTrapInt.cpp +++ b/libmbse/src/factors/FactorTrapInt.cpp @@ -17,7 +17,7 @@ FactorTrapInt::~FactorTrapInt() = default; gtsam::NonlinearFactor::shared_ptr FactorTrapInt::clone() const { - return boost::static_pointer_cast( + return std::static_pointer_cast( gtsam::NonlinearFactor::shared_ptr(new This(*this))); } @@ -45,9 +45,9 @@ bool FactorTrapInt::equals( gtsam::Vector FactorTrapInt::evaluateError( const state_t& x_k, const state_t& x_kp1, const state_t& v_k, - const state_t& v_kp1, boost::optional H1, - boost::optional H2, boost::optional H3, - boost::optional H4) const + const state_t& v_kp1, gtsam::OptionalMatrixType H1, + gtsam::OptionalMatrixType H2, gtsam::OptionalMatrixType H3, + gtsam::OptionalMatrixType H4) const { const auto n = x_k.size(); @@ -61,22 +61,22 @@ gtsam::Vector FactorTrapInt::evaluateError( // Jacobian of err respect to[x_k x_kp1 v_k v_kp1] if (H1) { - auto& H1v = H1.value(); + auto& H1v = *H1; H1v = -Eigen::MatrixXd::Identity(n, n); } if (H2) { - auto& H2v = H2.value(); + auto& H2v = *H2; H2v = Eigen::MatrixXd::Identity(n, n); } if (H3) { - auto& H3v = H3.value(); + auto& H3v = *H3; H3v = -0.5 * timestep_ * Eigen::MatrixXd::Identity(n, n); } if (H4) { - auto& H4v = H4.value(); + auto& H4v = *H4; H4v = -0.5 * timestep_ * Eigen::MatrixXd::Identity(n, n); } diff --git a/libmbse/tests/factor-dynamics-icoords-jacobian.cpp b/libmbse/tests/factor-dynamics-icoords-jacobian.cpp index 0948941..4cc730d 100644 --- a/libmbse/tests/factor-dynamics-icoords-jacobian.cpp +++ b/libmbse/tests/factor-dynamics-icoords-jacobian.cpp @@ -95,7 +95,7 @@ TEST(Jacobians, FactorDynamicsIndepCoords) gtsam::Values valuesForQ; // Create a dummy factor: - auto factorDyn = boost::make_shared( + auto factorDyn = std::make_shared( &dynSimul, noise_dyn, Z(1), DZ(1), DDZ(1), Q(1), valuesForQ); // For different instants of time and mechanism positions and @@ -141,7 +141,7 @@ TEST(Jacobians, FactorDynamicsIndepCoords) gtsam::Matrix H[3]; timlog.enter("factorsDynIndep.theoretical_jacob"); - factorDyn->evaluateError(z, dotz, ddotz, H[0], H[1], H[2]); + factorDyn->evaluateError(z, dotz, ddotz, &H[0], &H[1], &H[2]); timlog.leave("factorsDynIndep.theoretical_jacob"); diff --git a/libmbse/tests/factor-dynamics-jacobian.cpp b/libmbse/tests/factor-dynamics-jacobian.cpp index 5b5dade..2c7e9a0 100644 --- a/libmbse/tests/factor-dynamics-jacobian.cpp +++ b/libmbse/tests/factor-dynamics-jacobian.cpp @@ -71,7 +71,7 @@ TEST(Jacobians, FactorDynamics) auto noise_dyn = gtsam::noiseModel::Isotropic::Sigma(n, 0.1); // Create a dummy factor: - auto factorDyn = boost::make_shared( + auto factorDyn = std::make_shared( &dynSimul, noise_dyn, Q(1), V(1), A(1)); // For different instants of time and mechanism positions and @@ -105,7 +105,7 @@ TEST(Jacobians, FactorDynamics) gtsam::Matrix H[3]; timlog.enter("factorsDyn.theoretical_jacob"); - factorDyn->evaluateError(q, dotq, ddotq, H[0], H[1], H[2]); + factorDyn->evaluateError(q, dotq, ddotq, &H[0], &H[1], &H[2]); timlog.leave("factorsDyn.theoretical_jacob"); diff --git a/libmbse/tests/factor-gyroscope-jacobian.cpp b/libmbse/tests/factor-gyroscope-jacobian.cpp index 1f5440b..191b23f 100644 --- a/libmbse/tests/factor-gyroscope-jacobian.cpp +++ b/libmbse/tests/factor-gyroscope-jacobian.cpp @@ -74,7 +74,7 @@ TEST(Jacobians, gyroscope) FactorGyroscope::shared_ptr factorsGyro[3]; for (unsigned int body_idx = 0; body_idx < 3; body_idx++) { - factorsGyro[body_idx] = boost::make_shared( + factorsGyro[body_idx] = std::make_shared( *aMBS, body_idx, dummy_sensor_observation, noise_gyro, Q(1), V(1)); } @@ -109,7 +109,7 @@ TEST(Jacobians, gyroscope) gtsam::Matrix H[2]; timlog.enter("factorsGyro.theoretical_jacob"); - factorsGyro[body_idx]->evaluateError(q, dotq, H[0], H[1]); + factorsGyro[body_idx]->evaluateError(q, dotq, &H[0], &H[1]); timlog.leave("factorsGyro.theoretical_jacob");