From 2b5554ca1033c9d7b7ed73733ca32d4c23fd6892 Mon Sep 17 00:00:00 2001
From: Frank Dellaert <frank@skyd.io>
Date: Sun, 20 Dec 2015 16:59:33 -0800
Subject: [PATCH] Small comments

---
 gtsam/geometry/tests/testPose3.cpp |  3 ++-
 gtsam/geometry/tests/testRot3.cpp  | 19 ++++++++++++++++++-
 2 files changed, 20 insertions(+), 2 deletions(-)

diff --git a/gtsam/geometry/tests/testPose3.cpp b/gtsam/geometry/tests/testPose3.cpp
index 3507dc7af..b0d6c082d 100644
--- a/gtsam/geometry/tests/testPose3.cpp
+++ b/gtsam/geometry/tests/testPose3.cpp
@@ -717,9 +717,10 @@ TEST( Pose3, ExpmapDerivative1) {
 /* ************************************************************************* */
 TEST(Pose3, ExpmapDerivative2) {
   // Iserles05an (Lie-group Methods) says:
-  // scalar is easy: d exp(a(t)) / dt = exp(a(t) a'(t)
+  // scalar is easy: d exp(a(t)) / dt = exp(a(t)) a'(t)
   // matrix is hard: d exp(A(t)) / dt = exp(A(t)) dexp[-A(t)] A'(t)
   // where A(t): T -> se(3) is a trajectory in the tangent space of SE(3)
+  // and dexp[A] is a linear map from 4*4 to 4*4 derivatives of se(3)
   // Hence, the above matrix equation is typed: 4*4 = SE(3) * linear_map(4*4)
 
   // In GTSAM, we don't work with the Lie-algebra elements A directly, but with 6-vectors.
diff --git a/gtsam/geometry/tests/testRot3.cpp b/gtsam/geometry/tests/testRot3.cpp
index a70ff9fc3..eb6573c30 100644
--- a/gtsam/geometry/tests/testRot3.cpp
+++ b/gtsam/geometry/tests/testRot3.cpp
@@ -284,9 +284,10 @@ TEST( Rot3, ExpmapDerivative2)
 /* ************************************************************************* */
 TEST(Rot3, ExpmapDerivative3) {
   // Iserles05an (Lie-group Methods) says:
-  // scalar is easy: d exp(a(t)) / dt = exp(a(t) a'(t)
+  // scalar is easy: d exp(a(t)) / dt = exp(a(t)) a'(t)
   // matrix is hard: d exp(A(t)) / dt = exp(A(t)) dexp[-A(t)] A'(t)
   // where A(t): R -> so(3) is a trajectory in the tangent space of SO(3)
+  // and dexp[A] is a linear map from 3*3 to 3*3 derivatives of se(3)
   // Hence, the above matrix equation is typed: 3*3 = SO(3) * linear_map(3*3)
 
   // In GTSAM, we don't work with the skew-symmetric matrices A directly, but with 3-vectors.
@@ -307,6 +308,22 @@ TEST(Rot3, ExpmapDerivative3) {
   }
 }
 
+/* ************************************************************************* */
+TEST(Rot3, ExpmapDerivative4) {
+  auto w = [](double t) { return Vector3(2 * t, sin(t), 4 * t * t); };
+  auto w_dot = [](double t) { return Vector3(2, cos(t), 8 * t); };
+
+  // Same as above, but define R as mapping local coordinates to neighborhood aroud R0.
+  const Rot3 R0 = Rot3::Rodrigues(0.1, 0.4, 0.2);
+  auto R = [R0, w](double t) { return R0.expmap(w(t)); };
+
+  for (double t = -2.0; t < 2.0; t += 0.3) {
+    const Matrix expected = numericalDerivative11<Rot3, double>(R, t);
+    const Matrix actual = Rot3::ExpmapDerivative(w(t)) * w_dot(t);
+    CHECK(assert_equal(expected, actual, 1e-7));
+  }
+}
+
 /* ************************************************************************* */
 TEST( Rot3, jacobianExpmap )
 {