optionally init enu-map

include/gnss/processor/processor_gnss_fix.h

@@ -18,7 +18,7 @@ WOLF_STRUCT_PTR_TYPEDEFS(ParamsProcessorGnssFix);
 
 struct ParamsProcessorGnssFix : public ParamsProcessorBase
 {
-    bool fix_from_raw;
+    bool fix_from_raw, init_enu_map;
     GnssUtils::Options compute_pos_opt;
     double max_time_span;
     double dist_traveled;
@@ -28,10 +28,12 @@ struct ParamsProcessorGnssFix : public ParamsProcessorBase
     ParamsProcessorGnssFix(std::string _unique_name, const ParamsServer& _server):
         ParamsProcessorBase(_unique_name, _server)
     {
-        max_time_span           = _server.getParam<double>  (prefix + _unique_name + "/keyframe_vote/max_time_span");
-        dist_traveled           = _server.getParam<double>  (prefix + _unique_name + "/keyframe_vote/dist_traveled");
-        enu_map_init_dist_min   = _server.getParam<double>  (prefix + _unique_name + "/enu_map_init_dist_min");
-        fix_from_raw            = _server.getParam<bool>    (prefix + _unique_name + "/fix_from_raw");
+        max_time_span               = _server.getParam<double>  (prefix + _unique_name + "/keyframe_vote/max_time_span");
+        dist_traveled               = _server.getParam<double>  (prefix + _unique_name + "/keyframe_vote/dist_traveled");
+        init_enu_map                = _server.getParam<bool>    (prefix + _unique_name + "/init_enu_map");
+        if (init_enu_map)
+            enu_map_init_dist_min   = _server.getParam<double>  (prefix + _unique_name + "/enu_map_init_dist_min");
+        fix_from_raw                = _server.getParam<bool>    (prefix + _unique_name + "/fix_from_raw");
 
         // COMPUTE POS PARAMS (only if compute fix from yaw)
         if (fix_from_raw)
@@ -59,11 +61,14 @@ struct ParamsProcessorGnssFix : public ParamsProcessorBase
     std::string print() const
     {
         return "\n"                             + ParamsProcessorBase::print()              + "\n" +
-               "max_time_span: "                 + std::to_string(max_time_span)             + "\n" +
-               "dist_traveled: "                 + std::to_string(dist_traveled)             + "\n" +
-               "fix_from_raw: "                  + std::to_string(fix_from_raw)              + "\n" +
-               "enu_map_init_dist_min: "         + std::to_string(enu_map_init_dist_min)     + "\n" +
-               "keyframe_vote/max_time_span: "   + std::to_string(max_time_span)             + "\n" +
+               "max_time_span: "                + std::to_string(max_time_span)             + "\n" +
+               "dist_traveled: "                + std::to_string(dist_traveled)             + "\n" +
+               "fix_from_raw: "                 + std::to_string(fix_from_raw)              + "\n" +
+               "init_enu_map: "                 + std::to_string(init_enu_map)              + "\n" +
+               (init_enu_map ?
+                       "enu_map_init_dist_min: "+ std::to_string(enu_map_init_dist_min)     + "\n" : "") +
+               "enu_map_init_dist_min: "        + std::to_string(enu_map_init_dist_min)     + "\n" +
+               "keyframe_vote/max_time_span: "  + std::to_string(max_time_span)             + "\n" +
                (fix_from_raw ?
                        "gnss/sateph: "                   + std::to_string(compute_pos_opt.sateph)    + "\n" +
                        "gnss/ionoopt: "                  + std::to_string(compute_pos_opt.ionoopt)   + "\n" +

src/processor/processor_gnss_fix.cpp

@@ -132,7 +132,7 @@ void ProcessorGnssFix::processCapture(CaptureBasePtr _capture)
         first_feature_ = incoming_feature;
     }
     // Initialize ENU-MAP if: ENU defined and ENU-MAP not initialized
-    if (!first_capture_->isRemoving())
+    if (params_gnss_->init_enu_map and !first_capture_->isRemoving())
     {
         assert(first_capture_->getFrame() != nullptr && incoming_capture_->getFrame() != nullptr);
         if ( sensor_gnss_->isEnuDefined() && !sensor_gnss_->isEnuMapInitialized() )
@@ -219,9 +219,11 @@ bool ProcessorGnssFix::voteForKeyFrame() const
     }
 
     // ENU-MAP not initialized and can be initialized
-    if ( sensor_gnss_->isEnuDefined() &&
-            !sensor_gnss_->isEnuMapInitialized() &&
-            (first_feature_->getMeasurement()-incoming_pos_out_.pos).norm() > params_gnss_->enu_map_init_dist_min)
+    if (params_gnss_->init_enu_map and
+        sensor_gnss_->isEnuDefined() and
+        !sensor_gnss_->isEnuMapInitialized() and
+        !first_capture_->isRemoving() and
+        (first_feature_->getMeasurement()-incoming_pos_out_.pos).norm() > params_gnss_->enu_map_init_dist_min)
     {
         WOLF_DEBUG("KF because of enu map not initialized");
         assert(first_capture_ != nullptr);