fix ros gtsam 4.2a9 build (introlab#1108)

corelib/src/optimizer/OptimizerGTSAM.cpp

@@ -527,7 +527,11 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
 			{
 				float x,y,z,roll,pitch,yaw;
 				std::map<int, Transform> tmpPoses;
+#if GTSAM_VERSION_NUMERIC >= 40200
+				for(gtsam::Values::deref_iterator iter=optimizer->values().begin(); iter!=optimizer->values().end(); ++iter)
+#else
 				for(gtsam::Values::const_iterator iter=optimizer->values().begin(); iter!=optimizer->values().end(); ++iter)
+#endif
 				{
 					if(iter->value.dim() > 1)
 					{
@@ -630,7 +634,11 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
 				optimizer->iterations(), optimizer->error(), graph.error(initialEstimate), graph.error(optimizer->values()), timer.ticks());
 
 		float x,y,z,roll,pitch,yaw;
+#if GTSAM_VERSION_NUMERIC >= 40200
+		for(gtsam::Values::deref_iterator iter=optimizer->values().begin(); iter!=optimizer->values().end(); ++iter)
+#else
 		for(gtsam::Values::const_iterator iter=optimizer->values().begin(); iter!=optimizer->values().end(); ++iter)
+#endif
 		{
 			if(iter->value.dim() > 1)
 			{

corelib/src/optimizer/gtsam/GravityFactor.h

@@ -63,15 +63,21 @@ class GravityFactor {
 
   /** vector of errors */
   Vector attitudeError(const Rot3& p,
-      OptionalJacobian<2,3> H = boost::none) const;
+#if GTSAM_VERSION_NUMERIC >= 40300
+      OptionalJacobian<2,3> H = {}) const;
+#else
+  	  OptionalJacobian<2,3> H = boost::none) const;
+#endif
 
   /** Serialization function */
+#if defined(GTSAM_ENABLE_BOOST_SERIALIZATION) || GTSAM_VERSION_NUMERIC < 40300
   friend class boost::serialization::access;
   template<class ARCHIVE>
   void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
     ar & boost::serialization::make_nvp("nZ_", const_cast<Unit3&>(nZ_));
     ar & boost::serialization::make_nvp("bRef_", const_cast<Unit3&>(bRef_));
   }
+#endif
 };
 
 /**
@@ -85,8 +91,11 @@ class Rot3GravityFactor: public NoiseModelFactor1<Rot3>, public GravityFactor {
 public:
 
   /// shorthand for a smart pointer to a factor
+#if GTSAM_VERSION_NUMERIC >= 40300
+  typedef std::shared_ptr<Rot3GravityFactor> shared_ptr;
+#else
   typedef boost::shared_ptr<Rot3GravityFactor> shared_ptr;
-
+#endif
   /// Typedef to this class
   typedef Rot3GravityFactor This;
 
@@ -111,10 +120,13 @@ class Rot3GravityFactor: public NoiseModelFactor1<Rot3>, public GravityFactor {
 
   /// @return a deep copy of this factor
   virtual gtsam::NonlinearFactor::shared_ptr clone() const {
+#if GTSAM_VERSION_NUMERIC >= 40300
+    return std::static_pointer_cast<gtsam::NonlinearFactor>(
+#else
     return boost::static_pointer_cast<gtsam::NonlinearFactor>(
         gtsam::NonlinearFactor::shared_ptr(new This(*this)));
   }
-
+#endif
   /** print */
   virtual void print(const std::string& s, const KeyFormatter& keyFormatter =
       DefaultKeyFormatter) const;
@@ -124,7 +136,11 @@ class Rot3GravityFactor: public NoiseModelFactor1<Rot3>, public GravityFactor {
 
   /** vector of errors */
   virtual Vector evaluateError(const Rot3& nRb, //
-      boost::optional<Matrix&> H = boost::none) const {
+#if GTSAM_VERSION_NUMERIC >= 40300
+	OptionalMatrixType H = OptionalNone) const {
+#else
+	boost::optional<Matrix&> H = boost::none) const {
+#endif
     return attitudeError(nRb, H);
   }
   Unit3 nZ() const {
@@ -135,7 +151,7 @@ class Rot3GravityFactor: public NoiseModelFactor1<Rot3>, public GravityFactor {
   }
 
 private:
-
+#if defined(GTSAM_ENABLE_BOOST_SERIALIZATION) || GTSAM_VERSION_NUMERIC < 40300
   /** Serialization function */
   friend class boost::serialization::access;
   template<class ARCHIVE>
@@ -145,6 +161,7 @@ class Rot3GravityFactor: public NoiseModelFactor1<Rot3>, public GravityFactor {
     ar & boost::serialization::make_nvp("GravityFactor",
         boost::serialization::base_object<GravityFactor>(*this));
   }
+#endif
 
 public:
   EIGEN_MAKE_ALIGNED_OPERATOR_NEW
@@ -163,8 +180,11 @@ class Pose3GravityFactor: public NoiseModelFactor1<Pose3>,
 public:
 
   /// shorthand for a smart pointer to a factor
+#if GTSAM_VERSION_NUMERIC >= 40300
+  typedef std::shared_ptr<Pose3GravityFactor> shared_ptr;
+#else
   typedef boost::shared_ptr<Pose3GravityFactor> shared_ptr;
-
+#endif
   /// Typedef to this class
   typedef Pose3GravityFactor This;
 
@@ -189,7 +209,11 @@ class Pose3GravityFactor: public NoiseModelFactor1<Pose3>,
 
   /// @return a deep copy of this factor
   virtual gtsam::NonlinearFactor::shared_ptr clone() const {
+#if GTSAM_VERSION_NUMERIC >= 40300
+    return std::static_pointer_cast<gtsam::NonlinearFactor>(
+#else
     return boost::static_pointer_cast<gtsam::NonlinearFactor>(
+#endif
         gtsam::NonlinearFactor::shared_ptr(new This(*this)));
   }
 
@@ -202,7 +226,11 @@ class Pose3GravityFactor: public NoiseModelFactor1<Pose3>,
 
   /** vector of errors */
   virtual Vector evaluateError(const Pose3& nTb, //
+#if GTSAM_VERSION_NUMERIC >= 40300
+      OptionalMatrixType H = OptionalNone) const {
+#else
       boost::optional<Matrix&> H = boost::none) const {
+#endif
     Vector e = attitudeError(nTb.rotation(), H);
     if (H) {
       Matrix H23 = *H;
@@ -219,7 +247,7 @@ class Pose3GravityFactor: public NoiseModelFactor1<Pose3>,
   }
 
 private:
-
+#if defined(GTSAM_ENABLE_BOOST_SERIALIZATION) || GTSAM_VERSION_NUMERIC < 40300
   /** Serialization function */
   friend class boost::serialization::access;
   template<class ARCHIVE>
@@ -229,7 +257,7 @@ class Pose3GravityFactor: public NoiseModelFactor1<Pose3>,
     ar & boost::serialization::make_nvp("GravityFactor",
         boost::serialization::base_object<GravityFactor>(*this));
   }
-
+#endif
 public:
   EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 };

corelib/src/optimizer/gtsam/XYFactor.h

@@ -41,7 +41,12 @@ class XYFactor: public gtsam::NoiseModelFactor1<VALUE> {
   // error function
   // @param p    the pose in Pose2
   // @param H    the optional Jacobian matrix, which use boost optional and has default null pointer
-  gtsam::Vector evaluateError(const VALUE& p, boost::optional<gtsam::Matrix&> H = boost::none) const {
+  gtsam::Vector evaluateError(const VALUE& p,
+#if GTSAM_VERSION_NUMERIC >= 40300
+		  OptionalMatrixType H = OptionalNone) const {
+#else
+		  boost::optional<gtsam::Matrix&> H = boost::none) const {
+#endif
 
     // note that use boost optional like a pointer
     // only calculate jacobian matrix when non-null pointer exists

corelib/src/optimizer/gtsam/XYZFactor.h

@@ -41,14 +41,24 @@ class XYZFactor: public gtsam::NoiseModelFactor1<VALUE> {
   // error function
   // @param p    the pose in Pose
   // @param H    the optional Jacobian matrix, which use boost optional and has default null pointer
-  gtsam::Vector evaluateError(const gtsam::Pose3& p, boost::optional<gtsam::Matrix&> H = boost::none) const {
+  gtsam::Vector evaluateError(const gtsam::Pose3& p,
+#if GTSAM_VERSION_NUMERIC >= 40300
+		  OptionalMatrixType H = OptionalNone) const {
+#else
+		  boost::optional<gtsam::Matrix&> H = boost::none) const {
+#endif
     if(H)
     {
 	  p.translation(H);
     }
     return (gtsam::Vector3() << p.x() - mx_, p.y() - my_, p.z() - mz_).finished();
   }
-  gtsam::Vector evaluateError(const gtsam::Point3& p, boost::optional<gtsam::Matrix&> H = boost::none) const {
+  gtsam::Vector evaluateError(const gtsam::Point3& p,
+#if GTSAM_VERSION_NUMERIC >= 40300
+		  OptionalMatrixType H = OptionalNone) const {
+#else
+		  boost::optional<gtsam::Matrix&> H = boost::none) const {
+#endif
     return (gtsam::Vector3() << p.x() - mx_, p.y() - my_, p.z() - mz_).finished();
   }
 };

corelib/src/optimizer/vertigo/gtsam/DerivedValue.h

@@ -72,10 +72,15 @@ class DerivedValue : public gtsam::Value {
   /**
    * Clone this value (normal clone on the heap, delete with 'delete' operator)
    */
+#if GTSAM_VERSION_NUMERIC >= 40300
+  virtual std::shared_ptr<gtsam::Value> clone() const {
+    return std::make_shared<DERIVED>(static_cast<const DERIVED&>(*this));
+  }
+  #else
   virtual boost::shared_ptr<gtsam::Value> clone() const {
     return boost::make_shared<DERIVED>(static_cast<const DERIVED&>(*this));
   }
-
+#endif
   /// equals implementing generic Value interface
   virtual bool equals_(const gtsam::Value& p, double tol = 1e-9) const {
     // Cast the base class Value pointer to a derived class pointer

corelib/src/optimizer/vertigo/gtsam/betweenFactorMaxMix.h

@@ -12,7 +12,7 @@
 #include <Eigen/Eigen>
 #include <gtsam/config.h>
 
-#if GTSAM_VERSION_MAJOR > 4 || (GTSAM_VERSION_MAJOR==4 && GTSAM_VERSION_MINOR>=1)
+#if GTSAM_VERSION_NUMERIC >= 40100
 namespace gtsam {
 gtsam::Matrix inverse(const gtsam::Matrix & matrix)
 {
@@ -49,7 +49,7 @@ namespace vertigo {
           double nu1 = 1.0/sqrt(gtsam::inverse(info1).determinant());
           double l1 = nu1 * exp(-0.5*m1);
 
-#if GTSAM_VERSION_MAJOR > 4 || (GTSAM_VERSION_MAJOR==4 && GTSAM_VERSION_MINOR>=1)
+#if GTSAM_VERSION_NUMERIC >= 40100
           double m2 = nullHypothesisModel->squaredMahalanobisDistance(error);
 #else
           double m2 = nullHypothesisModel->distance(error);

corelib/src/optimizer/vertigo/gtsam/betweenFactorSwitchable.h

@@ -30,9 +30,15 @@ namespace vertigo {
         betweenFactor(key1, key2, measured, model) {};
 
       gtsam::Vector evaluateError(const VALUE& p1, const VALUE& p2, const SwitchVariableLinear& s,
+#if GTSAM_VERSION_NUMERIC >= 40300
+		  OptionalMatrixType H1 = OptionalNone,
+		  OptionalMatrixType H2 = OptionalNone,
+		  OptionalMatrixType H3 = OptionalNone) const
+#else
           boost::optional<gtsam::Matrix&> H1 = boost::none,
           boost::optional<gtsam::Matrix&> H2 =  boost::none,
           boost::optional<gtsam::Matrix&> H3 =  boost::none) const
+#endif
         {
 
           // calculate error
@@ -64,9 +70,15 @@ namespace vertigo {
         betweenFactor(key1, key2, measured, model) {};
 
       gtsam::Vector evaluateError(const VALUE& p1, const VALUE& p2, const SwitchVariableSigmoid& s,
+#if GTSAM_VERSION_NUMERIC >= 40300
+		  OptionalMatrixType H1 = OptionalNone,
+		  OptionalMatrixType H2 = OptionalNone,
+		  OptionalMatrixType H3 = OptionalNone) const
+#else
           boost::optional<gtsam::Matrix&> H1 = boost::none,
           boost::optional<gtsam::Matrix&> H2 =  boost::none,
           boost::optional<gtsam::Matrix&> H3 =  boost::none) const
+#endif
       {
 
         // calculate error

corelib/src/optimizer/vertigo/gtsam/switchVariableLinear.h

@@ -76,8 +76,13 @@ namespace vertigo {
 
     /** between operation */
     inline SwitchVariableLinear between(const SwitchVariableLinear& l2,
+#if GTSAM_VERSION_NUMERIC >= 40300
+		OptionalMatrixType H1=OptionalNone,
+		OptionalMatrixType H2=OptionalNone) const {
+#else
         boost::optional<gtsam::Matrix&> H1=boost::none,
         boost::optional<gtsam::Matrix&> H2=boost::none) const {
+#endif
       if(H1) *H1 = -gtsam::Matrix::Identity(1, 1);
       if(H2) *H2 = gtsam::Matrix::Identity(1, 1);
       return SwitchVariableLinear(l2.value() - value());
@@ -116,11 +121,19 @@ template<> struct traits<vertigo::SwitchVariableLinear> {
   typedef OptionalJacobian<3, 3> ChartJacobian;
   typedef gtsam::Vector TangentVector;
   static TangentVector Local(const vertigo::SwitchVariableLinear& origin, const vertigo::SwitchVariableLinear& other,
-  ChartJacobian Horigin = boost::none, ChartJacobian Hother = boost::none) {
+#if GTSAM_VERSION_NUMERIC >= 40300
+      ChartJacobian Horigin = {}, ChartJacobian Hother = {}) {
+#else
+      ChartJacobian Horigin = boost::none, ChartJacobian Hother = boost::none) {
+#endif
     return origin.localCoordinates(other);
   }
   static vertigo::SwitchVariableLinear Retract(const vertigo::SwitchVariableLinear& g, const TangentVector& v,
-        ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) {
+#if GTSAM_VERSION_NUMERIC >= 40300
+      ChartJacobian H1 = {}, ChartJacobian H2 = {}) {
+#else
+      ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) {
+#endif
       return g.retract(v);
     }
 };

corelib/src/optimizer/vertigo/gtsam/switchVariableSigmoid.h

@@ -76,8 +76,13 @@ namespace vertigo {
 
     /** between operation */
     inline SwitchVariableSigmoid between(const SwitchVariableSigmoid& l2,
+#if GTSAM_VERSION_NUMERIC >= 40300
+        OptionalMatrixType H1=OptionalNone,
+		OptionalMatrixType H2=OptionalNone) const {
+#else
         boost::optional<gtsam::Matrix&> H1=boost::none,
         boost::optional<gtsam::Matrix&> H2=boost::none) const {
+#endif
       if(H1) *H1 = -gtsam::Matrix::Identity(1, 1);
       if(H2) *H2 = gtsam::Matrix::Identity(1, 1);
       return SwitchVariableSigmoid(l2.value() - value());
@@ -117,11 +122,19 @@ template<> struct traits<vertigo::SwitchVariableSigmoid> {
   typedef OptionalJacobian<3, 3> ChartJacobian;
   typedef gtsam::Vector TangentVector;
   static TangentVector Local(const vertigo::SwitchVariableSigmoid& origin, const vertigo::SwitchVariableSigmoid& other,
-  ChartJacobian Horigin = boost::none, ChartJacobian Hother = boost::none) {
+#if GTSAM_VERSION_NUMERIC >= 40300
+	  ChartJacobian Horigin = {}, ChartJacobian Hother = {}) {
+#else
+	  ChartJacobian Horigin = boost::none, ChartJacobian Hother = boost::none) {
+#endif
     return origin.localCoordinates(other);
   }
   static vertigo::SwitchVariableSigmoid Retract(const vertigo::SwitchVariableSigmoid& g, const TangentVector& v,
+#if GTSAM_VERSION_NUMERIC >= 40300
+        ChartJacobian H1 = {}, ChartJacobian H2 = {}) {
+#else
         ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) {
+#endif
       return g.retract(v);
     }
 };