From b0b5b04ce8d2bad7fbb4f52dd1663374c07eeaa5 Mon Sep 17 00:00:00 2001 From: Asa Hammond Date: Thu, 11 Mar 2021 09:22:40 -0800 Subject: [PATCH] Avoid derivative calcs if they aren't asked for --- gtsam/navigation/ConstantVelocityFactor.h | 13 +++++++++++-- 1 file changed, 11 insertions(+), 2 deletions(-) diff --git a/gtsam/navigation/ConstantVelocityFactor.h b/gtsam/navigation/ConstantVelocityFactor.h index 6cb349869..0e1401375 100644 --- a/gtsam/navigation/ConstantVelocityFactor.h +++ b/gtsam/navigation/ConstantVelocityFactor.h @@ -52,11 +52,20 @@ 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; - 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; - 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) { *H1 = error_H_predicted * predicted_H_x1;