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: "