From cde44b2952cad20562225a31c0df35e02d03bd12 Mon Sep 17 00:00:00 2001 From: Duy-Nguyen Ta Date: Mon, 22 Apr 2013 08:34:40 +0000 Subject: [PATCH] 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 | 14 +++- gtsam/geometry/tests/testPose3.cpp | 17 +++- gtsam_unstable/dynamics/SimpleHelicopter.cpp | 8 ++ gtsam_unstable/dynamics/SimpleHelicopter.h | 73 ++++++++++++++++++ .../dynamics/tests/testSimpleHelicopter.cpp | 77 +++++++++++++++++++ 5 files changed, 186 insertions(+), 3 deletions(-) create mode 100644 gtsam_unstable/dynamics/SimpleHelicopter.cpp create mode 100644 gtsam_unstable/dynamics/SimpleHelicopter.h create mode 100644 gtsam_unstable/dynamics/tests/testSimpleHelicopter.cpp diff --git a/gtsam/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp index b0f958efb..799668dd4 100644 --- a/gtsam/geometry/Pose3.cpp +++ b/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( + boost::bind(testDerivExpmapInv, _1) + ), + LieVector(Vector::Zero(6)), 1e-5 + ); + + EXPECT(assert_equal(numericalDerivExpmapInv,res,1e-1)); } /* ************************************************************************* */ diff --git a/gtsam_unstable/dynamics/SimpleHelicopter.cpp b/gtsam_unstable/dynamics/SimpleHelicopter.cpp new file mode 100644 index 000000000..701060650 --- /dev/null +++ b/gtsam_unstable/dynamics/SimpleHelicopter.cpp @@ -0,0 +1,8 @@ +/* + * SimpleHelicopter.cpp + * + * Created on: Apr 21, 2013 + * Author: thduynguyen + */ + +#include "SimpleHelicopter.h" diff --git a/gtsam_unstable/dynamics/SimpleHelicopter.h b/gtsam_unstable/dynamics/SimpleHelicopter.h new file mode 100644 index 000000000..fd572cf24 --- /dev/null +++ b/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 +#include +#include + +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 { + + double h_; // time step + typedef NoiseModelFactor3 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::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 H1 = boost::none, + boost::optional H2 = boost::none, + boost::optional 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 */ diff --git a/gtsam_unstable/dynamics/tests/testSimpleHelicopter.cpp b/gtsam_unstable/dynamics/tests/testSimpleHelicopter.cpp new file mode 100644 index 000000000..2047b10cd --- /dev/null +++ b/gtsam_unstable/dynamics/tests/testSimpleHelicopter.cpp @@ -0,0 +1,77 @@ +/** + * @file testPendulumExplicitEuler.cpp + * @author Duy-Nguyen Ta + */ + +#include +#include +#include +#include + +/* ************************************************************************* */ +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( + 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( + boost::bind(&Reconstruction::evaluateError, constraint, _1, _2, _3, boost::none, boost::none, boost::none) + ), + g2, g1, v1, 1e-5 + ); + + Matrix numericalH2 = numericalDerivative32( + boost::function( + boost::bind(&Reconstruction::evaluateError, constraint, _1, _2, _3, boost::none, boost::none, boost::none) + ), + g2, g1, v1, 1e-5 + ); + + Matrix numericalH3 = numericalDerivative33( + boost::function( + 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); } +/* ************************************************************************* */