From 8acb05c725ab59f222d3cfc42e9f462d31fb6b54 Mon Sep 17 00:00:00 2001
From: joanvallve <jvallve@iri.upc.edu>
Date: Mon, 21 Jun 2021 19:46:27 +0200
Subject: [PATCH] adapted to new structor of params for tdcp
---
include/gnss/processor/processor_gnss_tdcp.h | 23 +++----
.../gnss/processor/processor_tracker_gnss.h | 61 ++++++++++---------
src/processor/processor_gnss_tdcp.cpp | 2 +-
src/processor/processor_tracker_gnss.cpp | 16 ++---
4 files changed, 52 insertions(+), 50 deletions(-)
diff --git a/include/gnss/processor/processor_gnss_tdcp.h b/include/gnss/processor/processor_gnss_tdcp.h
index 13291adca..dd43403b7 100644
--- a/include/gnss/processor/processor_gnss_tdcp.h
+++ b/include/gnss/processor/processor_gnss_tdcp.h
@@ -23,7 +23,7 @@ WOLF_STRUCT_PTR_TYPEDEFS(ParamsProcessorGnssTdcp);
struct ParamsProcessorGnssTdcp : public ParamsProcessorGnssFix
{
- struct GnssUtils::TdcpBatchParams tdcp;
+ struct GnssUtils::TdcpOptions tdcp;
ParamsProcessorGnssTdcp() = default;
ParamsProcessorGnssTdcp(std::string _unique_name, const ParamsServer& _server):
@@ -33,17 +33,18 @@ struct ParamsProcessorGnssTdcp : public ParamsProcessorGnssFix
tdcp.raim_n = _server.getParam<double>(prefix + _unique_name + "/tdcp/raim_n");
tdcp.max_residual = _server.getParam<double>(prefix + _unique_name + "/tdcp/max_residual");
tdcp.residual_opt = _server.getParam<int> (prefix + _unique_name + "/tdcp/residual_opt");
- tdcp.tdcp.sigma_atm = _server.getParam<double>(prefix + _unique_name + "/tdcp/sigma_atm");
- tdcp.tdcp.sigma_carrier = _server.getParam<double>(prefix + _unique_name + "/tdcp/sigma_carrier");
- tdcp.tdcp.time_window = _server.getParam<double>(prefix + _unique_name + "/tdcp/time_window");
+ tdcp.sigma_atm = _server.getParam<double>(prefix + _unique_name + "/tdcp/sigma_atm");
+ tdcp.sigma_carrier = _server.getParam<double>(prefix + _unique_name + "/tdcp/sigma_carrier");
+ tdcp.time_window = _server.getParam<double>(prefix + _unique_name + "/tdcp/time_window");
// hardcoded params
- tdcp.tdcp.corr_tropo = false;
- tdcp.tdcp.corr_iono = false;
+ compute_pos_opt.carrier_opt.corr_tropo = false;
+ compute_pos_opt.carrier_opt.corr_iono = false;
+ compute_pos_opt.carrier_opt.corr_clock = false;
tdcp.relinearize_jacobian = false;
tdcp.max_iterations = 1;
- tdcp.tdcp.use_old_nav = false;
- tdcp.tdcp.multi_freq = false;
+ tdcp.use_old_nav = false;
+ tdcp.multi_freq = false;
}
std::string print() const override
{
@@ -52,9 +53,9 @@ struct ParamsProcessorGnssTdcp : public ParamsProcessorGnssFix
+ "tdcp/raim_n: " + std::to_string(tdcp.raim_n) + "\n"
+ "tdcp/max_residual: " + std::to_string(tdcp.max_residual) + "\n"
+ "tdcp/residual_opt: " + std::to_string(tdcp.residual_opt) + "\n"
- + "tdcp/sigma_atm: " + std::to_string(tdcp.tdcp.sigma_atm) + "\n"
- + "tdcp/sigma_carrier: " + std::to_string(tdcp.tdcp.sigma_carrier)+ "\n"
- + "tdcp/time_window: " + std::to_string(tdcp.tdcp.time_window) + "\n";
+ + "tdcp/sigma_atm: " + std::to_string(tdcp.sigma_atm) + "\n"
+ + "tdcp/sigma_carrier: " + std::to_string(tdcp.sigma_carrier)+ "\n"
+ + "tdcp/time_window: " + std::to_string(tdcp.time_window) + "\n";
}
};
diff --git a/include/gnss/processor/processor_tracker_gnss.h b/include/gnss/processor/processor_tracker_gnss.h
index 67495633d..c821bffac 100644
--- a/include/gnss/processor/processor_tracker_gnss.h
+++ b/include/gnss/processor/processor_tracker_gnss.h
@@ -18,13 +18,14 @@ struct ParamsProcessorTrackerGnss : public ParamsProcessorTrackerFeature
{
GnssUtils::Options gnss_opt;
GnssUtils::Options fix_opt{GnssUtils::default_options};
- GnssUtils::TdcpBatchParams tdcp_batch_params;
+ GnssUtils::TdcpOptions tdcp_params;
double max_time_span;
bool remove_outliers, remove_outliers_tdcp, remove_outliers_with_fix;
double outlier_residual_th;
bool init_frames, pseudo_ranges, fix;
double enu_map_fix_dist;
int min_sbas_sats;
+ bool tdcp_enabled;
bool tdcp_all_against_all;
ParamsProcessorTrackerGnss() = default;
@@ -61,28 +62,27 @@ struct ParamsProcessorTrackerGnss : public ParamsProcessorTrackerFeature
gnss_opt.LEO = _server.getParam<bool>(prefix + _unique_name + "/gnss/constellations/LEO");
// TDCP
- gnss_opt.tdcp.enabled = _server.getParam<bool>(prefix + _unique_name + "/gnss/tdcp/enabled");
- if (gnss_opt.tdcp.enabled)
+ tdcp_enabled = _server.getParam<bool>(prefix + _unique_name + "/gnss/tdcp/enabled");
+ if (tdcp_enabled)
{
- remove_outliers_tdcp = _server.getParam<bool> (prefix + _unique_name + "/gnss/tdcp/remove_outliers");
- gnss_opt.tdcp.batch = _server.getParam<bool> (prefix + _unique_name + "/gnss/tdcp/batch");
- gnss_opt.tdcp.corr_iono = _server.getParam<bool> (prefix + _unique_name + "/gnss/tdcp/corr_iono");
- gnss_opt.tdcp.corr_tropo = _server.getParam<bool> (prefix + _unique_name + "/gnss/tdcp/corr_tropo");
- gnss_opt.tdcp.loss_function = _server.getParam<bool> (prefix + _unique_name + "/gnss/tdcp/loss_function");
- gnss_opt.tdcp.time_window = _server.getParam<double>(prefix + _unique_name + "/gnss/tdcp/time_window");
- gnss_opt.tdcp.sigma_atm = _server.getParam<double>(prefix + _unique_name + "/gnss/tdcp/sigma_atm");
- gnss_opt.tdcp.sigma_carrier = _server.getParam<double>(prefix + _unique_name + "/gnss/tdcp/sigma_carrier");
- gnss_opt.tdcp.multi_freq = _server.getParam<bool> (prefix + _unique_name + "/gnss/tdcp/multi_freq");
- if (gnss_opt.tdcp.batch)
+ remove_outliers_tdcp = _server.getParam<bool> (prefix + _unique_name + "/gnss/tdcp/remove_outliers");
+ tdcp_params.batch = _server.getParam<bool> (prefix + _unique_name + "/gnss/tdcp/batch");
+ gnss_opt.carrier_opt.corr_iono = _server.getParam<bool> (prefix + _unique_name + "/gnss/tdcp/corr_iono");
+ gnss_opt.carrier_opt.corr_tropo = _server.getParam<bool> (prefix + _unique_name + "/gnss/tdcp/corr_tropo");
+ gnss_opt.carrier_opt.corr_clock = _server.getParam<bool> (prefix + _unique_name + "/gnss/tdcp/corr_clock");
+ tdcp_params.loss_function = _server.getParam<bool> (prefix + _unique_name + "/gnss/tdcp/loss_function");
+ tdcp_params.time_window = _server.getParam<double>(prefix + _unique_name + "/gnss/tdcp/time_window");
+ tdcp_params.sigma_atm = _server.getParam<double>(prefix + _unique_name + "/gnss/tdcp/sigma_atm");
+ tdcp_params.sigma_carrier = _server.getParam<double>(prefix + _unique_name + "/gnss/tdcp/sigma_carrier");
+ tdcp_params.multi_freq = _server.getParam<bool> (prefix + _unique_name + "/gnss/tdcp/multi_freq");
+ if (tdcp_params.batch)
{
- tdcp_batch_params.min_common_sats = _server.getParam<int> (prefix + _unique_name + "/gnss/tdcp/min_common_sats");
- tdcp_batch_params.raim_n = _server.getParam<int> (prefix + _unique_name + "/gnss/tdcp/raim_n");
- tdcp_batch_params.max_residual = _server.getParam<double> (prefix + _unique_name + "/gnss/tdcp/max_residual");
- tdcp_batch_params.relinearize_jacobian = _server.getParam<bool> (prefix + _unique_name + "/gnss/tdcp/relinearize_jacobian");
- tdcp_batch_params.max_iterations = _server.getParam<int> (prefix + _unique_name + "/gnss/tdcp/max_iterations");
- tdcp_batch_params.residual_opt = _server.getParam<int> (prefix + _unique_name + "/gnss/tdcp/residual_opt");
- tdcp_batch_params.tdcp.multi_freq = gnss_opt.tdcp.multi_freq;
- tdcp_batch_params.tdcp = gnss_opt.tdcp;
+ tdcp_params.min_common_sats = _server.getParam<int> (prefix + _unique_name + "/gnss/tdcp/min_common_sats");
+ tdcp_params.raim_n = _server.getParam<int> (prefix + _unique_name + "/gnss/tdcp/raim_n");
+ tdcp_params.max_residual = _server.getParam<double> (prefix + _unique_name + "/gnss/tdcp/max_residual");
+ tdcp_params.relinearize_jacobian = _server.getParam<bool> (prefix + _unique_name + "/gnss/tdcp/relinearize_jacobian");
+ tdcp_params.max_iterations = _server.getParam<int> (prefix + _unique_name + "/gnss/tdcp/max_iterations");
+ tdcp_params.residual_opt = _server.getParam<int> (prefix + _unique_name + "/gnss/tdcp/residual_opt");
}
}
@@ -128,16 +128,17 @@ struct ParamsProcessorTrackerGnss : public ParamsProcessorTrackerFeature
+ "gnss/constellations/CMP: " + std::to_string(gnss_opt.CMP) + "\n"
+ "gnss/constellations/IRN: " + std::to_string(gnss_opt.IRN) + "\n"
+ "gnss/constellations/LEO: " + std::to_string(gnss_opt.LEO) + "\n"
- + "gnss/tdcp/enabled: " + std::to_string(gnss_opt.tdcp.enabled) + "\n"
+ + "gnss/tdcp/enabled: " + std::to_string(tdcp_enabled) + "\n"
+ "gnss/tdcp/all_agains_all: " + std::to_string(tdcp_all_against_all) + "\n"
- + "gnss/tdcp/batch: " + std::to_string(gnss_opt.tdcp.batch) + "\n"
- + "gnss/tdcp/corr_iono: " + std::to_string(gnss_opt.tdcp.corr_iono) + "\n"
- + "gnss/tdcp/corr_tropo: " + std::to_string(gnss_opt.tdcp.corr_tropo) + "\n"
- + "gnss/tdcp/loss_function: " + std::to_string(gnss_opt.tdcp.loss_function) + "\n"
- + "gnss/tdcp/time_window: " + std::to_string(gnss_opt.tdcp.time_window) + "\n"
- + "gnss/tdcp/sigma_atm: " + std::to_string(gnss_opt.tdcp.sigma_atm) + "\n"
- + "gnss/tdcp/sigma_carrier: " + std::to_string(gnss_opt.tdcp.sigma_carrier) + "\n"
- + "gnss/tdcp/multi_freq: " + std::to_string(gnss_opt.tdcp.multi_freq) + "\n";
+ + "gnss/tdcp/batch: " + std::to_string(tdcp_params.batch) + "\n"
+ + "gnss/tdcp/corr_iono: " + std::to_string(gnss_opt.carrier_opt.corr_iono)+ "\n"
+ + "gnss/tdcp/corr_tropo: " + std::to_string(gnss_opt.carrier_opt.corr_tropo)+ "\n"
+ + "gnss/tdcp/corr_clock: " + std::to_string(gnss_opt.carrier_opt.corr_clock)+ "\n"
+ + "gnss/tdcp/loss_function: " + std::to_string(tdcp_params.loss_function) + "\n"
+ + "gnss/tdcp/time_window: " + std::to_string(tdcp_params.time_window) + "\n"
+ + "gnss/tdcp/sigma_atm: " + std::to_string(tdcp_params.sigma_atm) + "\n"
+ + "gnss/tdcp/sigma_carrier: " + std::to_string(tdcp_params.sigma_carrier) + "\n"
+ + "gnss/tdcp/multi_freq: " + std::to_string(tdcp_params.multi_freq) + "\n";
}
};
diff --git a/src/processor/processor_gnss_tdcp.cpp b/src/processor/processor_gnss_tdcp.cpp
index 4e27bb919..3f2c15cfa 100644
--- a/src/processor/processor_gnss_tdcp.cpp
+++ b/src/processor/processor_gnss_tdcp.cpp
@@ -141,7 +141,7 @@ void ProcessorGnssTdcp::processKeyFrame(FrameBasePtr _keyframe, const double& _t
continue;
// stop if KF_ref too old w.r.t. keyframe
- if (_keyframe->getTimeStamp() - KF_ref->getTimeStamp() > params_tdcp_->tdcp.tdcp.time_window)
+ if (_keyframe->getTimeStamp() - KF_ref->getTimeStamp() > params_tdcp_->tdcp.time_window)
break;
// Get feature GNSS fix
diff --git a/src/processor/processor_tracker_gnss.cpp b/src/processor/processor_tracker_gnss.cpp
index ae1673267..ca533fc15 100644
--- a/src/processor/processor_tracker_gnss.cpp
+++ b/src/processor/processor_tracker_gnss.cpp
@@ -381,10 +381,10 @@ void ProcessorTrackerGnss::establishFactors()
}
// TDCP FACTORS (tracked sats)
- if (origin_ptr_ != last_ptr_ and params_tracker_gnss_->gnss_opt.tdcp.enabled)
+ if (origin_ptr_ != last_ptr_ and params_tracker_gnss_->tdcp_enabled)
{
// Displacement factor from batch TDCP (FactorGnssTdcp3d)
- if (params_tracker_gnss_->gnss_opt.tdcp.batch)
+ if (params_tracker_gnss_->tdcp_params.batch)
{
WOLF_DEBUG("TDCP BATCH frame ", last_ptr_->getFrame()->id());
@@ -414,7 +414,7 @@ void ProcessorTrackerGnss::establishFactors()
continue;
// within time window
- if (dt > params_tracker_gnss_->gnss_opt.tdcp.time_window)
+ if (dt > params_tracker_gnss_->tdcp_params.time_window)
break;
// discard removing Frame/capture/feature
@@ -455,7 +455,7 @@ void ProcessorTrackerGnss::establishFactors()
x_r,
common_sats,
Eigen::Vector4d::Zero(),
- params_tracker_gnss_->tdcp_batch_params);
+ params_tracker_gnss_->tdcp_params);
if (tdcp_output.success)
{
WOLF_DEBUG("TDCP BATCH d = ", tdcp_output.d.transpose());
@@ -517,7 +517,7 @@ void ProcessorTrackerGnss::establishFactors()
continue;
// within time window
- if (dt > params_tracker_gnss_->gnss_opt.tdcp.time_window)
+ if (dt > params_tracker_gnss_->tdcp_params.time_window)
break;
// discard removing Frame/capture/feature
@@ -530,14 +530,14 @@ void ProcessorTrackerGnss::establishFactors()
WOLF_DEBUG("previous feature at KF: ", ftr_r->getCapture()->getFrame()->id(), " sat: ", ftr_r->satNumber(), " id: ", ftr_r->id());
// emplace tdcp factor
- double var_tdcp = dt * std::pow(params_tracker_gnss_->gnss_opt.tdcp.sigma_atm,2) + std::pow(params_tracker_gnss_->gnss_opt.tdcp.sigma_carrier,2);
+ double var_tdcp = dt * std::pow(params_tracker_gnss_->tdcp_params.sigma_atm,2) + std::pow(params_tracker_gnss_->tdcp_params.sigma_carrier,2);
auto new_fac = FactorBase::emplace<FactorGnssTdcp>(ftr_k,
sqrt(var_tdcp),
ftr_r,
ftr_k,
sensor_gnss_,
shared_from_this(),
- params_tracker_gnss_->gnss_opt.tdcp.loss_function);
+ params_tracker_gnss_->tdcp_params.loss_function);
new_factors.push_back(new_fac);
// WOLF_DEBUG( "Factor: track: " , feature_in_last->trackId(),
@@ -799,7 +799,7 @@ void ProcessorTrackerGnss::removeOutliers(FactorBasePtrList fac_list, CaptureBas
"\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_->tdcp_enabled and
params_tracker_gnss_->remove_outliers_tdcp)
std::cout << "\tTDCP: "
<< outliers_tdcp_
--
GitLab