From 5bcc3d922c56c627eb3bb13fb5f583a3f3ebf53c Mon Sep 17 00:00:00 2001
From: dellaert <dellaert@cc.gatech.edu>
Date: Wed, 15 Oct 2014 14:28:47 +0200
Subject: [PATCH] Just always store transpose? Problem with optional was that
 the *empty* optional was copied from the Values, so we gained nothing.
 However, this is expensive space-wise (double), and optimizes for a
 particular usage of Rot3, so does not seem good practice (see stack overflow
 discussion, as well). But the alternative is cumbersome.

---
 gtsam/geometry/Rot3.h    |  8 +++-----
 gtsam/geometry/Rot3M.cpp | 20 ++++++++++++--------
 2 files changed, 15 insertions(+), 13 deletions(-)

diff --git a/gtsam/geometry/Rot3.h b/gtsam/geometry/Rot3.h
index 187723308..59a09ba39 100644
--- a/gtsam/geometry/Rot3.h
+++ b/gtsam/geometry/Rot3.h
@@ -74,7 +74,7 @@ namespace gtsam {
      * 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()
+    Matrix3 transpose_; ///< Cached R_.transpose()
 
   public:
 
@@ -376,12 +376,10 @@ namespace gtsam {
 
     /**
      * Return 3*3 transpose (inverse) rotation matrix
-     * Actually returns cached transpose, or computes it if not yet done
+     * Actually returns cached transpose
      */
     const Matrix3& transpose() const {
-      if (!transpose_)
-        transpose_.reset(inverse().matrix());
-      return *transpose_;
+      return transpose_;
     }
 
     /// @deprecated, this is base 1, and was just confusing
diff --git a/gtsam/geometry/Rot3M.cpp b/gtsam/geometry/Rot3M.cpp
index 4d2b1e47a..77d97219c 100644
--- a/gtsam/geometry/Rot3M.cpp
+++ b/gtsam/geometry/Rot3M.cpp
@@ -33,13 +33,14 @@ namespace gtsam {
 static const Matrix3 I3 = Matrix3::Identity();
 
 /* ************************************************************************* */
-Rot3::Rot3() : rot_(Matrix3::Identity()) {}
+Rot3::Rot3() : rot_(Matrix3::Identity()), transpose_(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();
 }
 
 /* ************************************************************************* */
@@ -49,11 +50,13 @@ 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();
 }
 
 /* ************************************************************************* */
@@ -61,13 +64,13 @@ 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 Matrix3& R) : rot_(R) {}
-
 /* ************************************************************************* */
-Rot3::Rot3(const Quaternion& q) : rot_(q.toRotationMatrix()) {}
+Rot3::Rot3(const Quaternion& q) : rot_(q.toRotationMatrix()) {
+  transpose_ = rot_.transpose();
+}
 
 /* ************************************************************************* */
 Rot3 Rot3::Rx(double t) {
@@ -172,7 +175,7 @@ Rot3 Rot3::operator*(const Rot3& R2) const {
 /* ************************************************************************* */
 Rot3 Rot3::inverse(boost::optional<Matrix&> H1) const {
   if (H1) *H1 = -rot_;
-  return Rot3(Matrix3(rot_.transpose()));
+  return Rot3(transpose());
 }
 
 /* ************************************************************************* */
@@ -180,7 +183,8 @@ Rot3 Rot3::between (const Rot3& R2,
     boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
   if (H1) *H1 = -(R2.transpose()*rot_);
   if (H2) *H2 = I3;
-  return Rot3(Matrix3(rot_.transpose()*R2.rot_));
+  Matrix3 R12 = transpose()*R2.rot_;
+  return Rot3(R12);
 }
 
 /* ************************************************************************* */
@@ -312,7 +316,7 @@ Quaternion Rot3::toQuaternion() const {
 /* ************************************************************************* */
 Point3 Rot3::unrotate(const Point3& p) const {
   // Eigen expression
-  return Point3(rot_.transpose()*p.vector()); // q = Rt*p
+  return Point3(transpose()*p.vector()); // q = Rt*p
 }
 
 /* ************************************************************************* */