some improvements in processors

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"

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;
 }

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!");
             }
         }