all INFO -> DEBUG

src/processor/processor_tracker_gnss.cpp

@@ -28,7 +28,9 @@ void ProcessorTrackerGnss::preProcess()
 
     GnssUtils::SnapshotPtr inc_snapshot = std::static_pointer_cast<CaptureGnss>(incoming_ptr_)->getSnapshot();
 
+#ifdef _WOLF_DEBUG
     int n_initial = inc_snapshot->getObservations()->size();
+#endif
 
     // compute satellites positions
     if (!inc_snapshot ->satellitesComputed())
@@ -63,7 +65,7 @@ void ProcessorTrackerGnss::preProcess()
     // Set ECEF-ENU
     if (!sensor_gnss_->isEnuDefined() and sensor_gnss_->isEnuModeAuto() and fix_incoming_.success)
     {
-        WOLF_INFO("setting ECEF-ENU: ", fix_incoming_.pos.transpose());
+        WOLF_DEBUG("setting ECEF-ENU: ", fix_incoming_.pos.transpose());
         sensor_gnss_->setEcefEnu(fix_incoming_.pos, true);
     }
     // Fix ENU-MAP
@@ -77,7 +79,7 @@ void ProcessorTrackerGnss::preProcess()
 
     WOLF_DEBUG("TS: ", incoming_ptr_->getTimeStamp(), " - Fix solution (ECEF): ", fix_incoming_.pos.transpose(), " - Fix solution (GEO): ", fix_incoming_.lat_lon.transpose());
 
-    //WOLF_INFO("preprocess: RTKLIB excluded observations: ", fix_incoming_.discarded_sats.size());
+    //WOLF_DEBUG("preprocess: RTKLIB excluded observations: ", fix_incoming_.discarded_sats.size());
     // filter observations (available ephemeris, constellations and elevation&SNR)
     inc_snapshot->filterObservations(fix_incoming_.discarded_sats, // discarded sats
                                      fix_incoming_.sat_azel,
@@ -85,7 +87,7 @@ void ProcessorTrackerGnss::preProcess()
                                      false, // check carrier phase
                                      params_tracker_gnss_->gnss_opt);
 
-    //WOLF_INFO("preprocess: filtered observations: ", inc_snapshot->getObservations()->size());
+    //WOLF_DEBUG("preprocess: filtered observations: ", inc_snapshot->getObservations()->size());
 
     // compute corrected Ranges
     inc_snapshot->computeRanges(fix_incoming_.sat_azel,
@@ -103,7 +105,7 @@ void ProcessorTrackerGnss::preProcess()
         untracked_incoming_features_[feat->satNumber()] = feat;
     }
 
