点云投影图片

标定结果使用

使用calibration_toolkit工具获取的标定文件无法直接使用，需要进行转化

相机外参矩阵中的旋转矩阵，首先需要进行转置，然后再进行罗德里格斯变换
相机外参矩阵中的平移矩阵，需要做进行换位处理
对于相机内参矩阵和相机形变矩阵则不需要做任何改变

CameraExtrinsicMat = [
			[R[0][0], R[1][0], R[2][0], t[1]],
			[R[0][1], R[1][1], R[2][1], t[2]],
			[R[0][2], R[1][2], R[2][2], -t[0]],
			[0., 0., 0., 1.]]

transfrom.py

import cv2
import numpy as np


# 先做个转置
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]
])

# 换位
# autoware在得到平移矩阵后进行了如下操作：
# camera_velodyne_translation.x = -camera_velodyne_point.z;
# camera_velodyne_translation.y = camera_velodyne_point.x;
# camera_velodyne_translation.z = camera_velodyne_point.y;
t_true = np.float64([[t[1]], [t[2]], [-t[0]]])
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()