diff --git a/gtsam/base/GenericValue.h b/gtsam/base/GenericValue.h index 229050448..67cc2646e 100644 --- a/gtsam/base/GenericValue.h +++ b/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 clone() const { - return boost::make_shared(*this); + return boost::allocate_shared(Eigen::aligned_allocator(), *this); } /// Generic Value interface version of retract diff --git a/gtsam/geometry/Rot2.h b/gtsam/geometry/Rot2.h index 65dd5f609..eaec62b48 100644 --- a/gtsam/geometry/Rot2.h +++ b/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); } diff --git a/gtsam/inference/FactorGraph.h b/gtsam/inference/FactorGraph.h index 016488701..6be3b89c2 100644 --- a/gtsam/inference/FactorGraph.h +++ b/gtsam/inference/FactorGraph.h @@ -26,6 +26,8 @@ #include #include +#include // for Eigen::aligned_allocator + #include #include #include @@ -161,11 +163,11 @@ namespace gtsam { factors_.push_back(factor); } /** Emplace a factor */ - template - typename std::enable_if::value>::type - emplace_shared(Args&&... args) { - factors_.push_back(boost::make_shared(std::forward(args)...)); - } + template + typename std::enable_if::value>::type + emplace_shared(Args&&... args) { + factors_.push_back(boost::allocate_shared(Eigen::aligned_allocator(), std::forward(args)...)); + } /** push back many factors with an iterator over shared_ptr (factors are not copied) */ template @@ -194,7 +196,7 @@ namespace gtsam { template typename std::enable_if::value>::type push_back(const DERIVEDFACTOR& factor) { - factors_.push_back(boost::make_shared(factor)); + factors_.push_back(boost::allocate_shared(Eigen::aligned_allocator(), factor)); } //#endif diff --git a/gtsam/navigation/CombinedImuFactor.cpp b/gtsam/navigation/CombinedImuFactor.cpp index 21c4200a9..1967e4a56 100644 --- a/gtsam/navigation/CombinedImuFactor.cpp +++ b/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 p = - boost::make_shared(pim.p()); + using P = CombinedPreintegratedMeasurements::Params; + auto p = boost::allocate_shared

(Eigen::aligned_allocator

(), pim.p()); p->n_gravity = n_gravity; p->omegaCoriolis = omegaCoriolis; p->body_P_sensor = body_P_sensor; 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(*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