diff --git a/gtsam/geometry/OrientedPlane3.cpp b/gtsam/geometry/OrientedPlane3.cpp index b6594c29c..dff4eac9e 100644 --- a/gtsam/geometry/OrientedPlane3.cpp +++ b/gtsam/geometry/OrientedPlane3.cpp @@ -90,14 +90,4 @@ Vector3 OrientedPlane3::localCoordinates(const OrientedPlane3& y) const { return e; } -/* ************************************************************************* */ -Vector4 OrientedPlane3::planeCoefficients() const { - Vector unit_vec = n_.unitVector(); - 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 e75e32c96..8ff29b88a 100644 --- a/gtsam/geometry/OrientedPlane3.h +++ b/gtsam/geometry/OrientedPlane3.h @@ -108,12 +108,21 @@ public: Vector3 localCoordinates(const OrientedPlane3& s) const; /// Returns the plane coefficients - Vector4 planeCoefficients() const; + inline Vector4 planeCoefficients() const { + Vector3 unit_vec = n_.unitVector(); + return Vector4(unit_vec[0], unit_vec[1], unit_vec[2], d_); + } + /// Return the normal inline Unit3 normal() const { return n_; } + /// Return the perpendicular distance to the origin + inline double distance() const { + return d_; + } + /// @} }; diff --git a/gtsam/geometry/tests/testOrientedPlane3.cpp b/gtsam/geometry/tests/testOrientedPlane3.cpp index 8a0c2f846..c6189b970 100644 --- a/gtsam/geometry/tests/testOrientedPlane3.cpp +++ b/gtsam/geometry/tests/testOrientedPlane3.cpp @@ -35,10 +35,14 @@ TEST (OrientedPlane3, get) { c << -1, 0, 0, 5; OrientedPlane3 plane1(c); OrientedPlane3 plane2(c[0], c[1], c[2], c[3]); - Vector coefficient1 = plane1.planeCoefficients(); + Vector4 coefficient1 = plane1.planeCoefficients(); + double distance1 = plane1.distance(); EXPECT(assert_equal(coefficient1, c, 1e-8)); - Vector coefficient2 = plane2.planeCoefficients(); + EXPECT_DOUBLES_EQUAL(distance1, 5, 1e-8); + Vector4 coefficient2 = plane2.planeCoefficients(); + double distance2 = plane2.distance(); EXPECT(assert_equal(coefficient2, c, 1e-8)); + EXPECT_DOUBLES_EQUAL(distance2, 5, 1e-8); } //*******************************************************************************