diff --git a/demos/processor_imu_bootstrap.yaml b/demos/processor_imu_bootstrap.yaml
index 813c6023e91b1d0198412b150ec1512c6a451b64..3a6c6c9587ab23aeb66ae95495c8054be46e0c96 100644
--- a/demos/processor_imu_bootstrap.yaml
+++ b/demos/processor_imu_bootstrap.yaml
@@ -12,5 +12,6 @@ keyframe_vote:
bootstrap:
enable: true
- method: "G"
- averaging_length: 0.10 # seconds
\ No newline at end of file
+ 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/src/processor/processor_imu.cpp b/src/processor/processor_imu.cpp
index 9bc043dc6bc6854a83abe66ded087810c6104db6..caf8f76fd07e46835b9fb7656a81d1d25cebaa8b 100644
--- a/src/processor/processor_imu.cpp
+++ b/src/processor/processor_imu.cpp
@@ -249,33 +249,28 @@ Eigen::VectorXd ProcessorImu::correctDelta (const Eigen::VectorXd& delta_preint,
void ProcessorImu::bootstrap()
{
- // TODO bootstrap strategies.
+ // 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");
- bool bootstrap_done = false;
switch (params_motion_Imu_->bootstrap_method)
{
case ParamsProcessorImu::BootstrapMethod::BOOTSTRAP_STATIC:
- {
- // TODO implement static strategy
- break;
- }
- case ParamsProcessorImu::BootstrapMethod::BOOTSTRAP_G: {
- // Implementation of G strategy.
- CaptureBasePtr first_capture = bootstrapOrigin();
- TimeStamp t_current = last_ptr_->getBuffer().back().ts_;
+
+ // Implementation of static strategy
if (t_current - first_capture->getTimeStamp() >= params_motion_Imu_->bootstrap_averaging_length)
{
- // frames:
- // w: world global ( where g = [0,0,-9.806] );
- // l: world local;
- // r: robot;
- // s: sensor (IMU)
-
// get initial IMU frame 's' expressed in local world frame 'l'
- Quaterniond q_l_r(first_capture->getFrame()->getStateVector("O").data());
- Quaterniond q_r_s(first_capture->getSensor()->getStateVector("O").data());
+ 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();
@@ -283,46 +278,97 @@ void ProcessorImu::bootstrap()
// 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_r * q_r_s) * dv / dt); // See eq. (20)
+ 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 local origin
- Quaterniond q_w_l = Quaterniond::FromTwoVectors(g_l, g_w); //
- transformation.at('P') = p_w_l; //
- transformation.at('O') = q_w_l.coeffs(); //
+ 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;
+ }
- bootstrap_done = true;
+ 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: {
+
+ 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:
- break;
- }
-
- if (bootstrap_done)
- {
- // 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();
- }
+ default:
- // Clear bootstrapping flag. This marks the end of the bootstrapping process
- bootstrapping_ = false;
+ // No strategy provided: clear bootstrapping flag and warn
+ WOLF_WARN("Bootstrapping enabled but no viable strategy detected. Disabling bootstrap!");
+ bootstrapping_ = false;
+ break;
}
}
@@ -365,8 +411,9 @@ VectorXd ProcessorImu::bootstrapDelta() const
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())))
+ 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_)
@@ -384,10 +431,11 @@ bool ProcessorImu::recomputeStates() const
}
return true;
}
- else return false;
+ else
+ return false;
}
-} // namespace wolf
+} // namespace wolf
// Register in the FactoryProcessor
#include "core/processor/factory_processor.h"