KITTI激光雷达点云解析与图像反投影

旧城等待， 2023-01-05 15:27 243阅读 0赞

# 介绍 #

KITTI作为广为人知的自动驾驶数据集，很多创业公司喜欢拿来做算法排名。  
官网下载比较慢，这里参考文末博客给出百度云下载（27G）

> 链接：[https://pan.baidu.com/s/1-4WchJlcZ2guwcfbHqrdFw][https_pan.baidu.com_s_1-4WchJlcZ2guwcfbHqrdFw]  
> 提取码：grys

# 解析 #

我的目的是解析三维激光点云并投影至二维图像坐标，得到类似RGBD相机的效果。  
需要用到的文件包括：二进制Velodyne点云、双目RGB相机左眼cam2图像、激光到相机矩阵等标定文件

各传感器坐标在KIT的文章Vision meets Robotics: The KITTI Dataset中给出  
[![8aa94516ba6209765e76c8e7ac80b133.png][]][8aa94516ba6209765e76c8e7ac80b133.png 1]

参考semantic-kitti-api的开源代码，使用numpy读取点云bin文件reshape为x,y,z,r（回波强度）格式  
点云index从外圈到内圈顺时针存储

# read raw data from binary
    scan = np.fromfile(binary, dtype=np.float32).reshape((-1,4))
    points = scan[:, 0:3] # lidar xyz (front, left, up)
    remissions = scan[:, 3]

标定文件calib/\*.txt解读：

P0: 7.215377000000e+02 0.000000000000e+00 6.095593000000e+02 0.000000000000e+00 0.000000000000e+00 7.215377000000e+02 1.728540000000e+02 0.000000000000e+00 0.000000000000e+00 0.000000000000e+00 1.000000000000e+00 0.000000000000e+00
    P1: 7.215377000000e+02 0.000000000000e+00 6.095593000000e+02 -3.875744000000e+02 0.000000000000e+00 7.215377000000e+02 1.728540000000e+02 0.000000000000e+00 0.000000000000e+00 0.000000000000e+00 1.000000000000e+00 0.000000000000e+00
    P2: 7.215377000000e+02 0.000000000000e+00 6.095593000000e+02 4.485728000000e+01 0.000000000000e+00 7.215377000000e+02 1.728540000000e+02 2.163791000000e-01 0.000000000000e+00 0.000000000000e+00 1.000000000000e+00 2.745884000000e-03
    P3: 7.215377000000e+02 0.000000000000e+00 6.095593000000e+02 -3.395242000000e+02 0.000000000000e+00 7.215377000000e+02 1.728540000000e+02 2.199936000000e+00 0.000000000000e+00 0.000000000000e+00 1.000000000000e+00 2.729905000000e-03
    R0_rect: 9.999239000000e-01 9.837760000000e-03 -7.445048000000e-03 -9.869795000000e-03 9.999421000000e-01 -4.278459000000e-03 7.402527000000e-03 4.351614000000e-03 9.999631000000e-01
    Tr_velo_to_cam: 7.533745000000e-03 -9.999714000000e-01 -6.166020000000e-04 -4.069766000000e-03 1.480249000000e-02 7.280733000000e-04 -9.998902000000e-01 -7.631618000000e-02 9.998621000000e-01 7.523790000000e-03 1.480755000000e-02 -2.717806000000e-01
    Tr_imu_to_velo: 9.999976000000e-01 7.553071000000e-04 -2.035826000000e-03 -8.086759000000e-01 -7.854027000000e-04 9.998898000000e-01 -1.482298000000e-02 3.195559000000e-01 2.024406000000e-03 1.482454000000e-02 9.998881000000e-01 -7.997231000000e-01

P0-P3为3x4相机投影矩阵，0=gray\_L 1=gray\_R 2=rgb\_L 3=rgb\_R  
R0\_rect为3x3相机旋转矩阵  
Tr\_velo\_to\_cam为3x4激光到相机RT矩阵  
点云对图像投影使用如下公式

Z[u v 1]T = P2 * R0_rect * Tr_velo_to_cam * [x y z 1]T

写为齐次形式，R0在右下角补1变为4x4，Tr最后一列补1变为4x4，大写Z为相机深度

# 代码 #

使用python实现，过滤了激光雷达背后深度为负的点云，保留图像宽高内点，深度以colormap表示  
源码与测试数据已开源至github仓库[azureology/kitti-velo2cam: project lidar point cloud to camera image][azureology_kitti-velo2cam_ project lidar point cloud to camera image]

# 效果 #

[![05e886791883470d3637b3b191a2688e.png][]][05e886791883470d3637b3b191a2688e.png 1]

# 参考 #

*  [KITTI数据集下载（百度云）\_u013086672的博客-CSDN博客][KITTI_u013086672_-CSDN]
 *  [Vision meets Robotics: The KITTI Dataset][Vision meets Robotics_ The KITTI Dataset]
 *  [PRBonn/semantic-kitti-api: SemanticKITTI API for visualizing dataset, processing data, and evaluating results][PRBonn_semantic-kitti-api_ SemanticKITTI API for visualizing dataset_ processing data_ and evaluating results]
 *  [numpy.logical\_or — NumPy v1.19 Manual][numpy.logical_or _ NumPy v1.19 Manual]
 *  [matplotlib - python 3 scatter plot gives "ValueError: Masked arrays must be 1-D" even though i am not using any masked array - Stack Overflow][matplotlib - python 3 scatter plot gives _ValueError_ Masked arrays must be 1-D_ even though i am not using any masked array - Stack Overflow]

[https_pan.baidu.com_s_1-4WchJlcZ2guwcfbHqrdFw]: https://pan.baidu.com/s/1-4WchJlcZ2guwcfbHqrdFw
[8aa94516ba6209765e76c8e7ac80b133.png]: /images/20221119/b4514f1e907f4aeeb8d2f0bc22d31606.png
[8aa94516ba6209765e76c8e7ac80b133.png 1]: https://img2020.cnblogs.com/blog/1642690/202011/1642690-20201119102232526-1403056560.png
[azureology_kitti-velo2cam_ project lidar point cloud to camera image]: https://github.com/azureology/kitti-velo2cam
[05e886791883470d3637b3b191a2688e.png]: https://img-blog.csdnimg.cn/img_convert/05e886791883470d3637b3b191a2688e.png
[05e886791883470d3637b3b191a2688e.png 1]: https://img2020.cnblogs.com/blog/1642690/202011/1642690-20201119102606940-1126757485.png
[KITTI_u013086672_-CSDN]: https://blog.csdn.net/u013086672/article/details/103910266
[Vision meets Robotics_ The KITTI Dataset]: https://www.mrt.kit.edu/z/publ/download/2013/GeigerAl2013IJRR.pdf
[PRBonn_semantic-kitti-api_ SemanticKITTI API for visualizing dataset_ processing data_ and evaluating results]: https://github.com/PRBonn/semantic-kitti-api
[numpy.logical_or _ NumPy v1.19 Manual]: https://numpy.org/doc/stable/reference/generated/numpy.logical_or.html
[matplotlib - python 3 scatter plot gives _ValueError_ Masked arrays must be 1-D_ even though i am not using any masked array - Stack Overflow]: https://stackoverflow.com/questions/44224076/python-3-scatter-plot-gives-valueerror-masked-arrays-must-be-1-d-even-though