Skip to content
Snippets Groups Projects
Commit cf80004f authored by Mederic Fourmy's avatar Mederic Fourmy
Browse files

[skip-ci] Fix bootstrap getState

parent f6de3763
No related branches found
No related tags found
1 merge request!54devel->main
Hide whitespace changes
Inline Side-by-side
src/processor/processor_imu.cpp
+ 6
− 6
View file @ cf80004f
...@@ -268,9 +268,9 @@ void ProcessorImu::bootstrap() ...@@ -268,9 +268,9 @@ void ProcessorImu::bootstrap()
if (t_current - first_capture->getTimeStamp() >= params_motion_Imu_->bootstrap_averaging_length) if (t_current - first_capture->getTimeStamp() >= params_motion_Imu_->bootstrap_averaging_length)
{ {
// get initial IMU frame 's' expressed in local world frame 'l' // get initial IMU frame 's' expressed in local world frame 'l'
Map<const Quaterniond> q_l_r(first_capture->getFrame()->getStateVector("O").data()); Quaterniond q_l_r; q_l_r.coeffs() = first_capture->getFrame()->getO()->getState();
Map<const Quaterniond> q_r_s(first_capture->getSensor()->getStateVector("O").data()); Quaterniond q_r_s; q_r_s.coeffs() = first_capture->getSensor()->getO()->getState();
const auto& q_l_s = q_l_r * q_r_s; const auto& q_l_s = q_l_r * q_r_s;
// Compute total integrated delta during bootstrap period // Compute total integrated delta during bootstrap period
VectorXd delta_int = bootstrapDelta(); VectorXd delta_int = bootstrapDelta();
...@@ -317,9 +317,9 @@ void ProcessorImu::bootstrap() ...@@ -317,9 +317,9 @@ void ProcessorImu::bootstrap()
if (t_current - first_capture->getTimeStamp() >= params_motion_Imu_->bootstrap_averaging_length) if (t_current - first_capture->getTimeStamp() >= params_motion_Imu_->bootstrap_averaging_length)
{ {
// get initial IMU frame 's' expressed in local world frame 'l' // get initial IMU frame 's' expressed in local world frame 'l'
Map<const Quaterniond> q_l_r(first_capture->getFrame()->getStateVector("O").data()); Quaterniond q_l_r; q_l_r.coeffs() = first_capture->getFrame()->getO()->getState();
Map<const Quaterniond> q_r_s(first_capture->getSensor()->getStateVector("O").data()); Quaterniond q_r_s; q_r_s.coeffs() = first_capture->getSensor()->getO()->getState();
const auto& q_l_s = q_l_r * q_r_s; const auto& q_l_s = q_l_r * q_r_s;
// Compute total integrated delta during bootstrap period // Compute total integrated delta during bootstrap period
VectorXd delta_int = bootstrapDelta(); VectorXd delta_int = bootstrapDelta();
......
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