⭐️ OGM2PGBM ⭐️
Occupancy Grid Map to Pose Graph-based Map for long-term 2D LiDAR-based localization

Achieve a 78% improvement in real-time localization accuracy by transitioning from classical particle filter methods to robust pose graph-based algorithms using OGM2PGBM. This approach enables you to leverage any existing 2D reference map for enhanced precision and reliability.

ArXiv Paper • Data

//: //:

//:

//: //:

This repo contains the following two applications:

1. OGM2PGBM: generate pose graph-based maps on 2D occupancy grid maps, which can be created from a TLS Point cloud or a BIM/CAD model. This pose graph-based maps can be used for accurate localization in changing and dynamic environments, as demostrated in our paper.

   The following animation shows an overview of the method and compares AMCL and Cartographer; the latter can be applied after leveraging the Ogm2Pgbm package. It is clear that for AMCL is more difficult to keep track of the pose of the robot in comparison with the performance of Cartographer.

   

2. GMCL & CARTO/SLAM_toolbox: conbine the fast global localization feature of GMCL with the more accurate pose tracking performance of Cartographer/SLAM_toolbox

Additionally, it includes the packages amcl, gmcl, cartographer and slam_toolbox, so that they can be used and compared with a bagfile that should be located in the mounted directory ~/workspace easily.

Table of Contents

• Requirements
• OGM2PGBM
  • Principle
  • Running the code
  • I. Refine and Edit the PGM Map
  • II. Use Ogm2Pgbm to Obtain a Rosbag with PointCloud and Poses
    • 2.1 Use Docker to Obtain the Package
    • 2.2 Start the Package
  • III. For ROS1 users
  • IV. For ROS2 users
    • 4.1 Download the Rosbag to the Host Machine
    • 4.2 Convert .bag to .db3
    • 4.3 Play the Rosbag and Generate the Map
    • 4.4 Save the .posegraph Map
• GMCL & CARTO/SLAM_toolbox
• Citation
• Reference projects

Requirements

If you plan to use our docker container, with all the methods installed (Warning: it requires >5 GB of space) you only need to install docker.

If you don't want to use docker and only want to use the Ogm2Pgbm package, you can install the content of this folder and use it as a normal catkin package.

If you still want to install all the different localization methods, you can have a look at the docker file and install the respective dependencies on your local machine.

OGM2PGBM

Principle

The workflow of OGM2PGBM is as follows, see the function new_map_callback(self, grid_map) for details:

1. Subscribe map from the map topic
2. Skeletonize the map and get its voronoi waypoint (see self.skeletonize())
3. Perform a coverage path planning on it (see self.CPP())
   • Extract the farest endpoint pair first as the start and goal point
   • Then dilate it with a 2x2 kernel to bold the centerline
   • Do CPP wavefront algorithm and get the sorted waypoint
4. Do a raytracing on the waypoints one by one, and publish the /laserscan topic (see self.raytracer())

It produces /tf, /clock, /odom, /scan topics with frame robot_map, robot_odom and robot_base_link.

Since /tf is needed, python2.7 is used in this script.

Running the code

This package already provides a Dockerfile, which means you can run it on any Ubuntu version. However, for ROS2 users, the final step of generating .pbstream or .posegraph is different. I will explain this in detail below.

This might also be a way to edit the posegraph map, but it's a bit convoluted.

I. Refine and Edit the PGM Map

Modify the map as needed by adding or removing barriers. The most crucial step is to completely black out the obstacles, ensuring there are no white areas within any obstacles!

Before	After

II. Use Ogm2Pgbm to Obtain a Rosbag with PointCloud and Poses

2.1 Use Docker to Obtain the Package

1. First clone the repository

   git clone https://github.com/MigVega/Ogm2Pgbm.git
   cd Ogm2Pgbm

2. Pull the Docker image

   LihanChen has build a Docker image for easy deployment. You can also choose to build your own Docker image.

   docker pull lihanchen2004/ogm2pgbm:latest

3. Create the container based on docker image

   If you are using a self-built image, please modify the image_name at the beginning of autorun.sh.

   ./autorun.sh

2.2 Start the Package

Copy the .pgm and .yaml files you need to convert to the Ogm2Pgbm/workspace/map/ directory on the host machine.

