Overview
Here is a post about the LiDAR Working Principle and different types of LiDARs.
One lidar model is the Range-Azimuth-Elevation model (RAE)
Its cartesian coordinates are:
\[\begin{gather*} \begin{aligned} & [r cos(E) cos(A), r cos(E) sin(A), r sin(E)] \end{aligned} \end{gather*}\]If given $[x,y,z]$ coordinates, it’s easy to go the other way, too:
\[\begin{gather*} \begin{aligned} & r = \sqrt{x^2 + y^2 + z^2} \\ & A = arctan2(\frac{y}{x}) \\ & E = arctan2(\frac{z}{r}) \end{aligned} \end{gather*}\]Some point clouds have meta data such as reflectance (intensity), RGB, etc. Reflectance can be used for ground detection, lane detection. Based on the above, LiDAR manufacturers transmit packets (usually) through UDP after measurements. Velodyne HDL-64S3 is a 64 line LiDAR. At each azimuth angle azimuth, (rotational position in the chart), a message contains on laser block that contains 32 lines (along the elevation angle). Its packet looks like:
- So, to find the
[x,y,z]of a single lidar point,elevation_angle = 32*block_id + array_indexazimuth = rotational_position * 0.01 deg / 180 deg * pi(Velodyne lidar azimuth increments in 0.01 deg)range = array[array_index]
- In total, we need
2 + 2 + 32 *3 = 100 bytesto represent the LiDAR Data. If we use thepcl::PointCloud, each point would be [x, y, z, intensity], and that’s32 * (4 + 4 + 4 + 1) = 416bytes. So packets are compressed LiDAR data.
Aside from 3D Point Cloud, another 3D representation is a Surface Element Map (Surfel). A surface element is a small 3D patch that contains:
x,y,z- Surface normal vector
- Color / texture
- Size / Radius
- Confidence
Brute Force KNN Search
KNN search is very commonly used in SLAM. A tiny bit of performance improvement can create a huge difference! Therefore, we must consider parallelism in implementation. Brute-Force KNN is very easy to parallelize, and it could be more efficient than more sophisticated algorithms.
Brute force KNN is:
- Given a point, search through all points to find their distances. Find the K shortest distances
Of course, we need an extra sorting process to find the k shortest distances
Pixel & Voxelized Data Structures to Store Point Cloud
Brute force is a good starting point for KNN. To speed up, there are two ways:
- Point cloud is spatial. We can store them in spatial representations for faster indexing. (This section)
- We can store a point cloud in a tree, a binary-search tree /KD tree, or a quad-tree or octo tree.
Hash is used to store points into an unordered_map
