雷达到相机的重投影

实现条件

1. 相机图像、内参矩阵[3×3]、形变参数[1×5]
2. 雷达点云
3. 二者外参[4×4], 包含旋转、平移

上代码

导入库文件

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

IMAGE_FILE = "/home/txz/数据集/0318_1613/1650/png/000.png"
PCD_FILE   = "/home/txz/数据集/0318_1613/1650/pcd/1679129443.995743513.pcd"
extrinsic  = "/home/txz/lidar_camera_ws/data_set/result/extrinsic.txt"

cloud = o3d.io.read_point_cloud(PCD_FILE)  # 需要准备自己的pcd文件
cloud = np.asarray(cloud.points)

# 参数提取
L2C_MAT = np.loadtxt(extrinsic, delimiter=",",dtype=np.float32)
rot_mat = L2C_MAT[:3,:3]
# 经过矩阵转置，以及罗德里格斯变换得到的旋转矩阵
rvec,_ = cv2.Rodrigues(rot_mat)
# 经过排序修改后得到的平移矩阵
tvec  = L2C_MAT[:3,3]

# 相机内参
camera_matrix = np.array([909.5165940000001, 0, 613.6350650000001, 0, 913.203517, 396.620893, 0, 0, 1]).reshape(-1,3)
# 相机形变
distCoeffs = np.array([0.08947899999999999, -0.06453299999999999, -0.002094, -0.011071, 0])

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



# 重投影绘制在图像上
im = cv2.imread(IMAGE_FILE, cv2.IMREAD_COLOR)
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 <= 1280 and 0 <= y_2d <= 720:
        x.append(x_2d)
        y.append(y_2d)

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

原始图像

投影结果