目录
LiDAR点云转深度图
KERO
编辑于 2023年12月07日 17:47

之前写了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里面直接上传到了根目录

代码块
Python
自动换行
复制代码
import numpy as np
import cv2
import matplotlib.pyplot as plt


pointcloud = np.fromfile("/content/0000000150.bin",dtype=np.float32).reshape(-1,4)
pointcloud = pointcloud[:,:3].astype(np.float64)
image = cv2.imread("/content/0000000150.png")
复制成功

kitti数据集的LiDAR数据有4位 分别是 X Y Z 反射率，反射率本次暂时用不到 所以只取前三位

    

投影变换 1

想要做出深度图，首先得将点云给投影到图像上，取得像素坐标值

一个简单的方法是利用opencv的函数 cv2.projectPoints

函数需要的参数有

  1. 点云

  2. 点云坐标系到相机坐标系的旋转矩阵

  3. 点云坐标系到相机坐标系的平移向量

  4. 相机内参

  5. 相机畸变参数

其实就是所谓的 外参（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示例配置文件

代码块
Python
自动换行
复制代码
rotation = np.array([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],dtype="float").reshape(3,3)
translation = np.array([-4.069766e-03, -7.631618e-02, -2.717806e-01],dtype="float").reshape(1,3)
distortion = np.array([[0,0,0,0]],dtype="float")
camera = np.array((721.5377,0,609.5593,
        0,721.5377,172.854,
        0,0,1),dtype="float").reshape(3,3)
复制成功

并且，相机的视角是朝前方的 后面的点云自然不可能投影到相机上，所以这里把点云中在后方的点去掉，也就是X为负的部分

代码块
Python
自动换行
复制代码
a = np.where(pointcloud[:,0]>=0)
pointcloud = pointcloud[a]
复制成功

接着把上面的参数全部传入cv2.projectPoints

就可以得到点云投影在画像上的像素坐标，因为像素坐标是只有整数 所以转为整数型

代码块
Python
自动换行
复制代码
reTransform = cv2.projectPoints(pointcloud,rotation,translation,camera,distortion)
reTransform = reTransform[0][:,0].astype(int)
复制成功

刚刚去除了LiDAR中心后半部分的点

因为相机有FOV 视野的限制，所以前半部分的点也并不是全部都能投影到画像上

所以这里把坐标超出画像的无效点给去除

代码块
Python
自动换行
复制代码
pixel = reTransform
filter = np.where((pixel[:,0]<1242)&(pixel[:,1]<375)&(pixel[:,0]>=0)&(pixel[:,1]>=0))
pixel = pixel[filter]
复制成功

画出来观察一下

代码块
Python
自动换行
复制代码
plt.scatter(pixel[:,0], pixel[:,1] , s=100)
plt.imshow(image)
复制成功

看起来没毛病。虽然仔细看有的点也并不是和物体严丝合缝，比如右手边的胖大叔

不过这应该是LIDAR数据的采集时间和拍照时间有细微的时间差

数据集里面有时间戳，可以看得出两个数据之间略微有时间差

投影变换 2

刚才利用了opencv的函数一下就完成了点云到画像的投影。

不过很可惜这样只能知道点云对应的画像坐标，并不能达到我们的目的——做出深度图

为了做出深度图 除了点云的画像坐标以外，我们还需要每个点的深度

而深度——depth的定义，是一个点在相机坐标系下的Z值

相机坐标系和画像坐标系

刚才opencv的函数拿了相机和LiDAR的外参以及内参

一口气完成了如下的变换

LiDAR坐标系——外参——相机坐标系——内参——画像坐标系

而为了得到深度值，我们需要点群在相机坐标系下的坐标

所以先来手动写一下前面这部分变换

在齐次坐标系下 坐标系和坐标系的变换可以写成

中间的那个矩阵也就是相机的外参矩阵 也叫三维空间几何变换矩阵

其中 左上角的3x3为旋转矩阵 而右边下3x1为平移向量

重新导入我们的LiDAR点云，这次不删掉第四位 而是将其全部设置成1 让XYZ变成齐次坐标XYZ1

代码块
Python
自动换行
复制代码
pointcloud = np.fromfile("/content/0000000150.bin",dtype=np.float32).reshape(-1,4)
pointcloud[:,3] = 1
复制成功

然后把之前的旋转矩阵以及平移向量拼接一下 得到变换矩阵

代码块
Python
自动换行
复制代码
transform = np.hstack((rotation,translation.reshape(3,-1)))
transform = np.vstack((transform,np.array([0,0,0,1])))
复制成功

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坐标系到相机坐标系的变换

代码块
Python
自动换行
复制代码
for i in range(pointcloud.shape[0]):
    pointcloud[i] = np.dot(transform,pointcloud[i].reshape(4,-1)).reshape(-1,4)

pointcloud=pointcloud[:,:3]
复制成功

之后是相机坐标系到画像坐标系的变换

因为相机背面的点不会投影到相机上，所以我们删掉背面的点

而相机坐标系下，背面指的也就Z值小于0的部分

代码块
Python
自动换行
复制代码
a = np.where(pointcloud[:,2]>=0)
pointcloud = pointcloud[a]
复制成功

和前面的坐标变换一样，在有内参矩阵的情况下，乘以所有点就可以完成变换

但是这里偷个懒，继续使用我们的cv2.projectPoints

不过因为我们已经自己完成了到相机坐标系的变换，所以这时候外参就应该等于0

将旋转以及平移都设为0 0 0

代码块
Python
自动换行
复制代码
rotation=np.array([0,0,0],dtype="float").reshape(3,1)
translation=np.array([0,0,0],dtype="float").reshape(1,3)

reTransform_2=cv2.projectPoints(pointcloud,rotation,translation,camera,distortion)
reTransform_2 = reTransform_2[0][:,0].astype(int)
复制成功

接下来一样是处理一下画面外的点以及可视化

代码块
Python
自动换行
复制代码
pixel = reTransform_2
filter = np.where((pixel[:,0]<1242)&(pixel[:,1]<375)&(pixel[:,0]>=0)&(pixel[:,1]>=0))
pixel = pixel[filter]

plt.scatter(pixel[:,0], pixel[:,1] , s=100)
plt.imshow(image)
复制成功

当然，两次的结果肯定是完全一致的，区别就是这次我们取得了点云在相机坐标系下的数据 

可以来做我们的深度图了

深度图作成

上一步的最后我们用一个filter来去除了超出图片范围之外的无效点，同理用这个filter也删除对应点云里的点。

这样两边剩下的点就能按顺序一一对应

而我们想要的深度值是Z，那便取XYZ点云的第三位

代码块
Python
自动换行
复制代码
depth = pointcloud[:,2].reshape(-1,1)[filter]
复制成功

接着用numpy创建一个和画像长宽一致的二维矩阵

把先前计算的像素坐标pixel里面填入对应的深度值，一张稀疏的深度图就做好了

代码块
Python
自动换行
复制代码
depth_image=np.full(image.shape[0:2],np.nan)
depth_image[pixel[:,1],pixel[:,0]] = depth[:,0]
复制成功

用opencv保存后

RGB点云

取得深度图以后当然用途很多，不过你可以选择再一次将他投射成点云

不过这一次因为你知道了每个点对应图像的坐标值，可以从彩色图像中获取RGB值

也就可以投影出所谓的RGB点云

cv18420092