Merge remote-tracking branch 'origin/fix/alignment' into fix/msvc2017

gtsam/base/GenericValue.h

@@ -110,7 +110,7 @@ public:
      * Clone this value (normal clone on the heap, delete with 'delete' operator)
      */
     virtual boost::shared_ptr<Value> clone() const {
-      return boost::make_shared<GenericValue>(*this);
+		return boost::allocate_shared<GenericValue>(Eigen::aligned_allocator<GenericValue>(), *this);
     }
 
     /// Generic Value interface version of retract

gtsam/geometry/Rot2.h

@@ -59,7 +59,7 @@ namespace gtsam {
 
     /// Named constructor from angle in degrees
     static Rot2 fromDegrees(double theta) {
-      const double degree = M_PI / 180;
+      static const double degree = M_PI / 180;
       return fromAngle(theta * degree);
     }
 

gtsam/inference/FactorGraph.h

@@ -26,6 +26,8 @@
 #include <gtsam/base/FastVector.h>
 #include <gtsam/inference/Key.h>
 
+#include <Eigen/Core>  // for Eigen::aligned_allocator
+
 #include <boost/serialization/nvp.hpp>
 #include <boost/assign/list_inserter.hpp>
 #include <boost/bind.hpp>
@@ -161,11 +163,11 @@ namespace gtsam {
       factors_.push_back(factor); }
 
     /** Emplace a factor */
-    template<class DERIVEDFACTOR, class... Args>
+	template<class DERIVEDFACTOR, class... Args>
-    typename std::enable_if<std::is_base_of<FactorType, DERIVEDFACTOR>::value>::type
+	typename std::enable_if<std::is_base_of<FactorType, DERIVEDFACTOR>::value>::type
-    emplace_shared(Args&&... args) {
+		emplace_shared(Args&&... args) {
-        factors_.push_back(boost::make_shared<DERIVEDFACTOR>(std::forward<Args>(args)...));
+		factors_.push_back(boost::allocate_shared<DERIVEDFACTOR>(Eigen::aligned_allocator<DERIVEDFACTOR>(), std::forward<Args>(args)...));
-    }
+	}
 
     /** push back many factors with an iterator over shared_ptr (factors are not copied) */
     template<typename ITERATOR>
@@ -194,7 +196,7 @@ namespace gtsam {
     template<class DERIVEDFACTOR>
     typename std::enable_if<std::is_base_of<FactorType, DERIVEDFACTOR>::value>::type
       push_back(const DERIVEDFACTOR& factor) {
-        factors_.push_back(boost::make_shared<DERIVEDFACTOR>(factor));
+        factors_.push_back(boost::allocate_shared<DERIVEDFACTOR>(Eigen::aligned_allocator<DERIVEDFACTOR>(), factor));
     }
 //#endif
 

gtsam/navigation/CombinedImuFactor.cpp

@@ -252,8 +252,8 @@ CombinedImuFactor::CombinedImuFactor(
 : Base(noiseModel::Gaussian::Covariance(pim.preintMeasCov_), pose_i, vel_i,
     pose_j, vel_j, bias_i, bias_j),
 _PIM_(pim) {
-  boost::shared_ptr<CombinedPreintegratedMeasurements::Params> p =
+  using P = CombinedPreintegratedMeasurements::Params;
-  boost::make_shared<CombinedPreintegratedMeasurements::Params>(pim.p());
+  auto p = boost::allocate_shared<P>(Eigen::aligned_allocator<P>(), pim.p());
   p->n_gravity = n_gravity;
   p->omegaCoriolis = omegaCoriolis;
   p->body_P_sensor = body_P_sensor;

gtsam/navigation/CombinedImuFactor.h

@@ -320,6 +320,9 @@ private:
          boost::serialization::base_object<Base>(*this));
     ar & BOOST_SERIALIZATION_NVP(_PIM_);
   }
+
+public:
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 };
 // class CombinedImuFactor
 

gtsam/navigation/PreintegratedRotation.h

@@ -59,8 +59,11 @@ struct GTSAM_EXPORT PreintegratedRotationParams {
     ar & BOOST_SERIALIZATION_NVP(body_P_sensor);
   }
 
+#ifdef GTSAM_USE_QUATERNIONS
+  // Align if we are using Quaternions
 public:
-  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+	EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+#endif
 };
 
 /**
@@ -169,8 +172,11 @@ class GTSAM_EXPORT PreintegratedRotation {
     ar& BOOST_SERIALIZATION_NVP(delRdelBiasOmega_);
   }
 
- public:
+#ifdef GTSAM_USE_QUATERNIONS
-  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+  // Align if we are using Quaternions
+  public:
+	  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+#endif
 };
 
 template <>

gtsam/navigation/PreintegrationBase.h

@@ -202,7 +202,8 @@ public:
   /// @}
 #endif
 
-  /// @}
+public:
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 };
 
 } /// namespace gtsam

gtsam/navigation/PreintegrationParams.h

@@ -75,8 +75,11 @@ protected:
     ar & BOOST_SERIALIZATION_NVP(n_gravity);
   }
 
+#ifdef GTSAM_USE_QUATERNIONS
+  // Align if we are using Quaternions
 public:
-  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+	EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+#endif
 };
 
 } // namespace gtsam

gtsam/nonlinear/ExpressionFactor.h

@@ -205,6 +205,11 @@ private:
  BOOST_SERIALIZATION_SPLIT_MEMBER()
 
  friend class boost::serialization::access;
+
+ // Alignment, see https://eigen.tuxfamily.org/dox/group__TopicStructHavingEigenMembers.html
+ enum { NeedsToAlign = (sizeof(T) % 16) == 0 };
+  public:
+	  EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign)
 };
 // ExpressionFactor
 

gtsam/slam/ProjectionFactor.h

@@ -186,7 +186,10 @@ namespace gtsam {
       ar & BOOST_SERIALIZATION_NVP(throwCheirality_);
       ar & BOOST_SERIALIZATION_NVP(verboseCheirality_);
     }
-  };
+
+  public:
+    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+};
 
   /// traits
   template<class POSE, class LANDMARK, class CALIBRATION>