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

processor_odom_icp vote if invalid only if last valid

parent 3b505bc2
No related branches found
No related tags found
2 merge requests!30Release after RAL,!29After 2nd RAL submission
...@@ -185,16 +185,21 @@ inline bool ProcessorOdomIcp::voteForKeyFrameTime() const ...@@ -185,16 +185,21 @@ inline bool ProcessorOdomIcp::voteForKeyFrameTime() const
inline bool ProcessorOdomIcp::voteForKeyFrameMatchQuality() const inline bool ProcessorOdomIcp::voteForKeyFrameMatchQuality() const
{ {
bool vote = trf_origin_incoming_.error/trf_origin_incoming_.nvalid > params_odom_icp_->vfk_min_error || trf_origin_incoming_.nvalid < params_odom_icp_->vfk_max_points || trf_origin_incoming_.valid != 1; bool origin_incoming_invalid = trf_origin_incoming_.error/trf_origin_incoming_.nvalid > params_odom_icp_->vfk_min_error ||
trf_origin_incoming_.nvalid < params_odom_icp_->vfk_max_points ||
if (vote) trf_origin_incoming_.valid != 1;
bool origin_last_invalid = trf_origin_last_.error/trf_origin_last_.nvalid > params_odom_icp_->vfk_min_error ||
trf_origin_last_.nvalid < params_odom_icp_->vfk_max_points ||
trf_origin_last_.valid != 1;
if (origin_incoming_invalid and not origin_last_invalid)
{ {
WOLF_DEBUG("ProcessorOdomIcp::voteForKeyFrameMatchQuality: Voting..."); WOLF_DEBUG("ProcessorOdomIcp::voteForKeyFrameMatchQuality: Voting...");
WOLF_DEBUG_COND(trf_origin_incoming_.error / trf_origin_incoming_.nvalid > params_odom_icp_->vfk_min_error, "error greater than min. threshold" ); WOLF_DEBUG_COND(trf_origin_incoming_.error / trf_origin_incoming_.nvalid > params_odom_icp_->vfk_min_error, "error greater than min. threshold" );
WOLF_DEBUG_COND(trf_origin_incoming_.nvalid < params_odom_icp_->vfk_max_points, "nvalid points below max" ); WOLF_DEBUG_COND(trf_origin_incoming_.nvalid < params_odom_icp_->vfk_max_points, "nvalid points below max" );
WOLF_DEBUG_COND(trf_origin_incoming_.valid != 1, "CSM transform not valid"); WOLF_DEBUG_COND(trf_origin_incoming_.valid != 1, "CSM transform not valid");
} }
return vote; return origin_incoming_invalid and not origin_last_invalid;
} }
void ProcessorOdomIcp::advanceDerived() void ProcessorOdomIcp::advanceDerived()
......
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