Merge pull request #668 from dwisth/feature/add-getters-to-line3

Add getters to Line3

gtsam/geometry/Line3.h

@@ -102,6 +102,27 @@ class Line3 {
    */
   Point3 point(double distance = 0) const;
 
+  /**
+   * Return the rotation of the line.
+   */
+  inline Rot3 R() const {
+    return R_;
+  }
+
+  /**
+   * Return the x-coordinate of the intersection of the line with the xy plane.
+   */
+  inline double a() const {
+    return a_;
+  }
+
+  /**
+   * Return the y-coordinate of the intersection of the line with the xy plane.
+   */
+  inline double b() const {
+    return b_;
+  }
+
   /**
    * Transform a line from world to camera frame
    * @param wTc - Pose3 of camera in world frame

gtsam/geometry/tests/testLine3.cpp

@@ -14,6 +14,16 @@ GTSAM_CONCEPT_MANIFOLD_INST(Line3)
 
 static const Line3 l(Rot3(), 1, 1);
 
+// Testing getters
+TEST(Line3, getMethods) {
+  const double a = 5, b = 10;
+  const Rot3 R = Rot3::Expmap(Vector3(0.1, 0.2, 0.3));
+  const Line3 line(R, a, b);
+  EXPECT_DOUBLES_EQUAL(a, line.a(), 1e-8);
+  EXPECT_DOUBLES_EQUAL(b, line.b(), 1e-8);
+  EXPECT(assert_equal(R, line.R(), 1e-8));
+}
+
 // Testing equals function of Line3
 TEST(Line3, equals) {
   Line3 l_same = l;