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