From b8f3cd0f137beefa53253ecbada5af560942cbe2 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 4 Nov 2018 13:05:28 -0500 Subject: [PATCH 1/3] 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 From 6e1994abd3b509b3db3507d808900c7332b8c66c Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 4 Nov 2018 22:41:15 -0500 Subject: [PATCH 2/3] No longer uses ProductManifold, compilation issue with aligned operator. --- gtsam/geometry/EssentialMatrix.h | 94 ++++++++++++++++---------------- 1 file changed, 48 insertions(+), 46 deletions(-) diff --git a/gtsam/geometry/EssentialMatrix.h b/gtsam/geometry/EssentialMatrix.h index 9dec574eb..308250033 100644 --- a/gtsam/geometry/EssentialMatrix.h +++ b/gtsam/geometry/EssentialMatrix.h @@ -11,7 +11,9 @@ #include #include #include + #include +#include namespace gtsam { @@ -21,19 +23,13 @@ namespace gtsam { * but here we choose instead to parameterize it as a (Rot3,Unit3) pair. * We can then non-linearly optimize immediately on this 5-dimensional manifold. */ -class GTSAM_EXPORT EssentialMatrix : private ProductManifold { - -private: - typedef ProductManifold Base; - Matrix3 E_; ///< Essential matrix - - /// Construct from Base - EssentialMatrix(const Base& base) : - Base(base), E_(direction().skew() * rotation().matrix()) { - } - -public: +class GTSAM_EXPORT EssentialMatrix { + private: + Rot3 R_; ///< Rotation + Unit3 t_; ///< Translation + Matrix3 E_; ///< Essential matrix + public: /// Static function to convert Point2 to homogeneous coordinates static Vector3 Homogeneous(const Point2& p) { return Vector3(p.x(), p.y(), 1); @@ -43,13 +39,12 @@ public: /// @{ /// Default constructor - EssentialMatrix() : - Base(Rot3(), Unit3(1, 0, 0)), E_(direction().skew()) { + EssentialMatrix() :E_(t_.skew()) { } /// Construct from rotation and translation EssentialMatrix(const Rot3& aRb, const Unit3& aTb) : - Base(aRb, aTb), E_(direction().skew() * rotation().matrix()) { + R_(aRb), t_(aTb), E_(t_.skew() * R_.matrix()) { } /// Named constructor with derivatives @@ -79,27 +74,32 @@ public: /// assert equality up to a tolerance bool equals(const EssentialMatrix& other, double tol = 1e-8) const { - return rotation().equals(other.rotation(), tol) - && direction().equals(other.direction(), tol); + return R_.equals(other.R_, tol) + && t_.equals(other.t_, tol); } /// @} /// @name Manifold /// @{ + enum { dimension = 5 }; + inline static size_t Dim() { return dimension;} + inline size_t dim() const { return dimension;} - using Base::dimension; - using Base::dim; - using Base::Dim; + typedef OptionalJacobian ChartJacobian; /// Retract delta to manifold - EssentialMatrix retract(const TangentVector& v) const { - return Base::retract(v); + EssentialMatrix retract(const Vector5& xi) const { + return EssentialMatrix(R_.retract(xi.head<3>()), t_.retract(xi.tail<2>())); } /// Compute the coordinates in the tangent space - TangentVector localCoordinates(const EssentialMatrix& other) const { - return Base::localCoordinates(other); + Vector5 localCoordinates(const EssentialMatrix& other) const { + auto v1 = R_.localCoordinates(other.R_); + auto v2 = t_.localCoordinates(other.t_); + Vector5 v; + v << v1, v2; + return v; } /// @} @@ -108,12 +108,12 @@ public: /// Rotation inline const Rot3& rotation() const { - return this->first; + return R_; } /// Direction inline const Unit3& direction() const { - return this->second; + return t_; } /// Return 3*3 matrix representation @@ -123,12 +123,12 @@ public: /// Return epipole in image_a , as Unit3 to allow for infinity inline const Unit3& epipole_a() const { - return direction(); + return t_; } /// Return epipole in image_b, as Unit3 to allow for infinity inline Unit3 epipole_b() const { - return rotation().unrotate(direction()); + return R_.unrotate(t_); } /** @@ -139,8 +139,8 @@ public: * @return point in pose coordinates */ Point3 transform_to(const Point3& p, - OptionalJacobian<3,5> DE = boost::none, - OptionalJacobian<3,3> Dpoint = boost::none) const; + OptionalJacobian<3, 5> DE = boost::none, + OptionalJacobian<3, 3> Dpoint = boost::none) const; /** * Given essential matrix E in camera frame B, convert to body frame C @@ -160,8 +160,8 @@ public: } /// epipolar error, algebraic - double error(const Vector3& vA, const Vector3& vB, // - OptionalJacobian<1,5> H = boost::none) const; + double error(const Vector3& vA, const Vector3& vB, + OptionalJacobian<1, 5> H = boost::none) const; /// @} @@ -176,8 +176,7 @@ public: /// @} -private: - + private: /// @name Advanced Interface /// @{ @@ -185,21 +184,24 @@ private: friend class boost::serialization::access; template void serialize(ARCHIVE & ar, const unsigned int /*version*/) { - ar & BOOST_SERIALIZATION_NVP(first); - ar & BOOST_SERIALIZATION_NVP(second); + ar & BOOST_SERIALIZATION_NVP(R_); + ar & BOOST_SERIALIZATION_NVP(t_); - ar & boost::serialization::make_nvp("E11", E_(0,0)); - ar & boost::serialization::make_nvp("E12", E_(0,1)); - ar & boost::serialization::make_nvp("E13", E_(0,2)); - ar & boost::serialization::make_nvp("E21", E_(1,0)); - ar & boost::serialization::make_nvp("E22", E_(1,1)); - ar & boost::serialization::make_nvp("E23", E_(1,2)); - ar & boost::serialization::make_nvp("E31", E_(2,0)); - ar & boost::serialization::make_nvp("E32", E_(2,1)); - ar & boost::serialization::make_nvp("E33", E_(2,2)); + ar & boost::serialization::make_nvp("E11", E_(0, 0)); + ar & boost::serialization::make_nvp("E12", E_(0, 1)); + ar & boost::serialization::make_nvp("E13", E_(0, 2)); + ar & boost::serialization::make_nvp("E21", E_(1, 0)); + ar & boost::serialization::make_nvp("E22", E_(1, 1)); + ar & boost::serialization::make_nvp("E23", E_(1, 2)); + ar & boost::serialization::make_nvp("E31", E_(2, 0)); + ar & boost::serialization::make_nvp("E32", E_(2, 1)); + ar & boost::serialization::make_nvp("E33", E_(2, 2)); } /// @} + + public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW }; template<> @@ -208,5 +210,5 @@ struct traits : public internal::Manifold {}; template<> struct traits : public internal::Manifold {}; -} // gtsam +} // namespace gtsam From 5409477c45d7532fe7b1bd325e8b4551d1da4bca Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Mon, 5 Nov 2018 00:19:04 -0500 Subject: [PATCH 3/3] Add aligned new operator --- gtsam/slam/EssentialMatrixConstraint.h | 3 +++ 1 file changed, 3 insertions(+) diff --git a/gtsam/slam/EssentialMatrixConstraint.h b/gtsam/slam/EssentialMatrixConstraint.h index 92a78279b..179200fe1 100644 --- a/gtsam/slam/EssentialMatrixConstraint.h +++ b/gtsam/slam/EssentialMatrixConstraint.h @@ -103,6 +103,9 @@ private: boost::serialization::base_object(*this)); ar & BOOST_SERIALIZATION_NVP(measuredE_); } + +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW }; // \class EssentialMatrixConstraint