Merge branch 'master' of...

Merge branch 'master' of ssh://gitlab.iri.upc.edu:2202/mobile_robotics/adc/adc_2021/iri_adc_landmarks_slam_solver

README.md

@@ -4,8 +4,17 @@ This node provides a global localization based on landmarks. On the ADC competit
 
 The following image is a skecth of the input and output of the node.
 
-The following image is a basic representation of the slam problem.
+<img src="doc/images/landmarks_slam_solver_sketch.png" alt="Image: General overview">
 
+The following image is a basic representation of the slam problem. Is a sequence of robot poses with their landmarks detections connected by the odometry.
+
+<img src="doc/images/slam_problem_sketch.png" alt="Image: Slam problem overview">
+
+When searching for new landmarks, there is a landmark time persistance filter that checks that a landmark is seen enough times on the same location before adding it as a mapped landmarks.
+
+To match a detection with a landmark the [Mahalanobis distance](https://en.wikipedia.org/wiki/Mahalanobis_distance) is used. Basically, a match between a detection and a landmark is done when the detection is inside an ellipse centered on the landmark. The ellipse is defined by the sensor noise and the mahalanobis distance parameter.
+
+On [parameters_adjust.md](./doc/parameters_adjust.md) there is an small explanation on how to adjust the node parameters.
 
 # ROS Interface
 ### Topic publishers
@@ -21,10 +30,60 @@ The following image is a basic representation of the slam problem.
   - ~**rear_features** (iri_adc_msgs/feature_array.msg): Incoming detections from the rear camera.
 
 ### Parameters
+#####General
 - ~**rate** (Double; default: 10.0; min: 0.1; max: 1000) The main node thread loop rate in Hz. 
+- ~**global_frame** (String; default: map) Global frame id. 
+- ~**odom_frame** (String; default: odom) Odometry frame id. 
+- ~**base_link_frame** (String; default: base_link) Robot's base link frame id. 
+- ~**tf_timeout** (Double; default: 0.2; min: 0.1; max: 2.0) Timeout to find a transform. 
+- ~**old_feature_timeout** (Double; default: 0.5; min: 0.1; max: 2.0) Timeout to set a features source as old. 
+- ~**amcl_pose_estimated_sigma** (Double; default: 1.0; min: 0.1; max: 10.0) AMCL pose sigma when using AMCL localization.
+- ~**publish_pose_rate** (Double; default:1.0; min: 0.00001; max: 10) Rate to publish the robot pose. 
+- ~**publish_pose_distance** (Double; default: 1.0; min: 0.1; max: 100000) Distance from last robot state to publish the robot pose. 
+- ~**publish_pose_angle** (Double; default: 0.2; min: 0.05; max: 3.14) Angle inc from last robot state to publish the robot pose. 
+- ~**write_output_files** (Boolean; default: False) Boolean to print landmarks info to a txt file. 
+- ~**output_files_folder** (String; default: ./) Output txt file path. 
+- ~**err_msg_rate** (Double; default: 0.5; min: 0.1; max: 1.0) Rate to publish error messages.
+- ~**add_frame_and_update** (Boolean; default: False) Boolean to add current frame and optimize the solution.
+- ~**init_pose_covariance** (Double; default: 0.01; min: 0.00001; max: 0.1) Initial pose covariance. 
+##### Landmark candidates filter
+- ~**landmarks_candidates_filter_en** (Boolean; default: False) Boolean to filter landmarks checking if it's a feature on the following scans before adding it as landmark.  
+- ~**landmarks_filter_orientation_en** (Boolean; default: False) Boolean to add orientation filter to landmarks time persistance filter.  
+- ~**landmark_mahalanobis_dist** (Double; default: 2.0; min: 0.01; max: 20.0) Mahalonibis distance parameter. 
+- ~**landmarks_min_detections** (Integer; default: 3; min: 1; max: 30) The number of detections on a row to trust it as a new landmark.
+- ~**landmarks_orientation_th** (Double; default: 0.4; min: 0.01; max: 0.8) Maximum orientation diff from the last detection on following inputs. 
+##### Ceres problem
+- ~**update_problem_rate** (Double; default: 1.0; min: 0.00001; max: 10.0) Rate to update the ceres problem.
+- ~**update_problem_distance** (Double; default: 1.0; min: 0.1; max: 1000000.0) Distance from last robot state to update the ceres problem.
+- ~**update_problem_angle** (Double; default: 0.2; min: 0.05; max: 3.14) Angle inc from last robot state to update the ceres problem.
+- ~**update_problem_features_detected** (Integer; default: 3; min: 1; max: 10) Update problem when a lot of features are detected.
+- ~**wait_feature_detected_timeout** (Double; default: 0.2; min: 0.01; max: 0.5) Timeout on a update problem event to wait for a feature detection.
+- ~**landmarks_map_file** (String; default: landmarks.txt) Landmarks map txt file path. 
+- ~**problem_frame_window** (Integer; default: 120; min: 50; max: 1000) Max number of frames to add.
+##### Flags
+- ~**publish_visualization** (Boolean; default: False) Boolean to publish visualization markers.
+- ~**load_landmarks** (Boolean; default: False) Boolean to load_landmarks from txt file.
+- ~**landmarks_pos_fixed** (Boolean; default: False) Boolean to fix landmarks positions.
+- ~**search_for_new_landmarks** (Boolean; default: False) Boolean to search for new landmarks.
+- ~**first_robot_state_estimated** (Boolean; default: False) Boolean to add estimated residuals at the first robot position.
+- ~**last_robot_state_estimated** (Boolean; default: False) Boolean to add estimated residuals at the last robot position.
+- ~**all_robot_states_estimated** (Boolean; default: False) Boolean to add estimated residuals at all robot positions.
+- ~**amcl_localization** (Boolean; default: False) Boolean to use amcl localization.
+- ~**calculate_covariance** (Boolean; default: False) Boolean to calculate robot pose covariance.
+- ~**publish_tf_map_odom** (Boolean; default: False) Boolean to publish tf from map to odom.
+##### Sensor noise
+- ~**sensor_sigma_th** (Double; default: 0.035; min: 0.001; max: 1.0) Sensor angular sigma. 
+- ~**sensor_sigma_r** (Double; default: 0.05; min: 0.01; max:1.0) Sensor radial sigma.
+##### Odometry noise
+- ~**odom_fxy** (Double; default: 0.05; min: 0.01; max: 1.0) Odom linear sigma factor. 
+- ~**odom_fth** (Double; default: 0.05; min: 0.01; max: 1.0) Odom angular sigma factor. 
+- ~**odom_fxyth** (Double; default: 0.05; min: 0.01; max: 1.0) Odom xyth sigma factor. 
+- ~**odom_sigma_min** (Double; default: 0.000001; min: 0.0000001; max: 1.0) Odom sigma min value. 
 
 ## Installation
 
+[Download](http://ceres-solver.org/installation.html#getting-the-source-code) the latest stable Ceres release and install it following the [linux installation tutorial](http://ceres-solver.org/installation.html#linux).
+
 Move to the active workspace:
 ```bash
 roscd && cd ../src
@@ -39,6 +98,7 @@ roscd
 cd ..
 rosdep install -i -r --from-paths src
 ```
+
 Compile the workspace:
 ```
 catkin_make
@@ -46,7 +106,28 @@ catkin_make
 
 ## How to use it
 
-- Standalone test
+This node provides a basic launch file named **node.launch** intended to be included on a general launch file with the following arguments:
+- *node_name*: Node's name.
+- *output*: ROS output parameter.
+- *launch_prefix*: Node's launch prefix.
+- *config_file*: Path to the configuration file.
+- *landmarks_map_file*: Landmarks txt map file.
+- *initial_pose_x*: Initial robot x position.
+- *initial_pose_y*: Initial robot y position.
+- *initial_pose_yaw*: Initial robot orientation.
+- *estimated_pose_topic_name*: Estimated pose topic name.
+- *initialpose_topic_name*: Initial pose topic name.
+- *front_features_topic_name*: Incoming front features topic name.
+- *rear_features_topic_name*: Incoming rear features topic name.
+
+It provides the following default configuration files on [config](./config) directory:
+  * **amcl_mapping.yaml**: To map landmarks using AMCL localization.
+  * **slam_mapping.yaml**: To map landmarks without AMCL localization.
+  * **localization.yaml**: To localizate the robot.
+  * **landmarks_calibration.yaml**: To calibrate landmarks positions without AMCL localization.
+  * **landmarks_calibration_amcl.yaml**: To calibrate landmarks positionsn using AMCL localization.
+
+It also provides an example of launch file named **test.launch**. It can be launch with the following command:
 
   `roslaunch iri_adc_landmarks_slam_solver test.launch`
 

config/amcl_mapping.yaml

-rate: 200.0
+rate: 20.0
 global_frame: "map"
 odom_frame: "adc_car/odom"
 base_link_frame: "adc_car/base_link"
-tf_timeout: 0.1
+tf_timeout: 0.2
+old_feature_timeout: 0.5
 amcl_pose_estimated_sigma: 0.25
 publish_pose_rate: 0.001
 publish_pose_distance: 0.1
@@ -19,19 +20,21 @@ amcl_localization: True
 calculate_covariance: False
 publish_tf_map_odom: False
 
-update_problem_rate: 0.1
-update_problem_distance: 0.2
-update_problem_angle: 0.1
+update_problem_rate: 0.001
+update_problem_distance: 0.07
+update_problem_angle: 0.07
 update_problem_features_detected: 3
 wait_feature_detected_timeout: 0.2
 problem_frame_window: -1
 
 landmarks_candidates_filter_en: True
-landmark_mahalanobis_dist: 3.0
+landmark_mahalanobis_dist: 3.5
 landmarks_min_detections: 4
+landmarks_filter_orientation_en: True
+landmarks_orientation_th: 0.4
 
 sensor_sigma_r: 0.2
-sensor_sigma_th: 0.1
+sensor_sigma_th: 0.3
 
 odom_fxy: 0.1
 odom_fth: 0.15

config/landmarks_calibration.yaml

-rate: 200.0
+rate: 20.0
 global_frame: "map"
 odom_frame: "adc_car/odom"
 base_link_frame: "adc_car/base_link"
-tf_timeout: 0.1
+tf_timeout: 0.2
+old_feature_timeout: 0.5
 amcl_pose_estimated_sigma: 0.25
 publish_pose_rate: 0.001
 publish_pose_distance: 0.1
@@ -19,19 +20,21 @@ amcl_localization: False
 calculate_covariance: False
 publish_tf_map_odom: True
 
-update_problem_rate: 0.1
-update_problem_distance: 0.2
-update_problem_angle: 0.1
+update_problem_rate: 0.001
+update_problem_distance: 0.07
+update_problem_angle: 0.07
 update_problem_features_detected: 3
 wait_feature_detected_timeout: 0.2
 problem_frame_window: -1
 
 landmarks_candidates_filter_en: True
-landmark_mahalanobis_dist: 3.0
+landmark_mahalanobis_dist: 3.5
 landmarks_min_detections: 4
+landmarks_filter_orientation_en: True
+landmarks_orientation_th: 0.4
 
 sensor_sigma_r: 0.2
-sensor_sigma_th: 0.1
+sensor_sigma_th: 0.3
 
 odom_fxy: 0.1
 odom_fth: 0.15

config/landmarks_calibration_amcl.yaml

-rate: 200.0
+rate: 20.0
 global_frame: "map"
 odom_frame: "adc_car/odom"
 base_link_frame: "adc_car/base_link"
-tf_timeout: 0.1
+tf_timeout: 0.2
+old_feature_timeout: 0.5
 amcl_pose_estimated_sigma: 0.25
 publish_pose_rate: 0.001
 publish_pose_distance: 0.1
@@ -19,19 +20,21 @@ amcl_localization: True
 calculate_covariance: False
 publish_tf_map_odom: False
 
-update_problem_rate: 0.1
-update_problem_distance: 0.2
-update_problem_angle: 0.1
+update_problem_rate: 0.001
+update_problem_distance: 0.07
+update_problem_angle: 0.07
 update_problem_features_detected: 3
 wait_feature_detected_timeout: 0.2
 problem_frame_window: -1
 
 landmarks_candidates_filter_en: True
-landmark_mahalanobis_dist: 3.0
+landmark_mahalanobis_dist: 3.5
 landmarks_min_detections: 4
+landmarks_filter_orientation_en: True
+landmarks_orientation_th: 0.4
 
 sensor_sigma_r: 0.2
-sensor_sigma_th: 0.1
+sensor_sigma_th: 0.3
 
 odom_fxy: 0.1
 odom_fth: 0.15

config/localization.yaml

-rate: 200.0
+rate: 20.0
 global_frame: "map"
 odom_frame: "adc_car/odom"
 base_link_frame: "adc_car/base_link"
-tf_timeout: 0.1
+tf_timeout: 0.2
+old_feature_timeout: 0.5
 amcl_pose_estimated_sigma: 0.25
 publish_pose_rate: 0.001
 publish_pose_distance: 0.1
@@ -19,19 +20,21 @@ amcl_localization: False
 calculate_covariance: True
 publish_tf_map_odom: False
 
-update_problem_rate: 0.1
-update_problem_distance: 0.2
-update_problem_angle: 0.1
+update_problem_rate: 0.001
+update_problem_distance: 0.07
+update_problem_angle: 0.07
 update_problem_features_detected: 3
 wait_feature_detected_timeout: 0.2
-problem_frame_window: 60
+problem_frame_window: 120
 
 landmarks_candidates_filter_en: True
-landmark_mahalanobis_dist: 3.0
+landmark_mahalanobis_dist: 3.5
 landmarks_min_detections: 4
+landmarks_filter_orientation_en: True
+landmarks_orientation_th: 0.4
 
 sensor_sigma_r: 0.2
-sensor_sigma_th: 0.1
+sensor_sigma_th: 0.3
 
 odom_fxy: 0.1
 odom_fth: 0.15

config/slam_mapping.yaml

-rate: 200.0
+rate: 20.0
 global_frame: "map"
 odom_frame: "adc_car/odom"
 base_link_frame: "adc_car/base_link"
-tf_timeout: 0.1
+tf_timeout: 0.2
+old_feature_timeout: 0.5
 amcl_pose_estimated_sigma: 0.25
 publish_pose_rate: 0.001
 publish_pose_distance: 0.1
@@ -12,26 +13,28 @@ publish_visualization: True
 load_landmarks: False
 landmarks_pos_fixed: False
 search_for_new_landmarks: True
-first_robot_state_fixed: True
-last_robot_state_fixed: False
-all_robot_states_fixed: False
+first_robot_state_estimated: True
+last_robot_state_estimated: False
+all_robot_states_estimated: False
 amcl_localization: False
 calculate_covariance: False
 publish_tf_map_odom: True
 
-update_problem_rate: 0.1
-update_problem_distance: 0.2
-update_problem_angle: 0.1
+update_problem_rate: 0.001
+update_problem_distance: 0.07
+update_problem_angle: 0.07
 update_problem_features_detected: 3
 wait_feature_detected_timeout: 0.2
 problem_frame_window: -1
 
 landmarks_candidates_filter_en: True
-landmark_mahalanobis_dist: 3.0
+landmark_mahalanobis_dist: 3.5
 landmarks_min_detections: 4
+landmarks_filter_orientation_en: True
+landmarks_orientation_th: 0.4
 
 sensor_sigma_r: 0.2
-sensor_sigma_th: 0.1
+sensor_sigma_th: 0.3
 
 odom_fxy: 0.1
 odom_fth: 0.15

doc/parameters_adjust.md

+# Description
+On this document there are some guidelines to adjust some important parameters.
+
+
+# Adjust landmarks matching
+If you realize that some detections are close to a landmark but is not matched with the landmark, you can adjust the following parameters:
+* **sensor_sigma_th** and **sensor_sigma_r**: These parameters define the basic ellipse for matching purposes. Bigger values on these parameters make a bigger ellipse. **WARNING:** Modifiying these parameters also affects to the covariance calculation and optimization.
+* **landmark_mahalanobis_dist**: This parameters amplifies the ellipse.
+
+# Adjust how much you trust on the camera information
+You can modify the following parameters to adjust how much you trust on your ar detection information. **WARNING:** Modifiying these parameters also afects to the covariance calculation and optimization:
+* **sensor_sigma_th** and **sensor_sigma_r**: Bigger values on these parameters mean less trust on the laser information. **WARNING:** Modifiying these parameters also afects to the landmarks matching.
+
+# Adjust how much you trust on the odometry information
+You can modify the following parameters to adjust how much you trust on your odometry information. **WARNING:** Modifiying these parameters also afects to the covariance calculation and optimization:
+* **odom_fxy**, **odom_fth**, **odom_fxyth** and **odom_sigma_min**: Bigger values on these parameters mean less trust on the odometry information. 
+
+# Adjust how much you trust on the landmark localization
+To modify the covariance calculation you can adjust the sensor sigma parameters and the odom noise parameters. Bigger values on these parameters mean less trust on the localization. **WARNING:** Modifying sensor sigmas also afects to the landmarks matching.
+
+# Adjust how much you trust on the amcl localization
+You can modify the following parameters to adjust how much you trust on your amcl localization. **WARNING:** Modifiying these parameters also afects to the covariance calculation and optimization:
+* **amcl_pose_estimated_sigma**: Bigger values on these parameters mean less trust on the amcl localization.