added distance_traveled param and corresponding voting for KF condition

include/gnss/processor/processor_gnss_fix.h

@@ -18,18 +18,21 @@ WOLF_STRUCT_PTR_TYPEDEFS(ProcessorParamsGnssFix);
 struct ProcessorParamsGnssFix : public ProcessorParamsBase
 {
     Scalar time_th;
+    Scalar dist_traveled;
     Scalar enu_map_init_dist_min;
     ProcessorParamsGnssFix() = default;
     ProcessorParamsGnssFix(std::string _unique_name, const paramsServer& _server):
         ProcessorParamsBase(_unique_name, _server)
     {
         time_th                 = _server.getParam<Scalar>(_unique_name + "/time_th");
+        dist_traveled           = _server.getParam<Scalar>(_unique_name + "/dist_traveled");
         enu_map_init_dist_min   = _server.getParam<Scalar>(_unique_name + "/enu_map_init_dist_min");
     }
     std::string print()
     {
         return "\n" + ProcessorParamsBase::print()
             + "time_th: " + std::to_string(time_th) + "\n";
+            + "dist_traveled: " + std::to_string(dist_traveled) + "\n";
             + "enu_map_init_dist_min: " + std::to_string(enu_map_init_dist_min) + "\n";
     }
 };

include/gnss/processor/processor_gnss_single_diff.h

@@ -18,18 +18,21 @@ WOLF_STRUCT_PTR_TYPEDEFS(ProcessorParamsGnssSingleDiff);
 struct ProcessorParamsGnssSingleDiff : public ProcessorParamsBase
 {
     Scalar time_th;
+    Scalar dist_traveled;
     Scalar enu_map_init_dist_min;
     ProcessorParamsGnssSingleDiff() = default;
     ProcessorParamsGnssSingleDiff(std::string _unique_name, const paramsServer& _server):
         ProcessorParamsBase(_unique_name, _server)
     {
         time_th                 = _server.getParam<Scalar>(_unique_name + "/time_th");
+        dist_traveled           = _server.getParam<Scalar>(_unique_name + "/dist_traveled");
         enu_map_init_dist_min   = _server.getParam<Scalar>(_unique_name + "/enu_map_init_dist_min");
     }
     std::string print()
     {
         return "\n" + ProcessorParamsBase::print()
             + "time_th: " + std::to_string(time_th) + "\n";
+            + "dist_traveled: " + std::to_string(dist_traveled) + "\n";
             + "enu_map_init_dist_min: " + std::to_string(enu_map_init_dist_min) + "\n";
     }
 };

src/processor/processor_gnss_fix.cpp

@@ -145,6 +145,10 @@ bool ProcessorGnssFix::voteForKeyFrame()
     if (!last_KF_ || (incoming_capture_->getTimeStamp() - last_KF_->getTimeStamp()) > params_gnss_->time_th)
         return true;
 
+    // Distance criterion
+    if ((incoming_capture_->getFrame()->getP()->getState() - last_KF_->getP()->getState()).norm() > params_gnss_->dist_traveled)
+        return true;
+
     // TODO: more alternatives?
 
     // otherwise

src/processor/processor_gnss_single_diff.cpp

@@ -101,6 +101,12 @@ bool ProcessorGnssSingleDiff::voteForKeyFrame()
     if (last_KF_==nullptr || (last_KF_->getTimeStamp() - incoming_capture_->getTimeStamp()) > params_gnss_->time_th)
         return true;
 
+    // Distance criterion
+    std::cout << "params_gnss_->dist_traveled" << params_gnss_->dist_traveled << std::endl;
+    std::cout << "incoming_capture_->getData().norm()" << incoming_capture_->getData().norm() << std::endl;
+    if (incoming_capture_->getData().norm() > params_gnss_->dist_traveled)
+        return true;
+
     // TODO: more alternatives?
 
     // otherwise