feature: add inline get methods

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;
-}
-
-/* ************************************************************************* */
-
 }

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_;
+  }
+
   /// @}
 };
 

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);
 }
 
 //*******************************************************************************