From ad2c9b66832abd411b4c54d1f154df2e01a3d10e Mon Sep 17 00:00:00 2001 From: akrishnan86 Date: Sat, 9 May 2020 08:28:44 -0400 Subject: [PATCH] additional jacobian for retract and local and unit tests --- gtsam/geometry/Line3.cpp | 47 +++++++++++++++++++----------- gtsam/geometry/Line3.h | 25 +++++++++++----- gtsam/geometry/tests/testLine3.cpp | 27 +++++++++++++++-- 3 files changed, 72 insertions(+), 27 deletions(-) diff --git a/gtsam/geometry/Line3.cpp b/gtsam/geometry/Line3.cpp index 577982b99..e3b4841e0 100644 --- a/gtsam/geometry/Line3.cpp +++ b/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(); - H->block<2, 2>(0, 0) = Dw.block<2, 2>(0, 0); + Dv->setIdentity(); + 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 { - Vector3 localR; - Vector4 local; - if (H) { - Matrix3 Dw; - localR = Rot3::Logmap(R_.inverse() * q.R_, Dw); - H->setIdentity(); - H->block<2, 2>(0, 0) = Dw.block<2, 2>(0, 0); - } else { - localR = Rot3::Logmap(R_.inverse() * q.R_); +Vector4 Line3::localCoordinates(const Line3 &q, OptionalJacobian<4, 4> Dp, + OptionalJacobian<4, 4> Dq) const { + Vector3 omega; + Matrix3 D_log; + omega = Rot3::Logmap(R_.inverse() * q.R_, D_log); + if (Dp) { + Matrix3 D_log_wp = -((q.R_).matrix()).transpose() * R_.matrix(); + Matrix3 Dwp = D_log * D_log_wp; + Dp->setIdentity(); + 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(); diff --git a/gtsam/geometry/Line3.h b/gtsam/geometry/Line3.h index fb0c878ea..1c7ed2f4c 100644 --- a/gtsam/geometry/Line3.h +++ b/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 - * @return: resulting line after adding the increment and mapping to the manifold + * @param Dp: increment of retraction with respect to this line + * @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. - * @param q Line3 on manifold - * @param H OptionalJacobian of localCoordinates with respect to line - * @return difference in the tangent space + * between two lines in the tangent space. It computes v = q - p where q is an input + * line, p is this line and v is their difference in the tangent space. + * @param q: Line3 on manifold + * @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 diff --git a/gtsam/geometry/tests/testLine3.cpp b/gtsam/geometry/tests/testLine3.cpp index 4593a31d8..9812c0c2d 100644 --- a/gtsam/geometry/tests/testLine3.cpp +++ 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; - r4 = r4.Expmap(Vector3(0, M_PI / 4, 0)); + Rot3 r4 = Rot3::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 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 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