律师事务所网站建设重要性,金蝶直播软件,怎么做营销,html5国内网站文章目录 前言一、读取点云二、点云投影图片三、读取检测信息四、点云投影测距五、学习交流 前言
雷达点云投影相机。图片目标检测#xff0c;通过检测框约束等等对目标赋予距离。计算消耗较大#xff0c;适合离线验证操作。在线操作可以只投影雷达检测框。 一、读取点云 py… 文章目录 前言一、读取点云二、点云投影图片三、读取检测信息四、点云投影测距五、学习交流 前言
雷达点云投影相机。图片目标检测通过检测框约束等等对目标赋予距离。计算消耗较大适合离线验证操作。在线操作可以只投影雷达检测框。 一、读取点云 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, encodingutf-8) as f:result yaml.load(f.read(), Loaderyaml.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, showFalse):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, axis0)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(keylambda x: float(x[:-4]))pcd_file_paths.sort(keylambda 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, showshow)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.mp4video cv2.VideoWriter(video_dir, cv2.VideoWriter_fourcc(m, p, 4, v), 10, (1920, 1080)) # 保存视频cali_dir sensor.yamlmtx, 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()
五、学习交流
有任何疑问可以私信我欢迎交流学习。
