From 8ac3f76c8edcb714ebd3aa3b9181070540a8a70f Mon Sep 17 00:00:00 2001 From: joanvallve <jvallve@iri.upc.edu> Date: Tue, 2 Jun 2020 09:49:02 +0200 Subject: [PATCH] better commented --- src/problem/problem.cpp | 34 ++++++++++++++++++++++++---------- 1 file changed, 24 insertions(+), 10 deletions(-) diff --git a/src/problem/problem.cpp b/src/problem/problem.cpp index 1936420a1..6d3c459bb 100644 --- a/src/problem/problem.cpp +++ b/src/problem/problem.cpp @@ -367,30 +367,44 @@ void Problem::getCurrentState(Eigen::VectorXd& _state) const void Problem::getCurrentStateAndStamp(Eigen::VectorXd& _state, TimeStamp& _ts) const { + // Prior if not set yet if (!isPriorSet() and prior_options_->mode != "nothing") { _state = prior_options_->state; _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; - for (auto proc: processor_is_motion_list_){ - proc_is_motion_current_ts.push_back(proc->getCurrentTimeStamp()); + for (auto proc: processor_is_motion_list_) + { + 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) trajectory_ptr_->getLastKeyOrAuxFrame()->getTimeStamp(_ts); trajectory_ptr_->getLastKeyOrAuxFrame()->getState(_state); + return; } - else - _state = zeroState(); + + // zeroState otherwise + _state = zeroState(); } -- GitLab