From c58a78b60a8b15a73fa4b72020166c8a31b65cc8 Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@cc.gatech.edu>
Date: Tue, 6 Nov 2018 00:29:38 -0500
Subject: [PATCH] More aligned new operators

---
 gtsam/navigation/CombinedImuFactor.h     |  3 +++
 gtsam/navigation/PreintegratedRotation.h | 12 +++++++++---
 gtsam/navigation/PreintegrationBase.h    |  3 ++-
 gtsam/navigation/PreintegrationParams.h  |  5 ++++-
 gtsam/nonlinear/ExpressionFactor.h       |  5 +++++
 gtsam/slam/ProjectionFactor.h            |  5 ++++-
 6 files changed, 27 insertions(+), 6 deletions(-)

diff --git a/gtsam/navigation/CombinedImuFactor.h b/gtsam/navigation/CombinedImuFactor.h
index 6fd98bfcb..7ca7fe463 100644
--- a/gtsam/navigation/CombinedImuFactor.h
+++ b/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
 
diff --git a/gtsam/navigation/PreintegratedRotation.h b/gtsam/navigation/PreintegratedRotation.h
index 54320417d..bf2f5c0c8 100644
--- a/gtsam/navigation/PreintegratedRotation.h
+++ b/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:
-  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+#ifdef GTSAM_USE_QUATERNIONS
+  // Align if we are using Quaternions
+  public:
+	  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+#endif
 };
 
 template <>
diff --git a/gtsam/navigation/PreintegrationBase.h b/gtsam/navigation/PreintegrationBase.h
index cf5465c05..3c22a1d00 100644
--- a/gtsam/navigation/PreintegrationBase.h
+++ b/gtsam/navigation/PreintegrationBase.h
@@ -202,7 +202,8 @@ public:
   /// @}
 #endif
 
-  /// @}
+public:
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 };
 
 } /// namespace gtsam
diff --git a/gtsam/navigation/PreintegrationParams.h b/gtsam/navigation/PreintegrationParams.h
index 7bff82365..3a2fb467a 100644
--- a/gtsam/navigation/PreintegrationParams.h
+++ b/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
diff --git a/gtsam/nonlinear/ExpressionFactor.h b/gtsam/nonlinear/ExpressionFactor.h
index 64a8a6bb6..34ba8e1ff 100644
--- a/gtsam/nonlinear/ExpressionFactor.h
+++ b/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
 
diff --git a/gtsam/slam/ProjectionFactor.h b/gtsam/slam/ProjectionFactor.h
index d13c28e11..8332acabc 100644
--- a/gtsam/slam/ProjectionFactor.h
+++ b/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>