diff --git a/gtsam/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp index b7a0c1714..faf5d4bbb 100644 --- a/gtsam/geometry/Pose3.cpp +++ b/gtsam/geometry/Pose3.cpp @@ -263,7 +263,7 @@ Point3 Pose3::transform_to(const Point3& p, boost::optional Dpose, boost::optional Dpoint) const { // Only get transpose once, to avoid multiple allocations, // as well as multiple conversions in the Quaternion case - Matrix3 Rt(R_.transpose()); + const Matrix3& Rt = R_.transpose(); const Point3 q(Rt*(p - t_).vector()); if (Dpose) { const double wx = q.x(), wy = q.y(), wz = q.z(); @@ -280,7 +280,7 @@ Point3 Pose3::transform_to(const Point3& p, boost::optional Dpose, /* ************************************************************************* */ Point3 Pose3::transform_to(const Point3& p, boost::optional Dpose, boost::optional Dpoint) const { - Matrix3 Rt(R_.transpose()); + const Matrix3& Rt = R_.transpose(); const Point3 q(Rt*(p - t_).vector()); if (Dpose) { const double wx = q.x(), wy = q.y(), wz = q.z(); diff --git a/gtsam/geometry/Rot3.cpp b/gtsam/geometry/Rot3.cpp index daa8eeda1..56acab747 100644 --- a/gtsam/geometry/Rot3.cpp +++ b/gtsam/geometry/Rot3.cpp @@ -101,7 +101,7 @@ Unit3 Rot3::operator*(const Unit3& p) const { // see doc/math.lyx, SO(3) section Point3 Rot3::unrotate(const Point3& p, boost::optional H1, boost::optional H2) const { - Matrix3 Rt(transpose()); + const Matrix3& Rt = transpose(); Point3 q(Rt * p.vector()); // q = Rt*p const double wx = q.x(), wy = q.y(), wz = q.z(); if (H1) @@ -115,7 +115,7 @@ Point3 Rot3::unrotate(const Point3& p, boost::optional H1, // see doc/math.lyx, SO(3) section Point3 Rot3::unrotate(const Point3& p, boost::optional H1, boost::optional H2) const { - Matrix3 Rt(transpose()); + const Matrix3& Rt = transpose(); Point3 q(Rt * p.vector()); // q = Rt*p const double wx = q.x(), wy = q.y(), wz = q.z(); if (H1) { diff --git a/gtsam/geometry/Rot3.h b/gtsam/geometry/Rot3.h index 115f288e3..187723308 100644 --- a/gtsam/geometry/Rot3.h +++ b/gtsam/geometry/Rot3.h @@ -70,6 +70,12 @@ namespace gtsam { Matrix3 rot_; #endif + /** + * transpose() is used millions of times in linearize, so cache it + * This also avoids multiple expensive conversions in the quaternion case + */ + mutable boost::optional transpose_; ///< Cached R_.transpose() + public: /// @name Constructors and named constructors @@ -368,8 +374,15 @@ namespace gtsam { /** return 3*3 rotation matrix */ Matrix3 matrix() const; - /** return 3*3 transpose (inverse) rotation matrix */ - Matrix3 transpose() const; + /** + * Return 3*3 transpose (inverse) rotation matrix + * Actually returns cached transpose, or computes it if not yet done + */ + const Matrix3& transpose() const { + if (!transpose_) + transpose_.reset(inverse().matrix()); + return *transpose_; + } /// @deprecated, this is base 1, and was just confusing Point3 column(int index) const; @@ -439,6 +452,7 @@ namespace gtsam { GTSAM_EXPORT friend std::ostream &operator<<(std::ostream &os, const Rot3& p); private: + /** Serialization function */ friend class boost::serialization::access; template @@ -463,6 +477,7 @@ namespace gtsam { ar & boost::serialization::make_nvp("z", quaternion_.z()); #endif } + }; /// @} diff --git a/gtsam/geometry/Rot3M.cpp b/gtsam/geometry/Rot3M.cpp index 96ebcac08..4d2b1e47a 100644 --- a/gtsam/geometry/Rot3M.cpp +++ b/gtsam/geometry/Rot3M.cpp @@ -295,11 +295,6 @@ Matrix3 Rot3::matrix() const { return rot_; } -/* ************************************************************************* */ -Matrix3 Rot3::transpose() const { - return rot_.transpose(); -} - /* ************************************************************************* */ Point3 Rot3::r1() const { return Point3(rot_.col(0)); } diff --git a/gtsam/geometry/Rot3Q.cpp b/gtsam/geometry/Rot3Q.cpp index 4344a623c..a5eabc78d 100644 --- a/gtsam/geometry/Rot3Q.cpp +++ b/gtsam/geometry/Rot3Q.cpp @@ -156,9 +156,6 @@ namespace gtsam { /* ************************************************************************* */ Matrix3 Rot3::matrix() const {return quaternion_.toRotationMatrix();} - /* ************************************************************************* */ - Matrix3 Rot3::transpose() const {return quaternion_.toRotationMatrix().transpose();} - /* ************************************************************************* */ Point3 Rot3::r1() const { return Point3(quaternion_.toRotationMatrix().col(0)); }