激光雷达点云与图像配准-投影(雷达投影相机/相机投影雷达)

小拳拳锤你胸口

已于 2024-05-29 17:41:07 修改

阅读量451

点赞数 2

分类专栏：点云文章标签： python

于 2024-05-28 16:21:00 首次发布

原文链接：https://blog.csdn.net/qq_43627520/article/details/128686512?spm=1001.2014.3001.5502

版权

点云专栏收录该内容

2 篇文章

订阅专栏

相机投影雷达

import numpy as np
import cv2
import open3d as o3d


def rgb_vis(point_cloud):
    vis = o3d.visualization.Visualizer()
    vis.create_window()
    vis.add_geometry(point_cloud)
    vis.run()
    vis.destroy_window()


def main():
    # 文件读取
    path = "2.pcd"
    imgPath = "2.jpeg"

    # 读取点云
    pcd = o3d.io.read_point_cloud(path)
    point_cloud = np.asarray(pcd.points)

    # 读取图像
    img = cv2.imread(imgPath)
    if img.shape[2] != 3:
        print("RGB pics needed.")
        return

    # 相机内参 3x3
    K = np.array([[1.9506592935364870e+03, 0.0, 1.9770478473401959e+03],
                  [0.0, 1.9508117738745232e+03, 1.0786204201895550e+03],
                  [0.0, 0.0, 1.0]])

    # 相机内参 3x4
    camera_par = np.array([[1.9506592935364870e+03, 0.0, 1.9770478473401959e+03, 0],
                           [0.0, 1.9508117738745232e+03, 1.0786204201895550e+03, 0],
                           [0.0, 0.0, 1.0, 0]])

    # 相机畸变参数 k1 k2 t1 t2 k3
    D = np.array([-0.050519061674533024, -0.007992982752507883, 0.00970045657644595, -0.004354775040194558, 0.0])

    # 去畸变
    UndistortImage = cv2.undistort(img, K, D)
    rows, cols = UndistortImage.shape[:2]

    # 雷达坐标到相机坐标变换矩阵
    t_word_to_cam = np.array(
        [[2.4747462378258280e-02, -9.9955232303502073e-01, -1.6839925611563663e-02, -9.2541271346932907e-02],
         [-1.3087302341509554e-02, 1.6519577885364300e-02, -9.9977861656954914e-01, 2.4302538338292576e+00],
         [9.9960858363250360e-01, 2.4962312041639460e-02, -1.2672595327247342e-02, -5.0924142692133323e+00],
         [0., 0., 0., 1.]])

    # 世界坐标》相机坐标》像素坐标
    colors = []
    for point in point_cloud:
        c_x, c_y, c_z = point
        word_h = np.array([c_x, c_y, c_z, 1])
        p_result = camera_par @ t_word_to_cam @ word_h

        p_w = p_result[2]
        p_u = int(p_result[0] / p_w)
        p_v = int(p_result[1] / p_w)

        if 0 <= p_u < cols and 0 <= p_v < rows and p_w > 0:
            b, g, r = UndistortImage[p_v, p_u]
            colors.append([r / 255.0, g / 255.0, b / 255.0])
        else:
            colors.append([1.0, 1.0, 1.0])

    pcd.colors = o3d.utility.Vector3dVector(colors)

    # 保存处理后的点云为新的PCD文件
    output_path = "colored_output.pcd"
    o3d.io.write_point_cloud(output_path, pcd)
    print(f"Colored point cloud saved to {output_path}")

    # 可视化处理后的点云
    rgb_vis(pcd)


if __name__ == "__main__":
    main()

雷达投影相机

import numpy as np
import cv2
import open3d as o3d

def rgb_vis(point_cloud):
    vis = o3d.visualization.Visualizer()
    vis.create_window()
    vis.add_geometry(point_cloud)
    vis.run()
    vis.destroy_window()

