LiDAR S-Graphs

Situational graphs (S-Graphs) is a ROS2 package for generating in real-time four-layered hierarchical factor graphs representing a scene graph including Keyframes registring the robot poses, Walls which map wall planes, Rooms Layer constraining the wall planes using 4 wall-room or 2 wall-room factors, Floors constraining the rooms within a given floor level. It also supports several graph constraints, such as GPS, IMU acceleration (gravity vector), IMU orientation (magnetic sensor). We have tested this package mostly with Velodyne (VLP16) sensors in structured indoor environments. This work is a fork of hdl_graph_slam which as previously in ROS1.

📜 Table of contents

📖 Published Papers
⚙️ Installation
- 📦 Installation From Source
🧪 Unit Tests
🐳 Docker
🚀 Examples on Datasets
🛠️ Run S_Graphs On Your Data
🤖 ROS Related

📖 Published Papers

S-Graphs+: Real-time Localization and Mapping leveraging Hierarchical Representations

@ARTICLE{10168233,
 author={Bavle, Hriday and Sanchez-Lopez, Jose Luis and Shaheer, Muhammad and Civera, Javier and Voos, Holger},
 journal={IEEE Robotics and Automation Letters},
 title={S-Graphs+: Real-Time Localization and Mapping Leveraging Hierarchical Representations},
 year={2023},
 volume={8},
 number={8},
 pages={4927-4934},
 doi={10.1109/LRA.2023.3290512}}

Situational Graphs for Robot Navigation in Structured Indoor Environments

  @ARTICLE{9826367,
    author={Bavle, Hriday and Sanchez-Lopez, Jose Luis and Shaheer, Muhammad and Civera, Javier and Voos, Holger},
    journal={IEEE Robotics and Automation Letters},
    title={Situational Graphs for Robot Navigation in Structured Indoor Environments},
    year={2022},
    volume={7},
    number={4},
    pages={9107-9114},
    doi={10.1109/LRA.2022.3189785}}

⚙️ Installation

Note

S-Graphs+ was only tested on Ubuntu 20.04, with ROS1 Noetic and ROS2 Foxy distro.

📦 Installation From Source

The installation consists of 2 parts. One for ROS1 and another for ROS2. You can use just the ROS2 version, however it will not be working to it's full potential.

Important

Before proceeding, make sure you have rosdep installed. You can install it using sudo apt-get install python3-rosdep In addition, ssh keys are needed to be configured on you GitHub account. If you haven't yet configured ssh keys, follow this tutorial

1️⃣ Installation on ROS2

Update Rosdep:

rosdep init && rosdep update --include-eol-distros

Create a ROS2 workspace for S-Graphs

mkdir -p $HOME/workspaces/s_graphs_ros2_ws/src && cd $HOME/workspaces/s_graphs_ros2_ws/src && source /opt/ros/foxy/setup.bash

Clone the S-Graphs repository into the created workspace

git clone git@github.com:snt-arg/lidar_s_graphs.git -b feature/ros2 s_graphs

Install required dependencies using vcstool

cd s_graphs && vcs import --recursive ../ < .rosinstall_ros2

Install required ROS2 packages

cd ../../ && rosdep install --from-paths src -y --ignore-src -r

Build workspace

colcon build --symlink-install

Note

If you want to compile with debug traces (from backward_cpp ) run:

colcon build --symlink-install --cmake-args -DCMAKE_BUILD_TYPE=RelWithDebInfo

Source workspace

source install/setup.bash

2️⃣ Download ROS Bridge

source /opt/ros/foxy/setup.bash && sudo apt install ros-foxy-ros1-bridge

3️⃣ Installation on ROS1

Important

Before following the instructions from below, ensure that you are in a fresh terminal, without ROS2 sourced.

Create a ROS1 workspace for S-Graphs

mkdir -p $HOME/workspaces/s_graphs_ros1_ws/src && cd $HOME/workspaces/s_graphs_ros1_ws/src && source /opt/ros/noetic/setup.bash

