Add small adjoint, hat, vee

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