diff --git a/src/processor/processor_tracker_gnss.cpp b/src/processor/processor_tracker_gnss.cpp index 4902a7b0a80c54e652352bf72294cf0ba43bda03..e48badba30f70600497e3034ff51b09fe4f079f8 100644 --- a/src/processor/processor_tracker_gnss.cpp +++ b/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: "