additional jacobian for retract and local and unit tests

gtsam/geometry/Line3.cpp

@@ -2,15 +2,21 @@
 
 namespace gtsam {
 
-Line3 Line3::retract(const Vector4 &v, OptionalJacobian<4, 4> H) const {
+Line3 Line3::retract(const Vector4 &v, OptionalJacobian<4, 4> Dp, OptionalJacobian<4, 4> Dv) const {
   Vector3 w;
   w << v[0], v[1], 0;
   Rot3 incR;
-  if (H) {
+
+  if (Dp) {
+    Dp->setIdentity();
+    incR = Rot3::Expmap(w);
+    Dp->block<2, 2>(0, 0) = ((incR.matrix()).transpose()).block<2, 2>(0, 0);
+  }
+  if (Dv) {
     Matrix3 Dw;
     incR = Rot3::Expmap(w, Dw);
-    H->setIdentity();
+    Dv->setIdentity();
-    H->block<2, 2>(0, 0) = Dw.block<2, 2>(0, 0);
+    Dv->block<2, 2>(0, 0) = Dw.block<2, 2>(0, 0);
   } else {
     incR = Rot3::Expmap(w);
   }
@@ -18,18 +24,25 @@ Line3 Line3::retract(const Vector4 &v, OptionalJacobian<4, 4> H) const {
   return Line3(Rt, a_ + v[2], b_ + v[3]);
 }
 
-Vector4 Line3::localCoordinates(const Line3 &q, OptionalJacobian<4, 4> H) const {
+Vector4 Line3::localCoordinates(const Line3 &q, OptionalJacobian<4, 4> Dp,
-  Vector3 localR;
+                                OptionalJacobian<4, 4> Dq) const {
-  Vector4 local;
+  Vector3 omega;
-  if (H) {
+  Matrix3 D_log;
-    Matrix3 Dw;
+  omega = Rot3::Logmap(R_.inverse() * q.R_, D_log);
-    localR = Rot3::Logmap(R_.inverse() * q.R_, Dw);
+  if (Dp) {
-    H->setIdentity();
+    Matrix3 D_log_wp = -((q.R_).matrix()).transpose() * R_.matrix();
-    H->block<2, 2>(0, 0) = Dw.block<2, 2>(0, 0);
+    Matrix3 Dwp = D_log * D_log_wp;
-  } else {
+    Dp->setIdentity();
-    localR = Rot3::Logmap(R_.inverse() * q.R_);
+    Dp->block<2, 2>(0, 0) = Dwp.block<2, 2>(0, 0);
+    (*Dp)(2, 2) = -1;
+    (*Dp)(3, 3) = -1;
   }
-  local << localR[0], localR[1], q.a_ - a_, q.b_ - b_;
+  if (Dq) {
+    Dq->setIdentity();
+    Dq->block<2, 2>(0, 0) = D_log.block<2, 2>(0, 0);
+  }
+  Vector4 local;
+  local << omega[0], omega[1], q.a_ - a_, q.b_ - b_;
   return local;
 }
 
@@ -92,9 +105,9 @@ Line3 transformTo(const Pose3 &wTc, const Line3 &wL,
     Matrix3 lRc = (cRl.matrix()).transpose();
     Dpose->setZero();
     // rotation
-    Dpose->block<2, 3>(0, 0) = -lRc.block<2,3>(0, 0);
+    Dpose->block<2, 3>(0, 0) = -lRc.block<2, 3>(0, 0);
     // translation
-    Dpose->block<2, 3>(2, 3) = -lRc.block<2,3>(0, 0);
+    Dpose->block<2, 3>(2, 3) = -lRc.block<2, 3>(0, 0);
   }
   if (Dline) {
     Dline->setIdentity();

gtsam/geometry/Line3.h

@@ -44,22 +44,31 @@ class Line3 {
 
   /**
    * The retract method maps from the tangent space back to the manifold.
+   * The method q = p + v, where p is this line, v is an increment along
+   * the tangent space and q is the resulting line.
    * 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
+   * @param Dp: increment of retraction with respect to this line
-   * @return: resulting line after adding the increment and mapping to the manifold
+   * @param Dv: Jacobian of retraction with respect to the increment
+   * @return q: resulting line after adding the increment and mapping to the manifold
    */
-  Line3 retract(const Vector4 &v, OptionalJacobian<4, 4> H = boost::none) const;
+  Line3 retract(const Vector4 &v,
+                OptionalJacobian<4, 4> Dp = boost::none,
+                OptionalJacobian<4, 4> Dv = boost::none) const;
 
   /**
    * The localCoordinates method is the inverse of retract and finds the difference
-   * between two lines in the tangent space.
+   * between two lines in the tangent space. It computes v = q - p where q is an input
-   * @param q Line3 on manifold
+   * line, p is this line and v is their difference in the tangent space.
-   * @param H OptionalJacobian of localCoordinates with respect to line
+   * @param q: Line3 on manifold
-   * @return difference in the tangent space
+   * @param Dp: OptionalJacobian of localCoordinates with respect to this line
+   * @param Dq: OptionalJacobian of localCoordinates with respect to this line
+   * @return v: difference in the tangent space
    */
-  Vector4 localCoordinates(const Line3 &q, OptionalJacobian<4, 4> H = boost::none) const;
+  Vector4 localCoordinates(const Line3 &q,
+                           OptionalJacobian<4, 4> Dp = boost::none,
+                           OptionalJacobian<4, 4> Dq = boost::none) const;
 
   /**
    * Print R, a, b

gtsam/geometry/tests/testLine3.cpp

@@ -41,11 +41,22 @@ TEST(Line3, localCoordinates) {
   CHECK(assert_equal(l3.localCoordinates(l), v3));
 
   // l4 and l differ in R_y
-  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);
   Vector4 v4(0, -M_PI / 4, 0, 0);
   CHECK(assert_equal(l4.localCoordinates(l), v4));
+
+  // Jacobians
+  Line3 l5(Rot3::Expmap(Vector3(M_PI / 3, -M_PI / 4, 0)), -0.8, 2);
+  Values val;
+  val.insert(1, l);
+  val.insert(2, l5);
+  Line3_ l_(1);
+  Line3_ l5_(2);
+  SharedNoiseModel model = noiseModel::Isotropic::Sigma(4, 0.1);
+  Vector4_ local_(l5_, &Line3::localCoordinates, l_);
+  ExpressionFactor<Vector4> f(model, l5.localCoordinates(l), local_);
+  EXPECT_CORRECT_FACTOR_JACOBIANS(f, val, 1e-5, 1e-7);
 }
 
 // testing retract along 4 dimensions
@@ -72,6 +83,18 @@ TEST(Line3, retract) {
   Rot3 r4 = Rot3::Expmap(Vector3(0, M_PI / 4, 0));
   Line3 l4(r4, 1, 1);
   EXPECT(l4.equals(l.retract(v4)));
+
+  // Jacobians
+  Vector4 v5(M_PI / 3, -M_PI / 4, -0.4, 1.2); // arbitrary vector
+  Values val;
+  val.insert(1, l);
+  val.insert(2, v5);
+  Line3_ l_(1);
+  Vector4_ v5_(2);
+  SharedNoiseModel model = noiseModel::Isotropic::Sigma(4, 0.1);
+  Line3_ retract_(l_, &Line3::retract, v5_);
+  ExpressionFactor<Line3> f(model, l.retract(v5), retract_);
+  EXPECT_CORRECT_FACTOR_JACOBIANS(f, val, 1e-5, 1e-7);
 }
 
 // testing manifold property - Retract(p, Local(p,q)) == q