From ebc3a6c9bc0abaedc4d2cac39024e0dd5367994f Mon Sep 17 00:00:00 2001
From: Joan Sola <jsola@iri.upc.edu>
Date: Wed, 21 Mar 2018 14:51:28 +0100
Subject: [PATCH] Fix some oldities in test so that it works again
---
src/examples/test_kf_callback.cpp | 24 +++++++++++++++---------
1 file changed, 15 insertions(+), 9 deletions(-)
diff --git a/src/examples/test_kf_callback.cpp b/src/examples/test_kf_callback.cpp
index 75d0c2939..d8becd821 100644
--- a/src/examples/test_kf_callback.cpp
+++ b/src/examples/test_kf_callback.cpp
@@ -21,7 +21,10 @@ int main()
ProblemPtr problem = Problem::create("PO 2D");
SensorBasePtr sen_odo = problem->installSensor ("ODOM 2D", "main odometer", (Vector3s()<<0,0,0).finished(),"");
- ProcessorBasePtr prc_odo = problem->installProcessor("ODOM 2D", "odometry integrator", "main odometer", "");
+ ProcessorParamsOdom2DPtr params_odo = std::make_shared<ProcessorParamsOdom2D>();
+ params_odo->elapsed_time_th_ = 2;
+ params_odo->theta_traveled_th_ = M_PI; // 180 degrees turn
+ ProcessorBasePtr prc_odo = problem->installProcessor("ODOM 2D", "odometry integrator", sen_odo, params_odo);
prc_odo->setTimeTolerance(0.1);
SensorBasePtr sen_ftr = problem->installSensor ("ODOM 2D", "other odometer", (Vector3s()<<0,0,0).finished(),"");
@@ -34,29 +37,32 @@ int main()
cout << "Motion processor : " << problem->getProcessorMotionPtr()->getName() << endl;
TimeStamp t(0);
+ cout << "=======================\n>> TIME: " << t.get() << endl;
Vector3s x({0,0,0});
- problem->getProcessorMotionPtr()->setOrigin(x, t);
+ Matrix3s P; P.setZero();
+ problem->setPrior(x, P, t, 0.01);
- cout << "x(0) = " << problem->getCurrentState().transpose() << endl;
+ cout << "x(" << t.get() << ") = " << problem->getCurrentState().transpose() << endl;
- Vector2s odo_data; odo_data << .1, (M_PI / 2);
+ Vector2s odo_data; odo_data << .1, (M_PI / 10);
- problem->print(2, false, true, false); // print(level, constr_by, metric, state_blocks)
+ problem->print(2, false, true, true); // print(level, constr_by, metric, state_blocks)
Scalar dt = 1;
for (auto i = 0; i < 4; i++)
{
- t += dt;
- cout << "=======================\n>> TIME: " << t.get() << endl;
cout << "Tracker----------------" << endl;
sen_ftr->process(make_shared<CaptureVoid>(t, sen_ftr));
- problem->print(2, false, true, false); // print(level, constr_by, metric, state_blocks)
+ problem->print(2, false, true, true); // print(level, constr_by, metric, state_blocks)
+
+ t += dt;
+ cout << "=======================\n>> TIME: " << t.get() << endl;
cout << "Motion-----------------" << endl;
sen_odo->process(make_shared<CaptureMotion>(t, sen_odo, odo_data, 3, 3, nullptr));
cout << "x(" << t.get() << ") = " << problem->getCurrentState().transpose() << endl;
- problem->print(2, false, true, false); // print(level, constr_by, metric, state_blocks)
+ problem->print(2, false, true, true); // print(level, constr_by, metric, state_blocks)
}
--
GitLab