diff --git a/src/test/gtest_odom_2D.cpp b/src/test/gtest_odom_2D.cpp index dca613a8f69513ffbcc9ebe4d10aa7400ac28339..f4f0f22a619ab8fa6dae7871322b78dd6a3fc481 100644 --- a/src/test/gtest_odom_2D.cpp +++ b/src/test/gtest_odom_2D.cpp @@ -402,7 +402,7 @@ TEST(Odom2D, KF_callback) ceres_manager.computeCovariances(ALL_MARGINALS); ASSERT_POSE2D_APPROX(problem->getLastKeyFramePtr()->getState() , integrated_pose_vector[n_split], 1e-6); - ASSERT_MATRIX_APPROX(problem->getLastKeyFrameCovariance() , integrated_cov_vector [n_split], 1e-6); + ASSERT_MATRIX_APPROX(problem->getLastKeyFrameCovariance() , integrated_cov_vector [n_split], 1e-6); //////////////////////////////////////////////////////////////// // Split between keyframes, exact timestamp @@ -410,6 +410,8 @@ TEST(Odom2D, KF_callback) t_split = t0 + m_split*dt; // std::cout << "-----------------------------\nSplit between KFs; time: " << t_split - t0 << std::endl; + problem->print(4,1,1,1); + x_split = processor_odom2d->getState(t_split); FrameBasePtr keyframe_1 = problem->emplaceFrame(KEY_FRAME, x_split, t_split);