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

Organize and document bootstrap code

parent b8dd308b
No related branches found
No related tags found
2 merge requests!54devel->main,!50Resolve "Bootstrap sequence"
Hide whitespace changes
Inline Side-by-side
demos/processor_imu_bootstrap.yaml
+ 3
− 2
View file @ 1d3666c7
...@@ -12,5 +12,6 @@ keyframe_vote: ...@@ -12,5 +12,6 @@ keyframe_vote:
bootstrap: bootstrap:
enable: true enable: true
method: "G" method: "G" # methods: "G", "STATIC" or "V0_G"
averaging_length: 0.10 # seconds averaging_length: 0.10 # seconds
\ No newline at end of file keyframe_provider_processor_name: "processor_other_name" # Not yet implemented
\ No newline at end of file
src/processor/processor_imu.cpp
+ 103
− 55
View file @ 1d3666c7
...@@ -249,33 +249,28 @@ Eigen::VectorXd ProcessorImu::correctDelta (const Eigen::VectorXd& delta_preint, ...@@ -249,33 +249,28 @@ Eigen::VectorXd ProcessorImu::correctDelta (const Eigen::VectorXd& delta_preint,
void ProcessorImu::bootstrap() void ProcessorImu::bootstrap()
{ {
// TODO bootstrap strategies. // TODO bootstrap strategies.
// See Sola-22 "Imu bootstrap strategies" https://www.overleaf.com/project/629e276e7f68b0c2bfa469ac // 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"); VectorComposite transformation("PO");
bool bootstrap_done = false;
switch (params_motion_Imu_->bootstrap_method) switch (params_motion_Imu_->bootstrap_method)
{ {
case ParamsProcessorImu::BootstrapMethod::BOOTSTRAP_STATIC: case ParamsProcessorImu::BootstrapMethod::BOOTSTRAP_STATIC:
{
// TODO implement static strategy // Implementation of static strategy
break;
}
case ParamsProcessorImu::BootstrapMethod::BOOTSTRAP_G: {
// Implementation of G strategy.
CaptureBasePtr first_capture = bootstrapOrigin();
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:
// 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' // get initial IMU frame 's' expressed in local world frame 'l'
Quaterniond q_l_r(first_capture->getFrame()->getStateVector("O").data()); Map<const Quaterniond> q_l_r(first_capture->getFrame()->getStateVector("O").data());
Quaterniond q_r_s(first_capture->getSensor()->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 // Compute total integrated delta during bootstrap period
VectorXd delta_int = bootstrapDelta(); VectorXd delta_int = bootstrapDelta();
...@@ -283,46 +278,97 @@ void ProcessorImu::bootstrap() ...@@ -283,46 +278,97 @@ void ProcessorImu::bootstrap()
// compute local g and transformation to global g // compute local g and transformation to global g
double dt = t_current - first_capture->getTimeStamp(); // double dt = t_current - first_capture->getTimeStamp(); //
const auto& dv = delta_int.segment(7, 3); // 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& g_w = gravity(); //
const auto& p_w_l = Vector3d::Zero(); // will pivot around the local origin const auto& p_w_l = Vector3d::Zero(); // will pivot around the origin
Quaterniond q_w_l = Quaterniond::FromTwoVectors(g_l, g_w); // Quaterniond q_w_l = Quaterniond::FromTwoVectors(g_l, g_w); //
transformation.at('P') = p_w_l; // transformation.at('P') = p_w_l; //
transformation.at('O') = q_w_l.coeffs(); // 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; break;
}
case ParamsProcessorImu::BootstrapMethod::BOOTSTRAP_V0_G: { case ParamsProcessorImu::BootstrapMethod::BOOTSTRAP_V0_G:
// TODO implement v0-g strategy // TODO implement v0-g strategy
WOLF_WARN("Bootstrapping strategy BOOTSTRAP_V0_G not yet implemented. Disabling bootstrap!");
bootstrapping_ = false;
break; 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 default:
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 // No strategy provided: clear bootstrapping flag and warn
bootstrapping_ = false; WOLF_WARN("Bootstrapping enabled but no viable strategy detected. Disabling bootstrap!");
bootstrapping_ = false;
break;
} }
} }
...@@ -365,8 +411,9 @@ VectorXd ProcessorImu::bootstrapDelta() const ...@@ -365,8 +411,9 @@ VectorXd ProcessorImu::bootstrapDelta() const
bool ProcessorImu::recomputeStates() const bool ProcessorImu::recomputeStates() const
{ {
const auto& mp = getProblem()->getMotionProviderMap(); const auto& mp = getProblem()->getMotionProviderMap();
if (not mp.empty() and mp.begin()->second == std::static_pointer_cast<const MotionProvider>( if (not mp.empty() and
std::static_pointer_cast<const ProcessorMotion>(shared_from_this()))) mp.begin()->second == std::static_pointer_cast<const MotionProvider>(
std::static_pointer_cast<const ProcessorMotion>(shared_from_this())))
{ {
WOLF_DEBUG("Recomputing IMU keyframe states..."); WOLF_DEBUG("Recomputing IMU keyframe states...");
for (const auto& fac : list_fac_inactive_bootstrap_) for (const auto& fac : list_fac_inactive_bootstrap_)
...@@ -384,10 +431,11 @@ bool ProcessorImu::recomputeStates() const ...@@ -384,10 +431,11 @@ bool ProcessorImu::recomputeStates() const
} }
return true; return true;
} }
else return false; else
return false;
} }
} // namespace wolf } // namespace wolf
// Register in the FactoryProcessor // Register in the FactoryProcessor
#include "core/processor/factory_processor.h" #include "core/processor/factory_processor.h"
......
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