diff --git a/.gitignore b/.gitignore index b4e53703e896e0c36e288c8c4e1e13e71c42ab2d..4d9c52e82fe8dee50c99c9a1c8548575962ce658 100644 --- a/.gitignore +++ b/.gitignore @@ -39,4 +39,5 @@ src/examples/map_apriltag_save.yaml .ccls-root compile_commands.json .vimspector.json -est.csv +est*.csv +.clang-format diff --git a/demos/processor_imu.yaml b/demos/processor_imu.yaml index e1b0c2e86eb5212679e7c75db2ba2f25fc039348..1afbb00a6ab5112cbad1555098985ae1f0687f8b 100644 --- a/demos/processor_imu.yaml +++ b/demos/processor_imu.yaml @@ -9,3 +9,8 @@ keyframe_vote: max_buff_length: 20000 # motion deltas dist_traveled: 2.0 # meters angle_turned: 0.2 # radians (1 rad approx 57 deg, approx 60 deg) + +bootstrap: + enable: false + method: "G" + averaging_length: 0.10 # seconds \ No newline at end of file diff --git a/demos/processor_imu_bootstrap.yaml b/demos/processor_imu_bootstrap.yaml new file mode 100644 index 0000000000000000000000000000000000000000..3a6c6c9587ab23aeb66ae95495c8054be46e0c96 --- /dev/null +++ b/demos/processor_imu_bootstrap.yaml @@ -0,0 +1,17 @@ +type: "ProcessorImu" # This must match the KEY used in the SensorFactory. Otherwise it is an error. + +time_tolerance: 0.0025 # Time tolerance for joining KFs +unmeasured_perturbation_std: 0.00001 + +keyframe_vote: + voting_active: false + max_time_span: 2.0 # seconds + max_buff_length: 20000 # motion deltas + dist_traveled: 2.0 # meters + angle_turned: 0.2 # radians (1 rad approx 57 deg, approx 60 deg) + +bootstrap: + enable: true + method: "G" # methods: "G", "STATIC" or "V0_G" + averaging_length: 0.10 # seconds + keyframe_provider_processor_name: "processor_other_name" # Not yet implemented \ No newline at end of file diff --git a/include/imu/math/imu2d_tools.h b/include/imu/math/imu2d_tools.h index f579ae2dc44fc2f7cd5a6d1b70ba49df21dfdcb7..316405e50afa3ce2a620766e6c3a910615797536 100644 --- a/include/imu/math/imu2d_tools.h +++ b/include/imu/math/imu2d_tools.h @@ -257,7 +257,7 @@ inline void compose(const MatrixBase<D1>& d1, // Matrix3d dR1 = q2R(d1.at('O')); //dq1.matrix(); // First Delta, DR // Matrix3d dR2 = q2R(d2.at('O')); //dq2.matrix(); // Second delta, dR // -// // Jac wrt first delta // TODO find optimal way to re-use memory allocation!!! +// // Jac wrt first delta // J_sum_d1.clear(); // J_sum_d1.emplace('P','P', Matrix3d::Identity()); // dDp'/dDp = I // J_sum_d1.emplace('P','O', - dR1 * skew(d2.at('P'))) ; // dDp'/dDo diff --git a/include/imu/math/imu_tools.h b/include/imu/math/imu_tools.h index 7101c04eb11afd5285fd2f77dd2f9c281780c5fc..c5744fbae31fea609aad91e70da9cf55cf83bb66 100644 --- a/include/imu/math/imu_tools.h +++ b/include/imu/math/imu_tools.h @@ -282,7 +282,7 @@ inline void compose(const VectorComposite& d1, Matrix3d dR1 = q2R(d1.at('O')); //dq1.matrix(); // First Delta, DR Matrix3d dR2 = q2R(d2.at('O')); //dq2.matrix(); // Second delta, dR - // Jac wrt first delta // TODO find optimal way to re-use memory allocation!!! + // Jac wrt first delta J_sum_d1.clear(); J_sum_d1.emplace('P','P', Matrix3d::Identity()); // dDp'/dDp = I J_sum_d1.emplace('P','O', - dR1 * skew(d2.at('P'))) ; // dDp'/dDo @@ -868,6 +868,65 @@ Matrix<typename D1::Scalar, 6, 1> motion2data(const MatrixBase<D1>& motion, cons return data; } +/** extract local g and v0 from three keyframes and deltas + * @param p0 position of kf0 + * @param q0 orientation of kf0 + * @param p1 position of kf0 + * @param q1 orientation of kf0 + * @param p2 position of kf0 + * @param Dt01 time delta between kf0 and kf1 + * @param Dp01 position delta between kf0 and kf1 + * @param Dv01 orientation delta between kf0 and kf1 + * @param Dt12 time delta between kf1 and kf2 + * @param Dp12 position delta between kf1 and kf2 + * @param Dv12 orientation delta between kf1 and kf2 + * + * This method needs 3 kfs to solve. Inspired on Lupton-11 with a revised + * implementation by jsola-22, see https://www.overleaf.com/project/629e276e7f68b0c2bfa469ac + */ +template <typename TP0, + typename TQ0, + typename TP1, + typename TQ1, + typename TP2, + typename TDT01, + typename TDP01, + typename TDV01, + typename TDT12, + typename TDP12, + typename TV0, + typename TG> +void extract_v0_g(const MatrixBase<TP0> &p0, + const QuaternionBase<TQ0> &q0, + const MatrixBase<TP1> &p1, + const QuaternionBase<TQ1> &q1, + const MatrixBase<TP2> &p2, + const TDT01 &Dt01, + const MatrixBase<TDP01> &Dp01, + const MatrixBase<TDV01> &Dv01, + const TDT12 &Dt12, + const MatrixBase<TDP12> &Dp12, + MatrixBase<TV0> &v0, + MatrixBase<TG> &g) +{ + // full time period between kfs 0 and 2 + const TDT01 Dt02 = Dt01 + Dt12; + + // Scalar coefficients of entries of 6x6 matrix A.inv + const TDT01 ai_v0 = (Dt02 / (Dt01 * Dt12)); + const TDT01 ai_v1 = (-Dt01 / (Dt12 * Dt02)); + const TDT01 ai_g0 = (-2 / (Dt01 * Dt12)); + const TDT01 ai_g1 = (2 / (Dt12 * Dt02)); + + // 3-vector entries of 6-vector b + Matrix<typename TV0::Scalar, 3, 1> b0 = p1 - p0 - q0 * Dp01; + Matrix<typename TV0::Scalar, 3, 1> b1 = p2 - p0 - q0 * (Dp01 + Dv01 * Dt12) - q1 * Dp12; + + // output vectors -- this is x = A.inv * b + v0 = ai_v0 * b0 + ai_v1 * b1; + g = ai_g0 * b0 + ai_g1 * b1; +} + } // namespace imu } // namespace wolf diff --git a/include/imu/processor/processor_imu.h b/include/imu/processor/processor_imu.h index d8efc7cbd3221654da4721ff8bc21e6c15bc4353..94388550eedf13e50e2a16be7d3ca060159e21c4 100644 --- a/include/imu/processor/processor_imu.h +++ b/include/imu/processor/processor_imu.h @@ -32,11 +32,41 @@ WOLF_STRUCT_PTR_TYPEDEFS(ParamsProcessorImu); struct ParamsProcessorImu : public ParamsProcessorMotion { + bool bootstrap_enable; + enum BootstrapMethod + { + BOOTSTRAP_STATIC, + BOOTSTRAP_G, + BOOTSTRAP_V0_G + } bootstrap_method; + double bootstrap_averaging_length; + ParamsProcessorImu() = default; ParamsProcessorImu(std::string _unique_name, const ParamsServer& _server): ParamsProcessorMotion(_unique_name, _server) { - // + bootstrap_enable = _server.getParam<bool>(prefix + _unique_name + "/bootstrap/enable"); + if (_server.hasParam(prefix + _unique_name + "/bootstrap/method")) + { + string str = _server.getParam<string>(prefix + _unique_name + "/bootstrap/method"); + std::transform(str.begin(), str.end(), str.begin(), ::toupper); + if (str == "STATIC" /**/) + { + bootstrap_method = BOOTSTRAP_STATIC; + bootstrap_averaging_length = + _server.getParam<double>(prefix + _unique_name + "/bootstrap/averaging_length"); + } + if (str == "G" /* */) + { + bootstrap_method = BOOTSTRAP_G; + bootstrap_averaging_length = + _server.getParam<double>(prefix + _unique_name + "/bootstrap/averaging_length"); + } + if (str == "V0_G" /* */) + { + bootstrap_method = BOOTSTRAP_V0_G; + } + } } std::string print() const override { @@ -51,55 +81,60 @@ class ProcessorImu : public ProcessorMotion{ public: ProcessorImu(ParamsProcessorImuPtr _params_motion_Imu); ~ProcessorImu() override; + WOLF_PROCESSOR_CREATE(ProcessorImu, ParamsProcessorImu); void configure(SensorBasePtr _sensor) override { }; - WOLF_PROCESSOR_CREATE(ProcessorImu, ParamsProcessorImu); void preProcess() override; + void bootstrapEnable(bool _bootstrap_enable = true); - 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; + 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; - protected: - ParamsProcessorImuPtr params_motion_Imu_; -}; + virtual void bootstrap() override; + CaptureBasePtr bootstrapOrigin() const; + VectorXd bootstrapDelta() const; + bool recomputeStates() const; + protected: + ParamsProcessorImuPtr params_motion_Imu_; + std::list<FactorBasePtr> list_fac_inactive_bootstrap_; +}; } ///////////////////////////////////////////////////////// diff --git a/src/capture/capture_imu.cpp b/src/capture/capture_imu.cpp index f8a4b0b4118c299fda57c59e77df1b922b2e292a..14a604c9261613207b5f73c1a899bcf6f782136d 100644 --- a/src/capture/capture_imu.cpp +++ b/src/capture/capture_imu.cpp @@ -21,43 +21,51 @@ //--------LICENSE_END-------- #include "imu/capture/capture_imu.h" #include "imu/sensor/sensor_imu.h" -#include "core/state_block/state_quaternion.h" + +#include <core/state_block/state_quaternion.h> +#include <core/state_block/state_block_derived.h> namespace wolf { -CaptureImu::CaptureImu(const TimeStamp& _init_ts, - SensorBasePtr _sensor_ptr, +CaptureImu::CaptureImu(const TimeStamp& _init_ts, + SensorBasePtr _sensor_ptr, const Eigen::Vector6d& _acc_gyro_data, const Eigen::MatrixXd& _data_cov, - CaptureBasePtr _capture_origin_ptr) : - CaptureMotion("CaptureImu", - _init_ts, - _sensor_ptr, - _acc_gyro_data, - _data_cov, - _capture_origin_ptr, - nullptr, - nullptr, - std::make_shared<StateBlock>((_sensor_ptr->getProblem()->getDim() == 2 ? 3 : 6), false)) + CaptureBasePtr _capture_origin_ptr) + : CaptureMotion( + "CaptureImu", + _init_ts, + _sensor_ptr, + _acc_gyro_data, + _data_cov, + _capture_origin_ptr, + nullptr, + nullptr, + (_sensor_ptr->getProblem()->getDim() == 2) + ? std::static_pointer_cast<StateBlock>(std::make_shared<StateParams3>(Vector3d::Zero(), false)) + : std::static_pointer_cast<StateBlock>(std::make_shared<StateParams6>(Vector6d::Zero(), false))) { // } -CaptureImu::CaptureImu(const TimeStamp& _init_ts, - SensorBasePtr _sensor_ptr, +CaptureImu::CaptureImu(const TimeStamp& _init_ts, + SensorBasePtr _sensor_ptr, const Eigen::Vector6d& _acc_gyro_data, const Eigen::MatrixXd& _data_cov, - const VectorXd& _bias, - CaptureBasePtr _capture_origin_ptr) : - CaptureMotion("CaptureImu", - _init_ts, - _sensor_ptr, - _acc_gyro_data, - _data_cov, - _capture_origin_ptr, - nullptr, - nullptr, - std::make_shared<StateBlock>(_bias, false)) + const VectorXd& _bias, + CaptureBasePtr _capture_origin_ptr) + : CaptureMotion( + "CaptureImu", + _init_ts, + _sensor_ptr, + _acc_gyro_data, + _data_cov, + _capture_origin_ptr, + nullptr, + nullptr, + (_bias.size() == 3) + ? std::static_pointer_cast<StateBlock>(std::make_shared<StateParams3>(Vector3d::Zero(), false)) + : std::static_pointer_cast<StateBlock>(std::make_shared<StateParams6>(Vector6d::Zero(), false))) { assert((_bias.size() == 3) or (_bias.size() == 6)); } diff --git a/src/processor/processor_imu.cpp b/src/processor/processor_imu.cpp index 5e429fd80355bddf587eaccafec7bf4db8476b02..b72182fcdda85f359e9da9f3a67a52b0c6ccd4f3 100644 --- a/src/processor/processor_imu.cpp +++ b/src/processor/processor_imu.cpp @@ -31,9 +31,10 @@ namespace wolf { ProcessorImu::ProcessorImu(ParamsProcessorImuPtr _params_motion_imu) : ProcessorMotion("ProcessorImu", "POV", 3, 10, 10, 9, 6, 6, _params_motion_imu), - params_motion_Imu_(std::make_shared<ParamsProcessorImu>(*_params_motion_imu)) + params_motion_Imu_(_params_motion_imu) { - // + bootstrapping_ = params_motion_Imu_->bootstrap_enable; + list_fac_inactive_bootstrap_.clear(); } ProcessorImu::~ProcessorImu() @@ -48,13 +49,13 @@ void ProcessorImu::preProcess() bool ProcessorImu::voteForKeyFrame() const { // time span - if (getBuffer().back().ts_ - getBuffer().front().ts_ > params_motion_Imu_->max_time_span) + if (getBuffer().back().ts_ - getBuffer().front().ts_ >= params_motion_Imu_->max_time_span - Constants::EPS) { WOLF_DEBUG( "PM: vote: time span" ); return true; } // buffer length - if (getBuffer().size() > params_motion_Imu_->max_buff_length) + if (getBuffer().size() -1 > params_motion_Imu_->max_buff_length) { WOLF_DEBUG( "PM: vote: buffer length" ); return true; @@ -122,6 +123,12 @@ FactorBasePtr ProcessorImu::emplaceFactor(FeatureBasePtr _feature_motion, Captur auto fac_imu = FactorBase::emplace<FactorImu>(_feature_motion, ftr_imu, cap_imu, shared_from_this(), params_->apply_loss_function); + if (bootstrapping_) + { + fac_imu->setStatus(FAC_INACTIVE); + list_fac_inactive_bootstrap_.push_back(fac_imu); + } + return fac_imu; } @@ -240,9 +247,195 @@ Eigen::VectorXd ProcessorImu::correctDelta (const Eigen::VectorXd& delta_preint, return imu::plus(delta_preint, delta_step); } +void ProcessorImu::bootstrap() +{ + // TODO bootstrap strategies. + // See Sola-22 "Imu bootstrap strategies" https://www.overleaf.com/project/629e276e7f68b0c2bfa469ac + // frames: + // w: world global ( where g = [0,0,-9.806] ); + // l: world local; + // r: robot; + // s: sensor (IMU) + + CaptureBasePtr first_capture = bootstrapOrigin(); + TimeStamp t_current = last_ptr_->getBuffer().back().ts_; + VectorComposite transformation("PO"); + switch (params_motion_Imu_->bootstrap_method) + { + case ParamsProcessorImu::BootstrapMethod::BOOTSTRAP_STATIC: + + // Implementation of static strategy + 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; + + // Compute total integrated delta during bootstrap period + VectorXd delta_int = bootstrapDelta(); + + // compute local g and transformation to global g + double dt = t_current - first_capture->getTimeStamp(); // + const auto& dv = delta_int.segment(7, 3); // + Vector3d g_l = -(q_l_s * dv / dt); // See eq. (20) + const auto& g_w = gravity(); // + const auto& p_w_l = Vector3d::Zero(); // will pivot around the origin + Quaterniond q_w_l = Quaterniond::FromTwoVectors(g_l, g_w); // + transformation.at('P') = p_w_l; // + transformation.at('O') = q_w_l.coeffs(); // + + // Transform problem to new reference + getProblem()->transform(transformation); + + // Recompute states at keyframes if they were provided by this processor + bool recomputed = recomputeStates(); + if (recomputed) + { + WOLF_DEBUG("IMU Keyframe states have been recomputed!"); + } + + // TODO: add factors for the STATIC strategy: + // - zero-velocity factors (at each KF) + // - zero-displaecement odom3d factors (between KFs) + + // Activate factors that were inactive during bootstrap + while (not list_fac_inactive_bootstrap_.empty()) + { + list_fac_inactive_bootstrap_.front()->setStatus(FAC_ACTIVE); + list_fac_inactive_bootstrap_.pop_front(); + } + + // Clear bootstrapping flag. This marks the end of the bootstrapping process + bootstrapping_ = false; + } + break; + + case ParamsProcessorImu::BootstrapMethod::BOOTSTRAP_G: + + // Implementation of G strategy. + 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; + + // Compute total integrated delta during bootstrap period + VectorXd delta_int = bootstrapDelta(); + + // compute local g and transformation to global g + double dt = t_current - first_capture->getTimeStamp(); // + const auto& dv_l = delta_int.segment(7, 3); // + Vector3d g_l = -(q_l_s * dv_l / dt); // See eq. (20) + const auto& g_w = gravity(); // + const auto& p_w_l = Vector3d::Zero(); // will pivot around the origin + Quaterniond q_w_l = Quaterniond::FromTwoVectors(g_l, g_w); // + transformation.at('P') = p_w_l; // + transformation.at('O') = q_w_l.coeffs(); // + + // Transform problem to new reference + getProblem()->transform(transformation); + + // Recompute states at keyframes if they were provided by this processor + bool recomputed = recomputeStates(); + if (recomputed) + { + WOLF_DEBUG("IMU Keyframe states have been recomputed!"); + } + + // Activate factors that were inactive during bootstrap + while (not list_fac_inactive_bootstrap_.empty()) + { + list_fac_inactive_bootstrap_.front()->setStatus(FAC_ACTIVE); + list_fac_inactive_bootstrap_.pop_front(); + } + + // Clear bootstrapping flag. This marks the end of the bootstrapping process + bootstrapping_ = false; + } + break; + + case ParamsProcessorImu::BootstrapMethod::BOOTSTRAP_V0_G: + + // TODO implement v0-g strategy + WOLF_WARN("Bootstrapping strategy BOOTSTRAP_V0_G not yet implemented. Disabling bootstrap!"); + bootstrapping_ = false; + break; + + default: + + // No strategy provided: clear bootstrapping flag and warn + WOLF_WARN("Bootstrapping enabled but no viable strategy detected. Disabling bootstrap!"); + bootstrapping_ = false; + break; + } +} + +void ProcessorImu::bootstrapEnable(bool _bootstrap_enable) +{ + params_motion_Imu_->bootstrap_enable = _bootstrap_enable; + bootstrapping_ = _bootstrap_enable; +}; + +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_->getBuffer().back().delta_integr_; + delta_int = imu::compose(delta_int, delta, dt); + return delta_int; +} + +bool ProcessorImu::recomputeStates() const +{ + const auto& mp = getProblem()->getMotionProviderMap(); + if (not mp.empty() and + mp.begin()->second == std::static_pointer_cast<const MotionProvider>( + std::static_pointer_cast<const ProcessorMotion>(shared_from_this()))) + { + WOLF_DEBUG("Recomputing IMU keyframe states..."); + for (const auto& fac : list_fac_inactive_bootstrap_) + { + const auto& ftr = fac->getFeature(); + const auto& cap = std::static_pointer_cast<CaptureMotion>(ftr->getCapture()); + const auto& frm = cap->getFrame(); + const auto& cap_origin = cap->getOriginCapture(); + const auto& frm_origin = cap_origin->getFrame(); + const auto& delta = VectorComposite(ftr->getMeasurement(), "POV", {3, 4, 3}); + const auto& x_origin = frm_origin->getState(); + auto dt = cap->getTimeStamp() - cap_origin->getTimeStamp(); + auto x = imu::composeOverState(x_origin, delta, dt); + frm->setState(x); + } + return true; + } + else + return false; +} -} // namespace wolf +} // namespace wolf // Register in the FactoryProcessor #include "core/processor/factory_processor.h" diff --git a/src/sensor/sensor_imu.cpp b/src/sensor/sensor_imu.cpp index bb6ec35e0061202819385fe04b5b2c58c54a3160..fba6cdf5e008b6dbe12dc1c7df5875ffadef59d3 100644 --- a/src/sensor/sensor_imu.cpp +++ b/src/sensor/sensor_imu.cpp @@ -20,8 +20,9 @@ // //--------LICENSE_END-------- #include "imu/sensor/sensor_imu.h" -#include "core/state_block/state_block.h" -#include "core/state_block/state_quaternion.h" + +#include <core/state_block/state_quaternion.h> +#include <core/state_block/state_block_derived.h> namespace wolf { @@ -33,9 +34,9 @@ SensorImu::SensorImu(const Eigen::VectorXd& _extrinsics, ParamsSensorImuPtr _par SensorImu::SensorImu(const Eigen::VectorXd& _extrinsics, const ParamsSensorImu& _params) : SensorBase("SensorImu", - std::make_shared<StateBlock>(_extrinsics.head(3), true), - std::make_shared<StateQuaternion>(_extrinsics.tail(4), true), - std::make_shared<StateBlock>(6, false, nullptr), + std::make_shared<StatePoint3d>(_extrinsics.head(3), true, false), + std::make_shared<StateQuaternion>(_extrinsics.tail(4), true, false), + std::make_shared<StateParams6>(Vector6d::Zero(), false), (Eigen::Vector6d()<<_params.a_noise,_params.a_noise,_params.a_noise,_params.w_noise,_params.w_noise,_params.w_noise).finished(), false, false, true), a_noise(_params.a_noise), diff --git a/src/sensor/sensor_imu2d.cpp b/src/sensor/sensor_imu2d.cpp index f74b3baa23383ea5265d7d288abc8d97d225a0ff..ca6130465acdac333330b3c97c656ac12a038778 100644 --- a/src/sensor/sensor_imu2d.cpp +++ b/src/sensor/sensor_imu2d.cpp @@ -19,8 +19,9 @@ // along with this program. If not, see <http://www.gnu.org/licenses/>. // //--------LICENSE_END-------- -#include <imu/sensor/sensor_imu2d.h> -#include <core/state_block/state_block.h> +#include "imu/sensor/sensor_imu2d.h" + +#include <core/state_block/state_block_derived.h> #include <core/state_block/state_angle.h> namespace wolf { @@ -31,20 +32,27 @@ SensorImu2d::SensorImu2d(const Eigen::VectorXd& _extrinsics, ParamsSensorImu2dPt // } -SensorImu2d::SensorImu2d(const Eigen::VectorXd& _extrinsics, const ParamsSensorImu2d& _params) : - SensorBase("SensorImu2d", std::make_shared<StateBlock>(_extrinsics.head(2), true), std::make_shared<StateAngle>(_extrinsics(2), true), std::make_shared<StateBlock>(3, false, nullptr), (Eigen::Vector3d()<<_params.a_noise,_params.a_noise,_params.w_noise).finished(), false, false, true), - a_noise(_params.a_noise), - w_noise(_params.w_noise), - ab_initial_stdev(_params.ab_initial_stdev), - wb_initial_stdev(_params.wb_initial_stdev), - ab_rate_stdev(_params.ab_rate_stdev), - wb_rate_stdev(_params.wb_rate_stdev), - orthogonal_gravity(_params.orthogonal_gravity) +SensorImu2d::SensorImu2d(const Eigen::VectorXd& _extrinsics, const ParamsSensorImu2d& _params) + : SensorBase("SensorImu2d", + std::make_shared<StatePoint2d>(_extrinsics.head(2), true, false), + std::make_shared<StateAngle>(_extrinsics(2), true, false), + std::make_shared<StateParams3>(Vector3d::Zero(3), false), + (Eigen::Vector3d() << _params.a_noise, _params.a_noise, _params.w_noise).finished(), + false, + false, + true), + a_noise(_params.a_noise), + w_noise(_params.w_noise), + ab_initial_stdev(_params.ab_initial_stdev), + wb_initial_stdev(_params.wb_initial_stdev), + ab_rate_stdev(_params.ab_rate_stdev), + wb_rate_stdev(_params.wb_rate_stdev), + orthogonal_gravity(_params.orthogonal_gravity) { assert(_extrinsics.size() == 3 && "Wrong extrinsics vector size! Should be 3 for 2d."); - if(!orthogonal_gravity) + if (!orthogonal_gravity) { - addStateBlock('G', std::make_shared<StateBlock>(2, false), false); + addStateBlock('G', std::make_shared<StateVector2d>(Vector2d::Zero(), false, true), false); } } diff --git a/src/yaml/processor_imu_yaml.cpp b/src/yaml/processor_imu_yaml.cpp index 199f59af44bd61d5dcf60ce1a815def2e3cb950a..c5fd0333e06d6732128516081705339840100665 100644 --- a/src/yaml/processor_imu_yaml.cpp +++ b/src/yaml/processor_imu_yaml.cpp @@ -48,17 +48,41 @@ static ParamsProcessorBasePtr createProcessorImuParams(const std::string & _file if (config["type"].as<std::string>() == "ProcessorImu") { - YAML::Node kf_vote = config["keyframe_vote"]; ParamsProcessorImuPtr params = std::make_shared<ParamsProcessorImu>(); params->time_tolerance = config["time_tolerance"] .as<double>(); params->unmeasured_perturbation_std = config["unmeasured_perturbation_std"].as<double>(); - params->max_time_span = kf_vote["max_time_span"] .as<double>(); - params->max_buff_length = kf_vote["max_buff_length"] .as<SizeEigen>(); - params->dist_traveled = kf_vote["dist_traveled"] .as<double>(); - params->angle_turned = kf_vote["angle_turned"] .as<double>(); - params->voting_active = kf_vote["voting_active"] .as<bool>(); + YAML::Node kf_vote = config["keyframe_vote"]; + params->max_time_span = kf_vote["max_time_span"].as<double>(); + params->max_buff_length = kf_vote["max_buff_length"].as<SizeEigen>(); + params->dist_traveled = kf_vote["dist_traveled"].as<double>(); + params->angle_turned = kf_vote["angle_turned"].as<double>(); + params->voting_active = kf_vote["voting_active"].as<bool>(); + + YAML::Node bootstrap = config["bootstrap"]; + params->bootstrap_enable = bootstrap["enable"].as<bool>(); + if (bootstrap["method"]) + // if (params->bootstrap_enable) + { + string str = bootstrap["method"].as<string>(); + std::transform(str.begin(), str.end(), str.begin(), ::toupper); + if (str == "STATIC" /**/) + { + params->bootstrap_method = ParamsProcessorImu::BootstrapMethod::BOOTSTRAP_STATIC; + params->bootstrap_averaging_length = bootstrap["averaging_length"].as<int>(); + } + if (str == "G" /* */) + { + params->bootstrap_method = ParamsProcessorImu::BootstrapMethod::BOOTSTRAP_G; + params->bootstrap_averaging_length = bootstrap["averaging_length"].as<double>(); + } + if (str == "V0_G" /* */) + { + params->bootstrap_method = ParamsProcessorImu::BootstrapMethod::BOOTSTRAP_V0_G; + } + } + return params; } diff --git a/test/gtest_imu.cpp b/test/gtest_imu.cpp index 6e63ec0bba182b5b0e163ed3c7ff094896f4a296..90d12807fe10a24496ab97c4c1e7a532ea566797 100644 --- a/test/gtest_imu.cpp +++ b/test/gtest_imu.cpp @@ -1539,10 +1539,44 @@ TEST_F(Process_Factor_Imu_ODO, RecoverTrajectory_MotionRandom_PqV_b__pqV_b) // F } +TEST_F(Process_Factor_Imu, bootstrap) +{ + processor_imu->setVotingActive(true); + processor_imu->setMaxTimeSpan(0.04); + processor_imu->bootstrapEnable(true); + + auto KF0 = problem->emplaceFrame(0.0); + problem->keyFrameCallback(KF0,nullptr); + problem->print(4, 0, 1, 0); + + // Vector6d data(0,0,9.806, 0,0,0); // gravity on Z + // Vector6d data(0.0, 9.806, 0.0, 0.0, 0.0, 0.0); // gravity on Y + Vector6d data; + data << 0.0, 9.806, 0.0, 0.0, 0.0, 0.0 ; // gravity on Y + + capture_imu = std::make_shared<CaptureImu>(0.0, sensor_imu, data, Matrix6d::Identity()); + + for (t = 0; t < 0.14; t += dt) + { + capture_imu->setTimeStamp(t); + capture_imu->process(); + } + + problem->print(4, 0, 1, 1); + + Quaterniond qref(AngleAxisd(M_PI / 2, Vector3d::UnitX())); // turn of +90deg over X + for (auto pair_ts_frame : problem->getTrajectory()->getFrameMap()) + { + ASSERT_MATRIX_APPROX(pair_ts_frame.second->getStateVector("P"), Vector3d::Zero(), 1e-10); + ASSERT_MATRIX_APPROX(pair_ts_frame.second->getStateVector("O"), qref.coeffs(), 1e-10); + ASSERT_MATRIX_APPROX(pair_ts_frame.second->getStateVector("V"), Vector3d::Zero(), 1e-10); + } +} + int main(int argc, char **argv) { testing::InitGoogleTest(&argc, argv); - // ::testing::GTEST_FLAG(filter) = "Process_Factor_Imu.*"; + // ::testing::GTEST_FLAG(filter) = "Process_Factor_Imu.bootstrap"; // ::testing::GTEST_FLAG(filter) = "Process_Factor_Imu_ODO.*"; // ::testing::GTEST_FLAG(filter) = "Process_Factor_Imu_ODO.RecoverTrajectory_MotionRandom_PqV_b__pqV_b"; diff --git a/test/gtest_imu_tools.cpp b/test/gtest_imu_tools.cpp index 4053b15f912ef7e5cb4b39181e7a5b81395d0c6a..12c6c97f966c2a78dc961d2bec6f39f4698adadf 100644 --- a/test/gtest_imu_tools.cpp +++ b/test/gtest_imu_tools.cpp @@ -641,6 +641,116 @@ TEST(Imu_tools, full_delta_residual) // WOLF_TRACE("Delta err exp-pre: ", Delta_err.transpose()); } +TEST(extract_v0_g, all_zeros) +{ + Vector3d p0(0, 0, 0); + Quaterniond q0(1, 0, 0, 0); + Vector3d p1(0, 0, 0); + Quaterniond q1(1, 0, 0, 0); + Vector3d p2(0, 0, 0); + double Dt01(1), Dt12(1); + Vector3d Dp01(0, 0, 0); + Vector3d Dv01(0, 0, 0); + Vector3d Dp12(0, 0, 0); + Vector3d v0, g; + + extract_v0_g(p0, q0, p1, q1, p2, Dt01, Dp01, Dv01, Dt12, Dp12, v0, g); + + ASSERT_MATRIX_APPROX(v0, Vector3d(0, 0, 0), 1e-15); + ASSERT_MATRIX_APPROX(g, Vector3d(0, 0, 0), 1e-15); +} + +TEST(extract_v0_g, free_fall_no_rotation) +{ + /** free fall v0 = 0 + * + * X p0 = 0, v0 = 0 + * . + * X p1 = -4.9m + * . + * . + * . + * . + * X p2 = -19,6m + */ + + Vector3d p0(0, 0, 0); + Quaterniond q0(1, 0, 0, 0); + Vector3d p1(0, 0, -9.8 / 2); + Quaterniond q1(1, 0, 0, 0); + Vector3d p2(0, 0, -9.8 * 4 / 2); + double Dt01(1), Dt12(1); + Vector3d Dp01(0, 0, 0); + Vector3d Dv01(0, 0, 0); + Vector3d Dp12(0, 0, 0); + Vector3d v0, g; + + extract_v0_g(p0, q0, p1, q1, p2, Dt01, Dp01, Dv01, Dt12, Dp12, v0, g); + + ASSERT_MATRIX_APPROX(v0, Vector3d(0, 0, 0), 1e-15); + ASSERT_MATRIX_APPROX(g, Vector3d(0, 0, -9.8), 1e-15); +} + +TEST(extract_v0_g, free_fall_no_rotation_hor_vel) +{ + /** free fall v0 = 1m/s horizontally --> parabolic fall + * + * X p0 = (0,0)m, v0 = (1,0)m/s + * . + * X p1 = (1,-4.9)m + * . + * . + * . + * . + * X p2 = (2,-19,6)m + */ + Vector3d p0(0, 0, 0); + Quaterniond q0(1, 0, 0, 0); + Vector3d p1(1, 0, -9.8 / 2); + Quaterniond q1(1, 0, 0, 0); + Vector3d p2(2, 0, -9.8 * 4 / 2); + double Dt01(1), Dt12(1); + Vector3d Dp01(0, 0, 0); + Vector3d Dv01(0, 0, 0); + Vector3d Dp12(0, 0, 0); + Vector3d v0, g; + + extract_v0_g(p0, q0, p1, q1, p2, Dt01, Dp01, Dv01, Dt12, Dp12, v0, g); + + ASSERT_MATRIX_APPROX(v0, Vector3d(1, 0, 0), 1e-15); + ASSERT_MATRIX_APPROX(g, Vector3d(0, 0, -9.8), 1e-15); +} + +TEST(extract_v0_g, free_fall_no_rotation_diag_vel) +{ + /** free fall v0 = 1m/s horizontally and 9.8 vert --> parabolic ballistic flight + * + * + * X p1 = (1, +4.9)m + * . . + * . . + * . . + * X p0 = (0,0)m X p2 = (2, 0)m + * v0 = (1, +9.8)m/s + */ + + Vector3d p0(0, 0, 0); + Quaterniond q0(1, 0, 0, 0); + Vector3d p1(1, 0, +9.8 / 2); + Quaterniond q1(1, 0, 0, 0); + Vector3d p2(2, 0, 0); + double Dt01(1), Dt12(1); + Vector3d Dp01(0, 0, 0); + Vector3d Dv01(0, 0, 0); + Vector3d Dp12(0, 0, 0); + Vector3d v0, g; + + extract_v0_g(p0, q0, p1, q1, p2, Dt01, Dp01, Dv01, Dt12, Dp12, v0, g); + + ASSERT_MATRIX_APPROX(v0, Vector3d(1, 0, 9.8), 1e-15); + ASSERT_MATRIX_APPROX(g, Vector3d(0, 0, -9.8), 1e-15); +} + int main(int argc, char **argv) { testing::InitGoogleTest(&argc, argv); diff --git a/test/yaml/imu_static_init.yaml b/test/yaml/imu_static_init.yaml index 46393f30b8f24bf645bfb1272764e6c8915e5dc2..cc07835915541bd4243b33370f3cf45c496cda32 100644 --- a/test/yaml/imu_static_init.yaml +++ b/test/yaml/imu_static_init.yaml @@ -10,3 +10,8 @@ keyframe_vote: max_buff_length: 1000000000 # motion deltas dist_traveled: 100000000000 # meters angle_turned: 10000000000 # radians (1 rad approx 57 deg, approx 60 deg) + +bootstrap: + enable: false + method: "G" + averaging_length: 10 \ No newline at end of file