Added flag to prevent when seeing a lot of features that the problem always updates

include/adc_landmarks_slam_solver_alg_node.h

@@ -111,6 +111,8 @@ class AdcLandmarksSlamSolverAlgNode : public algorithm_base::IriBaseAlgorithm<Ad
     Eigen::Vector3d estimated_sigma_; ///< Estimated main sigmas.
     bool reset_estimated_pose_; ///< Boolena to set new_estimated_pose to false.
 
+    bool last_frame_min_detections_; ///< Flag to know that last frame was because a lot of features seen.
+
     // [publisher attributes]
     ros::Publisher frame_data_publisher_;
     visualization_msgs::MarkerArray frame_data_MarkerArray_msg_;

src/adc_landmarks_slam_solver_alg_node.cpp

@@ -150,6 +150,8 @@ AdcLandmarksSlamSolverAlgNode::AdcLandmarksSlamSolverAlgNode(void) :
   this->features_stamp_ = ros::Time::now();
   this->tf_map_odom_ << 0.0, 0.0, 0.0;
 
+  this->last_frame_min_detections_ = false;
+
   this->robot_pose_msg_.header.frame_id = this->config_.global_frame;
   this->robot_pose_msg_.pose.pose.position.z = 0.0;
   for (unsigned int i = 0; i < 36; i++)
@@ -307,8 +309,9 @@ 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_ || this->features_.size() >= this->config_.update_problem_features_detected)
+         (inc_ang > this->config_.update_problem_angle) || this->new_estimated_pose_ || (!this->last_frame_min_detections_ && (this->features_.size() >= this->config_.update_problem_features_detected)))
       {
+        this->last_frame_min_detections_ = !this->last_frame_min_detections_;
         if (!this->waiting_for_feature_)
           this->update_event_t_ = this->features_stamp_;
         if (this->features_.size() == 0 && ((this->features_stamp_ - this->update_event_t_) < ros::Duration(this->config_.wait_feature_detected_timeout)))
@@ -475,7 +478,7 @@ void AdcLandmarksSlamSolverAlgNode::mainNodeThread(void)
       ROS_ERROR("AdcLandmarksSlamSolverAlgNode::mainNodeThread -> tf_map_odom not initialized");
       this->last_update_t_ = t;
     }
-}
+  }
 
   // [fill msg structures]
   // Initialize the topic message structure