def main():
    # 文件读取
    path = "2.pcd"
    imgPath = "2.png"

    # 读取点云
    pcd = o3d.io.read_point_cloud(path)
    point_cloud = np.asarray(pcd.points)

    # 读取图像
    img = cv2.imread(imgPath)
    if img.shape[2] != 3:
        print("RGB pics needed.")
        return

    # 相机内参 3x3
    K = np.array([
        [1591.79363371, 0, 1192.55683638],
        [0, 1592.95157283, 735.66773825],
        [0, 0, 1]
    ])

    # 相机内参 3x4
    # 相机内参扩展为3x4 (assuming no skew and principal point at the center)
    camera_par = np.concatenate((K, np.zeros((3, 1))), axis=1)

    # 相机畸变参数 k1 k2 t1 t2 k3
    D = np.array([-0.342347892791610, 0.107009428540231, 0, 0, 0])

    # 去畸变
    UndistortImage = cv2.undistort(img, K, D)
    rows, cols = UndistortImage.shape[:2]

    # 雷达坐标到相机坐标变换矩阵
    t_word_to_cam = np.array(
        [
            [-0.066931452111297, -0.997397056468959, 0.026819628359261, -0.183806356422023],
            [0.242404431640872, -0.042329624814497, -0.969251409276216, -0.354916895554880],
            [0.967863767396600, -0.058372207514532, 0.244606649849797, 0.0103779925979218],
            [0, 0, 0, 1]
        ]
        )

    # 创建一个复制的图像用于绘制投影点
    img_with_points = UndistortImage.copy()

    # 世界坐标》相机坐标》像素坐标
    for point in point_cloud:
        c_x, c_y, c_z = point
        word_h = np.array([c_x, c_y, c_z, 1])
        p_result = camera_par @ t_word_to_cam @ word_h

        p_w = p_result[2]
        p_u = int(p_result[0] / p_w)
        p_v = int(p_result[1] / p_w)

        if 0 <= p_u < cols and 0 <= p_v < rows and p_w > 0:
            cv2.circle(img_with_points, (p_u, p_v), 2, (0, 0, 255), -1)

    # 保存带有投影点的图像
    output_image_path = "projected_points_image.jpeg"
    cv2.imwrite(output_image_path, img_with_points)
    print(f"Projected points image saved to {output_image_path}")

    # 显示图像
    cv2.imshow("Projected Points", img_with_points)
    cv2.waitKey(0)
    cv2.destroyAllWindows()

if __name__ == "__main__":
    main()

画框

import numpy as np
import cv2
import open3d as o3d
from PIL import ImageFont, ImageDraw, Image


def rgb_vis(point_cloud):
    vis = o3d.visualization.Visualizer()
    vis.create_window()
    vis.add_geometry(point_cloud)
    vis.run()
    vis.destroy_window()


def put_chinese_text(image, text, position, font_path, font_size, color):
    img_pil = Image.fromarray(image)
    draw = ImageDraw.Draw(img_pil)
    font = ImageFont.truetype(font_path, font_size, encoding="utf-8")
    draw.text(position, text, font=font, fill=color)

    return np.array(img_pil)


def get_box_points(center, length, width, height):
    l, w, h = length / 2, width / 2, height / 2
    cx, cy, cz = center

    points = [
        [cx - l, cy - w, cz - h],
        [cx - l, cy + w, cz - h],
        [cx + l, cy - w, cz - h],
        [cx + l, cy + w, cz - h],
        [cx - l, cy - w, cz + h],
        [cx - l, cy + w, cz + h],
        [cx + l, cy - w, cz + h],
        [cx + l, cy + w, cz + h]
    ]

    return points


def draw_cube_edges(image, points, color):
    edges = [
        (0, 1), (1, 3), (3, 2), (2, 0),
        (4, 5), (5, 7), (7, 6), (6, 4),
        (0, 4), (1, 5), (2, 6), (3, 7)
    ]

    for edge in edges:
        start_point = points[edge[0]]
        end_point = points[edge[1]]
        cv2.line(image, start_point, end_point, color, 2)

    return image


