diff --git a/include/gnss/processor/processor_gnss_fix.h b/include/gnss/processor/processor_gnss_fix.h index 39223788b660132dcaa79af840539258a8a53f70..ce27d06339b96c1fbcea291e5efedfdb3ab356d9 100644 --- a/include/gnss/processor/processor_gnss_fix.h +++ b/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" + diff --git a/src/processor/processor_gnss_fix.cpp b/src/processor/processor_gnss_fix.cpp index 036a91d51842ceff2bb0e5cc0a1c2f43dc176caf..13322f533e7a2fa3cd293f8ba1ae0918f7406c43 100644 --- a/src/processor/processor_gnss_fix.cpp +++ b/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);