From 33afb6df6286708ee369c549e1ffbaff6c5cc09f Mon Sep 17 00:00:00 2001
From: joanvallve <jvallve@iri.upc.edu>
Date: Thu, 16 Dec 2021 17:55:19 +0100
Subject: [PATCH] processor_odom_icp vote if invalid only if last valid
---
src/processor/processor_odom_icp.cpp | 13 +++++++++----
1 file changed, 9 insertions(+), 4 deletions(-)
diff --git a/src/processor/processor_odom_icp.cpp b/src/processor/processor_odom_icp.cpp
index 2b75292da..12838dd0c 100644
--- a/src/processor/processor_odom_icp.cpp
+++ b/src/processor/processor_odom_icp.cpp
@@ -185,16 +185,21 @@ inline bool ProcessorOdomIcp::voteForKeyFrameTime() 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;
-
- if (vote)
+ 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 ||
+ 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_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_.valid != 1, "CSM transform not valid");
}
- return vote;
+ return origin_incoming_invalid and not origin_last_invalid;
}
void ProcessorOdomIcp::advanceDerived()
--
GitLab