Merge pull request #23 from bergercookie/graphslam-devel

mrpt_graphslam_2d:Add to ini config files, instructions
mrpt-ros-pkg · Jun 13, 2017 · 81552f0 · 81552f0
2 parents 2feed28 + 9ceff9a
commit 81552f0
Show file tree

Hide file tree

Showing 33 changed files with 2,921 additions and 544 deletions.
diff --git a/README.md b/README.md
@@ -1,11 +1,11 @@
 [![Build Status](https://travis-ci.org/mrpt-ros-pkg/mrpt_slam.svg?branch=master)](https://travis-ci.org/mrpt-ros-pkg/mrpt_slam)
 
-MRPT-based SLAM packages 
+MRPT-based SLAM packages
 -------------------------
 
-ROS wrappers for SLAM algorithms in Mobile Robot Programming Toolkit (MRPT). 
+ROS wrappers for SLAM algorithms in Mobile Robot Programming Toolkit (MRPT).
 Refer to http://wiki.ros.org/mrpt_slam for further documentation.
 
-Branches: 
+Branches:
   * `master`: Intended for use with latest MRPT versions (1.5.x, 2.0.x).
   * `compat-mrpt-1.3`: Stall branch, archived for backwards compatibility with MRPT versions 1.3.x or older.
diff --git a/mrpt_graphslam_2d/.gitignore b/mrpt_graphslam_2d/.gitignore
@@ -1 +1,3 @@
-rosbags/records_with_gt.bag
+rosbags/*
+rviz/*bag*
+rviz/*gazebo*
diff --git a/mrpt_graphslam_2d/README.md b/mrpt_graphslam_2d/README.md
@@ -7,7 +7,7 @@ mrpt\_graphslam\_2d is heavily based on the native MRPT *graphslam-engine*
 application. Furthermore the command-line arguments offered by the latter can
 be provided by the user as parameters in the ROS Parameter Server. See the
 provided roslaunch files for examples of this. Also see the API of
-mrpt\_graphslam\_2d available [here](// TODO - add it.)
+mrpt\_graphslam\_2d available [here](http://docs.ros.org/lunar/api/mrpt_graphslam_2d/html/index.html)
 
 
 ## Additional information:
@@ -19,7 +19,7 @@ mrpt\_graphslam\_2d available [here](// TODO - add it.)
 
 ### Real-Time experiment - short loop
 
-- A sample rosbag is included in the rosbags/demo_short_loop directory. To run
+- A sample rosbag is included in the `rosbags/demo_short_loop` directory. To run
     this just launch the sr_graphslam_demo.launch file:
 
     `roslaunch mrpt_graphslam_2d sr_graphslam_demo.launch start_rviz:=True`
@@ -36,14 +36,15 @@ mrpt\_graphslam\_2d available [here](// TODO - add it.)
     ceiling. The cameras using the ar_sys ROS package are tracking Aruco
     markers that act as the workspace origin (static marker) and as the robot
     executing  SLAM (moving marker) respectively.
-    To run this demo, just download the whole folder place it directly under
-    the rosbags directory of the mrpt_graphslam_2d package and
-    run the sr_graphslam_demo_gt.launch file.
+    To run this demo, simply download the entire folder, place it directly under
+    the `rosbags` directory of the mrpt_graphslam_2d package and run the
+    sr_graphslam_demo_gt.launch file.
 
     `roslaunch mrpt_graphslam_2d sr_graphslam_demo_gt.launch`
 
 - One can also tinker with the aforementioned launchfile to enable/disable the
-    different visualization features.
+    different visualization features, use another decider/optimizer class, or
+    modify the .ini file to change the algorithm behavior.
 
 - Robot movement starts after ~60''. Due to different timestamps in the
     laserscans, odometry topics the algorithm feedback is lagging a bit compared
@@ -61,14 +62,53 @@ mrpt\_graphslam\_2d available [here](// TODO - add it.)
 
 ## Multi-robot algorithm
 
+Multi-robot support is provided via the
+[csl_mr_slam](http://github.com/bergercookie/csl_mr_slam) package suite as well
+as the use of multimaster_fkie packages utilized for communication across the
+various ROS agents.
+
+### Multi-robot real-time experiments - rosbag
+
+A rosbag collected from real-robots can be found in the following link (more
+rosbags to be added in the MRPT-2.0 branch):
+
+- [multi_robot_graphSLAM_short](https://www.dropbox.com/sh/mxnij0jxvubyu2h/AADC8k6p-ZSq2nipGi4CiesFa?dl=0)
+
+To run a demo using one of the corresponding rosbags download that directory,
+place it directly under the `rosbags` directory of the mrpt_graphslam_2d
+package and run the `run_mr_graphslam_demo.sh` script of the `csl_robots_gazebo
+package`. The latter script (as is standard with the nodes of the
+`csl_robots_gazebo` package reads its configuration parameters off the shell
+environment at hand and adjusts its behavior accordingly. As an examaple users
+can use the following command for running a multi-robot demo.
+
+```sh
+# see the script for configuration variables
+rosrun csl_robots_gazebo run_mr_graphslam_demo.sh
+```
+
+**Warning**
+
+- For this to work, you need to have the csl_mr_slam package suite (as well as the
+packages that the latter depends on) in your catkin workspace.
+
+- When executing multi-robot graphSLAM using either Gazebo (via the
+    `csl_robots_gazebo` package or using measurements from the prerecorded
+    rosbags, you have to, a priori, generate the necessary rviz files from the
+    template files found in `$(rospack find mrpt_graphslam_2d)/rviz/templates`.
+    To do that you have to run the `$(rospack find
+    mrpt_graphslam_2d)/nodes/rename_rviz_topics.py` script which changes the
+    necessary topic names based on the running computer's hostname. See
+    documentation of the latter script for more on its usage.
+
+
 ### Multi-robot simulations in Gazebo - [csl_mr_slam](http://github.com/bergercookie/csl_mr_slam)
 
 Multi-robot simulations are supported in the Gazebo Simulator via the
 [csl_hw_setup](https://github.com/bergercookie/csl_mr_slam/tree/master/csl_hw_setup)
-ROS package. An example of running such a simulation is given
-below.
+ROS package. An example of running such a simulation is given below.
 
-![](https://media.giphy.com/media/l0Iy3H3H4eJQFxqlW/giphy.gif)
+![](https://media.giphy.com/media/l0Iydx6Dq3T7GSJVK/giphy.gif)
 
 A complete example of executing multi-robot graphSLAM in the Gazebo simulation
 environment is presented in the following video:

diff --git a/mrpt_graphslam_2d/config/ros_laser_odometry.ini b/mrpt_graphslam_2d/config/ros_laser_odometry.ini
@@ -27,9 +27,6 @@ save_3DScene_fname = output_scene.3DScene
 save_graph = true
 save_graph_fname = output_graph.graph
 
-;ground_truth_file_format = RGBD_TUM // variable for determining how to parse the groundtruth file.
-ground_truth_file_format = NavSimul
-
 ; min node difference for an edge to be considered as a loop closure
 ; Used in both the visualization procedure for updating the Loop closures
 ; counter as well as the optimization procedure since it affects how often we
@@ -40,7 +37,7 @@ class_verbosity = 1
 ########################################################
 [NodeRegistrationDeciderParameters]
 
-registration_max_distance = 0.2 // meters
+registration_max_distance = 0.1 // meters
 registration_max_angle = 20 // degrees
 
 ; number of ICP goodness values to consider for estimating the current ICP threshold
@@ -53,8 +50,7 @@ class_verbosity = 1
 [EdgeRegistrationDeciderParameters]
 
 ; Works on the 2D observation-only dataset
-ICP_goodness_thresh = 0.80 // threshold for accepting the ICP constraint in the graph - needed by CICPCriteriaERD
-use_scan_matching = false
+ICP_goodness_thresh = 0.60 // threshold for accepting the ICP constraint in the graph - needed by CICPCriteriaERD
 
 ; Either check ICP using distance OR node count from current node
 ICP_max_distance =  2 // maximum distance for checking other nodes for ICP constraints
@@ -75,6 +71,19 @@ scale_hessian = 0.2
 tau = 1e-3
 class_verbosity = 1
 
+[MappingParameters]
+
+resolution=0.03
+mapAltitude = 0
+useMapAltitude = 0
+maxDistanceInsertion = 5
+maxOccupancyUpdateCertainty = 0.80
+considerInvalidRangesAsFreeSpace = 1
+decimation = 1
+horizontalTolerance = 0.00088
+wideningBeamsWithDistance = 1
+
+
 ########################################################
 # seems that it doesn't read hex, using cfg_file.read_int
 # hardcode the integer values instead
@@ -126,3 +135,16 @@ ICP_algorithm              = icpLevenbergMarquardt
 # decimation to apply to the point cloud being registered against the map
 # Reduce to "1" to obtain the best accuracy
 corresponding_points_decimation = 10
+
+[MappingParameters]
+
+mapAltitude = 0
+useMapAltitude = 0
+maxDistanceInsertion = 5
+maxOccupancyUpdateCertainty = 0.65
+considerInvalidRangesAsFreeSpace = 0
+decimation = 1
+horizontalTolerance = 0.00088
+wideningBeamsWithDistance = 1
+
+
diff --git a/mrpt_graphslam_2d/config/ros_laser_odometry_LC_MR_real.ini b/mrpt_graphslam_2d/config/ros_laser_odometry_LC_MR_real.ini
@@ -27,7 +27,6 @@ save_3DScene = true
 save_3DScene_fname = output_scene.3DScene
 save_graph = true
 save_graph_fname = output_graph.graph
-ground_truth_file_format = NavSimul // variable for determining how to parse the groundtruth file.
 
 ; Set the verbosity of the output messages. Only messages over the specified
 ; will be printed to the console.
@@ -36,7 +35,7 @@ ground_truth_file_format = NavSimul // variable for determining how to parse the
 ; LVL_INFO  => 1,
 ; LVL_WARN  => 2,
 ; LVL_ERROR => 3
-class_verbosity = 0
+class_verbosity = 1
 
 
 ; min node difference for an edge to be considered as a loop closure
@@ -68,7 +67,7 @@ consec_icp_constraint_factor = 0.91
 lc_icp_constraint_factor = 0.80
 
 
-class_verbosity = 0
+class_verbosity = 1
 
 // Graph Partitioning Parameters
 forceBisectionOnly = false

diff --git a/mrpt_graphslam_2d/config/ros_odometry_2DRangeScans.ini b/mrpt_graphslam_2d/config/ros_odometry_2DRangeScans.ini
@@ -27,7 +27,6 @@ save_3DScene = true
 save_3DScene_fname = output_scene.3DScene
 save_graph = true
 save_graph_fname = output_graph.graph
-ground_truth_file_format = NavSimul // variable for determining how to parse the groundtruth file.
 
 ; Set the verbosity of the output messages. Only messages over the specified
 ; will be printed to the console.
@@ -80,6 +79,19 @@ tau = 1e-3
 
 class_verbosity = 1
 
+[MappingParameters]
+
+resolution=0.03
+mapAltitude = 0
+useMapAltitude = 0
+maxDistanceInsertion = 5
+maxOccupancyUpdateCertainty = 0.80
+considerInvalidRangesAsFreeSpace = 1
+decimation = 1
+horizontalTolerance = 0.00088
+wideningBeamsWithDistance = 1
+
+
 ########################################################
 # seems that it doesn't read hex, using cfg_file.read_int
 # hardcode the integer values instead

diff --git a/mrpt_graphslam_2d/config/ros_odometry_2DRangeScans_LC.ini b/mrpt_graphslam_2d/config/ros_odometry_2DRangeScans_LC.ini
@@ -27,7 +27,6 @@ save_3DScene = true
 save_3DScene_fname = output_scene.3DScene
 save_graph = true
 save_graph_fname = output_graph.graph
-ground_truth_file_format = NavSimul // variable for determining how to parse the groundtruth file.
 
 ; Set the verbosity of the output messages. Only messages over the specified
 ; will be printed to the console.

diff --git a/mrpt_graphslam_2d/config/ros_odometry_2DRangeScans_LC_MR_real.ini b/mrpt_graphslam_2d/config/ros_odometry_2DRangeScans_LC_MR_real.ini
@@ -19,6 +19,7 @@
 ###############################################################
 
 
+########################################################
 [GeneralConfiguration]
 
 output_dir_fname = graphslam_results
@@ -27,7 +28,6 @@ save_3DScene = true
 save_3DScene_fname = output_scene.3DScene
 save_graph = true
 save_graph_fname = output_graph.graph
-ground_truth_file_format = NavSimul // variable for determining how to parse the groundtruth file.
 
 ; Set the verbosity of the output messages. Only messages over the specified
 ; will be printed to the console.
@@ -48,7 +48,7 @@ LC_min_nodeid_diff= 10
 #######################################################
 [NodeRegistrationDeciderParameters]
 
-registration_max_distance = 0.2
+registration_max_distance = 0.1
 ;registration_max_distance = 1 // meters
 registration_max_angle = 15 // degrees
 class_verbosity = 1
@@ -58,15 +58,14 @@ class_verbosity = 1
 
 use_scan_matching = true // strongly suggested that this is set to TRUE
 ;ICP_goodness_thresh = 0.80 // threshold for accepting the ICP constraint in the graph
-prev_nodes_for_ICP = 5
-LC_eigenvalues_ratio_thresh = 2.5
-LC_min_remote_nodes = 10 // how many out "remote" nodes should exist in a partition for the partition to be examined for potential loop closures
+prev_nodes_for_ICP = 10
+LC_eigenvalues_ratio_thresh = 2
+LC_min_remote_nodes = 2 // how many out "remote" nodes should exist in a partition for the partition to be examined for potential loop closures
 LC_check_curr_partition_only = true
 
 consec_icp_constraint_factor = 0.96
 lc_icp_constraint_factor = 0.95
 
-
 class_verbosity = 1
 
 // Graph Partitioning Parameters
@@ -86,10 +85,11 @@ useMapMatching = true
 // Loop Closing Parameters
 
 
+########################################################
 [OptimizerParameters]
 
-optimization_on_second_thread = false
-optimization_distance = 0.4;
+/ optimization_on_second_thread = false
+optimization_distance = 3.0;
 ;optimization_distance = -1 // optimize whole graph every time.
 
 // Levenberg-Marquardt parameters
@@ -101,6 +101,34 @@ tau = 1e-3
 
 class_verbosity = 1
 
+[MappingParameters]
+
+resolution=0.05
+mapAltitude = 0
+useMapAltitude = 0
+maxDistanceInsertion = 5
+maxOccupancyUpdateCertainty = 0.80
+considerInvalidRangesAsFreeSpace = 1
+decimation = 1
+horizontalTolerance = 0.00088
+wideningBeamsWithDistance = 1
+
+########################################################
+[AlignmentParameters]
+
+min_ICP_goodness = 0.60
+maxKLd_for_merge = 0.90
+ransac_minSetSizeRatio = 0.40
+
+########################################################
+[MultiRobotParameters]
+
+nodes_integration_batch_size = 5
+num_last_registered_nodes = 10
+conservative_find_initial_tfs_to_neighbors = false
+inter_group_node_count_thresh = 30
+
+
 ########################################################
 # seems that it doesn't read hex, using cfg_file.read_int
 # hardcode the integer values instead
@@ -131,6 +159,7 @@ visualize_SLAM_metric = true        // extra displayPlots showing the evolution
 visualize_map_partitions = true
 enable_curr_pos_viewport = true
 
+########################################################
 [ICP]
 maxIterations              = 100   // The maximum number of iterations to execute if convergence is not achieved before
 minAbsStep_trans           = 1e-6  // If the correction in all translation coordinates (X,Y,Z) is below this threshold (in meters), iterations are terminated: