Skip to content
Snippets Groups Projects
Commit 8ac3f76c authored by Joan Vallvé Navarro's avatar Joan Vallvé Navarro
Browse files

better commented

parent d1fabfae
No related branches found
No related tags found
No related merge requests found
Pipeline #5489 passed
...@@ -367,30 +367,44 @@ void Problem::getCurrentState(Eigen::VectorXd& _state) const ...@@ -367,30 +367,44 @@ void Problem::getCurrentState(Eigen::VectorXd& _state) const
void Problem::getCurrentStateAndStamp(Eigen::VectorXd& _state, TimeStamp& _ts) const void Problem::getCurrentStateAndStamp(Eigen::VectorXd& _state, TimeStamp& _ts) const
{ {
// Prior if not set yet
if (!isPriorSet() and prior_options_->mode != "nothing") if (!isPriorSet() and prior_options_->mode != "nothing")
{ {
_state = prior_options_->state; _state = prior_options_->state;
_ts = 0; _ts = 0;
return;
} }
else if (!processor_is_motion_list_.empty())
// retrieve the minimum of the most recent ts in all processor is motion then call getSate(ts, state)
if (!processor_is_motion_list_.empty())
{ {
// retrieve the minimum of the most recent ts in all processor is motion then call getSate(ts, state)
std::list<TimeStamp> proc_is_motion_current_ts; std::list<TimeStamp> proc_is_motion_current_ts;
for (auto proc: processor_is_motion_list_){ for (auto proc: processor_is_motion_list_)
proc_is_motion_current_ts.push_back(proc->getCurrentTimeStamp()); {
auto ts = proc->getCurrentTimeStamp();
if (ts != 0) // a not initialized PM provides 0 timestamp FIXME: change by isMotion::ready()?
proc_is_motion_current_ts.push_back(ts);
}
if (!proc_is_motion_current_ts.empty())
{
auto min_it = std::min_element(proc_is_motion_current_ts.begin(), proc_is_motion_current_ts.end());
getState(*min_it, _state);
_ts = *min_it;
return;
} }
auto min_it = std::min_element(proc_is_motion_current_ts.begin(), proc_is_motion_current_ts.end());
getState(*min_it, _state);
_ts = *min_it;
} }
else if (trajectory_ptr_->getLastKeyOrAuxFrame() != nullptr)
// Last KF state
if (trajectory_ptr_->getLastKeyOrAuxFrame() != nullptr)
{ {
// kind of redundant with getState(_ts, _state) // kind of redundant with getState(_ts, _state)
trajectory_ptr_->getLastKeyOrAuxFrame()->getTimeStamp(_ts); trajectory_ptr_->getLastKeyOrAuxFrame()->getTimeStamp(_ts);
trajectory_ptr_->getLastKeyOrAuxFrame()->getState(_state); trajectory_ptr_->getLastKeyOrAuxFrame()->getState(_state);
return;
} }
else
_state = zeroState(); // zeroState otherwise
_state = zeroState();
} }
......
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