Add smalladj()

include/IMU/math/IMU_tools_Lie.h

@@ -215,14 +215,44 @@ inline Matrix<typename D::Scalar, 10, 10> adjoint(const MatrixBase<D>& delta){
 }
 
 
-// template<typename D1, typename D2>
-// inline Matrix<T, 11, 1> smalladjoint(const MatrixBase<D1> d, MatrixBase<D1> sadjd)
-// {
-//     // Adjoint matrix associated to the adjoint operator
-//         TODO  
+template<typename D1, typename D2>
+inline Matrix<T, 10, 10> smalladjoint(const MatrixBase<D1> tau)
+{
+    MatrixSizeCheck<10, 1>::check(tau);
+
+    typedef typename D1::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);
+
+    // Adjoint matrix associated to the adjoint operator
+    Matrix<T, 3, 3> px  = skew(p);
+    Matrix<T, 3, 3> thx = skew(th);
+    Matrix<T, 3, 3> vx  = skew(v);
+
+    Matrix<T, 10, 10> adj;
+    adj.setZero();
     
-//     return ret;
-// }
+    /* Attention to variable order in the adjoint!
+     *
+     * Paper: PVOT = 0639
+     * WOLF:  POVT = 0369
+     *
+     */
+
+    adj.block<3,3>(0,0) = thx;                              // 0,0 in paper
+    adj.block<3,3>(0,3) = px;                               // 0,6 in paper
+    adj.block<3,3>(0,6) = - Matrix<T,3,3>::Identity() * t;  // 0,3 in paper
+    adj.block<3,1>(0,9) = v;                                // 0,9 in paper
+    adj.block<3,3>(3,3) = thx;                              // 6,6 in paper
+    adj.block<3,3>(6,3) = vx;                               // 3,6 in paper
+    adj.block<3,3>(6,6) = thx;                              // 3,3 in paper
+
+    return adj;
+}
 
 template<typename D1, typename D2, typename D3, typename D4, typename D5>
 inline void compose(const MatrixBase<D1>& delta1,