LiDAR点云转深度图
之前写了python的深度图转点云的文章
这次文章说的是它的逆变换 点云——到深度图
测试用例
本文用KITTI数据集来作为测试用例
以下的图片以及LiDAR数据取自2011_09_26_drive_0005
代码的colab在线版：https://colab.research.google.com/drive/1uq2wKbfqYI9LMkPRL-Zm22ljwfLutYln?usp=sharing
数据导入
图像在kitti数据集中的路径为
kitti/2011_09_26/2011_09_26_drive_0005_sync/image_00/0000000150.png
LiDAR的二进制文件路径为
kitti/2011_09_26/2011_09_26_drive_0005_sync/velodyne_points/data/0000000150.bin
colab里面直接上传到了根目录
kitti数据集的LiDAR数据有4位 分别是 X Y Z 反射率，反射率本次暂时用不到 所以只取前三位
投影变换 1
想要做出深度图，首先得将点云给投影到图像上，取得像素坐标值
一个简单的方法是利用opencv的函数 CV2.projectPoints
函数需要的参数有
点云
点云坐标系到相机坐标系的旋转矩阵
点云坐标系到相机坐标系的平移向量
相机内参
相机畸变参数
其实就是所谓的 外参（2+3）和内参（4+5）
LiDAR和相机1的外参可以在数据集中
/kitti/2011_09_26/calib_velo_to_cam.txt
里找到
calib_time: 15-Mar-2012 11:37:16
R: 7.533745e-03 -9.999714e-01 -6.166020e-04 1.480249e-02 7.280733e-04 -9.998902e-01 9.998621e-01 7.523790e-03 1.480755e-02
T: -4.069766e-03 -7.631618e-02 -2.717806e-01
相机内参则是参考ORB_SLAM3的kitti示例配置文件
并且，相机的视角是朝前方的 后面的点云自然不可能投影到相机上，所以这里把点云中在后方的点去掉，也就是X为负的部分
接着把上面的参数全部传入CV2.projectPoints
就可以得到点云投影在画像上的像素坐标，因为像素坐标是只有整数 所以转为整数型
刚刚去除了LiDAR中心后半部分的点
因为相机有FOV 视野的限制，所以前半部分的点也并不是全部都能投影到画像上
所以这里把坐标超出画像的无效点给去除
画出来观察一下
看起来没毛病。虽然仔细看有的点也并不是和物体严丝合缝，比如右手边的胖大叔
不过这应该是LIDAR数据的采集时间和拍照时间有细微的时间差
数据集里面有时间戳，可以看得出两个数据之间略微有时间差
投影变换 2
刚才利用了opencv的函数一下就完成了点云到画像的投影。
不过很可惜这样只能知道点云对应的画像坐标，并不能达到我们的目的——做出深度图
为了做出深度图 除了点云的画像坐标以外，我们还需要每个点的深度
而深度——depth的定义，是一个点在相机坐标系下的Z值
刚才opencv的函数拿了相机和LiDAR的外参以及内参
一口气完成了如下的变换
LiDAR坐标系——外参——相机坐标系——内参——画像坐标系
而为了得到深度值，我们需要点群在相机坐标系下的坐标
所以先来手动写一下前面这部分变换
在齐次坐标系下 坐标系和坐标系的变换可以写成
中间的那个矩阵也就是相机的外参矩阵 也叫三维空间几何变换矩阵
其中 左上角的3x3为旋转矩阵 而右边下3x1为平移向量
重新导入我们的LiDAR点云，这次不删掉第四位 而是将其全部设置成1 让XYZ变成齐次坐标XYZ1
然后把之前的旋转矩阵以及平移向量拼接一下 得到变换矩阵
7.533745e-03 -9.999714e-01 -6.166020e-04 -4.069766e-03
1.480249e-02 7.280733e-04 -9.998902e-01 -7.631618e-02
9.998621e-01 7.523790e-03 1.480755e-02 -2.717806e-01
0.000000e+00 0.000000e+00 0.000000e+00 1.000000e+00
接下来用矩阵乘以点云里所有的点，就完成了点云从LiDAR坐标系到相机坐标系的变换
之后是相机坐标系到画像坐标系的变换
因为相机背面的点不会投影到相机上，所以我们删掉背面的点
而相机坐标系下，背面指的也就Z值小于0的部分
和前面的坐标变换一样，在有内参矩阵的情况下，乘以所有点就可以完成变换
但是这里偷个懒，继续使用我们的CV2.projectPoints
不过因为我们已经自己完成了到相机坐标系的变换，所以这时候外参就应该等于0
将旋转以及平移都设为0 0 0
接下来一样是处理一下画面外的点以及可视化
当然，两次的结果肯定是完全一致的，区别就是这次我们取得了点云在相机坐标系下的数据
可以来做我们的深度图了
深度图作成
上一步的最后我们用一个filter来去除了超出图片范围之外的无效点，同理用这个filter也删除对应点云里的点。
这样两边剩下的点就能按顺序一一对应
而我们想要的深度值是Z，那便取XYZ点云的第三位
接着用numpy创建一个和画像长宽一致的二维矩阵
把先前计算的像素坐标pixel里面填入对应的深度值，一张稀疏的深度图就做好了
用opencv保存后
RGB点云
取得深度图以后当然用途很多，不过你可以选择再一次将他投射成点云
不过这一次因为你知道了每个点对应图像的坐标值，可以从彩色图像中获取RGB值
也就可以投影出所谓的RGB点云