All following operations should be performed inside the Docker container.
Ensure the current terminal is within the Docker container.

MAP_NAME=OGM_empty

roslaunch ogm2pgbm ogm2pgbm.launch map_file:=/root/workspace/map/$MAP_NAME.yaml record:=true

Wait for 2-3 minutes. After completion, the program will output "Done" in the terminal.

Terminate the program with Ctrl+C. The rosbag will automatically be saved to /root/.ros/ogm2pgbm_sensordata.bag.

III. For ROS1 users

If you want to use posegraph or pbstream file directly in ROS1, please follow the tutorial in this section.

After generating bagfiles, use Cartographer to generate pbstream or SLAM toolbox to generate posegraph maps. With the following command Cartographer will run in offline mode, which will generate pbstream quite fast, but without any visual output in rviz.

roslaunch cartographer_ros ogm2pgbm_my_robot.launch bag_filename:=/root/.ros/ogm2pgbm_sensordata.bag

You can also launch Slam_toolbox. (There will be some error report in the terminal, just ignore them and wait for some seconds.)

roslaunch slam_toolbox ogm2pgbm.launch bag_filename:=/root/.ros/ogm2pgbm_sensordata.bag

The target pbstream file will be generated automatically at /root/.ros/ogm2pgbm_sensordata.bag.pbstream after . For slam_toolbox, you also need to click on the serialization button on the rviz plugin. The target files are also located at /root/.ros.

IV. For ROS2 users

If you want to use posegraph or pbstream file in ROS2, please follow the tutorial in this section.

All following operations should be performed on the host machine.

4.1 Download the Rosbag to the Host Machine

1. Get the CONTAINER ID of ogm2pgbm:

   docker ps

2. Save the rosbag to the host machine:

   Remember to modify CONTAINER_ID! Here, I save the rosbag from the container to the host's Download directory:

   CONTAINER_ID=xxx
   CONTAINER_PATH=/root/.ros/ogm2pgbm_sensordata.bag
   DST_PATH=~/Downloads/
   
   docker cp $CONTAINER_ID:$CONTAINER_PATH $DST_PATH

4.2 Convert .bag to .db3

In ROS1, rosbag files have a .bag suffix, which is a binary format for storing ROS messages. ROS2 has improved and extended the rosbag format, adopting an SQLite-based database format that includes a .db3 database and a .yaml file.

1. Install the rosbags conversion package:

   pip install rosbags

2. Navigate to the directory containing the rosbag and execute the following command to convert ogm2pgbm_sensordata.bag to ogm2pgbm_sensordata.db3:

   rosbags-convert --src ogm2pgbm_sensordata.bag \
   --dst ogm2pgbm_sensordata \
   --src-typestore empty \
   --dst-typestore ros2_humble \
   --exclude-topic /rosout /robot/map /rosout_agg

4.3 Play the Rosbag and Generate the Map

