From 3ed5d05b5b9423a87c670fcd0f93282598ee5f2d Mon Sep 17 00:00:00 2001
From: Frank Dellaert <frank@skyd.io>
Date: Mon, 1 Feb 2016 09:13:38 -0800
Subject: [PATCH] ApplyInvDexp works !!!

---
 .../tests/testPreintegrationBase.cpp          | 24 +++++++++++++++++++
 1 file changed, 24 insertions(+)

diff --git a/gtsam/navigation/tests/testPreintegrationBase.cpp b/gtsam/navigation/tests/testPreintegrationBase.cpp
index 0ad4d4903..ea362e5ee 100644
--- a/gtsam/navigation/tests/testPreintegrationBase.cpp
+++ b/gtsam/navigation/tests/testPreintegrationBase.cpp
@@ -17,6 +17,9 @@
 
 #include <gtsam/navigation/PreintegrationBase.h>
 #include <gtsam/base/numericalDerivative.h>
+#include <gtsam/nonlinear/expressions.h>
+#include <gtsam/nonlinear/ExpressionFactor.h>
+#include <gtsam/nonlinear/expressionTesting.h>
 
 #include <CppUnitLite/TestHarness.h>
 #include <boost/bind.hpp>
@@ -40,6 +43,27 @@ static boost::shared_ptr<PreintegrationParams> Params() {
 }
 }
 
+/* ************************************************************************* */
+TEST(ExpressionFactor, ApplyInvDexp) {
+  auto model = noiseModel::Isotropic::Sigma(3, 1);
+
+  /// Functor implements ExpmapDerivative(omega).inverse() * v, with derivatives
+  MultiplyWithInverseFunction<Vector3, 3> applyInvDexp(SO3::ApplyExpmapDerivative);
+  Vector3_ f_expr(applyInvDexp, Vector3_(0), Vector3_(1));
+
+  // Check derivatives
+  Vector3 omega(1, 2, 3);
+  const Vector3 v(0.1, 0.2, 0.3);
+  const Vector3 expected = SO3::ExpmapDerivative(omega).inverse() * v;
+  CHECK(assert_equal(expected, applyInvDexp(omega,v)));
+
+  Values values;
+  values.insert<Vector3>(0, omega);
+  values.insert<Vector3>(1, v);
+  ExpressionFactor<Vector3> factor(model, Vector3::Zero(), f_expr);
+  EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-5, 1e-5);
+}
+
 /* ************************************************************************* */
 TEST(PreintegrationBase, UpdateEstimate1) {
   PreintegrationBase pim(testing::Params());