added fix functionality to processortrackergnss

include/gnss/processor/processor_tracker_gnss.h

@@ -22,7 +22,7 @@ struct ParamsProcessorTrackerGnss : public ParamsProcessorTrackerFeature
     double max_time_span;
     bool remove_outliers, remove_outliers_tdcp, remove_outliers_with_fix;
     double outlier_residual_th;
-    bool init_frames, pseudo_ranges;
+    bool init_frames, pseudo_ranges, fix;
     double enu_map_fix_dist;
     int min_sbas_sats;
 
@@ -36,6 +36,7 @@ struct ParamsProcessorTrackerGnss : public ParamsProcessorTrackerFeature
         init_frames                 = _server.getParam<bool>    (prefix + _unique_name + "/init_frames");
         max_time_span               = _server.getParam<double>  (prefix + _unique_name + "/keyframe_vote/max_time_span");
         enu_map_fix_dist            = _server.getParam<double>  (prefix + _unique_name + "/enu_map_fix_dist");
+        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");
 

src/processor/processor_tracker_gnss.cpp

@@ -2,9 +2,11 @@
 
 #include "gnss/capture/capture_gnss.h"
 #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_pseudo_range.h"
+#include "gnss/factor/factor_gnss_fix_3d.h"
 #include "gnss_utils/utils/rcv_position.h"
 #include "gnss_utils/tdcp.h"
 
@@ -48,8 +50,7 @@ void ProcessorTrackerGnss::preProcess()
     // compute satellites positions
     inc_snapshot ->computeSatellites(copy_opt.sateph);
 
-    /* iono, tropo, eph and sbas options are the less restrictive to avoid RTKLIB to discard satellites
-     * corrections are applied in computeRanges, this fix is only used for:
+    /* this fix is used for:
      *   - set ENU
      *   - compute azimuths and elevations
      *   - Take the eventual discarded sats by RAIM
@@ -321,6 +322,28 @@ void ProcessorTrackerGnss::establishFactors()
 
     FactorBasePtrList new_factors;
 
+    // FIX FACTOR
+    if (params_tracker_gnss_->fix)
+    {
+        GnssUtils::SnapshotPtr last_snapshot = std::static_pointer_cast<CaptureGnss>(last_ptr_)->getSnapshot();
+
+
+        auto fix = GnssUtils::computePos(*last_snapshot->getObservations(),
+                                          *last_snapshot->getNavigation(),
+                                          params_tracker_gnss_->gnss_opt);
+        if (fix.success)
+        {
+            FeatureBasePtr ftr = FeatureBase::emplace<FeatureGnssFix>(last_ptr_, fix);
+
+            FactorBase::emplace<FactorGnssFix3d>(ftr,
+                                                 ftr,
+                                                 sensor_gnss_,
+                                                 shared_from_this(),
+                                                 params_tracker_gnss_->apply_loss_function,
+                                                 FAC_ACTIVE);
+        }
+    }
+
     // PSEUDO RANGE FACTORS (all sats)
     if (params_tracker_gnss_->pseudo_ranges)
         for (auto ftr : last_ptr_->getFeatureList())
@@ -411,17 +434,17 @@ void ProcessorTrackerGnss::establishFactors()
                     common_sats.insert(std::static_pointer_cast<FeatureGnssSatellite>(match.second.first)->satNumber());
                 }
                 WOLF_DEBUG("TDCP BATCH common_sats: ", common_sats.size());
-                for (auto sat : common_sats)
-                    std::cout << sat << " ";
-                std::cout << std::endl;
+                //for (auto sat : common_sats)
+                //    std::cout << sat << " ";
+                //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());
-                for (auto sat : common_sats_debug)
-                    std::cout << sat << " ";
-                std::cout << std::endl;
+                //for (auto sat : common_sats_debug)
+                //    std::cout << sat << " ";
+                //std::cout << std::endl;
 
                 // reference ECEF position
                 Eigen::Vector3d x_r = sensor_gnss_->computeFrameAntennaPosEcef(KF);
@@ -440,14 +463,13 @@ void ProcessorTrackerGnss::establishFactors()
 
                     // 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>(cap_gnss, tdcp_output.d, tdcp_output.cov_d);
 
                     // EMPLACE FACTOR
                     FactorBase::emplace<FactorGnssTdcp3d>(ftr, ftr, KF, sensor_gnss_, shared_from_this());
                 }
                 else
                 {
-                    WOLF_WARN("TDCP BATCH failed with msg: ", tdcp_output.msg);
+                    WOLF_DEBUG("TDCP BATCH failed with msg: ", tdcp_output.msg);
                 }
             }
         }