Skip to content
Snippets Groups Projects
Commit a8c9d3c7 authored by Joan Vallvé Navarro's avatar Joan Vallvé Navarro
Browse files

optionally init enu-map

parent d14cf727
No related branches found
No related tags found
2 merge requests!28release after RAL,!27After 2nd RAL submission
......@@ -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" +
......
......@@ -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);
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment