From 11b196b52843b1a2146a509e648b02a808ddb186 Mon Sep 17 00:00:00 2001 From: Asa Hammond Date: Thu, 11 Mar 2021 11:41:16 -0800 Subject: [PATCH] Move to fixed size matrix for derivative calculations --- gtsam/navigation/ConstantVelocityFactor.h | 17 ++++------------- 1 file changed, 4 insertions(+), 13 deletions(-) diff --git a/gtsam/navigation/ConstantVelocityFactor.h b/gtsam/navigation/ConstantVelocityFactor.h index 0e1401375..ed68ac077 100644 --- a/gtsam/navigation/ConstantVelocityFactor.h +++ b/gtsam/navigation/ConstantVelocityFactor.h @@ -52,20 +52,11 @@ class ConstantVelocityFactor : public NoiseModelFactor2 { 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;