1. Preparation

   You can use the following rviz configuration file for visualization.

   ogm2pgbm.rviz (Click to expand)

   ```yaml Panels: - Class: rviz_common/Displays Help Height: 78 Name: Displays Property Tree Widget: Expanded: - /Global Options1 - /Status1 Splitter Ratio: 0.5 Tree Height: 684 - Class: rviz_common/Selection Name: Selection - Class: rviz_common/Tool Properties Expanded: - /2D Goal Pose1 - /Publish Point1 Name: Tool Properties Splitter Ratio: 0.5886790156364441 - Class: rviz_common/Views Expanded: - /Current View1 Name: Views Splitter Ratio: 0.5 - Class: rviz_common/Time Experimental: false Name: Time SyncMode: 0 SyncSource: LaserScan - Class: slam_toolbox::SlamToolboxPlugin Name: SlamToolboxPlugin Visualization Manager: Class: "" Displays: - Alpha: 0.699999988079071 Class: rviz_default_plugins/Map Color Scheme: map Draw Behind: false Enabled: true Name: Map Topic: Depth: 5 Durability Policy: Volatile Filter size: 10 History Policy: Keep Last Reliability Policy: Reliable Value: /map Update Topic: Depth: 5 Durability Policy: Volatile History Policy: Keep Last Reliability Policy: Reliable Value: /map_updates Use Timestamp: false Value: true - Alpha: 1 Autocompute Intensity Bounds: true Autocompute Value Bounds: Max Value: 10 Min Value: -10 Value: true Axis: Z Channel Name: intensity Class: rviz_default_plugins/LaserScan Color: 255; 255; 255 Color Transformer: Intensity Decay Time: 0 Enabled: true Invert Rainbow: false Max Color: 255; 255; 255 Max Intensity: 4096 Min Color: 0; 0; 0 Min Intensity: 0 Name: LaserScan Position Transformer: XYZ Selectable: true Size (Pixels): 3 Size (m): 0.009999999776482582 Style: Points Topic: Depth: 5 Durability Policy: Volatile Filter size: 10 History Policy: Keep Last Reliability Policy: Reliable Value: /scan Use Fixed Frame: true Use rainbow: true Value: true - Class: rviz_default_plugins/TF Enabled: true Frame Timeout: 15 Frames: All Enabled: true base_link: Value: true lidar_odom: Value: true livox_frame: Value: true odom: Value: true robot_base_link: Value: true robot_map: Value: true robot_odom: Value: true wheel_1: Value: true wheel_2: Value: true wheel_3: Value: true wheel_4: Value: true Marker Scale: 1 Name: TF Show Arrows: true Show Axes: true Show Names: false Tree: robot_map: robot_base_link: {} robot_odom: {} Update Interval: 0 Value: true Enabled: true Global Options: Background Color: 48; 48; 48 Fixed Frame: robot_base_link Frame Rate: 30 Name: root Tools: - Class: rviz_default_plugins/Interact Hide Inactive Objects: true - Class: rviz_default_plugins/MoveCamera - Class: rviz_default_plugins/Select - Class: rviz_default_plugins/FocusCamera - Class: rviz_default_plugins/Measure Line color: 128; 128; 0 - Class: rviz_default_plugins/SetInitialPose Covariance x: 0.25 Covariance y: 0.25 Covariance yaw: 0.06853891909122467 Topic: Depth: 5 Durability Policy: Volatile History Policy: Keep Last Reliability Policy: Reliable Value: /initialpose - Class: rviz_default_plugins/SetGoal Topic: Depth: 5 Durability Policy: Volatile History Policy: Keep Last Reliability Policy: Reliable Value: /goal_pose - Class: rviz_default_plugins/PublishPoint Single click: true Topic: Depth: 5 Durability Policy: Volatile History Policy: Keep Last Reliability Policy: Reliable Value: /clicked_point Transformation: Current: Class: rviz_default_plugins/TF Value: true Views: Current: Class: rviz_default_plugins/Orbit Distance: 25.282127380371094 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 Swap Stereo Eyes: false Value: false Focal Point: X: -1.1839663982391357 Y: 1.0030579566955566 Z: -0.871300458908081 Focal Shape Fixed Size: true Focal Shape Size: 0.05000000074505806 Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 Pitch: 1.1747967004776 Target Frame: Value: Orbit (rviz) Yaw: 3.125408411026001 Saved: ~ Window Geometry: Displays: collapsed: false Height: 1376 Hide Left Dock: false Hide Right Dock: false QMainWindow State: 000000ff00000000fd000000040000000000000217000004c2fc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f00700065007200740069006500730200000438000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d00000337000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb000000220053006c0061006d0054006f006f006c0062006f00780050006c007500670069006e010000037a000001850000018500ffffff000000010000010f000002b0fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d000002b0000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000009ba0000003efc0100000002fb0000000800540069006d00650100000000000009ba000002fb00fffffffb0000000800540069006d006501000000000000045000000000000000000000079d000004c200000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Selection: collapsed: false SlamToolboxPlugin: collapsed: false Time: collapsed: false Tool Properties: collapsed: false Views: collapsed: false Width: 2490 X: 70 Y: 27 ```

2. In the 1st Shell: Start the slam_toolbox to prepare for mapping

   ros2 run slam_toolbox async_slam_toolbox_node --ros-args \
   -p use_sim_time:=True \
   -p odom_frame:=robot_odom \
   -p base_frame:=robot_base_link \
   -p map_frame:=robot_map \
   -p do_loop_closing:=False \
   -p max_laser_range:=10.0

