prints

src/processor/processor_tracker_gnss.cpp

@@ -146,6 +146,8 @@ unsigned int ProcessorTrackerGnss::trackFeatures(const FeatureBasePtrList& _feat
 
     assert(_capture == incoming_ptr_);
 
+    int common_sats = 0;
+
     for (auto feat_in : _features_in)
     {
         auto feat_in_gnss = std::dynamic_pointer_cast<FeatureGnssSatellite>(feat_in);
@@ -155,6 +157,9 @@ unsigned int ProcessorTrackerGnss::trackFeatures(const FeatureBasePtrList& _feat
 
         WOLF_DEBUG("tracking " , feat_in->trackId() , ", sat number ", sat_num);
 
+        if (untracked_incoming_features_.count(sat_num) != 0)
+            common_sats++;
+
         if (untracked_incoming_features_.count(sat_num) != 0 and
             std::abs(untracked_incoming_features_.at(sat_num)->getObservation().L[0]) > 1e-12) // Track only carrier phase valid
         {
@@ -171,7 +176,9 @@ unsigned int ProcessorTrackerGnss::trackFeatures(const FeatureBasePtrList& _feat
             WOLF_DEBUG_COND(untracked_incoming_features_.count(sat_num) == 1, "\tnot tracked, wrong carrier phase value" );
         }
     }
-
+    WOLF_WARN_COND(_features_out.empty() and not _features_in.empty(),
+                   "ProcessorTrackerGnss::trackFeatures: LOST TRACK OF ALL SATELLITES of ", _features_in.size(),
+                   " - ", common_sats, " wrong Carrier Phase info.");
     WOLF_DEBUG("ProcessorTrackerGnss::trackFeatures: tracked " , _features_out.size(), " (of ", _features_in.size(), ")");
 
     return _features_out.size();
@@ -680,9 +687,13 @@ void ProcessorTrackerGnss::removeOutliers(FactorBasePtrList fac_list, CaptureBas
         //assert(false);
         fac->remove();
     }
-    WOLF_INFO("ProcessorTrackerGnss::removeOutliers:",
-               "\n\tPseudorange: ", outliers_pseudorange_, "\t( ", (100.0 * outliers_pseudorange_)/(outliers_pseudorange_+inliers_pseudorange_), " %)");
-    if (params_tracker_gnss_->gnss_opt.tdcp.enabled and params_tracker_gnss_->remove_outliers_tdcp)
+    WOLF_INFO_COND(not remove_fac.empty(),
+                   "ProcessorTrackerGnss::removeOutliers:",
+                   "\n\tPseudorange: ", outliers_pseudorange_,
+                   "\t( ", (100.0 * outliers_pseudorange_)/(outliers_pseudorange_+inliers_pseudorange_), " %)");
+    if (not remove_fac.empty() and
+            params_tracker_gnss_->gnss_opt.tdcp.enabled and
+            params_tracker_gnss_->remove_outliers_tdcp)
         std::cout << "\tTDCP:        "
                   << outliers_tdcp_
                   << "\t( "