Auto Complete Graph (ACG)

Using emergency maps to help robots save you in emergencies

Description

The goal of this program is described in this blog post and can be summarized by this sentence:

a method to integrate an emergency map into a robot map, so that the robot can plan its way toward places it has not yet explored.

Result example

Paper

The auto-complete graph uses a circular strategy to integrate an emergency map and a robot build map in a global representation. The robot build a map of the environment using NDT mapping, and in parallel do localization in the emergency map using Monte-Carlo Localization. Corners are extracted in both the robot map and the emergency map. Using the information from the localization, a graph-SLAM is created where observation of the emergency map corner are determined using the localization covariance and the position of the emergency map's corners compared to the position of corners detected in the robot map. The graph is further constrained by having emergency map walls able to stretch or shrink but being hard to rotate. This is done because emergency maps have usually local scaling problems but do have correct topology. The paper for the method has been published through MDPI.

@article{Mielle_2019,
 title={The Auto-Complete Graph: Merging and Mutual Correction of Sensor and Prior Maps for SLAM},
 volume={8},
 ISSN={2218-6581},
 url={http://dx.doi.org/10.3390/robotics8020040},
 DOI={10.3390/robotics8020040},
 number={2},
 journal={Robotics},
 publisher={MDPI AG},
 author={Mielle, Malcolm and Magnusson, Martin and Lilienthal, Achim J.},
 year={2019},
 month={May},
 pages={40}
}

A first version of the auto complete graph method is presented in this article on arxiv and was publish in SSRR2017 where it got the best student paper award. This version integrates the emergency map in the robot map using a specific graph matching strategy.

To run the code

Two launch file are used to run the code: one for the mapping and localization, and another for the graph-SLAM optimization.

Mapping and localization parameters

use_mcl: if true, use MCL in the emergency map.
use_graph_map_registration: if true, use NDT mapping.
zfilter_min:
fraction:
cutoff:
init_var:
scale_gaussian_mcl: scaling factor for the gaussian in MCL localization
cell_neighborhood_size_mcl_in_meters: if mcl is allozed to use cell further away from the actual cell scan, the parameter is the size of the neighbor that MCL is allowed to search.
use_euclidean_mcl: use the euclidean distance as a measure for the cell fitness in MCL.
use_mean_score_mcl: use the mean value of all cell within the neighbor
use_euclidean_for_long_distances: only use the euclidean distance if the cell is further than the distance_euclid parameter.
use_hybrid_strategy_mcl: use an hybrid strategy in MCL where either we find a cell corresponding exactly to the scan and we use it as is, or we consider the neighbor and use the mean of all cell fitness. This parameter forces the use of the euclidean distance.
cov_x_mcl: starting cov along the x axis for the MCL.
cov_y_mcl: starting cov along the y axis for the MCL.
cov_yaw_mcl: starting cov along the yaw axis for the MCL.

ACG parameters

pause_for_testing: if true, the program will stop at every step.
export_iteration_count: if true, export the number of iteration per kernel in the file "/home/malcolm/ros_catkin_ws/lunar_ws/iterations.dat". The file needs to exist.
add_noise_odometry: if true, noise is added to the odometry edges.
not_incremental_optimization: if true, and if add_noise_odometry is true, the graph will use the position of the previous node in the graph_map instead of the previous node in acg when adding new node. This is useful if, when adding noise, the graph is not updated at each turn. Indeed, since the localization is based on the position in the graph map but the position in the ACG has noise, the prior matching will be wrong otherwise.
use_prior: if true, integrate the emergency map.
optimize_prior: if true, the emergency map is optimized to fit the robot map.
use_robot_maps: if true, the mapping given by the robot is used.
use_corner: extract corners from the robot map.
use_corner_orientation: use the corner orientation as a parameter for corner matching.
corner_covariance: use an approximation of the corner covariance given the NDT used to find it.
own_registration: register the submaps.
world_frame: the world frame
sensor_frame: the sensor framev
gaussian_scaling_factor: scaling factor of the MCL covariance.
threshold_score_link_creation: probability above which the corner are considered the same and matched. This should be only slightly above zero.
prior_file: file localization for the emergency map image.
max_deviation_corner_in_prior: minimum angle to extract a corner from the emergency map.
scale: scale of the emergency map.

Run the code example

Run both launch files hannover.launch and acg.launch.

In case the prior map of the mapping system doesn't update according to the updates of the ACG, you have to remap the topic acg_maps of hannover.launch and of acg.launch so that they have the same namespace. They need to communicate for the prior map update of the ACG to be linked to the prior map of the mapping.

In Rviz, one can give an approximation of the position_in_robot_frame of the prior compared to robot map using the Publish point button. Only two links are needed to initialize. First click on corner in the emergency map and then in the equivalent corner in the robot map.

Dependencies

ROS

Topics

The results of the ACG can be fetched on the topic /auto_complete_graph_rviz_small_optimi/acg_maps. The message format is as follow:

Header header #standard header information

ndt_map/NDTVectorMapMsg ndt_maps #All robot submap and associated translations
grid_map_msgs/GridMap prior #Prior map as a gridMap with resolution 0.1 and frame "/world"

Each ndt map is centered where the corresponding robot pose node is situated. The robot pose node can be found be adding all transformations.

Another topic where one can find the results is /auto_complete_graph_rviz_small_optimi/acg_maps_om. The message format is as follow:

Header header #standard header information

grid_map_msgs/GridMap[] ndt_maps_om #All robot submap as grid maps. The layer is name 'ndt'
geometry_msgs/Pose[] robot_poses #transformation between the previous submap and the next.
grid_map_msgs/GridMap prior #Prior map as a gridMap with resolution 0.1 and origin frame "/world"

Each ndt map is centered where the corresponding robot pose node is situated.

The results can be visualized using the [auto-complete-graph visualization]() package.

Details about the method

Corner matching using MCL

The MCL poses can be considered as "where the robot thinks he is in the emergency map when considering the laser scanner input". Hence, here is the strategy used to find association between the emergency map and the robot map:

Every MCL pose is associated with an equivalent robot map pose and a submap.
For every corner in the submap, its coordinate are changed to assume it is seen from the MCL position instead of the robot map pose, i.e. we translate the submap from the robot map pose to the MCL pose.
We score the likeliness that a corner correspond to a emergency map corner by using the MCL covariance:
- We calculate the mahalanobis distance using the MCL covariance: (pose_prior - pose_landmark).dot( mcl_cov_inverse * (pose_prior - pose_landmark));
- We then calculate the probability of the corners being the same: prob = exp(mahalanobis distance) This probability will be only slightly superior to zero if the corner pose "fit in the covariance". Hence we only need to look for a score slightly superior to zero to get a matching ; we use 5%.
If the corner match we create an observation from the robot pose to emergency map corner. This observation's covariance is the equivalent MCL pose covariance.

MalcolmMielle / Auto-Complete-Graph

readme