3. In the 2nd Shell: Play the rosbag

   ros2 bag play ogm2pgbm_sensordata

4. In the 3rd Shell: Start rviz2 for visualization

   Ensure to modify the rviz2 configuration file path

   rviz2 -d ~/Downloads/ogm2pgbm.rviz 

4.4 Save the .posegraph Map

After the rosbag finishes playing, set the filename in the SlamToolBoxPlugin on the left side of the rviz2 interface, and click Serialize Map button to save the posegraph map.

Note

1. The reason for not using Ogm2Pgbm to directly generate the .posegraph map is due to a previously encountered issue where maps generated in ROS1 could not be loaded in ROS2. This is likely due to a version issue with slam_toolbox.

   [localization_slam_toolbox_node-12] [ERROR] [1715064875.276643345] [slam_toolbox]: serialization::Read: Failed to read file: Exception: unregistered class
   [localization_slam_toolbox_node-12] [ERROR] [1715064875.276682895] [slam_toolbox]: DeserializePoseGraph: Failed to read file: /home/lihanchen/NAVIGATION_WS/RM2024_SMBU_auto_sentry_ws/install/rm_bringup/share/rm_bringup/map/RMUC.

2. As it is already a package managed by catkin, if you have changed the src code or configuation inside the container, you need to do the following instructions to compile and install the package and source the env.

   catkin_make_isolated --install --use-ninja --pkg ogm2pgbm
   source install_isolated/setup.bash

3. All the node will be closed when the rviz is closed, including the rosbag node

4. The parameters of cartographer when using OGM2PGBM scripts to generate pbstream are as follows (see cartographer_ros/configuration_files/ogm2pgbm_my_robot.lua)

5. remap /scan or /odom in launch file if needed

GMCL & CARTO/SLAM_toolbox

This project combines the pros of the two algorithms, using the fast global localization feature of GMCL and the accurate pose tracking performance of Cartographer or SLAM toolbox.

• First, change the bagname in the line 11 of the file ~/catkin_ws/src/gmcl_carto/gmcl_carto.py.
• Then, make sure the bagfile is located in the directory /root/workspace.
• At last, run the script directly python ~/catkin_ws/src/gmcl_carto/gmcl_carto.py.
• Do the same if you want to run gmcl_carto/gmcl_slamtoolbox.py instead.

Citation

If you use this library for an academic work, please cite the original paper.

@inproceedings{ ogm2pgbm:vega:2022:2DLidarLocalization,
    author = {Vega Torres, M.A. and Braun, A. and Borrmann, A.},
    title = {Occupancy Grid Map to Pose Graph-based Map: Robust BIM-based 2D- LiDAR Localization for Lifelong Indoor Navigation in Changing and Dynamic Environments},
    booktitle = {eWork and eBusiness in Architecture, Engineering and Construction: ECPPM 2022},
    editor = {Eilif Hjelseth, Sujesh F. Sujan, and Raimar Scherer},
    publisher = {CRC Press},
    year = {2022},
    month = {Sep},
    url = {https://publications.cms.bgu.tum.de/2022_ECPPM_Vega.pdf},
    isbn = {978-1-003-35422-2},
    doi = {10.1201/9781003354222-72}
}

Please also cite the code and data, if you use it for your experiments:

@software{ogm2pgbm:vega:code,
  author       = {Vega Torres, M.A.},
  title        = {{Occupancy Grid Map to Pose Graph-based Map for
                   long-term 2D LiDAR-based localization}},
  month        = nov,
  year         = 2022,
  publisher    = {Zenodo},
  version      = {1.0},
  doi          = {10.5281/zenodo.7330270},
  url          = {https://doi.org/10.5281/zenodo.7330270}
}

@misc{ogm2pgbm:vega:data,
    author = {Vega-Torres, Miguel A. and  Braun, Alexander and  Borrmann, André},
    title = {{OGM2PGBM}},
    year = {2024},
    type = {Dataset},
    keywords = {LiDAR; Localization; SLAM; OGM; BIM},
    doi = {10.14459/2024mp1749236}
}

Reference projects

• AMCL
• GMCL
• Cartographer
• SLAM_Toolbox