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

refactored test for readability

parent 484516be
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
+ 77
− 27
View file @ 1c242954
#include <fstream>
#include <core/ceres_wrapper/solver_ceres.h> #include <core/ceres_wrapper/solver_ceres.h>
#include <core/utils/utils_gtest.h> #include <core/utils/utils_gtest.h>
#include "imu/internal/config.h" #include "imu/internal/config.h"
...@@ -71,6 +72,12 @@ class ProcessorImu2dStaticInitTest : public testing::Test ...@@ -71,6 +72,12 @@ class ProcessorImu2dStaticInitTest : public testing::Test
}; };
/**
* SET TO TRUE TO PRODUCE CSV FILE
* SET TO FALSE TO STOP PRODUCING CSV FILE
*/
#define WRITE_CSV_FILE true
TEST_F(ProcessorImu2dStaticInitTest, static) TEST_F(ProcessorImu2dStaticInitTest, static)
{ {
// Set the origin // Set the origin
...@@ -92,6 +99,15 @@ TEST_F(ProcessorImu2dStaticInitTest, static) ...@@ -92,6 +99,15 @@ TEST_F(ProcessorImu2dStaticInitTest, static)
second_frame = false; second_frame = false;
_first_frame = nullptr; _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 i = 1;
int n_frames = 0; int n_frames = 0;
for(t = t0+dt; t <= t0 + 3.1 + dt/2; t+=dt){ for(t = t0+dt; t <= t0 + 3.1 + dt/2; t+=dt){
...@@ -110,6 +126,10 @@ TEST_F(ProcessorImu2dStaticInitTest, static) ...@@ -110,6 +126,10 @@ TEST_F(ProcessorImu2dStaticInitTest, static)
n_frames++; n_frames++;
_last_frame = _processor_motion->getOrigin()->getFrame(); _last_frame = _processor_motion->getOrigin()->getFrame();
_first_frame = _last_frame; _first_frame = _last_frame;
// fix Position and orientation
_last_frame->getP()->fix();
_last_frame->getO()->fix();
// impose zero velocity // impose zero velocity
_last_frame->getV()->setZero(); _last_frame->getV()->setZero();
...@@ -129,6 +149,10 @@ TEST_F(ProcessorImu2dStaticInitTest, static) ...@@ -129,6 +149,10 @@ TEST_F(ProcessorImu2dStaticInitTest, static)
n_frames++; n_frames++;
second_frame = true; second_frame = true;
_last_frame = _processor_motion->getOrigin()->getFrame(); _last_frame = _processor_motion->getOrigin()->getFrame();
// fix Position and orientation
_last_frame->getP()->fix();
_last_frame->getO()->fix();
// impose zero velocity // impose zero velocity
_last_frame->getV()->setZero(); _last_frame->getV()->setZero();
...@@ -174,37 +198,63 @@ TEST_F(ProcessorImu2dStaticInitTest, static) ...@@ -174,37 +198,63 @@ TEST_F(ProcessorImu2dStaticInitTest, static)
} }
//WOLF_INFO("The current problem is:"); //WOLF_INFO("The current problem is:");
//_problem_ptr->print(4); //_problem_ptr->print(4);
WOLF_INFO("Solving...");
std::string report = _solver->solve(SolverManager::ReportVerbosity::BRIEF);
auto state = _problem_ptr->getState(); auto state = _problem_ptr->getState();
auto bias_est = _sensor_ptr->getIntrinsic()->getState(); auto bias_est = _sensor_ptr->getIntrinsic()->getState();
auto bias_preint = _processor_motion->getLast()->getCalibrationPreint(); auto bias_preint = _processor_motion->getLast()->getCalibrationPreint();
WOLF_INFO("Solved"); #if WRITE_CSV_FILE
if(_first_frame) // pre-solve print to CSV
{ file_est << std::fixed << t << ";"
//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()); << state['P'](0) << ";"
//WOLF_INFO("its state vector is: \n", _first_frame->getStateVector()); << state['V'](0) << ";"
//WOLF_INFO_COND(second_frame, "The second frame has been created"); << bias_est(0) << ";"
//WOLF_INFO("5 - The last (current) frame is: ", _last_frame->id(), " and it's state vector is: \n", _last_frame->getStateVector()); << bias_preint(0) << "\n";
//WOLF_INFO("6 - The current frame's estimated bias is: \n", _last_frame->getCaptureOf(_sensor_ptr)->getStateBlock('I')->getState()); #endif
//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()); if(_problem_ptr->getTrajectory()->getFrameMap().size() > n_frames)
WOLF_INFO("The state is \n:", state); {
WOLF_INFO("The estimated bias is: \n", bias_est); WOLF_INFO("Solving...");
WOLF_INFO("The preintegrated bias is: \n", bias_preint); std::string report = _solver->solve(SolverManager::ReportVerbosity::BRIEF);
}
if(second_frame) state = _problem_ptr->getState();
{ bias_est = _sensor_ptr->getIntrinsic()->getState();
ASSERT_MATRIX_APPROX(state.vector("POV"), KF0->getState().vector("POV"), 1e-9); bias_preint = _processor_motion->getLast()->getCalibrationPreint();
ASSERT_MATRIX_APPROX(bias_est, bias_groundtruth, 1e-9);
} #if WRITE_CSV_FILE
if (n_frames > 2) // 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 << ";"
ASSERT_MATRIX_APPROX(bias_preint, bias_groundtruth, 1e-9); << state['P'](0) << ";"
} << state['V'](0) << ";"
WOLF_INFO("------------------------------------------------------------------------\n"); << 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");
}
} }
} }
......
test/gtest_imu_static_init.cpp
+ 298
− 136
View file @ 1c242954
#include <fstream>
#include <core/ceres_wrapper/solver_ceres.h> #include <core/ceres_wrapper/solver_ceres.h>
#include <core/utils/utils_gtest.h> #include <core/utils/utils_gtest.h>
#include "imu/internal/config.h" #include "imu/internal/config.h"
...@@ -11,6 +13,12 @@ ...@@ -11,6 +13,12 @@
using namespace Eigen; using namespace Eigen;
using namespace wolf; using namespace wolf;
/**
* SET TO TRUE TO PRODUCE CSV FILE
* SET TO FALSE TO STOP PRODUCING CSV FILE
*/
#define WRITE_CSV_FILE true
class ProcessorImuStaticInitTest : public testing::Test class ProcessorImuStaticInitTest : public testing::Test
{ {
...@@ -23,7 +31,6 @@ class ProcessorImuStaticInitTest : public testing::Test ...@@ -23,7 +31,6 @@ class ProcessorImuStaticInitTest : public testing::Test
double dt; double dt;
Vector6d data; Vector6d data;
Matrix6d data_cov; Matrix6d data_cov;
std::shared_ptr<wolf::CaptureImu> C;
FrameBasePtr KF0; FrameBasePtr KF0;
FrameBasePtr first_frame_; FrameBasePtr first_frame_;
FrameBasePtr last_frame_; FrameBasePtr last_frame_;
...@@ -49,17 +56,14 @@ class ProcessorImuStaticInitTest : public testing::Test ...@@ -49,17 +56,14 @@ class ProcessorImuStaticInitTest : public testing::Test
processor_motion_ = std::static_pointer_cast<ProcessorMotion>(processor_ptr); processor_motion_ = std::static_pointer_cast<ProcessorMotion>(processor_ptr);
// Time and data variables // Time and data variables
dt = 0.01; dt = 0.1;
t0.set(0); t0.set(0);
t = t0; t = t0;
data = Vector6d::Random(); data = Vector6d::Random();
data_cov = Matrix6d::Identity(); data_cov = Matrix6d::Identity() * 1e-3;
last_frame_ = nullptr; last_frame_ = nullptr;
first_frame_ = nullptr; first_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 // Create the solver
solver_ = make_shared<SolverCeres>(problem_ptr_); solver_ = make_shared<SolverCeres>(problem_ptr_);
...@@ -67,145 +71,303 @@ class ProcessorImuStaticInitTest : public testing::Test ...@@ -67,145 +71,303 @@ class ProcessorImuStaticInitTest : public testing::Test
VectorComposite x0c("POV", {Vector3d::Zero(), (Vector4d() << 0,0,0,1).finished(), Vector3d::Zero()}); VectorComposite x0c("POV", {Vector3d::Zero(), (Vector4d() << 0,0,0,1).finished(), Vector3d::Zero()});
WOLF_INFO("x0c is: \n", x0c); WOLF_INFO("x0c is: \n", x0c);
KF0 = problem_ptr_->setPriorFix(x0c, t0, dt/2); KF0 = problem_ptr_->setPriorFix(x0c, t0, dt/2);
KF0->fix();
processor_motion_->setOrigin(KF0); 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)
{
//Data
data = body_magnitudes + bias_groundtruth;
data.head(3) -= Quaterniond(Vector4d(KF0->getO()->getState())).conjugate() * wolf::gravity();
//Set bias initial guess
sensor_ptr_->getIntrinsic(t0)->setState(bias_initial_guess);
#if WRITE_CSV_FILE
std::fstream file_est;
file_est.open("./est-"+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";
file_est << header_est;
#endif
int n_frames = 0;
for(t = t0+dt; t <= t0 + 3 + dt/2; t+=dt){
WOLF_INFO("\n------------------------------------------------------------------------");
//Create and process capture
auto C = std::make_shared<CaptureImu>(t, sensor_ptr_, data, data_cov);
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
file_est << std::fixed << t << ";"
<< state['P'](testing_var) << ";"
<< state['V'](testing_var) << ";"
<< state['O'](testing_var) << ";"
<< bias_est(testing_var) << ";"
<< bias_preint(testing_var) << ";"
<< bias_est(testing_var+3) << ";"
<< 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 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
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();
#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'](testing_var) << ";"
<< state['V'](testing_var) << ";"
<< state['O'](testing_var) << ";"
<< bias_est(testing_var) << ";"
<< bias_preint(testing_var) << ";"
<< bias_est(testing_var+3) << ";"
<< 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)
{
//ASSERT_MATRIX_APPROX(state.vector("POV"), KF0->getState().vector("POV"), 1e-4);
//ASSERT_MATRIX_APPROX(bias_est, bias_groundtruth, 1e-6);
}
else if (n_frames > 2)
{
ASSERT_MATRIX_APPROX(state.vector("POV"), KF0->getState().vector("POV"), 1e-6);
ASSERT_MATRIX_APPROX(bias_est, bias_groundtruth, 1e-6);
ASSERT_MATRIX_APPROX(bias_preint, bias_groundtruth, 1e-6);
}
}
#if WRITE_CSV_FILE
file_est.close();
#endif
}
}; };
TEST_F(ProcessorImuStaticInitTest, static)
TEST_F(ProcessorImuStaticInitTest, static_bias_aX_initial_guess_zero)
{ {
WOLF_INFO("Starting Test"); Vector6d body_magnitudes = Vector6d::Zero();
Vector6d bias_groundtruth = (Vector6d() << 0.1, 0, 0, 0, 0, 0).finished();
data_cov *= 1e-3; Vector6d bias_initial_guess = Vector6d::Zero();
data << 1, 2, 3, 4, 5, 6;
Vector6d bias_groundtruth; TestStatic(body_magnitudes, bias_groundtruth, bias_initial_guess, "static_bias_aX_initial_guess_zero", 0);
//TODO: Fix this, it works because initial position and extrinsics are 0, but gravity has to be preceded by the quaternion that takes it to the IMU reference }
bias_groundtruth.head(3) = data.head(3) -wolf::gravity();
bias_groundtruth.tail(3) = data.tail(3); TEST_F(ProcessorImuStaticInitTest, static_bias_zero_initial_guess_aX)
{
//dt = 0.3; Vector6d body_magnitudes = Vector6d::Zero();
Vector6d bias_groundtruth = Vector6d::Zero();
WOLF_INFO("Data is: \n", data); Vector6d bias_initial_guess = (Vector6d() << 0.1, 0, 0, 0, 0, 0).finished();
int size = 7;
TestStatic(body_magnitudes, bias_groundtruth, bias_initial_guess, "static_bias_zero_initial_guess_aX", 0);
int i = 1;
int n_frames = 0;
for(t = t0+dt; t <= t0 + 3 + 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");
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_;
// 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++;
last_frame_ = processor_motion_->getOrigin()->getFrame();
// TODO: We have to impose that the position is the same as for the first frame and fix it, and do the same for the orientation
// 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);
zero_motion.tail<4>() = Eigen::Quaterniond::Identity().coeffs();
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(6,6) * 0.01);
FactorBase::emplace<FactorRelativePose3d>(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);
WOLF_INFO("Solving...");
std::string report = solver_->solve(SolverManager::ReportVerbosity::BRIEF);
auto state = problem_ptr_->getState();
auto bias_est = sensor_ptr_->getIntrinsic()->getState();
auto bias_preint = processor_motion_->getLast()->getCalibrationPreint();
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 ", n_frames, " with ID ", 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");
}
} }
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_initial_guess = Vector6d::Zero();
TestStatic(body_magnitudes, bias_groundtruth, bias_initial_guess, "static_bias_gX_initial_guess_zero", 0);
}
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();
TestStatic(body_magnitudes, bias_groundtruth, bias_initial_guess, "static_bias_zero_initial_guess_gX", 0);
}
//TEST_F(ProcessorImuStaticInitTest, static)
//{
// WOLF_INFO("Starting Test");
//
// data_cov *= 1e-3;
// data << 1, 2, 3, 4, 5, 6;
// data *= 0.01;
// data.head(3) -= wolf::gravity();
// sensor_ptr_->getIntrinsic(t0)->setState(data);
// data << -wolf::gravity(), 0,0,0;
// //Vector6d bias_groundtruth = data;
// //TODO: Fix this, it works because initial position and extrinsics are 0, but gravity has to be preceded by the quaternion that takes it to the IMU reference
// //WOLF_INFO("The Bias ground_truth is: \n", bias_groundtruth);
//
// //dt = 0.3;
//
// WOLF_INFO("Data is: \n", data);
// //int size = 7;
//
// int i = 1;
// int n_frames = 0;
// for(t = t0+dt; t <= t0 + 3 + dt/2; t+=dt){
// WOLF_INFO("Starting iteration ", i, " with timestamp ", t);
// ++i;
// auto C = std::make_shared<CaptureImu>(t, sensor_ptr_, data, data_cov);
// //WOLF_INFO("Created new Capture");
// //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++;
// last_frame_ = processor_motion_->getOrigin()->getFrame();
//
// // TODO: We have to impose that the position is the same as for the first frame and fix it, and do the same for the orientation
// // impose same Position
// last_frame_->getP()->setState(first_frame_->getP()->getState());
// last_frame_->getP()->fix();
// //
// // impose same Orientqation
// last_frame_->getO()->setState(first_frame_->getO()->getState());
// 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);
// //zero_motion.tail<4>() = Eigen::Quaterniond::Identity().coeffs();
//
// //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(6,6) * 0.01);
//
// // FactorBase::emplace<FactorRelativePose3d>(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);
// //WOLF_INFO("Solving...");
// std::string report = solver_->solve(SolverManager::ReportVerbosity::FULL);
// WOLF_INFO("Solver Report: ", report);
//
// auto state = problem_ptr_->getState();
// auto bias_est = sensor_ptr_->getIntrinsic()->getState();
// auto bias_preint = processor_motion_->getLast()->getCalibrationPreint();
//
// //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("Number of frames is ", n_frames, " with ID ", processor_motion_->getLast()->id());
// 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 > 1)
// {
// //EXPECT_MATRIX_APPROX(state.vector("POV"), KF0->getState().vector("POV"), 1e-9);
// //EXPECT_MATRIX_APPROX(bias_est, bias_groundtruth, 1e-9);
// }
// if (n_frames > 2)
// {
// //EXPECT_MATRIX_APPROX(bias_preint, bias_groundtruth, 1e-9);
// }
// WOLF_INFO("------------------------------------------------------------------------\n");
// }
//}
int main(int argc, char **argv) int main(int argc, char **argv)
{ {
testing::InitGoogleTest(&argc, argv); testing::InitGoogleTest(&argc, argv);
......
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