From daf3060acd8a742b8fd410893777d74e820c1100 Mon Sep 17 00:00:00 2001
From: Idril Geer <igeer@iri.upc.edu>
Date: Wed, 8 Sep 2021 16:04:11 +0200
Subject: [PATCH] more work on the test
---
demos/imu2d_static_init.yaml | 6 +-
test/gtest_imu2d_static_init.cpp | 98 ++++++++++++++++++--------------
2 files changed, 59 insertions(+), 45 deletions(-)
diff --git a/demos/imu2d_static_init.yaml b/demos/imu2d_static_init.yaml
index c45b96666..c30a7d38d 100644
--- a/demos/imu2d_static_init.yaml
+++ b/demos/imu2d_static_init.yaml
@@ -7,6 +7,6 @@ keyframe_vote:
voting_active: true
voting_aux_active: false
max_time_span: 1.0 # seconds
- max_buff_length: 20000 # motion deltas
- dist_traveled: 2.0 # meters
- angle_turned: 0.2 # radians (1 rad approx 57 deg, approx 60 deg)
+ max_buff_length: 20000 # motion deltas
+ dist_traveled: 999 # meters
+ angle_turned: 999 # radians (1 rad approx 57 deg, approx 60 deg)
diff --git a/test/gtest_imu2d_static_init.cpp b/test/gtest_imu2d_static_init.cpp
index 5270c87fa..e6cac3b0e 100644
--- a/test/gtest_imu2d_static_init.cpp
+++ b/test/gtest_imu2d_static_init.cpp
@@ -16,8 +16,8 @@ class ProcessorImu2dStaticInitTest : public testing::Test
public: //These can be accessed in fixtures
wolf::ProblemPtr _problem_ptr;
- wolf::SensorBasePtr sensor_ptr;
- wolf::ProcessorMotionPtr processor_motion;
+ wolf::SensorBasePtr _sensor_ptr;
+ wolf::ProcessorMotionPtr _processor_motion;
wolf::TimeStamp t;
wolf::TimeStamp t0;
double mti_clock, tmp;
@@ -28,7 +28,8 @@ class ProcessorImu2dStaticInitTest : public testing::Test
VectorComposite x0c; // initial state composite
VectorComposite s0c; // initial sigmas composite
std::shared_ptr<wolf::CaptureImu> C;
- FrameBasePtr last_frame_;
+ FrameBasePtr _first_frame;
+ FrameBasePtr _last_frame;
SolverCeresPtr _solver;
//a new of this will be created for each new test
@@ -45,9 +46,9 @@ class ProcessorImu2dStaticInitTest : public testing::Test
// Wolf problem
_problem_ptr = Problem::create("POV", 2);
Vector3d extrinsics = (Vector3d() << 0,0, 0).finished();
- sensor_ptr = _problem_ptr->installSensor("SensorImu2d", "Main Imu", extrinsics, wolf_root + "/demos/sensor_imu2d.yaml");
+ _sensor_ptr = _problem_ptr->installSensor("SensorImu2d", "Main Imu", extrinsics, wolf_root + "/demos/sensor_imu2d.yaml");
ProcessorBasePtr processor_ptr = _problem_ptr->installProcessor("ProcessorImu2d", "Imu pre-integrator", "Main Imu", wolf_root + "/demos/imu2d_static_init.yaml");
- processor_motion = std::static_pointer_cast<ProcessorMotion>(processor_ptr);
+ _processor_motion = std::static_pointer_cast<ProcessorMotion>(processor_ptr);
// Time and data variables
second_frame = false;
@@ -56,10 +57,10 @@ class ProcessorImu2dStaticInitTest : public testing::Test
t = t0;
data = Vector6d::Random();
data_cov = Matrix6d::Identity();
- last_frame_ = nullptr;
+ _last_frame = nullptr;
// Create one capture to store the Imu data arriving from (sensor / callback / file / etc.)
- C = make_shared<CaptureImu>(t, sensor_ptr, data, data_cov, Vector3d::Zero());
+ C = make_shared<CaptureImu>(t, _sensor_ptr, data, data_cov, Vector3d::Zero());
// Create the solver
_solver = make_shared<SolverCeres>(_problem_ptr);
@@ -189,66 +190,69 @@ TEST_F(ProcessorImu2dStaticInitTest, static)
x0c['V'] = Vector2d(0, 0);
data_cov *= 1e-3;
- //dt = 0.3;
+ data << 1, 2, 3, 4, 5, 6;
+ //dt = 0.0001;
auto KF0 = _problem_ptr->setPriorFix(x0c, t0, dt/2);
- processor_motion->setOrigin(KF0);
+ KF0->fix();
+ _processor_motion->setOrigin(KF0);
- data = Vector6d::Random();
WOLF_INFO("Data is: \n", data);
int size = 3;
second_frame = false;
- FrameBasePtr first_frame = nullptr;
+ _first_frame = nullptr;
int i = 1;
- for(t = t0+dt; t <= t0 + 2 + dt/2; t+=dt){
+ for(t = t0+dt; t <= t0 + 5 + dt/2; t+=dt){
WOLF_INFO("Starting iteration ", i, " with timestamp ", t);
++i;
- C = std::make_shared<CaptureImu>(t, sensor_ptr, data, data_cov, Vector3d::Zero());
+ C = std::make_shared<CaptureImu>(t, _sensor_ptr, data, data_cov, Vector3d::Zero());
WOLF_INFO("Created new Capture");
if(second_frame){
- //ASSERT_MATRIX_APPROX(processor_motion->getState().vector("POV"), Vector5d::Zero(), 1e-9);
+ //ASSERT_MATRIX_APPROX(_processor_motion->getState().vector("POV"), Vector5d::Zero(), 1e-9);
}
+ WOLF_INFO("Processing capture");
+ C->process();
WOLF_INFO("Doing the static initialization stuff");
- if (not last_frame_)
+ if (not _last_frame)
{
- last_frame_ = processor_motion->getOrigin()->getFrame();
- first_frame = last_frame_;
+ _last_frame = _processor_motion->getOrigin()->getFrame();
+ _first_frame = _last_frame;
// impose zero velocity
- last_frame_->getV()->setZero();
- last_frame_->getV()->fix();
+ _last_frame->getV()->setZero();
+ _last_frame->getV()->fix();
// impose zero odometry
- //processor_motion_->setOdometry(sensor_ptr->getProblem()->stateZero(processor_motion_->getStateStructure()));
+ _processor_motion->setOdometry(_sensor_ptr->getProblem()->stateZero(_processor_motion->getStateStructure()));
}
else
{
- assert(processor_motion->getOrigin() && "SubscriberImuEnableable::callback: nullptr origin");
- assert(processor_motion->getOrigin()->getFrame() && "SubscriberImuEnableable::callback: nullptr origin frame");
+ assert(_processor_motion->getOrigin() && "SubscriberImuEnableable::callback: nullptr origin");
+ assert(_processor_motion->getOrigin()->getFrame() && "SubscriberImuEnableable::callback: nullptr origin frame");
// new frame
- if (last_frame_ != processor_motion->getOrigin()->getFrame())
+ if (_last_frame != _processor_motion->getOrigin()->getFrame())
{
second_frame = true;
- last_frame_ = processor_motion->getOrigin()->getFrame();
+ _last_frame = _processor_motion->getOrigin()->getFrame();
// impose zero velocity
- last_frame_->getV()->setZero();
- last_frame_->getV()->fix();
+ _last_frame->getV()->setZero();
+ _last_frame->getV()->fix();
// impose zero odometry
- //processor_motion_->setOdometry(sensor_ptr->getProblem()->stateZero(processor_motion_->getStateStructure()));
+ _processor_motion->setOdometry(_sensor_ptr->getProblem()->stateZero(_processor_motion->getStateStructure()));
// add zero displacement and rotation capture & feature & factor with all previous frames
- assert(sensor_ptr->getProblem());
+ assert(_sensor_ptr->getProblem());
Eigen::VectorXd zero_motion = Eigen::VectorXd::Zero(size);
- for (auto frm_pair = sensor_ptr->getProblem()->getTrajectory()->begin();
- frm_pair != sensor_ptr->getProblem()->getTrajectory()->end();
+ for (auto frm_pair = _sensor_ptr->getProblem()->getTrajectory()->begin();
+ frm_pair != _sensor_ptr->getProblem()->getTrajectory()->end();
frm_pair++)
{
- if (frm_pair->second == last_frame_)
+ if (frm_pair->second == _last_frame)
break;
- auto capture_zero = CaptureBase::emplace<CaptureVoid>(last_frame_, last_frame_->getTimeStamp(), nullptr);
+ auto capture_zero = CaptureBase::emplace<CaptureVoid>(_last_frame, _last_frame->getTimeStamp(), nullptr);
auto feature_zero = FeatureBase::emplace<FeatureBase>(capture_zero,
"FeatureZeroOdom",
zero_motion,
@@ -265,20 +269,30 @@ TEST_F(ProcessorImu2dStaticInitTest, static)
}
}
WOLF_INFO("Static initialization done");
- if(first_frame)
+ if(_first_frame)
{
- WOLF_INFO("0 - The first frame is ", first_frame->id(), " and it's currently estimated bias is: \n", first_frame->getCaptureOf(sensor_ptr)->getStateBlock('I')->getState());
- WOLF_INFO("its state vector is: \n", last_frame_->getStateVector());
+ WOLF_INFO("0 - The first frame is ", _first_frame->id(), " and it's currently estimated bias is: \n", _first_frame->getCaptureOf(_sensor_ptr)->getStateBlock('I')->getState());
+ WOLF_INFO("its state vector is: \n", _first_frame->getStateVector());
WOLF_INFO_COND(second_frame, "The second frame has been created");
- WOLF_INFO("1 - The last (current) frame is: ", last_frame_->id(), " and it's state vector is: \n", last_frame_->getStateVector());
- WOLF_INFO("2 - The current frame's estimated bias is: \n", last_frame_->getCaptureOf(sensor_ptr)->getStateBlock('I')->getState());
- WOLF_INFO("3 - The current state is (from processor_motion): \n", processor_motion->getState().vector("POV"));
+ WOLF_INFO("1 - The last (current) frame is: ", _last_frame->id(), " and it's state vector is: \n", _last_frame->getStateVector());
+ WOLF_INFO("2 - The current frame's estimated bias is: \n", _last_frame->getCaptureOf(_sensor_ptr)->getStateBlock('I')->getState());
+ WOLF_INFO("3 - The current state is (from _processor_motion): \n", _processor_motion->getState().vector("POV"));
}
- //WOLF_INFO("The current problem is:");
- //_problem_ptr->print(4);
- WOLF_INFO("Processing capture and solving... \n -----------------------------------------------------------------------------------");
- C->process();
+ WOLF_INFO("The current problem is:");
+ _problem_ptr->print(4);
+ WOLF_INFO("Solving...");
std::string report = _solver->solve(SolverManager::ReportVerbosity::BRIEF);
+ WOLF_INFO("Solved");
+ if(_first_frame)
+ {
+ WOLF_INFO("4 - The first frame is ", _first_frame->id(), " and it's currently estimated bias is: \n", _first_frame->getCaptureOf(_sensor_ptr)->getStateBlock('I')->getState());
+ WOLF_INFO("its state vector is: \n", _first_frame->getStateVector());
+ WOLF_INFO_COND(second_frame, "The second frame has been created");
+ WOLF_INFO("5 - The last (current) frame is: ", _last_frame->id(), " and it's state vector is: \n", _last_frame->getStateVector());
+ WOLF_INFO("6 - The current frame's estimated bias is: \n", _last_frame->getCaptureOf(_sensor_ptr)->getStateBlock('I')->getState());
+ WOLF_INFO("7 - The current state is (from _processor_motion): \n", _processor_motion->getState().vector("POV"));
+ }
+ WOLF_INFO("------------------------------------------------------------------------\n");
}
}
--
GitLab