diff --git a/include/IMU/math/IMU_tools_Lie.h b/include/IMU/math/IMU_tools_Lie.h index 72b3e5236c495971dccc8c8aaf5fe84af936c48b..41a046762fb1f4712f5d3a0974be9ff4961e7c3d 100644 --- a/include/IMU/math/IMU_tools_Lie.h +++ b/include/IMU/math/IMU_tools_Lie.h @@ -516,12 +516,13 @@ Matrix<typename Derived::Scalar, 11, 1> exp_IMU(const MatrixBase<Derived>& d_in) typedef typename Derived::Scalar T; - Matrix<T, 11, 1> ret; - Map<const Matrix<T, 3, 1> > dp_in ( & d_in(0) ); Map<const Matrix<T, 3, 1> > do_in ( & d_in(3) ); Map<const Matrix<T, 3, 1> > dv_in ( & d_in(6) ); const T& dt_in = d_in(9); + + Matrix<T, 11, 1> ret; + Map<Matrix<T, 3, 1> > dp ( & ret(0) ); Map<Quaternion<T> > dq ( & ret(3) ); Map<Matrix<T, 3, 1> > dv ( & ret(7) ); diff --git a/test/gtest_IMU_tools_Lie.cpp b/test/gtest_IMU_tools_Lie.cpp index 2b6fa4653f54d107ec01e755f3cadbb20201de2b..adf91fba9325af594474f8e9d6bb0ee6a4d176e8 100644 --- a/test/gtest_IMU_tools_Lie.cpp +++ b/test/gtest_IMU_tools_Lie.cpp @@ -169,13 +169,21 @@ TEST(IMU_tools, plus_minus) TEST(IMU_tools, adjoint) { VectorXs delta1(11), delta2(11); + VectorXs tau(10); 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; + tau << .1, .2, .3, -.1, -.2, -.3, .3, .2, .1, 0.1; ASSERT_MATRIX_APPROX(adjoint(delta1).inverse(), adjoint(inverse(delta1)), 1e-10); ASSERT_MATRIX_APPROX(adjoint(compose(delta1, delta2)), adjoint(delta1)*adjoint(delta2), 1e-10); + + MatrixXs tau2 = adjoint(delta1)*tau; +// MatrixXs d3 = compose(delta1, exp_IMU(tau)); +// MatrixXs d4 = compose(exp_IMU(tau2),delta1); + ASSERT_MATRIX_APPROX(compose(delta1, exp_IMU(tau)), compose(exp_IMU(tau2),delta1), 1e-10); +// ASSERT_MATRIX_APPROX(compose(delta1, exp_IMU(tau2)), compose(exp_IMU(tau),delta1), 1e-10); }