From d14f4cf2c1f3d06f08cdab15e3bfd28c68404192 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Joan=20Sol=C3=A0?= <jsola@iri.upc.edu>
Date: Thu, 24 Oct 2019 15:53:05 +0200
Subject: [PATCH] Add small adjoint, hat, vee

---
 include/IMU/math/IMU_tools_Lie.h | 44 +++++++++++++++++++++++++++++---
 1 file changed, 41 insertions(+), 3 deletions(-)

diff --git a/include/IMU/math/IMU_tools_Lie.h b/include/IMU/math/IMU_tools_Lie.h
index 0bf90615a..72b3e5236 100644
--- a/include/IMU/math/IMU_tools_Lie.h
+++ b/include/IMU/math/IMU_tools_Lie.h
@@ -215,13 +215,51 @@ inline Matrix<typename D::Scalar, 10, 10> adjoint(const MatrixBase<D>& delta){
 }
 
 
-template<typename D1, typename D2>
-inline Matrix<T, 10, 10> smalladjoint(const MatrixBase<D1> tau)
+template<typename D>
+inline Matrix<typename D::Scalar, 5, 5> hat(const MatrixBase<D> tau)
 {
     MatrixSizeCheck<10, 1>::check(tau);
 
-    typedef typename D1::Scalar T;
+    typedef typename D::Scalar T;
+
+
+    Map<const Matrix<T, 3, 1> >   p   ( & tau(0) );
+    Map<const Matrix<T, 3, 1> >   th  ( & tau(3) );
+    Map<const Matrix<T, 3, 1> >   v   ( & tau(6) );
+    const T&                      t =     tau(9);
+
+    Matrix<T, 5, 5> taux;
+    taux.setZero();
+
+    taux.block<3,3>(0,0) = skew(th);
+    taux.block<3,1>(0,3) = v;
+    taux.block<3,1>(0,4) = p;
+    taux.block<3,1>(3,4) = t;
+
+    return taux;
+}
+
+template<typename D>
+inline Matrix<typename D::Scalar, 10, 1> vee(const MatrixBase<D> taux)
+{
+    MatrixSizeCheck<5, 5>::check(taux);
+
+    Matrix<typename D::Scalar, 5, 5> tau;
+
+    tau.segment<3>(0) =     taux.block<3,1>(0,4);  // p
+    tau.segment<3>(3) = vee(taux.block<3,3>(0,0)); // th
+    tau.segment<3>(6) =     taux.block<3,1>(0,3);  // v
+    tau           (9) =     taux           (3,4);  // t
+
+    return tau;
+}
+
+template<typename D>
+inline Matrix<typename D::Scalar, 10, 10> smalladjoint(const MatrixBase<D> tau)
+{
+    MatrixSizeCheck<10, 1>::check(tau);
 
+    typedef typename D::Scalar T;
 
     Map<const Matrix<T, 3, 1> >   p   ( & tau(0) );
     Map<const Matrix<T, 3, 1> >   th  ( & tau(3) );
-- 
GitLab