From ff14e576ee27e0db81c61b716a6c3ef02d3b4984 Mon Sep 17 00:00:00 2001
From: zhaoyang <lvzhaoyang1990@gmail.com>
Date: Wed, 24 Jun 2015 16:18:33 -0400
Subject: [PATCH] fix: return plane coefficents dimension was wrong, should be
 Vector4, not Vector3

---
 gtsam/geometry/OrientedPlane3.cpp           |  4 ++--
 gtsam/geometry/OrientedPlane3.h             |  2 +-
 gtsam/geometry/tests/testOrientedPlane3.cpp | 12 ++++++++++++
 3 files changed, 15 insertions(+), 3 deletions(-)

diff --git a/gtsam/geometry/OrientedPlane3.cpp b/gtsam/geometry/OrientedPlane3.cpp
index e96708942..b6594c29c 100644
--- a/gtsam/geometry/OrientedPlane3.cpp
+++ b/gtsam/geometry/OrientedPlane3.cpp
@@ -91,9 +91,9 @@ Vector3 OrientedPlane3::localCoordinates(const OrientedPlane3& y) const {
 }
 
 /* ************************************************************************* */
-Vector3 OrientedPlane3::planeCoefficients() const {
+Vector4 OrientedPlane3::planeCoefficients() const {
   Vector unit_vec = n_.unitVector();
-  Vector3 a;
+  Vector4 a;
   a << unit_vec[0], unit_vec[1], unit_vec[2], d_;
   return a;
 }
diff --git a/gtsam/geometry/OrientedPlane3.h b/gtsam/geometry/OrientedPlane3.h
index 661577739..e75e32c96 100644
--- a/gtsam/geometry/OrientedPlane3.h
+++ b/gtsam/geometry/OrientedPlane3.h
@@ -108,7 +108,7 @@ public:
   Vector3 localCoordinates(const OrientedPlane3& s) const;
 
   /// Returns the plane coefficients
-  Vector3 planeCoefficients() const;
+  Vector4 planeCoefficients() const;
 
   inline Unit3 normal() const {
     return n_;
diff --git a/gtsam/geometry/tests/testOrientedPlane3.cpp b/gtsam/geometry/tests/testOrientedPlane3.cpp
index b2b4ecc43..8a0c2f846 100644
--- a/gtsam/geometry/tests/testOrientedPlane3.cpp
+++ b/gtsam/geometry/tests/testOrientedPlane3.cpp
@@ -29,6 +29,18 @@ using boost::none;
 GTSAM_CONCEPT_TESTABLE_INST(OrientedPlane3)
 GTSAM_CONCEPT_MANIFOLD_INST(OrientedPlane3)
 
+//*******************************************************************************
+TEST (OrientedPlane3, get) {
+  Vector4 c;
+  c << -1, 0, 0, 5;
+  OrientedPlane3 plane1(c);
+  OrientedPlane3 plane2(c[0], c[1], c[2], c[3]);
+  Vector coefficient1 = plane1.planeCoefficients();
+  EXPECT(assert_equal(coefficient1, c, 1e-8));
+  Vector coefficient2 = plane2.planeCoefficients();
+  EXPECT(assert_equal(coefficient2, c, 1e-8));
+}
+
 //*******************************************************************************
 TEST (OrientedPlane3, transform) {
   // Test transforming a plane to a pose