需要配置PCD文件路径, 图片路径,相机内参,相机和雷达的外参; 单文件, Windows , liunx 都可以运行。
雷达和相机外参如何标定请看我的另外一篇标定的代码文章。
效果如下图:
附上代码:
# coding:utf-8
import cv2
import numpy as np
import open3d as o3d
def get_UV(matrix_in, matrix_out, x, y, z):
coordinate = np.array([[x], [y], [z], [1]])
result = np.dot(np.dot(matrix_in, matrix_out), coordinate)
u = result[0, 0]
v = result[1, 0]
depth = result[2, 0]
return u / depth, v / depth
def get_color(matrix_in, matrix_out, x, y, z, row, col, color_vector):
u, v = get_UV(matrix_in, matrix_out, x, y, z)
u = int(u)
v = int(v)
index = v * col + u
if index < row * col and index >= 0:
return color_vector[index]
e