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

Don't use numerical derivative

release/4.3a0
Frank Dellaert 2016-01-30 12:45:27 -08:00
parent f498a96582
commit ba5d4ffa6c
3 changed files with 4 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
gtsam/navigation/PreintegrationBase.cpp
View File

@ -144,8 +144,9 @@ Vector9 PreintegrationBase::UpdateEstimate(const Vector3& a_body,
velocity + a_nav* dt; // velocity velocity + a_nav* dt; // velocity
if (A) { if (A) {
#define USE_NUMERICAL_DERIVATIVE
#ifdef USE_NUMERICAL_DERIVATIVE #ifdef USE_NUMERICAL_DERIVATIVE
// The use of this yields much more accurate derivatives, but it's slow!
// TODO(frank): find a cheap closed form solution (look at Iserles)
auto f = [w_body](const Vector3& theta) { auto f = [w_body](const Vector3& theta) {
return Rot3::ExpmapDerivative(theta).inverse() * w_body; return Rot3::ExpmapDerivative(theta).inverse() * w_body;
}; };
@ -153,7 +154,6 @@ Vector9 PreintegrationBase::UpdateEstimate(const Vector3& a_body,
numericalDerivative11<Vector3, Vector3>(f, theta); numericalDerivative11<Vector3, Vector3>(f, theta);
#else #else
// First order (small angle) approximation of derivative of invH*w: // First order (small angle) approximation of derivative of invH*w:
// TODO(frank): find a cheap closed form solution (look at Iserles)
// NOTE(frank): Rot3::ExpmapDerivative(w_body) is a less accurate approximation // NOTE(frank): Rot3::ExpmapDerivative(w_body) is a less accurate approximation
const Matrix3 invHw_H_theta = skewSymmetric(-0.5 * w_body); const Matrix3 invHw_H_theta = skewSymmetric(-0.5 * w_body);
#endif #endif

2
gtsam/navigation/tests/testCombinedImuFactor.cpp
View File

@ -139,7 +139,7 @@ TEST(CombinedImuFactor, FirstOrderPreIntegratedMeasurements) {
EXPECT(assert_equal(numericalDerivative21(zeta, Z_3x1, Z_3x1), EXPECT(assert_equal(numericalDerivative21(zeta, Z_3x1, Z_3x1),
pim.zeta_H_biasAcc())); pim.zeta_H_biasAcc()));
EXPECT(assert_equal(numericalDerivative22(zeta, Z_3x1, Z_3x1), EXPECT(assert_equal(numericalDerivative22(zeta, Z_3x1, Z_3x1),
pim.zeta_H_biasOmega(), 1e-7)); pim.zeta_H_biasOmega(), 1e-3));
} }
/* ************************************************************************* */ /* ************************************************************************* */

2
gtsam/navigation/tests/testImuFactor.cpp
View File

@ -453,7 +453,7 @@ TEST(ImuFactor, FirstOrderPreIntegratedMeasurements) {
EXPECT(assert_equal(numericalDerivative21(zeta, Z_3x1, Z_3x1), EXPECT(assert_equal(numericalDerivative21(zeta, Z_3x1, Z_3x1),
pim.zeta_H_biasAcc())); pim.zeta_H_biasAcc()));
EXPECT(assert_equal(numericalDerivative22(zeta, Z_3x1, Z_3x1), EXPECT(assert_equal(numericalDerivative22(zeta, Z_3x1, Z_3x1),
pim.zeta_H_biasOmega(), 1e-7)); pim.zeta_H_biasOmega(), 1e-3));
} }
/* ************************************************************************* */ /* ************************************************************************* */