From b704b7b1a1e054e964b96901643076eab0f158a6 Mon Sep 17 00:00:00 2001
From: dellaert <dellaert@cc.gatech.edu>
Date: Tue, 7 Oct 2014 19:34:45 +0200
Subject: [PATCH] Faster versions

---
 gtsam/geometry/Pose3.cpp | 12 ++----------
 gtsam/geometry/Rot3.cpp  | 26 +++++++++++++++++++++++---
 gtsam/geometry/Rot3.h    | 15 ++++++++++-----
 gtsam/geometry/Rot3M.cpp |  6 ++++++
 gtsam/geometry/Rot3Q.cpp |  5 +++++
 5 files changed, 46 insertions(+), 18 deletions(-)

diff --git a/gtsam/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp
index f82c8e588..b7a0c1714 100644
--- a/gtsam/geometry/Pose3.cpp
+++ b/gtsam/geometry/Pose3.cpp
@@ -255,8 +255,6 @@ Point3 Pose3::transform_from(const Point3& p, boost::optional<Matrix&> Dpose,
 
 /* ************************************************************************* */
 Point3 Pose3::transform_to(const Point3& p) const {
-  // Only get transpose once, to avoid multiple allocations,
-  // as well as multiple conversions in the Quaternion case
   return R_.unrotate(p - t_);
 }
 
@@ -268,9 +266,7 @@ Point3 Pose3::transform_to(const Point3& p, boost::optional<Matrix36&> Dpose,
   Matrix3 Rt(R_.transpose());
   const Point3 q(Rt*(p - t_).vector());
   if (Dpose) {
-    double wx = q.x();
-    double wy = q.y();
-    double wz = q.z();
+    const double wx = q.x(), wy = q.y(), wz = q.z();
     (*Dpose) <<
         0.0, -wz, +wy,-1.0, 0.0, 0.0,
         +wz, 0.0, -wx, 0.0,-1.0, 0.0,
@@ -284,14 +280,10 @@ 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 {
-  // Only get transpose once, to avoid multiple allocations,
-  // as well as multiple conversions in the Quaternion case
   Matrix3 Rt(R_.transpose());
   const Point3 q(Rt*(p - t_).vector());
   if (Dpose) {
-    double wx = q.x();
-    double wy = q.y();
-    double wz = q.z();
+    const double wx = q.x(), wy = q.y(), wz = q.z();
     Dpose->resize(3, 6);
     (*Dpose) <<
         0.0, -wz, +wy,-1.0, 0.0, 0.0,
diff --git a/gtsam/geometry/Rot3.cpp b/gtsam/geometry/Rot3.cpp
index 37aa78a78..daa8eeda1 100644
--- a/gtsam/geometry/Rot3.cpp
+++ b/gtsam/geometry/Rot3.cpp
@@ -97,13 +97,33 @@ Unit3 Rot3::operator*(const Unit3& p) const {
   return rotate(p);
 }
 
+/* ************************************************************************* */
+// 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());
+  Point3 q(Rt * p.vector()); // q = Rt*p
+  const double wx = q.x(), wy = q.y(), wz = q.z();
+  if (H1)
+    *H1 << 0.0, -wz, +wy, +wz, 0.0, -wx, -wy, +wx, 0.0;
+  if (H2)
+    *H2 = Rt;
+  return q;
+}
+
 /* ************************************************************************* */
 // see doc/math.lyx, SO(3) section
 Point3 Rot3::unrotate(const Point3& p,
     boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
-  Point3 q(transpose()*p.vector()); // q = Rt*p
-  if (H1) *H1 = skewSymmetric(q.x(), q.y(), q.z());
-  if (H2) *H2 = transpose();
+  Matrix3 Rt(transpose());
+  Point3 q(Rt * p.vector()); // q = Rt*p
+  const double wx = q.x(), wy = q.y(), wz = q.z();
+  if (H1) {
+    H1->resize(3,3);
+    *H1 << 0.0, -wz, +wy, +wz, 0.0, -wx, -wy, +wx, 0.0;
+  }
+  if (H2)
+    *H2 = Rt;
   return q;
 }
 
diff --git a/gtsam/geometry/Rot3.h b/gtsam/geometry/Rot3.h
index fa9787076..115f288e3 100644
--- a/gtsam/geometry/Rot3.h
+++ b/gtsam/geometry/Rot3.h
@@ -335,11 +335,16 @@ namespace gtsam {
     /// rotate point from rotated coordinate frame to world = R*p
     Point3 operator*(const Point3& p) const;
 
-    /**
-     * rotate point from world to rotated frame \f$ p^c = (R_c^w)^T p^w \f$
-     */
-    Point3 unrotate(const Point3& p, boost::optional<Matrix&> H1 = boost::none,
-        boost::optional<Matrix&> H2 = boost::none) const;
+    /// rotate point from world to rotated frame \f$ p^c = (R_c^w)^T p^w \f$
+    Point3 unrotate(const Point3& p) const;
+
+    /// rotate point from world to rotated frame \f$ p^c = (R_c^w)^T p^w \f$
+    Point3 unrotate(const Point3& p, boost::optional<Matrix3&> H1,
+        boost::optional<Matrix3&> H2) const;
+
+    /// rotate point from world to rotated frame \f$ p^c = (R_c^w)^T p^w \f$
+    Point3 unrotate(const Point3& p, boost::optional<Matrix&> H1,
+        boost::optional<Matrix&> H2) const;
 
     /// @}
     /// @name Group Action on Unit3
diff --git a/gtsam/geometry/Rot3M.cpp b/gtsam/geometry/Rot3M.cpp
index 7db3acaa2..96ebcac08 100644
--- a/gtsam/geometry/Rot3M.cpp
+++ b/gtsam/geometry/Rot3M.cpp
@@ -314,6 +314,12 @@ Quaternion Rot3::toQuaternion() const {
   return Quaternion(rot_);
 }
 
+/* ************************************************************************* */
+Point3 Rot3::unrotate(const Point3& p) const {
+  // Eigen expression
+  return Point3(rot_.transpose()*p.vector()); // q = Rt*p
+}
+
 /* ************************************************************************* */
 
 } // namespace gtsam
diff --git a/gtsam/geometry/Rot3Q.cpp b/gtsam/geometry/Rot3Q.cpp
index dd0d39ffa..4344a623c 100644
--- a/gtsam/geometry/Rot3Q.cpp
+++ b/gtsam/geometry/Rot3Q.cpp
@@ -171,6 +171,11 @@ namespace gtsam {
   /* ************************************************************************* */
   Quaternion Rot3::toQuaternion() const { return quaternion_; }
 
+  /* ************************************************************************* */
+  Point3 Rot3::unrotate(const Point3& p) const {
+    return Point3(transpose()*p.vector()); // q = Rt*p
+  }
+
  /* ************************************************************************* */
 
 } // namespace gtsam