Summary

After getting a pointcloud map with for example LeGo-LOAM in rosbag file, load it to JOSM with real scale for easier lane map drawing.

  1. Convert bag to pcd
  2. Convert pcd to bt
  3. Convert bt to pgm map file
  4. Visualization in JOSM

End results:

pcl map with lane 1 pcl map with lane 2

Prerequisite

1 - Convert bag to pcd

rosrun pcl_ros bag_to_pcd <file_in.bag> <topic> <output_directory> [<target_frame>]

2 - Convert pcd to bt

roslaunch 3D_Slam_tools straightener.launch input_path:="PATH/TO/PCD.pcd" output_path:="PATH/TO/OUTPUT.bt"

Use arrow key to adjust the map distortion if needed, ctrl-c to close and save to .bt file.

3 - Convert bt to pgm map file

roslaunch 3D_Slam_tools octomap_mapping.launch path:="PATH/TO/OUTPUT.bt" z_min:=-10  z_max:=0.5
  • Setting a large z_min allow the map to keep all the ground points which are very useful for creating lanes.
  • Adjust z_max to remove useless point clouds, like tree and building.

Run map_saver to save the map:

rosrun map_server map_saver

Example output:

map.yaml

image: map.pgm
resolution: 0.050000
origin: [-220.099997, -104.099997, -nan]
negate: 0
occupied_thresh: 0.65
free_thresh: 0.196

map.pgm

map from pcl

4 - Visualization in JOSM

Add PicLayer plugin in JOSM:

mkdir -p ~/.josm/plugins
open josm
Edit -> Preferences -> Plugins click Download list, wait for the plugins list, check the PicLayer plugin checkbox, click update plugins
checkout files in ~/.josm/plugins,  restart if needed

Convert pgm to png

convert map.pgm  -fuzz 20% -transparent white output.png

It becomes a rgba image with transparent background, very useful to overlay on satellite image. map_rbga from pcl

Add png in JOSM

josm_ui

Imagery->New picture layer from file... -> choose the previous output.png
Imagery->Mapbox Satellite, this is usually a good satellite image

josm_map

Save calibration, open .cal file, change INITIAL_SCALE value to 100* the resolution in map.yaml ( in map.pgm resolution=0.05 so 0.05*100 = 5.0)

#JOSM PicLayer plugin calibration data
#Wed Apr 28 13:10:02 CST 2021
POSITION_X=1.3358406457062645E7
POSITION_Y=3539617.8704096507
INITIAL_SCALE=5.0  
M00=0.3857612552908848
M11=0.3857612552908848
M10=-0.922598641835333
M02=29.753357212335082
M01=0.922598641835333
M12=79.0004318155288

Reload the png, the png will be at the right scale, follow step 2 and 3 in the previous picture to align (in step 2, you can put the order and opacity of layers that’s why a rgba image comes handy)

Result: josm_map_overlay

0 - Next

How to draw lane with josm, load it in RVIZ with Lanelets and planning with mission planner.

planning_preview