From 65c0515829dce54c53bd74904e4d158f1b5253bf Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Tue, 6 Nov 2018 00:28:36 -0500 Subject: [PATCH 1/4] Ignore windows temps --- .gitignore | 2 ++ 1 file changed, 2 insertions(+) diff --git a/.gitignore b/.gitignore index 2682a6748..42bd27466 100644 --- a/.gitignore +++ b/.gitignore @@ -17,3 +17,5 @@ cython/gtsam.so cython/gtsam_wrapper.pxd .vscode .env +/.vs/ +/CMakeSettings.json From 2584d421b8b78c974a8f7f938ea749c5ee97ae96 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Tue, 6 Nov 2018 00:29:02 -0500 Subject: [PATCH 2/4] Weird alignment issue -> changed to static --- gtsam/geometry/Rot2.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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); } From c58a78b60a8b15a73fa4b72020166c8a31b65cc8 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Tue, 6 Nov 2018 00:29:38 -0500 Subject: [PATCH 3/4] 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(*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 From 3c4aadc712d19acc7bb164c54c18099e6c0c565b Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Tue, 6 Nov 2018 00:30:18 -0500 Subject: [PATCH 4/4] Use allocate_shared rather than make_shared to make sure alignment is good --- gtsam/base/GenericValue.h | 2 +- gtsam/inference/FactorGraph.h | 14 ++++++++------ gtsam/navigation/CombinedImuFactor.cpp | 4 ++-- 3 files changed, 11 insertions(+), 9 deletions(-) diff --git a/gtsam/base/GenericValue.h b/gtsam/base/GenericValue.h index 93a7d0db5..87ddf574c 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/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;