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;