12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

fix testSimpleHelicopter

release/4.3a0
thduynguyen 2014-12-25 17:25:42 -05:00
parent 2aaa7e7564
commit 11858da42b
2 changed files with 12 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
gtsam_unstable/dynamics/SimpleHelicopter.h
View File

@ -46,28 +46,32 @@ public:
boost::optional<Matrix&> H2 = boost::none,
boost::optional<Matrix&> 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 newPose = Pose3::Expmap(h_*xik);
Pose3 gkxi = gk.compose(newPose, D_gkxi_gk, D_gkxi_exphxi);
Matrix D_hx_gk1, D_hx_gkxi;
Pose3 hx = gkxi.between(gk1, D_hx_gkxi, D_hx_gk1);
Matrix D_log_hx;
Vector error = Pose3::Logmap(hx, D_log_hx);
if (H1) {
*H1 = D_hx_gk1;
*H1 = D_log_hx*D_hx_gk1;
}
if (H2) {
Matrix D_hx_gk = D_hx_gkxi * D_gkxi_gk;
*H2 = D_hx_gk;
*H2 = D_log_hx*D_hx_gk;
}
if (H3) {
Matrix D_exphxi_xi = Pose3::LogmapDerivative(newPose)*h_;
Matrix D_hx_xi = D_hx_gkxi * D_gkxi_exphxi * D_exphxi_xi;
*H3 = D_hx_xi;
*H3 = D_log_hx*D_hx_xi;
}
return Pose3::Logmap(hx);
return error;
}
};

4
gtsam_unstable/dynamics/tests/testSimpleHelicopter.cpp
View File

@ -55,8 +55,8 @@ TEST(Reconstruction, ExpmapInvDeriv) {
boost::function<Vector(const Vector6&)>(boost::bind(testExpmapDeriv, _1)),
Vector6(Vector::Zero(6)), 1e-5);
Pose3 newPose = Pose3::Expmap(h * V1_g1);
Matrix dExpInv = Pose3::LogmapDerivative(newPose);
EXPECT(assert_equal(numericalExpmap, dExpInv, 1e-2));
Matrix dexp = Pose3::ExpmapDerivative(h * V1_g1);
EXPECT(assert_equal(numericalExpmap, dexp, 1e-2));
}
/* ************************************************************************* */