From cfe56a0aa8069ed425d3f3ac3260e0e436b2331e Mon Sep 17 00:00:00 2001
From: dellaert <dellaert@cc.gatech.edu>
Date: Thu, 23 Oct 2014 19:13:37 +0200
Subject: [PATCH] Removed transpose_. It did speed up things but was bad
 design. Will need to profile again and find different ways to cope with
 transpose() overhead. One way is to return a Eigen::Transpose<> object as
 hinted to in comments.

---
 gtsam/geometry/Rot3.cpp  |  2 +-
 gtsam/geometry/Rot3.h    | 12 ++----------
 gtsam/geometry/Rot3M.cpp | 15 ++++++++-------
 gtsam/geometry/Rot3Q.cpp | 41 +++++++++++++++++++++++-----------------
 4 files changed, 35 insertions(+), 35 deletions(-)

diff --git a/gtsam/geometry/Rot3.cpp b/gtsam/geometry/Rot3.cpp
index 13a041a5b..c9b351138 100644
--- a/gtsam/geometry/Rot3.cpp
+++ b/gtsam/geometry/Rot3.cpp
@@ -258,7 +258,7 @@ ostream &operator<<(ostream &os, const Rot3& R) {
 /* ************************************************************************* */
 Point3 Rot3::unrotate(const Point3& p) const {
   // Eigen expression
-  return Point3(transpose_*p.vector()); // q = Rt*p
+  return Point3(transpose()*p.vector()); // q = Rt*p
 }
 
 /* ************************************************************************* */
diff --git a/gtsam/geometry/Rot3.h b/gtsam/geometry/Rot3.h
index b5e065a03..7215e159f 100644
--- a/gtsam/geometry/Rot3.h
+++ b/gtsam/geometry/Rot3.h
@@ -69,12 +69,6 @@ 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
-     */
-    Matrix3 transpose_; ///< Cached R_.transpose()
-
   public:
 
     /// @name Constructors and named constructors
@@ -375,11 +369,9 @@ namespace gtsam {
 
     /**
      * Return 3*3 transpose (inverse) rotation matrix
-     * Actually returns cached transpose
      */
-    const Matrix3& transpose() const {
-      return transpose_;
-    }
+    Matrix3 transpose() const;
+    // TODO: const Eigen::Transpose<const Matrix3> transpose() const;
 
     /// @deprecated, this is base 1, and was just confusing
     Point3 column(int index) const;
diff --git a/gtsam/geometry/Rot3M.cpp b/gtsam/geometry/Rot3M.cpp
index 3f2f13057..d0c7e95f3 100644
--- a/gtsam/geometry/Rot3M.cpp
+++ b/gtsam/geometry/Rot3M.cpp
@@ -33,14 +33,13 @@ namespace gtsam {
 static const Matrix3 I3 = Matrix3::Identity();
 
 /* ************************************************************************* */
-Rot3::Rot3() : rot_(Matrix3::Identity()), transpose_(Matrix3::Identity()) {}
+Rot3::Rot3() : rot_(Matrix3::Identity()) {}
 
 /* ************************************************************************* */
 Rot3::Rot3(const Point3& col1, const Point3& col2, const Point3& col3) {
   rot_.col(0) = col1.vector();
   rot_.col(1) = col2.vector();
   rot_.col(2) = col3.vector();
-  transpose_ = rot_.transpose();
 }
 
 /* ************************************************************************* */
@@ -50,13 +49,11 @@ Rot3::Rot3(double R11, double R12, double R13,
     rot_ << R11, R12, R13,
         R21, R22, R23,
         R31, R32, R33;
-    transpose_ = rot_.transpose();
 }
 
 /* ************************************************************************* */
 Rot3::Rot3(const Matrix3& R) {
   rot_ = R;
-  transpose_ = rot_.transpose();
 }
 
 /* ************************************************************************* */
@@ -64,12 +61,10 @@ Rot3::Rot3(const Matrix& R) {
   if (R.rows()!=3 || R.cols()!=3)
     throw invalid_argument("Rot3 constructor expects 3*3 matrix");
   rot_ = R;
-  transpose_ = rot_.transpose();
 }
 
 /* ************************************************************************* */
 Rot3::Rot3(const Quaternion& q) : rot_(q.toRotationMatrix()) {
-  transpose_ = rot_.transpose();
 }
 
 /* ************************************************************************* */
@@ -172,10 +167,16 @@ Rot3 Rot3::operator*(const Rot3& R2) const {
   return Rot3(Matrix3(rot_*R2.rot_));
 }
 
+/* ************************************************************************* */
+// TODO const Eigen::Transpose<const Matrix3> Rot3::transpose() const {
+Matrix3 Rot3::transpose() const {
+  return rot_.transpose();
+}
+
 /* ************************************************************************* */
 Rot3 Rot3::inverse(boost::optional<Matrix&> H1) const {
   if (H1) *H1 = -rot_;
-  return Rot3(transpose());
+  return Rot3(Matrix3(transpose()));
 }
 
 /* ************************************************************************* */
diff --git a/gtsam/geometry/Rot3Q.cpp b/gtsam/geometry/Rot3Q.cpp
index 191b52378..19de92ca2 100644
--- a/gtsam/geometry/Rot3Q.cpp
+++ b/gtsam/geometry/Rot3Q.cpp
@@ -30,36 +30,35 @@ namespace gtsam {
   static const Matrix I3 = eye(3);
 
   /* ************************************************************************* */
-  Rot3::Rot3() :
-      quaternion_(Quaternion::Identity()), transpose_(Matrix3::Identity()) {
-  }
+  Rot3::Rot3() : quaternion_(Quaternion::Identity()) {}
 
   /* ************************************************************************* */
-  Rot3::Rot3(const Point3& col1, const Point3& col2, const Point3& col3) {
-    transpose_ << col1.x(), col1.y(), col1.z(), //
-        col2.x(), col2.y(), col2.z(), //
-        col3.x(), col3.y(), col3.z();
-    quaternion_ = Quaternion(transpose_.transpose());
-  }
+  Rot3::Rot3(const Point3& col1, const Point3& col2, const Point3& col3) :
+      quaternion_((Eigen::Matrix3d() <<
+          col1.x(), col2.x(), col3.x(),
+          col1.y(), col2.y(), col3.y(),
+          col1.z(), col2.z(), col3.z()).finished()) {}
 
   /* ************************************************************************* */
-  Rot3::Rot3(double R11, double R12, double R13, double R21, double R22,
-      double R23, double R31, double R32, double R33) {
-    transpose_ << R11, R21, R31, R12, R22, R32, R13, R23, R33;
-    quaternion_ = Quaternion(transpose_.transpose());
-  }
+  Rot3::Rot3(double R11, double R12, double R13,
+      double R21, double R22, double R23,
+      double R31, double R32, double R33) :
+        quaternion_((Eigen::Matrix3d() <<
+            R11, R12, R13,
+            R21, R22, R23,
+            R31, R32, R33).finished()) {}
 
   /* ************************************************************************* */
   Rot3::Rot3(const Matrix3& R) :
-      quaternion_(R), transpose_(R.transpose()) {}
+      quaternion_(R) {}
 
   /* ************************************************************************* */
   Rot3::Rot3(const Matrix& R) :
-      quaternion_(Matrix3(R)), transpose_(R.transpose()) {}
+      quaternion_(Matrix3(R)) {}
 
   /* ************************************************************************* */
   Rot3::Rot3(const Quaternion& q) :
-      quaternion_(q), transpose_(q.toRotationMatrix().transpose()) {
+      quaternion_(q) {
   }
 
   /* ************************************************************************* */
@@ -120,6 +119,14 @@ namespace gtsam {
     return Rot3(quaternion_.inverse());
   }
 
+  /* ************************************************************************* */
+  // TODO: Could we do this? It works in Rot3M but not here, probably because
+  // here we create an intermediate value by calling matrix()
+  // const Eigen::Transpose<const Matrix3> Rot3::transpose() const {
+  Matrix3 Rot3::transpose() const {
+    return matrix().transpose();
+  }
+
   /* ************************************************************************* */
   Rot3 Rot3::between(const Rot3& R2,
   boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {