Cached Rot3::transpose(). Inspired by @cbeall3 fix of Unit3, I realized that with one million points and 1000 cameras, the same Matrix3 (R_.transpose()) was computed a 1000 more times than needed. Especially with quaternions this would be expensive, even with Rot3Q.

gtsam/geometry/Pose3.cpp

@@ -263,7 +263,7 @@ Point3 Pose3::transform_to(const Point3& p, boost::optional<Matrix36&> Dpose,
     boost::optional<Matrix3&> 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<Matrix36&> Dpose,
 /* ************************************************************************* */
 Point3 Pose3::transform_to(const Point3& p, boost::optional<Matrix&> Dpose,
     boost::optional<Matrix&> 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();

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<Matrix3&> H1,
     boost::optional<Matrix3&> 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<Matrix3&> H1,
 // see doc/math.lyx, SO(3) section
 Point3 Rot3::unrotate(const Point3& p,
     boost::optional<Matrix&> H1, boost::optional<Matrix&> 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) {

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<Matrix3> 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<class ARCHIVE>
@@ -463,6 +477,7 @@ namespace gtsam {
       ar & boost::serialization::make_nvp("z", quaternion_.z());
 #endif
     }
+
   };
 
   /// @}

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)); }
 

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)); }