diff --git a/gtsam/geometry/Line3.cpp b/gtsam/geometry/Line3.cpp index a61cfc251..4982df7b1 100644 --- a/gtsam/geometry/Line3.cpp +++ b/gtsam/geometry/Line3.cpp @@ -78,13 +78,12 @@ Unit3 Line3::project(OptionalJacobian<2, 4> Dline) const { return l; } -Point3 Line3::point(int scale) const { +Point3 Line3::point(double distance) const { // defining "center" of the line to be the point where it - // intercepts rotated XY axis + // intersects rotated XY axis Point3 center(a_, b_, 0); Point3 rotated_center = R_ * center; - Point3 direction = (R_.r3()).normalized(); - return rotated_center + scale * direction; + return rotated_center + distance * R_.r3(); } Line3 transformTo(const Pose3 &wTc, const Line3 &wL, diff --git a/gtsam/geometry/Line3.h b/gtsam/geometry/Line3.h index 06637319e..a176039d5 100644 --- a/gtsam/geometry/Line3.h +++ b/gtsam/geometry/Line3.h @@ -84,12 +84,14 @@ public: Unit3 project(OptionalJacobian<2, 4> Dline = boost::none) const; /** - * Returns point on the line that is at a distance "scalestarting from the point where the rotated XY axis - * intersects the line, . - * @param scale - * @return + * Returns point on the line that is at a certain distance starting from the + * point where the rotated XY axis intersects the line. + * @param distance (can be positive or negative) - positive is the positive + * direction of the rotated z axis that forms the line. The default value of zero + * returns the point where the rotated XY axis intersects the line. + * @return Point on the line */ - Point3 point(int scale = 0) const; + Point3 point(double distance = 0) const; /** * Transform a line from world to camera frame