temporary variables for optim code

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;
     }
 }