Lidar Data Processing for Autonomous Systems
Overview
The introduction of low-cost lidar sensors has significantly impacted various industries, making lidar data processing technology more accessible and crucial for advancements in automated driving, robotics, and aerospace. This webinar is dedicated to exploring lidar data processing, pivotal for enhancing perception and navigation in autonomous systems. Participants will gain insights into leveraging lidar data processing for advanced workflows, essential for the development of autonomous technologies.
Highlights
This webinar will cover both basic and advanced aspects of lidar data processing, providing participants with a comprehensive understanding of how to effectively utilize this technology. Key areas of focus will include:
- Reading and streaming point cloud data.
- Applying deep learning algorithms for object detection and semantic segmentation.
- Enhancing accuracy with cross calibration between lidar and camera sensors.
- Map generation and SLAM (Simultaneous Localization and Mapping).
- Object tracking through sequential analysis of lidar point cloud data.
By the end of this webinar, attendees will be well-versed in lidar data processing and its application in the development of cutting-edge autonomous systems.
Recorded: 17 Feb 2021
Hello, everyone. Welcome to the Lidar Processing in MATLAB webinar. My name is Minhaj Falaki, and I'm the product manager for the Lidar Toolbox. Today, I'll demonstrate how you can use MATLAB to design and analyze lidar processing systems. Lidars are now being used in autonomous systems, along with other sensors, such as cameras and radars for environmental perception. There are three key benefits of using lidars in our systems. Firstly, it provides inaccurate, depth-of-field of the surrounding objects.
Secondly, it provides a 360 view of our surroundings with just one sensor, while we need multiple calibrated sensors in case of cameras and radars. Finally, the dense data from lidar sensors improves the accuracy when we are using machine learning and deep learning algorithms. In this session, using three examples from three different application areas, like automated driving, aerospace, and robotics, I'll show you how to build lidar processing systems in MATLAB. The first example is object tracking for automated driving applications.
This uses real-world data to track cars and trucks from an entire point cloud sequence collected from any vehicle. We'll see how to use a deep learning-based approach for object tracking. And then we'll track our object using a JPDA tracker. As a final step, we'll generate C code for this algorithm that can be deployed into production. In the second example, we'll implement simultaneous localization and mapping on a point cloud data stream from a lidar sensor mounted on a UAV in the simulation environment.
We'll show how to perform feature mapping lidar SLAM in this example. In the third example, we'll see how to create a 3D-colored map of an office arena using a ground robot. We'll perform lidar camera calibration and use this data to combine the camera data with the lidar data. And then we'll perform mapping with these colored point clouds. Let's start a quick introduction to lidar technology. Lidar sensors are range motion sensors like radars and sonars.
It a 3D or 2D representation of the surroundings by measuring distance to objects around them using pulsed-lights. Lidar sensors captures surrounding data as point clouds. A point cloud data is a set of data points in space which represents a 3D structure of the object by providing x, y and z-coordinates. There are basically two types of point cloud data, unorganized point clouds and organized point clouds.
A row point could in M cross C format is called an unorganized point clouds, where M is the number of points in the point could and C is the number of channels in the point clouds. Lidar sensors generally create organized point clouds. Sensors like stereo cameras create organized point clouds that are arranged in M cross N cross C format. Here, M is the length of the point cloud, N the width, and C the number of channels in the point cloud.
Adjacent points in an organized point cloud are related to each other due to proximity in physical space between them. If we project an unorganized point cloud on to a sphere, we'll get an organized point cloud. This conversion will be very useful in implementing certain lidar processing algorithms. To implement these workflows, we'll be using the newly launched Lidar Toolbox. The Lidar Toolbox provides algorithms, tools, and apps to design, test, and analyze lidar processing systems.
Now let's take a deep dive into our first example to detect and track objects for automated driving applications. Here is our workflow looks like. We'll first feed point cloud data into MATLAB and then do some preprocessing. After that, we'll detect objects on the preprocessed point cloud data. Then we'll track these objects using a JPDA tracker. Finally, we'll create a C code for this algorithm to deploy it on our hardware.
Let's start with bringing point cloud data into MATLAB. MATLAB provides you flexibility to read point cloud data from multiple file formats like PCD, PLY, PCAP, et cetera. You can also stream point cloud data directly from the Velodyne sensors. In addition to this, you can read point cloud data stored in rosbag files. To demonstrate this in MATLAB, we'll use the Velodyne file reader function to read point cloud data in the PCAP format. To visualize the point cloud, will use PC player, which is a very efficient way to visualize and analyze point clouds.
It allows you to zoom, rotate, and pan point clouds. To find details about any selected point in the point cloud, we can make use of that particular feature. In MATLAB, point cloud data is stored as a point cloud object. It encloses all the properties associated with the point cloud, such as location, intensity, color, count, xyz limits, et cetera. On this point cloud, you can find nearest neighbors, find points in any specific region of interest, you can remove invalid points in the point cloud, and select any points in the point cloud using indices.
Now that we have our point cloud data, we can start to do some basic preprocessing. For example, you can use the pcdenoise function to denoise the point cloud based on a threshold value you're defining. And you can use pcdownsample to downsample the point cloud without compromising the structure of the point cloud. As seen here, the number of points in the point cloud have reduced from 34,000 to 26,000, while the structure still remains the same.
As the final step in point cloud preprocessing, we'll segment and then remove the ground from the point cloud. To segment the ground plane, we can use the pcfitplane function which fits a mathematical plane model on the point cloud. Or you can use the segmentGroundFromLidarData function. This function is designed for much faster execution and should be applied with organized point clouds.
After submitting the ground, we'll remove the ground plane. This is a common practice in automated driving applications. This improves processing time and performance. Refer to the Lidar Toolbox documentation to find more preprocessing functions that MATLAB supports. Now that we have preprocessed our point cloud data, as a next step, we'll move on to object detection. There are two ways with which you can detect objects and create bounding boxes for tracking.
The first method is segmenting point clouds and then fitting an orienting bounding boxes around each object. In the second method, we'll use object detection networks like PointPillars, which can detect objects and create bounding boxes around them. We'll talk about the first method in more details. There are two techniques to segment the point cloud. The first is by using clustering algorithms and second is semantic segmentation using deep neural networks.
In clustering, we'll segment the point clouds based on the distance between points in the point cloud. This method doesn't classify points based on the objects in them, but the execution will be much faster compared to the deep learning-based approach. Semantic segmentation, on the other hand, will provide better results since it classifies each points in the point using deep learning. But this technique is computationally intensive.
In MATLAB, we can use the pcsegdist function or segmentLidarData function for clustering. The pcsegdist function segment point clouds into clusters based on the Euclidean distance between them. We'll give a minimum threshold value between the points from different clusters and then apply the function. While pieces of this function can be used on both organized and unorganized point clouds, the segmentLidarData function is designed to cluster organized point clouds resulting in faster execution.
The second technique is to use deep learning models. The neural networks here assigns class-labels to each point in the point cloud. We can apply state of the art networks like PointSeg and SqueezeSegV2 on the road point cloud data. It segments the point cloud into three classes here, cars, trucks, and background. Let's look at the code for this. We'll first load our data and a pretrained SqueezeSegV2 network. To apply the network, we'll preprocess the point cloud using a helper function.
This function first converts the unorganized point cloud into an organized point cloud by projecting an unorganized point cloud onto a sphere. To do this, we need sensor parameters, like field of use, resolution, beam configuration, et cetera. After this conversion it extracts the intensity image from the organized point cloud. Now, we'll define the classes and apply the pretrained SqueezeSegV2 network on our data.
The output from the network shows the intensity image segmented into car, truck, and background. After segmentation, we'll feed oriented bounding boxes around the segmented objects using the pcfitcuboid function. It uses L-shape fitting to perfectly fit oriented bounding boxes around the objects. The second technique to detect objects from point clouds for tracking is to use object detection networks. Here we apply PointPillars network on the point cloud.
It can detect objects and fit oriented bounding boxes around them. Refer to the MathWorks documentation example which explains in detail how to model and train the PointPillars network from scratch. This example is shipped with a pre-label data set. But to use your own data with this example, you can make use of the Lidar Labeler app to label your data. You can use this app to label your point cloud for any object detection workflows.
You can apply some basic operations like clustering, ground removal, et cetera to make more sense about your point clouds, and then you can create bounding boxes around the objects to label them. The app allows in-built or custom automation algorithms to automate the labeling process. So here is the output of our object detection. We can see bounding boxes around cars and trucks in our point clouds. Now using these bounding boxes as inputs to our tracker, we'll track these objects through the entire point cloud sequence.
Here is how we are going to do that. We'll use an IMM filter which is a great way to create complex records. In this demo, we are using a JPDA tracker that can track multiple targets from multiple sensors. Here you can observe that the car and truck are being tracked through the entire sequence. As the final step, let's see how to generate code for the object detection and tracking algorithm we have developed now.
From MATLAB, you can generate C++ as well as CUDA codes. We can use MATLAB Coder for C++ code generation. To do this, first specify the MATLAB function name and the input attributes. After that, we'll generate the code. The generated code provides a report which can be used to track it back to the corresponding MATLAB code. We can use GPU Coder to generate CUDA code for GPUs.
Refer to this example in the Lidar Toolbox documentation. It explains in detail the entire process of CUDA code generation. You need a CUDA-enabled NVIDIA GPU and a compatible driver for this. So we have demonstrated how to build MATLAB systems for object detection and tracking for autonomous driving applications. If you have any questions regarding this, feel free to ask them in the Q&A section by the end of this webinar.
We'll now move on to the next example, which is aerial lidar SLAM. Here, we'll implement simultaneous localization and mapping, or SLAM, on point cloud data streamed from a lidar sensor which is mounted on a UAV. This will happen in a simulated environment. Here we'll demonstrate how to do feature-based matching with this point cloud data. This example use a simulation environment available in the UAV Toolbox.
We have a broad range of simulation environments like this, which can be used to implement similar workflows in different application areas. The Automated Driving Toolbox contains Unreal-based simulators and cuboid simulation environments. The UAV Toolbox supports Unreal as well as static mesh-based simulation environments. You also have RoadRunner to create static objects for Unreal environments. Having this prebuilt simulation environment helps to build and test your algorithms before deploying it in the real world.
You can save a lot of time and money, and of course it's a lot safer. Coming back to our aerial lidar SLAM, our workflow starts with stimulating the city environment with a UAV in it. After that, we'll register point clouds based on feature descriptors extracted from each point cloud. Finally, we'll implement pose graph optimization to correct the drifts that happen in the registration step and build the complete map. Let's start with stimulating the environment.
We'll make use of a city simulation containing lots of roads and buildings. We'll first define the trajectory for our UAV. Here, we have overlaid the x- and y-coordinates of our trajectory on the top view of the map. Note that two trajectory loops have been utilized to ensure loop closure, which will be useful in pose graph optimization. We are using Simulink to perform this simulation. So we can load the Simulink model by running this script.
There are four main parts to this Simulink model. The first one is the simulation 3D scene configuration block. You can find this block from the Simulink library by searching simulation 3D scene. This configures the scene to the US city block that we saw earlier. The second block is our simulation 3D UAV vehicle model. This places our UAV on the scene and moves it through the trajectory using xyz and your data that we loaded before.
The third block here is the simulation 3D lidar block. We are using the lidar model available in the UAV Toolbox for this example. This mounts the lidar sensor on top of our UAV. You can select the sensor and configure parameters like detection range, range resolution, field of view, et cetera. And the fourth block we have is our MATLAB system. Here, we'll process the lidar data from the sensor to iteratively create our map. Now let's run our simulation.
Running the simulation shows the UAV flying on the predefined trajectory and capturing live lidar data. The MATLAB system in our Simulink model collects this data and stitches it together to build a map of the arena. Before I explain how the MATLAB system block operates, I'll briefly explain point cloud registration. So point cloud registration is the process of stitching multiple point clouds to create a combined bigger point cloud.
If we have point cloud data and accurate pose data for each instances, we can use the pose info to stitch the point cloud. But this is not always the case. Our pose info will not be that accurate. So here I'll show how to create a 3D map with only point cloud data in MATLAB. We first read two consecutive point clouds and then do some basic preprocessing like ground removal and downsampling. After that, we'll extract FPFH descriptors from both point clouds the extractFPFHFeatures function.
To match them, we'll use the pcmatchfeatures function. Here you can see that we have two point clouds in red and blue colors, and the match features are connected with yellow lights. Now, we'll use these matched features to estimate the transformation between the point clouds using the estimateGeometricTransform3D function. This will give us the rigid transformation between the point cloud. We'll now use the pcalign function using the rigid transformation we got. Here is our aligned point cloud based on the match features.
This is pretty much what is happening in the MATLAB system also. Here we'll do basic preprocessing like removing invalid points in the point cloud. We'll then extract and match FPFH descriptors and estimate transformation between the point clouds. After that, we'll align the point cloud. We'll do this for every third point cloud that is fed into the MATLAB system. This accelerates the processing and accumulates enough motion between scans.
Here is the output of our simulation. You can see that the map is not aligned properly. This is because there were minor drifts in each registration step, which then accumulated over time then we built the map. So now we'll apply pose graph optimization to correct these drifts and build the map properly. For this, we'll create a pcviewset object, and store the point cloud odometry and SLAM data as a set of views and pairwise connection between the views. We'll do this for the entire point cloud sequence.
We'll then find loop closure candidates from the scan, create pose graph for the view set using the createPoseGraph function, and then optimize the poses using the optimizePoses function. Pose graph optimization is a separate topic by its own. Due to time constraints, I would recommend you to watch this MATLAB Tech Talks. It explains pose graph optimization and how it helps to have a better estimate of the current and past poses and a better model of the environment.
So here is our final output after pose graph optimization. You can see our optimized viewpoint poses in red color, and the map is now aligned properly. It is much cleaner than what we got earlier and it resembles our arena. With this, we conclude the aerial lidar SLAM workflow. And of course, I'll be very happy to answer any of your questions regarding this in the Q&A session. Now let's move on to our final example, which is to create a colored 3D map of an office arena using a ground robot.
Here we'll perform lidar camera calibration to combine camera data with lidar data. We'll then use these colored point clouds to create the 3D map of the arena. To create this demo, we ran a TurtleBot in one of our office arenas. We have collected camera, lidar, and automated data into a rosbag. We'll use this data to create a colored 3D map of our arena as you can see in the video.
Here is the workflow we have used. We'll start with reading lidar and camera data from the recorder rosbag. Then, we'll use the checkerboard image and corresponding point clouds to do lidar camera calibration. After that, we'll use the calibration data to project the camera data onto lidar data. Finally, we'll use this colored point cloud to create a colored map of our office environment
We'll start by reading the data from the rosbag using rosbag functions. In this example, we used a Velodyne sensor to record point cloud data by topic name velodyne_points. Similarly, we have collected images into a topic by the name, image_color. We'll now extract this data along with the time of capturing. This will be useful to time sync the lidar data with the corresponding camera data. We'll use the synchronize function for this.
To implement lidar camera calibration, we have used the checkerboard calibration method. To do this, we need to fix the sensor poses on our robot and then capture checkerboard data from both these sensors with overlapping field of view. We need to extract the checkerboard features from the cameras and lidar's data now. Then, we'll use extracted features to find the transformation between the sensors and then fuse the image data with the point cloud data.
Let's see how to do that in MATLAB. We have read radar and camera data from the rosbag along with the intrinsic parameters of the camera. We have used MATLAB's Camera Calibrator app to determine the camera intrinsics. We'll now use the estimateCheckerboardCorners3D function to find the checkerboard features from images. And then use detectRectangularPlanePoints function to detect the checkerboard pattern from the point clouds.
Now, we'll you estimateLidarCameraTransform function to find the transformation between the sensors. Here is our result. On the top, we have projected point clouds on the Images. And in the bottom, you can see that color inflow from the camera is added on to the point cloud. Now that we have done the lidar camera calibration, we'll need to project the camera data onto the lidar data. To do this, we can use the fuseCameraToLidar.
This function uses the intrinsic parameter of the camera and the camera-to-lidar rigid transformation to fuse image information with the point clouds. So we load the transformation and then apply this function. Here is an example of the color point cloud we got by fusing image data with the corresponding point cloud data. The next step is to create a 3D map with the colored point clouds. For that, we'll be using the same FPFH-base map creation workflow that we saw in the aerial lidar SLAM workflow.
Previously, we used non-colored point clouds to extract FPFH descriptors. Is it possible to do that on colored point clouds? This is a common question that we get from our customers. To answer this, let us take a look at the colored point cloud data. Here you can see that there is a property by the name, location, which provides x, y, and z values of the row point cloud. There's another property by the name color which stores the RGB values of each points in the point cloud.
During extraction and matching of FPFH descriptors, the function will use data from the location. And while aligning the point clouds, it will use both location and color parameters. As I mentioned before, we'll now create a 3D map using the same approach we have used in the aerial SLAM example. We extract and match FPFH descriptors, estimate transformation, and align point clouds from the entire sequence in loop to create the map.
In the final output, you can see captured camera data, point cloud data, the top view of the point cloud map that is being created, and the colored point cloud map. In the final generated 3D map, we can see that the points in the entire point cloud is colored. This will be very useful in robotics for object detection and obstacle avoidance. So we have showcased how to use MATLAB to build lidar processing systems for different application areas of automated driving, robotics, and aerospace.
The benefits of using MATLAB for lidar processing workflows are the ability to create complete workflow to apply deep learning algorithms on lidar data from labeling to deployment, the ability to simulate lidar data for UAV as well as automated driving applications, a library of pre-built algorithms to do preprocessing, object detection and tracking, mapping and SLAM, lidar camera calibration, and many more. In addition to designing and analyzing, MATLAB enables C, C++, and CUDA code generation to deploy your algorithms on a target hardware. To learn more about that process, we encourage you to go through these resources in the slide.