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

finished static init gtests

parent 3d3639c6
No related branches found
No related tags found
4 merge requests!39release after RAL,!38After 2nd RAL submission,!33Imu2d static initialization,!31Imu2d static initialization
Hide whitespace changes
Inline Side-by-side
test/gtest_imu2d_static_init.cpp
+ 700
− 205
View file @ c637e962
......@@ -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);
......
test/gtest_imu_static_init.cpp
+ 307
− 19
View file @ c637e962
......@@ -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)
......
test/gtest_processor_motion_intrinsics_update.cpp
+ 1
− 1
View file @ c637e962
......@@ -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)
{
......
demos/imu2d_static_init.yaml → test/yaml/imu2d_static_init.yaml
+ 12
− 0
View file @ c637e962
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
unmeasured_perturbation_std: 0.0001
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)
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)
test/yaml/sensor_imu2d_static_init.yaml 0 → 100644
+ 9
− 0
View file @ c637e962
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)
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment