将激光雷达点云转换为和RGB图相对应的深度图（RGB-D）

---

步骤

1、匹配时间戳最相近的图片和点云文件，一张图片对应一个点云文件（或者一张图片对应多个点云文件，多张图片对应一个点云文件也可以），时间戳越相近越好，如果时间差比较大，需要做一些线性运动方程更新。
2、将点云文件投影到图片上形成深度图。

---

参考文献

1、深度图中的深度图值和点的真实距离之间的关系（相对深度与绝对深度，深度图与真实距离）
2、不同激光雷达的x、y、z轴和相机的x、y、z轴的方向可能不同，所以外参可能不匹配（Mark一下～激光雷达点云投影到图像的方法（基于autoware的lidar_camera_calibration,外参不匹配的一些坑））
3、三维点云到图像的投影保存和显示（.pcd格式的3维点云到图像平面的投影）
4、OpenCV 中的 cv.ProjectPoints() 函数的原理（OpenCV 2.4.13.7 documentation » OpenCV API Reference » calib3d. Camera Calibration and 3D Reconstruction »）
5、旋转向量和旋转矩阵的互相转换（旋转向量和旋转矩阵的互相转换（python cv2.Rodrigues()函数））
6、激光点云投影到图像的原理和具体步骤（自动驾驶视觉融合-相机校准与激光点云投影）
7、python_kitti_激光点云变换到图像平面并保存成int16灰度图（python_kitti_激光点云变换到图像平面并保存成int16灰度图）

---

1.1、匹配时间戳最相近的图片和点云文件（一张图片对应一个点云文件）

全部代码如下（python）：

import os
import shutil

#图片和点云文件的文件名都是各自对应的时间戳
image_path = "E:\\shu_ju\\selection\\2021-03-03-15-57-52\\image2\\"
pcd_path = "E:\\shu_ju\\selection\\2021-03-03-15-57-52\\pcd(Python)\\"
pcd_copy_path = "E:\\shu_ju\\selection\\2021-03-03-15-57-52\\pcd_1\\"

image_list = os.listdir(image_path)
pcd_list = os.listdir(pcd_path)
k = 0
for i in image_list:
    image_time = float(i[:-4])  #去掉文件名后缀，读取图片的时间戳
    min_time = 10
    min_pcd = ''
    for j in pcd_list:
        pcd_time = float(j[:-4])  #去掉文件名后缀，读取点云文件的时间戳
        if abs(image_time - pcd_time) < min_time:
            min_time = abs(image_time - pcd_time)
            min_pcd = j
    pcd_copy = pcd_path + min_pcd
    shutil.copy(pcd_copy, pcd_copy_path)  #复制指定文件到指定路径下
    os.rename(pcd_copy_path+min_pcd, pcd_copy_path+i[:-4]+'.txt')  #重命名指定文件
    print(k, min_time)
    k += 1

---

1.2、匹配时间戳最相近的图片和点云文件（一张图片对应三个点云文件）

全部代码如下（python）：

import os
import shutil

#图片和点云文件的文件名都是各自对应的时间戳
image_path = "E:\\shu_ju\\selection\\2021-03-03-15-17-24\\image2\\"
pcd_path = "E:\\shu_ju\\selection\\2021-03-03-15-17-24\\pcd(Python)\\"
pcd_copy_path = "E:\\shu_ju\\selection\\2021-03-03-15-17-24\\pcd_3\\"

image_list = os.listdir(image_path)
pcd_list = os.listdir(pcd_path)
k = 0
for i in image_list:
    image_time = float(i[:-4])  #去掉文件名后缀，读取图片的时间戳
    min_time = [10]
    min_pcd = ['']
    for j in pcd_list:
        pcd_time = float(j[:-4])  #去掉文件名后缀，读取点云文件的时间戳
        if len(min_time) < 3:
            min_time.append(abs(image_time - pcd_time))
            min_pcd.append(j)
        elif abs(image_time - pcd_time) < max(min_time):
            min_time.pop(min_time.index(max(min_time)))
            min_time.append(abs(image_time - pcd_time))
            min_pcd.pop(min_time.index(max(min_time)))
            min_pcd.append(j)
    for j in min_pcd:
        pcd_copy = pcd_path + j
        shutil.copy(pcd_copy, pcd_copy_path)  #复制指定文件到指定路径下
        os.rename(pcd_copy_path+j, pcd_copy_path+str(k)+'.txt')  #重命名指定文件
        print(k, max(min_time))
        k += 1

---

2、将点云文件投影到图片上形成深度图。

全部代码如下（python）：

# -*- coding: utf-8 -*-
import os
import cv2
import numpy as np
import matplotlib.image as mpimg
import scipy.misc
import sys
import matplotlib.pyplot as plt
from PIL import Image
import math

