Removing non-static objects from point clouds
Adapting LOAM to work with REMOVERT and running it on an example from nuScenes dataset
Introduction
Creating a map from from LiDAR scans is nowadays a standard process. It’s known as a process called point cloud registration. It’s done by stitching together several point clouds that are recorded in each instance of the car moving along the road but several artefacts in the final cloud can cause problems when used for localization, map updates, and change detection.
- non-static objects (moving cars, pedestrians, cyclists, …)
- movable objects (parked cars, traffic cones, stationary pedestrians, …)
Movable objects will show up in every scan at the same position. To remove them, segmentation-based approaches are necessary where, e.g., the cars are identified, segmented, and simply cut out in each scan. In this article, however, we will focus on the removal of non-static objects. In recent years, several research projects attempted to solve this issue. Here are three recent examples that also provide the code:
- (2013) Explicit 3D change detection using ray-tracin in spherical coordinates
- (2018) Removing non-static objects from 3D laser scan data
- (2020) Remove-then-revert, short Removert
The origin of these techniques dates back to at least the 80s under the name of change detection for remote sensing. We will give a brief overview of Removert and how static and dynamic maps can be created from multiple scans when the poses of the car or LiDAR are known for each of them.
Basics of Removert
The basic idea is to create a map and compare this information to each newly registered scan. Given the pose of the new scan, we can perform ray tracng from the new scan and see if the global map (grey in picture below) still makes sense. If our rays go through an object (right panel) it means it has moved. Due to occlusion, this is not always easy to identify. It turns out that often static points of the point cloud will be removed in this step.
On top, ray tracing is very computationally expensive. Removert therefore choses to check the differences in range images (color shows the distance of the pixel to the LiDAR). Again we make use of the pose of the next scan and move forward by the driven distance in our global 3D map. Then we can compare the range image of the new scan with the range image of the global map at the new pose. The differences show highlight dynamic points.
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-miniTo 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_61Next, 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”.
cd catkin_ws/src
git clone -b extract-scans https://github.com/yodayoda/SC-LeGO-LOAM.git
cd ..
catkin_make
source devel/setup.bash
roslaunch 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-32eIf 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.launchYou 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:
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.
This article was brought to you by yodayoda Inc., your expert in automotive and robot mapping systems.
If you want to join our virtual bar time on Wednesdays at 9 pm PST/PDT, please send an email to talk_at_yodayoda.co, and don’t forget to subscribe.
