Testing the Adjoint matrix, WIP

include/IMU/math/IMU_tools_Lie.h

@@ -78,41 +78,6 @@ inline Matrix<T, 11, 1> identity()
     return ret;
 }
 
-template<typename D1, typename D2>
-inline void adjoint(const MatrixBase<D1> d, MatrixBase<D1> adjd)
-{
-    // Adjoint matrix associated to the adjoint operator
-    Matrix<typename D1::Scalar, 3, 1> dp = d.segment<3>(0);
-    Matrix<typename D1::Scalar, 3, 3> dR = q2R(d.segment<4>(3));
-    Matrix<typename D1::Scalar, 3, 1> dv = d.segment<3>(7);
-    typename D1::Scalar dt = d(10);
-
-    adjd.setIdentity();
-    adjd<3,3>(0,0) = dR;
-    adjd<3,3>(0,3) = skew(dp - dv * dt) * dR;
-    adjd<3,3>(0,6) = -dR * dt;
-    adjd<3,1>(0,9) = dv;
-    adjd<3,3>(3,3) = dR;
-    adjd<3,3>(6,3) = skew(dv) * dR;
-    adjd<3,3>(6,6) = dR;
-}
-
-template<typename D>
-inline Matrix<typename D::Scalar, 10, 10> adjoint(const MatrixBase<D> d){
-    Matrix<typename D::Scalar, 10, 10> adjd;
-    adjoint(d, adjd);
-    return adjd;
-}
-
-
-// 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  
-    
-//     return ret;
-// }
 
 template<typename D1, typename D2, typename D3, typename D4, typename D5, typename D6, typename T>
 inline void inverse(const MatrixBase<D1>& dp, const QuaternionBase<D2>& dq, const MatrixBase<D3>& dv, const T& dt,
@@ -217,6 +182,47 @@ inline Matrix<typename D1::Scalar, 11, 1> compose(const MatrixBase<D1>& delta1,
     return ret;
 }
 
+
+template<typename D1, typename D2>
+inline void adjoint(const MatrixBase<D1>& d, MatrixBase<D2>& adjd)
+{
+    MatrixSizeCheck<11, 1>::check(d);
+    MatrixSizeCheck<10, 10>::check(adjd);
+
+    // Adjoint matrix associated to the adjoint operator
+    Matrix<typename D1::Scalar, 3, 1> dp ( & d( 0 ) );
+    Matrix<typename D1::Scalar, 3, 3> dR ( & d( 3 ) );
+    Matrix<typename D1::Scalar, 3, 1> dv ( & d( 7 ) );
+    const typename D1::Scalar& dt = d(10);
+    
+    // pqvt impl -> problem with gtest
+    adjd.setIdentity();
+    adjd.block(0,0,3,3) = dR;
+    adjd.block(0,3,3,3) = skew(dp - dv * dt) * dR;
+    adjd.block(0,6,3,3) = -dR * dt;
+    adjd.block(0,9,3,1) = dv;
+    adjd.block(3,3,3,3) = dR;
+    adjd.block(6,3,3,3) = skew(dv) * dR;
+    adjd.block(6,6,3,3) = dR;
+}
+
+template<typename D>
+inline Matrix<typename D::Scalar, 10, 10> adjoint(const MatrixBase<D>& delta){
+    Matrix<typename D::Scalar, 10, 10> adjd;
+    adjoint(delta, adjd);
+    return adjd;
+}
+
+
+// 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  
+    
+//     return ret;
+// }
+
 template<typename D1, typename D2, typename D3, typename D4, typename D5>
 inline void compose(const MatrixBase<D1>& delta1,
                     const MatrixBase<D2>& delta2,

test/gtest_IMU_tools_Lie.cpp

@@ -161,12 +161,24 @@ TEST(IMU_tools, plus_minus)
     Vector4s qv2 = (Vector4s() << 6, 5, 4, 3).finished().normalized();
     delta1 << 0, 1, 2, qv1, 7, 8, 9, 0.1;
     delta2 << 10, 11, 12, qv2, 17, 18, 19, 0.3;
-    err << 0.01, 0.02, 0.03, 0.04, 0.05, 0.06, 0.07, 0.08, 0.09, 0.1;
 
     delta2_eq = plus(delta1, diff(delta1, delta2));
     ASSERT_MATRIX_APPROX(delta2_eq, delta2, 1e-10);
 }
 
+TEST(IMU_tools, adjazdzadazoint)
+{
+    VectorXs delta1(11), delta2(11);
+    Vector4s qv1 = (Vector4s() << 3, 4, 5, 6).finished().normalized();
+    Vector4s qv2 = (Vector4s() << 6, 5, 4, 3).finished().normalized();
+    delta1 << 0, 1, 2, qv1, 7, 8, 9, 0.1;
+    delta2 << 10, 11, 12, qv2, 17, 18, 19, 0.3;
+
+    ASSERT_MATRIX_APPROX(adjoint(delta1).inverse(), adjoint(inverse(delta1)), 1e-6);
+    // ASSERT_MATRIX_APPROX(adjoint(compose(delta1, delta2)), adjoint(delta1)*adjoint(delta2), 1e-10);
+}
+
+
 // TEST(IMU_tools, plus)
 // {
     // FORSTER's formulas -> TODO remove?