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

Fix gtest Odom2D

parent 9f3e5396
No related branches found
No related tags found
1 merge request!157Kfpackmanager
...@@ -322,6 +322,7 @@ TEST(Odom2D, KF_callback) ...@@ -322,6 +322,7 @@ TEST(Odom2D, KF_callback)
Matrix3s unmeasured_cov = params->unmeasured_perturbation_std_*params->unmeasured_perturbation_std_*Matrix3s::Identity(); Matrix3s unmeasured_cov = params->unmeasured_perturbation_std_*params->unmeasured_perturbation_std_*Matrix3s::Identity();
ProcessorBasePtr prc_base = problem->installProcessor("ODOM 2D", "odom", sensor_odom2d, params); ProcessorBasePtr prc_base = problem->installProcessor("ODOM 2D", "odom", sensor_odom2d, params);
ProcessorOdom2DPtr processor_odom2d = std::static_pointer_cast<ProcessorOdom2D>(prc_base); ProcessorOdom2DPtr processor_odom2d = std::static_pointer_cast<ProcessorOdom2D>(prc_base);
processor_odom2d->setTimeTolerance(dt/2);
// Ceres wrapper // Ceres wrapper
CeresManager ceres_manager(problem); CeresManager ceres_manager(problem);
...@@ -390,7 +391,11 @@ TEST(Odom2D, KF_callback) ...@@ -390,7 +391,11 @@ TEST(Odom2D, KF_callback)
FrameBasePtr keyframe_2 = problem->emplaceFrame(KEY_FRAME, x_split, t_split); FrameBasePtr keyframe_2 = problem->emplaceFrame(KEY_FRAME, x_split, t_split);
ASSERT_TRUE(problem->check(0)); ASSERT_TRUE(problem->check(0));
processor_odom2d->keyFrameCallback(keyframe_2, 0); processor_odom2d->keyFrameCallback(keyframe_2, dt/2);
ASSERT_TRUE(problem->check(0));
t += dt;
capture->setTimeStamp(t);
processor_odom2d->process(capture);
ASSERT_TRUE(problem->check(0)); ASSERT_TRUE(problem->check(0));
CaptureMotionPtr key_capture_n = std::static_pointer_cast<CaptureMotion>(keyframe_2->getCaptureList().front()); CaptureMotionPtr key_capture_n = std::static_pointer_cast<CaptureMotion>(keyframe_2->getCaptureList().front());
...@@ -416,7 +421,11 @@ TEST(Odom2D, KF_callback) ...@@ -416,7 +421,11 @@ TEST(Odom2D, KF_callback)
FrameBasePtr keyframe_1 = problem->emplaceFrame(KEY_FRAME, x_split, t_split); FrameBasePtr keyframe_1 = problem->emplaceFrame(KEY_FRAME, x_split, t_split);
ASSERT_TRUE(problem->check(0)); ASSERT_TRUE(problem->check(0));
processor_odom2d->keyFrameCallback(keyframe_1, 0); processor_odom2d->keyFrameCallback(keyframe_1, dt/2);
ASSERT_TRUE(problem->check(0));
t += dt;
capture->setTimeStamp(t);
processor_odom2d->process(capture);
ASSERT_TRUE(problem->check(0)); ASSERT_TRUE(problem->check(0));
CaptureMotionPtr key_capture_m = std::static_pointer_cast<CaptureMotion>(keyframe_1->getCaptureList().front()); CaptureMotionPtr key_capture_m = std::static_pointer_cast<CaptureMotion>(keyframe_1->getCaptureList().front());
...@@ -429,7 +438,6 @@ TEST(Odom2D, KF_callback) ...@@ -429,7 +438,6 @@ TEST(Odom2D, KF_callback)
keyframe_2->setState(Vector3s(3,1,2)); keyframe_2->setState(Vector3s(3,1,2));
report = ceres_manager.solve(1); report = ceres_manager.solve(1);
// std::cout << report << std::endl;
ceres_manager.computeCovariances(ALL_MARGINALS); ceres_manager.computeCovariances(ALL_MARGINALS);
// check the split KF // check the split KF
......
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