Commit ca816cd7 authored by Alejandro Lopez Gestoso's avatar Alejandro Lopez Gestoso
Browse files
parents 2ab0e421 a3aedbb3
......@@ -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`
......
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
......
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
......
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
......
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
......
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
......
# 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.
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment