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

文章目录 前言一、读取点云二、点云投影图片三、读取检测信息四、点云投影测距五、学习交流

前言 雷达点云投影相机。图片目标检测，通过检测框约束等等对目标赋予距离。计算消耗较大，适合离线验证操作。在线操作可以只投影雷达检测框。 一、读取点云 python 读取点云，我这里用的是 open3d 这个库。 import open3d as o3d pcd_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 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) 三、读取检测信息 图像目标检测信息保存在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帧检测结果 四、点云投影测距 判断点云是否在图像目标检测框内。对于图片目标检测框有重复的情况，需要对目标检测框进行排列，距离靠前的检测框优先计算。选取点云中 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() 五、学习交流

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