【目标测距】雷达投影测距

前言

1. 雷达点云投影相机。图片目标检测，通过检测框约束等等对目标赋予距离。
2. 计算消耗较大，适合离线验证操作。在线操作可以只投影雷达检测框。

---

一、读取点云

• python 读取点云，我这里用的是 open3d 这个库。

import open3d as o3d

pcd_path = "1.pcd"
pcd = o3d.io.read_point_cloud(pcd_path)  # 点云
1
2
3
4

二、点云投影图片

• 明白标定原理，这部分就很简单，就是一个矩阵运算。投影像素误差多少与传感器标定强相关。
• 下面代码中 mtx：相机内参 r_camera_to_lidar_inv:相机到雷达的旋转矩阵的逆矩阵 t_camera_to_lidar:相机到雷达的平移向量
  

import open3d as o3d
import numpy as np
import cv2

color_label = [(255, 0, 0), (121, 68, 222), (0, 0, 255), (0, 255, 0), (199, 199, 53)]  # 红黄蓝绿青


# 不同距显示不同颜色
def get_color(distance):
    for i in range(2, 50):
        if i < distance < i + 1:
            return color_label[i % len(color_label)]
    return color_label[0]


pcd_path = "1.pcd"
pcd = o3d.io.read_point_cloud(pcd_path)  # 点云
image = cv2.imread("1.jpg")

cloud = np.asarray(pcd.points)
for point in cloud:
    camera_coordinate = np.dot(mtx, np.dot(r_camera_to_lidar_inv, point.reshape(3, 1) - t_camera_to_lidar))
    pixe_coordinate = camera_coordinate / camera_coordinate[2]
    x_pixe, y_pixe, _ = pixe_coordinate
    cv2.circle(image, (int(x_pixe), int(y_pixe)), 1, get_color(camera_coordinate[2]), 2)
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25

三、读取检测信息

• 图像目标检测信息保存在txt文件。格式： frame , x_center , y_cente , width , height , score。

import numpy as np


def GetDetFrameRes(seq_dets, frame):
    detects = seq_dets[(seq_dets[:, 0] == frame) & (seq_dets[:, 5] <= 6), 1:6]
    detects[:, 0:2] -= detects[:, 2:4] / 2  # convert to [x中心,y中心,w,h] to [x左上,y左上,w,h]
    detects[:, 2:4] += detects[:, 0:2]  # convert to [x左上,y左上,w,h] to [x左上,y左上,x右下,y右下]
    return detects


det_dir = "result.txt"
det_data = np.loadtxt(det_dir, delimiter=',')
# 假如有100帧图片
for i in range(100):
    dets_frame = GetDetFrameRes(det_data, i)  # 获取第i帧检测结果
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15

四、点云投影测距

• 判断点云是否在图像目标检测框内。
• 对于图片目标检测框有重复的情况，需要对目标检测框进行排列，距离靠前的检测框优先计算。
• 选取点云中 x 最小的为目标的距离，y 距离取目标框内平均值
  

import os
import cv2
import yaml
import numpy as np
import open3d as o3d
from datetime import datetime


def read_yaml(path):
    with open(path, 'r', encoding='utf-8') as f:
        result = yaml.load(f.read(), Loader=yaml.FullLoader)
    camera_mtx = result["camera"]["front_center"]["K"]
    r_camera = result["camera"]["front_center"]["rotation"]
    t_camera = result["camera"]["front_center"]["translation"]
    lidar_to_car = result["lidar"]["top_front"]["coordinate_transfer"]
    c_m = np.array([camera_mtx]).reshape(3, 3)
    r_c = np.array([r_camera]).reshape(3, 3)
    t_c = np.array([t_camera]).reshape(3, 1)
    l_c = np.array([lidar_to_car]).reshape(4, 4)
    return c_m, r_c, t_c, l_c


def get_box_color(index):
    color_list = [(96, 48, 176), (105, 165, 218), (18, 153, 255)]
    return color_list[index % len(color_list)]


# 不同距显示不同颜色
def get_color(distance):
    for i in range(2, 50):
        if i < distance < i + 1:
            return color_label[i % len(color_label)]
    return color_label[0]


def GetDetFrameRes(seq_dets, frame):
    detects = seq_dets[(seq_dets[:, 0] == frame) & (seq_dets[:, 5] <= 6), 1:6]
    detects[:, 0:2] -= detects[:, 2:4] / 2  # convert to [x中心,y中心,w,h] to [x左上,y左上,w,h]
    detects[:, 2:4] += detects[:, 0:2]  # convert to [x左上,y左上,w,h] to [x左上,y左上,x右下,y右下]
    return detects


# 点云投影到图片
def point_to_image(image_path, pcd_point, det_data, show=False):
    cloud = np.asarray(pcd_point.points)
    image = cv2.imread(image_path)
    det_data = det_data[np.argsort(det_data[:, 3])[::-1]]
    n = len(det_data)
    point_dict = {i: [] for i in range(n)}
    for point in cloud:
        if 2 < point[0] < 100 and -30 < point[1] < 30:
            camera_coordinate = np.dot(mtx, np.dot(r_camera_to_lidar_inv, point.reshape(3, 1) - t_camera_to_lidar))
            pixe_coordinate = camera_coordinate / camera_coordinate[2]
            x_pixe, y_pixe, _ = pixe_coordinate

            # 判断一个点是否在检测框里面
            idx = np.argwhere((x_pixe >= det_data[:, 0]) & (x_pixe <= det_data[:, 2]) &
                              (y_pixe >= det_data[:, 1]) & (y_pixe <= det_data[:, 3])).reshape(-1)
            if list(idx):
                index = int(idx[0])
                cv2.circle(image, (int(x_pixe), int(y_pixe)), 1, get_box_color(index), 2)
                point_dict[index].append([point[0], point[1]])

    for i in range(n):
        cv2.rectangle(image, (int(det_data[i][0]), int(det_data[i][1])), (int(det_data[i][2]), int(det_data[i][3])),
                      get_box_color(int(det_data[i][4])), 2)  # 不同类别画不同颜色框
        np_data = np.array(point_dict[i])
        if len(np_data) < 3:
            continue
        x = np.min(np_data[:, 0])
        min_index = np.argmin(np_data, axis=0)
        y = np.average(np_data[min_index, 1])
        cv2.putText(image, '{},{}'.format(round(x, 1), round(y, 1)), (int(det_data[i][0]), int(det_data[i][1]) - 10),
                    cv2.FONT_HERSHEY_COMPLEX, 1, get_box_color(int(det_data[i][4])), 2)
    video.write(image)
    if show:
        cv2.namedWindow("show", 0)
        cv2.imshow("show", image)
        cv2.waitKey(0)


def main():
    pcd_file_paths = os.listdir(pcd_dir)
    img_file_paths = os.listdir(img_dir)
    len_diff = max(0, len(pcd_file_paths) - len(img_file_paths))
    img_file_paths.sort(key=lambda x: float(x[:-4]))
    pcd_file_paths.sort(key=lambda x: float(x[:-4]))
    pcd_file_paths = [pcd_dir + x for x in pcd_file_paths]
    img_file_paths = [img_dir + x for x in img_file_paths]
    det_data = np.loadtxt(det_dir, delimiter=',')
    for i in range(min(len(img_file_paths), len(pcd_file_paths))):
        pcd = o3d.io.read_point_cloud(pcd_file_paths[i + len_diff])  # 点云
        now = datetime.now()
        dets_frame = GetDetFrameRes(det_data, i)
        point_to_image(img_file_paths[i], pcd, dets_frame, show=show)
        print(i, datetime.now() - now)
    video.release()


if __name__ == '__main__':
    color_label = [(255, 0, 0), (121, 68, 222), (0, 0, 255), (0, 255, 0), (199, 199, 53)]  # 红黄蓝绿青
    path = "F:\\data\\"
    pcd_dir = path + "lidar_points\\"  # 点云文件夹绝对路径
    img_dir = path + "image_raw\\"  # 图片文件夹绝对路径
    det_dir = path + "result.txt"  # 目标检测信息
    video_dir = path + "point_img4.mp4"
    video = cv2.VideoWriter(video_dir, cv2.VideoWriter_fourcc('m', 'p', '4', 'v'), 10, (1920, 1080))  # 保存视频
    cali_dir = "sensor.yaml"
    mtx, r_camera, t_camera, lidar_to_car = read_yaml(cali_dir)
    r_lidar, t_lidar = lidar_to_car[:3, :3], lidar_to_car[:3, -1].reshape(3, 1)
    r_camera_to_lidar = np.linalg.inv(r_lidar) @ r_camera
    r_camera_to_lidar_inv = np.linalg.inv(r_camera_to_lidar)
    t_camera_to_lidar = np.linalg.inv(r_lidar) @ (t_camera - t_lidar)  # 前相机，主雷达标定结果
    show = True
    main()

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116

五、学习交流

有任何疑问可以私信我，欢迎交流学习。