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

  • Post author:
  • Post category:python




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
  1. 检验是否安装成功(在终端依次输入以下内容)
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]]



五. 文件夹构造

bydemo



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

步骤说明:

  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()



七. 结果展示
bydemo



八. 感悟与分享

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


    cv2.projectPoints


    来的更准确些。



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



版权声明:本文为weixin_40247876原创文章,遵循 CC 4.0 BY-SA 版权协议,转载请附上原文出处链接和本声明。