From 36bc1d3e3fd2cd908c2e1827eae442b2ff8c77d2 Mon Sep 17 00:00:00 2001
From: dellaert <dellaert@cc.gatech.edu>
Date: Sun, 23 Nov 2014 13:57:27 +0100
Subject: [PATCH] Small savings of matrix-vector mult

---
 gtsam/navigation/AHRSFactor.cpp | 5 +++--
 1 file changed, 3 insertions(+), 2 deletions(-)

diff --git a/gtsam/navigation/AHRSFactor.cpp b/gtsam/navigation/AHRSFactor.cpp
index 5f8d48e28..c4309e16f 100644
--- a/gtsam/navigation/AHRSFactor.cpp
+++ b/gtsam/navigation/AHRSFactor.cpp
@@ -139,14 +139,15 @@ void AHRSFactor::PreintegratedMeasurements::integrateMeasurement(
 Vector3 AHRSFactor::PreintegratedMeasurements::predict(
     const imuBias::ConstantBias& bias, boost::optional<Matrix&> H) const {
   const Vector3 biasOmegaIncr = bias.gyroscope() - biasHat_.gyroscope();
+  Vector3 delRdelBiasOmega_biasOmegaIncr = delRdelBiasOmega_ * biasOmegaIncr;
   const Rot3 deltaRij_biascorrected = deltaRij_.retract(
-      delRdelBiasOmega_ * biasOmegaIncr, Rot3::EXPMAP);
+      delRdelBiasOmega_biasOmegaIncr, Rot3::EXPMAP);
   const Vector3 theta_biascorrected = Rot3::Logmap(deltaRij_biascorrected);
   if (H) {
     const Matrix3 Jrinv_theta_bc = //
         Rot3::rightJacobianExpMapSO3inverse(theta_biascorrected);
     const Matrix3 Jr_JbiasOmegaIncr = //
-        Rot3::rightJacobianExpMapSO3(delRdelBiasOmega_ * biasOmegaIncr);
+        Rot3::rightJacobianExpMapSO3(delRdelBiasOmega_biasOmegaIncr);
     (*H) = Jrinv_theta_bc * Jr_JbiasOmegaIncr * delRdelBiasOmega_;
   }
   return theta_biascorrected;