processor_odom_icp vote if invalid only if last valid

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()