-    WOLF_INFO("ProcessorTrackerGnss::preProcess()",
+    WOLF_DEBUG("ProcessorTrackerGnss::preProcess()",
               "\n\tinitial observations: ", n_initial,
               "\n\tRTKLIB discarded: ", fix_incoming_.discarded_sats.size(),
               "\n\tgnssutils discarded: ", n_initial - untracked_incoming_features_.size() - fix_incoming_.discarded_sats.size(),
@@ -118,7 +120,7 @@ unsigned int ProcessorTrackerGnss::trackFeatures(const FeatureBasePtrList& _feat
     if (_features_in.empty())
         return 0;
 
-    //WOLF_INFO("tracking " , _features_in.size() , " features...");
+    //WOLF_DEBUG("tracking " , _features_in.size() , " features...");
 
     assert(_capture == incoming_ptr_);
 
@@ -141,14 +143,14 @@ unsigned int ProcessorTrackerGnss::trackFeatures(const FeatureBasePtrList& _feat
         }
     }
 
-    WOLF_INFO("ProcessorTrackerGnss::trackFeatures: tracked " , _features_out.size(), " (of ", _features_in.size(), ")");
+    WOLF_DEBUG("ProcessorTrackerGnss::trackFeatures: tracked " , _features_out.size(), " (of ", _features_in.size(), ")");
 
     return _features_out.size();
 }
 
 bool ProcessorTrackerGnss::voteForKeyFrame() const
 {
-    //WOLF_INFO("ProcessorTrackerGnss::voteForKeyFrame");
+    //WOLF_DEBUG("ProcessorTrackerGnss::voteForKeyFrame");
 
     // too old origin
     if (origin_ptr_== nullptr or (last_ptr_->getTimeStamp() - origin_ptr_->getTimeStamp()) > params_tracker_gnss_->max_time_span )
@@ -165,7 +167,7 @@ bool ProcessorTrackerGnss::voteForKeyFrame() const
     if (known_features_incoming_.size() < params_tracker_feature_->min_features_for_keyframe
         and not untracked_last_features_.empty() )
     {
-        WOLF_INFO( "Vote for KF because of too less known_features_incoming and not empty untracked in last" );
+        WOLF_DEBUG( "Vote for KF because of too less known_features_incoming and not empty untracked in last" );
         return true;
     }
 
@@ -195,7 +197,7 @@ unsigned int ProcessorTrackerGnss::detectNewFeatures(const int& _max_new_feature
         _features_out.push_back(feat_pair.second);
         WOLF_DEBUG("feature " , feat_pair.second->id() , " detected!" );
     }
-    WOLF_INFO(_features_out.size() , " new features detected!");
+    WOLF_DEBUG(_features_out.size() , " new features detected!");
 
     return _features_out.size();
 }
@@ -253,7 +255,7 @@ void ProcessorTrackerGnss::establishFactors()
         // FACTOR per pair of KF (FactorGnssTdcp3d)
         if (params_tracker_gnss_->gnss_opt.tdcp.batch)
         {
-            WOLF_INFO("TDCP BATCH frame ", last_ptr_->getFrame()->id());
+            WOLF_DEBUG("TDCP BATCH frame ", last_ptr_->getFrame()->id());
 
             for (auto KF_rit = getProblem()->getTrajectory()->getFrameList().rbegin();
                  KF_rit != getProblem()->getTrajectory()->getFrameList().rend();
@@ -261,7 +263,7 @@ void ProcessorTrackerGnss::establishFactors()
             {
                 FrameBasePtr KF = (*KF_rit);
 
-                WOLF_INFO("TDCP BATCH ref frame ", KF->id());
+                WOLF_DEBUG("TDCP BATCH ref frame ", KF->id());
 
                 // discard non-key frames, last-last pair and frames without CaptureGnss
                 if (not KF->isKey() or
@@ -275,7 +277,7 @@ void ProcessorTrackerGnss::establishFactors()
 
                 // dt
                 double dt = last_ptr_->getTimeStamp() - ref_cap_gnss->getTimeStamp();
-                WOLF_INFO("TDCP BATCH dt = ", dt);
+                WOLF_DEBUG("TDCP BATCH dt = ", dt);
 
                 // discard strange cases
                 if (dt <= 0)
@@ -291,7 +293,7 @@ void ProcessorTrackerGnss::establishFactors()
 
                 // get common sats from tracking
                 auto matches = track_matrix_.matches(ref_cap_gnss, last_cap_gnss);
-                WOLF_INFO("TDCP BATCH matches ", matches.size());
+                WOLF_DEBUG("TDCP BATCH matches ", matches.size());
                 std::set<int> common_sats;
                 for (auto match : matches)
                 {
@@ -301,7 +303,7 @@ void ProcessorTrackerGnss::establishFactors()
                            std::static_pointer_cast<FeatureGnssSatellite>(match.second.second)->satNumber());
                     common_sats.insert(std::static_pointer_cast<FeatureGnssSatellite>(match.second.first)->satNumber());
                 }
-                WOLF_INFO("TDCP BATCH common_sats: ", common_sats.size());
+                WOLF_DEBUG("TDCP BATCH common_sats: ", common_sats.size());
                 for (auto sat : common_sats)
                     std::cout << sat << " ";
                 std::cout << std::endl;
@@ -309,7 +311,7 @@ void ProcessorTrackerGnss::establishFactors()
                 // DEBUG: FIND COMMON SATELLITES OBSERVATIONS
                 std::set<int> common_sats_debug = GnssUtils::Observations::findCommonObservations(*ref_cap_gnss->getSnapshot()->getObservations(),
                                                                                                   *last_cap_gnss->getSnapshot()->getObservations());
-                WOLF_INFO("TDCP BATCH common_sats_debug: ", common_sats_debug.size());
+                WOLF_DEBUG("TDCP BATCH common_sats_debug: ", common_sats_debug.size());
                 for (auto sat : common_sats_debug)
                     std::cout << sat << " ";
                 std::cout << std::endl;
@@ -329,8 +331,8 @@ void ProcessorTrackerGnss::establishFactors()
                                     std::set<int>(),
                                     params_tracker_gnss_->tdcp_batch_params))
                 {
-                    WOLF_INFO("TDCP BATCH d = ", d.transpose());
-                    WOLF_INFO("TDCP BATCH cov =\n", cov_d);
+                    WOLF_DEBUG("TDCP BATCH d = ", d.transpose());
+                    WOLF_DEBUG("TDCP BATCH cov =\n", cov_d);
 
                     // EMPLACE FEATURE
                     auto ftr = FeatureBase::emplace<FeatureGnssTdcp>(last_cap_gnss, Eigen::Vector3d(d.head<3>()), Eigen::Matrix3d(cov_d.topLeftCorner<3,3>()));
@@ -339,7 +341,9 @@ void ProcessorTrackerGnss::establishFactors()
                     FactorBase::emplace<FactorGnssTdcp3d>(ftr, ftr, KF, sensor_gnss_, shared_from_this());
                 }
                 else
-                    WOLF_INFO("TDCP BATCH failed");
+                {
+                    WOLF_DEBUG("TDCP BATCH failed");
+                }
             }
         }
         // FACTOR per SATELLITE (FactorGnssTdcp)
