Fixed all alignment problems

gtsam.h

@@ -281,7 +281,7 @@ virtual class Value {
 };
 
 #include <gtsam/base/GenericValue.h>
-template<T = {Vector, gtsam::Point2, gtsam::Point3, gtsam::Rot2, gtsam::Rot3, gtsam::Pose2, gtsam::Pose3, gtsam::StereoPoint2, gtsam::Cal3_S2, gtsam::CalibratedCamera, gtsam::SimpleCamera, gtsam::imuBias::ConstantBias}>
+template<T = {Vector, Matrix, gtsam::Point2, gtsam::Point3, gtsam::Rot2, gtsam::Rot3, gtsam::Pose2, gtsam::Pose3, gtsam::StereoPoint2, gtsam::Cal3_S2, gtsam::Cal3DS2, gtsam::Cal3Bundler, gtsam::EssentialMatrix, gtsam::CalibratedCamera, gtsam::SimpleCamera, gtsam::imuBias::ConstantBias}>
 virtual class GenericValue : gtsam::Value {
   void serializable() const;
 };

gtsam/base/make_shared.h

@@ -0,0 +1,44 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2020, Georgia Tech Research Corporation,
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ * @file make_shared.h
+ * @brief make_shared trampoline function to ensure proper alignment
+ * @author Fan Jiang
+ */
+
+#pragma once
+
+#include <boost/make_shared.hpp>
+#include <gtsam/base/types.h>
+
+#include <type_traits>
+
+#include <Eigen/Core>
+
+namespace gtsam {
+
+  /**
+   * And our own `make_shared` as a layer of wrapping on `boost::make_shared`
+   */
+  template<typename T, typename ... Args>
+  boost::enable_if_t<has_custom_allocator<T>::value, boost::shared_ptr<T>> make_shared(Args&&... args)
+  {
+    return boost::allocate_shared<T>(Eigen::aligned_allocator<T>(), std::forward<Args> (args)...);
+  }
+
+  template<typename T, typename ... Args>
+  boost::enable_if_t<!has_custom_allocator<T>::value, boost::shared_ptr<T>> make_shared(Args&&... args)
+  {
+    return boost::make_shared<T>(std::forward<Args> (args)...);
+  }
+
+}

gtsam/base/types.h

@@ -230,3 +230,29 @@ namespace std {
 #ifdef ERROR
 #undef ERROR
 #endif
+
+namespace gtsam {
+
+  /**
+   * A trait to mark classes that need special alignment.
+   * Please refer to https://github.com/PointCloudLibrary/pcl/pull/3163
+   */
+#if __cplusplus < 201703L
+  template<typename ...> using void_t = void;
+#endif
+
+  template<typename, typename = void_t<>>
+  struct has_custom_allocator : std::false_type {
+  };
+  template<typename T>
+  struct has_custom_allocator<T, void_t<typename T::_custom_allocator_type_trait>> : std::true_type {
+  };
+
+}
+
+/**
+ * This is necessary for the Cython wrapper to work properly, and possibly reduce future misalignment problems.
+ */
+#define GTSAM_MAKE_ALIGNED_OPERATOR_NEW \
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW \
+  using _custom_allocator_type_trait = void;

gtsam/geometry/SOn.h

@@ -20,8 +20,8 @@
 
 #include <gtsam/base/Lie.h>
 #include <gtsam/base/Manifold.h>
+#include <gtsam/base/make_shared.h>
 #include <gtsam/dllexport.h>
-
 #include <Eigen/Core>
 
 #include <iostream> // TODO(frank): how to avoid?
@@ -54,7 +54,7 @@ class SO : public LieGroup<SO<N>, internal::DimensionSO(N)> {
   using VectorN2 = Eigen::Matrix<double, internal::NSquaredSO(N), 1>;
   using MatrixDD = Eigen::Matrix<double, dimension, dimension>;
 
-  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+  GTSAM_MAKE_ALIGNED_OPERATOR_NEW
 
  protected:
   MatrixNN matrix_;  ///< Rotation matrix

wrap/Module.cpp

@@ -350,7 +350,10 @@ void Module::emit_cython_pxd(FileWriter& pxdFile) const {
                  "        T* get()\n"
                  "        long use_count() const\n"
                  "        T& operator*()\n\n"
-                 "    cdef shared_ptr[T] dynamic_pointer_cast[T,U](const shared_ptr[U]& r)\n"
+                 "    cdef shared_ptr[T] dynamic_pointer_cast[T,U](const shared_ptr[U]& r)\n\n";
+
+  // gtsam alignment-friendly shared_ptr
+  pxdFile.oss << "cdef extern from \"gtsam/base/make_shared.h\" namespace \"gtsam\":\n"
                  "    cdef shared_ptr[T] make_shared[T](const T& r)\n\n";
 
   for(const TypedefPair& types: typedefs)