From e439e1110f3898fc8068910bccf8603c60572064 Mon Sep 17 00:00:00 2001
From: David <davidwisth@gmail.com>
Date: Thu, 14 Jan 2021 11:56:34 +0000
Subject: [PATCH] Add getters to line3

---
 gtsam/geometry/Line3.h             | 21 +++++++++++++++++++++
 gtsam/geometry/tests/testLine3.cpp | 10 ++++++++++
 2 files changed, 31 insertions(+)

diff --git a/gtsam/geometry/Line3.h b/gtsam/geometry/Line3.h
index 1c7ed2f4c8..f70e13ca79 100644
--- a/gtsam/geometry/Line3.h
+++ b/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
diff --git a/gtsam/geometry/tests/testLine3.cpp b/gtsam/geometry/tests/testLine3.cpp
index 9ed12aef7d..09371bad4c 100644
--- a/gtsam/geometry/tests/testLine3.cpp
+++ b/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;