diff --git a/gtsam_unstable/dynamics/SimpleHelicopter.h b/gtsam_unstable/dynamics/SimpleHelicopter.h index b1c254a9e..88f0acdce 100644 --- a/gtsam_unstable/dynamics/SimpleHelicopter.h +++ b/gtsam_unstable/dynamics/SimpleHelicopter.h @@ -60,7 +60,7 @@ public: } if (H3) { - Matrix D_exphxi_xi = Pose3::dExpInv_exp(h_*xik)*h_; + Matrix D_exphxi_xi = Pose3::ExpmapDerivative(h_*xik)*h_; Matrix D_hx_xi = D_hx_gkxi * D_gkxi_exphxi * D_exphxi_xi; *H3 = D_hx_xi; } diff --git a/gtsam_unstable/dynamics/tests/testSimpleHelicopter.cpp b/gtsam_unstable/dynamics/tests/testSimpleHelicopter.cpp index bdc71fea2..34bcdea86 100644 --- a/gtsam_unstable/dynamics/tests/testSimpleHelicopter.cpp +++ b/gtsam_unstable/dynamics/tests/testSimpleHelicopter.cpp @@ -57,7 +57,7 @@ TEST(Reconstruction, ExpmapInvDeriv) { ), Vector6(Vector::Zero(6)), 1e-5 ); - Matrix dExpInv = Pose3::dExpInv_exp(h*V1_g1); + Matrix dExpInv = Pose3::ExpmapDerivative(h*V1_g1); EXPECT(assert_equal(numericalExpmap, dExpInv, 1e-2)); }