Merged in fix/alignment (pull request #333)

Fixing more alignment issues

.gitignore

@@ -17,3 +17,5 @@ cython/gtsam.so
 cython/gtsam_wrapper.pxd
 .vscode
 .env
+/.vs/
+/CMakeSettings.json

examples/SolverComparer.cpp

@@ -251,10 +251,10 @@ void runIncremental()
   Key firstPose;
   while(nextMeasurement < datasetMeasurements.size())
   {
-    if(BetweenFactor<Pose>::shared_ptr measurement =
+    if(BetweenFactor<Pose>::shared_ptr factor =
       boost::dynamic_pointer_cast<BetweenFactor<Pose> >(datasetMeasurements[nextMeasurement]))
     {
-      Key key1 = measurement->key1(), key2 = measurement->key2();
+      Key key1 = factor->key1(), key2 = factor->key2();
       if(((int)key1 >= firstStep && key1 < key2) || ((int)key2 >= firstStep && key2 < key1)) {
         // We found an odometry starting at firstStep
         firstPose = std::min(key1, key2);
@@ -302,52 +302,53 @@ void runIncremental()
 
       NonlinearFactor::shared_ptr measurementf = datasetMeasurements[nextMeasurement];
 
-      if(BetweenFactor<Pose>::shared_ptr measurement =
+      if(BetweenFactor<Pose>::shared_ptr factor =
         boost::dynamic_pointer_cast<BetweenFactor<Pose> >(measurementf))
       {
         // Stop collecting measurements that are for future steps
-        if(measurement->key1() > step || measurement->key2() > step)
+        if(factor->key1() > step || factor->key2() > step)
           break;
 
         // Require that one of the nodes is the current one
-        if(measurement->key1() != step && measurement->key2() != step)
+        if(factor->key1() != step && factor->key2() != step)
           throw runtime_error("Problem in data file, out-of-sequence measurements");
 
         // Add a new factor
-        newFactors.push_back(measurement);
+        newFactors.push_back(factor);
+        const auto& measured = factor->measured();
 
         // Initialize the new variable
-        if(measurement->key1() > measurement->key2()) {
+        if(factor->key1() > factor->key2()) {
-          if(!newVariables.exists(measurement->key1())) { // Only need to check newVariables since loop closures come after odometry
+          if(!newVariables.exists(factor->key1())) { // Only need to check newVariables since loop closures come after odometry
             if(step == 1)
-              newVariables.insert(measurement->key1(), measurement->measured().inverse());
+              newVariables.insert(factor->key1(), measured.inverse());
             else {
-              Pose prevPose = isam2.calculateEstimate<Pose>(measurement->key2());
+              Pose prevPose = isam2.calculateEstimate<Pose>(factor->key2());
-              newVariables.insert(measurement->key1(), prevPose * measurement->measured().inverse());
+              newVariables.insert(factor->key1(), prevPose * measured.inverse());
             }
           }
         } else {
-          if(!newVariables.exists(measurement->key2())) { // Only need to check newVariables since loop closures come after odometry
+          if(!newVariables.exists(factor->key2())) { // Only need to check newVariables since loop closures come after odometry
             if(step == 1)
-              newVariables.insert(measurement->key2(), measurement->measured());
+              newVariables.insert(factor->key2(), measured);
             else {
-              Pose prevPose = isam2.calculateEstimate<Pose>(measurement->key1());
+              Pose prevPose = isam2.calculateEstimate<Pose>(factor->key1());
-              newVariables.insert(measurement->key2(), prevPose * measurement->measured());
+              newVariables.insert(factor->key2(), prevPose * measured);
             }
           }
         }
       }
-      else if(BearingRangeFactor<Pose, Point2>::shared_ptr measurement =
+      else if(BearingRangeFactor<Pose, Point2>::shared_ptr factor =
         boost::dynamic_pointer_cast<BearingRangeFactor<Pose, Point2> >(measurementf))
       {
-        Key poseKey = measurement->keys()[0], lmKey = measurement->keys()[1];
+        Key poseKey = factor->keys()[0], lmKey = factor->keys()[1];
 
         // Stop collecting measurements that are for future steps
         if(poseKey > step)
           throw runtime_error("Problem in data file, out-of-sequence measurements");
 
         // Add new factor
-        newFactors.push_back(measurement);
+        newFactors.push_back(factor);
 
         // Initialize new landmark
         if(!isam2.getLinearizationPoint().exists(lmKey))
@@ -357,8 +358,9 @@ void runIncremental()
             pose = isam2.calculateEstimate<Pose>(poseKey);
           else
             pose = newVariables.at<Pose>(poseKey);
-          Rot2 measuredBearing = measurement->measured().first;
+          const auto& measured = factor->measured();
-          double measuredRange = measurement->measured().second;
+          Rot2 measuredBearing = measured.bearing();
+          double measuredRange = measured.range();
           newVariables.insert(lmKey, 
             pose.transform_from(measuredBearing.rotate(Point2(measuredRange, 0.0))));
         }

gtsam/3rdparty/Eigen/Eigen/src/Core/products/GeneralMatrixMatrixTriangular_BLAS.h

@@ -52,7 +52,7 @@ struct general_matrix_matrix_triangular_product<Index,Scalar,LhsStorageOrder,Con
   static EIGEN_STRONG_INLINE void run(Index size, Index depth,const Scalar* lhs, Index lhsStride, \
                           const Scalar* rhs, Index rhsStride, Scalar* res, Index resStride, Scalar alpha, level3_blocking<Scalar, Scalar>& blocking) \
   { \
-    if ( lhs==rhs && ((UpLo&(Lower|Upper)==UpLo)) ) { \
+    if ( lhs==rhs && ((UpLo&(Lower|Upper))==UpLo) ) { \
       general_matrix_matrix_rankupdate<Index,Scalar,LhsStorageOrder,ConjugateLhs,ColMajor,UpLo> \
       ::run(size,depth,lhs,lhsStride,rhs,rhsStride,res,resStride,alpha,blocking); \
     } else { \

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/base/Manifold.h

@@ -168,9 +168,9 @@ struct FixedDimension {
       "FixedDimension instantiated for dymanically-sized type.");
 };
 
+#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4
 /// Helper class to construct the product manifold of two other manifolds, M1 and M2
-/// Assumes nothing except manifold structure for M1 and M2, and the existence
+/// Deprecated because of limited usefulness, maximum obfuscation 
-/// of default constructor for those types
 template<typename M1, typename M2>
 class ProductManifold: public std::pair<M1, M2> {
   BOOST_CONCEPT_ASSERT((IsManifold<M1>));
@@ -209,12 +209,19 @@ public:
     v << v1, v2;
     return v;
   }
+
+  // Alignment, see https://eigen.tuxfamily.org/dox/group__TopicStructHavingEigenMembers.html
+  enum { NeedsToAlign = (sizeof(M1) % 16) == 0 || (sizeof(M2) % 16) == 0
+  };
+public:
+	EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign)
 };
 
 // Define any direct product group to be a model of the multiplicative Group concept
 template<typename M1, typename M2>
 struct traits<ProductManifold<M1, M2> > : internal::Manifold<ProductManifold<M1, M2> > {
 };
+#endif
 
 } // \ namespace gtsam
 

gtsam/geometry/BearingRange.h

@@ -45,23 +45,39 @@ struct Range;
  * and BearingRange<Pose3,Point3>(pose,point) will return pair<Unit3,double>
  */
 template <typename A1, typename A2>
-struct BearingRange
+struct BearingRange {
-    : public ProductManifold<typename Bearing<A1, A2>::result_type,
+private:
-                             typename Range<A1, A2>::result_type> {
   typedef typename Bearing<A1, A2>::result_type B;
   typedef typename Range<A1, A2>::result_type R;
-  typedef ProductManifold<B, R> Base;
+  B bearing_;
+  R range_;
+
+public:
+  enum { dimB = traits<B>::dimension };
+  enum { dimR = traits<R>::dimension };
+  enum { dimension = dimB + dimR };
+
+  /// @name Standard Constructors
+  /// @{
 
   BearingRange() {}
-  BearingRange(const ProductManifold<B, R>& br) : Base(br) {}
+  BearingRange(const B& b, const R& r) : bearing_(b), range_(r) {}
-  BearingRange(const B& b, const R& r) : Base(b, r) {}
+
+  /// @}
+  /// @name Standard Interface
+  /// @{
+
+  /// Return bearing measurement
+  const B& bearing() const { return bearing_; }
+
+  /// Return range measurement
+  const R& range() const { return range_; }
 
   /// Prediction function that stacks measurements
   static BearingRange Measure(
-      const A1& a1, const A2& a2,
+    const A1& a1, const A2& a2,
-      OptionalJacobian<Base::dimension, traits<A1>::dimension> H1 = boost::none,
+    OptionalJacobian<dimension, traits<A1>::dimension> H1 = boost::none,
-      OptionalJacobian<Base::dimension, traits<A2>::dimension> H2 =
+    OptionalJacobian<dimension, traits<A2>::dimension> H2 = boost::none) {
-          boost::none) {
     typename MakeJacobian<B, A1>::type HB1;
     typename MakeJacobian<B, A2>::type HB2;
     typename MakeJacobian<R, A1>::type HR1;
@@ -75,32 +91,75 @@ struct BearingRange
     return BearingRange(b, r);
   }
 
+  /// @}
+  /// @name Testable
+  /// @{
+
   void print(const std::string& str = "") const {
     std::cout << str;
-    traits<B>::Print(this->first, "bearing ");
+    traits<B>::Print(bearing_, "bearing ");
-    traits<R>::Print(this->second, "range ");
+    traits<R>::Print(range_, "range ");
   }
   bool equals(const BearingRange<A1, A2>& m2, double tol = 1e-8) const {
-    return traits<B>::Equals(this->first, m2.first, tol) &&
+    return traits<B>::Equals(bearing_, m2.bearing_, tol) &&
-           traits<R>::Equals(this->second, m2.second, tol);
+      traits<R>::Equals(range_, m2.range_, tol);
   }
 
- private:
+  /// @}
+  /// @name Manifold
+  /// @{
+
+  inline static size_t Dim() { return dimension; }
+  inline size_t dim() const { return dimension; }
+
+  typedef Eigen::Matrix<double, dimension, 1> TangentVector;
+  typedef OptionalJacobian<dimension, dimension> ChartJacobian;
+
+  /// Retract delta to manifold
+  BearingRange retract(const TangentVector& xi) const {
+    B m1 = traits<B>::Retract(bearing_, xi.template head<dimB>());
+    R m2 = traits<R>::Retract(range_, xi.template tail<dimR>());
+    return BearingRange(m1, m2);
+  }
+
+  /// Compute the coordinates in the tangent space
+  TangentVector localCoordinates(const BearingRange& other) const {
+    typename traits<B>::TangentVector v1 = traits<B>::Local(bearing_, other.bearing_);
+    typename traits<R>::TangentVector v2 = traits<R>::Local(range_, other.range_);
+    TangentVector v;
+    v << v1, v2;
+    return v;
+  }
+
+  /// @}
+  /// @name Advanced Interface
+  /// @{
+
+private:
   /// Serialization function
   template <class ARCHIVE>
   void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
-    ar& boost::serialization::make_nvp("bearing", this->first);
+    ar& boost::serialization::make_nvp("bearing", bearing_);
-    ar& boost::serialization::make_nvp("range", this->second);
+    ar& boost::serialization::make_nvp("range", range_);
   }
 
   friend class boost::serialization::access;
+
+  /// @}
+
+  // Alignment, see https://eigen.tuxfamily.org/dox/group__TopicStructHavingEigenMembers.html
+  enum {
+    NeedsToAlign = (sizeof(B) % 16) == 0 || (sizeof(R) % 16) == 0
+  };
+public:
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign)
 };
 
 // Declare this to be both Testable and a Manifold
 template <typename A1, typename A2>
 struct traits<BearingRange<A1, A2> >
-    : Testable<BearingRange<A1, A2> >,
+  : Testable<BearingRange<A1, A2> >,
-      internal::ManifoldTraits<BearingRange<A1, A2> > {};
+  internal::ManifoldTraits<BearingRange<A1, A2> > {};
 
 // Helper class for to implement Range traits for classes with a bearing method
 // For example, to specialize Bearing to Pose3 and Point3, using Pose3::bearing, it suffices to say

gtsam/geometry/CalibratedCamera.h

@@ -229,10 +229,6 @@ private:
   void serialize(Archive & ar, const unsigned int /*version*/) {
     ar & BOOST_SERIALIZATION_NVP(pose_);
   }
-
-public:
-  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
-
 };
 // end of class PinholeBase
 
@@ -416,9 +412,6 @@ private:
   }
 
   /// @}
-
-public:
-  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 };
 
 // manifold traits

gtsam/geometry/EssentialMatrix.h

@@ -11,7 +11,9 @@
 #include <gtsam/geometry/Unit3.h>
 #include <gtsam/geometry/Point2.h>
 #include <gtsam/base/Manifold.h>
+
 #include <iosfwd>
+#include <string>
 
 namespace gtsam {
 
@@ -21,19 +23,13 @@ namespace gtsam {
  * but here we choose instead to parameterize it as a (Rot3,Unit3) pair.
  * We can then non-linearly optimize immediately on this 5-dimensional manifold.
  */
-class GTSAM_EXPORT EssentialMatrix : private ProductManifold<Rot3, Unit3> {
+class GTSAM_EXPORT EssentialMatrix {
-
+ private:
-private:
+  Rot3 R_;     ///< Rotation
-  typedef ProductManifold<Rot3, Unit3> Base;
+  Unit3 t_;    ///< Translation
-  Matrix3 E_; ///< Essential matrix
+  Matrix3 E_;  ///< Essential matrix
-
-  /// Construct from Base
-  EssentialMatrix(const Base& base) :
-      Base(base), E_(direction().skew() * rotation().matrix()) {
-  }
-
-public:
 
+ public:
   /// Static function to convert Point2 to homogeneous coordinates
   static Vector3 Homogeneous(const Point2& p) {
     return Vector3(p.x(), p.y(), 1);
@@ -43,13 +39,12 @@ public:
   /// @{
 
   /// Default constructor
-  EssentialMatrix() :
+  EssentialMatrix() :E_(t_.skew()) {
-      Base(Rot3(), Unit3(1, 0, 0)), E_(direction().skew()) {
   }
 
   /// Construct from rotation and translation
   EssentialMatrix(const Rot3& aRb, const Unit3& aTb) :
-      Base(aRb, aTb), E_(direction().skew() * rotation().matrix()) {
+      R_(aRb), t_(aTb), E_(t_.skew() * R_.matrix()) {
   }
 
   /// Named constructor with derivatives
@@ -79,27 +74,32 @@ public:
 
   /// assert equality up to a tolerance
   bool equals(const EssentialMatrix& other, double tol = 1e-8) const {
-    return rotation().equals(other.rotation(), tol)
+    return R_.equals(other.R_, tol)
-        && direction().equals(other.direction(), tol);
+        && t_.equals(other.t_, tol);
   }
 
   /// @}
 
   /// @name Manifold
   /// @{
+  enum { dimension = 5 };
+  inline static size_t Dim() { return dimension;}
+  inline size_t dim() const { return dimension;}
 
-  using Base::dimension;
+  typedef OptionalJacobian<dimension, dimension> ChartJacobian;
-  using Base::dim;
-  using Base::Dim;
 
   /// Retract delta to manifold
-  EssentialMatrix retract(const TangentVector& v) const {
+  EssentialMatrix retract(const Vector5& xi) const {
-    return Base::retract(v);
+    return EssentialMatrix(R_.retract(xi.head<3>()), t_.retract(xi.tail<2>()));
   }
 
   /// Compute the coordinates in the tangent space
-  TangentVector localCoordinates(const EssentialMatrix& other) const {
+  Vector5 localCoordinates(const EssentialMatrix& other) const {
-    return Base::localCoordinates(other);
+    auto v1 = R_.localCoordinates(other.R_);
+    auto v2 = t_.localCoordinates(other.t_);
+    Vector5 v;
+    v << v1, v2;
+    return v;
   }
   /// @}
 
@@ -108,12 +108,12 @@ public:
 
   /// Rotation
   inline const Rot3& rotation() const {
-    return this->first;
+    return R_;
   }
 
   /// Direction
   inline const Unit3& direction() const {
-    return this->second;
+    return t_;
   }
 
   /// Return 3*3 matrix representation
@@ -123,12 +123,12 @@ public:
 
   /// Return epipole in image_a , as Unit3 to allow for infinity
   inline const Unit3& epipole_a() const {
-    return direction();
+    return t_;
   }
 
   /// Return epipole in image_b, as Unit3 to allow for infinity
   inline Unit3 epipole_b() const {
-    return rotation().unrotate(direction());
+    return R_.unrotate(t_);
   }
 
   /**
@@ -139,8 +139,8 @@ public:
    * @return point in pose coordinates
    */
   Point3 transform_to(const Point3& p,
-      OptionalJacobian<3,5> DE = boost::none,
+      OptionalJacobian<3, 5> DE = boost::none,
-      OptionalJacobian<3,3> Dpoint = boost::none) const;
+      OptionalJacobian<3, 3> Dpoint = boost::none) const;
 
   /**
    * Given essential matrix E in camera frame B, convert to body frame C
@@ -160,8 +160,8 @@ public:
   }
 
   /// epipolar error, algebraic
-  double error(const Vector3& vA, const Vector3& vB, //
+  double error(const Vector3& vA, const Vector3& vB,
-      OptionalJacobian<1,5> H = boost::none) const;
+      OptionalJacobian<1, 5> H = boost::none) const;
 
   /// @}
 
@@ -176,8 +176,7 @@ public:
 
   /// @}
 
-private:
+ private:
-
   /// @name Advanced Interface
   /// @{
 
@@ -185,21 +184,24 @@ private:
   friend class boost::serialization::access;
   template<class ARCHIVE>
     void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
-      ar & BOOST_SERIALIZATION_NVP(first);
+      ar & BOOST_SERIALIZATION_NVP(R_);
-      ar & BOOST_SERIALIZATION_NVP(second);
+      ar & BOOST_SERIALIZATION_NVP(t_);
 
-      ar & boost::serialization::make_nvp("E11", E_(0,0));
+      ar & boost::serialization::make_nvp("E11", E_(0, 0));
-      ar & boost::serialization::make_nvp("E12", E_(0,1));
+      ar & boost::serialization::make_nvp("E12", E_(0, 1));
-      ar & boost::serialization::make_nvp("E13", E_(0,2));
+      ar & boost::serialization::make_nvp("E13", E_(0, 2));
-      ar & boost::serialization::make_nvp("E21", E_(1,0));
+      ar & boost::serialization::make_nvp("E21", E_(1, 0));
-      ar & boost::serialization::make_nvp("E22", E_(1,1));
+      ar & boost::serialization::make_nvp("E22", E_(1, 1));
-      ar & boost::serialization::make_nvp("E23", E_(1,2));
+      ar & boost::serialization::make_nvp("E23", E_(1, 2));
-      ar & boost::serialization::make_nvp("E31", E_(2,0));
+      ar & boost::serialization::make_nvp("E31", E_(2, 0));
-      ar & boost::serialization::make_nvp("E32", E_(2,1));
+      ar & boost::serialization::make_nvp("E32", E_(2, 1));
-      ar & boost::serialization::make_nvp("E33", E_(2,2));
+      ar & boost::serialization::make_nvp("E33", E_(2, 2));
     }
 
   /// @}
+
+ public:
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 };
 
 template<>
@@ -208,5 +210,5 @@ struct traits<EssentialMatrix> : public internal::Manifold<EssentialMatrix> {};
 template<>
 struct traits<const EssentialMatrix> : public internal::Manifold<EssentialMatrix> {};
 
-} // gtsam
+}  // namespace gtsam
 

gtsam/geometry/Pose2.h

@@ -278,6 +278,12 @@ private:
     ar & BOOST_SERIALIZATION_NVP(t_);
     ar & BOOST_SERIALIZATION_NVP(r_);
   }
+
+#ifdef GTSAM_TYPEDEF_POINTS_TO_VECTORS
+public:
+  // Make sure Pose2 is aligned if it contains a Vector2
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+#endif
 }; // Pose2
 
 /** specialization for pose2 wedge function (generic template in Lie.h) */

gtsam/geometry/Pose3.h

@@ -327,9 +327,11 @@ public:
   }
   /// @}
 
- public:
+#ifdef GTSAM_USE_QUATERNIONS
-  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+  // Align if we are using Quaternions
-
+  public:
+    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+#endif
 };
 // Pose3 class
 

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/geometry/Rot3.h

@@ -508,9 +508,11 @@ namespace gtsam {
 #endif
     }
 
-   public:
+#ifdef GTSAM_USE_QUATERNIONS
+  // only align if quaternion, Matrix3 has no alignment requirements
+  public:
     EIGEN_MAKE_ALIGNED_OPERATOR_NEW
-
+#endif
   };
 
   /**

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/AttitudeFactor.h

@@ -129,6 +129,9 @@ private:
     ar & BOOST_SERIALIZATION_NVP(nZ_);
     ar & BOOST_SERIALIZATION_NVP(bRef_);
   }
+
+public:
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 };
 
 /// traits
@@ -213,6 +216,9 @@ private:
     ar & BOOST_SERIALIZATION_NVP(nZ_);
     ar & BOOST_SERIALIZATION_NVP(bRef_);
   }
+
+public:
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 };
 
 /// traits

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/navigation/tests/testGPSFactor.cpp

@@ -22,19 +22,26 @@
 
 #include <CppUnitLite/TestHarness.h>
 
+#include <GeographicLib/Config.h>
 #include <GeographicLib/LocalCartesian.hpp>
 
 using namespace std;
 using namespace gtsam;
 using namespace GeographicLib;
 
+#if GEOGRAPHICLIB_VERSION_MINOR<37
+static const auto& kWGS84 = Geocentric::WGS84;
+#else
+static const auto& kWGS84 = Geocentric::WGS84();
+#endif
+
 // *************************************************************************
 namespace example {
 // ENU Origin is where the plane was in hold next to runway
 const double lat0 = 33.86998, lon0 = -84.30626, h0 = 274;
 
 // Convert from GPS to ENU
-LocalCartesian origin_ENU(lat0, lon0, h0, Geocentric::WGS84);
+LocalCartesian origin_ENU(lat0, lon0, h0, kWGS84);
 
 // Dekalb-Peachtree Airport runway 2L
 const double lat = 33.87071, lon = -84.30482, h = 274;
@@ -107,8 +114,7 @@ TEST(GPSData, init) {
   // GPS Reading 1 will be ENU origin
   double t1 = 84831;
   Point3 NED1(0, 0, 0);
-  LocalCartesian enu(35.4393283333333, -119.062986666667, 275.54,
+  LocalCartesian enu(35.4393283333333, -119.062986666667, 275.54, kWGS84);
-      Geocentric::WGS84);
 
   // GPS Readin 2
   double t2 = 84831.5;

gtsam/navigation/tests/testGeographicLib.cpp

@@ -15,6 +15,7 @@
  * @author  Frank Dellaert
  */
 
+#include <GeographicLib/Config.h>
 #include <GeographicLib/Geocentric.hpp>
 #include <GeographicLib/UTMUPS.hpp>
 #include <GeographicLib/LocalCartesian.hpp>
@@ -29,21 +30,27 @@ using namespace std;
 using namespace GeographicLib;
 
 // Dekalb-Peachtree Airport runway 2L
-const double lat = 33.87071, lon = -84.30482, h = 274;
+static const double lat = 33.87071, lon = -84.30482, h = 274;
+
+#if GEOGRAPHICLIB_VERSION_MINOR<37
+static const auto& kWGS84 = Geocentric::WGS84;
+#else
+static const auto& kWGS84 = Geocentric::WGS84();
+#endif
 
 //**************************************************************************
 TEST( GeographicLib, Geocentric) {
 
   // From lat-lon to geocentric
   double X, Y, Z;
-  Geocentric::WGS84.Forward(lat, lon, h, X, Y, Z);
+  kWGS84.Forward(lat, lon, h, X, Y, Z);
   EXPECT_DOUBLES_EQUAL(526, X/1000, 1);
   EXPECT_DOUBLES_EQUAL(-5275, Y/1000, 1);
   EXPECT_DOUBLES_EQUAL(3535, Z/1000, 1);
 
   // From geocentric to lat-lon
   double lat_, lon_, h_;
-  Geocentric::WGS84.Reverse(X, Y, Z, lat_, lon_, h_);
+  kWGS84.Reverse(X, Y, Z, lat_, lon_, h_);
   EXPECT_DOUBLES_EQUAL(lat, lat_, 1e-5);
   EXPECT_DOUBLES_EQUAL(lon, lon_, 1e-5);
   EXPECT_DOUBLES_EQUAL(h, h_, 1e-5);
@@ -69,11 +76,9 @@ TEST( GeographicLib, UTM) {
 //**************************************************************************
 TEST( GeographicLib, ENU) {
 
-  const Geocentric& earth = Geocentric::WGS84;
-
   // ENU Origin is where the plane was in hold next to runway
   const double lat0 = 33.86998, lon0 = -84.30626, h0 = 274;
-  LocalCartesian enu(lat0, lon0, h0, earth);
+  LocalCartesian enu(lat0, lon0, h0, kWGS84);
 
   // From lat-lon to geocentric
   double E, N, U;

gtsam/nonlinear/Expression-inl.h

@@ -200,7 +200,7 @@ T Expression<T>::valueAndJacobianMap(const Values& values,
   // allocated on Visual Studio. For more information see the issue below
   // https://bitbucket.org/gtborg/gtsam/issue/178/vlas-unsupported-in-visual-studio
 #ifdef _MSC_VER
-  internal::ExecutionTraceStorage* traceStorage = new internal::ExecutionTraceStorage[size];
+  auto traceStorage = static_cast<internal::ExecutionTraceStorage*>(_aligned_malloc(size, internal::TraceAlignment));
 #else
   internal::ExecutionTraceStorage traceStorage[size];
 #endif
@@ -210,7 +210,7 @@ T Expression<T>::valueAndJacobianMap(const Values& values,
   trace.startReverseAD1(jacobians);
 
 #ifdef _MSC_VER
-  delete[] traceStorage;
+  _aligned_free(traceStorage);
 #endif
 
   return value;

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/nonlinear/ExpressionFactorGraph.h

@@ -42,7 +42,8 @@ public:
   template<typename T>
   void addExpressionFactor(const Expression<T>& h, const T& z,
       const SharedNoiseModel& R) {
-    push_back(boost::make_shared<ExpressionFactor<T> >(R, z, h));
+    using F = ExpressionFactor<T>;
+    push_back(boost::allocate_shared<F>(Eigen::aligned_allocator<F>(), R, z, h));
   }
 
   /// @}

gtsam/slam/BetweenFactor.h

@@ -122,6 +122,11 @@ namespace gtsam {
           boost::serialization::base_object<Base>(*this));
       ar & BOOST_SERIALIZATION_NVP(measured_);
     }
+  
+	  // Alignment, see https://eigen.tuxfamily.org/dox/group__TopicStructHavingEigenMembers.html
+	  enum { NeedsToAlign = (sizeof(VALUE) % 16) == 0 };
+    public:
+      EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign)
   }; // \class BetweenFactor
 
   /// traits

gtsam/slam/EssentialMatrixConstraint.h

@@ -103,6 +103,9 @@ private:
             boost::serialization::base_object<Base>(*this));
     ar & BOOST_SERIALIZATION_NVP(measuredE_);
   }
+
+public:
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 };
 // \class EssentialMatrixConstraint
 

gtsam/slam/PriorFactor.h

@@ -104,6 +104,11 @@ namespace gtsam {
           boost::serialization::base_object<Base>(*this));
       ar & BOOST_SERIALIZATION_NVP(prior_);
     }
+  
+	// Alignment, see https://eigen.tuxfamily.org/dox/group__TopicStructHavingEigenMembers.html
+	enum { NeedsToAlign = (sizeof(T) % 16) == 0 };
+  public:
+	EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign)
   };
 
 } /// namespace gtsam

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>