TDCP all_against_all only optional

include/gnss/processor/processor_tracker_gnss.h

@@ -25,6 +25,7 @@ struct ParamsProcessorTrackerGnss : public ParamsProcessorTrackerFeature
     bool init_frames, pseudo_ranges, fix;
     double enu_map_fix_dist;
     int min_sbas_sats;
+    bool tdcp_all_against_all;
 
     ParamsProcessorTrackerGnss() = default;
     ParamsProcessorTrackerGnss(std::string _unique_name, const ParamsServer& _server):
@@ -39,6 +40,7 @@ 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)
@@ -127,6 +129,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(gnss_opt.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"

src/processor/processor_tracker_gnss.cpp

@@ -383,7 +383,7 @@ void ProcessorTrackerGnss::establishFactors()
     // TDCP FACTORS (tracked sats)
     if (origin_ptr_ != last_ptr_ and params_tracker_gnss_->gnss_opt.tdcp.enabled)
     {
-        // FACTOR per pair of KF (FactorGnssTdcp3d)
+        // Displacement factor from batch TDCP (FactorGnssTdcp3d)
         if (params_tracker_gnss_->gnss_opt.tdcp.batch)
         {
             WOLF_DEBUG("TDCP BATCH frame ", last_ptr_->getFrame()->id());
@@ -462,15 +462,25 @@ void ProcessorTrackerGnss::establishFactors()
                     WOLF_DEBUG("TDCP BATCH cov =\n", tdcp_output.cov_d);
 
                     // EMPLACE FEATURE
-                    auto ftr = FeatureBase::emplace<FeatureGnssTdcp>(last_cap_gnss, Eigen::Vector3d(tdcp_output.d.head<3>()), Eigen::Matrix3d(tdcp_output.cov_d.topLeftCorner<3,3>()));
+                    auto ftr = FeatureBase::emplace<FeatureGnssTdcp>(last_cap_gnss,
+                                                                     Eigen::Vector3d(tdcp_output.d.head<3>()),
+                                                                     Eigen::Matrix3d(tdcp_output.cov_d.topLeftCorner<3,3>()));
 
                     // EMPLACE FACTOR
-                    FactorBase::emplace<FactorGnssTdcp3d>(ftr, ftr, KF, sensor_gnss_, shared_from_this());
+                    FactorBase::emplace<FactorGnssTdcp3d>(ftr,
+                                                          ftr,
+                                                          KF,
+                                                          sensor_gnss_,
+                                                          shared_from_this());
                 }
                 else
                 {
                     WOLF_DEBUG("TDCP BATCH failed with msg: ", tdcp_output.msg);
                 }
+
+                // just one factor
+                if (not params_tracker_gnss_->tdcp_all_against_all)
+                    break;
             }
         }
         // FACTOR per SATELLITE (FactorGnssTdcp)
@@ -533,6 +543,10 @@ void ProcessorTrackerGnss::establishFactors()
                     // 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)
+                        break;
                 }
                 WOLF_DEBUG("All TDCP factors emplaced!");
             }