Clone the S-Graphs repository into the created workspace

git clone git@github.com:snt-arg/lidar_s_graphs.git -b feature/ros2 s_graphs

Install required dependencies using vcstool

cd s_graphs && vcs import --recursive ../ < .rosinstall_ros1

Install required ROS packages

cd ../../ && rosdep install --from-paths src --ignore-src -y -r

Install pcl_ros

sudo apt install ros-noetic-pcl-ros

Build workspace

Important

Make sure s_graphs_ros1_ws is built in Release otherwise the room extraction won't work properly.

catkin config --cmake-args -DCMAKE_BUILD_TYPE=Release && catkin build

🧪 Unit Tests

Some unit tests are available. In case you want to test, run the following command:

colcon test --packages-select s_graphs --event-handler=console_direct+

🐳 Docker

Build Docker Image

Create a ROS2 workspace for S-Graphs

mkdir -p $HOME/workspaces/s_graphs_ros2_ws/src && cd $HOME/workspaces/s_graphs_ros2_ws/src

Change directory to where Dockerfile is located in s_graphs

git clone git@github.com:snt-arg/lidar_s_graphs.git -b feature/ros2 s_graphs && cd $HOME/workspaces/s_graphs_ros2_ws/src/s_graphs/docker/foxy_noetic

Build image

Note

In case you have a different ssh key name for your GitHub account, change id_ed25519 oto yours.

docker build -t sntarg/s_graphs --ssh default=$HOME/.ssh/id_ed25519 .

🚀 Example on Datasets

Warning

For execution of the experiments we use mprocs, which makes the process of launching different processes easier.

Real Dataset

Important

Download real dataset using this link and store it in the folder ~/Downloads/real, the below mprocs script will not work otherwise.

cd $HOME/workspaces/s_graphs_ros2_ws/src/s_graphs && mprocs --config .real_mprocs.yaml

Virtual Dataset

Important

Download virtual dataset using this link and store it in the folder ~/Downloads/virtual, the below mprocs script will not work otherwise.

cd $HOME/workspaces/s_graphs_ros2_ws/src/s_graphs && mprocs --config .virtual_mprocs.yaml

Running S_Graphs with Docker

Note

This tutorial assumes that you have followed the instructions to setup docker in section 🐳 Docker

Create a container for the s_graphs image.

docker run -dit --volume=/tmp/.X11-unix:/tmp/.X11-unix:rw --network=host -e DISPLAY=$DISPLAY --name s_graphs_container sntarg/s_graphs

Download the dataset you desire from above to your local machine.
Move the rosbag inside docker container

docker cp ~/Downloads/real s_graphs_container:/root/Downloads/real # For real dataset
# OR
docker cp ~/Downloads/virtual s_graphs_container:/root/Downloads/virtual # For virtual dataset

Execute the container

docker exec -ti s_graphs_container bash

Run mprocs

mprocs_real # To run on a real robot or a real dataset
# OR
mprocs_virtual # To run on a simulation or virtual dataset

Note

Press reset on rviz2 once in a while when running S-Graphs to avoid freezing effect caused by rviz2 on foxy.

🛠️ Run S_Graphs On Your Data

Define the transformation between your sensors (LIDAR, IMU, GPS) and base_link of your system using static_transform_publisher (see line, s_graphs_launch.py). All the sensor data will be transformed into the common base_link frame, and then fed to the SLAM algorithm. Note: base_link frame in virtual dataset is set to base_footprint and in real dataset is set to body
Remap the point cloud topic in s_graphs_launch.py of s_graphs_prefiltering_node and s_graphs_node. Like:

    remappings=[
            ("velodyne_points", "/rs_lidar/points"),
            ("imu/data", "/platform/imu/data"),
        ],
  ...

