diff --git a/src/problem/problem.cpp b/src/problem/problem.cpp index b5be6eeacd6b7803951a3f19fb4daa7edbd43cf6..eeb6fe5f67dc5ddc9f81ecee16578a2f93314a49 100644 --- a/src/problem/problem.cpp +++ b/src/problem/problem.cpp @@ -358,16 +358,15 @@ void Problem::getState(const TimeStamp& _ts, Eigen::VectorXd& _state) const { // if _ts is too recent, for some of the processor is motion, they return the state corresponding to their last frame timestamp FrameBasePtr closest_frame = trajectory_ptr_->closestKeyOrAuxFrameToTimeStamp(_ts); - Eigen::VectorXd closest_frame_state = closest_frame->getState(); if (processor_is_motion_list_.empty()){ if (closest_frame != nullptr) - _state = closest_frame_state; + _state = closest_frame->getState(); else _state = zeroState(); } // RETRIEVE FROM PROCESSOR MOTION - // TODO: current implementation really messy, would be much easier with a state being an std::unordered_map + // TODO: current implementation messy, would be much easier with a state being an std::unordered_map else { // Iterate over the problem state structure and get the corresponding state // in the first processor is motion that provides it @@ -383,7 +382,7 @@ void Problem::getState(const TimeStamp& _ts, Eigen::VectorXd& _state) const // not already there if (states_to_concat_map.find(sb_name) == states_to_concat_map.end()){ if (sb_name == 'O'){ - int size_sb = dim_ == 3 ? 4 : 1; // really bad... + int size_sb = dim_ == 3 ? 4 : 1; // really bad: should be more transparent states_to_concat_map[sb_name] = proc_state.segment(idx, size_sb); idx += size_sb; }