Finally finish implementing the Reconstruction factor for updating the poses of holonomic vehicles using discrete variational integrators (eq. 10 in [Kobilarov09siggraph]). Also, better approximation for the derivative of the inverse expmap wrt the lie algebra in Pose3. Test with numericalderivative.

gtsam/geometry/Pose3.cpp

@@ -64,7 +64,19 @@ namespace gtsam {
 
   /* ************************************************************************* */
   Matrix6 Pose3::dExpInv_TLN(const Vector& xi) {
-    return I6 - 0.5*adjoint(xi);
+    // Bernoulli numbers, from Wikipedia
+    static const Vector B = Vector_(9, 1.0, -1.0/2.0, 1./6., 0.0, -1.0/30.0, 0.0, 1.0/42.0, 0.0, -1.0/30);
+    static const int N = 5; // order of approximation
+    Matrix res = I6;
+    Matrix6 ad_i = I6;
+    Matrix6 ad_xi = adjoint(xi);
+    double fac = 1.0;
+    for (int i = 1 ; i<N; ++i) {
+      ad_i = ad_xi * ad_i;
+      fac = fac*i;
+      res = res + B(i)/fac*ad_i;
+    }
+    return res;
   }
 
   /* ************************************************************************* */

gtsam/geometry/tests/testPose3.cpp

@@ -634,10 +634,23 @@ TEST( Pose3, adjoint) {
 }
 
 /* ************************************************************************* */
+/// exp(xi) exp(y) = exp(xi + x)
+/// Hence, y = log (exp(-xi)*exp(xi+x))
+Vector testDerivExpmapInv(const LieVector& dxi) {
+  return Pose3::Logmap(Pose3::Expmap(-screw::xi)*Pose3::Expmap(screw::xi+dxi));
+}
+
 TEST( Pose3, dExpInv_TLN) {
   Matrix res = Pose3::dExpInv_TLN(screw::xi);
-  Matrix6 expected = eye(6,6) - 0.5*Pose3::adjoint(screw::xi);
-  EXPECT(assert_equal(expected,res,1e-5));
+
+  Matrix numericalDerivExpmapInv = numericalDerivative11(
+      boost::function<Vector(const LieVector&)>(
+          boost::bind(testDerivExpmapInv,  _1)
+          ),
+      LieVector(Vector::Zero(6)), 1e-5
+      );
+
+  EXPECT(assert_equal(numericalDerivExpmapInv,res,1e-1));
 }
 
 /* ************************************************************************* */

gtsam_unstable/dynamics/SimpleHelicopter.cpp

@@ -0,0 +1,8 @@
+/*
+ * SimpleHelicopter.cpp
+ *
+ *  Created on: Apr 21, 2013
+ *      Author: thduynguyen
+ */
+
+#include "SimpleHelicopter.h"

gtsam_unstable/dynamics/SimpleHelicopter.h

@@ -0,0 +1,73 @@
+/*
+ * @file SimpleHelicopter.h
+ * @brief Implement SimpleHelicopter discrete dynamics model and variational integrator,
+ *        following [Kobilarov09siggraph]
+ * @author Duy-Nguyen Ta
+ */
+
+#pragma once
+
+#include <gtsam/geometry/Pose3.h>
+#include <gtsam/base/LieVector.h>
+#include <gtsam/nonlinear/NonlinearFactor.h>
+
+namespace gtsam {
+
+/**
+ * Implement the Reconstruction equation: \f$ g_{k+1} = g_k \exp (h\xi_k) \f$, where
+ *      \f$ h \f$: timestep (parameter)
+ *      \f$ g_{k+1}, g_{k} \f$: poses at the current and the next timestep
+ *      \f$ \xi_k \f$: the body-fixed velocity (Lie algebra)
+ * It is somewhat similar to BetweenFactor, but treats the body-fixed velocity
+ * \f$ \xi_k \f$ as a variable. So it is a three-way factor.
+ */
+class Reconstruction : public NoiseModelFactor3<Pose3, Pose3, LieVector>  {
+
+  double h_;  // time step
+  typedef NoiseModelFactor3<Pose3, Pose3, LieVector> Base;
+public:
+  Reconstruction(Key gKey1, Key gKey, Key xiKey, double h, double mu = 1000.0) :
+    Base(noiseModel::Constrained::All(Pose3::Dim()*3, fabs(mu)), gKey1, gKey,
+        xiKey), h_(h) {
+  }
+  virtual ~Reconstruction() {}
+
+  /// @return a deep copy of this factor
+  virtual gtsam::NonlinearFactor::shared_ptr clone() const {
+    return boost::static_pointer_cast<gtsam::NonlinearFactor>(
+        gtsam::NonlinearFactor::shared_ptr(new Reconstruction(*this))); }
+
+  /** \f$ log((g_k\exp(h\xi_k))^{-1}g_{k+1}) = 0, with optional derivatives */
+  Vector evaluateError(const Pose3& gk1, const Pose3& gk, const LieVector& xik,
+      boost::optional<Matrix&> H1 = boost::none,
+      boost::optional<Matrix&> H2 = boost::none,
+      boost::optional<Matrix&> H3 = boost::none) const {
+
+    static const bool debug = false;
+
+    Matrix gkxiHgk, gkxiHexpxi;
+    Pose3 gkxi = gk.compose(Pose3::Expmap(h_*xik), gkxiHgk, gkxiHexpxi);
+
+    Matrix hxHgk1, hxHgkxi;
+    Pose3 hx = gkxi.between(gk1, hxHgkxi, hxHgk1);
+
+    if (H1) {
+      *H1 = hxHgk1;
+    }
+    if (H2) {
+      Matrix hxHgk = hxHgkxi*gkxiHgk;
+      *H2 = hxHgk;
+    }
+
+    if (H3) {
+      Matrix expxiHxi = Pose3::dExpInv_TLN(xik);
+      Matrix hxHxi = hxHgkxi*gkxiHexpxi*expxiHxi;
+      *H3 = hxHxi;
+    }
+
+    return Pose3::Logmap(hx);
+  }
+
+};
+
+} /* namespace gtsam */

gtsam_unstable/dynamics/tests/testSimpleHelicopter.cpp

@@ -0,0 +1,77 @@
+/**
+ * @file testPendulumExplicitEuler.cpp
+ * @author Duy-Nguyen Ta
+ */
+
+#include <CppUnitLite/TestHarness.h>
+#include <gtsam/base/numericalDerivative.h>
+#include <gtsam/nonlinear/Symbol.h>
+#include <gtsam_unstable/dynamics/SimpleHelicopter.h>
+
+/* ************************************************************************* */
+using namespace gtsam;
+using namespace gtsam::symbol_shorthand;
+
+const double tol=1e-5;
+const double h = 1.0;
+const double deg2rad = M_PI/180.0;
+
+Pose3 g1(Rot3::ypr(deg2rad*10.0, deg2rad*20.0, deg2rad*30.0), Point3(100.0, 200.0, 300.0));
+LieVector v1(Vector_(6, 0.1, 0.05, 0.02, 10.0, 20.0, 30.0));
+Pose3 g2(g1.retract(h*v1, Pose3::EXPMAP));
+
+/* ************************************************************************* */
+Vector testExpmapDeriv(const LieVector& v) {
+  return Pose3::Logmap(Pose3::Expmap(-v1)*Pose3::Expmap(v1+v));
+}
+
+TEST(Reconstruction, ExpmapInvDeriv) {
+  Matrix numericalExpmap = numericalDerivative11(
+      boost::function<Vector(const LieVector&)>(
+          boost::bind(testExpmapDeriv,  _1)
+          ),
+      LieVector(Vector::Zero(6)), 1e-3
+      );
+  Matrix dExpInv = Pose3::dExpInv_TLN(v1);
+  EXPECT(assert_equal(numericalExpmap, dExpInv, 5e-1));
+}
+
+/* ************************************************************************* */
+TEST( Reconstruction, evaluateError) {
+  // hard constraints don't need a noise model
+  Reconstruction constraint(G(2), G(1), V(1), h);
+
+  // verify error function
+  Matrix H1, H2, H3;
+  EXPECT(assert_equal(zero(6), constraint.evaluateError(g2, g1, v1, H1, H2, H3), tol));
+
+
+  Matrix numericalH1 = numericalDerivative31(
+      boost::function<Vector(const Pose3&, const Pose3&, const LieVector&)>(
+          boost::bind(&Reconstruction::evaluateError, constraint, _1, _2, _3, boost::none, boost::none, boost::none)
+          ),
+      g2, g1, v1, 1e-5
+      );
+
+  Matrix numericalH2 = numericalDerivative32(
+      boost::function<Vector(const Pose3&, const Pose3&, const LieVector&)>(
+          boost::bind(&Reconstruction::evaluateError, constraint, _1, _2, _3, boost::none, boost::none, boost::none)
+          ),
+      g2, g1, v1, 1e-5
+      );
+
+  Matrix numericalH3 = numericalDerivative33(
+      boost::function<Vector(const Pose3&, const Pose3&, const LieVector&)>(
+          boost::bind(&Reconstruction::evaluateError, constraint, _1, _2, _3, boost::none, boost::none, boost::none)
+          ),
+      g2, g1, v1, 1e-5
+      );
+
+  EXPECT(assert_equal(numericalH1,H1,1e-5));
+  EXPECT(assert_equal(numericalH2,H2,1e-5));
+  EXPECT(assert_equal(numericalH3,H3,5e-1));
+}
+
+/* ************************************************************************* */
+int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
+/* ************************************************************************* */