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

Recompute states after bootstrap

parent 9d201787
No related branches found
No related tags found
2 merge requests!54devel->main,!50Resolve "Bootstrap sequence"
...@@ -307,6 +307,13 @@ void ProcessorImu::bootstrap() ...@@ -307,6 +307,13 @@ void ProcessorImu::bootstrap()
// Transform problem to new reference // Transform problem to new reference
getProblem()->transform(transformation); 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 // Activate factors that were inactive during bootstrap
while (not list_fac_inactive_bootstrap_.empty()) while (not list_fac_inactive_bootstrap_.empty())
{ {
...@@ -349,6 +356,31 @@ VectorXd ProcessorImu::bootstrapDelta() const ...@@ -349,6 +356,31 @@ VectorXd ProcessorImu::bootstrapDelta() const
return delta_int; 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 // Register in the FactoryProcessor
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment