diff --git a/src/processor/processor_tracker_gnss.cpp b/src/processor/processor_tracker_gnss.cpp index 6bbe11a7105a10ce544a4572401be211a4556b6c..b4452e99c21e12c5841653876a56804e2d1ae8dc 100644 --- a/src/processor/processor_tracker_gnss.cpp +++ b/src/processor/processor_tracker_gnss.cpp @@ -24,7 +24,7 @@ void ProcessorTrackerGnss::preProcess() GnssUtils::SnapshotPtr inc_snapshot = std::static_pointer_cast<CaptureGnss>(incoming_ptr_)->getSnapshot(); - WOLF_INFO("preprocess: initial observations: ", inc_snapshot->getObservations()->size()); + int n_initial = inc_snapshot->getObservations()->size(); // compute satellites positions if (!inc_snapshot ->satellitesComputed()) @@ -49,7 +49,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_INFO("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, @@ -57,7 +57,7 @@ void ProcessorTrackerGnss::preProcess() false, // check carrier phase params_tracker_gnss_->gnss_opt); - WOLF_INFO("preprocess: filtered observations: ", inc_snapshot->getObservations()->size()); + //WOLF_INFO("preprocess: filtered observations: ", inc_snapshot->getObservations()->size()); // compute corrected Ranges inc_snapshot->computeRanges(fix_incoming_.sat_azel, @@ -75,7 +75,11 @@ void ProcessorTrackerGnss::preProcess() untracked_incoming_features_[feat->satNumber()] = feat; } - WOLF_INFO("preprocess: untracked incoming features: ", untracked_incoming_features_.size()); + WOLF_INFO("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(), + "\n\tdetected incoming features: ", untracked_incoming_features_.size()); } unsigned int ProcessorTrackerGnss::trackFeatures(const FeatureBasePtrList& _features_in, @@ -86,7 +90,7 @@ unsigned int ProcessorTrackerGnss::trackFeatures(const FeatureBasePtrList& _feat if (_features_in.empty()) return 0; - WOLF_INFO("tracking " , _features_in.size() , " features..."); + //WOLF_INFO("tracking " , _features_in.size() , " features..."); assert(_capture == incoming_ptr_); @@ -107,7 +111,7 @@ unsigned int ProcessorTrackerGnss::trackFeatures(const FeatureBasePtrList& _feat } } - WOLF_INFO("Tracked features: " , _features_out.size()); + WOLF_INFO("ProcessorTrackerGnss::trackFeatures: tracked " , _features_out.size(), " (of ", _features_in.size(), ")"); return _features_out.size(); } @@ -119,7 +123,7 @@ bool ProcessorTrackerGnss::voteForKeyFrame() const // too old origin if (origin_ptr_== nullptr or (last_ptr_->getTimeStamp() - origin_ptr_->getTimeStamp()) > params_tracker_gnss_->max_time_span ) { - WOLF_INFO( "Vote for KF because of time span or null origin" ); + WOLF_DEBUG( "Vote for KF because of time span or null origin" ); return true; } @@ -128,7 +132,7 @@ bool ProcessorTrackerGnss::voteForKeyFrame() const if (known_features_incoming_.size() < params_tracker_feature_->min_features_for_keyframe and !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; } @@ -539,17 +543,21 @@ void ProcessorTrackerGnss::removeOutliers(FactorBasePtrList fac_list, CaptureBas fac->remove(); } WOLF_INFO("ProcessorTrackerGnss::removeOutliers:", - "\n\tPseudorange: ", outliers_pseudorange_, "\t( ", (100.0 * outliers_pseudorange_)/(outliers_pseudorange_+inliers_pseudorange_), " %)", - "\n\tTDCP: ", outliers_tdcp_, "\t( ", (100.0 * outliers_tdcp_)/(outliers_tdcp_+inliers_tdcp_), " %)", - "\n\tsats:"); - std::string sat_out_str(""); - for (auto sat_out : sat_outliers_) - { - const int& sat = sat_out.first; - int sys = satsys(sat,NULL); - const unsigned int& outliers = sat_out.second; - std::cout << "\t\tsat " << sat << "(" << sys << "): " << outliers << std::endl; - } + "\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: " + << outliers_tdcp_ + << "\t( " + << (100.0 * outliers_tdcp_)/(outliers_tdcp_+inliers_tdcp_) + << " \%)\n"; + //std::cout << "\tsats:"; + //for (auto sat_out : sat_outliers_) + //{ + // const int& sat = sat_out.first; + // int sys = satsys(sat,NULL); + // const unsigned int& outliers = sat_out.second; + // std::cout << "\t\tsat " << sat << "(" << sys << "): " << outliers << std::endl; + //} } } // namespace wolf