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

Update both Prb::getState()

parent 86510a97
No related branches found
No related tags found
1 merge request!380Resolve "Problem::getState/getTimeStamp considering not initialized processors"
Pipeline #5672 failed
......@@ -446,39 +446,40 @@ TimeStamp Problem::getTimeStamp ( ) const
VectorComposite Problem::getState(const StateStructure& _structure) const
{
StateStructure structure = (_structure == "" ? getFrameStructure() : _structure);
VectorComposite state;
if (not processor_is_motion_list_.empty() ) // Compose from different processor motion
// compose the states of all processor motions into one only state
for (const auto& prc : processor_is_motion_list_)
{
// compose the states of all processor motions into one only state
for (const auto& prc : processor_is_motion_list_)
const auto& prc_state = prc->getState();
for (const auto& pair_key_vec : prc_state)
{
const auto& prc_state = prc->getState();
for (const auto& pair_key_vec : prc_state)
{
if (state.count(pair_key_vec.first) == 0) // only add those keys that do not exist yet
state.insert(pair_key_vec);
}
if (state.count(pair_key_vec.first) == 0) // only add those keys that do not exist yet
state.insert(pair_key_vec);
}
}
// check for empty blocks and fill them with zeros
for (const auto& ckey : frame_structure_)
// check for empty blocks and fill them with the last KF, or with zeros in the worst case
VectorComposite state_last;
const auto& last_kf_or_aux = trajectory_ptr_->getLastKeyOrAuxFrame();
if (last_kf_or_aux) state_last = last_kf_or_aux->getState(structure);
for (const auto& ckey : structure)
{
const auto& key = string(1,ckey);
if (state.count(key) == 0)
{
const auto& key = string(1,ckey);
if (state.count(key) == 0)
auto state_last_it = state_last.find(key);
if (state_last_it != state_last.end())
state.emplace(key, state_last_it->second);
else
state.emplace(key, stateZero(key).at(key));
}
}
else // Use last estimated frame's state
{
auto last_kf_or_aux = trajectory_ptr_->getLastKeyOrAuxFrame();
if (last_kf_or_aux)
state = last_kf_or_aux->getState(structure);
else
state = stateZero(structure);
}
return state;
}
......@@ -488,42 +489,37 @@ VectorComposite Problem::getState (const TimeStamp& _ts, const StateStructure& _
VectorComposite state;
if (not processor_is_motion_list_.empty() )
for (const auto& prc : processor_is_motion_list_)
{
for (const auto& prc : processor_is_motion_list_)
{
const auto& prc_state = prc->getState(_ts);
const auto& prc_state = prc->getState(_ts);
// transfer processor vector blocks to problem state
for (const auto& pair_key_vec : prc_state)
{
const auto& key = pair_key_vec.first;
if (state.count(key) == 0) // Only write once. This gives priority to processors upfront in the list
state.insert(pair_key_vec);
}
}
// check for empty blocks and fill them with zeros
for (const auto& ckey : frame_structure_)
// transfer processor vector blocks to problem state
for (const auto& pair_key_vec : prc_state)
{
const auto& key = string(1,ckey);
if (state.count(key) == 0)
state.emplace(key, stateZero(key).at(key));
}
const auto& key = pair_key_vec.first;
if (state.count(key) == 0) // Only write once. This gives priority to processors upfront in the list
state.insert(pair_key_vec);
}
}
else // Use last estimated frame's state
// check for empty blocks and fill them with the closest KF to ts, or with zeros in the worst case
VectorComposite state_last;
const auto& last_kf_or_aux = trajectory_ptr_->closestKeyOrAuxFrameToTimeStamp(_ts);
if (last_kf_or_aux) state_last = last_kf_or_aux->getState(structure);
for (const auto& ckey : structure)
{
const auto& last_kf_or_aux = trajectory_ptr_->closestKeyOrAuxFrameToTimeStamp(_ts);
if (last_kf_or_aux)
{
state = last_kf_or_aux->getState(structure);
}
else
const auto& key = string(1,ckey);
if (state.count(key) == 0)
{
state = stateZero(structure);
auto state_last_it = state_last.find(key);
if (state_last_it != state_last.end())
state.emplace(key, state_last_it->second);
else
state.emplace(key, stateZero(key).at(key));
}
}
......
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