Port to gtsam 4.2

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:

apps/mbse-fg-smoother-forward-dynamics-icoords/main.cpp

@@ -25,7 +25,7 @@
 #include <gtsam/nonlinear/Values.h>
 #include <gtsam/slam/BetweenFactor.h>
 #include <gtsam/slam/PriorFactor.h>
-#include <gtsam_unstable/nonlinear/BatchFixedLagSmoother.h>
+#include <gtsam/nonlinear/BatchFixedLagSmoother.h>
 #include <gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.h>
 #include <iostream>
 #include <mbse/AssembledRigidModel.h>
@@ -186,10 +186,10 @@ void test_smoother()
 
 	// Create Prior factors:
 	factorsByTime.emplace(
-		0.0, boost::make_shared<gtsam::PriorFactor<state_t>>(
+		0.0, std::make_shared<gtsam::PriorFactor<state_t>>(
 				 sZ(0), z_0, noise_prior_z_0));
 	factorsByTime.emplace(
-		0.0, boost::make_shared<gtsam::PriorFactor<state_t>>(
+		0.0, std::make_shared<gtsam::PriorFactor<state_t>>(
 				 sZp(0), zeros_z, noise_prior_dz_0));
 
 	const double lag = arg_lag_time.getValue();	 // seconds
@@ -278,67 +278,67 @@ void test_smoother()
 
 		// Create Trapezoidal Integrator factors:
 		factorsByTime.emplace(
-			t, boost::make_shared<FactorTrapInt>(
+			t, std::make_shared<FactorTrapInt>(
 				   dt, noise_vel_z, sZ(timeStep), sZ(timeStep + 1),
 				   sZp(timeStep), sZp(timeStep + 1)));
 		factorsByTime.emplace(
-			t, boost::make_shared<FactorTrapInt>(
+			t, std::make_shared<FactorTrapInt>(
 				   dt, noise_acc_z, sZp(timeStep), sZp(timeStep + 1),
 				   sZpp(timeStep), sZpp(timeStep + 1)));
 
 		// Create Dynamics factors:
 		factorsByTime.emplace(
 			t + dt,
-			boost::make_shared<FactorDynamicsIndep>(
+			std::make_shared<FactorDynamicsIndep>(
 				&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<FactorDynamicsIndep>(
+				t, std::make_shared<FactorDynamicsIndep>(
 					   &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<gtsam::BetweenFactor<state_t>>(
+			t, std::make_shared<gtsam::BetweenFactor<state_t>>(
 				   sQ(timeStep), sQ(timeStep + 1), zeros_q, softBetweenNoise));
 
 		// Add dependent-coordinates constraint factor:
 		if (timeStep == 0)
 		{
 			factorsByTime.emplace(
-				t, boost::make_shared<FactorConstraintsIndep>(
+				t, std::make_shared<FactorConstraintsIndep>(
 					   aMBS, indepCoordIndices, noise_constr_z, sZ(timeStep),
 					   sQ(timeStep)));
 
 			factorsByTime.emplace(
-				t, boost::make_shared<FactorConstraintsVelIndep>(
+				t, std::make_shared<FactorConstraintsVelIndep>(
 					   aMBS, indepCoordIndices, noise_constr_dz, sQ(timeStep),
 					   sQp(timeStep), sZp(timeStep)));
 
 			factorsByTime.emplace(
-				t, boost::make_shared<FactorConstraintsAccIndep>(
+				t, std::make_shared<FactorConstraintsAccIndep>(
 					   aMBS, indepCoordIndices, noise_constr_dz, sQ(timeStep),
 					   sQp(timeStep), sQpp(timeStep), sZpp(timeStep)));
 		}
 
 		// if (timeStep < N - 1)
 		{
 			factorsByTime.emplace(
-				t + dt, boost::make_shared<FactorConstraintsIndep>(
+				t + dt, std::make_shared<FactorConstraintsIndep>(
 							aMBS, indepCoordIndices, noise_constr_z,
 							sZ(timeStep + 1), sQ(timeStep + 1)));
 
 			factorsByTime.emplace(
 				t + dt,
-				boost::make_shared<FactorConstraintsVelIndep>(
+				std::make_shared<FactorConstraintsVelIndep>(
 					aMBS, indepCoordIndices, noise_constr_dz, sQ(timeStep + 1),
 					sQp(timeStep + 1), sZp(timeStep + 1)));
 
 			factorsByTime.emplace(
 				t + dt,
-				boost::make_shared<FactorConstraintsAccIndep>(
+				std::make_shared<FactorConstraintsAccIndep>(
 					aMBS, indepCoordIndices, noise_constr_dz, sQ(timeStep + 1),
 					sQp(timeStep + 1), sQpp(timeStep + 1), sZpp(timeStep + 1)));
 		}

apps/mbse-fg-smoother-forward-dynamics/main.cpp

@@ -14,7 +14,7 @@
 #include <gtsam/nonlinear/Values.h>
 #include <gtsam/slam/PriorFactor.h>
 #include <gtsam/nonlinear/NonlinearEquality.h>
-#include <gtsam_unstable/nonlinear/BatchFixedLagSmoother.h>
+#include <gtsam/nonlinear/BatchFixedLagSmoother.h>
 #include <gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.h>
 #include <iostream>
 #include <mbse/AssembledRigidModel.h>
@@ -164,9 +164,9 @@ void test_smoother()
 
 	// Create Prior factors:
 	factorsByTime.emplace(
-		0.0, boost::make_shared<gtsam::NonlinearEquality<state_t>>(Q(0), q_0));
+		0.0, std::make_shared<gtsam::NonlinearEquality<state_t>>(Q(0), q_0));
 	factorsByTime.emplace(
-		0.0, boost::make_shared<gtsam::PriorFactor<state_t>>(
+		0.0, std::make_shared<gtsam::PriorFactor<state_t>>(
 				 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<FactorTrapInt>(
+			t, std::make_shared<FactorTrapInt>(
 				   dt, noise_vel, Q(timeStep), Q(timeStep + 1), V(timeStep),
 				   V(timeStep + 1)));
 		factorsByTime.emplace(
-			t, boost::make_shared<FactorTrapInt>(
+			t, std::make_shared<FactorTrapInt>(
 				   dt, noise_acc, V(timeStep), V(timeStep + 1), A(timeStep),
 				   A(timeStep + 1)));
 
 		// Create Dynamics factors:
 		factorsByTime.emplace(
-			t + dt, boost::make_shared<FactorDynamics>(
+			t + dt, std::make_shared<FactorDynamics>(
 						&dynSimul, noise_dyn, Q(timeStep + 1), V(timeStep + 1),
 						A(timeStep + 1)));
 		if (timeStep == 0)
 			factorsByTime.emplace(
-				t, boost::make_shared<FactorDynamics>(
+				t, std::make_shared<FactorDynamics>(
 					   &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<FactorConstraints>(
+				t, std::make_shared<FactorConstraints>(
 					   aMBS, noise_constr_q, Q(timeStep)));
 
 		if (!arg_dont_add_dq_constraints.isSet())
 			factorsByTime.emplace(
-				t, boost::make_shared<FactorConstraintsVel>(
+				t, std::make_shared<FactorConstraintsVel>(
 					   aMBS, noise_constr_dq, Q(timeStep), V(timeStep)));
 
 		// Create initial estimates:

libmbse/include/mbse/factors/FactorConstraints.h

@@ -29,7 +29,7 @@ class FactorConstraints : public gtsam::NoiseModelFactor1<state_t>
 
    public:
 	// shorthand for a smart pointer to a factor
-	using shared_ptr = boost::shared_ptr<This>;
+	using shared_ptr = std::shared_ptr<This>;
 
 	/** default constructor - only use for serialization */
 	FactorConstraints() = default;
@@ -61,7 +61,7 @@ class FactorConstraints : public gtsam::NoiseModelFactor1<state_t>
 	/** vector of errors */
 	gtsam::Vector evaluateError(
 		const state_t& q_k,
-		boost::optional<gtsam::Matrix&> 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<state_t>
 	template <class ARCHIVE>
 	void serialize(ARCHIVE& ar, const unsigned int /*version*/)
 	{
+#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
 		ar& boost::serialization::make_nvp(
 			"FactorConstraints",
 			boost::serialization::base_object<Base>(*this));
+#endif
 	}
 };
 

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<This>;
+	using shared_ptr = std::shared_ptr<This>;
 
 	/** 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<gtsam::Matrix&> de_dq = boost::none,
-		boost::optional<gtsam::Matrix&> de_dqp = boost::none,
-		boost::optional<gtsam::Matrix&> de_dqpp = boost::none,
-		boost::optional<gtsam::Matrix&> 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 <class ARCHIVE>
 	void serialize(ARCHIVE& ar, const unsigned int /*version*/)
 	{
+#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
 		ar& boost::serialization::make_nvp(
 			"FactorConstraintsAccIndep",
 			boost::serialization::base_object<Base>(*this));
+#endif
 	}
 };
 

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<This>;
+	using shared_ptr = std::shared_ptr<This>;
 
 	/** 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<gtsam::Matrix&> de_dz = boost::none,
-		boost::optional<gtsam::Matrix&> 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 <class ARCHIVE>
 	void serialize(ARCHIVE& ar, const unsigned int /*version*/)
 	{
+#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
 		ar& boost::serialization::make_nvp(
 			"FactorConstraintsIndep",
 			boost::serialization::base_object<Base>(*this));
+#endif
 	}
 };
 

libmbse/include/mbse/factors/FactorConstraintsVel.h

@@ -31,7 +31,7 @@ class FactorConstraintsVel : public gtsam::NoiseModelFactor2<state_t, state_t>
 
    public:
 	// shorthand for a smart pointer to a factor
-	using shared_ptr = boost::shared_ptr<This>;
+	using shared_ptr = std::shared_ptr<This>;
 
 	/** default constructor - only use for serialization */
 	FactorConstraintsVel() = default;
@@ -64,8 +64,8 @@ class FactorConstraintsVel : public gtsam::NoiseModelFactor2<state_t, state_t>
 	/** vector of errors */
 	gtsam::Vector evaluateError(
 		const state_t& q_k, const state_t& dotq_k,
-		boost::optional<gtsam::Matrix&> H1 = boost::none,
-		boost::optional<gtsam::Matrix&> 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<state_t, state_t>
 	template <class ARCHIVE>
 	void serialize(ARCHIVE& ar, const unsigned int /*version*/)
 	{
+#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
 		ar& boost::serialization::make_nvp(
 			"FactorConstraintsVel",
 			boost::serialization::base_object<Base>(*this));
+#endif
 	}
 };
 

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<This>;
+	using shared_ptr = std::shared_ptr<This>;
 
 	/** 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<gtsam::Matrix&> de_dq = boost::none,
-		boost::optional<gtsam::Matrix&> de_dqp = boost::none,
-		boost::optional<gtsam::Matrix&> 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 <class ARCHIVE>
 	void serialize(ARCHIVE& ar, const unsigned int /*version*/)
 	{
+#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
 		ar& boost::serialization::make_nvp(
 			"FactorConstraintsVelIndep",
 			boost::serialization::base_object<Base>(*this));
+#endif
 	}
 };
 

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<This>;
+	using shared_ptr = std::shared_ptr<This>;
 
 	/** 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<gtsam::Matrix&> H1 = boost::none,
-		boost::optional<gtsam::Matrix&> H2 = boost::none,
-		boost::optional<gtsam::Matrix&> 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 <class ARCHIVE>
 	void serialize(ARCHIVE& ar, const unsigned int /*version*/)
 	{
+#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
 		ar& boost::serialization::make_nvp(
 			"FactorDynamics", boost::serialization::base_object<Base>(*this));
+#endif
 	}
 };