Skip to content
Snippets Groups Projects
Commit 88806bdf authored by Joan Solà Ortega's avatar Joan Solà Ortega
Browse files

Extract local methods

parent ad65da44
No related branches found
No related tags found
2 merge requests!54devel->main,!50Resolve "Bootstrap sequence"
Hide whitespace changes
Inline Side-by-side
include/imu/processor/processor_imu.h
+ 41
− 40
View file @ 88806bdf
...@@ -86,46 +86,47 @@ class ProcessorImu : public ProcessorMotion{ ...@@ -86,46 +86,47 @@ class ProcessorImu : public ProcessorMotion{
WOLF_PROCESSOR_CREATE(ProcessorImu, ParamsProcessorImu); WOLF_PROCESSOR_CREATE(ProcessorImu, ParamsProcessorImu);
void preProcess() override; void preProcess() override;
protected: protected:
void computeCurrentDelta(const Eigen::VectorXd& _data, void computeCurrentDelta(const Eigen::VectorXd& _data,
const Eigen::MatrixXd& _data_cov, const Eigen::MatrixXd& _data_cov,
const Eigen::VectorXd& _calib, const Eigen::VectorXd& _calib,
const double _dt, const double _dt,
Eigen::VectorXd& _delta, Eigen::VectorXd& _delta,
Eigen::MatrixXd& _delta_cov, Eigen::MatrixXd& _delta_cov,
Eigen::MatrixXd& _jacobian_calib) const override; Eigen::MatrixXd& _jacobian_calib) const override;
void deltaPlusDelta(const Eigen::VectorXd& _delta_preint, void deltaPlusDelta(const Eigen::VectorXd& _delta_preint,
const Eigen::VectorXd& _delta, const Eigen::VectorXd& _delta,
const double _dt, const double _dt,
Eigen::VectorXd& _delta_preint_plus_delta) const override; Eigen::VectorXd& _delta_preint_plus_delta) const override;
void deltaPlusDelta(const Eigen::VectorXd& _delta_preint, void deltaPlusDelta(const Eigen::VectorXd& _delta_preint,
const Eigen::VectorXd& _delta, const Eigen::VectorXd& _delta,
const double _dt, const double _dt,
Eigen::VectorXd& _delta_preint_plus_delta, Eigen::VectorXd& _delta_preint_plus_delta,
Eigen::MatrixXd& _jacobian_delta_preint, Eigen::MatrixXd& _jacobian_delta_preint,
Eigen::MatrixXd& _jacobian_delta) const override; Eigen::MatrixXd& _jacobian_delta) const override;
void statePlusDelta(const VectorComposite& _x, void statePlusDelta(const VectorComposite& _x,
const Eigen::VectorXd& _delta, const Eigen::VectorXd& _delta,
const double _Dt, const double _Dt,
VectorComposite& _x_plus_delta) const override; VectorComposite& _x_plus_delta) const override;
Eigen::VectorXd deltaZero() const override; Eigen::VectorXd deltaZero() const override;
Eigen::VectorXd correctDelta(const Eigen::VectorXd& delta_preint, Eigen::VectorXd correctDelta(const Eigen::VectorXd& delta_preint,
const Eigen::VectorXd& delta_step) const override; const Eigen::VectorXd& delta_step) const override;
VectorXd getCalibration (const CaptureBaseConstPtr _capture = nullptr) const override; VectorXd getCalibration(const CaptureBaseConstPtr _capture = nullptr) const override;
void setCalibration(const CaptureBasePtr _capture, const VectorXd& _calibration) override; void setCalibration(const CaptureBasePtr _capture, const VectorXd& _calibration) override;
bool voteForKeyFrame() const override; bool voteForKeyFrame() const override;
CaptureMotionPtr emplaceCapture(const FrameBasePtr& _frame_own, CaptureMotionPtr emplaceCapture(const FrameBasePtr& _frame_own,
const SensorBasePtr& _sensor, const SensorBasePtr& _sensor,
const TimeStamp& _ts, const TimeStamp& _ts,
const VectorXd& _data, const VectorXd& _data,
const MatrixXd& _data_cov, const MatrixXd& _data_cov,
const VectorXd& _calib, const VectorXd& _calib,
const VectorXd& _calib_preint, const VectorXd& _calib_preint,
const CaptureBasePtr& _capture_origin) override; const CaptureBasePtr& _capture_origin) override;
FeatureBasePtr emplaceFeature(CaptureMotionPtr _capture_motion) override; FeatureBasePtr emplaceFeature(CaptureMotionPtr _capture_motion) override;
FactorBasePtr emplaceFactor(FeatureBasePtr _feature_motion, FactorBasePtr emplaceFactor(FeatureBasePtr _feature_motion, CaptureBasePtr _capture_origin) override;
CaptureBasePtr _capture_origin) override; virtual void bootstrap() override;
virtual void bootstrap() override; CaptureBasePtr bootstrapOrigin() const;
VectorXd bootstrapDelta() const;
protected: protected:
ParamsProcessorImuPtr params_motion_Imu_; ParamsProcessorImuPtr params_motion_Imu_;
......
src/processor/processor_imu.cpp
+ 34
− 20
View file @ 88806bdf
...@@ -263,13 +263,8 @@ void ProcessorImu::bootstrap() ...@@ -263,13 +263,8 @@ void ProcessorImu::bootstrap()
} }
case ParamsProcessorImu::BootstrapMethod::BOOTSTRAP_G: { case ParamsProcessorImu::BootstrapMethod::BOOTSTRAP_G: {
// Implementation of G strategy. // Implementation of G strategy.
CaptureBasePtr first_capture; CaptureBasePtr first_capture = bootstrapOrigin();
if (list_fac_inactive_bootstrap_.empty()) first_capture = origin_ptr_; TimeStamp t_current = last_ptr_->getBuffer().back().ts_;
else
first_capture =
std::static_pointer_cast<CaptureMotion>(list_fac_inactive_bootstrap_.front()->getCapture())
->getOriginCapture();
TimeStamp t_current = last_ptr_->getBuffer().back().ts_;
if (t_current - first_capture->getTimeStamp() >= params_motion_Imu_->bootstrap_averaging_length) if (t_current - first_capture->getTimeStamp() >= params_motion_Imu_->bootstrap_averaging_length)
{ {
// frames: // frames:
...@@ -283,21 +278,10 @@ void ProcessorImu::bootstrap() ...@@ -283,21 +278,10 @@ void ProcessorImu::bootstrap()
Quaterniond q_r_s(first_capture->getSensor()->getStateVector("O").data()); Quaterniond q_r_s(first_capture->getSensor()->getStateVector("O").data());
// Compute total integrated delta during bootstrap period // Compute total integrated delta during bootstrap period
// first, integrate all deltas in previous factors VectorXd delta_int = bootstrapDelta();
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);
// compute local g and transformation to global g // 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 dv = delta_int.segment(7, 3); //
Vector3d g_l = -((q_l_r * q_r_s) * dv / dt); // See eq. (20) Vector3d g_l = -((q_l_r * q_r_s) * dv / dt); // See eq. (20)
const auto& g_w = gravity(); // const auto& g_w = gravity(); //
...@@ -335,6 +319,36 @@ void ProcessorImu::bootstrap() ...@@ -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 } // namespace wolf
// Register in the FactoryProcessor // Register in the FactoryProcessor
......
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