前言
在自动驾驶领域,激光雷达(LiDAR)技术因其高精度的三维空间测量能力而变得至关重要。除了点云数据,激光雷达系统还能提供丰富的标签数据,这些数据对于对象识别和场景理解非常重要。本文将探讨如何将这些标签数据有效地投影到二维图片上,以便于分析和可视化。
投影原理
激光雷达数据通常以点云的形式存在,每个点包含三维坐标(X, Y, Z)。要将这些点投影到二维图像上,需要利用相机模型和外参(即两个传感器之间的相对位置和姿态),遵循针孔相机模型的基本原理。关键步骤包括:
-
坐标变换:首先,将激光雷达的点云坐标从激光雷达坐标系转换到相机坐标系。这涉及到旋转和平移操作,通常由外参矩阵(包括旋转矩阵R和平移向量T)完成。
-
深度映射:接着,利用相机内参(焦距f、主点坐标(cx, cy))将相机坐标系下的三维点转换为归一化图像坐标(u, v)。
-
畸变校正:最后,考虑镜头畸变(径向和切向畸变),将归一化坐标映射到实际像素坐标,并进行必要的畸变。
投影代码示例
以下是一个简化的Python代码示例,展示了如何将激光雷达点云数据投影到图像上。
1、获取内外参数
def obtain_camera_param(yaml_file_path):
# 使用FileStorage打开YAML文件,使用READ标志读取文件
fs = cv2.FileStorage(yaml_file_path, cv2.FILE_STORAGE_READ)
if not fs.isOpened():
print("Error: Could not open YAML file.")
exit()
# 读取相机矩阵
camera_matrix = fs.getNode('CameraMat').mat()
camera_extrin_matrix = fs.getNode('CameraExtrinsicMat').mat()
# 读取畸变系数
dist_coeffs = fs.getNode('DistCoeff').mat()
# 打印参数
print("Camera Matrix:\n", camera_matrix)
print("Distortion Coefficients:\n", dist_coeffs)
print("Camera extri Matrinx:\n", camera_extrin_matrix)
# 关闭FileStorage对象
fs.release()
return camera_matrix, dist_coeffs, camera_extrin_matrix
2、使用标签获取角点信息
def get_3d_box_corners(x, y, z, l, w, h, yaw):
center = [x, y, z ]
size = [l, w, h]
rot = np.asmatrix([[math.cos(yaw), -math.sin(yaw)],\
[math.sin(yaw), math.cos(yaw)]])
plain_pts = np.asmatrix([[0.5 * size[0], 0.5*size[1]],\
[0.5 * size[0], -0.5*size[1]],\
[-0.5 * size[0], -0.5*size[1]],\
[-0.5 * size[0], 0.5*size[1]]])
tran_pts = np.asarray(rot * plain_pts.transpose())
tran_pts = tran_pts.transpose()
corners = np.arange(24).astype(np.float32).reshape(8, 3)
for i in range(8):
corners[i][0] = center[0] + tran_pts[i%4][0]
corners[i][1] = center[1] + tran_pts[i%4][1]
corners[i][2] = center[2] + (float(i >= 4) - 0.5) * size[2]
return corners
3、将角点投影到图片上
def project_to_2d(points_3d, camera_matrix, dist_coeffs, camera_extrin_matrix):
R = camera_extrin_matrix[0:3, 0:3]
T = camera_extrin_matrix[0:3, 3]
points_3d = points_3d.astype(np.float32)
points_2d, _ = cv2.projectPoints(points_3d, R, T, camera_matrix, dist_coeffs)
return points_2d.reshape(-1, 2)
4、结果可视化
源码下载:
关注我的公众号auto_driver_ai(Ai fighting), 第一时间获取更新内容。