Skip to content
Snippets Groups Projects
Commit 8afb7294 authored by Joan Vallvé Navarro's avatar Joan Vallvé Navarro
Browse files

changing bootstrap static to work during bootstrap

parent 7b866384
No related branches found
No related tags found
1 merge request!49Draft: Resolve "Adapt to new sensor constructors in core"
Hide whitespace changes
Inline Side-by-side
include/imu/processor/processor_imu_3d.h
+ 18
− 18
View file @ 8afb7294
...@@ -54,36 +54,36 @@ class ProcessorImu3d : public ProcessorMotion ...@@ -54,36 +54,36 @@ class ProcessorImu3d : public ProcessorMotion
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;
virtual void emplaceFeaturesAndFactors(CaptureBasePtr _capture_origin, CaptureMotionPtr _capture_own) override; virtual void emplaceFeaturesAndFactors(CaptureBasePtr _capture_origin, CaptureMotionPtr _capture_own) override;
...@@ -115,9 +115,9 @@ class ProcessorImu3d : public ProcessorMotion ...@@ -115,9 +115,9 @@ class ProcessorImu3d : public ProcessorMotion
bool recomputeStates() const; bool recomputeStates() const;
protected: protected:
std::list<FactorBasePtr> bootstrap_factor_list_; ///< List of all IMU factors created while IMU is std::list<FactorBasePtr> bootstrap_factor_list_; ///< List of IMU factors created while bootstrapping
///< bootstrapping FrameBasePtr bootstrap_last_frame_; ///< Last frame processed by the bootstrapping
SensorImu3dPtr sensor_imu_; SensorImu3dPtr sensor_imu_;
// parameters // parameters
BootstrapMethod bootstrap_method_; BootstrapMethod bootstrap_method_;
......
src/processor/processor_imu_3d.cpp
+ 63
− 65
View file @ 8afb7294
...@@ -37,7 +37,8 @@ ProcessorImu3d::ProcessorImu3d(const YAML::Node &_params) ...@@ -37,7 +37,8 @@ ProcessorImu3d::ProcessorImu3d(const YAML::Node &_params)
9, 9,
6, 6,
6, 6,
_params) _params),
bootstrap_last_frame_(nullptr)
{ {
bootstrapping_ = _params["bootstrap"]["enabled"].as<bool>(); bootstrapping_ = _params["bootstrap"]["enabled"].as<bool>();
bootstrap_factor_list_.clear(); bootstrap_factor_list_.clear();
...@@ -99,13 +100,13 @@ bool ProcessorImu3d::voteForKeyFrame() const ...@@ -99,13 +100,13 @@ bool ProcessorImu3d::voteForKeyFrame() const
return false; return false;
} }
CaptureMotionPtr ProcessorImu3d::emplaceCapture(const FrameBasePtr & _frame_own, CaptureMotionPtr ProcessorImu3d::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) const CaptureBasePtr &_capture_origin)
{ {
auto cap_motion = std::static_pointer_cast<CaptureMotion>( auto cap_motion = std::static_pointer_cast<CaptureMotion>(
...@@ -177,9 +178,9 @@ void ProcessorImu3d::computeCurrentDelta(const Eigen::VectorXd &_data, ...@@ -177,9 +178,9 @@ void ProcessorImu3d::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 & _jac_delta_calib) const Eigen::MatrixXd &_jac_delta_calib) const
{ {
assert(_data.size() == data_size_ && "Wrong data size!"); assert(_data.size() == data_size_ && "Wrong data size!");
...@@ -218,7 +219,7 @@ void ProcessorImu3d::computeCurrentDelta(const Eigen::VectorXd &_data, ...@@ -218,7 +219,7 @@ void ProcessorImu3d::computeCurrentDelta(const Eigen::VectorXd &_data,
void ProcessorImu3d::deltaPlusDelta(const Eigen::VectorXd &_delta_preint, void ProcessorImu3d::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 Eigen::VectorXd &_delta_preint_plus_delta) const
{ {
/* MATHS according to Sola-16 /* MATHS according to Sola-16
* Dp' = Dp + Dv*dt + 1/2*Dq*(a-a_b)*dt^2 = Dp + Dv*dt + Dq*dp if dp = * Dp' = Dp + Dv*dt + 1/2*Dq*(a-a_b)*dt^2 = Dp + Dv*dt + Dq*dp if dp =
...@@ -235,7 +236,7 @@ void ProcessorImu3d::deltaPlusDelta(const Eigen::VectorXd &_delta_preint, ...@@ -235,7 +236,7 @@ void ProcessorImu3d::deltaPlusDelta(const Eigen::VectorXd &_delta_preint,
void ProcessorImu3d::statePlusDelta(const VectorComposite &_x, void ProcessorImu3d::statePlusDelta(const VectorComposite &_x,
const Eigen::VectorXd &_delta, const Eigen::VectorXd &_delta,
const double _dt, const double _dt,
VectorComposite & _x_plus_delta) const VectorComposite &_x_plus_delta) const
{ {
assert(_delta.size() == 10 && "Wrong _delta vector size"); assert(_delta.size() == 10 && "Wrong _delta vector size");
assert(_dt >= 0 && "Time interval _dt is negative!"); assert(_dt >= 0 && "Time interval _dt is negative!");
...@@ -255,9 +256,9 @@ void ProcessorImu3d::statePlusDelta(const VectorComposite &_x, ...@@ -255,9 +256,9 @@ void ProcessorImu3d::statePlusDelta(const VectorComposite &_x,
void ProcessorImu3d::deltaPlusDelta(const Eigen::VectorXd &_delta_preint, void ProcessorImu3d::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 Eigen::MatrixXd &_jacobian_delta) const
{ {
/* /*
* Expression of the delta integration step, D' = D (+) d: * Expression of the delta integration step, D' = D (+) d:
...@@ -314,7 +315,46 @@ void ProcessorImu3d::bootstrap() ...@@ -314,7 +315,46 @@ void ProcessorImu3d::bootstrap()
{ {
case BootstrapMethod::BOOTSTRAP_STATIC: case BootstrapMethod::BOOTSTRAP_STATIC:
// Implementation of static strategy // ALL TIMES:
// To all frames not processed yet:
// Fix zero velocity, initialize zero PO, and add zero 3D relative pose factors
while (bootstrap_last_frame_ != getProblem()->getLastFrame())
{
// emplace missing states
getProblem()->emplaceStatesToFrame(bootstrap_last_frame_, getProblem()->stateZero(getStateKeys()));
// Fix zero-velocity
bootstrap_last_frame_->getV()->setZero();
bootstrap_last_frame_->getV()->fix();
// Initialize state as same as first frame
bootstrap_last_frame_->setState(first_capture->getFrame()->getState("PO"));
// Factor zero-relative pose
if (bootstrap_last_frame_->getPreviousFrame())
{
auto capture_zero = CaptureBase::emplace<CaptureVoid>(
bootstrap_last_frame_, bootstrap_last_frame_->getTimeStamp(), nullptr);
auto feature_zero = FeatureBase::emplace<FeatureBase>(
capture_zero, "FeatureZeroOdom", Vector7d::Zero(), Eigen::MatrixXd::Identity(6, 6) * 0.01);
FactorBase::emplace<FactorRelativePose3d>(feature_zero,
feature_zero->getMeasurement(),
feature_zero->getMeasurementSquareRootInformationUpper(),
bootstrap_last_frame_->getPreviousFrame(),
bootstrap_last_frame_,
nullptr,
false,
TOP_MOTION);
}
// next
bootstrap_last_frame_ = bootstrap_last_frame_->getNextFrame();
}
// ONLY AFTER bootstrap_averaging_length_ seconds:
// Orientation initialization aligning with g
if (t_current - first_capture->getTimeStamp() >= bootstrap_averaging_length_) if (t_current - first_capture->getTimeStamp() >= 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'
...@@ -340,44 +380,6 @@ void ProcessorImu3d::bootstrap() ...@@ -340,44 +380,6 @@ void ProcessorImu3d::bootstrap()
// Transform problem to new reference // Transform problem to new reference
getProblem()->transform(transfo_w_l); getProblem()->transform(transfo_w_l);
// Recompute states at keyframes if they were provided by this processor
bool recomputed = recomputeStates();
if (recomputed)
{
WOLF_DEBUG("IMU Keyframe states have been recomputed!");
}
// Add factors for the STATIC strategy:
auto last_frame = getProblem()->getLastFrame();
while (last_frame)
{
// Fix zero-velocity
last_frame->getV()->setZero();
last_frame->getV()->fix();
// Factor zero-displacement
if (last_frame->getPreviousFrame())
{
auto capture_zero =
CaptureBase::emplace<CaptureVoid>(last_frame, last_frame->getTimeStamp(), nullptr);
auto feature_zero = FeatureBase::emplace<FeatureBase>(
capture_zero, "FeatureZeroOdom", Vector7d::Zero(), Eigen::MatrixXd::Identity(6, 6) * 0.01);
FactorBase::emplace<FactorRelativePose3d>(
feature_zero,
feature_zero->getMeasurement(),
feature_zero->getMeasurementSquareRootInformationUpper(),
last_frame->getPreviousFrame(),
last_frame,
nullptr,
false,
TOP_MOTION);
}
// next
last_frame = last_frame->getPreviousFrame();
}
// Activate factors that were inactive during bootstrap // Activate factors that were inactive during bootstrap
while (not bootstrap_factor_list_.empty()) while (not bootstrap_factor_list_.empty())
{ {
...@@ -385,10 +387,10 @@ void ProcessorImu3d::bootstrap() ...@@ -385,10 +387,10 @@ void ProcessorImu3d::bootstrap()
bootstrap_factor_list_.pop_front(); bootstrap_factor_list_.pop_front();
} }
// Clear bootstrapping flag. This marks the end of the bootstrapping // Clear bootstrapping flag. This marks the end of the bootstrapping process
// process
bootstrapping_ = false; bootstrapping_ = false;
} }
break; break;
case BootstrapMethod::BOOTSTRAP_G: case BootstrapMethod::BOOTSTRAP_G:
...@@ -423,10 +425,7 @@ void ProcessorImu3d::bootstrap() ...@@ -423,10 +425,7 @@ void ProcessorImu3d::bootstrap()
// Recompute states at keyframes if they were provided by this processor // Recompute states at keyframes if they were provided by this processor
bool recomputed = recomputeStates(); bool recomputed = recomputeStates();
if (recomputed) WOLF_DEBUG_COND(recomputed, "IMU Keyframe states have been recomputed!");
{
WOLF_DEBUG("IMU Keyframe states have been recomputed!");
}
// Activate factors that were inactive during bootstrap // Activate factors that were inactive during bootstrap
while (not bootstrap_factor_list_.empty()) while (not bootstrap_factor_list_.empty())
...@@ -435,8 +434,7 @@ void ProcessorImu3d::bootstrap() ...@@ -435,8 +434,7 @@ void ProcessorImu3d::bootstrap()
bootstrap_factor_list_.pop_front(); bootstrap_factor_list_.pop_front();
} }
// Clear bootstrapping flag. This marks the end of the bootstrapping // Clear bootstrapping flag. This marks the end of the bootstrapping process
// process
bootstrapping_ = false; bootstrapping_ = false;
} }
break; break;
...@@ -532,8 +530,8 @@ bool ProcessorImu3d::recomputeStates() const ...@@ -532,8 +530,8 @@ bool ProcessorImu3d::recomputeStates() const
const auto &cap_origin = cap->getOriginCapture(); const auto &cap_origin = cap->getOriginCapture();
const auto &frm_origin = cap_origin->getFrame(); const auto &frm_origin = cap_origin->getFrame();
const auto &delta = VectorComposite({{'P', ftr->getMeasurement().head<3>()}, const auto &delta = VectorComposite({{'P', ftr->getMeasurement().head<3>()},
{'O', ftr->getMeasurement().segment<4>(3)}, {'O', ftr->getMeasurement().segment<4>(3)},
{'V', ftr->getMeasurement().tail<3>()}}); {'V', ftr->getMeasurement().tail<3>()}});
const auto &x_origin = frm_origin->getState(); const auto &x_origin = frm_origin->getState();
auto dt = cap->getTimeStamp() - cap_origin->getTimeStamp(); auto dt = cap->getTimeStamp() - cap_origin->getTimeStamp();
auto x = imu::composeOverState(x_origin, delta, dt); auto x = imu::composeOverState(x_origin, delta, dt);
......
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