Added update_problem_features_detected

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)

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
 

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
 

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
 

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
 

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
 

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_;