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));