Python实现激光雷达点云的向图像的重投影（世界坐标转换为像素坐标 + Autoware标定结果使用方法）- - 附源码 * 更新版 - -

Autonomous vehicle 杂谈_05

---

一. 写在前面

很久之前博主有写过一篇 Python实现三维世界坐标向二维像素坐标的转换（附源码） 的博文，但在最近重新研究过后，发现计算过程中确实有些纰漏，为了提醒自己，博主决定重新整理一下。

二. 使用Autoware获取自己的LiDAR&Camera校准结果

这里不加赘述，详细步骤可参考以下博文：
无人驾驶汽车系统入门（二十二）——使用Autoware实践激光雷达与摄像机组合标定
这篇文章写得很清晰，大家按照这个博文细心仔细地一步步来，一定是没有问题的。

三. 安装所需功能包：

1. 打开你的终端，安装过程紧跟博主的操作，前提是你在电脑上已经安装了Anaconda3。

1| conda create --name Open3D python=3.8  # 创建Open3D环境

2| conda activate Open3D  # 激活环境

3| conda install numpy  # 安装numpy

4| conda install matplotlib  # 安装matplotlib

5| conda install opencv  # 安装opencv

6| conda install -c open3d-admin open3d  # 安装Open3D

2. 检验是否安装成功（在终端依次输入以下内容）

1| python
2| import open3d

如果不报错，则证明安装成功。

四. 处理你所获得的标定结果

正常情况下你会得到一个像这样的标定结果：

%YAML:1.0
---
// 相机外参矩阵
CameraExtrinsicMat: !!opencv-matrix
   rows: 4
   cols: 4
   dt: d
   data: [ 6.4384993725688400e-02, -2.9614494224688315e-02,
       9.9748561609416664e-01, 7.0567131528775830e-03,
       -9.9779108558785690e-01, -1.8293328335903247e-02,
       6.3861597692206229e-02, 6.9659648287774212e-02,
       1.6356102969515951e-02, -9.9939398430759574e-01,
       -3.0726894171716257e-02, 7.3964965903196039e-02, 0., 0., 0., 1. ]

// 相机内参矩阵
CameraMat: !!opencv-matrix
   rows: 3
   cols: 3
   dt: d
   data: [ 6.0094877060462500e+02, 0., 3.0507696130640221e+02, 0.,
       6.1174212550675293e+02, 2.5274596287337977e+02, 0., 0., 1. ]

// 相机形变矩阵
DistCoeff: !!opencv-matrix
   rows: 1
   cols: 5
   dt: d
   data: [ 2.3030430710414049e-01, -9.1560321189489913e-01,
       1.0374975865423207e-02, -8.9662215743119679e-04,
       1.3506515085650497e+00 ]

// 做校准所用图片的分辨率
ImageSize: [ 640, 480 ] 

// 重投影精度 
ReprojectionError: 4.4973948452563883e-01

但是使用Autoware所获取的标定结果是不能直接使用的，你需要对其进行一定的处理！具体处理方法如下所示！

1. 对于相机外参矩阵中的旋转矩阵：首先你要对该矩阵进行转置处理，之后再进行罗德里格斯变换
2. 对于相机外参矩阵中的平移矩阵：你需要做一些换位处理
	t_true[0] = -t[2]
	t_true[1] = t[0]
	t_true[2] = t[1]
3. 对于相机内参矩阵和相机形变矩阵则不需要做任何改变

废话不多说，直接上代码：

import cv2
import numpy as np

# 原始旋转矩阵
R = np.float64([[6.4384993725688400e-02, -2.9614494224688315e-02, 9.9748561609416664e-01],
    [-9.9779108558785690e-01, -1.8293328335903247e-02, 6.3861597692206229e-02],
    [1.6356102969515951e-02, -9.9939398430759574e-01, -3.0726894171716257e-02]])

# 先做个转置
RT = np.transpose(R)
print(RT)

# 再求罗德里德斯变换
rvec = cv2.Rodrigues(RT)[0]
print(rvec)

# 结果
# [[ 1.27379905]
#  [-1.17541056]
#  [ 1.15989273]]


# 原始平移矩阵
t = np.mat([
    [7.0567131528775830e-03],
    [6.9659648287774212e-02],
    [7.3964965903196039e-02]
])

# 换位
t_true = np.float64([
    [-t[2]],
    [t[0]],
    [t[1]]
])

print(t_true)

# 得到的真实平移向量
# [[-0.07396497],
#  [0.00705671],
#  [0.06965965]]

五. 文件夹构造

六. 点云重投影（世界坐标转换为像素坐标）

步骤说明：

1. 使用open3d功能包中的函数，从 .pcd 文件中读取出激光雷达点云坐标
2. 声明各种内外参矩阵
3. 使用opencv功能包中的函数，进行世界坐标向像素坐标的转换
4. 点云重投影

废话不多说，直接上代码：

#-*- coding:utf-8 -*-
"""使用获取的相机内外参数实现重投影"""

import open3d as o3d
import cv2
from PIL import Image
from pylab import *
import matplotlib.pyplot as plt

# 读取pcd点云文件，保存为array数组
cloud = o3d.io.read_point_cloud('1.pcd')  # 需要准备自己的pcd文件
cloud = np.asarray(cloud.points)  # 改变点云的数据类型

# 输入 projectpoints 函数的各项参数数值

# 经过矩阵转置，以及罗德里格斯变换得到的旋转矩阵
rvec = np.float64([1.27379905, -1.17541056, 1.15989273])  

# 经过排序修改后得到的平移矩阵
tvec = np.float64([-0.07396497, 0.00705671, 0.06965965]) 

# 相机内部参数
camera_matrix = np.float64([[6.0094877060462500e+02, 0,  3.0507696130640221e+02],
                            [0, 6.1174212550675293e+02,  2.5274596287337977e+02],
                            [0, 0, 1]])  # 相机内部参数

# 相机形变矩阵
distCoeffs = np.float64([2.3030430710414049e-01, -9.1560321189489913e-01,
                         1.0374975865423207e-02, -8.9662215743119679e-04, 1.3506515085650497e+00]) 


# 进行点云由3D到2D的转换
point_2d, _ = cv2.projectPoints(cloud, rvec, tvec, camera_matrix, distCoeffs)

print(point_2d)


# 重投影绘制在图像上
im = Image.open('1.jpg')
x = []
y = []

m = -1
for point in point_2d:
    m = m+1
    x_2d = point[0][0]
    y_2d = point[0][1]

    if 0 <= x_2d <= 640 and 0 <= y_2d <= 480:
        x.append(x_2d)
        y.append(y_2d)

x = np.array(x)
y = np.array(y)
plt.scatter(x, y, s=1)
plt.imshow(im)
plt.show()

七. 结果展示

八. 感悟与分享

1. 学会如何处理从Autoware获取的标定结果十分重要。
2. 点云还携带距离信息，这个距离信息可以发挥出意想不到的作用。
3. 坐标转换过程，还是直接cv2.projectPoints来的更准确些。

---

如有问题，敬请指正。欢迎转载，但请注明出处。