diff --git a/include/imu/processor/processor_imu_3d.h b/include/imu/processor/processor_imu_3d.h index 69b7557e51c8847f0966dbaa4b5ee5b95e7f5d7e..6a44e268bd00411833e869023e81f9bbe0697102 100644 --- a/include/imu/processor/processor_imu_3d.h +++ b/include/imu/processor/processor_imu_3d.h @@ -54,36 +54,36 @@ class ProcessorImu3d : public ProcessorMotion 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; + 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; + 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; + 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; + 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, + 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; virtual void emplaceFeaturesAndFactors(CaptureBasePtr _capture_origin, CaptureMotionPtr _capture_own) override; @@ -115,9 +115,9 @@ class ProcessorImu3d : public ProcessorMotion bool recomputeStates() const; protected: - std::list<FactorBasePtr> bootstrap_factor_list_; ///< List of all IMU factors created while IMU is - ///< bootstrapping - SensorImu3dPtr sensor_imu_; + std::list<FactorBasePtr> bootstrap_factor_list_; ///< List of IMU factors created while bootstrapping + FrameBasePtr bootstrap_last_frame_; ///< Last frame processed by the bootstrapping + SensorImu3dPtr sensor_imu_; // parameters BootstrapMethod bootstrap_method_; diff --git a/src/processor/processor_imu_3d.cpp b/src/processor/processor_imu_3d.cpp index 086c6a434f3e7e429b432c4194330cda880c8d95..bd682e851326af51e11a5be84f2d33b54883f202 100644 --- a/src/processor/processor_imu_3d.cpp +++ b/src/processor/processor_imu_3d.cpp @@ -37,7 +37,8 @@ ProcessorImu3d::ProcessorImu3d(const YAML::Node &_params) 9, 6, 6, - _params) + _params), + bootstrap_last_frame_(nullptr) { bootstrapping_ = _params["bootstrap"]["enabled"].as<bool>(); bootstrap_factor_list_.clear(); @@ -99,13 +100,13 @@ bool ProcessorImu3d::voteForKeyFrame() const return false; } -CaptureMotionPtr ProcessorImu3d::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, +CaptureMotionPtr ProcessorImu3d::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) { auto cap_motion = std::static_pointer_cast<CaptureMotion>( @@ -177,9 +178,9 @@ void ProcessorImu3d::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 & _jac_delta_calib) const + Eigen::VectorXd &_delta, + Eigen::MatrixXd &_delta_cov, + Eigen::MatrixXd &_jac_delta_calib) const { assert(_data.size() == data_size_ && "Wrong data size!"); @@ -218,7 +219,7 @@ void ProcessorImu3d::computeCurrentDelta(const Eigen::VectorXd &_data, void ProcessorImu3d::deltaPlusDelta(const Eigen::VectorXd &_delta_preint, const Eigen::VectorXd &_delta, const double _dt, - Eigen::VectorXd & _delta_preint_plus_delta) const + Eigen::VectorXd &_delta_preint_plus_delta) const { /* MATHS according to Sola-16 * 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, void ProcessorImu3d::statePlusDelta(const VectorComposite &_x, const Eigen::VectorXd &_delta, const double _dt, - VectorComposite & _x_plus_delta) const + VectorComposite &_x_plus_delta) const { assert(_delta.size() == 10 && "Wrong _delta vector size"); assert(_dt >= 0 && "Time interval _dt is negative!"); @@ -255,9 +256,9 @@ void ProcessorImu3d::statePlusDelta(const VectorComposite &_x, void ProcessorImu3d::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 + Eigen::VectorXd &_delta_preint_plus_delta, + Eigen::MatrixXd &_jacobian_delta_preint, + Eigen::MatrixXd &_jacobian_delta) const { /* * Expression of the delta integration step, D' = D (+) d: @@ -314,7 +315,46 @@ void ProcessorImu3d::bootstrap() { 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_) { // get initial IMU frame 's' expressed in local world frame 'l' @@ -340,44 +380,6 @@ void ProcessorImu3d::bootstrap() // Transform problem to new reference 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 while (not bootstrap_factor_list_.empty()) { @@ -385,10 +387,10 @@ void ProcessorImu3d::bootstrap() bootstrap_factor_list_.pop_front(); } - // Clear bootstrapping flag. This marks the end of the bootstrapping - // process + // Clear bootstrapping flag. This marks the end of the bootstrapping process bootstrapping_ = false; } + break; case BootstrapMethod::BOOTSTRAP_G: @@ -423,10 +425,7 @@ void ProcessorImu3d::bootstrap() // Recompute states at keyframes if they were provided by this processor bool recomputed = recomputeStates(); - if (recomputed) - { - WOLF_DEBUG("IMU Keyframe states have been recomputed!"); - } + WOLF_DEBUG_COND(recomputed, "IMU Keyframe states have been recomputed!"); // Activate factors that were inactive during bootstrap while (not bootstrap_factor_list_.empty()) @@ -435,8 +434,7 @@ void ProcessorImu3d::bootstrap() bootstrap_factor_list_.pop_front(); } - // Clear bootstrapping flag. This marks the end of the bootstrapping - // process + // Clear bootstrapping flag. This marks the end of the bootstrapping process bootstrapping_ = false; } break; @@ -532,8 +530,8 @@ bool ProcessorImu3d::recomputeStates() const const auto &cap_origin = cap->getOriginCapture(); const auto &frm_origin = cap_origin->getFrame(); const auto &delta = VectorComposite({{'P', ftr->getMeasurement().head<3>()}, - {'O', ftr->getMeasurement().segment<4>(3)}, - {'V', ftr->getMeasurement().tail<3>()}}); + {'O', ftr->getMeasurement().segment<4>(3)}, + {'V', ftr->getMeasurement().tail<3>()}}); const auto &x_origin = frm_origin->getState(); auto dt = cap->getTimeStamp() - cap_origin->getTimeStamp(); auto x = imu::composeOverState(x_origin, delta, dt);