Skip to content
Snippets Groups Projects
Commit f302d009 authored by Joan Solà Ortega's avatar Joan Solà Ortega
Browse files

Fix adjoint()

parent f2667665
No related branches found
No related tags found
No related merge requests found
...@@ -191,11 +191,12 @@ inline void adjoint(const MatrixBase<D1>& d, MatrixBase<D2>& adjd) ...@@ -191,11 +191,12 @@ inline void adjoint(const MatrixBase<D1>& d, MatrixBase<D2>& adjd)
// Adjoint matrix associated to the adjoint operator // Adjoint matrix associated to the adjoint operator
Matrix<typename D1::Scalar, 3, 1> dp ( & d( 0 ) ); Matrix<typename D1::Scalar, 3, 1> dp ( & d( 0 ) );
Matrix<typename D1::Scalar, 3, 3> dR ( & d( 3 ) ); Quaternion<typename D1::Scalar> dq ( &d(3) );
Matrix<typename D1::Scalar, 3, 3> dR = q2R(dq);
Matrix<typename D1::Scalar, 3, 1> dv ( & d( 7 ) ); Matrix<typename D1::Scalar, 3, 1> dv ( & d( 7 ) );
const typename D1::Scalar& dt = d(10); const typename D1::Scalar& dt = d(10);
// pqvt impl -> problem with gtest // pqvt impl
adjd.setIdentity(); adjd.setIdentity();
adjd.block(0,0,3,3) = dR; adjd.block(0,0,3,3) = dR;
adjd.block(0,3,3,3) = skew(dp - dv * dt) * dR; adjd.block(0,3,3,3) = skew(dp - dv * dt) * dR;
...@@ -496,7 +497,7 @@ Matrix<typename Derived::Scalar, 10, 1> log_IMU(const MatrixBase<Derived>& delta ...@@ -496,7 +497,7 @@ Matrix<typename Derived::Scalar, 10, 1> log_IMU(const MatrixBase<Derived>& delta
dv_ret = Qinv*dv_in; dv_ret = Qinv*dv_in;
dt_ret = dt_in; dt_ret = dt_in;
std::cout << "Log ret" << ret.transpose() << std::endl; // std::cout << "Log ret" << ret.transpose() << std::endl;
return ret; return ret;
} }
......
...@@ -166,7 +166,7 @@ TEST(IMU_tools, plus_minus) ...@@ -166,7 +166,7 @@ TEST(IMU_tools, plus_minus)
ASSERT_MATRIX_APPROX(delta2_eq, delta2, 1e-10); ASSERT_MATRIX_APPROX(delta2_eq, delta2, 1e-10);
} }
TEST(IMU_tools, adjazdzadazoint) TEST(IMU_tools, adjoint)
{ {
VectorXs delta1(11), delta2(11); VectorXs delta1(11), delta2(11);
Vector4s qv1 = (Vector4s() << 3, 4, 5, 6).finished().normalized(); Vector4s qv1 = (Vector4s() << 3, 4, 5, 6).finished().normalized();
...@@ -174,8 +174,8 @@ TEST(IMU_tools, adjazdzadazoint) ...@@ -174,8 +174,8 @@ TEST(IMU_tools, adjazdzadazoint)
delta1 << 0, 1, 2, qv1, 7, 8, 9, 0.1; delta1 << 0, 1, 2, qv1, 7, 8, 9, 0.1;
delta2 << 10, 11, 12, qv2, 17, 18, 19, 0.3; 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(delta1).inverse(), adjoint(inverse(delta1)), 1e-10);
// ASSERT_MATRIX_APPROX(adjoint(compose(delta1, delta2)), adjoint(delta1)*adjoint(delta2), 1e-10); ASSERT_MATRIX_APPROX(adjoint(compose(delta1, delta2)), adjoint(delta1)*adjoint(delta2), 1e-10);
} }
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment