From 89eec718dad11cf15ebcc7b471766ab2eb4f8080 Mon Sep 17 00:00:00 2001
From: zhaoyang <lvzhaoyang1990@gmail.com>
Date: Wed, 24 Jun 2015 22:34:38 -0400
Subject: [PATCH] change: matrix with fixed size in OrientedPlane3

---
 gtsam/geometry/OrientedPlane3.cpp | 14 ++++++--------
 gtsam/geometry/Unit3.h            |  2 +-
 2 files changed, 7 insertions(+), 9 deletions(-)

diff --git a/gtsam/geometry/OrientedPlane3.cpp b/gtsam/geometry/OrientedPlane3.cpp
index 48bd07382..f40ad1c49 100644
--- a/gtsam/geometry/OrientedPlane3.cpp
+++ b/gtsam/geometry/OrientedPlane3.cpp
@@ -35,15 +35,13 @@ void OrientedPlane3::print(const string& s) const {
 OrientedPlane3 OrientedPlane3::Transform(const OrientedPlane3& plane,
     const Pose3& xr, OptionalJacobian<3, 6> Hr,
     OptionalJacobian<3, 3> Hp) {
-  Matrix n_hr;
-  Matrix n_hp;
+  Matrix23 n_hr;
+  Matrix22 n_hp;
   Unit3 n_rotated = xr.rotation().unrotate(plane.n_, n_hr, n_hp);
 
   Vector3 n_unit = plane.n_.unitVector();
   Vector3 unit_vec = n_rotated.unitVector();
   double pred_d = n_unit.dot(xr.translation().vector()) + plane.d_;
-  OrientedPlane3 transformed_plane(unit_vec(0), unit_vec(1), unit_vec(2),
-      pred_d);
 
   if (Hr) {
     *Hr = zeros(3, 6);
@@ -51,16 +49,16 @@ OrientedPlane3 OrientedPlane3::Transform(const OrientedPlane3& plane,
     (*Hr).block<1, 3>(2, 3) = unit_vec;
   }
   if (Hp) {
-    Vector xrp = xr.translation().vector();
-    Matrix n_basis = plane.n_.basis();
-    Vector hpp = n_basis.transpose() * xrp;
+    Vector3 xrp = xr.translation().vector();
+    Matrix32 n_basis = plane.n_.basis();
+    Vector2 hpp = n_basis.transpose() * xrp;
     *Hp = zeros(3, 3);
     (*Hp).block<2, 2>(0, 0) = n_hp;
     (*Hp).block<1, 2>(2, 0) = hpp;
     (*Hp)(2, 2) = 1;
   }
 
-  return (transformed_plane);
+  return OrientedPlane3(unit_vec(0), unit_vec(1), unit_vec(2), pred_d);
 }
 
 /* ************************************************************************* */
diff --git a/gtsam/geometry/Unit3.h b/gtsam/geometry/Unit3.h
index 12bfac5ce..f8cea092e 100644
--- a/gtsam/geometry/Unit3.h
+++ b/gtsam/geometry/Unit3.h
@@ -22,7 +22,7 @@
 
 #include <gtsam/base/Manifold.h>
 #include <gtsam/geometry/Point3.h>
-#include <gtsam/base/DerivedValue.h>
+
 #include <boost/random/mersenne_twister.hpp>
 #include <boost/optional.hpp>