diff --git a/test/gtest_imu2d_static_init.cpp b/test/gtest_imu2d_static_init.cpp index 9283b56e3160fd128dbf2a2e5f12f4b2f00c224f..5270c87fa4a95f5ed3521b0694d202fef0621f0a 100644 --- a/test/gtest_imu2d_static_init.cpp +++ b/test/gtest_imu2d_static_init.cpp @@ -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); } }