Skip to content
Snippets Groups Projects
Commit 81e8e728 authored by Idril-Tadzio Geer Cousté's avatar Idril-Tadzio Geer Cousté
Browse files

work on the static init test

parent 3d860679
No related branches found
No related tags found
4 merge requests!39release after RAL,!38After 2nd RAL submission,!33Imu2d static initialization,!31Imu2d static initialization
......@@ -29,6 +29,7 @@ class ProcessorImu2dStaticInitTest : public testing::Test
VectorComposite s0c; // initial sigmas composite
std::shared_ptr<wolf::CaptureImu> C;
FrameBasePtr last_frame_;
SolverCeresPtr _solver;
//a new of this will be created for each new test
void SetUp() override
......@@ -59,6 +60,9 @@ class ProcessorImu2dStaticInitTest : public testing::Test
// 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());
// Create the solver
_solver = make_shared<SolverCeres>(_problem_ptr);
}
};
......@@ -185,32 +189,26 @@ TEST_F(ProcessorImu2dStaticInitTest, static)
x0c['V'] = Vector2d(0, 0);
data_cov *= 1e-3;
dt = 0.5;
//dt = 0.3;
auto KF0 = _problem_ptr->setPriorFix(x0c, t0, dt/2);
processor_motion->setOrigin(KF0);
data = Vector6d::Random();
WOLF_INFO("Data is: ", data);
WOLF_INFO("Data is: \n", data);
int size = 3;
second_frame = false;
FrameBasePtr first_frame = nullptr;
for(t = t0+dt; t <= t0 + 4 + dt/2; t+=dt){
if(first_frame) WOLF_INFO("The first frame is ", first_frame, " and it's currently estimated bias is: ", first_frame->getCaptureOf(sensor_ptr)->getStateBlock('I')->getState());
WOLF_INFO("Last frame is: ", last_frame_);
WOLF_INFO("Current state is: ", processor_motion->getState().vector("POV"));
WOLF_INFO("second_frame is: ", second_frame);
WOLF_INFO("Currently estimated bias is: ", C->getStateBlock('I')->getState());
WOLF_INFO("The current problem is:");
_problem_ptr->print(4);
C->setTimeStamp(t);
C->setData(data);
C->setDataCovariance(data_cov);
C->process();
int i = 1;
for(t = t0+dt; t <= t0 + 2 + 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());
WOLF_INFO("Created new Capture");
if(second_frame){
//ASSERT_MATRIX_APPROX(processor_motion->getState().vector("POV"), Vector5d::Zero(), 1e-9);
}
WOLF_INFO("Doing the static initialization stuff");
if (not last_frame_)
{
last_frame_ = processor_motion->getOrigin()->getFrame();
......@@ -252,20 +250,35 @@ TEST_F(ProcessorImu2dStaticInitTest, static)
break;
auto capture_zero = CaptureBase::emplace<CaptureVoid>(last_frame_, last_frame_->getTimeStamp(), nullptr);
auto feature_zero = FeatureBase::emplace<FeatureBase>(capture_zero,
"FeatureZeroOdom",
zero_motion,
Eigen::MatrixXd::Identity(size,size) * 0.01);
"FeatureZeroOdom",
zero_motion,
Eigen::MatrixXd::Identity(size,size) * 0.01);
FactorBase::emplace<FactorRelativePose2d>(feature_zero,
feature_zero,
frm_pair->second,
nullptr,
false,
TOP_MOTION);
feature_zero,
frm_pair->second,
nullptr,
false,
TOP_MOTION);
}
}
}
WOLF_INFO("Static initialization done");
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_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("The current problem is:");
//_problem_ptr->print(4);
WOLF_INFO("Processing capture and solving... \n -----------------------------------------------------------------------------------");
C->process();
std::string report = _solver->solve(SolverManager::ReportVerbosity::BRIEF);
}
}
......
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