diff --git a/include/imu/processor/processor_imu.h b/include/imu/processor/processor_imu.h
index e7b6b9549e9481269262ad7dd3122cc6a0e6a187..ff144eda4a0032fdcb72870bbc578db1c5cf5471 100644
--- a/include/imu/processor/processor_imu.h
+++ b/include/imu/processor/processor_imu.h
@@ -86,46 +86,47 @@ class ProcessorImu : public ProcessorMotion{
WOLF_PROCESSOR_CREATE(ProcessorImu, ParamsProcessorImu);
void preProcess() override;
- protected:
- void computeCurrentDelta(const Eigen::VectorXd& _data,
- const Eigen::MatrixXd& _data_cov,
- const Eigen::VectorXd& _calib,
- const double _dt,
- Eigen::VectorXd& _delta,
- Eigen::MatrixXd& _delta_cov,
- Eigen::MatrixXd& _jacobian_calib) const override;
- void deltaPlusDelta(const Eigen::VectorXd& _delta_preint,
- const Eigen::VectorXd& _delta,
- const double _dt,
- Eigen::VectorXd& _delta_preint_plus_delta) const override;
- void deltaPlusDelta(const Eigen::VectorXd& _delta_preint,
- const Eigen::VectorXd& _delta,
- const double _dt,
- Eigen::VectorXd& _delta_preint_plus_delta,
- Eigen::MatrixXd& _jacobian_delta_preint,
- Eigen::MatrixXd& _jacobian_delta) const override;
- void statePlusDelta(const VectorComposite& _x,
- const Eigen::VectorXd& _delta,
- const double _Dt,
- VectorComposite& _x_plus_delta) const override;
- Eigen::VectorXd deltaZero() const override;
- Eigen::VectorXd correctDelta(const Eigen::VectorXd& delta_preint,
- const Eigen::VectorXd& delta_step) const override;
- VectorXd getCalibration (const CaptureBaseConstPtr _capture = nullptr) const override;
- void setCalibration(const CaptureBasePtr _capture, const VectorXd& _calibration) override;
- bool voteForKeyFrame() const override;
- CaptureMotionPtr emplaceCapture(const FrameBasePtr& _frame_own,
- const SensorBasePtr& _sensor,
- const TimeStamp& _ts,
- const VectorXd& _data,
- const MatrixXd& _data_cov,
- const VectorXd& _calib,
- const VectorXd& _calib_preint,
- const CaptureBasePtr& _capture_origin) override;
- FeatureBasePtr emplaceFeature(CaptureMotionPtr _capture_motion) override;
- FactorBasePtr emplaceFactor(FeatureBasePtr _feature_motion,
- CaptureBasePtr _capture_origin) override;
- virtual void bootstrap() override;
+ protected:
+ void computeCurrentDelta(const Eigen::VectorXd& _data,
+ const Eigen::MatrixXd& _data_cov,
+ const Eigen::VectorXd& _calib,
+ const double _dt,
+ Eigen::VectorXd& _delta,
+ Eigen::MatrixXd& _delta_cov,
+ Eigen::MatrixXd& _jacobian_calib) const override;
+ void deltaPlusDelta(const Eigen::VectorXd& _delta_preint,
+ const Eigen::VectorXd& _delta,
+ const double _dt,
+ Eigen::VectorXd& _delta_preint_plus_delta) const override;
+ void deltaPlusDelta(const Eigen::VectorXd& _delta_preint,
+ const Eigen::VectorXd& _delta,
+ const double _dt,
+ Eigen::VectorXd& _delta_preint_plus_delta,
+ Eigen::MatrixXd& _jacobian_delta_preint,
+ Eigen::MatrixXd& _jacobian_delta) const override;
+ void statePlusDelta(const VectorComposite& _x,
+ const Eigen::VectorXd& _delta,
+ const double _Dt,
+ VectorComposite& _x_plus_delta) const override;
+ Eigen::VectorXd deltaZero() const override;
+ Eigen::VectorXd correctDelta(const Eigen::VectorXd& delta_preint,
+ const Eigen::VectorXd& delta_step) const override;
+ VectorXd getCalibration(const CaptureBaseConstPtr _capture = nullptr) const override;
+ void setCalibration(const CaptureBasePtr _capture, const VectorXd& _calibration) override;
+ bool voteForKeyFrame() const override;
+ CaptureMotionPtr emplaceCapture(const FrameBasePtr& _frame_own,
+ const SensorBasePtr& _sensor,
+ const TimeStamp& _ts,
+ const VectorXd& _data,
+ const MatrixXd& _data_cov,
+ const VectorXd& _calib,
+ const VectorXd& _calib_preint,
+ const CaptureBasePtr& _capture_origin) override;
+ FeatureBasePtr emplaceFeature(CaptureMotionPtr _capture_motion) override;
+ FactorBasePtr emplaceFactor(FeatureBasePtr _feature_motion, CaptureBasePtr _capture_origin) override;
+ virtual void bootstrap() override;
+ CaptureBasePtr bootstrapOrigin() const;
+ VectorXd bootstrapDelta() const;
protected:
ParamsProcessorImuPtr params_motion_Imu_;
diff --git a/src/processor/processor_imu.cpp b/src/processor/processor_imu.cpp
index 8f3433ea0bf34d322988fed3bbfb6dca91d92e34..87582d9746f745d96805fda31f47d8755d04f524 100644
--- a/src/processor/processor_imu.cpp
+++ b/src/processor/processor_imu.cpp
@@ -263,13 +263,8 @@ void ProcessorImu::bootstrap()
}
case ParamsProcessorImu::BootstrapMethod::BOOTSTRAP_G: {
// Implementation of G strategy.
- CaptureBasePtr first_capture;
- if (list_fac_inactive_bootstrap_.empty()) first_capture = origin_ptr_;
- else
- first_capture =
- std::static_pointer_cast<CaptureMotion>(list_fac_inactive_bootstrap_.front()->getCapture())
- ->getOriginCapture();
- TimeStamp t_current = last_ptr_->getBuffer().back().ts_;
+ CaptureBasePtr first_capture = bootstrapOrigin();
+ TimeStamp t_current = last_ptr_->getBuffer().back().ts_;
if (t_current - first_capture->getTimeStamp() >= params_motion_Imu_->bootstrap_averaging_length)
{
// frames:
@@ -283,21 +278,10 @@ void ProcessorImu::bootstrap()
Quaterniond q_r_s(first_capture->getSensor()->getStateVector("O").data());
// Compute total integrated delta during bootstrap period
- // first, integrate all deltas in previous factors
- VectorXd delta_int = deltaZero();
- for (const auto& fac : list_fac_inactive_bootstrap_)
- // here, we take advantage of the list of IMU factors to recover all deltas
- {
- double dt = fac->getCapture()->getTimeStamp() - fac->getCaptureOther()->getTimeStamp();
- const auto& delta = fac->getFeature()->getMeasurement(); // In FeatImu, delta = measurement
- delta_int = imu::compose(delta_int, delta, dt);
- }
- // now compose with delta in last_ptr_
- double dt = t_current - origin_ptr_->getTimeStamp();
- delta_int = imu::compose(delta_int, last_ptr_->getDeltaPreint(), dt);
+ VectorXd delta_int = bootstrapDelta();
// compute local g and transformation to global g
- dt = t_current - first_capture->getTimeStamp(); //
+ double dt = t_current - first_capture->getTimeStamp(); //
Vector3d dv = delta_int.segment(7, 3); //
Vector3d g_l = -((q_l_r * q_r_s) * dv / dt); // See eq. (20)
const auto& g_w = gravity(); //
@@ -335,6 +319,36 @@ void ProcessorImu::bootstrap()
}
}
+CaptureBasePtr ProcessorImu::bootstrapOrigin() const
+{
+ if (list_fac_inactive_bootstrap_.empty())
+ return origin_ptr_;
+ else
+ return std::static_pointer_cast<CaptureMotion>(list_fac_inactive_bootstrap_.front()->getCapture())
+ ->getOriginCapture();
+}
+
+VectorXd ProcessorImu::bootstrapDelta() const
+{
+ // Compute total integrated delta during bootstrap period
+ // first, integrate all deltas in previous factors
+ VectorXd delta_int = deltaZero();
+ double dt;
+ for (const auto& fac : list_fac_inactive_bootstrap_)
+ // here, we take advantage of the list of IMU factors to recover all deltas
+ {
+ dt = fac->getCapture()->getTimeStamp() - fac->getCaptureOther()->getTimeStamp();
+ const auto& delta = fac->getFeature()->getMeasurement(); // In FeatImu, delta = measurement
+ delta_int = imu::compose(delta_int, delta, dt);
+ }
+ // now compose with delta in last_ptr_
+ dt = last_ptr_->getBuffer().back().ts_ - origin_ptr_->getTimeStamp();
+ const auto& delta = last_ptr_->getDeltaPreint();
+ delta_int = imu::compose(delta_int, delta, dt);
+
+ return delta_int;
+}
+
} // namespace wolf
// Register in the FactoryProcessor