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

Improve test with optional input argument of final TimeStamp

parent 0fad9201
No related branches found
No related tags found
No related merge requests found
...@@ -14,6 +14,8 @@ ...@@ -14,6 +14,8 @@
#include "landmark_base.h" #include "landmark_base.h"
#include "ceres_wrapper/ceres_manager.h" #include "ceres_wrapper/ceres_manager.h"
#include <cstdlib>
using namespace wolf; using namespace wolf;
using std::cout; using std::cout;
using std::endl; using std::endl;
...@@ -38,13 +40,14 @@ void print(Problem* P) ...@@ -38,13 +40,14 @@ void print(Problem* P)
cout << "T" << endl; cout << "T" << endl;
for (auto F : *(P->getTrajectoryPtr()->getFrameListPtr() ) ) for (auto F : *(P->getTrajectoryPtr()->getFrameListPtr() ) )
{ {
cout << (F->isKey() ? " KF" : " F") << F->id() << (F->isFixed() ? ", fixed" : ", estim") << ", ts=" << F->getTimeStamp().get() << endl; cout << (F->isKey() ? " KF" : " F") << F->id() << (F->isFixed() ? ", fixed" : ", estim") << ", ts=" << F->getTimeStamp().get();
cout << ",\t x = ( " << F->getState().transpose() << ")" << endl;
for (auto C : *(F->getCaptureListPtr() ) ) for (auto C : *(F->getCaptureListPtr() ) )
{ {
cout << " C" << C->id() << endl; cout << " C" << C->id() << endl;
for (auto f : *(C->getFeatureListPtr() ) ) for (auto f : *(C->getFeatureListPtr() ) )
{ {
cout << " f" << f->id() << ", m = ( " << std::setprecision(2) << f->getMeasurement().transpose() << ")" << endl; cout << " f" << f->id() << ", \t m = ( " << std::setprecision(2) << f->getMeasurement().transpose() << ")" << endl;
for (auto c : *(f->getConstraintListPtr() ) ) for (auto c : *(f->getConstraintListPtr() ) )
{ {
cout << " c" << c->id(); cout << " c" << c->id();
...@@ -75,8 +78,19 @@ void print(Problem* P) ...@@ -75,8 +78,19 @@ void print(Problem* P)
} }
int main () int main (int argc, char** argv)
{ {
cout << "\n========= Test ProcessorOdom3D ===========" << endl;
TimeStamp tf;
if (argc == 1)
tf = 0.3;
else
{
tf.set(strtod(argv[1],nullptr));
}
cout << "Final timestamp tf = " << tf.get() << " s" << endl;
Problem* problem = new Problem(FRM_PO_3D); Problem* problem = new Problem(FRM_PO_3D);
CeresManager ceres_manager(problem); CeresManager ceres_manager(problem);
...@@ -92,16 +106,20 @@ int main () ...@@ -92,16 +106,20 @@ int main ()
Scalar dt = 0.1; Scalar dt = 0.1;
for (TimeStamp t = dt; t < .4 - Constants::EPS; t += dt) cout << "t: " << 0 << " \t\t\t x = ( " << problem->getCurrentState().transpose() << ")" << endl;
print(problem);
cout << "--------------------------------------------------------------" << endl;
for (TimeStamp t = dt; t < tf - Constants::EPS; t += dt)
{ {
CaptureMotion* cap_odo = new CaptureMotion(t, sen, data); CaptureMotion* cap_odo = new CaptureMotion(t, sen, data);
cap_odo->process(); cap_odo->process();
cout << "t: " << t.get() << " x: " << problem->getCurrentState().transpose() << endl; cout << "t: " << t.get() << " \t\t x = ( " << problem->getCurrentState().transpose() << ")" << endl;
print(problem); print(problem);
cout << "--------------------------------------------------------------" << endl;
ceres::Solver::Summary summary = ceres_manager.solve(); ceres::Solver::Summary summary = ceres_manager.solve();
......
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