From 412234b53502f4989442d74fe2cdd002b83fba1a Mon Sep 17 00:00:00 2001
From: joanvallve <jvallve@iri.upc.edu>
Date: Sat, 3 Jul 2021 00:00:42 +0200
Subject: [PATCH] some improvements in processors
---
.../gnss/processor/processor_tracker_gnss.h | 9 ++-
src/processor/processor_gnss_tdcp.cpp | 4 +-
src/processor/processor_tracker_gnss.cpp | 59 +++++++++++++------
3 files changed, 49 insertions(+), 23 deletions(-)
diff --git a/include/gnss/processor/processor_tracker_gnss.h b/include/gnss/processor/processor_tracker_gnss.h
index c821bffac..9984f8014 100644
--- a/include/gnss/processor/processor_tracker_gnss.h
+++ b/include/gnss/processor/processor_tracker_gnss.h
@@ -26,7 +26,7 @@ struct ParamsProcessorTrackerGnss : public ParamsProcessorTrackerFeature
double enu_map_fix_dist;
int min_sbas_sats;
bool tdcp_enabled;
- bool tdcp_all_against_all;
+ std::string tdcp_structure;
ParamsProcessorTrackerGnss() = default;
ParamsProcessorTrackerGnss(std::string _unique_name, const ParamsServer& _server):
@@ -41,7 +41,6 @@ struct ParamsProcessorTrackerGnss : public ParamsProcessorTrackerFeature
fix = _server.getParam<bool> (prefix + _unique_name + "/fix");
pseudo_ranges = _server.getParam<bool> (prefix + _unique_name + "/pseudo_ranges");
min_sbas_sats = _server.getParam<int> (prefix + _unique_name + "/gnss/min_sbas_sats");
- tdcp_all_against_all = _server.getParam<bool> (prefix + _unique_name + "/gnss/tdcp/all_against_all");
// GNSS OPTIONS (see rtklib.h)
gnss_opt.sateph = _server.getParam<int> (prefix + _unique_name + "/gnss/sateph"); // satellite ephemeris option: EPHOPT_BRDC(0):broadcast ephemeris, EPHOPT_PREC(1): precise ephemeris, EPHOPT_SBAS(2): broadcast + SBAS, EPHOPT_SSRAPC(3): broadcast + SSR_APC, EPHOPT_SSRCOM(4): broadcast + SSR_COM, EPHOPT_LEX(5): QZSS LEX ephemeris, EPHOPT_SBAS2(6):broadcast + SBAS(sats with SBAS corr and sats with BRDC eph), EPHOPT_SBAS3(7):broadcast + SBAS(EPHOPT_SBAS if possible, otherwise EPHOPT_SBAS2), EPHOPT_SBAS4(8):broadcast + SBAS(EPHOPT_SBAS if possible, otherwise EPHOPT_BRDC)
@@ -65,6 +64,7 @@ struct ParamsProcessorTrackerGnss : public ParamsProcessorTrackerFeature
tdcp_enabled = _server.getParam<bool>(prefix + _unique_name + "/gnss/tdcp/enabled");
if (tdcp_enabled)
{
+ tdcp_structure = _server.getParam<std::string>(prefix + _unique_name + "/gnss/tdcp/structure");
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");
@@ -84,6 +84,9 @@ struct ParamsProcessorTrackerGnss : public ParamsProcessorTrackerFeature
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");
}
+
+ if (tdcp_structure != "all-all" and tdcp_structure != "consecutive" and tdcp_structure != "first-all")
+ throw std::runtime_error("unknown value for '/gnss/tdcp/structure', should be 'all-all', 'consecutive' or 'first-all'");
}
// COMPUTE FIX OPTIONS (RAIM)
@@ -129,7 +132,7 @@ struct ParamsProcessorTrackerGnss : public ParamsProcessorTrackerFeature
+ "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(tdcp_enabled) + "\n"
- + "gnss/tdcp/all_agains_all: " + std::to_string(tdcp_all_against_all) + "\n"
+ + "gnss/tdcp/structure: " + tdcp_structure + "\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"
diff --git a/src/processor/processor_gnss_tdcp.cpp b/src/processor/processor_gnss_tdcp.cpp
index 3f2c15cfa..80cc5c99d 100644
--- a/src/processor/processor_gnss_tdcp.cpp
+++ b/src/processor/processor_gnss_tdcp.cpp
@@ -190,10 +190,10 @@ FactorBasePtr ProcessorGnssTdcp::emplaceFactor(FeatureBasePtr _ftr, FrameBasePtr
//WOLF_DEBUG("creating the factor...");
// 2D
if (getProblem()->getDim() == 2)
- return FactorBase::emplace<FactorGnssTdcp2d>(_ftr, _ftr, _frm_ref, sensor_gnss_, shared_from_this());
+ return FactorBase::emplace<FactorGnssTdcp2d>(_ftr, _ftr, _frm_ref, sensor_gnss_, shared_from_this(), params_->apply_loss_function);
// 3D
else
- return FactorBase::emplace<FactorGnssTdcp3d>(_ftr, _ftr, _frm_ref, sensor_gnss_, shared_from_this());
+ return FactorBase::emplace<FactorGnssTdcp3d>(_ftr, _ftr, _frm_ref, sensor_gnss_, shared_from_this(), params_->apply_loss_function);
return nullptr;
}
diff --git a/src/processor/processor_tracker_gnss.cpp b/src/processor/processor_tracker_gnss.cpp
index ca533fc15..e3dd9e6ef 100644
--- a/src/processor/processor_tracker_gnss.cpp
+++ b/src/processor/processor_tracker_gnss.cpp
@@ -4,7 +4,7 @@
#include "gnss/feature/feature_gnss_tdcp.h"
#include "gnss/feature/feature_gnss_fix.h"
#include "gnss/factor/factor_gnss_tdcp.h"
-#include "gnss/factor/factor_gnss_tdcp_3d.h"
+#include "gnss/factor/factor_gnss_tdcp_batch.h"
#include "gnss/factor/factor_gnss_pseudo_range.h"
#include "gnss/factor/factor_gnss_fix_3d.h"
#include "gnss_utils/utils/rcv_position.h"
@@ -387,6 +387,7 @@ void ProcessorTrackerGnss::establishFactors()
if (params_tracker_gnss_->tdcp_params.batch)
{
WOLF_DEBUG("TDCP BATCH frame ", last_ptr_->getFrame()->id());
+ FactorBasePtr last_fac_ptr = nullptr;
for (auto KF_rit = getProblem()->getTrajectory()->rbegin();
KF_rit != getProblem()->getTrajectory()->rend();
@@ -439,9 +440,9 @@ void ProcessorTrackerGnss::establishFactors()
//std::cout << std::endl;
// 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_DEBUG("TDCP BATCH common_sats_debug: ", common_sats_debug.size());
+ //std::set<int> common_sats_debug = GnssUtils::Observations::findCommonObservations(*ref_cap_gnss->getSnapshot()->getObservations(),
+ // *last_cap_gnss->getSnapshot()->getObservations());
+ //WOLF_DEBUG("TDCP BATCH common_sats_debug: ", common_sats_debug.size());
//for (auto sat : common_sats_debug)
// std::cout << sat << " ";
//std::cout << std::endl;
@@ -462,25 +463,39 @@ void ProcessorTrackerGnss::establishFactors()
WOLF_DEBUG("TDCP BATCH cov =\n", tdcp_output.cov_d);
// EMPLACE FEATURE
+ auto factor_status = FAC_ACTIVE;
+ if (params_tracker_gnss_->tdcp_structure == "first-all")
+ factor_status = FAC_INACTIVE;
+
auto ftr = FeatureBase::emplace<FeatureGnssTdcp>(last_cap_gnss,
- Eigen::Vector3d(tdcp_output.d.head<3>()),
- Eigen::Matrix3d(tdcp_output.cov_d.topLeftCorner<3,3>()));
+ tdcp_output.d,
+ tdcp_output.cov_d);
// EMPLACE FACTOR
- FactorBase::emplace<FactorGnssTdcp3d>(ftr,
- ftr,
- KF,
- sensor_gnss_,
- shared_from_this());
+ last_fac_ptr = FactorBase::emplace<FactorGnssTdcpBatch>(ftr,
+ ftr,
+ ref_cap_gnss,
+ sensor_gnss_,
+ shared_from_this(),
+ params_->apply_loss_function,
+ factor_status);
+
+ WOLF_DEBUG("TDCP BATCH factor ", last_fac_ptr->id() , " emplaced as ", (last_fac_ptr->getStatus() == FAC_ACTIVE ? "ACTIVE" : "INACTIVE"));
+
+ // only first (with previous frame) factor in "consecutive" structure
+ if (params_tracker_gnss_->tdcp_structure == "consecutive")
+ break;
}
else
{
WOLF_DEBUG("TDCP BATCH failed with msg: ", tdcp_output.msg);
}
-
- // just one factor
- if (not params_tracker_gnss_->tdcp_all_against_all)
- break;
+ }
+ // only last (with first frame) factor in "first-all" structure
+ if (params_tracker_gnss_->tdcp_structure == "first-all" and last_fac_ptr)
+ {
+ last_fac_ptr->setStatus(FAC_ACTIVE);
+ WOLF_DEBUG("TDCP BATCH factor ", last_fac_ptr->id() , " changed to ", (last_fac_ptr->getStatus() == FAC_ACTIVE ? "ACTIVE" : "INACTIVE"));
}
}
// FACTOR per SATELLITE (FactorGnssTdcp)
@@ -530,6 +545,9 @@ 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
+ auto factor_status = FAC_ACTIVE;
+ if (params_tracker_gnss_->tdcp_structure == "first-all")
+ factor_status = FAC_INACTIVE;
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),
@@ -537,17 +555,22 @@ void ProcessorTrackerGnss::establishFactors()
ftr_k,
sensor_gnss_,
shared_from_this(),
- params_tracker_gnss_->tdcp_params.loss_function);
+ params_tracker_gnss_->tdcp_params.loss_function,
+ factor_status);
new_factors.push_back(new_fac);
// WOLF_DEBUG( "Factor: track: " , feature_in_last->trackId(),
// " origin: " , feature_in_origin->id() ,
// " from last: " , feature_in_last->id() );
- // just one factor
- if (not params_tracker_gnss_->tdcp_all_against_all)
+ // only first (with previous frame) factor in "consecutive" structure
+ if (params_tracker_gnss_->tdcp_structure == "consecutive")
break;
}
+ // only last (with first frame) factor in "first-all" structure
+ if (params_tracker_gnss_->tdcp_structure == "first-all")
+ new_factors.back()->setStatus(FAC_ACTIVE);
+
WOLF_DEBUG("All TDCP factors emplaced!");
}
}
--
GitLab