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

Add inverse() to imu_tools.h and gtest it

parent 50512c42
No related branches found
No related tags found
1 merge request!123Calibration
...@@ -26,20 +26,43 @@ inline Matrix<T, 10, 1> identity() ...@@ -26,20 +26,43 @@ inline Matrix<T, 10, 1> identity()
return ret; return ret;
} }
//template<> template<typename D1, typename D2>
//inline Matrix<Scalar, 10, 1> identity() inline void inverse(const MatrixBase<D1>& d,
//{ typename D1::Scalar dt,
// Matrix<Scalar, 10, 1> ret; MatrixBase<D2>& id)
// ret.setZero(); {
// ret(6, 1) = 1.0; MatrixSizeCheck<10, 1>::check(d);
// return ret; MatrixSizeCheck<10, 1>::check(id);
//}
Map<const Matrix<typename D1::Scalar, 3, 1> > dp ( &d( 0 ) );
Map<const Quaternion<typename D1::Scalar> > dq ( &d( 3 ) );
Map<const Matrix<typename D1::Scalar, 3, 1> > dv ( &d( 7 ) );
Map<Matrix<typename D2::Scalar, 3, 1> > idp ( &id( 0 ));
Map<Quaternion<typename D2::Scalar> > idq ( &id( 3 ));
Map<Matrix<typename D2::Scalar, 3, 1> > idv ( &id( 7 ));
idp = - ( dq.conjugate() * (dp - dv * dt) );
idv = - ( dq.conjugate() * dv );
idq = dq.conjugate();
}
template<typename D>
inline Matrix<typename D::Scalar, 10, 1> inverse(const MatrixBase<D>& d,
typename D::Scalar dt)
{
Matrix<typename D::Scalar, 10, 1> id;
inverse(d, dt, id);
return id;
}
template<typename D1, typename D2, typename D3> template<typename D1, typename D2, typename D3>
inline void compose(const MatrixBase<D1>& d1, inline void compose(const MatrixBase<D1>& d1,
const MatrixBase<D2>& d2, const MatrixBase<D2>& d2,
const Scalar& dt, Scalar dt,
MatrixBase<D3>& sum) MatrixBase<D3>& sum)
{ {
MatrixSizeCheck<10, 1>::check(d1); MatrixSizeCheck<10, 1>::check(d1);
MatrixSizeCheck<10, 1>::check(d2); MatrixSizeCheck<10, 1>::check(d2);
...@@ -56,15 +79,62 @@ inline void compose(const MatrixBase<D1>& d1, ...@@ -56,15 +79,62 @@ inline void compose(const MatrixBase<D1>& d1,
Map<Matrix<typename D3::Scalar, 3, 1> > sum_v ( &sum( 7 )); Map<Matrix<typename D3::Scalar, 3, 1> > sum_v ( &sum( 7 ));
sum_p = dp1 + dv1*dt + dq1*dp2; sum_p = dp1 + dv1*dt + dq1*dp2;
sum_q = dq1*dq2;
sum_v = dv1 + dq1*dv2; sum_v = dv1 + dq1*dv2;
sum_q = dq1*dq2; // dq here to avoid possible aliasing between d1 and sum
}
template<typename D1, typename D2, typename D3, typename D4, typename D5>
inline void compose(const MatrixBase<D1>& d1,
const MatrixBase<D2>& d2,
Scalar dt,
MatrixBase<D3>& sum,
MatrixBase<D4>& J_sum_d1,
MatrixBase<D5>& J_sum_d2)
{
MatrixSizeCheck<10, 1>::check(d1);
MatrixSizeCheck<10, 1>::check(d2);
MatrixSizeCheck<10, 1>::check(sum);
MatrixSizeCheck< 9, 1>::check(J_sum_d1);
MatrixSizeCheck< 9, 1>::check(J_sum_d2);
// Maps over provided data
Map<const Matrix<typename D1::Scalar, 3, 1> > dp1 ( &d1( 0 ) );
Map<const Quaternion<typename D1::Scalar> > dq1 ( &d1( 3 ) );
Map<const Matrix<typename D1::Scalar, 3, 1> > dv1 ( &d1( 7 ) );
Map<const Matrix<typename D2::Scalar, 3, 1> > dp2 ( &d2( 0 ) );
Map<const Quaternion<typename D2::Scalar> > dq2 ( &d2( 3 ) );
Map<const Matrix<typename D2::Scalar, 3, 1> > dv2 ( &d2( 7 ) );
Map<Matrix<typename D3::Scalar, 3, 1> > sum_p ( &sum( 0 ));
Map<Quaternion<typename D3::Scalar> > sum_q ( &sum( 3 ));
Map<Matrix<typename D3::Scalar, 3, 1> > sum_v ( &sum( 7 ));
// Some useful temporaries
Matrix<typename D1::Scalar, 3, 3> dR1 = dq1.matrix(); // First Delta, DR
Matrix<typename D2::Scalar, 3, 3> dR2 = dq2.matrix(); // Second delta, dR
// Jac wrt first delta
J_sum_d1.setIdentity(); // dDp'/dDp = dDv'/dDv = I
J_sum_d1.block(0,3,3,3).noalias() = - dR1 * skew(dp2) ; // dDp'/dDo
J_sum_d1.block(0,6,3,3) = Matrix3s::Identity() * dt; // dDp'/dDv = I*dt
J_sum_d1.block(3,3,3,3) = dR2.transpose(); // dDo'/dDo
J_sum_d1.block(6,3,3,3).noalias() = - dR1 * skew(dv2) ; // dDv'/dDo
// Jac wrt second delta
J_sum_d2.setIdentity(); //
J_sum_d2.block(0,0,3,3) = dR1; // dDp'/ddp
J_sum_d2.block(6,6,3,3) = dR1; // dDv'/ddv
// J_sum_d2.block(3,3,3,3) = Matrix3s::Identity(); // dDo'/ddo = I
// compose deltas -- done here to avoid aliasing when calling with `d1` and `sum` pointing to the same variable
compose(d1, d2, dt, sum);
} }
template<typename D1, typename D2, typename D3> template<typename D1, typename D2, typename D3>
inline void between(const MatrixBase<D1>& d1, inline void between(const MatrixBase<D1>& d1,
const MatrixBase<D2>& d2, const MatrixBase<D2>& d2,
const Scalar& dt, Scalar dt,
MatrixBase<D3>& d2_minus_d1) MatrixBase<D3>& d2_minus_d1)
{ {
MatrixSizeCheck<10, 1>::check(d1); MatrixSizeCheck<10, 1>::check(d1);
MatrixSizeCheck<10, 1>::check(d2); MatrixSizeCheck<10, 1>::check(d2);
...@@ -129,8 +199,8 @@ Matrix<typename Derived::Scalar, 10, 1> retract(const MatrixBase<Derived>& d_in) ...@@ -129,8 +199,8 @@ Matrix<typename Derived::Scalar, 10, 1> retract(const MatrixBase<Derived>& d_in)
template<typename D1, typename D2, typename D3> template<typename D1, typename D2, typename D3>
inline void compare(const MatrixBase<D1>& d1, inline void compare(const MatrixBase<D1>& d1,
const MatrixBase<D2>& d2, const MatrixBase<D2>& d2,
MatrixBase<D3>& err) MatrixBase<D3>& err)
{ {
Matrix<typename D3::Scalar, 10, 1> delta_err; Matrix<typename D3::Scalar, 10, 1> delta_err;
......
...@@ -56,13 +56,9 @@ target_link_libraries(gtest_feature_base ${PROJECT_NAME}) ...@@ -56,13 +56,9 @@ target_link_libraries(gtest_feature_base ${PROJECT_NAME})
wolf_add_gtest(gtest_frame_base gtest_frame_base.cpp) wolf_add_gtest(gtest_frame_base gtest_frame_base.cpp)
target_link_libraries(gtest_frame_base ${PROJECT_NAME}) target_link_libraries(gtest_frame_base ${PROJECT_NAME})
# FrameIMU class test
wolf_add_gtest(gtest_frame_imu gtest_frame_imu.cpp)
target_link_libraries(gtest_frame_imu ${PROJECT_NAME})
# IMU tools test # IMU tools test
wolf_add_gtest(gtest_imu_tools gtest_imu_tools.cpp) wolf_add_gtest(gtest_imu_tools gtest_imu_tools.cpp)
target_link_libraries(gtest_imu_tools ${PROJECT_NAME}) # target_link_libraries(gtest_imu_tools ${PROJECT_NAME}) // WOLF library not needed
# LocalParametrizationXxx classes test # LocalParametrizationXxx classes test
wolf_add_gtest(gtest_local_param gtest_local_param.cpp) wolf_add_gtest(gtest_local_param gtest_local_param.cpp)
...@@ -110,6 +106,10 @@ target_link_libraries(gtest_constraint_odom_3D ${PROJECT_NAME}) ...@@ -110,6 +106,10 @@ target_link_libraries(gtest_constraint_odom_3D ${PROJECT_NAME})
wolf_add_gtest(gtest_feature_imu gtest_feature_imu.cpp) wolf_add_gtest(gtest_feature_imu gtest_feature_imu.cpp)
target_link_libraries(gtest_feature_imu ${PROJECT_NAME}) target_link_libraries(gtest_feature_imu ${PROJECT_NAME})
# FrameIMU class test
wolf_add_gtest(gtest_frame_imu gtest_frame_imu.cpp)
target_link_libraries(gtest_frame_imu ${PROJECT_NAME})
# IMU Bias + estimation tests # IMU Bias + estimation tests
#wolf_add_gtest(gtest_imuBias gtest_imuBias.cpp) #wolf_add_gtest(gtest_imuBias gtest_imuBias.cpp)
#target_link_libraries(gtest_imuBias ${PROJECT_NAME}) #target_link_libraries(gtest_imuBias ${PROJECT_NAME})
......
...@@ -19,10 +19,27 @@ TEST(IMU_tools, identity) ...@@ -19,10 +19,27 @@ TEST(IMU_tools, identity)
id_true.setZero(10); id_true.setZero(10);
id_true(6) = 1.0; id_true(6) = 1.0;
VectorXs id = imu::identity(); VectorXs id = imu::identity<Scalar>();
ASSERT_MATRIX_APPROX(id, id_true, 1e-10); ASSERT_MATRIX_APPROX(id, id_true, 1e-10);
} }
TEST(IMU_tools, inverse)
{
VectorXs d(10), id(10), iid(10), iiid(10);
Vector4s qv;
Scalar dt = 0.1;
qv = (Vector4s() << 3, 4, 5, 6).finished().normalized();
d << 0, 1, 2, qv, 7, 8, 9;
id = imu::inverse(d, dt);
imu::inverse(id, -dt, iid); // Observe -dt is reversed !!
iiid = imu::inverse(iid, dt);
ASSERT_MATRIX_APPROX( d, iid, 1e-10);
ASSERT_MATRIX_APPROX(id, iiid, 1e-10);
}
TEST(IMU_tools, compose_between) TEST(IMU_tools, compose_between)
{ {
VectorXs dx1(10), dx2(10), dx3(10); VectorXs dx1(10), dx2(10), dx3(10);
...@@ -51,7 +68,6 @@ TEST(IMU_tools, compose_between) ...@@ -51,7 +68,6 @@ TEST(IMU_tools, compose_between)
Matrix<Scalar,10,1> diff; Matrix<Scalar,10,1> diff;
imu::between(d1, d3, dt, diff); imu::between(d1, d3, dt, diff);
ASSERT_MATRIX_APPROX(diff, d2, 1e-10); ASSERT_MATRIX_APPROX(diff, d2, 1e-10);
} }
TEST(IMU_tools, lift_retract) TEST(IMU_tools, lift_retract)
......
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