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

Move to fixed size matrix for derivative calculations

release/4.3a0
Asa Hammond 2021-03-11 11:41:16 -08:00
parent b0b5b04ce8
commit 11b196b528
1 changed files with 4 additions and 13 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

17
gtsam/navigation/ConstantVelocityFactor.h
View File

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