diff --git a/src/test/gtest_odom_3D.cpp b/src/test/gtest_odom_3D.cpp index 2b83a0278befb8a5a14a8ffe99811c025806e8b3..0632acd149f425954c39cbd98e965e045d38bf77 100644 --- a/src/test/gtest_odom_3D.cpp +++ b/src/test/gtest_odom_3D.cpp @@ -26,9 +26,11 @@ VectorXs DDo(7); \ for (int i = 0; i< 7; i++) {\ dd = d;\ + DD = D; \ dd(i) += dx;\ prc_ptr->deltaPlusDelta(D, dd, dt, DDo);\ J_d.col(i) = (DDo - Do)/dx; \ + dd = d;\ DD = D; \ DD(i) += dx; \ prc_ptr->deltaPlusDelta(DD, d, dt, DDo); \ @@ -45,7 +47,7 @@ using namespace wolf; class ProcessorOdom3DTest : public ProcessorOdom3D { public: - ProcessorOdom3DTest() : ProcessorOdom3D() {} + ProcessorOdom3DTest(); // getters :-D !! VectorXs& delta() {return delta_;} @@ -55,9 +57,14 @@ class ProcessorOdom3DTest : public ProcessorOdom3D Scalar& kdd() {return k_disp_to_disp_;} Scalar& kdr() {return k_disp_to_rot_;} Scalar& krr() {return k_rot_to_rot_;} - Scalar& dvar() {return min_disp_var_;} - Scalar& rvar() {return min_rot_var_;} + Scalar& dvar_min() {return min_disp_var_;} + Scalar& rvar_min() {return min_rot_var_;} }; +ProcessorOdom3DTest::ProcessorOdom3DTest() : ProcessorOdom3D() +{ + dvar_min() = 0.5; + rvar_min() = 0.25; +} TEST(ProcessorOdom3D, data2delta) { @@ -80,8 +87,8 @@ TEST(ProcessorOdom3D, data2delta) // construct covariance from processor parameters and motion magnitudes Scalar disp = data_dp.norm(); Scalar rot = data_do.norm(); - Scalar dvar = prc.dvar() + prc.kdd()*disp; - Scalar rvar = prc.rvar() + prc.kdr()*disp + prc.krr()*rot; + Scalar dvar = prc.dvar_min() + prc.kdd()*disp; + Scalar rvar = prc.rvar_min() + prc.kdr()*disp + prc.krr()*rot; Vector6s diag; diag << dvar, dvar, dvar, rvar, rvar, rvar; Matrix6s data_cov = diag.asDiagonal(); Matrix6s delta_cov = data_cov; @@ -100,39 +107,47 @@ TEST(ProcessorOdom3D, deltaPlusDelta) VectorXs D(7); D.setRandom(); D.tail<4>().normalize(); VectorXs d(7); d.setRandom(); d *= 1; d.tail<4>().normalize(); - VectorXs Dpd_check(7); - // Dp <-- Dp + Dq * dp - // Dq <-- Dq * dq - Dpd_check.head<3>() = D.head<3>() + Quaternions(D.data()+3)*d.head<3>(); - Dpd_check.tail<4>() = (Quaternions(D.data()+3)*Quaternions(d.data()+3)).coeffs(); + // Integrated delta value to check aginst + // Dp_int <-- Dp + Dq * dp + // Dq_int <-- Dq * dq + VectorXs D_int_check(7); + D_int_check.head<3>() = D.head<3>() + Quaternions(D.data()+3) * d.head<3>(); + D_int_check.tail<4>() = (Quaternions(D.data()+3) * Quaternions(d.data()+3)).coeffs(); - Scalar dt = 1; // dummy + Scalar dt = 1; // dummy, not used in Odom3D - VectorXs Dpd(7); + VectorXs D_int(7); - prc.deltaPlusDelta(D, d, dt, Dpd); + prc.deltaPlusDelta(D, d, dt, D_int); - ASSERT_TRUE((Dpd - Dpd_check).isMuchSmallerThan(1, 1e-10)) - << "\nDpd : " << Dpd.transpose() - << "\ncheck: " << Dpd_check.transpose(); + ASSERT_TRUE((D_int - D_int_check).isMuchSmallerThan(1, 1e-10)) + << "\nDpd : " << D_int.transpose() + << "\ncheck: " << D_int_check.transpose(); } TEST(ProcessorOdom3D, deltaPlusDelta_Jac) { - ProcessorOdom3DTest prc; + std::shared_ptr<ProcessorOdom3DTest> prc_ptr = std::make_shared<ProcessorOdom3DTest>(); VectorXs D(7); D.setRandom(); D.tail<4>().normalize(); VectorXs d(7); d.setRandom(); d *= 1; d.tail<4>().normalize(); Scalar dt = 1; VectorXs Do(7); - MatrixXs DD(6,6); - MatrixXs Dd(6,6); + MatrixXs D_D(6,6); + MatrixXs D_d(6,6); + + prc_ptr->deltaPlusDelta(D, d, dt, Do, D_D, D_d); + + WOLF_DEBUG("DD:\n ", D_D); + WOLF_DEBUG("Dd:\n ", D_d); + + MatrixXs J_D(7,7), J_d(7,7); - prc.deltaPlusDelta(D, d, dt, Do, DD, Dd); + JAC_NUMERIC(prc_ptr, D, d, dt, J_D, J_d, Constants::EPS); + WOLF_DEBUG("J_D:\n ", J_D); + WOLF_DEBUG("J_d:\n ", J_d); - WOLF_DEBUG("DD:\n ", DD); - WOLF_DEBUG("Dd:\n ", Dd); }