If you have an odometry source convert it to base ENU frame, then set the arg compute_odom to false in s_graphs_ros2_launch.py and then remap odom topic in s_graphs_node like

     remappings=[
            ("velodyne_points", "/platform/velodyne_points"),
            ("/odom", "/husky/odometry"),
        ],
  ...

Note

If you want to visualize the tfs correctly from your odom source, you MUST provide a tf from the odom to base_link frame.

🤖 ROS Related

📥 Subscribed Topics

`s_graphs` node

Topic name	Message Type	Description
`/odom`	nav_msgs/Odometry	The odometry from the robot.
`/filtered_points`	sensor_msgs/PointCloud2	The filtered data from the LiDAR sensor.

`room_segmentation` node

Topic name	Message Type	Description
`/voxblox_skeletonizer/sparse_graph`	visualization_msgs/MarkerArray	Represents the free space where the robot can go to. Also known as free-space clusters.
`/s_graphs/map_planes`	s_graphs/PlanesData	Planes seen by the current robot keyframe.

`floor_plan` node

Topic name	Message Type	Description
`/s_graphs/all_map_planes`	visualization_msgs/MarkerArray	All the planes that have been seen by the robot.

📤 Published Topics

`s_graphs` node

Topic name	Message Type	Description
`/s_graphs/markers`	visualization_msgs/MarkerArray	These markers represent the different s_graphs layers.
`/s_graphs/odom2map`	geometry_msgs/TransformStamped	The estimated drift of the robot within its map frame (world).
`/s_graphs/odom_pose_corrected`	geometry_msgs/PoseStamped	The optimized/drift-free pose of the robot once odom2map is applied.
`/s_graphs/odom_path_corrected`	nav_msgs/Path	The optimized/drift-free pose path of the robot once the odom2map is applied.
`/s_graphs/map_points`	sensor_msgs/PointCloud2	The projected 3D points using the optimized robot pose.
`/s_graphs/map_planes`	s_graphs/PlanesData	Planes seen by the current robot keyframe.
`/s_graphs/all_map_planes`	s_graphs/PlanesData	All the planes that have been seen by the robot.

`room_segmentation` node

Topic name	Message Type	Description
`/room_segmentation/room_data`	s_graphs/RoomsData	Contains all the necessary information about the rooms on a given floor.

`floor_plan` node

Topic name	Message Type	Description
`/floor_plan/floor_data`	s_graphs/RoomData	Contains all the necessary information about each floor.

🔄 ROS Services

Topic name	Message Type	Description
`/s_graphs/dump`	s_graphs/DumpGraph	Save all the internal data (point clouds, floor coeffs, odoms, and pose graph) to a directory.

Topic name	Message Type	Description
`/s_graphs/save_map`	s_graphs/SaveMap	Save the generated 3D map as a PCD file.

⚙️ ROS Parameters

All the configurable parameters are listed in config folder as ros params.

🌐 Published TFs

map2odom: The transform published between the map frame and the odom frame after the corrections have been applied.
The entire tf_tree for the virtual experiment can be seen in the figure below.

haiyang2022/lidar_s_graphs

LiDAR S-Graphs

📜 Table of contents

📖 Published Papers

⚙️ Installation

📦 Installation From Source

1️⃣ Installation on ROS2

2️⃣ Download ROS Bridge

3️⃣ Installation on ROS1

🧪 Unit Tests

🐳 Docker

Build Docker Image

🚀 Example on Datasets

Real Dataset

Virtual Dataset

Running S_Graphs with Docker

🛠️ Run S_Graphs On Your Data

🤖 ROS Related

📥 Subscribed Topics

s_graphs node

room_segmentation node

floor_plan node

📤 Published Topics

s_graphs node

room_segmentation node

floor_plan node

🔄 ROS Services

⚙️ ROS Parameters

🌐 Published TFs

`s_graphs` node

`room_segmentation` node

`floor_plan` node

`s_graphs` node

`room_segmentation` node

`floor_plan` node