From b8f3cd0f137beefa53253ecbada5af560942cbe2 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 4 Nov 2018 13:05:28 -0500 Subject: [PATCH] Merge in alignment-related changes from 'origin/fix/msvc2017' --- gtsam/base/Manifold.h | 6 ++++++ gtsam/geometry/CalibratedCamera.h | 7 ------- gtsam/geometry/Point3.h | 24 ++++++++++++------------ gtsam/geometry/Pose2.h | 6 ++++++ gtsam/geometry/Pose3.h | 8 +++++--- gtsam/geometry/Rot3.h | 6 ++++-- gtsam/navigation/AttitudeFactor.h | 6 ++++++ gtsam/slam/BetweenFactor.h | 5 +++++ gtsam/slam/PriorFactor.h | 5 +++++ 9 files changed, 49 insertions(+), 24 deletions(-) diff --git a/gtsam/base/Manifold.h b/gtsam/base/Manifold.h index b30edb3df..f89680b7c 100644 --- a/gtsam/base/Manifold.h +++ b/gtsam/base/Manifold.h @@ -209,6 +209,12 @@ public: v << v1, v2; return v; } + + // Alignment, see https://eigen.tuxfamily.org/dox/group__TopicStructHavingEigenMembers.html + enum { NeedsToAlign = (sizeof(M1) % 16) == 0 || (sizeof(M2) % 16) == 0 + }; +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign) }; // Define any direct product group to be a model of the multiplicative Group concept diff --git a/gtsam/geometry/CalibratedCamera.h b/gtsam/geometry/CalibratedCamera.h index db9caf13b..acb3cacaf 100644 --- a/gtsam/geometry/CalibratedCamera.h +++ b/gtsam/geometry/CalibratedCamera.h @@ -229,10 +229,6 @@ private: void serialize(Archive & ar, const unsigned int /*version*/) { ar & BOOST_SERIALIZATION_NVP(pose_); } - -public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW - }; // end of class PinholeBase @@ -416,9 +412,6 @@ private: } /// @} - -public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW }; // manifold traits diff --git a/gtsam/geometry/Point3.h b/gtsam/geometry/Point3.h index 99cb6c2e7..215161b3a 100644 --- a/gtsam/geometry/Point3.h +++ b/gtsam/geometry/Point3.h @@ -157,28 +157,28 @@ struct traits : public internal::VectorSpace {}; // Convenience typedef typedef std::pair Point3Pair; -std::ostream &operator<<(std::ostream &os, const gtsam::Point3Pair &p); +GTSAM_EXPORT std::ostream &operator<<(std::ostream &os, const gtsam::Point3Pair &p); /// distance between two points -double distance3(const Point3& p1, const Point3& q, - OptionalJacobian<1, 3> H1 = boost::none, - OptionalJacobian<1, 3> H2 = boost::none); +GTSAM_EXPORT double distance3(const Point3& p1, const Point3& q, + OptionalJacobian<1, 3> H1 = boost::none, + OptionalJacobian<1, 3> H2 = boost::none); /// Distance of the point from the origin, with Jacobian -double norm3(const Point3& p, OptionalJacobian<1, 3> H = boost::none); +GTSAM_EXPORT double norm3(const Point3& p, OptionalJacobian<1, 3> H = boost::none); /// normalize, with optional Jacobian -Point3 normalize(const Point3& p, OptionalJacobian<3, 3> H = boost::none); +GTSAM_EXPORT Point3 normalize(const Point3& p, OptionalJacobian<3, 3> H = boost::none); /// cross product @return this x q -Point3 cross(const Point3& p, const Point3& q, - OptionalJacobian<3, 3> H_p = boost::none, - OptionalJacobian<3, 3> H_q = boost::none); +GTSAM_EXPORT Point3 cross(const Point3& p, const Point3& q, + OptionalJacobian<3, 3> H_p = boost::none, + OptionalJacobian<3, 3> H_q = boost::none); /// dot product -double dot(const Point3& p, const Point3& q, - OptionalJacobian<1, 3> H_p = boost::none, - OptionalJacobian<1, 3> H_q = boost::none); +GTSAM_EXPORT double dot(const Point3& p, const Point3& q, + OptionalJacobian<1, 3> H_p = boost::none, + OptionalJacobian<1, 3> H_q = boost::none); template struct Range; diff --git a/gtsam/geometry/Pose2.h b/gtsam/geometry/Pose2.h index 1ba384857..f03e0852e 100644 --- a/gtsam/geometry/Pose2.h +++ b/gtsam/geometry/Pose2.h @@ -278,6 +278,12 @@ private: ar & BOOST_SERIALIZATION_NVP(t_); ar & BOOST_SERIALIZATION_NVP(r_); } + +#ifdef GTSAM_TYPEDEF_POINTS_TO_VECTORS +public: + // Make sure Pose2 is aligned if it contains a Vector2 + EIGEN_MAKE_ALIGNED_OPERATOR_NEW +#endif }; // Pose2 /** specialization for pose2 wedge function (generic template in Lie.h) */ diff --git a/gtsam/geometry/Pose3.h b/gtsam/geometry/Pose3.h index 3229cbbbe..ca0fdff10 100644 --- a/gtsam/geometry/Pose3.h +++ b/gtsam/geometry/Pose3.h @@ -327,9 +327,11 @@ public: } /// @} - public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW - +#ifdef GTSAM_USE_QUATERNIONS + // Align if we are using Quaternions + public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW +#endif }; // Pose3 class diff --git a/gtsam/geometry/Rot3.h b/gtsam/geometry/Rot3.h index 263301122..985c521a2 100644 --- a/gtsam/geometry/Rot3.h +++ b/gtsam/geometry/Rot3.h @@ -508,9 +508,11 @@ namespace gtsam { #endif } - public: +#ifdef GTSAM_USE_QUATERNIONS + // only align if quaternion, Matrix3 has no alignment requirements + public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW - +#endif }; /** diff --git a/gtsam/navigation/AttitudeFactor.h b/gtsam/navigation/AttitudeFactor.h index 21f74ac06..4ae6078e9 100644 --- a/gtsam/navigation/AttitudeFactor.h +++ b/gtsam/navigation/AttitudeFactor.h @@ -129,6 +129,9 @@ private: ar & BOOST_SERIALIZATION_NVP(nZ_); ar & BOOST_SERIALIZATION_NVP(bRef_); } + +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW }; /// traits @@ -213,6 +216,9 @@ private: ar & BOOST_SERIALIZATION_NVP(nZ_); ar & BOOST_SERIALIZATION_NVP(bRef_); } + +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW }; /// traits diff --git a/gtsam/slam/BetweenFactor.h b/gtsam/slam/BetweenFactor.h index f3fd49fa7..89291fac5 100644 --- a/gtsam/slam/BetweenFactor.h +++ b/gtsam/slam/BetweenFactor.h @@ -122,6 +122,11 @@ namespace gtsam { boost::serialization::base_object(*this)); ar & BOOST_SERIALIZATION_NVP(measured_); } + + // Alignment, see https://eigen.tuxfamily.org/dox/group__TopicStructHavingEigenMembers.html + enum { NeedsToAlign = (sizeof(VALUE) % 16) == 0 }; + public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign) }; // \class BetweenFactor /// traits diff --git a/gtsam/slam/PriorFactor.h b/gtsam/slam/PriorFactor.h index 9a3a4a47a..3c5c42ccc 100644 --- a/gtsam/slam/PriorFactor.h +++ b/gtsam/slam/PriorFactor.h @@ -104,6 +104,11 @@ namespace gtsam { boost::serialization::base_object(*this)); ar & BOOST_SERIALIZATION_NVP(prior_); } + + // Alignment, see https://eigen.tuxfamily.org/dox/group__TopicStructHavingEigenMembers.html + enum { NeedsToAlign = (sizeof(T) % 16) == 0 }; + public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign) }; } /// namespace gtsam