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

Avoid derivative calcs if they aren't asked for

release/4.3a0
Asa Hammond 2021-03-11 09:22:40 -08:00
parent f48a2f6273
commit b0b5b04ce8
1 changed files with 11 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

13
gtsam/navigation/ConstantVelocityFactor.h
View File

@ -52,11 +52,20 @@ class ConstantVelocityFactor : public NoiseModelFactor2<NavState, NavState> {
static const Vector3 b_accel{0.0, 0.0, 0.0}; static const Vector3 b_accel{0.0, 0.0, 0.0};
static const Vector3 b_omega{0.0, 0.0, 0.0}; static const Vector3 b_omega{0.0, 0.0, 0.0};
NavState predicted;
Matrix predicted_H_x1; Matrix predicted_H_x1;
NavState predicted = x1.update(b_accel, b_omega, dt_, predicted_H_x1, {}, {});
if (H1)
predicted = x1.update(b_accel, b_omega, dt_, predicted_H_x1, {}, {});
else
predicted = x1.update(b_accel, b_omega, dt_, boost::none, {}, {});
Matrix error_H_predicted; Matrix error_H_predicted;
Vector9 error = predicted.localCoordinates(x2, error_H_predicted, H2); Vector9 error;
if (H1)
error = predicted.localCoordinates(x2, error_H_predicted, H2);
else
error = predicted.localCoordinates(x2, boost::none, H2);
if (H1) { if (H1) {
*H1 = error_H_predicted * predicted_H_x1; *H1 = error_H_predicted * predicted_H_x1;