From 598808df8ff6ebd69e4e8d03328d46f2fc9c7ce5 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Joan=20Sol=C3=A0?= <jsola@iri.upc.edu>
Date: Thu, 24 Oct 2019 14:23:50 +0200
Subject: [PATCH] Add smalladj()
---
include/IMU/math/IMU_tools_Lie.h | 44 +++++++++++++++++++++++++++-----
1 file changed, 37 insertions(+), 7 deletions(-)
diff --git a/include/IMU/math/IMU_tools_Lie.h b/include/IMU/math/IMU_tools_Lie.h
index a2140e099..0bf90615a 100644
--- a/include/IMU/math/IMU_tools_Lie.h
+++ b/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,
--
GitLab