【目标测距】雷达投影测距
文章目录
- 前言
- 一、读取点云
- 二、点云投影图片
- 三、读取检测信息
- 四、点云投影测距
- 五、学习交流
前言
- 雷达点云投影相机。图片目标检测，通过检测框约束等等对目标赋予距离。
- 计算消耗较大，适合离线验证操作。在线操作可以只投影雷达检测框。
一、读取点云
- 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()
五、学习交流
有任何疑问可以私信我，欢迎交流学习。