@@ -399,7 +403,7 @@ void ProcessorTrackerGnss::establishFactors()
                                                                        params_tracker_gnss_->gnss_opt.tdcp.loss_function);
                     new_factors.push_back(new_fac);
 
-                    // WOLF_INFO( "Factor: track: " , feature_in_last->trackId(),
+                    // WOLF_DEBUG( "Factor: track: " , feature_in_last->trackId(),
                     //             " origin: "       , feature_in_origin->id() ,
                     //             " from last: "    , feature_in_last->id() );
                 }
@@ -444,7 +448,7 @@ void ProcessorTrackerGnss::removeOutliers(FactorBasePtrList fac_list, CaptureBas
 
     FactorBasePtrList remove_fac;
 
-    //WOLF_INFO( "PR ", getName()," rejectOutlier...");
+    //WOLF_DEBUG( "PR ", getName()," rejectOutlier...");
 
     // PseudoRange states
     Eigen::Vector3d x;
@@ -586,8 +590,8 @@ void ProcessorTrackerGnss::removeOutliers(FactorBasePtrList fac_list, CaptureBas
             // RTKLIB FIX error
             //int sat = std::static_pointer_cast<FeatureGnssSatellite>(fac->getFeature())->getSatellite().sat;
             //assert(fix_last_.prange_residuals.count(sat) && "sat not used when computing fix!");
-            //WOLF_INFO("FactorGnssPseudoRange error = ", fac->getMeasurementSquareRootInformationUpper().inverse()*residual);
-            //WOLF_INFO("RTKLIB pntpos error         = ", fix_last_.prange_residuals.at(sat));
+            //WOLF_DEBUG("FactorGnssPseudoRange error = ", fac->getMeasurementSquareRootInformationUpper().inverse()*residual);
+            //WOLF_DEBUG("RTKLIB pntpos error         = ", fix_last_.prange_residuals.at(sat));
 
             // discard if residual too high evaluated at the current estimation
             if (std::abs(residual) > params_tracker_gnss_->outlier_residual_th)
@@ -621,7 +625,7 @@ void ProcessorTrackerGnss::removeOutliers(FactorBasePtrList fac_list, CaptureBas
             // evaluate
             fac_tdcp->evaluate(parameters_tdcp, &residual, nullptr);
 
-            //WOLF_INFO("FactorGnssTdcp with residual = ", residual);
+            //WOLF_DEBUG("FactorGnssTdcp with residual = ", residual);
 
             // discard if residual too high evaluated at the current estimation
             if (std::abs(residual) > params_tracker_gnss_->outlier_residual_th)
@@ -647,7 +651,7 @@ void ProcessorTrackerGnss::removeOutliers(FactorBasePtrList fac_list, CaptureBas
         //assert(false);
         fac->remove();
     }
-    WOLF_INFO("ProcessorTrackerGnss::removeOutliers:",
+    WOLF_DEBUG("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)
         std::cout << "\tTDCP:        "