From 37673771aec059e14705fad69a0282a9c55e3eba Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Sat, 20 Jun 2020 22:21:17 -0400 Subject: [PATCH 01/10] Fixed all alignment problems --- gtsam.h | 2 +- gtsam/base/make_shared.h | 44 ++++++++++++++++++++++++++++++++++++++++ gtsam/base/types.h | 26 ++++++++++++++++++++++++ gtsam/geometry/SOn.h | 4 ++-- wrap/Module.cpp | 5 ++++- 5 files changed, 77 insertions(+), 4 deletions(-) create mode 100644 gtsam/base/make_shared.h diff --git a/gtsam.h b/gtsam.h index 614db91c7..cf232efa5 100644 --- a/gtsam.h +++ b/gtsam.h @@ -281,7 +281,7 @@ virtual class Value { }; #include -template +template virtual class GenericValue : gtsam::Value { void serializable() const; }; diff --git a/gtsam/base/make_shared.h b/gtsam/base/make_shared.h new file mode 100644 index 000000000..c8b63f5d4 --- /dev/null +++ b/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 +#include + +#include + +#include + +namespace gtsam { + + /** + * And our own `make_shared` as a layer of wrapping on `boost::make_shared` + */ + template + boost::enable_if_t::value, boost::shared_ptr> make_shared(Args&&... args) + { + return boost::allocate_shared(Eigen::aligned_allocator(), std::forward (args)...); + } + + template + boost::enable_if_t::value, boost::shared_ptr> make_shared(Args&&... args) + { + return boost::make_shared(std::forward (args)...); + } + +} \ No newline at end of file diff --git a/gtsam/base/types.h b/gtsam/base/types.h index 2fa6eebb6..3abbdfe81 100644 --- a/gtsam/base/types.h +++ b/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 using void_t = void; +#endif + + template> + struct has_custom_allocator : std::false_type { + }; + template + struct has_custom_allocator> : 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; \ No newline at end of file diff --git a/gtsam/geometry/SOn.h b/gtsam/geometry/SOn.h index 5313d3018..767b12020 100644 --- a/gtsam/geometry/SOn.h +++ b/gtsam/geometry/SOn.h @@ -20,8 +20,8 @@ #include #include +#include #include - #include #include // TODO(frank): how to avoid? @@ -54,7 +54,7 @@ class SO : public LieGroup, internal::DimensionSO(N)> { using VectorN2 = Eigen::Matrix; using MatrixDD = Eigen::Matrix; - EIGEN_MAKE_ALIGNED_OPERATOR_NEW + GTSAM_MAKE_ALIGNED_OPERATOR_NEW protected: MatrixNN matrix_; ///< Rotation matrix diff --git a/wrap/Module.cpp b/wrap/Module.cpp index a7db9e1f6..ec02dc412 100644 --- a/wrap/Module.cpp +++ b/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) From fad4fe39f28797f11a61461f3f0224f173046b2e Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Sat, 20 Jun 2020 22:48:40 -0400 Subject: [PATCH 02/10] Add missing include --- gtsam/base/make_shared.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/base/make_shared.h b/gtsam/base/make_shared.h index c8b63f5d4..c5ac98f9c 100644 --- a/gtsam/base/make_shared.h +++ b/gtsam/base/make_shared.h @@ -20,7 +20,7 @@ #include #include -#include +#include #include From 693253f3768bbd618a688a8c173b6f1bd576fdec Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Sat, 20 Jun 2020 22:52:58 -0400 Subject: [PATCH 03/10] Fix wrap tests --- wrap/tests/expected-cython/geometry.pxd | 2 ++ 1 file changed, 2 insertions(+) diff --git a/wrap/tests/expected-cython/geometry.pxd b/wrap/tests/expected-cython/geometry.pxd index 0d9adac5f..3527840a3 100644 --- a/wrap/tests/expected-cython/geometry.pxd +++ b/wrap/tests/expected-cython/geometry.pxd @@ -16,6 +16,8 @@ cdef extern from "boost/shared_ptr.hpp" namespace "boost": T& operator*() cdef shared_ptr[T] dynamic_pointer_cast[T,U](const shared_ptr[U]& r) + +cdef extern from "gtsam/base/make_shared.h" namespace "gtsam": cdef shared_ptr[T] make_shared[T](const T& r) cdef extern from "gtsam/geometry/Point2.h" namespace "gtsam": From eab53f69de01ac06b9d2d20821539b4be4fade21 Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Mon, 22 Jun 2020 13:37:50 -0400 Subject: [PATCH 04/10] Address Frank's comments --- gtsam/base/make_shared.h | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/gtsam/base/make_shared.h b/gtsam/base/make_shared.h index c5ac98f9c..03de30497 100644 --- a/gtsam/base/make_shared.h +++ b/gtsam/base/make_shared.h @@ -17,28 +17,28 @@ #pragma once -#include #include -#include - #include +#include +#include + + namespace gtsam { - /** - * And our own `make_shared` as a layer of wrapping on `boost::make_shared` - */ + /// Add our own `make_shared` as a layer of wrapping on `boost::make_shared` template boost::enable_if_t::value, boost::shared_ptr> make_shared(Args&&... args) { return boost::allocate_shared(Eigen::aligned_allocator(), std::forward (args)...); } + /// Fall back to the boost version if no need for alignment template boost::enable_if_t::value, boost::shared_ptr> make_shared(Args&&... args) { return boost::make_shared(std::forward (args)...); } -} \ No newline at end of file +} From 8f923fa081f0702cb36681675833476c17f692ce Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Mon, 22 Jun 2020 15:18:31 -0400 Subject: [PATCH 05/10] Move away from boost --- gtsam/base/make_shared.h | 13 ++++++++++--- 1 file changed, 10 insertions(+), 3 deletions(-) diff --git a/gtsam/base/make_shared.h b/gtsam/base/make_shared.h index 03de30497..966dd516d 100644 --- a/gtsam/base/make_shared.h +++ b/gtsam/base/make_shared.h @@ -22,21 +22,28 @@ #include #include -#include +#include + +#if __cplusplus <= 201103L +namespace gtsam { + template + using enable_if_t = typename std::enable_if::type; +} +#endif namespace gtsam { /// Add our own `make_shared` as a layer of wrapping on `boost::make_shared` template - boost::enable_if_t::value, boost::shared_ptr> make_shared(Args&&... args) + gtsam::enable_if_t::value, boost::shared_ptr> make_shared(Args&&... args) { return boost::allocate_shared(Eigen::aligned_allocator(), std::forward (args)...); } /// Fall back to the boost version if no need for alignment template - boost::enable_if_t::value, boost::shared_ptr> make_shared(Args&&... args) + gtsam::enable_if_t::value, boost::shared_ptr> make_shared(Args&&... args) { return boost::make_shared(std::forward (args)...); } From b41809203f636878dbbc0aad718ac6f7343e82e6 Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Tue, 23 Jun 2020 18:37:12 -0400 Subject: [PATCH 06/10] Revise comments --- gtsam/base/types.h | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/gtsam/base/types.h b/gtsam/base/types.h index 3abbdfe81..93c475c40 100644 --- a/gtsam/base/types.h +++ b/gtsam/base/types.h @@ -235,7 +235,8 @@ namespace gtsam { /** * A trait to mark classes that need special alignment. - * Please refer to https://github.com/PointCloudLibrary/pcl/pull/3163 + * This is required to make boost::make_shared and etc respect alignment, which is essential for the Python + * wrappers to work properly. */ #if __cplusplus < 201703L template using void_t = void; From 7d7475b881ffc9f21ce2f49c3e4d3890e9e6d21f Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Wed, 24 Jun 2020 14:15:46 -0400 Subject: [PATCH 07/10] Style fixes as commented by @dellaert --- gtsam/base/make_shared.h | 22 +++++++++++++--------- gtsam/base/types.h | 19 ++++++++++--------- 2 files changed, 23 insertions(+), 18 deletions(-) diff --git a/gtsam/base/make_shared.h b/gtsam/base/make_shared.h index 966dd516d..c7d98cdee 100644 --- a/gtsam/base/make_shared.h +++ b/gtsam/base/make_shared.h @@ -25,27 +25,31 @@ #include -#if __cplusplus <= 201103L namespace gtsam { template using enable_if_t = typename std::enable_if::type; } -#endif namespace gtsam { - /// Add our own `make_shared` as a layer of wrapping on `boost::make_shared` + /** Add our own `make_shared` as a layer of wrapping on `boost::make_shared` + * This solves the problem with the stock `make_shared` that custom alignment is not respected, causing SEGFAULTs + * at runtime, which is notoriously hard to debug. + * + * @tparam T The type of object being constructed + * @tparam Args Type of the arguments of the constructor + * @param args Arguments of the constructor + * @return The object created as a boost::shared_ptr + */ template - gtsam::enable_if_t::value, boost::shared_ptr> make_shared(Args&&... args) - { - return boost::allocate_shared(Eigen::aligned_allocator(), std::forward (args)...); + gtsam::enable_if_t::value, boost::shared_ptr> make_shared(Args &&... args) { + return boost::allocate_shared(Eigen::aligned_allocator(), std::forward(args)...); } /// Fall back to the boost version if no need for alignment template - gtsam::enable_if_t::value, boost::shared_ptr> make_shared(Args&&... args) - { - return boost::make_shared(std::forward (args)...); + gtsam::enable_if_t::value, boost::shared_ptr> make_shared(Args &&... args) { + return boost::make_shared(std::forward(args)...); } } diff --git a/gtsam/base/types.h b/gtsam/base/types.h index 93c475c40..a4b2fb6ea 100644 --- a/gtsam/base/types.h +++ b/gtsam/base/types.h @@ -233,27 +233,28 @@ namespace std { namespace gtsam { + /// Convenience void_t as we assume C++11, it will not conflict the std one in C++17 as this is in `gtsam::` + template using void_t = void; + /** - * A trait to mark classes that need special alignment. + * A SFINAE trait to mark classes that need special alignment. * This is required to make boost::make_shared and etc respect alignment, which is essential for the Python * wrappers to work properly. */ -#if __cplusplus < 201703L - template using void_t = void; -#endif - template> - struct has_custom_allocator : std::false_type { + struct needs_eigen_aligned_allocator : std::false_type { }; template - struct has_custom_allocator> : std::true_type { + struct needs_eigen_aligned_allocator> : std::true_type { }; } /** - * This is necessary for the Cython wrapper to work properly, and possibly reduce future misalignment problems. + * This marks a GTSAM object to require alignment. With this macro an object will automatically be allocated in aligned + * memory when one uses `gtsam::make_shared`. It reduces future misalignment problems that is hard to debug. + * See https://eigen.tuxfamily.org/dox/group__DenseMatrixManipulation__Alignement.html for details */ #define GTSAM_MAKE_ALIGNED_OPERATOR_NEW \ EIGEN_MAKE_ALIGNED_OPERATOR_NEW \ - using _custom_allocator_type_trait = void; \ No newline at end of file + using _eigen_aligned_allocator_trait = void; From 6dbd7c243abc91489c010261bde27685c491acac Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Wed, 24 Jun 2020 16:25:46 -0400 Subject: [PATCH 08/10] Add comments --- gtsam/base/make_shared.h | 14 +++++++++++++- gtsam/base/types.h | 12 +++++++++++- 2 files changed, 24 insertions(+), 2 deletions(-) diff --git a/gtsam/base/make_shared.h b/gtsam/base/make_shared.h index c7d98cdee..5281eec6d 100644 --- a/gtsam/base/make_shared.h +++ b/gtsam/base/make_shared.h @@ -26,16 +26,28 @@ #include namespace gtsam { + /// An shorthand alias for accessing the ::type inside std::enable_if that can be used in a template directly template using enable_if_t = typename std::enable_if::type; } namespace gtsam { - /** Add our own `make_shared` as a layer of wrapping on `boost::make_shared` + /** + * Add our own `make_shared` as a layer of wrapping on `boost::make_shared` * This solves the problem with the stock `make_shared` that custom alignment is not respected, causing SEGFAULTs * at runtime, which is notoriously hard to debug. * + * Explanation + * =============== + * The template `needs_eigen_aligned_allocator::value` will evaluate to `std::true_type` if the type alias + * `_eigen_aligned_allocator_trait = void` is present in a class, which is automatically added by the + * `GTSAM_MAKE_ALIGNED_OPERATOR_NEW` macro. + * + * This function declaration will only be taken when the above condition is true, so if some object does not need to + * be aligned, `gtsam::make_shared` will fall back to the next definition, which is a simple wrapper for + * `boost::make_shared`. + * * @tparam T The type of object being constructed * @tparam Args Type of the arguments of the constructor * @param args Arguments of the constructor diff --git a/gtsam/base/types.h b/gtsam/base/types.h index a4b2fb6ea..b54cc2f2a 100644 --- a/gtsam/base/types.h +++ b/gtsam/base/types.h @@ -238,8 +238,18 @@ namespace gtsam { /** * A SFINAE trait to mark classes that need special alignment. + * * This is required to make boost::make_shared and etc respect alignment, which is essential for the Python * wrappers to work properly. + * + * Explanation + * ============= + * When a GTSAM type is not declared with the type alias `_eigen_aligned_allocator_trait = void`, the first template + * will be taken so `needs_eigen_aligned_allocator` will be resolved to `std::false_type`. + * + * Otherwise, it will resolve to the second template, which will be resolved to `std::true_type`. + * + * Please refer to `gtsam/base/make_shared.h` for an example. */ template> struct needs_eigen_aligned_allocator : std::false_type { @@ -253,7 +263,7 @@ namespace gtsam { /** * This marks a GTSAM object to require alignment. With this macro an object will automatically be allocated in aligned * memory when one uses `gtsam::make_shared`. It reduces future misalignment problems that is hard to debug. - * See https://eigen.tuxfamily.org/dox/group__DenseMatrixManipulation__Alignement.html for details + * See https://eigen.tuxfamily.org/dox/group__DenseMatrixManipulation__Alignement.html for detailed explanation. */ #define GTSAM_MAKE_ALIGNED_OPERATOR_NEW \ EIGEN_MAKE_ALIGNED_OPERATOR_NEW \ From fb21c553a05555ce4be3de305836d62fa1113bbe Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Wed, 24 Jun 2020 17:15:00 -0400 Subject: [PATCH 09/10] Switch to the new alignment marker type --- gtsam/base/GenericValue.h | 2 +- gtsam/base/Manifold.h | 2 +- gtsam/base/types.h | 9 +++++++++ gtsam/geometry/BearingRange.h | 2 +- gtsam/geometry/CameraSet.h | 2 +- gtsam/geometry/EssentialMatrix.h | 2 +- gtsam/geometry/PinholeCamera.h | 2 +- gtsam/geometry/PinholePose.h | 4 ++-- gtsam/geometry/Pose2.h | 2 +- gtsam/geometry/Pose3.h | 2 +- gtsam/geometry/Rot3.h | 2 +- gtsam/geometry/SOn.h | 2 +- gtsam/geometry/Unit3.h | 2 +- gtsam/geometry/triangulation.h | 2 +- gtsam/navigation/AttitudeFactor.h | 4 ++-- gtsam/navigation/CombinedImuFactor.h | 6 +++--- gtsam/navigation/ImuBias.h | 2 +- gtsam/navigation/PreintegratedRotation.h | 4 ++-- gtsam/navigation/PreintegrationBase.h | 2 +- gtsam/navigation/PreintegrationParams.h | 2 +- gtsam/navigation/TangentPreintegration.h | 2 +- gtsam/nonlinear/ExpressionFactor.h | 2 +- gtsam/nonlinear/NonlinearEquality.h | 6 +++--- gtsam/nonlinear/PriorFactor.h | 2 +- gtsam/nonlinear/internal/ExpressionNode.h | 2 +- gtsam/slam/BetweenFactor.h | 2 +- gtsam/slam/EssentialMatrixConstraint.h | 2 +- gtsam/slam/EssentialMatrixFactor.h | 6 +++--- gtsam/slam/ProjectionFactor.h | 2 +- gtsam/slam/RotateFactor.h | 2 +- gtsam/slam/SmartFactorBase.h | 2 +- 31 files changed, 48 insertions(+), 39 deletions(-) diff --git a/gtsam/base/GenericValue.h b/gtsam/base/GenericValue.h index e1cb3bc2c..2ac3eb80c 100644 --- a/gtsam/base/GenericValue.h +++ b/gtsam/base/GenericValue.h @@ -181,7 +181,7 @@ public: // Alignment, see https://eigen.tuxfamily.org/dox/group__TopicStructHavingEigenMembers.html enum { NeedsToAlign = (sizeof(T) % 16) == 0 }; public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign) + GTSAM_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign) }; /// use this macro instead of BOOST_CLASS_EXPORT for GenericValues diff --git a/gtsam/base/Manifold.h b/gtsam/base/Manifold.h index f3653f099..9feb2b451 100644 --- a/gtsam/base/Manifold.h +++ b/gtsam/base/Manifold.h @@ -214,7 +214,7 @@ public: enum { NeedsToAlign = (sizeof(M1) % 16) == 0 || (sizeof(M2) % 16) == 0 }; public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign) + GTSAM_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign) }; // Define any direct product group to be a model of the multiplicative Group concept diff --git a/gtsam/base/types.h b/gtsam/base/types.h index b54cc2f2a..924f339b3 100644 --- a/gtsam/base/types.h +++ b/gtsam/base/types.h @@ -268,3 +268,12 @@ namespace gtsam { #define GTSAM_MAKE_ALIGNED_OPERATOR_NEW \ EIGEN_MAKE_ALIGNED_OPERATOR_NEW \ using _eigen_aligned_allocator_trait = void; + +/** + * This marks a GTSAM object to require alignment. With this macro an object will automatically be allocated in aligned + * memory when one uses `gtsam::make_shared`. It reduces future misalignment problems that is hard to debug. + * See https://eigen.tuxfamily.org/dox/group__DenseMatrixManipulation__Alignement.html for detailed explanation. + */ +#define GTSAM_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign) \ + EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign) \ + using _eigen_aligned_allocator_trait = void; \ No newline at end of file diff --git a/gtsam/geometry/BearingRange.h b/gtsam/geometry/BearingRange.h index 7c73f3cbd..8db7abffe 100644 --- a/gtsam/geometry/BearingRange.h +++ b/gtsam/geometry/BearingRange.h @@ -162,7 +162,7 @@ private: NeedsToAlign = (sizeof(B) % 16) == 0 || (sizeof(R) % 16) == 0 }; public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign) + GTSAM_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign) }; // Declare this to be both Testable and a Manifold diff --git a/gtsam/geometry/CameraSet.h b/gtsam/geometry/CameraSet.h index ecf9a572d..feab8e0fa 100644 --- a/gtsam/geometry/CameraSet.h +++ b/gtsam/geometry/CameraSet.h @@ -319,7 +319,7 @@ private: } public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW + GTSAM_MAKE_ALIGNED_OPERATOR_NEW }; template diff --git a/gtsam/geometry/EssentialMatrix.h b/gtsam/geometry/EssentialMatrix.h index 3235fdedd..ca719eb37 100644 --- a/gtsam/geometry/EssentialMatrix.h +++ b/gtsam/geometry/EssentialMatrix.h @@ -212,7 +212,7 @@ class EssentialMatrix { /// @} public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW + GTSAM_MAKE_ALIGNED_OPERATOR_NEW }; template<> diff --git a/gtsam/geometry/PinholeCamera.h b/gtsam/geometry/PinholeCamera.h index c52127a45..9a80b937b 100644 --- a/gtsam/geometry/PinholeCamera.h +++ b/gtsam/geometry/PinholeCamera.h @@ -325,7 +325,7 @@ private: } public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW + GTSAM_MAKE_ALIGNED_OPERATOR_NEW }; // manifold traits diff --git a/gtsam/geometry/PinholePose.h b/gtsam/geometry/PinholePose.h index 935962423..60088577c 100644 --- a/gtsam/geometry/PinholePose.h +++ b/gtsam/geometry/PinholePose.h @@ -222,7 +222,7 @@ private: } public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW + GTSAM_MAKE_ALIGNED_OPERATOR_NEW }; // end of class PinholeBaseK @@ -425,7 +425,7 @@ private: } public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW + GTSAM_MAKE_ALIGNED_OPERATOR_NEW }; // end of class PinholePose diff --git a/gtsam/geometry/Pose2.h b/gtsam/geometry/Pose2.h index 388318827..2a1f108ca 100644 --- a/gtsam/geometry/Pose2.h +++ b/gtsam/geometry/Pose2.h @@ -317,7 +317,7 @@ public: public: // Align for Point2, which is either derived from, or is typedef, of Vector2 - EIGEN_MAKE_ALIGNED_OPERATOR_NEW + GTSAM_MAKE_ALIGNED_OPERATOR_NEW }; // Pose2 /** specialization for pose2 wedge function (generic template in Lie.h) */ diff --git a/gtsam/geometry/Pose3.h b/gtsam/geometry/Pose3.h index fa55f98de..ced3b904b 100644 --- a/gtsam/geometry/Pose3.h +++ b/gtsam/geometry/Pose3.h @@ -355,7 +355,7 @@ public: #ifdef GTSAM_USE_QUATERNIONS // Align if we are using Quaternions public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW + GTSAM_MAKE_ALIGNED_OPERATOR_NEW #endif }; // Pose3 class diff --git a/gtsam/geometry/Rot3.h b/gtsam/geometry/Rot3.h index fc3a8b3f2..8f24f07c8 100644 --- a/gtsam/geometry/Rot3.h +++ b/gtsam/geometry/Rot3.h @@ -544,7 +544,7 @@ namespace gtsam { #ifdef GTSAM_USE_QUATERNIONS // only align if quaternion, Matrix3 has no alignment requirements public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW + GTSAM_MAKE_ALIGNED_OPERATOR_NEW #endif }; diff --git a/gtsam/geometry/SOn.h b/gtsam/geometry/SOn.h index 767b12020..a08f87783 100644 --- a/gtsam/geometry/SOn.h +++ b/gtsam/geometry/SOn.h @@ -54,7 +54,7 @@ class SO : public LieGroup, internal::DimensionSO(N)> { using VectorN2 = Eigen::Matrix; using MatrixDD = Eigen::Matrix; - GTSAM_MAKE_ALIGNED_OPERATOR_NEW + GTSAM_MAKE_ALIGNED_OPERATOR_NEW_IF(true) protected: MatrixNN matrix_; ///< Rotation matrix diff --git a/gtsam/geometry/Unit3.h b/gtsam/geometry/Unit3.h index f1a9c1a69..b80e6e4d8 100644 --- a/gtsam/geometry/Unit3.h +++ b/gtsam/geometry/Unit3.h @@ -214,7 +214,7 @@ private: /// @} public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW + GTSAM_MAKE_ALIGNED_OPERATOR_NEW }; // Define GTSAM traits diff --git a/gtsam/geometry/triangulation.h b/gtsam/geometry/triangulation.h index 586b7b165..8cdf0fdc0 100644 --- a/gtsam/geometry/triangulation.h +++ b/gtsam/geometry/triangulation.h @@ -215,7 +215,7 @@ struct CameraProjectionMatrix { private: const Matrix3 K_; public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW + GTSAM_MAKE_ALIGNED_OPERATOR_NEW }; /** diff --git a/gtsam/navigation/AttitudeFactor.h b/gtsam/navigation/AttitudeFactor.h index db588008e..5a0031caf 100644 --- a/gtsam/navigation/AttitudeFactor.h +++ b/gtsam/navigation/AttitudeFactor.h @@ -139,7 +139,7 @@ private: } public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW + GTSAM_MAKE_ALIGNED_OPERATOR_NEW }; /// traits @@ -219,7 +219,7 @@ private: } public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW + GTSAM_MAKE_ALIGNED_OPERATOR_NEW }; /// traits diff --git a/gtsam/navigation/CombinedImuFactor.h b/gtsam/navigation/CombinedImuFactor.h index ca9b2ca1a..6b3bf979a 100644 --- a/gtsam/navigation/CombinedImuFactor.h +++ b/gtsam/navigation/CombinedImuFactor.h @@ -100,7 +100,7 @@ private: } public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW + GTSAM_MAKE_ALIGNED_OPERATOR_NEW }; @@ -210,7 +210,7 @@ public: } public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW + GTSAM_MAKE_ALIGNED_OPERATOR_NEW }; /** @@ -332,7 +332,7 @@ private: } public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW + GTSAM_MAKE_ALIGNED_OPERATOR_NEW }; // class CombinedImuFactor diff --git a/gtsam/navigation/ImuBias.h b/gtsam/navigation/ImuBias.h index ff1a53025..d52b4eb29 100644 --- a/gtsam/navigation/ImuBias.h +++ b/gtsam/navigation/ImuBias.h @@ -171,7 +171,7 @@ private: public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW + GTSAM_MAKE_ALIGNED_OPERATOR_NEW /// @} }; // ConstantBias class diff --git a/gtsam/navigation/PreintegratedRotation.h b/gtsam/navigation/PreintegratedRotation.h index 71ef5d08f..12938a625 100644 --- a/gtsam/navigation/PreintegratedRotation.h +++ b/gtsam/navigation/PreintegratedRotation.h @@ -69,7 +69,7 @@ struct GTSAM_EXPORT PreintegratedRotationParams { #ifdef GTSAM_USE_QUATERNIONS // Align if we are using Quaternions public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW + GTSAM_MAKE_ALIGNED_OPERATOR_NEW #endif }; @@ -182,7 +182,7 @@ class GTSAM_EXPORT PreintegratedRotation { #ifdef GTSAM_USE_QUATERNIONS // Align if we are using Quaternions public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW + GTSAM_MAKE_ALIGNED_OPERATOR_NEW #endif }; diff --git a/gtsam/navigation/PreintegrationBase.h b/gtsam/navigation/PreintegrationBase.h index 9926d207a..eb30c1f13 100644 --- a/gtsam/navigation/PreintegrationBase.h +++ b/gtsam/navigation/PreintegrationBase.h @@ -214,7 +214,7 @@ class GTSAM_EXPORT PreintegrationBase { #endif public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW + GTSAM_MAKE_ALIGNED_OPERATOR_NEW }; } /// namespace gtsam diff --git a/gtsam/navigation/PreintegrationParams.h b/gtsam/navigation/PreintegrationParams.h index 962fef277..4bff625ca 100644 --- a/gtsam/navigation/PreintegrationParams.h +++ b/gtsam/navigation/PreintegrationParams.h @@ -84,7 +84,7 @@ protected: #ifdef GTSAM_USE_QUATERNIONS // Align if we are using Quaternions public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW + GTSAM_MAKE_ALIGNED_OPERATOR_NEW #endif }; diff --git a/gtsam/navigation/TangentPreintegration.h b/gtsam/navigation/TangentPreintegration.h index 11945e53a..edf76e562 100644 --- a/gtsam/navigation/TangentPreintegration.h +++ b/gtsam/navigation/TangentPreintegration.h @@ -141,7 +141,7 @@ private: } public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW + GTSAM_MAKE_ALIGNED_OPERATOR_NEW }; } /// namespace gtsam diff --git a/gtsam/nonlinear/ExpressionFactor.h b/gtsam/nonlinear/ExpressionFactor.h index 04d82fe9a..c42b2bdfc 100644 --- a/gtsam/nonlinear/ExpressionFactor.h +++ b/gtsam/nonlinear/ExpressionFactor.h @@ -209,7 +209,7 @@ private: // Alignment, see https://eigen.tuxfamily.org/dox/group__TopicStructHavingEigenMembers.html enum { NeedsToAlign = (sizeof(T) % 16) == 0 }; public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign) + GTSAM_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign) }; // ExpressionFactor diff --git a/gtsam/nonlinear/NonlinearEquality.h b/gtsam/nonlinear/NonlinearEquality.h index d4eb655c3..1bba57051 100644 --- a/gtsam/nonlinear/NonlinearEquality.h +++ b/gtsam/nonlinear/NonlinearEquality.h @@ -175,7 +175,7 @@ public: /// @} - EIGEN_MAKE_ALIGNED_OPERATOR_NEW + GTSAM_MAKE_ALIGNED_OPERATOR_NEW private: @@ -265,7 +265,7 @@ public: traits::Print(value_, "Value"); } - EIGEN_MAKE_ALIGNED_OPERATOR_NEW + GTSAM_MAKE_ALIGNED_OPERATOR_NEW private: @@ -331,7 +331,7 @@ public: return traits::Local(x1,x2); } - EIGEN_MAKE_ALIGNED_OPERATOR_NEW + GTSAM_MAKE_ALIGNED_OPERATOR_NEW private: diff --git a/gtsam/nonlinear/PriorFactor.h b/gtsam/nonlinear/PriorFactor.h index 8d8c67d5c..0afbaa588 100644 --- a/gtsam/nonlinear/PriorFactor.h +++ b/gtsam/nonlinear/PriorFactor.h @@ -114,7 +114,7 @@ namespace gtsam { // Alignment, see https://eigen.tuxfamily.org/dox/group__TopicStructHavingEigenMembers.html enum { NeedsToAlign = (sizeof(T) % 16) == 0 }; public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign) + GTSAM_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign) }; } /// namespace gtsam diff --git a/gtsam/nonlinear/internal/ExpressionNode.h b/gtsam/nonlinear/internal/ExpressionNode.h index 0011efb74..0ae37f130 100644 --- a/gtsam/nonlinear/internal/ExpressionNode.h +++ b/gtsam/nonlinear/internal/ExpressionNode.h @@ -150,7 +150,7 @@ public: return constant_; } - EIGEN_MAKE_ALIGNED_OPERATOR_NEW + GTSAM_MAKE_ALIGNED_OPERATOR_NEW }; //----------------------------------------------------------------------------- diff --git a/gtsam/slam/BetweenFactor.h b/gtsam/slam/BetweenFactor.h index 23138b9e6..b1d4904aa 100644 --- a/gtsam/slam/BetweenFactor.h +++ b/gtsam/slam/BetweenFactor.h @@ -126,7 +126,7 @@ namespace gtsam { // Alignment, see https://eigen.tuxfamily.org/dox/group__TopicStructHavingEigenMembers.html enum { NeedsToAlign = (sizeof(VALUE) % 16) == 0 }; public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign) + GTSAM_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign) }; // \class BetweenFactor /// traits diff --git a/gtsam/slam/EssentialMatrixConstraint.h b/gtsam/slam/EssentialMatrixConstraint.h index 179200fe1..e474ce5b3 100644 --- a/gtsam/slam/EssentialMatrixConstraint.h +++ b/gtsam/slam/EssentialMatrixConstraint.h @@ -105,7 +105,7 @@ private: } public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW + GTSAM_MAKE_ALIGNED_OPERATOR_NEW }; // \class EssentialMatrixConstraint diff --git a/gtsam/slam/EssentialMatrixFactor.h b/gtsam/slam/EssentialMatrixFactor.h index 8bd155a14..c214a2f48 100644 --- a/gtsam/slam/EssentialMatrixFactor.h +++ b/gtsam/slam/EssentialMatrixFactor.h @@ -81,7 +81,7 @@ public: } public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW + GTSAM_MAKE_ALIGNED_OPERATOR_NEW }; /** @@ -201,7 +201,7 @@ public: } public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW + GTSAM_MAKE_ALIGNED_OPERATOR_NEW }; // EssentialMatrixFactor2 @@ -286,7 +286,7 @@ public: } public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW + GTSAM_MAKE_ALIGNED_OPERATOR_NEW }; // EssentialMatrixFactor3 diff --git a/gtsam/slam/ProjectionFactor.h b/gtsam/slam/ProjectionFactor.h index 856913aae..0bed15fdc 100644 --- a/gtsam/slam/ProjectionFactor.h +++ b/gtsam/slam/ProjectionFactor.h @@ -189,7 +189,7 @@ namespace gtsam { } public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW + GTSAM_MAKE_ALIGNED_OPERATOR_NEW }; /// traits diff --git a/gtsam/slam/RotateFactor.h b/gtsam/slam/RotateFactor.h index b6ccc36a2..948fffe13 100644 --- a/gtsam/slam/RotateFactor.h +++ b/gtsam/slam/RotateFactor.h @@ -113,7 +113,7 @@ public: return error; } - EIGEN_MAKE_ALIGNED_OPERATOR_NEW + GTSAM_MAKE_ALIGNED_OPERATOR_NEW }; } // namespace gtsam diff --git a/gtsam/slam/SmartFactorBase.h b/gtsam/slam/SmartFactorBase.h index 34f9b9e9f..717a9c1f2 100644 --- a/gtsam/slam/SmartFactorBase.h +++ b/gtsam/slam/SmartFactorBase.h @@ -81,7 +81,7 @@ protected: mutable FBlocks Fs; public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW + GTSAM_MAKE_ALIGNED_OPERATOR_NEW /// shorthand for a smart pointer to a factor typedef boost::shared_ptr shared_ptr; From 17568e67793ad831b02ec701ef6734bbfe950f68 Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Thu, 25 Jun 2020 00:14:21 -0400 Subject: [PATCH 10/10] Add missing lf --- gtsam/base/types.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/base/types.h b/gtsam/base/types.h index 924f339b3..c5dac9ab7 100644 --- a/gtsam/base/types.h +++ b/gtsam/base/types.h @@ -276,4 +276,4 @@ namespace gtsam { */ #define GTSAM_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign) \ EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign) \ - using _eigen_aligned_allocator_trait = void; \ No newline at end of file + using _eigen_aligned_allocator_trait = void;