From d5b4a0bc062747944c933350cb6e351f45471cd8 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Joan=20Sol=C3=A0?= <jsola@iri.upc.edu>
Date: Wed, 9 Oct 2019 20:31:47 +0200
Subject: [PATCH] temporary variables for optim code
---
include/IMU/math/IMU_tools_Lie.h | 12 ++++++++----
1 file changed, 8 insertions(+), 4 deletions(-)
diff --git a/include/IMU/math/IMU_tools_Lie.h b/include/IMU/math/IMU_tools_Lie.h
index 064606493..7dad22964 100644
--- a/include/IMU/math/IMU_tools_Lie.h
+++ b/include/IMU/math/IMU_tools_Lie.h
@@ -417,14 +417,18 @@ inline void QandPmat(const MatrixBase<D1>& th, MatrixBase<D2>& Q, MatrixBase<D3>
Id.setIdentity();
Matrix<T, 3, 1> u = th / thn;
Matrix<T, 3, 3> ux = skew(u);
+ Matrix<T, 3, 3> ux2 = ux*ux;
+ T thn2 = thn*thn;
+ T sin_thn = sin(thn);
+ T cos_thn = cos(thn);
Q = Id
- + ((1 - cos(thn))/thn)*ux
- + ((thn - sin(thn))/(thn*thn))*(ux*ux);
+ + ((1 - cos_thn)/thn)*ux
+ + ((thn - sin_thn)/thn2)*(ux2);
P = 0.5*Id
- + ((thn - sin(thn))/(thn*thn))*ux
- + ((cos(thn) + 0.5*thn*thn - 1)/(thn*thn))*(ux*ux);
+ + ((thn - sin_thn)/thn2)*ux
+ + ((cos_thn + 0.5*thn2 - 1)/thn2)*ux2;
}
}
--
GitLab