a numerical derivative version for DiscreteEulerPoincare'Factor, but currently disabled.

gtsam_unstable/dynamics/SimpleHelicopter.h

@@ -7,6 +7,7 @@
 
 #pragma once
 
+#include <gtsam/base/numericalDerivative.h>
 #include <gtsam/geometry/Pose3.h>
 #include <gtsam/base/LieVector.h>
 #include <gtsam/nonlinear/NonlinearFactor.h>
@@ -20,6 +21,8 @@ namespace gtsam {
  *      \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.
+ * Note: this factor is necessary if one needs to smooth the entire graph. It's not needed
+ *  in sequential update method.
  */
 class Reconstruction : public NoiseModelFactor3<Pose3, Pose3, LieVector>  {
 
@@ -149,6 +152,52 @@ public:
     return hx;
   }
 
+#if 0
+  Vector computeError(const LieVector& xik, const LieVector& xik_1, const Pose3& gk) const {
+    Vector pk = Pose3::dExpInv_exp(h_*xik).transpose()*Inertia_*xik;
+    Vector pk_1 = Pose3::dExpInv_exp(-h_*xik_1).transpose()*Inertia_*xik_1;
+
+    Point3 gravityBody = gk.rotation().unrotate(Point3(0.0, 0.0, -9.81*m_));
+    Vector f_ext = Vector_(6, 0.0, 0.0, 0.0, gravityBody.x(), gravityBody.y(), gravityBody.z());
+
+    Vector hx = pk - pk_1 - h_*Fu_ - h_*f_ext;
+
+    return hx;
+  }
+
+  Vector evaluateError(const LieVector& xik, const LieVector& xik_1, const Pose3& gk,
+      boost::optional<Matrix&> H1 = boost::none,
+      boost::optional<Matrix&> H2 = boost::none,
+      boost::optional<Matrix&> H3 = boost::none) const {
+    if (H1) {
+      (*H1) = numericalDerivative31(
+          boost::function<Vector(const LieVector&, const LieVector&, const Pose3&)>(
+              boost::bind(&DiscreteEulerPoincareHelicopter::computeError, *this, _1, _2, _3)
+          ),
+          xik, xik_1, gk, 1e-5
+      );
+    }
+    if (H2) {
+      (*H2) = numericalDerivative32(
+          boost::function<Vector(const LieVector&, const LieVector&, const Pose3&)>(
+              boost::bind(&DiscreteEulerPoincareHelicopter::computeError, *this, _1, _2, _3)
+          ),
+          xik, xik_1, gk, 1e-5
+      );
+    }
+    if (H3) {
+      (*H3) = numericalDerivative33(
+          boost::function<Vector(const LieVector&, const LieVector&, const Pose3&)>(
+              boost::bind(&DiscreteEulerPoincareHelicopter::computeError, *this, _1, _2, _3)
+          ),
+          xik, xik_1, gk, 1e-5
+      );
+    }
+
+    return computeError(xik, xik_1, gk);
+  }
+#endif
+
 };