相机图片给 Livox 激光雷达点云赋色（python代码 单文件）

需要配置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]
    else: