API changes after review1

gtsam/geometry/Line3.cpp

@@ -4,94 +4,115 @@
 namespace gtsam {
 
 Line3 Line3::retract(const Vector4 &v, OptionalJacobian<4, 4> H) const {
-  Vector3 w;
+    Vector3 w;
-  w << v[0], v[1], 0;
+    w << v[0], v[1], 0;
-  Rot3 eps;
+    Rot3 eps;
-  if (H) {
+    if (H) {
-    OptionalJacobian<3, 3> Dw;
+        OptionalJacobian<3, 3> Dw;
-    eps = Rot3::Expmap(w, Dw);
+        Dw->setZero();
-    H->block<2, 2>(0, 0) = Dw->block<2, 2>(0, 0);
+        eps = Rot3::Expmap(w, Dw);
-    (*H)(2, 2) = 1;
+        H->block<2, 2>(0, 0) = Dw->block<2, 2>(0, 0);
-    (*H)(3, 3) = 1;
+        (*H)(2, 2) = 1;
-  } else {
+        (*H)(3, 3) = 1;
-    eps = Rot3::Expmap(w);
+    } else {
-  }
+        eps = Rot3::Expmap(w);
-  Rot3 Rt = R_ * eps;
+    }
-  return Line3(Rt, a_ + v[2], b_ + v[3]);
+    Rot3 Rt = R_ * eps;
+    return Line3(Rt, a_ + v[2], b_ + v[3]);
 }
 
 Vector4 Line3::localCoordinates(const Line3 &q, OptionalJacobian<4, 4> H) const {
-  Vector3 local_rot;
+    Vector3 local_rot;
-  Vector4 local;
+    Vector4 local;
-  Vector2 ab_q(q.V());
+    if (H) {
-  if (H) {
+        OptionalJacobian<3, 3> Dw;
-    OptionalJacobian<3, 3> Dw;
+        Dw->setZero();
-    local_rot = Rot3::Logmap(R_.inverse() * q.R(), Dw);
+        local_rot = Rot3::Logmap(R_.inverse() * q.R_, Dw);
-    H->block<2, 2>(0, 0) = Dw->block<2, 2>(0, 0);
+        H->block<2, 2>(0, 0) = Dw->block<2, 2>(0, 0);
-    (*H)(2, 2) = 1;
+        (*H)(2, 2) = 1;
-    (*H)(3, 3) = 1;
+        (*H)(3, 3) = 1;
-  } else {
+    } else {
-    local_rot = Rot3::Logmap(R_.inverse() * q.R());
+        local_rot = Rot3::Logmap(R_.inverse() * q.R_);
-  }
+    }
-  local << local_rot[0], local_rot[1], ab_q[0] - a_, ab_q[1] - b_;
+    local << local_rot[0], local_rot[1], q.a_ - a_, q.b_ - b_;
-  return local;
+    return local;
 }
 
 void Line3::print(const std::string &s) const {
-  std::cout << s << std::endl;
+    std::cout << s << std::endl;
-  R_.print("R:\n");
+    R_.print("R:\n");
-  std::cout << "a: " << a_ << ", b: " << b_ << std::endl;
+    std::cout << "a: " << a_ << ", b: " << b_ << std::endl;
 }
 
 bool Line3::equals(const Line3 &l2, double tol) const {
-  Vector4 diff = localCoordinates(l2);
+    Vector4 diff = localCoordinates(l2);
-  return fabs(diff[0]) < tol && fabs(diff[1]) < tol
+    return fabs(diff[0]) < tol && fabs(diff[1]) < tol
-      && fabs(diff[2]) < tol && fabs(diff[3]) < tol;
+        && fabs(diff[2]) < tol && fabs(diff[3]) < tol;
 }
 
-Point3 Line3::project(OptionalJacobian<3, 4> Dline) const {
+Unit3 Line3::project(OptionalJacobian<2, 4> Dline) const {
-  Vector3 V_0;
+    Vector3 V_0;
-  V_0 << -b_, a_, 0;
+    V_0 << -b_, a_, 0.0;
 
-  Point3 l = R_ * V_0;
+    Unit3 l;
-  if (Dline) {
+    if (Dline) {
-    Dline->setZero();
+        // Jacobian of the normalized Unit3 projected line with respect to
-    Dline->col(0) = a_ * R_.r3();
+        // un-normalized Vector3 projected line in homogeneous coordinates.
-    Dline->col(1) = b_ * R_.r3();
+        Eigen::Matrix<double, 2, 3> D_mat = Eigen::Matrix<double, 2, 3>::Zero();
-    Dline->col(2) = R_.r2();
+        OptionalJacobian<2, 3> D_unit_line(D_mat);
-    Dline->col(3) = -R_.r1();
+        l = Unit3::FromPoint3(Point3(R_ * V_0), D_unit_line);
-  }
+        // Jacobian of the un-normalized Vector3 line with respect to
-  return l;
+        // input 3D line
+        Eigen::Matrix<double, 3, 4> D_vec_line = Eigen::Matrix<double, 3, 4>::Zero();
+        D_vec_line.col(0) = a_ * R_.r3();
+        D_vec_line.col(1) = b_ * R_.r3();
+        D_vec_line.col(2) = R_.r2();
+        D_vec_line.col(3) = -R_.r1();
+        // Jacobian of output wrt input is the product of the two.
+        Eigen::Matrix<double, 2, 4> Dline_mat = (*D_unit_line) * D_vec_line;
+        Dline = OptionalJacobian<2, 4>(Dline_mat);
+    } else {
+        l = Unit3::FromPoint3(Point3(R_ * V_0));
+    }
+    return l;
+}
+
+Point3 Line3::point(int scale) const {
+    // defining "center" of the line to be the point where it
+    // intercepts rotated XY axis
+    Point3 center(a_, b_, 0);
+    Point3 rotated_center = R_ * center;
+    Point3 direction = (R_.r3()).normalized();
+    return rotated_center + scale * direction;
 }
 
 Line3 transformTo(const Pose3 &wTc, const Line3 &wL,
                   OptionalJacobian<4, 6> Dpose, OptionalJacobian<4, 4> Dline) {
-  Rot3 wRl = wL.R();
+    Rot3 wRc = wTc.rotation();
-  Rot3 wRc = wTc.rotation();
+    Rot3 cRw = wRc.inverse();
-  Rot3 cRw = wRc.inverse();
+    Rot3 cRl = cRw * wL.R_;
-  Rot3 cRl = cRw * wRl;
 
-  Vector2 w_ab = wL.V();
+    Vector2 w_ab;
-  Vector3 t = (wRl.transpose() * wTc.translation());
+    Vector3 t = ((wL.R_).transpose() * wTc.translation());
-  Vector2 c_ab(w_ab[0] - t[0], w_ab[1] - t[1]);
+    Vector2 c_ab(wL.a_ - t[0], wL.b_ - t[1]);
 
-  if (Dpose) {
+    if (Dpose) {
-    // translation due to translation
+        // translation due to translation
-    Matrix3 cRl_mat = cRl.matrix();
+        Matrix3 cRl_mat = cRl.matrix();
-    Matrix3 lRc = cRl_mat.transpose();
+        Matrix3 lRc = cRl_mat.transpose();
-    Dpose->block<1, 3>(2, 3) = -lRc.row(0);
+        Dpose->block<1, 3>(2, 3) = -lRc.row(0);
-    Dpose->block<1, 3>(3, 3) = -lRc.row(1);
+        Dpose->block<1, 3>(3, 3) = -lRc.row(1);
 
-    Dpose->block<1, 3>(0, 0) = -lRc.row(0);
+        Dpose->block<1, 3>(0, 0) = -lRc.row(0);
-    Dpose->block<1, 3>(1, 0) = -lRc.row(1);
+        Dpose->block<1, 3>(1, 0) = -lRc.row(1);
-  }
+    }
-  if (Dline) {
+    if (Dline) {
-    Dline->col(0) << 1.0, 0.0, 0.0, -t[2];
+        Dline->col(0) << 1.0, 0.0, 0.0, -t[2];
-    Dline->col(1) << 0.0, 1.0, t[2], 0.0;
+        Dline->col(1) << 0.0, 1.0, t[2], 0.0;
-    Dline->col(2) << 0.0, 0.0, 1.0, 0.0;
+        Dline->col(2) << 0.0, 0.0, 1.0, 0.0;
-    Dline->col(3) << 0.0, 0.0, 0.0, 1.0;
+        Dline->col(3) << 0.0, 0.0, 0.0, 1.0;
-  }
+    }
-  return Line3(cRl, c_ab[0], c_ab[1]);
+    return Line3(cRl, c_ab[0], c_ab[1]);
 }
 
 }

gtsam/geometry/Line3.h

@@ -28,90 +28,83 @@ namespace gtsam {
  * \nosubgrouping
  */
 class Line3 {
- private:
+private:
-  Rot3 R_;    // Rotation of line about x and y in world frame
+    Rot3 R_;    // Rotation of line about x and y in world frame
-  double a_, b_;  // Intersection of line with the world x-y plane rotated by R_
+    double a_, b_;  // Intersection of line with the world x-y plane rotated by R_
-                  // Also the closest point on line to origin
+    // Also the closest point on line to origin
- public:
+public:
-  enum { dimension = 4 };
+    enum { dimension = 4 };
 
-  /** Default constructor is the Z axis **/
+    /** Default constructor is the Z axis **/
-  Line3() :
+    Line3() :
-      a_(0), b_(0) {}
+        a_(0), b_(0) {}
 
-  /** Constructor:
+    /** Constructor for general line from (R, a, b) **/
-   * Parallel to z axis, intersecting x-y plane at (a,b) **/
+    Line3(const Rot3 &R, const double a, const double b) :
-  Line3(const double a, const double b) :
+        R_(R), a_(a), b_(b) {}
-      a_(a), b_(b) {}
 
-  /** Constructor for general line from (R, a, b) **/
+    /**
-  Line3(const Rot3 &R, const double a, const double b) :
+     * The retract method maps from the tangent space back to the manifold.
-      R_(R), a_(a), b_(b) {}
+     * The tangent space for the rotation of a line is only two dimensional -
+     * rotation about x and y
+     * @param v: increment in tangent space
+     * @param H: Jacobian of retraction with respect to the increment
+     * @return: resulting line after adding the increment and mapping to the manifold
+     */
+    Line3 retract(const Vector4 &v, OptionalJacobian<4, 4> H = boost::none) const;
 
-  /**
+    /**
-   * The retract method maps from the tangent space back to the manifold.
+     * The localCoordinates method is the inverse of retract and finds the difference
-   * The tangent space for the rotation of a line is only two dimensional -
+     * between two lines in the tangent space.
-   * rotation about x and y
+     * @param q Line3 on manifold
-   * @param v: increment in tangent space
+     * @param H OptionalJacobian of localCoordinates with respect to line
-   * @param H: Jacobian of retraction with respect to the increment
+     * @return difference in the tangent space
-   * @return: resulting line after adding the increment and mapping to the manifold
+     */
-   */
+    Vector4 localCoordinates(const Line3 &q, OptionalJacobian<4, 4> H = boost::none) const;
-  Line3 retract(const Vector4 &v, OptionalJacobian<4, 4> H = boost::none) const;
 
-  /**
+    /**
-   * The localCoordinates method is the inverse of retract and finds the difference
+     * Print R, a, b
-   * between two lines in the tangent space.
+     * @param s: optional starting string
-   * @param q Line3 on manifold
+     */
-   * @param H OptionalJacobian of localCoordinates with respect to line
+    void print(const std::string &s = "") const;
-   * @return difference in the tangent space
-   */
-  Vector4 localCoordinates(const Line3 &q, OptionalJacobian<4, 4> H = boost::none) const;
 
-  /**
+    /**
-   * Rotation of line accessor
+     * Check if two lines are equal
-   * @return Rot3
+     * @param l2 - line to be compared
-   */
+     * @param tol : optional tolerance
-  Rot3 R() const {
+     * @return boolean - true if lines are equal
-    return R_;
+     */
-  }
+    bool equals(const Line3 &l2, double tol = 10e-9) const;
 
-  /**
+    /**
-   * Accessor for a, b
+     * Projecting a line to the image plane. Assumes this line is in camera frame.
-   * @return Vector2(a, b)
+     * @param Dline: OptionalJacobian of projected line with respect to this line
-   */
+     * @return Unit3 - projected line in image plane, in homogenous coordinates.
-  Vector2 V() const {
+     * We use Unit3 since it is a manifold with the right dimension.
-    return Vector2(a_, b_);
+     */
-  }
+    Unit3 project(OptionalJacobian<2, 4> Dline = boost::none) const;
 
-  /**
+    /**
-   * Print R, a, b
+     * Returns point on the line that is starting from the point where the rotated XY axis
-   * @param s: optional starting string
+     * intersects the line, in the direction of the line.
-   */
+     * @param scale
-  void print(const std::string &s = "") const;
+     * @return
+     */
+    Point3 point(int scale = 0) const;
 
-  /**
+    /**
-   * Check if two lines are equal
+     * Transform a line from world to camera frame
-   * @param l2 - line to be compared
+     * @param wTc - Pose3 of camera in world frame
-   * @param tol : optional tolerance
+     * @param wL - Line3 in world frame
-   * @return boolean - true if lines are equal
+     * @param Dpose - OptionalJacobian of transformed line with respect to p
-   */
+     * @param Dline -  OptionalJacobian of transformed line with respect to l
-  bool equals(const Line3 &l2, double tol = 10e-9) const;
+     * @return Transformed line in camera frame
-
+     */
-  /**
+    friend Line3 transformTo(const Pose3 &wTc, const Line3 &wL,
-   * Projecting a line to the image plane. Assumes this line is in camera frame.
+                             OptionalJacobian<4, 6> Dpose,
-   * @param Dline: OptionalJacobian of projected line with respect to this line
+                             OptionalJacobian<4, 4> Dline);
-   * @return Unit3 - projected line in image plane, in homogenous coordinates.
-   * We use Unit3 since it is a manifold with the right dimension.
-   */
-  Point3 project(OptionalJacobian<3, 4> Dline = boost::none) const;
 };
 
-template<>
-struct traits<Line3> : public internal::Manifold<Line3> {};
-
-template<>
-struct traits<const Line3> : public internal::Manifold<Line3> {};
-
 /**
  * Transform a line from world to camera frame
  * @param wTc - Pose3 of camera in world frame
@@ -123,4 +116,10 @@ struct traits<const Line3> : public internal::Manifold<Line3> {};
 Line3 transformTo(const Pose3 &wTc, const Line3 &wL,
                   OptionalJacobian<4, 6> Dpose = boost::none,
                   OptionalJacobian<4, 4> Dline = boost::none);
+
+template<>
+struct traits<Line3> : public internal::Manifold<Line3> {};
+
+template<>
+struct traits<const Line3> : public internal::Manifold<Line3> {};
 }

gtsam/geometry/tests/testLine3.cpp

@@ -7,112 +7,99 @@
 #include <gtsam/nonlinear/ExpressionFactor.h>
 #include <gtsam/nonlinear/expressionTesting.h>
 
-
 using namespace gtsam;
 
 GTSAM_CONCEPT_TESTABLE_INST(Line3)
 GTSAM_CONCEPT_MANIFOLD_INST(Line3)
 
-static const Line3 l(1, 1);
+static const Line3 l(Rot3(), 1, 1);
 
 // Testing equals function of Line3
-TEST(Line3, equals)
+TEST(Line3, equals) {
-{
     Line3 l_same = l;
     EXPECT(l.equals(l_same));
-    Line3 l2(1, 2);
+    Line3 l2(Rot3(), 1, 2);
     EXPECT(!l.equals(l2));
 }
 
 
 // testing localCoordinates along 4 dimensions
-TEST(Line3, localCoordinates)
+TEST(Line3, localCoordinates) {
-{
     // l1 and l differ only in a_
-    Line3 l1(2, 1);
+    Line3 l1(Rot3(), 2, 1);
     Vector4 v1(0, 0, -1, 0);
     CHECK(assert_equal(l1.localCoordinates(l), v1));
 
     // l2 and l differ only in b_
-    Line3 l2(1, 2);
+    Line3 l2(Rot3(), 1, 2);
     Vector4 v2(0, 0, 0, -1);
     CHECK(assert_equal(l2.localCoordinates(l), v2));
 
     // l3 and l differ in R_x
-    Rot3 r3;
+    Rot3 r3 = Rot3::Expmap(Vector3(M_PI / 4, 0, 0));
-    r3 = r3.Expmap(Vector3(M_PI/4, 0, 0));
     Line3 l3(r3, 1, 1);
-    Vector4 v3(-M_PI/4, 0, 0, 0);
+    Vector4 v3(-M_PI / 4, 0, 0, 0);
     CHECK(assert_equal(l3.localCoordinates(l), v3));
 
     // l4 and l differ in R_y
     Rot3 r4;
-    r4 = r4.Expmap(Vector3(0, M_PI/4, 0));
+    r4 = r4.Expmap(Vector3(0, M_PI / 4, 0));
     Line3 l4(r4, 1, 1);
-    Vector4 v4(0, -M_PI/4, 0, 0);
+    Vector4 v4(0, -M_PI / 4, 0, 0);
     CHECK(assert_equal(l4.localCoordinates(l), v4));
 }
 
 // testing retract along 4 dimensions
-TEST(Line3, retract)
+TEST(Line3, retract) {
-{
     // l1 and l differ only in a_
     Vector4 v1(0, 0, 0, 1);
-    Line3 l1(1,2);
+    Line3 l1(Rot3(), 1, 2);
     EXPECT(l1.equals(l.retract(v1)));
 
     // l2 and l differ only in b_
     Vector4 v2(0, 0, 1, 0);
-    Line3 l2(2,1);
+    Line3 l2(Rot3(), 2, 1);
     EXPECT(l2.equals(l.retract(v2)));
 
     // l3 and l differ in R_x
-    Vector4 v3(M_PI/4, 0, 0, 0);
+    Vector4 v3(M_PI / 4, 0, 0, 0);
     Rot3 r3;
-    r3 = r3.Expmap(Vector3(M_PI/4, 0, 0));
+    r3 = r3.Expmap(Vector3(M_PI / 4, 0, 0));
     Line3 l3(r3, 1, 1);
     EXPECT(l3.equals(l.retract(v3)));
 
     // l4 and l differ in R_y
-    Vector4 v4(0, M_PI/4, 0, 0);
+    Vector4 v4(0, M_PI / 4, 0, 0);
-    Rot3 r4;
+    Rot3 r4 = Rot3::Expmap(Vector3(0, M_PI / 4, 0));
-    r4 = r4.Expmap(Vector3(0, M_PI/4, 0));
     Line3 l4(r4, 1, 1);
     EXPECT(l4.equals(l.retract(v4)));
 }
 
 // testing manifold property - Retract(p, Local(p,q)) == q
-TEST(Line3, retractOfLocalCoordinates)
+TEST(Line3, retractOfLocalCoordinates) {
-{
+    Rot3 r2 = Rot3::Expmap(Vector3(M_PI / 4, M_PI / 3, 0));
-    Rot3 r2;
-    r2.Expmap(Vector3(M_PI/4, M_PI/3, 0));
     Line3 l2(r2, 5, 9);
     EXPECT(assert_equal(l.retract(l.localCoordinates(l2)), l2));
 }
 
 // testing manifold property - Local(p, Retract(p, v)) == v
-TEST(Line3, localCoordinatesOfRetract)
+TEST(Line3, localCoordinatesOfRetract) {
-{
     Vector4 r2(2.3, 0.987, -3, 4);
     EXPECT(assert_equal(l.localCoordinates(l.retract(r2)), r2));
 }
 
 // transform from world to camera test
-TEST(Line3, transformToExpressionJacobians)
+TEST(Line3, transformToExpressionJacobians) {
-{
+    Rot3 r = Rot3::Expmap(Vector3(0, M_PI / 3, 0));
-    Rot3 I;
-    Rot3 r = I.Expmap(Vector3(0, M_PI/3, 0));
     Vector3 t(0, 0, 0);
     Pose3 p(r, t);
 
     Line3 l_c(r.inverse(), 1, 1);
-    Line3 l_w(1,1);
+    Line3 l_w(Rot3(), 1, 1);
-
     EXPECT(l_c.equals(transformTo(p, l_w)));
 
     Line3_ l_(1);
     Pose3_ p_(2);
-
     Values val;
     val.insert(1, l_w);
     val.insert(2, p);
@@ -124,26 +111,23 @@ TEST(Line3, transformToExpressionJacobians)
 
 
 // projection in camera frame test
-TEST(Line3, projection)
+TEST(Line3, projection) {
-{
+    Rot3 r = Rot3::Expmap(Vector3(0, 0, 0));
-    Rot3 r;
-    r = r.Expmap(Vector3(0, 0, 0));
     Line3 wL(r, 1, 1);
 
-    Point3 expected(-1, 1, 0);
+    Unit3 expected = Unit3::FromPoint3(Point3(-1, 1, 0));
     EXPECT(expected.equals(wL.project()));
 
     Values val;
     val.insert(1, wL);
     Line3_ wL_(1);
 
-    SharedNoiseModel model = noiseModel::Isotropic::Sigma(3, 0.1);
+    SharedNoiseModel model = noiseModel::Isotropic::Sigma(2, 0.1);
-    Point3_ projected_(wL_, &Line3::project);
+    Unit3_ projected_(wL_, &Line3::project);
-    ExpressionFactor<Point3> f(model, expected, projected_);
+    ExpressionFactor<Unit3> f(model, expected, projected_);
     EXPECT_CORRECT_FACTOR_JACOBIANS(f, val, 1e-5, 1e-7);
 }
 
-
 int main() {
     TestResult tr;
     return TestRegistry::runAllTests(tr);