diff --git a/cfg/AdcLandmarksSlamSolver.cfg b/cfg/AdcLandmarksSlamSolver.cfg
index 0efb2fdfab4d178764fa420cb6e817e5a3ae11c7..c09c4695e671aa78ff71fc7cbae03ba39c5b2f45 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 22d7182e52ec28d89c278f2f920e99b831cfd3bb..ca1a01791c99037849b5b817ef80fcc210a950ba 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 7ef2a4015e04471b0fc44affa62e9f96d50a45c4..4a67e8f5508e837155d68ec56bf6b422384cedce 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 eda9c34bc8fc75c63403c4b47acef28013d6abd4..0a1f98751e2ed0681e49a22458ac7fe736ba458c 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 e3b571eb5f1adda0d3a632720febb023a0471e81..6aeb33a56d4a847457171c9295b42a57ab8495f5 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 99d51c40a6f731b2ac45d1864200f133ba09d718..2d3e6bf201b596ab8ae584c90bdf5d4e2adbe257 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 de16a2fb54c6f3afc3a88560898e8b20c39f861d..a5aeb962263b614a0c98124d4a44a395bf5f57e3 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_;