diff --git a/gtsam/geometry/Rot3.h b/gtsam/geometry/Rot3.h index 3e0e33609..fead20ea9 100644 --- a/gtsam/geometry/Rot3.h +++ b/gtsam/geometry/Rot3.h @@ -411,7 +411,7 @@ namespace gtsam { /** * Return 3*3 transpose (inverse) rotation matrix */ - const Eigen::Transpose transpose() const; + Matrix3 transpose() const; /// @deprecated, this is base 1, and was just confusing Point3 column(int index) const; @@ -440,16 +440,25 @@ namespace gtsam { /** * Accessor to get to component of angle representations + * NOTE: these are not efficient to get to multiple separate parts, + * you should instead use xyz() or ypr() + * TODO: make this more efficient */ inline double roll() const { return xyz()(0); } /** * Accessor to get to component of angle representations + * NOTE: these are not efficient to get to multiple separate parts, + * you should instead use xyz() or ypr() + * TODO: make this more efficient */ inline double pitch() const { return xyz()(1); } /** * Accessor to get to component of angle representations + * NOTE: these are not efficient to get to multiple separate parts, + * you should instead use xyz() or ypr() + * TODO: make this more efficient */ inline double yaw() const { return xyz()(2); } diff --git a/gtsam/geometry/Rot3M.cpp b/gtsam/geometry/Rot3M.cpp index 85d9923fb..46a07e50a 100644 --- a/gtsam/geometry/Rot3M.cpp +++ b/gtsam/geometry/Rot3M.cpp @@ -110,7 +110,7 @@ Rot3 Rot3::operator*(const Rot3& R2) const { } /* ************************************************************************* */ -const Eigen::Transpose Rot3::transpose() const { +Matrix3 Rot3::transpose() const { return rot_.matrix().transpose(); } diff --git a/gtsam/geometry/Rot3Q.cpp b/gtsam/geometry/Rot3Q.cpp index 64e8324b8..0116f137d 100644 --- a/gtsam/geometry/Rot3Q.cpp +++ b/gtsam/geometry/Rot3Q.cpp @@ -79,9 +79,10 @@ namespace gtsam { } /* ************************************************************************* */ - const Eigen::Transpose Rot3::transpose() const { + Matrix3 Rot3::transpose() const { // `eval` for immediate evaluation (allows compilation). - return Rot3(matrix()).matrix().eval().transpose(); + // return Rot3(matrix()).matrix().eval().transpose(); + return matrix().eval().transpose(); } /* ************************************************************************* */