Tutorials: 3D Mapping (ros1) - norlab-ulaval/Norlab_wiki GitHub Wiki
This tutorial is not up to date with the recent changes in the mapper. Refer to the ROS 2 version of the mapping guide instead: https://norlab-icp-mapper.readthedocs.io/en/latest/UsingInRos/#__tabbed_2_2
This tutorial was created by Simon-Pierre Deschênes (Thanks :tada:). Note that a newer, ROS 2 version of this tutorial exists. You can find it here.
Preparation
Copying the demonstration data
Copy the content from the Google Drive link in a folder named demo/ in your home folder.
(This tutorial is a copy from the PDF that you will find in the downloaded folder)
Installing libnabo
cd ~/
mkdir repos && cd repos/
git clone https://github.com/ethz-asl/libnabo.git
cd libnabo/
mkdir build && cd build/
cmake -DCMAKE_BUILD_TYPE=Release ..
make
sudo make install
N.B.: By default, parallel compilation is disabled. You can add the -j option followed by a number to set the maximum number of parallel jobs after the make command to allow a faster compilation. If you put nothing after -j , there will be no limit to the number of jobs.
Installing libpointmatcher
cd ~/repos/
git clone https://github.com/ethz-asl/libpointmatcher.git
cd libpointmatcher/
mkdir build && cd build/
cmake -DCMAKE_BUILD_TYPE=Release ..
make
sudo make install
Installing norlab_icp_mapper
cd ~/repos/
git clone https://github.com/norlab-ulaval/norlab_icp_mapper.git
cd norlab_icp_mapper/
mkdir build && cd build/
cmake -DCMAKE_BUILD_TYPE=Release ..
make
sudo make install
Fetching libpointmatcher_ros
cd ~/
mkdir -p catkin_ws/src && cd catkin_ws/src/
git clone -b melodic https://github.com/norlab-ulaval/libpointmatcher_ros.git
Fetching norlab_icp_mapper_ros
cd ~/catkin_ws/src/
git clone -b melodic https://github.com/norlab-ulaval/norlab_icp_mapper_ros.git
Fetching mapper_config_template
cd ~/catkin_ws/src/
git clone -b melodic https://github.com/norlab-ulaval/mapper_config_template.git
Compiling the catkin workspace
cd ~/catkin_ws/
catkin_make
Mapping basics
Behind the scenes
Behind the scenes, the mapper node takes a lidar point cloud, registers it in the existing map using libpointmatcher and add it at the right place in the map. All of this is done while keeping track of the pose of the robot. During this process, some filters are applied on point clouds before and after map updates. Furthermore, between map updates (slow), the mapper continuously localizes the robot in the map (fast) to ensure good localization at all times.
Mapper configuration files
The mapper configuration template contains four important files :
- params/icp_config.yaml : This file contains the ICP parameters used by libpointmatcher to do the registration of new point clouds in the map.
- params/input_filters_config.yaml : This file contains the list of filters which are applied to the new point clouds before being processed.
- params/map_post_filters_config.yaml : This file contains the list of filters which are applied to the map after adding the new point cloud.
- launch/mapper.launch : This file contains general mapping parameters (e.g. map density, map update conditions, etc.)
Setting up the mapper for the demo
Open the file ~/catkin_ws/src/mapper_config_template/launch/mapper.launch and change the following parameter: points_in : velodyne_points -> rslidar32_points
Running the demo
roscore
rosparam set use_sim_time true
roslaunch mapper_config_template mapper.launch
rviz -d ~/demo/config.rviz
rosbag play ~/demo/demo.bag --clock --keep-alive
Adjusting mapping parameters
Lowering the map density
Open the file ~/catkin_ws/src/mapper_config_template/launch/mapper.launch
and change
the following parameter:
min_dist_new_point : 0.03 -> 0.1
Changing the map update condition
Open the file ~/catkin_ws/src/mapper_config_template/launch/mapper.launch
and change the following parameters:
map_update_condition
:overlap
->distance
map_update_distance
:0.5
->5
Removing Warthog points
Open the file ~/catkin_ws/src/mapper_config_template/params/input_filters_config.yaml
and add the following lines:
- BoundingBoxDataPointsFilter:
xMin: -1.5
xMax: 0.5
yMin: -1
yMax: 1
zMin: -1
zMax: 0.5
removeInside: 1
Removing dynamic points
Open the file ~/catkin_ws/src/mapper_config_template/params/map_post_filters_config.yaml
and add the following lines:
- SurfaceNormalDataPointsFilter:
knn: 15
- CutAtDescriptorThresholdDataPointsFilter:
descName: probabilityDynamic
useLargerThan: 1
threshold: 0.8
Open the file ~/catkin_ws/src/mapper_config_template/launch/mapper.launch
and change the following parameter:
compute_prob_dynamic
: false
-> true
Saving the map
rosservice call /save_map "map_file_name:
data: '$HOME/demo/demo.vtk'"
Final Result
Further information
A detailed tutorial about the YAML configuration file of libpointmatcher is available here. Finally, a full list of the mapper parameters and their description can be found here.