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

work

parent d0d71ded
No related branches found
No related tags found
4 merge requests!39release after RAL,!38After 2nd RAL submission,!33Imu2d static initialization,!31Imu2d static initialization
...@@ -15,23 +15,19 @@ class ProcessorImuStaticInitTest : public testing::Test ...@@ -15,23 +15,19 @@ class ProcessorImuStaticInitTest : public testing::Test
{ {
public: //These can be accessed in fixtures public: //These can be accessed in fixtures
wolf::ProblemPtr _problem_ptr; wolf::ProblemPtr problem_ptr_;
wolf::SensorBasePtr _sensor_ptr; wolf::SensorBasePtr sensor_ptr_;
wolf::ProcessorMotionPtr _processor_motion; wolf::ProcessorMotionPtr processor_motion_;
wolf::TimeStamp t; wolf::TimeStamp t;
wolf::TimeStamp t0; wolf::TimeStamp t0;
double mti_clock, tmp;
double dt; double dt;
bool second_frame;
Vector6d data; Vector6d data;
Matrix6d data_cov; Matrix6d data_cov;
//VectorComposite x0c; // initial state composite
VectorComposite s0c; // initial sigmas composite
std::shared_ptr<wolf::CaptureImu> C; std::shared_ptr<wolf::CaptureImu> C;
FrameBasePtr KF0; FrameBasePtr KF0;
FrameBasePtr _first_frame; FrameBasePtr first_frame_;
FrameBasePtr _last_frame; FrameBasePtr last_frame_;
SolverCeresPtr _solver; SolverCeresPtr solver_;
//a new of this will be created for each new test //a new of this will be created for each new test
void SetUp() override void SetUp() override
...@@ -46,33 +42,33 @@ class ProcessorImuStaticInitTest : public testing::Test ...@@ -46,33 +42,33 @@ class ProcessorImuStaticInitTest : public testing::Test
std::string wolf_root = _WOLF_IMU_ROOT_DIR; std::string wolf_root = _WOLF_IMU_ROOT_DIR;
// Wolf problem // Wolf problem
_problem_ptr = Problem::create("POV", 3); problem_ptr_ = Problem::create("POV", 3);
Vector7d extrinsics = (Vector7d() << 0,0,0, 0, 0,0,0).finished(); Vector7d extrinsics = (Vector7d() << 0,0,0,0,0,0,1).finished();
_sensor_ptr = _problem_ptr->installSensor("SensorImu", "Main Imu", extrinsics, wolf_root + "/demos/sensor_imu.yaml"); sensor_ptr_ = problem_ptr_->installSensor("SensorImu", "Main Imu", extrinsics, wolf_root + "/demos/sensor_imu.yaml");
ProcessorBasePtr processor_ptr = _problem_ptr->installProcessor("ProcessorImu", "Imu pre-integrator", "Main Imu", wolf_root + "/demos/imu_static_init.yaml"); ProcessorBasePtr processor_ptr = problem_ptr_->installProcessor("ProcessorImu", "Imu pre-integrator", "Main Imu", wolf_root + "/demos/imu_static_init.yaml");
_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
second_frame = false;
dt = 0.01; dt = 0.01;
t0.set(0); t0.set(0);
t = t0; t = t0;
data = Vector6d::Random(); data = Vector6d::Random();
data_cov = Matrix6d::Identity(); data_cov = Matrix6d::Identity();
_last_frame = nullptr; last_frame_ = nullptr;
first_frame_ = nullptr;
// Create one capture to store the Imu data arriving from (sensor / callback / file / etc.) // 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()); 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_);
// Set the origin // Set the origin
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(); KF0->fix();
_processor_motion->setOrigin(KF0); processor_motion_->setOrigin(KF0);
} }
}; };
...@@ -88,68 +84,64 @@ TEST_F(ProcessorImuStaticInitTest, static) ...@@ -88,68 +84,64 @@ TEST_F(ProcessorImuStaticInitTest, static)
bias_groundtruth.head(3) = data.head(3) -wolf::gravity(); bias_groundtruth.head(3) = data.head(3) -wolf::gravity();
bias_groundtruth.tail(3) = data.tail(3); bias_groundtruth.tail(3) = data.tail(3);
//dt = 0.3;
//dt = 0.0001;
WOLF_INFO("Data is: \n", data); WOLF_INFO("Data is: \n", data);
int size = 7; int size = 7;
_first_frame = nullptr;
int i = 1; int i = 1;
int n_frames = 0; int n_frames = 0;
for(t = t0+dt; t <= t0 + 3 + dt/2; t+=dt){ for(t = t0+dt; t <= t0 + 3 + dt/2; t+=dt){
WOLF_INFO("Starting iteration ", i, " with timestamp ", t); WOLF_INFO("Starting iteration ", i, " with timestamp ", t);
++i; ++i;
C = std::make_shared<CaptureImu>(t, _sensor_ptr, data, data_cov, Vector3d::Zero()); C = std::make_shared<CaptureImu>(t, sensor_ptr_, data, data_cov, Vector3d::Zero());
WOLF_INFO("Created new Capture"); WOLF_INFO("Created new Capture");
if(second_frame){
//ASSERT_MATRIX_APPROX(_processor_motion->getState().vector("POV"), Vector5d::Zero(), 1e-9);
}
WOLF_INFO("Processing capture"); WOLF_INFO("Processing capture");
C->process(); C->process();
WOLF_INFO("Doing the static initialization stuff"); WOLF_INFO("Doing the static initialization stuff");
if (not _last_frame) if (not last_frame_)
{ {
n_frames++; n_frames++;
_last_frame = _processor_motion->getOrigin()->getFrame(); last_frame_ = processor_motion_->getOrigin()->getFrame();
_first_frame = _last_frame; first_frame_ = last_frame_;
// impose zero velocity // impose zero velocity
_last_frame->getV()->setZero(); last_frame_->getV()->setZero();
_last_frame->getV()->fix(); last_frame_->getV()->fix();
// impose zero odometry // impose zero odometry
_processor_motion->setOdometry(_sensor_ptr->getProblem()->stateZero(_processor_motion->getStateStructure())); processor_motion_->setOdometry(sensor_ptr_->getProblem()->stateZero(processor_motion_->getStateStructure()));
} }
else else
{ {
assert(_processor_motion->getOrigin() && "SubscriberImuEnableable::callback: nullptr origin"); assert(processor_motion_->getOrigin() && "SubscriberImuEnableable::callback: nullptr origin");
assert(_processor_motion->getOrigin()->getFrame() && "SubscriberImuEnableable::callback: nullptr origin frame"); assert(processor_motion_->getOrigin()->getFrame() && "SubscriberImuEnableable::callback: nullptr origin frame");
// new frame // new frame
if (_last_frame != _processor_motion->getOrigin()->getFrame()) if (last_frame_ != processor_motion_->getOrigin()->getFrame())
{ {
n_frames++; n_frames++;
_last_frame = _processor_motion->getOrigin()->getFrame(); last_frame_ = processor_motion_->getOrigin()->getFrame();
// impose zero velocity // impose zero velocity
_last_frame->getV()->setZero(); last_frame_->getV()->setZero();
_last_frame->getV()->fix(); last_frame_->getV()->fix();
// impose zero odometry // impose zero odometry
_processor_motion->setOdometry(_sensor_ptr->getProblem()->stateZero(_processor_motion->getStateStructure())); processor_motion_->setOdometry(sensor_ptr_->getProblem()->stateZero(processor_motion_->getStateStructure()));
// add zero displacement and rotation capture & feature & factor with all previous frames // add zero displacement and rotation capture & feature & factor with all previous frames
assert(_sensor_ptr->getProblem()); assert(sensor_ptr_->getProblem());
Eigen::VectorXd zero_motion = Eigen::VectorXd::Zero(size); Eigen::VectorXd zero_motion = Eigen::VectorXd::Zero(size);
zero_motion.tail<4>() = Eigen::Quaterniond::Identity().coeffs(); zero_motion.tail<4>() = Eigen::Quaterniond::Identity().coeffs();
for (auto frm_pair = _sensor_ptr->getProblem()->getTrajectory()->begin();
frm_pair != _sensor_ptr->getProblem()->getTrajectory()->end(); for (auto frm_pair = sensor_ptr_->getProblem()->getTrajectory()->begin();
frm_pair != sensor_ptr_->getProblem()->getTrajectory()->end();
frm_pair++) frm_pair++)
{ {
if (frm_pair->second == _last_frame) if (frm_pair->second == last_frame_)
break; break;
auto capture_zero = CaptureBase::emplace<CaptureVoid>(_last_frame, _last_frame->getTimeStamp(), nullptr); auto capture_zero = CaptureBase::emplace<CaptureVoid>(last_frame_, last_frame_->getTimeStamp(), nullptr);
auto feature_zero = FeatureBase::emplace<FeatureBase>(capture_zero, auto feature_zero = FeatureBase::emplace<FeatureBase>(capture_zero,
"FeatureZeroOdom", "FeatureZeroOdom",
zero_motion, zero_motion,
...@@ -166,34 +158,34 @@ TEST_F(ProcessorImuStaticInitTest, static) ...@@ -166,34 +158,34 @@ TEST_F(ProcessorImuStaticInitTest, static)
} }
} }
WOLF_INFO("Static initialization done"); WOLF_INFO("Static initialization done");
if(_first_frame) 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("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("its state vector is: \n", first_frame_->getStateVector());
//WOLF_INFO_COND(second_frame, "The second frame has been created"); //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("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("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("3 - The current state is (from processor_motion_): \n", processor_motion_->getState().vector("POV"));
} }
//WOLF_INFO("The current problem is:"); //WOLF_INFO("The current problem is:");
//_problem_ptr->print(4); //problem_ptr_->print(4);
WOLF_INFO("Solving..."); WOLF_INFO("Solving...");
std::string report = _solver->solve(SolverManager::ReportVerbosity::BRIEF); 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"); WOLF_INFO("Solved");
if(_first_frame) 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("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("its state vector is: \n", first_frame_->getStateVector());
//WOLF_INFO_COND(second_frame, "The second frame has been created"); //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("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("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("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("Current frame is ", n_frames, " with ID ", processor_motion_->getLast()->id());
WOLF_INFO("The state is \n:", state); WOLF_INFO("The state is \n:", state);
WOLF_INFO("The estimated bias is: \n", bias_est); WOLF_INFO("The estimated bias is: \n", bias_est);
WOLF_INFO("The preintegrated bias is: \n", bias_preint); WOLF_INFO("The preintegrated bias is: \n", bias_preint);
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment