From ca9c66073feb8352c5448db9941949bf0e0b5cdd Mon Sep 17 00:00:00 2001
From: nsrinivasan7 <natesh.rvce@gmail.com>
Date: Sun, 30 Nov 2014 16:09:46 -0500
Subject: [PATCH] Rot2 rotate() and unrotate() changes to OptionalJacobians

---
 gtsam/base/Matrix.h               |  7 +++++++
 gtsam/geometry/Rot2.cpp           | 24 +++++++++++++++++-------
 gtsam/geometry/Rot2.h             | 10 +++++-----
 gtsam/geometry/tests/testRot2.cpp |  4 +++-
 4 files changed, 32 insertions(+), 13 deletions(-)

diff --git a/gtsam/base/Matrix.h b/gtsam/base/Matrix.h
index fcdfbb757..47203f9b5 100644
--- a/gtsam/base/Matrix.h
+++ b/gtsam/base/Matrix.h
@@ -43,6 +43,13 @@ typedef Eigen::Matrix<double,1,4> Matrix14;
 typedef Eigen::Matrix<double,1,5> Matrix15;
 typedef Eigen::Matrix<double,1,6> Matrix16;
 
+typedef Eigen::Matrix<double,2,1> Matrix21;
+typedef Eigen::Matrix<double,3,1> Matrix31;
+typedef Eigen::Matrix<double,4,1> Matrix41;
+typedef Eigen::Matrix<double,5,1> Matrix51;
+typedef Eigen::Matrix<double,6,1> Matrix61;
+
+
 typedef Eigen::Matrix2d Matrix2;
 typedef Eigen::Matrix3d Matrix3;
 typedef Eigen::Matrix4d Matrix4;
diff --git a/gtsam/geometry/Rot2.cpp b/gtsam/geometry/Rot2.cpp
index 41cf2dd05..08279450e 100644
--- a/gtsam/geometry/Rot2.cpp
+++ b/gtsam/geometry/Rot2.cpp
@@ -71,16 +71,22 @@ Matrix2 Rot2::matrix() const {
 }
 
 /* ************************************************************************* */
-Matrix Rot2::transpose() const {
-  return (Matrix(2, 2) <<  c_, s_, -s_, c_).finished();
+Matrix2 Rot2::transpose() const {
+  Matrix2 rvalue_;
+  rvalue_ <<   c_, s_, -s_, c_;
+  return rvalue_;
 }
 
 /* ************************************************************************* */
 // see doc/math.lyx, SO(2) section
-Point2 Rot2::rotate(const Point2& p, boost::optional<Matrix&> H1,
-    boost::optional<Matrix&> H2) const {
+Point2 Rot2::rotate(const Point2& p, OptionalJacobian<2, 1> H1,
+    OptionalJacobian<2, 2> H2) const {
   const Point2 q = Point2(c_ * p.x() + -s_ * p.y(), s_ * p.x() + c_ * p.y());
-  if (H1) *H1 = (Matrix(2, 1) <<  -q.y(), q.x()).finished();
+  if (H1) {
+      Matrix21 H1_;
+      H1_ <<  -q.y(), q.x();
+      *H1 = H1_;
+  }
   if (H2) *H2 = matrix();
   return q;
 }
@@ -88,9 +94,13 @@ Point2 Rot2::rotate(const Point2& p, boost::optional<Matrix&> H1,
 /* ************************************************************************* */
 // see doc/math.lyx, SO(2) section
 Point2 Rot2::unrotate(const Point2& p,
-    boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
+    OptionalJacobian<2, 1> H1, OptionalJacobian<2, 2> H2) const {
   const Point2 q = Point2(c_ * p.x() + s_ * p.y(), -s_ * p.x() + c_ * p.y());
-  if (H1) *H1 = (Matrix(2, 1) << q.y(), -q.x()).finished();  // R_{pi/2}q
+  if (H1) {
+      Matrix21 H1_;
+      H1_ << q.y(), -q.x();
+      *H1 = H1_;  // R_{pi/2}q
+  }
   if (H2) *H2 = transpose();
   return q;
 }
diff --git a/gtsam/geometry/Rot2.h b/gtsam/geometry/Rot2.h
index 2052bb603..2f9d1a398 100644
--- a/gtsam/geometry/Rot2.h
+++ b/gtsam/geometry/Rot2.h
@@ -180,8 +180,8 @@ namespace gtsam {
     /**
      * rotate point from rotated coordinate frame to world \f$ p^w = R_c^w p^c \f$
      */
-    Point2 rotate(const Point2& p, boost::optional<Matrix&> H1 = boost::none,
-        boost::optional<Matrix&> H2 = boost::none) const;
+    Point2 rotate(const Point2& p, OptionalJacobian<2, 1> H1 = boost::none,
+        OptionalJacobian<2, 2> H2 = boost::none) const;
 
     /** syntactic sugar for rotate */
     inline Point2 operator*(const Point2& p) const {
@@ -191,8 +191,8 @@ namespace gtsam {
     /**
      * rotate point from world to rotated frame \f$ p^c = (R_c^w)^T p^w \f$
      */
-    Point2 unrotate(const Point2& p, boost::optional<Matrix&> H1 = boost::none,
-        boost::optional<Matrix&> H2 = boost::none) const;
+    Point2 unrotate(const Point2& p, OptionalJacobian<2, 1> H1 = boost::none,
+        OptionalJacobian<2, 2> H2 = boost::none) const;
 
     /// @}
     /// @name Standard Interface
@@ -228,7 +228,7 @@ namespace gtsam {
     Matrix2 matrix() const;
 
     /** return 2*2 transpose (inverse) rotation matrix   */
-    Matrix transpose() const;
+    Matrix2 transpose() const;
 
   private:
     /** Serialization function */
diff --git a/gtsam/geometry/tests/testRot2.cpp b/gtsam/geometry/tests/testRot2.cpp
index 8a1f942d2..cfb103c5d 100644
--- a/gtsam/geometry/tests/testRot2.cpp
+++ b/gtsam/geometry/tests/testRot2.cpp
@@ -49,7 +49,9 @@ TEST( Rot2, unit)
 /* ************************************************************************* */
 TEST( Rot2, transpose)
 {
-  CHECK(assert_equal(R.inverse().matrix(),R.transpose()));
+  Matrix expected = R.inverse().matrix();
+  Matrix actual = R.transpose();
+  CHECK(assert_equal(expected,actual));
 }
 
 /* ************************************************************************* */