diff --git a/.gitignore b/.gitignore index 3c6f4e4..5ae78d1 100644 --- a/.gitignore +++ b/.gitignore @@ -11,3 +11,4 @@ doc/source/mainpage.md doc/html experiments/*.txt .vscode +/cmake-build-debug/ diff --git a/.idea/.gitignore b/.idea/.gitignore new file mode 100644 index 0000000..6722cd9 --- /dev/null +++ b/.idea/.gitignore @@ -0,0 +1 @@ +*.xml diff --git a/.idea/.name b/.idea/.name new file mode 100644 index 0000000..8351b1d --- /dev/null +++ b/.idea/.name @@ -0,0 +1 @@ +MBSE \ No newline at end of file diff --git a/FourBar.YAML b/FourBar.YAML new file mode 100644 index 0000000..2d60bf3 --- /dev/null +++ b/FourBar.YAML @@ -0,0 +1,41 @@ +# A four bars mechanism +# Degrees of freedom in q=[x1 y1 x2 y2]^T +# Modeled in Natural coordinates: +# 2:(q3,q4) +# +--------- o +# | (xb,yb) +# | +# | +# o--------- + 1:(q1,q2) +# (xa,ya) + +parameters: + L: 1.0 # length [m] + xb: 4.0 + +points: + #0: + - {x: 0, y: 0, fixed: true} # A + - { x: L, y: 0 } + - { x: L, y: 2*L } + - { x: xb, y: 0, fixed: true } # B + +planar_bodies: +- + points: [0, 1] + length: L + mass: 1.0 + I0: (1/3)*mass*length^2 + cog: [0.5*length, 0.0] +- + points: [1, 2] + length: 2*L + mass: 2.0 + I0: (1/3)*mass*length^2 + cog: [0.5*length, 0.0] +- + points: [2, 3] + length: auto + mass: 4.0 + I0: (1/3)*mass*length^2 + cog: [0.5*length, 0.0] \ No newline at end of file diff --git a/README.md b/README.md index ec16c7e..8331a80 100644 --- a/README.md +++ b/README.md @@ -32,19 +32,19 @@ For the theory behinds this work, refer to : ![ScreenShot](https://raw.githubusercontent.com/MBDS/multibody-state-estimation/master/doc/source/_static/mbde-pf-screenshot.jpg) -## Compiling +## My config for running this project Dependencies: * A C++ compiler * CMake * 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) >=4.2 - * Ubuntu: Use [this PPA](https://launchpad.net/~borglab/+archive/ubuntu/gtsam-release-4.1) + * [GTSAM](https://github.com/borglab/gtsam) =4.2 + * Ubuntu: gtsam was built by using system's default Eigen v3.4 with command : `cmake -DGTSAM_USE_SYSTEM_EIGEN=ON ..` In Ubuntu, install dependencies with: - sudo apt-get install build-essential cmake libsuitesparse-dev libmrpt-dev libgtsam-dev + sudo apt-get install build-essential cmake libsuitesparse-dev To build: @@ -57,6 +57,12 @@ To build: You should also be able to compile this project under Windows and Visual Studio. +To Run the FourBar.YAML example do the following in terminal + + + /bin/mbse-dynamic-simulation --mechanism [other_optional_arguments] + + ## Using mbse as a library in a user program In your CMake project, add: diff --git a/libmbse/include/mbse/factors/FactorConstraints.h b/libmbse/include/mbse/factors/FactorConstraints.h index 0999806..f6b9d77 100644 --- a/libmbse/include/mbse/factors/FactorConstraints.h +++ b/libmbse/include/mbse/factors/FactorConstraints.h @@ -13,6 +13,7 @@ #include #include #include +#include namespace mbse { @@ -61,7 +62,7 @@ class FactorConstraints : public gtsam::NoiseModelFactor1 /** vector of errors */ gtsam::Vector evaluateError( const state_t& q_k, - gtsam::OptionalMatrixType H1 = OptionalNone) const override; + boost::optional H1 ) const override; /** numberof variable attached to this factor */ std::size_t size() const { return 1; } diff --git a/libmbse/include/mbse/factors/FactorConstraintsAccIndep.h b/libmbse/include/mbse/factors/FactorConstraintsAccIndep.h index f9b4e15..233ff03 100644 --- a/libmbse/include/mbse/factors/FactorConstraintsAccIndep.h +++ b/libmbse/include/mbse/factors/FactorConstraintsAccIndep.h @@ -67,10 +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, gtsam::OptionalMatrixType de_dq = OptionalNone, - gtsam::OptionalMatrixType de_dqp = OptionalNone, - gtsam::OptionalMatrixType de_dqpp = OptionalNone, - gtsam::OptionalMatrixType de_dzpp = OptionalNone) const override; + 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; /** numberof variable attached to this factor */ std::size_t size() const { return 4; } diff --git a/libmbse/include/mbse/factors/FactorConstraintsIndep.h b/libmbse/include/mbse/factors/FactorConstraintsIndep.h index 9498b69..878414e 100644 --- a/libmbse/include/mbse/factors/FactorConstraintsIndep.h +++ b/libmbse/include/mbse/factors/FactorConstraintsIndep.h @@ -63,8 +63,8 @@ class FactorConstraintsIndep /** vector of errors */ gtsam::Vector evaluateError( const state_t& z_k, const state_t& q_k, - gtsam::OptionalMatrixType de_dz = OptionalNone, - gtsam::OptionalMatrixType de_dq = OptionalNone) const override; + boost::optional de_dz = boost::none, + boost::optional de_dq = boost::none) const override; /** numberof variable attached to this factor */ std::size_t size() const { return 2; } diff --git a/libmbse/include/mbse/factors/FactorConstraintsVel.h b/libmbse/include/mbse/factors/FactorConstraintsVel.h index 7ce36f3..4b1f315 100644 --- a/libmbse/include/mbse/factors/FactorConstraintsVel.h +++ b/libmbse/include/mbse/factors/FactorConstraintsVel.h @@ -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, - gtsam::OptionalMatrixType H1 = OptionalNone, - gtsam::OptionalMatrixType H2 = OptionalNone) const override; + boost::optional H1 = boost::none, + boost::optional H2 = boost::none) const override; /** numberof variable attached to this factor */ std::size_t size() const { return 2; } diff --git a/libmbse/include/mbse/factors/FactorConstraintsVelIndep.h b/libmbse/include/mbse/factors/FactorConstraintsVelIndep.h index e17af71..73f463b 100644 --- a/libmbse/include/mbse/factors/FactorConstraintsVelIndep.h +++ b/libmbse/include/mbse/factors/FactorConstraintsVelIndep.h @@ -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, - gtsam::OptionalMatrixType de_dq = OptionalNone, - gtsam::OptionalMatrixType de_dqp = OptionalNone, - gtsam::OptionalMatrixType de_dzp = OptionalNone) const override; + boost::optional de_dq = boost::none, + boost::optional de_dqp = boost::none, + boost::optional de_dzp = boost::none) const override; /** numberof variable attached to this factor */ std::size_t size() const { return 3; } diff --git a/libmbse/include/mbse/factors/FactorDynamics.h b/libmbse/include/mbse/factors/FactorDynamics.h index 41c6563..41a63d6 100644 --- a/libmbse/include/mbse/factors/FactorDynamics.h +++ b/libmbse/include/mbse/factors/FactorDynamics.h @@ -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, - gtsam::OptionalMatrixType H1 = OptionalNone, - gtsam::OptionalMatrixType H2 = OptionalNone, - gtsam::OptionalMatrixType H3 = OptionalNone) const override; + boost::optional H1 = boost::none, + boost::optional H2 = boost::none, + boost::optional H3 = boost::none) const override; /** number of variables attached to this factor */ std::size_t size() const { return 3; } diff --git a/libmbse/include/mbse/factors/FactorDynamicsIndep.h b/libmbse/include/mbse/factors/FactorDynamicsIndep.h index 847beec..670d889 100644 --- a/libmbse/include/mbse/factors/FactorDynamicsIndep.h +++ b/libmbse/include/mbse/factors/FactorDynamicsIndep.h @@ -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, - gtsam::OptionalMatrixType de_dz = OptionalNone, - gtsam::OptionalMatrixType de_dzp = OptionalNone, - gtsam::OptionalMatrixType de_dzpp = OptionalNone) const override; + boost::optional de_dz = boost::none, + boost::optional de_dzp = boost::none, + boost::optional de_dzpp = boost::none) const override; /** number of variables attached to this factor */ std::size_t size() const { return 3; } diff --git a/libmbse/include/mbse/factors/FactorEulerInt.h b/libmbse/include/mbse/factors/FactorEulerInt.h index 4d4b9a8..a7f6958 100644 --- a/libmbse/include/mbse/factors/FactorEulerInt.h +++ b/libmbse/include/mbse/factors/FactorEulerInt.h @@ -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, - gtsam::OptionalMatrixType H1 = OptionalNone, - gtsam::OptionalMatrixType H2 = OptionalNone, - gtsam::OptionalMatrixType H3 = OptionalNone) const override; + boost::optional H1 = boost::none, + boost::optional H2 = boost::none, + boost::optional H3 = boost::none) const override; /** number of variables attached to this factor */ std::size_t size() const { return 3; } diff --git a/libmbse/include/mbse/factors/FactorGyroscope.h b/libmbse/include/mbse/factors/FactorGyroscope.h index 902736c..3a50eb6 100644 --- a/libmbse/include/mbse/factors/FactorGyroscope.h +++ b/libmbse/include/mbse/factors/FactorGyroscope.h @@ -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, - gtsam::OptionalMatrixType H1 = OptionalNone, - gtsam::OptionalMatrixType H2 = OptionalNone) const override; + boost::optional H1 = boost::none, + boost::optional H2 = boost::none) const override; /** number of variables attached to this factor */ std::size_t size() const { return 2; } diff --git a/libmbse/include/mbse/factors/FactorInverseDynamics.h b/libmbse/include/mbse/factors/FactorInverseDynamics.h index b6aaedb..4677def 100644 --- a/libmbse/include/mbse/factors/FactorInverseDynamics.h +++ b/libmbse/include/mbse/factors/FactorInverseDynamics.h @@ -95,7 +95,7 @@ class FactorInverseDynamics : public gtsam::NoiseModelFactor1 /** vector of errors */ gtsam::Vector evaluateError( const state_t& Q_k, - gtsam::OptionalMatrixType d_e_Q = OptionalNone) const override; + boost::optional d_e_Q = boost::none) const override; /** number of variables attached to this factor */ std::size_t size() const { return 2; } diff --git a/libmbse/include/mbse/factors/FactorTrapInt.h b/libmbse/include/mbse/factors/FactorTrapInt.h index 5defa0a..adeb2b9 100644 --- a/libmbse/include/mbse/factors/FactorTrapInt.h +++ b/libmbse/include/mbse/factors/FactorTrapInt.h @@ -75,10 +75,10 @@ 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, gtsam::OptionalMatrixType H1 = OptionalNone, - gtsam::OptionalMatrixType H2 = OptionalNone, - gtsam::OptionalMatrixType H3 = OptionalNone, - gtsam::OptionalMatrixType H4 = OptionalNone) const override; + 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; /** number of variables attached to this factor */ std::size_t size() const { return 4; } diff --git a/libmbse/src/factors/FactorConstraints.cpp b/libmbse/src/factors/FactorConstraints.cpp index fb054c1..f36e6e7 100644 --- a/libmbse/src/factors/FactorConstraints.cpp +++ b/libmbse/src/factors/FactorConstraints.cpp @@ -17,8 +17,7 @@ FactorConstraints::~FactorConstraints() = default; gtsam::NonlinearFactor::shared_ptr FactorConstraints::clone() const { - return std::static_pointer_cast( - gtsam::NonlinearFactor::shared_ptr(new This(*this))); + return gtsam::NonlinearFactor::shared_ptr(new This(*this)); } void FactorConstraints::print( @@ -37,7 +36,7 @@ bool FactorConstraints::equals( } gtsam::Vector FactorConstraints::evaluateError( - const state_t& q_k, gtsam::OptionalMatrixType H1) const + const state_t& q_k, boost::optional H1) const { MRPT_START diff --git a/libmbse/src/factors/FactorConstraintsAccIndep.cpp b/libmbse/src/factors/FactorConstraintsAccIndep.cpp index ed440dc..7d7f5da 100644 --- a/libmbse/src/factors/FactorConstraintsAccIndep.cpp +++ b/libmbse/src/factors/FactorConstraintsAccIndep.cpp @@ -31,8 +31,7 @@ FactorConstraintsAccIndep::~FactorConstraintsAccIndep() = default; gtsam::NonlinearFactor::shared_ptr FactorConstraintsAccIndep::clone() const { - return std::static_pointer_cast( - gtsam::NonlinearFactor::shared_ptr(new This(*this))); +return gtsam::NonlinearFactor::shared_ptr(new This(*this)); } void FactorConstraintsAccIndep::print( @@ -53,9 +52,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, gtsam::OptionalMatrixType de_dq, - gtsam::OptionalMatrixType de_dqp, gtsam::OptionalMatrixType de_dqpp, - gtsam::OptionalMatrixType de_dzpp) const + const state_t& ddotz_k, boost::optional de_dq, + boost::optional de_dqp, boost::optional de_dqpp, + boost::optional de_dzpp) const { MRPT_START diff --git a/libmbse/src/factors/FactorConstraintsIndep.cpp b/libmbse/src/factors/FactorConstraintsIndep.cpp index 4b70e47..f85b640 100644 --- a/libmbse/src/factors/FactorConstraintsIndep.cpp +++ b/libmbse/src/factors/FactorConstraintsIndep.cpp @@ -26,14 +26,16 @@ FactorConstraintsIndep::FactorConstraintsIndep( { } + + FactorConstraintsIndep::~FactorConstraintsIndep() = default; gtsam::NonlinearFactor::shared_ptr FactorConstraintsIndep::clone() const { - return std::static_pointer_cast( - gtsam::NonlinearFactor::shared_ptr(new This(*this))); + return gtsam::NonlinearFactor::shared_ptr(new This(*this)); } + void FactorConstraintsIndep::print( const std::string& s, const gtsam::KeyFormatter& keyFormatter) const { @@ -43,6 +45,8 @@ void FactorConstraintsIndep::print( noiseModel_->print(" noise model: "); } + + bool FactorConstraintsIndep::equals( const gtsam::NonlinearFactor& expected, double tol) const { @@ -50,9 +54,10 @@ bool FactorConstraintsIndep::equals( return e != nullptr && Base::equals(*e, tol); } + gtsam::Vector FactorConstraintsIndep::evaluateError( - const state_t& z_k, const state_t& q_k, gtsam::OptionalMatrixType de_dz, - gtsam::OptionalMatrixType de_dq) const + const state_t& z_k, const state_t& q_k, boost::optional de_dz, + boost::optional de_dq) const { MRPT_START diff --git a/libmbse/src/factors/FactorConstraintsVel.cpp b/libmbse/src/factors/FactorConstraintsVel.cpp index 489e97b..bcaa01c 100644 --- a/libmbse/src/factors/FactorConstraintsVel.cpp +++ b/libmbse/src/factors/FactorConstraintsVel.cpp @@ -27,8 +27,7 @@ FactorConstraintsVel::~FactorConstraintsVel() = default; gtsam::NonlinearFactor::shared_ptr FactorConstraintsVel::clone() const { - return std::static_pointer_cast( - gtsam::NonlinearFactor::shared_ptr(new This(*this))); +return gtsam::NonlinearFactor::shared_ptr(new This(*this)); } void FactorConstraintsVel::print( @@ -85,8 +84,8 @@ static void num_err_wrt_dq( #endif gtsam::Vector FactorConstraintsVel::evaluateError( - const state_t& q_k, const state_t& dotq_k, gtsam::OptionalMatrixType H1, - gtsam::OptionalMatrixType H2) const + const state_t& q_k, const state_t& dotq_k, boost::optional H1, + boost::optional H2) const { MRPT_START diff --git a/libmbse/src/factors/FactorConstraintsVelIndep.cpp b/libmbse/src/factors/FactorConstraintsVelIndep.cpp index d0a31dc..922c49f 100644 --- a/libmbse/src/factors/FactorConstraintsVelIndep.cpp +++ b/libmbse/src/factors/FactorConstraintsVelIndep.cpp @@ -31,8 +31,7 @@ FactorConstraintsVelIndep::~FactorConstraintsVelIndep() = default; gtsam::NonlinearFactor::shared_ptr FactorConstraintsVelIndep::clone() const { - return std::static_pointer_cast( - gtsam::NonlinearFactor::shared_ptr(new This(*this))); +return gtsam::NonlinearFactor::shared_ptr(new This(*this)); } void FactorConstraintsVelIndep::print( @@ -53,8 +52,8 @@ bool FactorConstraintsVelIndep::equals( gtsam::Vector FactorConstraintsVelIndep::evaluateError( const state_t& q_k, const state_t& dotq_k, const state_t& dotz_k, - gtsam::OptionalMatrixType de_dq, gtsam::OptionalMatrixType de_dqp, - gtsam::OptionalMatrixType de_dzp) const + boost::optional de_dq, boost::optional de_dqp, + boost::optional de_dzp) const { MRPT_START diff --git a/libmbse/src/factors/FactorDynamics.cpp b/libmbse/src/factors/FactorDynamics.cpp index 6f9c95c..35057bb 100644 --- a/libmbse/src/factors/FactorDynamics.cpp +++ b/libmbse/src/factors/FactorDynamics.cpp @@ -14,9 +14,9 @@ #include #include -#if defined(GTSAM_USE_TBB) -#error "So far, MBSE is incompatible with GTSAM+TBB!" -#endif +//#if defined(GTSAM_USE_TBB) +//#error "So far, MBSE is incompatible with GTSAM+TBB!" +//#endif MRPT_TODO( "**IMPORTANT** Refactor AssembledRigidModel to separate state and model " "data to avoid multithread errors using GTSAM+TBB") @@ -33,8 +33,7 @@ FactorDynamics::~FactorDynamics() = default; gtsam::NonlinearFactor::shared_ptr FactorDynamics::clone() const { - return std::static_pointer_cast( - gtsam::NonlinearFactor::shared_ptr(new This(*this))); + return gtsam::NonlinearFactor::shared_ptr(new This(*this)); } void FactorDynamics::print( @@ -97,8 +96,8 @@ bool FactorDynamics::equals( gtsam::Vector FactorDynamics::evaluateError( const state_t& q_k, const state_t& dq_k, const state_t& ddq_k, - gtsam::OptionalMatrixType H1, gtsam::OptionalMatrixType H2, - gtsam::OptionalMatrixType H3) const + boost::optional H1 , boost::optional H2, + boost::optional H3) const { MRPT_START diff --git a/libmbse/src/factors/FactorDynamicsIndep.cpp b/libmbse/src/factors/FactorDynamicsIndep.cpp index 200ff22..26c2e08 100644 --- a/libmbse/src/factors/FactorDynamicsIndep.cpp +++ b/libmbse/src/factors/FactorDynamicsIndep.cpp @@ -26,8 +26,7 @@ FactorDynamicsIndep::~FactorDynamicsIndep() = default; gtsam::NonlinearFactor::shared_ptr FactorDynamicsIndep::clone() const { - return std::static_pointer_cast( - gtsam::NonlinearFactor::shared_ptr(new This(*this))); +return gtsam::NonlinearFactor::shared_ptr(new This(*this)); } void FactorDynamicsIndep::print( @@ -117,8 +116,8 @@ bool FactorDynamicsIndep::equals( gtsam::Vector FactorDynamicsIndep::evaluateError( const state_t& z_k, const state_t& dz_k, const state_t& ddz_k, - gtsam::OptionalMatrixType de_dz, gtsam::OptionalMatrixType de_dzp, - gtsam::OptionalMatrixType de_dzpp) const + boost::optional de_dz, boost::optional de_dzp, + boost::optional de_dzpp) const { MRPT_START diff --git a/libmbse/src/factors/FactorEulerInt.cpp b/libmbse/src/factors/FactorEulerInt.cpp index 620bb1a..5cf0d23 100644 --- a/libmbse/src/factors/FactorEulerInt.cpp +++ b/libmbse/src/factors/FactorEulerInt.cpp @@ -17,8 +17,7 @@ FactorEulerInt::~FactorEulerInt() = default; gtsam::NonlinearFactor::shared_ptr FactorEulerInt::clone() const { - return std::static_pointer_cast( - gtsam::NonlinearFactor::shared_ptr(new This(*this))); + return gtsam::NonlinearFactor::shared_ptr(new This(*this)); } void FactorEulerInt::print( @@ -41,8 +40,8 @@ bool FactorEulerInt::equals( gtsam::Vector FactorEulerInt::evaluateError( const state_t& x_k, const state_t& x_kp1, const state_t& v_k, - gtsam::OptionalMatrixType H1, gtsam::OptionalMatrixType H2, - gtsam::OptionalMatrixType H3) const + boost::optional H1, boost::optional H2, + boost::optional H3) const { const auto n = x_k.size(); diff --git a/libmbse/src/factors/FactorGyroscope.cpp b/libmbse/src/factors/FactorGyroscope.cpp index 897bcd5..0446507 100644 --- a/libmbse/src/factors/FactorGyroscope.cpp +++ b/libmbse/src/factors/FactorGyroscope.cpp @@ -22,8 +22,7 @@ using namespace mrpt::math; gtsam::NonlinearFactor::shared_ptr FactorGyroscope::clone() const { - return std::static_pointer_cast( - gtsam::NonlinearFactor::shared_ptr(new This(*this))); +return gtsam::NonlinearFactor::shared_ptr(new This(*this)); } void FactorGyroscope::print( @@ -44,8 +43,8 @@ bool FactorGyroscope::equals( } gtsam::Vector FactorGyroscope::evaluateError( - const state_t& q_k, const state_t& dq_k, gtsam::OptionalMatrixType H1, - gtsam::OptionalMatrixType H2) const + const state_t& q_k, const state_t& dq_k, boost::optional H1, + boost::optional H2) const { const auto n = q_k.size(); if (dq_k.size() != n) diff --git a/libmbse/src/factors/FactorInverseDynamics.cpp b/libmbse/src/factors/FactorInverseDynamics.cpp index 1e8b50a..41781e3 100644 --- a/libmbse/src/factors/FactorInverseDynamics.cpp +++ b/libmbse/src/factors/FactorInverseDynamics.cpp @@ -24,8 +24,7 @@ FactorInverseDynamics::~FactorInverseDynamics() = default; gtsam::NonlinearFactor::shared_ptr FactorInverseDynamics::clone() const { - return std::static_pointer_cast( - gtsam::NonlinearFactor::shared_ptr(new This(*this))); +return gtsam::NonlinearFactor::shared_ptr(new This(*this)); } void FactorInverseDynamics::print( @@ -110,7 +109,7 @@ bool FactorInverseDynamics::equals( } gtsam::Vector FactorInverseDynamics::evaluateError( - const state_t& Q_k, gtsam::OptionalMatrixType d_e_Q) const + const state_t& Q_k, boost::optional d_e_Q) const { MRPT_START diff --git a/libmbse/src/factors/FactorTrapInt.cpp b/libmbse/src/factors/FactorTrapInt.cpp index a134561..d451fb0 100644 --- a/libmbse/src/factors/FactorTrapInt.cpp +++ b/libmbse/src/factors/FactorTrapInt.cpp @@ -17,8 +17,7 @@ FactorTrapInt::~FactorTrapInt() = default; gtsam::NonlinearFactor::shared_ptr FactorTrapInt::clone() const { - return std::static_pointer_cast( - gtsam::NonlinearFactor::shared_ptr(new This(*this))); +return gtsam::NonlinearFactor::shared_ptr(new This(*this)); } // Build function print defined in the header FactorTrapInt.h @@ -45,9 +44,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, gtsam::OptionalMatrixType H1, - gtsam::OptionalMatrixType H2, gtsam::OptionalMatrixType H3, - gtsam::OptionalMatrixType H4) const + const state_t& v_kp1, boost::optional H1, + boost::optional H2, boost::optional H3, + boost::optional H4) const { const auto n = x_k.size();