def main():
    # 文件读取
    path = "2.pcd"
    imgPath = "2.jpeg"
    font_path = "SimSun.ttf"  # 指定支持中文的字体文件路径

    # 读取点云
    pcd = o3d.io.read_point_cloud(path)
    point_cloud = np.asarray(pcd.points)

    # 读取图像
    img = cv2.imread(imgPath)
    if img.shape[2] != 3:
        print("RGB pics needed.")
        return

    # 相机内参 3x3
    K = np.array([[1.9506592935364870e+03, 0.0, 1.9770478473401959e+03],
                  [0.0, 1.9508117738745232e+03, 1.0786204201895550e+03],
                  [0.0, 0.0, 1.0]])

    # 相机内参 3x4
    camera_par = np.array([[1.9506592935364870e+03, 0.0, 1.9770478473401959e+03, 0],
                           [0.0, 1.9508117738745232e+03, 1.0786204201895550e+03, 0],
                           [0.0, 0.0, 1.0, 0]])

    # 相机畸变参数 k1 k2 t1 t2 k3
    D = np.array([-0.050519061674533024, -0.007992982752507883, 0.00970045657644595, -0.004354775040194558, 0.0])

    # 去畸变
    UndistortImage = cv2.undistort(img, K, D)
    rows, cols = UndistortImage.shape[:2]

    # 雷达坐标到相机坐标变换矩阵
    t_word_to_cam = np.array(
        [[2.4747462378258280e-02, -9.9955232303502073e-01, -1.6839925611563663e-02, -9.2541271346932907e-02],
         [-1.3087302341509554e-02, 1.6519577885364300e-02, -9.9977861656954914e-01, 2.4302538338292576e+00],
         [9.9960858363250360e-01, 2.4962312041639460e-02, -1.2672595327247342e-02, -5.0924142692133323e+00],
         [0., 0., 0., 1.]])

    # 定义中心点和立方体尺寸
    center = [25.912266, 3.457932, 1.448326]
    length, width, height = 10.0, 6.0, 8.0  # 自定义立方体的尺寸

    # 获取立方体的八个角点
    box_points = get_box_points(center, length, width, height)

    box_points1 = get_box_points([26.912266, 4.457932, 2.448326], 7, 4, 5)
    # 定义框的点和描述信息
    boxes = [
        {
            "points": box_points,
            "description": "距离:3.8米 物体:鬼魂"
        },
        # 添加更多的框和描述
        {
            "points": box_points1,
            "description": "距离:3.8米 物体:卑微小吴"
        },
    ]

    # 创建一个复制的图像用于绘制投影点
    img_with_points = UndistortImage.copy()

    for box in boxes:
        projected_points = []
        for point in box["points"]:
            c_x, c_y, c_z = point
            word_h = np.array([c_x, c_y, c_z, 1])
            p_result = camera_par @ t_word_to_cam @ word_h

            p_w = p_result[2]
            p_u = int(p_result[0] / p_w)
            p_v = int(p_result[1] / p_w)

            if 0 <= p_u < cols and 0 <= p_v < rows and p_w > 0:
                projected_points.append((p_u, p_v))

        if projected_points:
            # 绘制立方体的边
            img_with_points = draw_cube_edges(img_with_points, projected_points, (0, 255, 0))
            # 添加文字
            text_position = (int(projected_points[0][0]), int(projected_points[0][1]) - 50)  # 调整文字位置
            img_with_points = put_chinese_text(img_with_points, box["description"], text_position, font_path, 30,
                                               (255, 0, 0))  # 增加字体大小

    # 保存带有投影点的图像
    output_image_path = "projected_points_image.jpeg"
    cv2.imwrite(output_image_path, img_with_points)
    print(f"Projected points image saved to {output_image_path}")

    # 显示图像
    cv2.imshow("Projected Points", img_with_points)
    cv2.waitKey(0)
    cv2.destroyAllWindows()


if __name__ == "__main__":
    main()