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

前言

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

---

一、读取点云

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

import open3d as o3dpcd_path = "1.pcd"
pcd = o3d.io.read_point_cloud(pcd_path)  # 点云

二、点云投影图片

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

import open3d as o3d
import numpy as np
import cv2color_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_coordinatecv2.circle(image, (int(x_pixe), int(y_pixe)), 1, get_color(camera_coordinate[2]), 2)

三、读取检测信息

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

import numpy as npdef 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 detectsdet_dir = "result.txt"
det_data = np.loadtxt(det_dir, delimiter=',')
# 假如有100帧图片
for i in range(100):dets_frame = GetDetFrameRes(det_data, i)  # 获取第i帧检测结果

四、点云投影测距

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

import os
import cv2
import yaml
import numpy as np
import open3d as o3d
from datetime import datetimedef 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_cdef 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:continuex = 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_camerar_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 = Truemain()

五、学习交流

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