Removing non-static objects from point clouds
Tags: Lidar, Tutorial, Removert, Mapping, Nuscenes

Another feature is the “revert” step in which static points that were classified as dynamic are being re-added to the map using a lower resolution range image (see the orange box in the images above). This helps with two problems: keeping more points of the static map and also correcting for minor inaccuracies in the detected poses. The poses usually come from the IMU fusion or just from the LiDAR registration itself. They will not be 100% accurate but at a lower resolution the little difference of a few cm has no influence. Let’s look at how we can run the whole pipeline with a small example scene using the freely available nuScenes dataset created by Motional.

Mapping with LiDAR

The ROS package Lidar Odometry And Mapping (LOAM) is the basis of how we will derive poses just from the LiDAR data. It compares each scan in real time. The is the reason why the ROS framework comes in handy. It lets us process or replay sensor data and directly feed it into the LOAM package for mapping.

LOAM similarly to any other SLAM-based method localizes and maps at the same time. As processing LiDAR data is very computationally expensive due to the massive number of points recorded each second LOAM uses a two-step approach. About every tenth of a second it will produce a new pose but only add a new scan to the map about every second.

LOAM was developed in 2014 and has since been improved many times. The main additions were:

loop closure detection (LeGO-LOAM) first with ICP and then with the more advanced Scan Context (SC-XXX versions)

ground-plane detection using a segmentation algorithm making it perform better for vehicles (LeGO-LOAM)

IMU sensor fusion to optimize the poses (LIO-SAM).

Scan Context provides comparisons of scenes that do not depend on the viewpoint. This enables for example the detection of a loop closure if the car drives to the same point in the scene but in reverse (or any other) direction. For more about loop closure have a look at our previous post.

Preparing nuScenes for ROS

As the packages above are real-time methods, we first have to convert the LiDAR scans into ROSBAG files that we will replay and process with SC-LeGO-LOAM. For creating the ROSBAG files, we recommend the package nuScenes2Bag which does it for you. You will need to install the ROS framework which we will also need for later. Follow the instructions of the README to install it and let us know in the comments if you run into trouble.

Then download the “mini” version of the ”Full dataset (v1.0)” from nuScenes. This contains a few selected scenes. In our example we use scene 61. Convert it to a ROSBAG file with nuScenes2Bag. Be aware that nuScenes2bag by default only works with the “mini” download link. If you want more scenes we recommend adding a softlink in the folder “data_nuscenes_full” to let nuScenes2Bag find the files with the following command:

ln -s v1.0-trainval/ v1.0-mini

To produce the bag file we use the same command as recommended in the manual (replace YOUR_PATH to point to the dataset):

catkin_ws/src/ros-bridge/devel/lib/nuscenes2bag/nuscenes2bag --scene_number 61 --dataroot YOUR_PATH/data_nuscenes_full/ --out nuscenes_bags/

One other tip is to reduce the bag size be filtering it. We don’t make use of the images, all we need is the LiDAR information:

rosbag filter nuscenes_bags/61.bag nuscene_61_mini.bag 'topic == "/odom" or topic == "lidar_top" or topic == "tf"'

Storing this small version is very convenient.

Obtaining the poses

All the LiDAR scans are in the rosbag file in the topic called “lidar_top”. If you want (not a necessary step) you can also extract them again from the bag file with

/opt/ros/noetic/lib/pcl_ros/bag_to_pcd nuscene_61_mini.bag lidar_top extracted_scans_from_bag_61

Next, we calculate the poses that belong to the individual scans. We will rely on a method that does not use any IMU data. The main reason is that it’s easier to replicate with other scenes or your own dataset. IMU data can be finicky with sensor drift and coordinate system transformations. If you want to go for IMU data there is a good YouTube tutorial about using SC-SIO-SAM but you have been warned ;) In the following we will use SC-LeGO-LOAM.

SC-LeGO-LOAM does not support saving the poses in the standard code but we have you covered and implemented this feature ( a feature that exists in SC-LIO-SAM). You can find our version in our GitHUB here. Install as you do a normal ROS package in folder “catkin_ws/src”.

Poses extracted from SC-LeGO-LOAM. Alignment obtained by evo with the Umeyama alogrithm.



git clone -b extract-scans

cd ..

catkin_make

source devel/setup.bash

roslaunch lego_loam run.launch cd catkin_ws/srcgit clone -b extract-scans https://github.com/yodayoda/SC-LeGO-LOAM.git cd ..catkin_makesource devel/setup.bashroslaunch lego_loam run.launch # in another terminal

rosbag play nuscene_61_mini.bag # when the file finished playing you can abort the previous process and see the scans and poses in /tmp/ directory

We made a few changes to SC-LeGO-LOAM for it to work with the latest ROS Noetic+RViz versions and OpenCV2. If OpenCV gives you problems, try reverting this change.

We also added a config for the HDL-64E lidar and introduced two additional output files for the scan poses and the scan point clouds. Both are writting to /tmp folder but you can change the paths here and here.

The file in /tmp/optimzed_poses.txt gives us 12 values in the KITTI format, which correspond to the 3x3 rotation matrix and 3 values for the translation. We can visualize the poses on the Singapore One-North map and compare to the reference trajectory in 2D (see map above).

Removing non-static objects

Next install Removert by following the instructions in the README. In “config/params_mulran_scliosam.yaml” adjust the following parameters to make it work with nuScenes and SC-LeGO-LOAM: “save_pcd_directory”, “sequence_scan_dir”, “sequence_pose_path”. The last two need to be set to the output of SC-LeGO-LOAM. Also adjust:

use_keyframe_gap: false

start_idx: 0 # change this

remove_resolution_list: [2.5, 2.0, 1.5] # nuScenes uses a Velodnye HDL-32e

If it doesn’t compile because of you omp version do the same change as for SC-LeGO-LOAM described above.

Finally run:

roslaunch removert run_scliosam.launch

You will be left with two files that give you a static and a dynamic map. If you look at them with e.g. CloudCompare you will see something like this:

Automatic removal of the dynamic component with Removert. The result is a static map.

You can tune the “remove_resolution_list” to your taste to remove less of the road. This influences the low-res point cloud that we described in the beginning.

Another interesting thought is to combine SC-LeGO-LOAM and Removert in real-time. This is possible because both are ros packages. You would need to write the scans and poses to two ros topics and then read the topics in Removert.