pcd_path = "E:\\shu_ju\\selection\\2021-03-03-15-57-52\\pcd_1\\"  #点云文件为txt格式
shendu_path = "E:\\shu_ju\\selection\\2021-03-03-15-57-52\\shendu\\"  #深度图为16位png格式

image_h = 1200  #RGB图和深度图的高度
image_w = 1920  #RGB图和深度图的宽度
xuanzhuan = np.float64([[ 0.99943258, 0.02277891, 0.02481223],
                      [ 0.02716343, -0.10949544, -0.99361607],
                      [-0.01991666, 0.99372625, -0.11005207]])  #旋转矩阵
xuanzhuan_1 = np.float64([1.680647483853886, 0.03782614262625567, 0.003707885488685594])  #旋转向量
pingyi = np.float64([-0.3270823619145787, 1.994427053985835, -0.2688515838179673])  #平移向量
neican = np.float64([[1400.660521866737, 0, 989.6663020916587],
                   [0, 1397.477295771064, 594.9904177802305],
                   [0, 0, 1]])  #相机内参矩阵
jibian = np.float64([-0.05694747808562394, -0.08329212973455258, -0.0009314071183112955, 0.006712153417379347, 0.2493801178842133])  #相机畸变系数，[k1, k2, p1, p2, k3]
pcd_list = os.listdir(pcd_path)

for i in pcd_list:
    pcd = pcd_path + i
    data = np.loadtxt(pcd)
    
    #image_array = np.zeros((image_h, image_w), dtype=np.int32)
    image_array = np.zeros((image_h, image_w), dtype=np.uint16)  #深度图是以uint16格式存储的，16位png
    															#（深度图一定要特意保存为16位的png格式，默认是8位的，精度不够）
    k = 0
    k_1 = 0
    for j in data:
        if j[1] > 0:  #j是点的三维坐标，顺序分别是x、y、z，所以j[1]是点的y轴坐标，
        			  #我们的激光雷达的坐标系是x轴正方向向右，y轴正方向向前，z轴正方向向上，
        			  #所以 j[1]>0 是为了过滤掉后面的点，只保留前面的点，
        			  #你们用的时候到底是j[1]>0，还是j[0]>0或者j[2]>0，要根据你们向前的坐标轴是y轴，还是x轴或者z轴来确定。
            #print(j)
            #qici = np.array([j[0], j[1], j[2], 1.0])
            qici = np.float64(j)
            #print(qici)
            
            #三维点投影到图片上，得到的点的二维坐标，2×1向量，第一个数是x，x轴是图片的宽，右为x轴正方向
            #第二个数是y，y轴是图片的高，下为y轴正方向
            point_2d, _ = cv2.projectPoints(qici, xuanzhuan_1, pingyi, neican, jibian)
            point_2d_sque = np.squeeze(point_2d)  #删除多余的维度
            #print(point_2d_sque.shape)
            
            zuobiao_0 = np.dot(xuanzhuan, qici.T)
            zuobiao_01 = zuobiao_0 + pingyi
            #print(zuobiao_0.shape)      
            zuobiao_1 = np.dot(neican, zuobiao_01)
            #print(zuobiao_1)
            
            z = zuobiao_1[2] * 256  #z是深度图中点的相对深度，zuobiao_1[2]是点的绝对深度（真实距离），单位是米，256是深度图相对深度与真实距离的系数
            						#（https://blog.csdn.net/weixin_41423872/article/details/117522856）
            '''
            u = int(round(zuobiao_1[0] / zuobiao_1[2]))  #没有经过畸变矫正的点的x坐标
            v = int(round(zuobiao_1[1] / zuobiao_1[2]))  #没有经过畸变矫正的点的y坐标
            '''
            u = int(round(point_2d_sque[0]))  #经过畸变矫正的点的x坐标
            v = int(round(point_2d_sque[1]))  #经过畸变矫正的点的y坐标
            if u >= 0 and u < image_w and v >= 0 and v < image_h:  #注意宽是图片的x轴，正方向向右，高是图片的y轴，正方向向下
                k_1 += 1  #投影后在图片范围内的点的数量
               
                #！！！
                #注意是先 v，后 u!!! np.array的 x 轴向下，y 轴向右，和前面的正好相反！！！
                #！！！
                if image_array[v][u] == 0:  #去除投影到图片上同一坐标的重复点
                    image_array[v][u] = z
                    k += 1  #去除重复点后，投影到图片上的真实的点的数量
    
    #（深度图一定要特意保存为16位的png格式，默认是8位的，精度不够）
    img = scipy.misc.toimage(image_array, high=np.max(image_array), low=np.min(image_array), mode='I')  #保存16位的png深度图
    img.save(shendu_path + i[:-4] + ".png")
    print(i)
    print(k)
    print(k_1)