From 902e7e57e52448a8e28884e30646a43061c4291c Mon Sep 17 00:00:00 2001 From: dellaert Date: Fri, 26 Dec 2014 00:16:10 +0100 Subject: [PATCH] Fixing helicopter --- .cproject | 130 ++++++++++-------- gtsam_unstable/dynamics/SimpleHelicopter.h | 41 +++--- .../dynamics/tests/testSimpleHelicopter.cpp | 36 ++--- 3 files changed, 97 insertions(+), 110 deletions(-) diff --git a/.cproject b/.cproject index 90cd6c3bf..25a94b382 100644 --- a/.cproject +++ b/.cproject @@ -592,6 +592,7 @@ make + tests/testBayesTree.run true false @@ -599,6 +600,7 @@ make + testBinaryBayesNet.run true false @@ -646,6 +648,7 @@ make + testSymbolicBayesNet.run true false @@ -653,6 +656,7 @@ make + tests/testSymbolicFactor.run true false @@ -660,6 +664,7 @@ make + testSymbolicFactorGraph.run true false @@ -675,6 +680,7 @@ make + tests/testBayesTree true false @@ -1130,6 +1136,7 @@ make + testErrors.run true false @@ -1359,6 +1366,46 @@ true true + + make + -j5 + testBTree.run + true + true + true + + + make + -j5 + testDSF.run + true + true + true + + + make + -j5 + testDSFMap.run + true + true + true + + + make + -j5 + testDSFVector.run + true + true + true + + + make + -j5 + testFixedVector.run + true + true + true + make -j2 @@ -1441,7 +1488,6 @@ make - testSimulated2DOriented.run true false @@ -1481,7 +1527,6 @@ make - testSimulated2D.run true false @@ -1489,7 +1534,6 @@ make - testSimulated3D.run true false @@ -1503,46 +1547,6 @@ true true - - make - -j5 - testBTree.run - true - true - true - - - make - -j5 - testDSF.run - true - true - true - - - make - -j5 - testDSFMap.run - true - true - true - - - make - -j5 - testDSFVector.run - true - true - true - - - make - -j5 - testFixedVector.run - true - true - true - make -j5 @@ -1800,6 +1804,7 @@ cpack + -G DEB true false @@ -1807,6 +1812,7 @@ cpack + -G RPM true false @@ -1814,6 +1820,7 @@ cpack + -G TGZ true false @@ -1821,6 +1828,7 @@ cpack + --config CPackSourceConfig.cmake true false @@ -2297,6 +2305,14 @@ true true + + make + -j4 + testSimpleHelicopter.run + true + true + true + make -j1 @@ -2683,6 +2699,7 @@ make + testGraph.run true false @@ -2690,6 +2707,7 @@ make + testJunctionTree.run true false @@ -2697,6 +2715,7 @@ make + testSymbolicBayesNetB.run true false @@ -2790,14 +2809,6 @@ true true - - make - -j5 - testAdaptAutoDiff.run - true - true - true - make -j5 @@ -2806,14 +2817,6 @@ true true - - make - -j4 - testBasisDecompositions.run - true - true - true - make -j4 @@ -3190,6 +3193,14 @@ true true + + make + -j4 + testAdaptAutoDiff.run + true + true + true + make -j4 @@ -3248,7 +3259,6 @@ make - tests/testGaussianISAM2 true false diff --git a/gtsam_unstable/dynamics/SimpleHelicopter.h b/gtsam_unstable/dynamics/SimpleHelicopter.h index c2a893be9..fa52d8b45 100644 --- a/gtsam_unstable/dynamics/SimpleHelicopter.h +++ b/gtsam_unstable/dynamics/SimpleHelicopter.h @@ -46,32 +46,29 @@ public: boost::optional H2 = boost::none, boost::optional H3 = boost::none) const { - Matrix D_exphxi_xi; - Pose3 newPose = Pose3::Expmap(h_*xik, D_exphxi_xi); - D_exphxi_xi = D_exphxi_xi*h_; - Matrix D_gkxi_gk, D_gkxi_exphxi; - Pose3 gkxi = gk.compose(newPose, D_gkxi_gk, D_gkxi_exphxi); + if (H1 || H2 || H3) { + Matrix6 D_Xi_3; + Pose3 Xi = traits_x::Expmap(h_ * xik, D_Xi_3); - Matrix D_hx_gk1, D_hx_gkxi; - Pose3 hx = gkxi.between(gk1, D_hx_gkxi, D_hx_gk1); + Matrix6 D_gkxi_2, D_gkxi_Xi; + Pose3 gkxi = gk.compose(Xi, D_gkxi_2, D_gkxi_Xi); - Matrix D_log_hx; - Vector error = Pose3::Logmap(hx, D_log_hx); + Matrix6 D_hx_1, D_hx_gkxi; + Pose3 hx = gkxi.between(gk1, D_hx_gkxi, D_hx_1); - if (H1) { - *H1 = D_log_hx*D_hx_gk1; + Matrix6 D_e_hx; + Vector6 error = traits_x::Logmap(hx, D_e_hx); + if (H1) *H1 = D_e_hx * D_hx_1; + if (H2) *H2 = D_e_hx * D_hx_gkxi * D_gkxi_2; + if (H3) *H3 = D_e_hx * D_gkxi_Xi * D_Xi_3; + + return error; + } else { + Pose3 Xi = Pose3::Expmap(h_ * xik); + Pose3 gkxi = gk.compose(Xi); + Pose3 hx = gkxi.between(gk1); + return Pose3::Logmap(hx); } - if (H2) { - Matrix D_hx_gk = D_hx_gkxi * D_gkxi_gk; - *H2 = D_log_hx*D_hx_gk; - } - - if (H3) { - Matrix D_hx_xi = D_hx_gkxi * D_gkxi_exphxi * D_exphxi_xi; - *H3 = D_log_hx*D_hx_xi; - } - - return error; } }; diff --git a/gtsam_unstable/dynamics/tests/testSimpleHelicopter.cpp b/gtsam_unstable/dynamics/tests/testSimpleHelicopter.cpp index b725ac89a..a0d969c4d 100644 --- a/gtsam_unstable/dynamics/tests/testSimpleHelicopter.cpp +++ b/gtsam_unstable/dynamics/tests/testSimpleHelicopter.cpp @@ -45,20 +45,6 @@ Vector computeFu(const Vector& gamma, const Vector& control) { return F*control; } -/* ************************************************************************* */ -Vector testExpmapDeriv(const Vector6& v) { - return Pose3::Logmap(Pose3::Expmap(-h*V1_g1)*Pose3::Expmap(h*V1_g1+v)); -} - -TEST(Reconstruction, ExpmapInvDeriv) { - Matrix numericalExpmap = numericalDerivative11( - boost::function(boost::bind(testExpmapDeriv, _1)), - Vector6(Vector::Zero(6)), 1e-5); - Pose3 newPose = Pose3::Expmap(h * V1_g1); - Matrix dexp = Pose3::ExpmapDerivative(h * V1_g1); - EXPECT(assert_equal(numericalExpmap, dexp, 1e-2)); -} - /* ************************************************************************* */ TEST( Reconstruction, evaluateError) { // hard constraints don't need a noise model @@ -66,29 +52,23 @@ TEST( Reconstruction, evaluateError) { // verify error function Matrix H1, H2, H3; - EXPECT(assert_equal(zero(6), constraint.evaluateError(g2, g1, V1_g1, H1, H2, H3), tol)); - + EXPECT( + assert_equal(zero(6), constraint.evaluateError(g2, g1, V1_g1, 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_g1, 1e-5 - ); + boost::bind(&Reconstruction::evaluateError, constraint, _1, _2, _3, + boost::none, boost::none, boost::none)), g2, g1, V1_g1, 1e-5); Matrix numericalH2 = numericalDerivative32( boost::function( - boost::bind(&Reconstruction::evaluateError, constraint, _1, _2, _3, boost::none, boost::none, boost::none) - ), - g2, g1, V1_g1, 1e-5 - ); + boost::bind(&Reconstruction::evaluateError, constraint, _1, _2, _3, + boost::none, boost::none, boost::none)), g2, g1, V1_g1, 1e-5); Matrix numericalH3 = numericalDerivative33( boost::function( - boost::bind(&Reconstruction::evaluateError, constraint, _1, _2, _3, boost::none, boost::none, boost::none) - ), - g2, g1, V1_g1, 1e-5 - ); + boost::bind(&Reconstruction::evaluateError, constraint, _1, _2, _3, + boost::none, boost::none, boost::none)), g2, g1, V1_g1, 1e-5); EXPECT(assert_equal(numericalH1,H1,1e-5)); EXPECT(assert_equal(numericalH2,H2,1e-5));