diff --git a/demos/imu2d_static_init.yaml b/demos/imu2d_static_init.yaml
deleted file mode 100644
index c30a7d38d2c1a5b997463c3ce41dc0b84233669d..0000000000000000000000000000000000000000
--- a/demos/imu2d_static_init.yaml
+++ /dev/null
@@ -1,12 +0,0 @@
-type: "ProcessorImu2d" # This must match the KEY used in the SensorFactory. Otherwise it is an error.
-
-time_tolerance: 0.0025 # Time tolerance for joining KFs
-unmeasured_perturbation_std: 0.00001
-
-keyframe_vote:
- voting_active: true
- voting_aux_active: false
- max_time_span: 1.0 # seconds
- 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 fc472d915512f54e76e7839d01065bc5ea9fb1a1..b5324fbc9f89b395470479f5249b34ae0d9403ab 100644
--- a/test/gtest_imu2d_static_init.cpp
+++ b/test/gtest_imu2d_static_init.cpp
@@ -16,27 +16,23 @@ 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::ProblemPtr problem_ptr_;
+ wolf::SensorBasePtr sensor_ptr_;
+ wolf::ProcessorMotionPtr processor_motion_;
wolf::TimeStamp t;
wolf::TimeStamp t0;
- double mti_clock, tmp;
double dt;
- bool second_frame;
- bool third_frame;
Vector6d data;
Matrix6d data_cov;
- VectorComposite x0c; // initial state composite
- VectorComposite s0c; // initial sigmas composite
- std::shared_ptr<wolf::CaptureImu> C;
- FrameBasePtr _first_frame;
- FrameBasePtr _last_frame;
- SolverCeresPtr _solver;
+ FrameBasePtr KF0_;
+ FrameBasePtr first_frame_;
+ FrameBasePtr last_frame_;
+ SolverCeresPtr solver_;
//a new of this will be created for each new test
void SetUp() override
{
+ WOLF_INFO("Doing setup...");
using namespace Eigen;
using std::shared_ptr;
using std::make_shared;
@@ -46,218 +42,717 @@ class ProcessorImu2dStaticInitTest : public testing::Test
std::string wolf_root = _WOLF_IMU_ROOT_DIR;
// 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");
- 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);
+ problem_ptr_ = Problem::create("POV", 2);
+ Vector3d extrinsics = (Vector3d() << 0,0,0).finished();
+ sensor_ptr_ = problem_ptr_->installSensor("SensorImu2d", "Main Imu", extrinsics, wolf_root + "/test/yaml/sensor_imu2d_static_init.yaml");
+ ProcessorBasePtr processor_ptr = problem_ptr_->installProcessor("ProcessorImu2d", "Imu pre-integrator", "Main Imu", wolf_root + "/test/yaml/imu2d_static_init.yaml");
+ processor_motion_ = std::static_pointer_cast<ProcessorMotion>(processor_ptr);
// Time and data variables
- second_frame = false;
- third_frame = false;
- dt = 0.01;
+ dt = 0.1;
t0.set(0);
t = t0;
data = Vector6d::Random();
data_cov = Matrix6d::Identity();
- _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());
+ last_frame_ = nullptr;
+ first_frame_ = nullptr;
// Create the solver
- _solver = make_shared<SolverCeres>(_problem_ptr);
+ solver_ = make_shared<SolverCeres>(problem_ptr_);
+
+ // Set the origin
+ VectorComposite x_origin("POV", {Vector2d::Zero(), Vector1d::Zero(), Vector2d::Zero()});
+ VectorComposite std_origin("POV", {0.001*Vector2d::Ones(), 0.001*Vector1d::Ones(), 0.001*Vector2d::Ones()});
+ //KF0_ = problem_ptr_->setPriorFix(x_origin, t0, dt/2);
+ KF0_ = problem_ptr_->setPriorFactor(x_origin, std_origin, 0, 0.01);
+
}
-};
+ void TestStatic(const Vector3d& body_magnitudes, const Vector3d& bias_groundtruth, const Vector3d& bias_initial_guess, const string& test_name, int testing_var, bool small_tol)
+ {
+ //Data
+ data.head(2) = body_magnitudes.head(2);
+ data(5) = body_magnitudes(2);
+ data.head(2) += bias_groundtruth.head(2);
+ data(5) += bias_groundtruth(2);
+
+ //Set bias initial guess
+ sensor_ptr_->getIntrinsic(t0)->setState(bias_initial_guess);
+ processor_motion_->setOrigin(KF0_);
+
+#if WRITE_CSV_FILE
+ std::fstream file_est;
+ file_est.open("./est2d-"+test_name+".csv", std::fstream::out);
+ // std::string header_est = "t,px,py,pz,qx,qy,qz,qw,vx,vy,vz,bax_est,bax_preint\n";
+ std::string header_est;
+ if(testing_var == 0) header_est = "t;px;vx;o;bax_est;bg_est;bax_preint;bg_preint\n";
+ if(testing_var == 1) header_est = "t;py;vy;o;bay_est;bg_est;bay_preint;bg_preint\n";
+ //if(testing_var == 2) header_est = "t;pz;vz;oz:baz_est;bgz_est;baz_preint;bgz_preint\n";
+ if(testing_var == 3) header_est = "t;pnorm;vnorm;o;banorm_est;bg_est;banorm_preint;bg_preint\n";
+ file_est << header_est;
+#endif
+
+
+ int n_frames = 0;
+ for(t = t0; t <= t0 + 9.9 + dt/2; t+=dt){
+ WOLF_INFO("\n------------------------------------------------------------------------");
+
+ //Create and process capture
+ auto C = std::make_shared<CaptureImu>(t, sensor_ptr_, data, data_cov, KF0_->getCaptureList().front());
+ C->process();
+
+ auto state = problem_ptr_->getState();
+ auto bias_est = sensor_ptr_->getIntrinsic()->getState();
+ auto bias_preint = processor_motion_->getLast()->getCalibrationPreint();
+
+#if WRITE_CSV_FILE
+ // pre-solve print to CSV
+ if(testing_var == 3){
+ file_est << std::fixed << t << ";"
+ << state['P'].norm() << ";"
+ << state['V'].norm() << ";"
+ << state['O'] << ";"
+ << bias_est.head(2).norm() << ";"
+ << bias_est(2) << ";"
+ << bias_preint.head(2).norm() << ";"
+ << bias_preint(2) << "\n";
+ }
+ else
+ {
+ file_est << std::fixed << t << ";"
+ << state['P'](testing_var) << ";"
+ << state['V'](testing_var) << ";"
+ << state['O'] << ";"
+ << bias_est(testing_var) << ";"
+ << bias_est(2) << ";"
+ << bias_preint(testing_var) << ";"
+ << bias_preint(2) << "\n";
+
+ }
+#endif
+
+ // new frame
+ if (last_frame_ != processor_motion_->getOrigin()->getFrame())
+ {
+ n_frames++;
+ last_frame_ = processor_motion_->getOrigin()->getFrame();
+
+ // impose static
+ last_frame_->getP()->setState(KF0_->getP()->getState());
+ last_frame_->getO()->setState(KF0_->getO()->getState());
+ last_frame_->getV()->setZero();
+
+ //Fix frame
+ last_frame_->fix();
+
+ //Solve
+ if(n_frames > 0)
+ {
+ std::string report = solver_->solve(SolverManager::ReportVerbosity::FULL);
+ //WOLF_INFO("Solver Report: ", report);
+
+ state = problem_ptr_->getState();
+ bias_est = sensor_ptr_->getIntrinsic()->getState();
+ bias_preint = processor_motion_->getLast()->getCalibrationPreint();
+
+ WOLF_INFO("The current problem is:");
+ problem_ptr_->print(4);
+
+#if WRITE_CSV_FILE
+ // post-solve print to CSV with time-stamp shifted by dt/2 to separate from pre-solve result
+ if(testing_var == 3){
+ file_est << std::fixed << t+dt/2 << ";"
+ << state['P'].norm() << ";"
+ << state['V'].norm() << ";"
+ << state['O'] << ";"
+ << bias_est.head(2).norm() << ";"
+ << bias_est(2) << ";"
+ << bias_preint.head(2).norm() << ";"
+ << bias_preint(2) << "\n";
+ }
+ else
+ {
+ file_est << std::fixed << t+dt/2 << ";"
+ << state['P'](testing_var) << ";"
+ << state['V'](testing_var) << ";"
+ << state['O'] << ";"
+ << bias_est(testing_var) << ";"
+ << bias_est(2) << ";"
+ << bias_preint(testing_var) << ";"
+ << bias_preint(2) << "\n";
+
+ }
+#endif
+ }
+
+ }
+
+
+ WOLF_INFO("Number of frames is ", n_frames);
+ WOLF_INFO("The state is: ", state);
+ WOLF_INFO("The estimated bias is: ", bias_est.transpose());
+ WOLF_INFO("The preintegrated bias is: ", bias_preint.transpose());
+ if(small_tol)
+ {
+ if(n_frames == 2)
+ {
+ EXPECT_MATRIX_APPROX(state.vector("POV"), KF0_->getState().vector("POV"), 1e-6);
+ EXPECT_MATRIX_APPROX(bias_est, bias_groundtruth, 1e-6);
+ }
+ else if (n_frames > 2)
+ {
+ EXPECT_MATRIX_APPROX(state.vector("POV"), KF0_->getState().vector("POV"), 1e-6);
+ EXPECT_MATRIX_APPROX(bias_est, bias_groundtruth, 1e-6);
+ EXPECT_MATRIX_APPROX(bias_preint, bias_groundtruth, 1e-6);
+ }
+ }
+ else
+ {
+ if(n_frames == 2)
+ {
+ EXPECT_MATRIX_APPROX(state.vector("POV"), KF0_->getState().vector("POV"), 1e-3);
+ EXPECT_MATRIX_APPROX(bias_est, bias_groundtruth, 1e-3);
+ }
+ else if (n_frames > 2)
+ {
+ EXPECT_MATRIX_APPROX(state.vector("POV"), KF0_->getState().vector("POV"), 1e-3);
+ EXPECT_MATRIX_APPROX(bias_est, bias_groundtruth, 1e-3);
+ EXPECT_MATRIX_APPROX(bias_preint, bias_groundtruth, 1e-3);
+ }
+ }
+ }
+#if WRITE_CSV_FILE
+ file_est.close();
+#endif
-/**
- * SET TO TRUE TO PRODUCE CSV FILE
- * SET TO FALSE TO STOP PRODUCING CSV FILE
- */
-#define WRITE_CSV_FILE true
+ }
-TEST_F(ProcessorImu2dStaticInitTest, static)
-{
- // Set the origin
- x0c['P'] = Vector2d(0, 0);
- x0c['O'] = Vector1d(0);
- x0c['V'] = Vector2d(0, 0);
-
- data_cov *= 1e-3;
- data << 1, 2, 3, 4, 5, 6;
- Vector3d bias_groundtruth;
- bias_groundtruth << data(0), data(1), data(5);
- //dt = 0.0001;
- auto KF0 = _problem_ptr->setPriorFix(x0c, t0, dt/2);
- KF0->fix();
- _processor_motion->setOrigin(KF0);
-
- WOLF_INFO("Data is: \n", data);
- int size = 3;
- second_frame = false;
- _first_frame = nullptr;
-
- #if WRITE_CSV_FILE
- std::fstream file_est;
- file_est.open("./est.csv", std::fstream::out);
- // std::string header_est = "t,px,py,pz,qx,qy,qz,qw,vx,vy,vz,bax_est,bax_preint\n";
- std::string header_est = "t;px;vx;bax_est;bax_preint\n";
- file_est << header_est;
- #endif
-
-
- int i = 1;
- int n_frames = 0;
- for(t = t0+dt; t <= t0 + 3.1 + 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("Processing capture");
- C->process();
- WOLF_INFO("Doing the static initialization stuff");
- if (not _last_frame)
- {
- n_frames++;
- _last_frame = _processor_motion->getOrigin()->getFrame();
- _first_frame = _last_frame;
-
- // fix Position and orientation
- _last_frame->getP()->fix();
- _last_frame->getO()->fix();
-
- // impose zero velocity
- _last_frame->getV()->setZero();
- _last_frame->getV()->fix();
-
- // impose zero odometry
- _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");
-
- // new frame
- if (_last_frame != _processor_motion->getOrigin()->getFrame())
- {
- n_frames++;
- second_frame = true;
- _last_frame = _processor_motion->getOrigin()->getFrame();
-
- // fix Position and orientation
- _last_frame->getP()->fix();
- _last_frame->getO()->fix();
-
- // impose zero velocity
- _last_frame->getV()->setZero();
- _last_frame->getV()->fix();
-
- // impose zero odometry
- _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());
- Eigen::VectorXd zero_motion = Eigen::VectorXd::Zero(size);
- for (auto frm_pair = _sensor_ptr->getProblem()->getTrajectory()->begin();
- frm_pair != _sensor_ptr->getProblem()->getTrajectory()->end();
- frm_pair++)
- {
- if (frm_pair->second == _last_frame)
- 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);
-
- FactorBase::emplace<FactorRelativePose2d>(feature_zero,
- 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", _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("The current problem is:");
- //_problem_ptr->print(4);
- auto state = _problem_ptr->getState();
- auto bias_est = _sensor_ptr->getIntrinsic()->getState();
- auto bias_preint = _processor_motion->getLast()->getCalibrationPreint();
-
- #if WRITE_CSV_FILE
+ void TestStaticZeroOdom(const Vector3d& body_magnitudes, const Vector3d& bias_groundtruth, const Vector3d& bias_initial_guess, const string& test_name, int testing_var, bool small_tol)
+ {
+ //Data
+ data.head(2) = body_magnitudes.head(2);
+ data(5) = body_magnitudes(2);
+ data.head(2) += bias_groundtruth.head(2);
+ data(5) += bias_groundtruth(2);
+
+ //Set bias initial guess
+ sensor_ptr_->getIntrinsic(t0)->setState(bias_initial_guess);
+ processor_motion_->setOrigin(KF0_);
+
+#if WRITE_CSV_FILE
+ std::fstream file_est;
+ file_est.open("./est2dzeroodom-"+test_name+".csv", std::fstream::out);
+ // std::string header_est = "t,px,py,pz,qx,qy,qz,qw,vx,vy,vz,bax_est,bax_preint\n";
+ std::string header_est;
+ if(testing_var == 0) header_est = "t;px;vx;o;bax_est;bg_est;bax_preint;bg_preint\n";
+ if(testing_var == 1) header_est = "t;py;vy;o;bay_est;bg_est;bay_preint;bg_preint\n";
+ //if(testing_var == 2) header_est = "t;pz;vz;oz:baz_est;bgz_est;baz_preint;bgz_preint\n";
+ if(testing_var == 3) header_est = "t;pnorm;vnorm;o;banorm_est;bg_est;banorm_preint;bg_preint\n";
+ file_est << header_est;
+#endif
+
+ int n_frames = 0;
+ for(t = t0; t <= t0 + 9.9 + dt/2; t+=dt){
+ WOLF_INFO("\n------------------------------------------------------------------------");
+
+ //Create and process capture
+ auto C = std::make_shared<CaptureImu>(t, sensor_ptr_, data, data_cov, KF0_->getCaptureList().front());
+ C->process();
+
+ auto state = problem_ptr_->getState();
+ auto bias_est = sensor_ptr_->getIntrinsic()->getState();
+ auto bias_preint = processor_motion_->getLast()->getCalibrationPreint();
+
+#if WRITE_CSV_FILE
// pre-solve print to CSV
+ if(testing_var == 3){
+ file_est << std::fixed << t << ";"
+ << state['P'].norm() << ";"
+ << state['V'].norm() << ";"
+ << state['O'] << ";"
+ << bias_est.head(2).norm() << ";"
+ << bias_est(2) << ";"
+ << bias_preint.head(2).norm() << ";"
+ << bias_preint(2) << "\n";
+ }
+ else
+ {
file_est << std::fixed << t << ";"
- << state['P'](0) << ";"
- << state['V'](0) << ";"
- << bias_est(0) << ";"
- << bias_preint(0) << "\n";
- #endif
-
- if(_problem_ptr->getTrajectory()->getFrameMap().size() > n_frames)
- {
- WOLF_INFO("Solving...");
- std::string report = _solver->solve(SolverManager::ReportVerbosity::BRIEF);
-
- state = _problem_ptr->getState();
- bias_est = _sensor_ptr->getIntrinsic()->getState();
- bias_preint = _processor_motion->getLast()->getCalibrationPreint();
-
- #if WRITE_CSV_FILE
- // post-solve print to CSV with time-stamp shifted by dt/2 to separate from pre-solve result
+ << state['P'](testing_var) << ";"
+ << state['V'](testing_var) << ";"
+ << state['O'] << ";"
+ << bias_est(testing_var) << ";"
+ << bias_est(2) << ";"
+ << bias_preint(testing_var) << ";"
+ << bias_preint(2) << "\n";
+
+ }
+#endif
+
+ // new frame
+ if (last_frame_ != processor_motion_->getOrigin()->getFrame())
+ {
+ n_frames++;
+ last_frame_ = processor_motion_->getOrigin()->getFrame();
+
+ // impose zero odometry
+ 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());
+ for (auto frm_pair = sensor_ptr_->getProblem()->getTrajectory()->begin();
+ frm_pair != sensor_ptr_->getProblem()->getTrajectory()->end();
+ frm_pair++)
+ {
+ if (frm_pair->second == last_frame_)
+ break;
+ auto capture_zero = CaptureBase::emplace<CaptureVoid>(last_frame_, last_frame_->getTimeStamp(), nullptr);
+
+ auto feature_zero = FeatureBase::emplace<FeatureBase>(capture_zero,
+ "FeatureZeroOdom",
+ Vector3d::Zero(),
+ Eigen::MatrixXd::Identity(3,3) * 0.01);
+
+ FactorBase::emplace<FactorRelativePose2d>(feature_zero,
+ feature_zero,
+ frm_pair->second,
+ nullptr,
+ false,
+ TOP_MOTION);
+
+ }
+
+ // impose static
+ last_frame_->getV()->setZero();
+
+ //Fix frame
+ last_frame_->getV()->fix();
+
+ //Solve
+ if(n_frames > 0)
+ {
+ std::string report = solver_->solve(SolverManager::ReportVerbosity::FULL);
+ //WOLF_INFO("Solver Report: ", report);
+
+ state = problem_ptr_->getState();
+ bias_est = sensor_ptr_->getIntrinsic()->getState();
+ bias_preint = processor_motion_->getLast()->getCalibrationPreint();
+
+ WOLF_INFO("The current problem is:");
+ problem_ptr_->print(4);
+
+#if WRITE_CSV_FILE
+ // post-solve print to CSV with time-stamp shifted by dt/2 to separate from pre-solve result
+ if(testing_var == 3){
+ file_est << std::fixed << t+dt/2 << ";"
+ << state['P'].norm() << ";"
+ << state['V'].norm() << ";"
+ << state['O'] << ";"
+ << bias_est.head(2).norm() << ";"
+ << bias_est(2) << ";"
+ << bias_preint.head(2).norm() << ";"
+ << bias_preint(2) << "\n";
+ }
+ else
+ {
file_est << std::fixed << t+dt/2 << ";"
- << state['P'](0) << ";"
- << state['V'](0) << ";"
- << bias_est(0) << ";"
- << bias_preint(0) << "\n";
- #endif
-
-
- 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("Current frame is", _processor_motion->getLast()->id());
- WOLF_INFO("The state is \n:", state);
- WOLF_INFO("The estimated bias is: \n", bias_est);
- WOLF_INFO("The preintegrated bias is: \n", bias_preint);
- }
- if(n_frames > 1)
- {
- ASSERT_MATRIX_APPROX(state.vector("POV"), KF0->getState().vector("POV"), 1e-9);
- ASSERT_MATRIX_APPROX(bias_est, bias_groundtruth, 1e-9);
- }
- if (n_frames > 2)
- {
- ASSERT_MATRIX_APPROX(bias_preint, bias_groundtruth, 1e-9);
- }
- WOLF_INFO("------------------------------------------------------------------------\n");
+ << state['P'](testing_var) << ";"
+ << state['V'](testing_var) << ";"
+ << state['O'] << ";"
+ << bias_est(testing_var) << ";"
+ << bias_est(2) << ";"
+ << bias_preint(testing_var) << ";"
+ << bias_preint(2) << "\n";
+
+ }
+#endif
+ }
+
+ }
+
+
+
+ WOLF_INFO("Number of frames is ", n_frames);
+ WOLF_INFO("The state is: ", state);
+ WOLF_INFO("The estimated bias is: ", bias_est.transpose());
+ WOLF_INFO("The preintegrated bias is: ", bias_preint.transpose());
+ if(small_tol)
+ {
+ if(n_frames == 2)
+ {
+ EXPECT_MATRIX_APPROX(state.vector("POV"), KF0_->getState().vector("POV"), 1e-6);
+ EXPECT_MATRIX_APPROX(bias_est, bias_groundtruth, 1e-6);
+ }
+ else if (n_frames > 2)
+ {
+ EXPECT_MATRIX_APPROX(state.vector("POV"), KF0_->getState().vector("POV"), 1e-6);
+ EXPECT_MATRIX_APPROX(bias_est, bias_groundtruth, 1e-6);
+ EXPECT_MATRIX_APPROX(bias_preint, bias_groundtruth, 1e-6);
+ }
+ }
+ else
+ {
+ if(n_frames == 2)
+ {
+ EXPECT_MATRIX_APPROX(state.vector("POV"), KF0_->getState().vector("POV"), 1e-3);
+ EXPECT_MATRIX_APPROX(bias_est, bias_groundtruth, 1e-3);
+ }
+ else if (n_frames > 2)
+ {
+ EXPECT_MATRIX_APPROX(state.vector("POV"), KF0_->getState().vector("POV"), 1e-3);
+ EXPECT_MATRIX_APPROX(bias_est, bias_groundtruth, 1e-3);
+ EXPECT_MATRIX_APPROX(bias_preint, bias_groundtruth, 1e-3);
+ }
+ }
}
- }
+
+#if WRITE_CSV_FILE
+ file_est.close();
+#endif
+
+ }
+};
+
+
+
+TEST_F(ProcessorImu2dStaticInitTest, static_bias_aX_initial_guess_zero)
+{
+ Vector3d body_magnitudes = Vector3d::Zero();
+ Vector3d bias_groundtruth = (Vector3d() << 0.42, 0, 0).finished();
+ Vector3d bias_initial_guess = Vector3d::Zero();
+
+ TestStatic(body_magnitudes, bias_groundtruth, bias_initial_guess, "static_bias_aX_initial_guess_zero", 0, true);
+}
+
+TEST_F(ProcessorImu2dStaticInitTest, static_bias_zero_initial_guess_aX)
+{
+ Vector3d body_magnitudes = Vector3d::Zero();
+ Vector3d bias_groundtruth = Vector3d::Zero();
+ Vector3d bias_initial_guess = (Vector3d() << 0.42, 0, 0).finished();
+
+ TestStatic(body_magnitudes, bias_groundtruth, bias_initial_guess, "static_bias_zero_initial_guess_aX", 0, true);
+}
+
+TEST_F(ProcessorImu2dStaticInitTest, static_bias_gX_initial_guess_zero)
+{
+ Vector3d body_magnitudes = Vector3d::Zero();
+ Vector3d bias_groundtruth = (Vector3d() << 0, 0, 0.01).finished();
+ Vector3d bias_initial_guess = Vector3d::Zero();
+
+ TestStatic(body_magnitudes, bias_groundtruth, bias_initial_guess, "static_bias_gX_initial_guess_zero", 0, false);
+}
+
+TEST_F(ProcessorImu2dStaticInitTest, static_bias_zero_initial_guess_gX)
+{
+ Vector3d body_magnitudes = Vector3d::Zero();
+ Vector3d bias_groundtruth = Vector3d::Zero();
+ Vector3d bias_initial_guess = (Vector3d() << 0, 0, 0.01).finished();
+
+ TestStatic(body_magnitudes, bias_groundtruth, bias_initial_guess, "static_bias_zero_initial_guess_gX", 0, false);
+}
+
+TEST_F(ProcessorImu2dStaticInitTest, static_bias_aX_initial_guess_zero_zeroodom)
+{
+ Vector3d body_magnitudes = Vector3d::Zero();
+ Vector3d bias_groundtruth = (Vector3d() << 0.42, 0, 0).finished();
+ Vector3d bias_initial_guess = Vector3d::Zero();
+
+ TestStaticZeroOdom(body_magnitudes, bias_groundtruth, bias_initial_guess, "static_bias_aX_initial_guess_zero", 0, true);
+}
+
+TEST_F(ProcessorImu2dStaticInitTest, static_bias_zero_initial_guess_aX_zeroodom)
+{
+ Vector3d body_magnitudes = Vector3d::Zero();
+ Vector3d bias_groundtruth = Vector3d::Zero();
+ Vector3d bias_initial_guess = (Vector3d() << 0.42, 0, 0).finished();
+
+ TestStaticZeroOdom(body_magnitudes, bias_groundtruth, bias_initial_guess, "static_bias_zero_initial_guess_aX", 0, true);
+}
+
+TEST_F(ProcessorImu2dStaticInitTest, static_bias_gX_initial_guess_zero_zeroodom)
+{
+ Vector3d body_magnitudes = Vector3d::Zero();
+ Vector3d bias_groundtruth = (Vector3d() << 0, 0, 0.01).finished();
+ Vector3d bias_initial_guess = Vector3d::Zero();
+
+ TestStaticZeroOdom(body_magnitudes, bias_groundtruth, bias_initial_guess, "static_bias_gX_initial_guess_zero", 0, false);
+}
+
+TEST_F(ProcessorImu2dStaticInitTest, static_bias_zero_initial_guess_gX_zeroodom)
+{
+ Vector3d body_magnitudes = Vector3d::Zero();
+ Vector3d bias_groundtruth = Vector3d::Zero();
+ Vector3d bias_initial_guess = (Vector3d() << 0, 0, 0.01).finished();
+
+ TestStaticZeroOdom(body_magnitudes, bias_groundtruth, bias_initial_guess, "static_bias_zero_initial_guess_gX", 0, false);
}
+
+TEST_F(ProcessorImu2dStaticInitTest, static_bias_zero_initial_guess_a_random)
+{
+ Vector3d body_magnitudes = Vector3d::Zero();
+ Vector3d bias_groundtruth = Vector3d::Zero();
+ Vector3d bias_initial_guess = Vector3d::Random()*100;
+ bias_initial_guess(2) = 0;
+
+ TestStatic(body_magnitudes, bias_groundtruth, bias_initial_guess, "static_bias_zero_initial_guess_a_random", 3, true);
+}
+
+TEST_F(ProcessorImu2dStaticInitTest, static_bias_zero_initial_guess_random)
+{
+ Vector3d body_magnitudes = Vector3d::Zero();
+ Vector3d bias_groundtruth = Vector3d::Zero();
+ Vector3d bias_initial_guess = Vector3d::Random()*0.01;
+
+ TestStatic(body_magnitudes, bias_groundtruth, bias_initial_guess, "static_bias_zero_initial_guess_random", 3, false);
+}
+//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::TimeStamp t;
+// wolf::TimeStamp t0;
+// double mti_clock, tmp;
+// double dt;
+// bool second_frame;
+// bool third_frame;
+// Vector6d data;
+// Matrix6d data_cov;
+// VectorComposite x0c; // initial state composite
+// VectorComposite s0c; // initial sigmas composite
+// std::shared_ptr<wolf::CaptureImu> C;
+// FrameBasePtr _first_frame;
+// FrameBasePtr _last_frame;
+// SolverCeresPtr _solver;
+//
+// //a new of this will be created for each new test
+// void SetUp() override
+// {
+// using namespace Eigen;
+// using std::shared_ptr;
+// using std::make_shared;
+// using std::static_pointer_cast;
+// using namespace wolf::Constants;
+//
+// std::string wolf_root = _WOLF_IMU_ROOT_DIR;
+//
+// // 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");
+// 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);
+//
+// // Time and data variables
+// second_frame = false;
+// third_frame = false;
+// dt = 0.01;
+// t0.set(0);
+// t = t0;
+// data = Vector6d::Random();
+// data_cov = Matrix6d::Identity();
+// _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());
+//
+// // Create the solver
+// _solver = make_shared<SolverCeres>(_problem_ptr);
+// }
+//
+//};
+//
+//
+///**
+// * SET TO TRUE TO PRODUCE CSV FILE
+// * SET TO FALSE TO STOP PRODUCING CSV FILE
+// */
+//#define WRITE_CSV_FILE true
+//
+//TEST_F(ProcessorImu2dStaticInitTest, static)
+//{
+// // Set the origin
+// x0c['P'] = Vector2d(0, 0);
+// x0c['O'] = Vector1d(0);
+// x0c['V'] = Vector2d(0, 0);
+//
+// data_cov *= 1e-3;
+// data << 1, 2, 3, 4, 5, 6;
+// Vector3d bias_groundtruth;
+// bias_groundtruth << data(0), data(1), data(5);
+// //dt = 0.0001;
+// auto KF0 = _problem_ptr->setPriorFix(x0c, t0, dt/2);
+// KF0->fix();
+// _processor_motion->setOrigin(KF0);
+//
+// WOLF_INFO("Data is: \n", data);
+// int size = 3;
+// second_frame = false;
+// _first_frame = nullptr;
+//
+// #if WRITE_CSV_FILE
+// std::fstream file_est;
+// file_est.open("./est.csv", std::fstream::out);
+// // std::string header_est = "t,px,py,pz,qx,qy,qz,qw,vx,vy,vz,bax_est,bax_preint\n";
+// std::string header_est = "t;px;vx;bax_est;bax_preint\n";
+// file_est << header_est;
+// #endif
+//
+//
+// int i = 1;
+// int n_frames = 0;
+// for(t = t0+dt; t <= t0 + 3.1 + 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("Processing capture");
+// C->process();
+// WOLF_INFO("Doing the static initialization stuff");
+// if (not _last_frame)
+// {
+// n_frames++;
+// _last_frame = _processor_motion->getOrigin()->getFrame();
+// _first_frame = _last_frame;
+//
+// // fix Position and orientation
+// _last_frame->getP()->fix();
+// _last_frame->getO()->fix();
+//
+// // impose zero velocity
+// _last_frame->getV()->setZero();
+// _last_frame->getV()->fix();
+//
+// // impose zero odometry
+// _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");
+//
+// // new frame
+// if (_last_frame != _processor_motion->getOrigin()->getFrame())
+// {
+// n_frames++;
+// second_frame = true;
+// _last_frame = _processor_motion->getOrigin()->getFrame();
+//
+// // fix Position and orientation
+// _last_frame->getP()->fix();
+// _last_frame->getO()->fix();
+//
+// // impose zero velocity
+// _last_frame->getV()->setZero();
+// _last_frame->getV()->fix();
+//
+// // impose zero odometry
+// _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());
+// Eigen::VectorXd zero_motion = Eigen::VectorXd::Zero(size);
+// for (auto frm_pair = _sensor_ptr->getProblem()->getTrajectory()->begin();
+// frm_pair != _sensor_ptr->getProblem()->getTrajectory()->end();
+// frm_pair++)
+// {
+// if (frm_pair->second == _last_frame)
+// 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);
+//
+// FactorBase::emplace<FactorRelativePose2d>(feature_zero,
+// 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", _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("The current problem is:");
+// //_problem_ptr->print(4);
+// auto state = _problem_ptr->getState();
+// auto bias_est = _sensor_ptr->getIntrinsic()->getState();
+// auto bias_preint = _processor_motion->getLast()->getCalibrationPreint();
+//
+// #if WRITE_CSV_FILE
+// // pre-solve print to CSV
+// file_est << std::fixed << t << ";"
+// << state['P'](0) << ";"
+// << state['V'](0) << ";"
+// << bias_est(0) << ";"
+// << bias_preint(0) << "\n";
+// #endif
+//
+// if(_problem_ptr->getTrajectory()->getFrameMap().size() > n_frames)
+// {
+// WOLF_INFO("Solving...");
+// std::string report = _solver->solve(SolverManager::ReportVerbosity::BRIEF);
+//
+// state = _problem_ptr->getState();
+// bias_est = _sensor_ptr->getIntrinsic()->getState();
+// bias_preint = _processor_motion->getLast()->getCalibrationPreint();
+//
+// #if WRITE_CSV_FILE
+// // post-solve print to CSV with time-stamp shifted by dt/2 to separate from pre-solve result
+// file_est << std::fixed << t+dt/2 << ";"
+// << state['P'](0) << ";"
+// << state['V'](0) << ";"
+// << bias_est(0) << ";"
+// << bias_preint(0) << "\n";
+// #endif
+//
+//
+// 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("Current frame is", _processor_motion->getLast()->id());
+// WOLF_INFO("The state is \n:", state);
+// WOLF_INFO("The estimated bias is: \n", bias_est);
+// WOLF_INFO("The preintegrated bias is: \n", bias_preint);
+// }
+// if(n_frames > 1)
+// {
+// ASSERT_MATRIX_APPROX(state.vector("POV"), KF0->getState().vector("POV"), 1e-9);
+// ASSERT_MATRIX_APPROX(bias_est, bias_groundtruth, 1e-9);
+// }
+// if (n_frames > 2)
+// {
+// ASSERT_MATRIX_APPROX(bias_preint, bias_groundtruth, 1e-9);
+// }
+// WOLF_INFO("------------------------------------------------------------------------\n");
+// }
+// }
+//}
+//
int main(int argc, char **argv)
{
testing::InitGoogleTest(&argc, argv);
diff --git a/test/gtest_imu_static_init.cpp b/test/gtest_imu_static_init.cpp
index 22d47ae5df4e59c93c5bb7a0d7dbca8c4072262d..9c2bdbe07c0f8fef4c302d6ad47ab3d5ad13b523 100644
--- a/test/gtest_imu_static_init.cpp
+++ b/test/gtest_imu_static_init.cpp
@@ -77,10 +77,9 @@ class ProcessorImuStaticInitTest : public testing::Test
VectorComposite std_origin("POV", {0.001*Vector3d::Ones(), 0.001*Vector3d::Ones(), 0.001*Vector3d::Ones()});
KF0_ = problem_ptr_->setPriorFactor(x_origin, std_origin, 0, 0.01);
- processor_motion_->setOrigin(KF0_);
}
- void TestStatic(const Vector6d& body_magnitudes, const Vector6d& bias_groundtruth, const Vector6d& bias_initial_guess, const string& test_name, int testing_var)
+ void TestStatic(const Vector6d& body_magnitudes, const Vector6d& bias_groundtruth, const Vector6d& bias_initial_guess, const string& test_name, int testing_var, bool small_tol)
{
//Data
data = body_magnitudes;
@@ -89,6 +88,7 @@ class ProcessorImuStaticInitTest : public testing::Test
//Set bias initial guess
sensor_ptr_->getIntrinsic(t0)->setState(bias_initial_guess);
+ processor_motion_->setOrigin(KF0_);
#if WRITE_CSV_FILE
std::fstream file_est;
@@ -98,12 +98,13 @@ class ProcessorImuStaticInitTest : public testing::Test
if(testing_var == 0) header_est = "t;px;vx;ox;bax_est;bgx_est;bax_preint;bgx_preint\n";
if(testing_var == 1) header_est = "t;py;vy;oy;bay_est;bgy_est;bay_preint;bgy_preint\n";
if(testing_var == 2) header_est = "t;pz;vz;oz:baz_est;bgz_est;baz_preint;bgz_preint\n";
+ if(testing_var == 3) header_est = "t;pnorm;vnorm;onorm;banorm_est;bgnorm_est;banorm_preint;bgnorm_preint\n";
file_est << header_est;
#endif
int n_frames = 0;
- for(t = t0; t <= t0 + 3.9 + dt/2; t+=dt){
+ for(t = t0; t <= t0 + 9.9 + dt/2; t+=dt){
WOLF_INFO("\n------------------------------------------------------------------------");
//Create and process capture
@@ -116,6 +117,18 @@ class ProcessorImuStaticInitTest : public testing::Test
#if WRITE_CSV_FILE
// pre-solve print to CSV
+ if(testing_var == 3){
+ file_est << std::fixed << t << ";"
+ << state['P'].norm() << ";"
+ << state['V'].norm() << ";"
+ << state['O'].norm() << ";"
+ << bias_est.head(3).norm() << ";"
+ << bias_est.tail(3).norm() << ";"
+ << bias_preint.head(3).norm() << ";"
+ << bias_preint.tail(3).norm() << "\n";
+ }
+ else
+ {
file_est << std::fixed << t << ";"
<< state['P'](testing_var) << ";"
<< state['V'](testing_var) << ";"
@@ -124,6 +137,8 @@ class ProcessorImuStaticInitTest : public testing::Test
<< bias_est(testing_var+3) << ";"
<< bias_preint(testing_var) << ";"
<< bias_preint(testing_var+3) << "\n";
+
+ }
#endif
// new frame
@@ -149,10 +164,210 @@ class ProcessorImuStaticInitTest : public testing::Test
state = problem_ptr_->getState();
bias_est = sensor_ptr_->getIntrinsic()->getState();
bias_preint = processor_motion_->getLast()->getCalibrationPreint();
+
+ WOLF_INFO("The current problem is:");
+ problem_ptr_->print(4);
+
+#if WRITE_CSV_FILE
+ // post-solve print to CSV with time-stamp shifted by dt/2 to separate from pre-solve result
+ if(testing_var == 3){
+ file_est << std::fixed << t+dt/2 << ";"
+ << state['P'].norm() << ";"
+ << state['V'].norm() << ";"
+ << state['O'].norm() << ";"
+ << bias_est.head(3).norm() << ";"
+ << bias_est.tail(3).norm() << ";"
+ << bias_preint.head(3).norm() << ";"
+ << bias_preint.tail(3).norm() << "\n";
+ }
+ else
+ {
+ file_est << std::fixed << t+dt/2 << ";"
+ << state['P'](testing_var) << ";"
+ << state['V'](testing_var) << ";"
+ << state['O'](testing_var) << ";"
+ << bias_est(testing_var) << ";"
+ << bias_est(testing_var+3) << ";"
+ << bias_preint(testing_var) << ";"
+ << bias_preint(testing_var+3) << "\n";
+
+ }
+#endif
+ }
+
+ }
+
+
+
+ WOLF_INFO("Number of frames is ", n_frames);
+ WOLF_INFO("The state is: ", state);
+ WOLF_INFO("The estimated bias is: ", bias_est.transpose());
+ WOLF_INFO("The preintegrated bias is: ", bias_preint.transpose());
+ if(small_tol)
+ {
+ if(n_frames == 2)
+ {
+ EXPECT_MATRIX_APPROX(state.vector("POV"), KF0_->getState().vector("POV"), 1e-6);
+ EXPECT_MATRIX_APPROX(bias_est, bias_groundtruth, 1e-6);
+ }
+ else if (n_frames > 2)
+ {
+ EXPECT_MATRIX_APPROX(state.vector("POV"), KF0_->getState().vector("POV"), 1e-6);
+ EXPECT_MATRIX_APPROX(bias_est, bias_groundtruth, 1e-6);
+ EXPECT_MATRIX_APPROX(bias_preint, bias_groundtruth, 1e-6);
+ }
+ }
+ else
+ {
+ if(n_frames == 2)
+ {
+ EXPECT_MATRIX_APPROX(state.vector("POV"), KF0_->getState().vector("POV"), 1e-3);
+ EXPECT_MATRIX_APPROX(bias_est, bias_groundtruth, 1e-3);
+ }
+ else if (n_frames > 2)
+ {
+ EXPECT_MATRIX_APPROX(state.vector("POV"), KF0_->getState().vector("POV"), 1e-3);
+ EXPECT_MATRIX_APPROX(bias_est, bias_groundtruth, 1e-3);
+ EXPECT_MATRIX_APPROX(bias_preint, bias_groundtruth, 1e-3);
+ }
+ }
+ }
+
+#if WRITE_CSV_FILE
+ file_est.close();
+#endif
+
+ }
+
+ void TestStaticZeroOdom(const Vector6d& body_magnitudes, const Vector6d& bias_groundtruth, const Vector6d& bias_initial_guess, const string& test_name, int testing_var, bool small_tol)
+ {
+ //Data
+ data = body_magnitudes;
+ data.head(3) -= Quaterniond(Vector4d(KF0_->getO()->getState())).conjugate() * wolf::gravity();
+ data += bias_groundtruth;
+
+ //Set bias initial guess
+ sensor_ptr_->getIntrinsic(t0)->setState(bias_initial_guess);
+ processor_motion_->setOrigin(KF0_);
+
+#if WRITE_CSV_FILE
+ std::fstream file_est;
+ file_est.open("./estzeroodom-"+test_name+".csv", std::fstream::out);
+ // std::string header_est = "t,px,py,pz,qx,qy,qz,qw,vx,vy,vz,bax_est,bax_preint\n";
+ std::string header_est;
+ if(testing_var == 0) header_est = "t;px;vx;ox;bax_est;bgx_est;bax_preint;bgx_preint\n";
+ if(testing_var == 1) header_est = "t;py;vy;oy;bay_est;bgy_est;bay_preint;bgy_preint\n";
+ if(testing_var == 2) header_est = "t;pz;vz;oz:baz_est;bgz_est;baz_preint;bgz_preint\n";
+ if(testing_var == 3) header_est = "t;pnorm;vnorm;onorm;banorm_est;bgnorm_est;banorm_preint;bgnorm_preint\n";
+ file_est << header_est;
+#endif
+
+
+ int n_frames = 0;
+ for(t = t0; t <= t0 + 9.9 + dt/2; t+=dt){
+ WOLF_INFO("\n------------------------------------------------------------------------");
+
+ //Create and process capture
+ auto C = std::make_shared<CaptureImu>(t, sensor_ptr_, data, data_cov, KF0_->getCaptureList().front());
+ C->process();
+
+ auto state = problem_ptr_->getState();
+ auto bias_est = sensor_ptr_->getIntrinsic()->getState();
+ auto bias_preint = processor_motion_->getLast()->getCalibrationPreint();
+
+#if WRITE_CSV_FILE
+ // pre-solve print to CSV
+ if(testing_var == 3){
+ file_est << std::fixed << t << ";"
+ << state['P'].norm() << ";"
+ << state['V'].norm() << ";"
+ << state['O'].norm() << ";"
+ << bias_est.head(3).norm() << ";"
+ << bias_est.tail(3).norm() << ";"
+ << bias_preint.head(3).norm() << ";"
+ << bias_preint.tail(3).norm() << "\n";
+ }
+ else
+ {
+ file_est << std::fixed << t << ";"
+ << state['P'](testing_var) << ";"
+ << state['V'](testing_var) << ";"
+ << state['O'](testing_var) << ";"
+ << bias_est(testing_var) << ";"
+ << bias_est(testing_var+3) << ";"
+ << bias_preint(testing_var) << ";"
+ << bias_preint(testing_var+3) << "\n";
+
+ }
+#endif
+
+ // new frame
+ if (last_frame_ != processor_motion_->getOrigin()->getFrame())
+ {
+ n_frames++;
+ last_frame_ = processor_motion_->getOrigin()->getFrame();
+
+ // impose zero odometry
+ 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());
+ for (auto frm_pair = sensor_ptr_->getProblem()->getTrajectory()->begin();
+ frm_pair != sensor_ptr_->getProblem()->getTrajectory()->end();
+ frm_pair++)
+ {
+ if (frm_pair->second == last_frame_)
+ break;
+ auto capture_zero = CaptureBase::emplace<CaptureVoid>(last_frame_, last_frame_->getTimeStamp(), nullptr);
+
+ auto feature_zero = FeatureBase::emplace<FeatureBase>(capture_zero,
+ "FeatureZeroOdom",
+ Vector7d::Zero(),
+ Eigen::MatrixXd::Identity(6,6) * 0.01);
+
+ FactorBase::emplace<FactorRelativePose3d>(feature_zero,
+ feature_zero,
+ frm_pair->second,
+ nullptr,
+ false,
+ TOP_MOTION);
+
+ }
+
+ // impose static
+ last_frame_->getV()->setZero();
+
+ //Fix frame
+ last_frame_->getV()->fix();
+
+ //Solve
+ if(n_frames > 0)
+ {
+ std::string report = solver_->solve(SolverManager::ReportVerbosity::FULL);
+ //WOLF_INFO("Solver Report: ", report);
+
+ state = problem_ptr_->getState();
+ bias_est = sensor_ptr_->getIntrinsic()->getState();
+ bias_preint = processor_motion_->getLast()->getCalibrationPreint();
+
+ WOLF_INFO("The current problem is:");
+ problem_ptr_->print(4);
#if WRITE_CSV_FILE
// post-solve print to CSV with time-stamp shifted by dt/2 to separate from pre-solve result
- file_est << std::fixed << t+dt/2 << ";"
+ if(testing_var == 3){
+ file_est << std::fixed << t+dt/2 << ";"
+ << state['P'].norm() << ";"
+ << state['V'].norm() << ";"
+ << state['O'].norm() << ";"
+ << bias_est.head(3).norm() << ";"
+ << bias_est.tail(3).norm() << ";"
+ << bias_preint.head(3).norm() << ";"
+ << bias_preint.tail(3).norm() << "\n";
+ }
+ else
+ {
+ file_est << std::fixed << t+dt/2 << ";"
<< state['P'](testing_var) << ";"
<< state['V'](testing_var) << ";"
<< state['O'](testing_var) << ";"
@@ -160,29 +375,46 @@ class ProcessorImuStaticInitTest : public testing::Test
<< bias_est(testing_var+3) << ";"
<< bias_preint(testing_var) << ";"
<< bias_preint(testing_var+3) << "\n";
+
+ }
#endif
}
}
- //WOLF_INFO("The current problem is:");
- //problem_ptr_->print(4);
WOLF_INFO("Number of frames is ", n_frames);
WOLF_INFO("The state is: ", state);
WOLF_INFO("The estimated bias is: ", bias_est.transpose());
WOLF_INFO("The preintegrated bias is: ", bias_preint.transpose());
- if(n_frames == 2)
+ if(small_tol)
{
- //ASSERT_MATRIX_APPROX(state.vector("POV"), KF0_->getState().vector("POV"), 1e-4);
- //ASSERT_MATRIX_APPROX(bias_est, bias_groundtruth, 1e-6);
+ if(n_frames == 2)
+ {
+ EXPECT_MATRIX_APPROX(state.vector("POV"), KF0_->getState().vector("POV"), 1e-6);
+ EXPECT_MATRIX_APPROX(bias_est, bias_groundtruth, 1e-6);
+ }
+ else if (n_frames > 2)
+ {
+ EXPECT_MATRIX_APPROX(state.vector("POV"), KF0_->getState().vector("POV"), 1e-6);
+ EXPECT_MATRIX_APPROX(bias_est, bias_groundtruth, 1e-6);
+ EXPECT_MATRIX_APPROX(bias_preint, bias_groundtruth, 1e-6);
+ }
}
- else if (n_frames > 2)
+ else
{
- EXPECT_MATRIX_APPROX(state.vector("POV"), KF0_->getState().vector("POV"), 1e-6);
- EXPECT_MATRIX_APPROX(bias_est, bias_groundtruth, 1e-6);
- EXPECT_MATRIX_APPROX(bias_preint, bias_groundtruth, 1e-6);
+ if(n_frames == 2)
+ {
+ EXPECT_MATRIX_APPROX(state.vector("POV"), KF0_->getState().vector("POV"), 1e-3);
+ EXPECT_MATRIX_APPROX(bias_est, bias_groundtruth, 1e-3);
+ }
+ else if (n_frames > 2)
+ {
+ EXPECT_MATRIX_APPROX(state.vector("POV"), KF0_->getState().vector("POV"), 1e-3);
+ EXPECT_MATRIX_APPROX(bias_est, bias_groundtruth, 1e-3);
+ EXPECT_MATRIX_APPROX(bias_preint, bias_groundtruth, 1e-3);
+ }
}
}
@@ -201,7 +433,7 @@ TEST_F(ProcessorImuStaticInitTest, static_bias_aX_initial_guess_zero)
Vector6d bias_groundtruth = (Vector6d() << 0.42, 0, 0, 0, 0, 0).finished();
Vector6d bias_initial_guess = Vector6d::Zero();
- TestStatic(body_magnitudes, bias_groundtruth, bias_initial_guess, "static_bias_aX_initial_guess_zero", 0);
+ TestStatic(body_magnitudes, bias_groundtruth, bias_initial_guess, "static_bias_aX_initial_guess_zero", 0, true);
}
TEST_F(ProcessorImuStaticInitTest, static_bias_zero_initial_guess_aX)
@@ -210,25 +442,81 @@ TEST_F(ProcessorImuStaticInitTest, static_bias_zero_initial_guess_aX)
Vector6d bias_groundtruth = Vector6d::Zero();
Vector6d bias_initial_guess = (Vector6d() << 0.42, 0, 0, 0, 0, 0).finished();
- TestStatic(body_magnitudes, bias_groundtruth, bias_initial_guess, "static_bias_zero_initial_guess_aX", 0);
+ TestStatic(body_magnitudes, bias_groundtruth, bias_initial_guess, "static_bias_zero_initial_guess_aX", 0, true);
}
TEST_F(ProcessorImuStaticInitTest, static_bias_gX_initial_guess_zero)
{
Vector6d body_magnitudes = Vector6d::Zero();
- Vector6d bias_groundtruth = (Vector6d() << 0, 0, 0, 0.1, 0, 0).finished();
+ Vector6d bias_groundtruth = (Vector6d() << 0, 0, 0, 0.01, 0, 0).finished();
Vector6d bias_initial_guess = Vector6d::Zero();
- TestStatic(body_magnitudes, bias_groundtruth, bias_initial_guess, "static_bias_gX_initial_guess_zero", 0);
+ TestStatic(body_magnitudes, bias_groundtruth, bias_initial_guess, "static_bias_gX_initial_guess_zero", 0, false);
}
TEST_F(ProcessorImuStaticInitTest, static_bias_zero_initial_guess_gX)
{
Vector6d body_magnitudes = Vector6d::Zero();
Vector6d bias_groundtruth = Vector6d::Zero();
- Vector6d bias_initial_guess = (Vector6d() << 0, 0, 0, 0.1, 0, 0).finished();
+ Vector6d bias_initial_guess = (Vector6d() << 0, 0, 0, 0.01, 0, 0).finished();
+
+ TestStatic(body_magnitudes, bias_groundtruth, bias_initial_guess, "static_bias_zero_initial_guess_gX", 0, false);
+}
+
+TEST_F(ProcessorImuStaticInitTest, static_bias_aX_initial_guess_zero_zeroodom)
+{
+ Vector6d body_magnitudes = Vector6d::Zero();
+ Vector6d bias_groundtruth = (Vector6d() << 0.42, 0, 0, 0, 0, 0).finished();
+ Vector6d bias_initial_guess = Vector6d::Zero();
+
+ TestStaticZeroOdom(body_magnitudes, bias_groundtruth, bias_initial_guess, "static_bias_aX_initial_guess_zero", 0, true);
+}
+
+TEST_F(ProcessorImuStaticInitTest, static_bias_zero_initial_guess_aX_zeroodom)
+{
+ Vector6d body_magnitudes = Vector6d::Zero();
+ Vector6d bias_groundtruth = Vector6d::Zero();
+ Vector6d bias_initial_guess = (Vector6d() << 0.42, 0, 0, 0, 0, 0).finished();
+
+ TestStaticZeroOdom(body_magnitudes, bias_groundtruth, bias_initial_guess, "static_bias_zero_initial_guess_aX", 0, true);
+}
+
+TEST_F(ProcessorImuStaticInitTest, static_bias_gX_initial_guess_zero_zeroodom)
+{
+ Vector6d body_magnitudes = Vector6d::Zero();
+ Vector6d bias_groundtruth = (Vector6d() << 0, 0, 0, 0.01, 0, 0).finished();
+ Vector6d bias_initial_guess = Vector6d::Zero();
+
+ TestStaticZeroOdom(body_magnitudes, bias_groundtruth, bias_initial_guess, "static_bias_gX_initial_guess_zero", 0, false);
+}
+
+TEST_F(ProcessorImuStaticInitTest, static_bias_zero_initial_guess_gX_zeroodom)
+{
+ Vector6d body_magnitudes = Vector6d::Zero();
+ Vector6d bias_groundtruth = Vector6d::Zero();
+ Vector6d bias_initial_guess = (Vector6d() << 0, 0, 0, 0.01, 0, 0).finished();
+
+ TestStaticZeroOdom(body_magnitudes, bias_groundtruth, bias_initial_guess, "static_bias_zero_initial_guess_gX", 0, false);
+}
+
+
+TEST_F(ProcessorImuStaticInitTest, static_bias_zero_initial_guess_a_random)
+{
+ Vector6d body_magnitudes = Vector6d::Zero();
+ Vector6d bias_groundtruth = Vector6d::Zero();
+ Vector6d bias_initial_guess = Vector6d::Random()*100;
+ bias_initial_guess.tail(3) = Vector3d::Zero();
+
+ TestStatic(body_magnitudes, bias_groundtruth, bias_initial_guess, "static_bias_zero_initial_guess_a_random", 3, true);
+}
+
+TEST_F(ProcessorImuStaticInitTest, static_bias_zero_initial_guess_random)
+{
+ Vector6d body_magnitudes = Vector6d::Zero();
+ Vector6d bias_groundtruth = Vector6d::Zero();
+ Vector6d bias_initial_guess = Vector6d::Random()*0.01;
- TestStatic(body_magnitudes, bias_groundtruth, bias_initial_guess, "static_bias_zero_initial_guess_gX", 0);
+ TestStatic(body_magnitudes, bias_groundtruth, bias_initial_guess, "static_bias_zero_initial_guess_random", 3, false);
}
//TEST_F(ProcessorImuStaticInitTest, static)
diff --git a/test/gtest_processor_motion_intrinsics_update.cpp b/test/gtest_processor_motion_intrinsics_update.cpp
index 4dbac962133c7d8da9f76e152a09e0749c1f2e71..a013072abf3064c5a2ac26302a1fcfe145f1a0b5 100644
--- a/test/gtest_processor_motion_intrinsics_update.cpp
+++ b/test/gtest_processor_motion_intrinsics_update.cpp
@@ -155,7 +155,7 @@ TEST_F(ProcessorImuTest, test1)
* SET TO TRUE TO PRODUCE CSV FILE
* SET TO FALSE TO STOP PRODUCING CSV FILE
*/
-#define WRITE_CSV_FILE false
+#define WRITE_CSV_FILE true
TEST_F(ProcessorImuTest, getState)
{
diff --git a/test/yaml/imu2d_static_init.yaml b/test/yaml/imu2d_static_init.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..4978c8e2863908df72a336da1225667411d3022a
--- /dev/null
+++ b/test/yaml/imu2d_static_init.yaml
@@ -0,0 +1,12 @@
+type: "ProcessorImu2d" # This must match the KEY used in the SensorFactory. Otherwise it is an error.
+
+time_tolerance: 0.0025 # Time tolerance for joining KFs
+unmeasured_perturbation_std: 0.0001
+
+keyframe_vote:
+ voting_active: true
+ voting_aux_active: false
+ max_time_span: 0.9999 # seconds
+ max_buff_length: 1000000000 # motion deltas
+ dist_traveled: 100000000000 # meters
+ angle_turned: 10000000000 # radians (1 rad approx 57 deg, approx 60 deg)
diff --git a/test/yaml/sensor_imu2d_static_init.yaml b/test/yaml/sensor_imu2d_static_init.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..ce810d4f5f397a7bcb8647e086c026b125fbc936
--- /dev/null
+++ b/test/yaml/sensor_imu2d_static_init.yaml
@@ -0,0 +1,9 @@
+type: "SensorImu2d" # This must match the KEY used in the SensorFactory. Otherwise it is an error.
+
+motion_variances:
+ a_noise: 0.053 # standard deviation of Acceleration noise (same for all the axis) in m/s2
+ w_noise: 0.0011 # standard deviation of Gyroscope noise (same for all the axis) in rad/sec
+ ab_initial_stdev: 0.800 # m/s2 - initial bias
+ wb_initial_stdev: 0.350 # rad/sec - initial bias
+ ab_rate_stdev: 0.1 # m/s2/sqrt(s)
+ wb_rate_stdev: 0.0400 # rad/s/sqrt(s)