diff --git a/src/processor/processor_imu.cpp b/src/processor/processor_imu.cpp index b72182fcdda85f359e9da9f3a67a52b0c6ccd4f3..2446e1d7ecebbecfb5b2d165e5aaa599bc143b72 100644 --- a/src/processor/processor_imu.cpp +++ b/src/processor/processor_imu.cpp @@ -268,9 +268,9 @@ void ProcessorImu::bootstrap() if (t_current - first_capture->getTimeStamp() >= params_motion_Imu_->bootstrap_averaging_length) { // get initial IMU frame 's' expressed in local world frame 'l' - Map<const Quaterniond> q_l_r(first_capture->getFrame()->getStateVector("O").data()); - Map<const Quaterniond> q_r_s(first_capture->getSensor()->getStateVector("O").data()); - const auto& q_l_s = q_l_r * q_r_s; + Quaterniond q_l_r; q_l_r.coeffs() = first_capture->getFrame()->getO()->getState(); + Quaterniond q_r_s; q_r_s.coeffs() = first_capture->getSensor()->getO()->getState(); + const auto& q_l_s = q_l_r * q_r_s; // Compute total integrated delta during bootstrap period VectorXd delta_int = bootstrapDelta(); @@ -317,9 +317,9 @@ void ProcessorImu::bootstrap() if (t_current - first_capture->getTimeStamp() >= params_motion_Imu_->bootstrap_averaging_length) { // get initial IMU frame 's' expressed in local world frame 'l' - Map<const Quaterniond> q_l_r(first_capture->getFrame()->getStateVector("O").data()); - Map<const Quaterniond> q_r_s(first_capture->getSensor()->getStateVector("O").data()); - const auto& q_l_s = q_l_r * q_r_s; + Quaterniond q_l_r; q_l_r.coeffs() = first_capture->getFrame()->getO()->getState(); + Quaterniond q_r_s; q_r_s.coeffs() = first_capture->getSensor()->getO()->getState(); + const auto& q_l_s = q_l_r * q_r_s; // Compute total integrated delta during bootstrap period VectorXd delta_int = bootstrapDelta();