From 49e0f807a1764cb37ef23f70e33bbde42a23f54b Mon Sep 17 00:00:00 2001
From: Alejandro Lopez Gestoso <alopez@iri.upc.edu>
Date: Tue, 18 May 2021 15:49:54 +0200
Subject: [PATCH] Added update_problem_features_detected
---
cfg/AdcLandmarksSlamSolver.cfg | 1 +
config/amcl_mapping.yaml | 1 +
config/landmarks_calibration.yaml | 1 +
config/landmarks_calibration_amcl.yaml | 1 +
config/localization.yaml | 1 +
config/slam_mapping.yaml | 1 +
src/adc_landmarks_slam_solver_alg_node.cpp | 2 +-
7 files changed, 7 insertions(+), 1 deletion(-)
diff --git a/cfg/AdcLandmarksSlamSolver.cfg b/cfg/AdcLandmarksSlamSolver.cfg
index 0efb2fd..c09c469 100755
--- a/cfg/AdcLandmarksSlamSolver.cfg
+++ b/cfg/AdcLandmarksSlamSolver.cfg
@@ -62,6 +62,7 @@ landmarks.add("landmarks_min_detections", int_t, 0, "Th
ceres.add("update_problem_rate", double_t, 0, "Rate to update the ceres problem", 1.0, 0.00001, 10)
ceres.add("update_problem_distance", double_t, 0, "Distance from last robot state to update the ceres problem", 1.0, 0.1, 100000)
ceres.add("update_problem_angle", double_t, 0, "Angle inc from last robot state to update the ceres problem", 0.2, 0.05, 3.14)
+ceres.add("update_problem_features_detected", int_t, 0, "Update problem when a lot of features are detected", 3, 1, 10)
ceres.add("wait_feature_detected_timeout", double_t, 0, "Timeout on a update problem event to wait for a feature detection", 0.2, 0.01, 0.5)
ceres.add("landmarks_map_file", str_t, 0, "Input txt file path", "landmarks.txt")
ceres.add("problem_frame_window", int_t, 0, "Max number of frames to add", 30, -1, 120)
diff --git a/config/amcl_mapping.yaml b/config/amcl_mapping.yaml
index 22d7182..ca1a017 100644
--- a/config/amcl_mapping.yaml
+++ b/config/amcl_mapping.yaml
@@ -21,6 +21,7 @@ publish_tf_map_odom : False
update_problem_rate: 0.1
update_problem_distance: 0.2
update_problem_angle: 0.1
+update_problem_features_detected: 3
wait_feature_detected_timeout: 0.2
problem_frame_window: -1
diff --git a/config/landmarks_calibration.yaml b/config/landmarks_calibration.yaml
index 7ef2a40..4a67e8f 100644
--- a/config/landmarks_calibration.yaml
+++ b/config/landmarks_calibration.yaml
@@ -21,6 +21,7 @@ publish_tf_map_odom : True
update_problem_rate: 0.1
update_problem_distance: 0.2
update_problem_angle: 0.1
+update_problem_features_detected: 3
wait_feature_detected_timeout: 0.2
problem_frame_window: -1
diff --git a/config/landmarks_calibration_amcl.yaml b/config/landmarks_calibration_amcl.yaml
index eda9c34..0a1f987 100644
--- a/config/landmarks_calibration_amcl.yaml
+++ b/config/landmarks_calibration_amcl.yaml
@@ -21,6 +21,7 @@ publish_tf_map_odom : False
update_problem_rate: 0.1
update_problem_distance: 0.2
update_problem_angle: 0.1
+update_problem_features_detected: 3
wait_feature_detected_timeout: 0.2
problem_frame_window: -1
diff --git a/config/localization.yaml b/config/localization.yaml
index e3b571e..6aeb33a 100644
--- a/config/localization.yaml
+++ b/config/localization.yaml
@@ -21,6 +21,7 @@ publish_tf_map_odom : False
update_problem_rate: 0.1
update_problem_distance: 0.2
update_problem_angle: 0.1
+update_problem_features_detected: 3
wait_feature_detected_timeout: 0.2
problem_frame_window: 60
diff --git a/config/slam_mapping.yaml b/config/slam_mapping.yaml
index 99d51c4..2d3e6bf 100644
--- a/config/slam_mapping.yaml
+++ b/config/slam_mapping.yaml
@@ -21,6 +21,7 @@ publish_tf_map_odom : True
update_problem_rate: 0.1
update_problem_distance: 0.2
update_problem_angle: 0.1
+update_problem_features_detected: 3
wait_feature_detected_timeout: 0.2
problem_frame_window: -1
diff --git a/src/adc_landmarks_slam_solver_alg_node.cpp b/src/adc_landmarks_slam_solver_alg_node.cpp
index de16a2f..a5aeb96 100644
--- a/src/adc_landmarks_slam_solver_alg_node.cpp
+++ b/src/adc_landmarks_slam_solver_alg_node.cpp
@@ -218,7 +218,7 @@ void AdcLandmarksSlamSolverAlgNode::mainNodeThread(void)
inc_ang = std::fabs(this->tf_odom_base_(2) - this->last_update_odom_base_(2));
if (((this->features_stamp_ - this->last_update_t_) > ros::Duration(1/this->config_.update_problem_rate)) || (dist.norm() > this->config_.update_problem_distance) ||
- (inc_ang > this->config_.update_problem_angle) || this->new_estimated_pose_)
+ (inc_ang > this->config_.update_problem_angle) || this->new_estimated_pose_ || this->features_.size() >= this->config_.update_problem_features_detected)
{
if (!this->waiting_for_feature_)
this->update_event_t_ = this->features_stamp_;
--
GitLab