专业术语光心、焦距、光轴、内参、外参光心Optical Center你可以把它理解为镜头组的“中心点”或者小孔成像里的那个“小孔”。所有光线在进入相机时都必须穿过这个点。在数学上它是相机坐标系的原点。光轴Optical Axis穿过光心且垂直于成像平面的那条虚拟直线。它代表了镜头最正、畸变最小的那个朝向。如何从三维世界坐标转换到图像上的像素点坐标为什么需要相机内外参相机坐标PXYZ-- 像素坐标uv相机坐标是以光心为原点的一个三维坐标而图像的像素坐标是以左上角为原心的二维坐标。因此我们首先需要先将三维坐标投影到一个与像素坐标平行的平面利用相似三角形原理我们不难得出先投影到归一化平面穿过P点距离光心1m远平面由于存在焦距为什么焦距会有两个参数因为CMOS传感器上的单个像素通常不是完美的正方形宽高比可能不一致而且工厂在生产时横向和纵向的缩放倍率略有不同平面转化完后我们还要平移原点引入主点此时整个过程如下求解过程中的关键参数我们可以把他写作 此时我们就引入了内参的第一个关键参数。但是上述的步骤是基于完美的“针孔模型”但实际镜头是凸透镜光线在边缘会弯曲产生畸变。因此我们还要引入畸变参数这是我们内参的第二个关键参数畸变分为径向畸变由ki修正和切向畸变由pi修正。世界坐标 -- 相机坐标世界坐标和相机坐标同样是三维的区别在与他们的原点位置不同旋转角度不同。因此我们需要先将相机原点转移到世界坐标原点也就是说你需要把世界上所有的点减去相机的位置。即先计算然后将相机的坐标轴通过旋转矩阵把坐标系对齐这样就是我们外参的第一个关键参数。此时我们可以写作然后我们将括号拆开我们把后面的记作这也就是我们外参的第二个关键参数。整个过程如下。有了理论依据以后我们不难发现我们需要得到相机的内外参才能实现坐标之间的转换这也是跨越两个不同坐标系之间的关键鸿沟。如何求解相机内参和外参通过之前的理论推导聪明的你肯定能发现诶如果我相机摆放的位置姿态不一样那么我的旋转矩阵R和平移向量T也会不一样是的你的直觉很灵敏。因此我们在使用相机作业之前都需要固定相机的位置然后在这个位置上进行相机的标定来求解最佳的外参。当然啦这样你又会思考既然外参会变那么内参呢我们的一台定焦相机的焦距f、主点偏移cx, cy 以及畸变系数k1, k2...在物理出厂后只要你不去暴力撞击或剧烈温度变化它们就是固定的物理属性。但是注意啦敲黑板工厂给的数据比如镜头标称“4mm焦距”是极其粗糙的根本无法用于精确的坐标转换。此外镜片在组装时不可能绝对垂直于CMOS光轴会有细微歪斜导致主点偏移。像素的物理尺寸不可能是完美的正方形导致 fx ! fy。注塑镜头的曲面有微小公差导致独特的多项式畸变系数。那么我要怎么才能求解这些参数呢对啦只要我能知道像素坐标和世界坐标那么这些中间参数不就迎刃而解了。假设你现在手里有一台搭配了双目相机的设备通过拍摄得到的图片可以知道像素坐标你还准备了一张平面图这张图上可以知道世界坐标我们通常把这张平面图用棋盘来替代。当你固定好相机的位置后拍摄得到了一张棋盘格的图像对于世界坐标系我们可以很容易得到棋盘格的每一个坐标是多少。比如每个格子的边长是30毫米你规定棋盘格的左上角第一个角点为原点0,0,0因为棋盘格是绝对平面的所以所有角点的Z 轴坐标永远为 0。于是你完全不需要看照片就能用笔在纸上列出所有角点的三维坐标第 1 个点左上角0,0,0第 2 个点右边相邻30,0,0... 第 2 行第 1 个点0,30,0。这里按照严格的顺序从左到右、从上到下那么像素坐标我们要怎么得到呢这里我们可以借助OpenCV的函数算法在照片里会自动提取出黑白交界的角点像素位置其中corners就是坐标点(u,v)ret, corners cv2.findChessboardCorners(img, pattern_size)如此这样我们是不是就得到了我们想要的世界坐标和像素坐标呢。然后我们只要借助函数cv2.fisheye.calibrate就能得到这里是用于鱼眼/广角相机模型普通相机可以用cv2.calibrateCameraret_L, K_L, D_L, rvecs_L, tvecs_L cv2.fisheye.calibrate( object_points, image_points_L, (1280, 720), K_init, D_init, flagscv2.fisheye.CALIB_RECOMPUTE_EXTRINSIC cv2.fisheye.CALIB_CHECK_COND cv2.fisheye.CALIB_FIX_SKEW )其中1280,720是image_sizeobject_points和imgge_points分别对应多张图像的世界坐标和多张图像的像素坐标。返回值retval一个重投影误差的值用于评估标定质量。K计算出的内参矩阵。D计算出的畸变系数。rvecs一个列表包含每张图片对应的旋转向量外参的一部分。tvecs一个列表包含每张图片对应的平移向量外参的另一部分。这样我们所需左目相机的内参KD就得到了。右目相机的内参求解同样如此。现在我们需要求解外参 R旋转矩阵和 T平移向量。具体来说是右目相机相对于左目相机的空间位置关系。# 注意这里输入的是左、右目的单目标定结果以及它们各自看到的角点 criteria (cv2.TERM_CRITERIA_EPS cv2.TERM_CRITERIA_MAX_ITER, 30, 1e-6) ret_stereo, K_L, D_L, K_R, D_R, R, T, E, F cv2.fisheye.stereoCalibrate( object_points, image_points_L, image_points_R, K_L, D_L, K_R, D_R, (1280, 720), criteriacriteria, flagscv2.fisheye.CALIB_FIX_INTRINSIC # 固定内参只优化外参 ) print(旋转矩阵 R (右目-左目):\n, R) print(平移向量 T (右目-左目):\n, T) # 单位: mm为什么要求解这两个的空间关系呢因为现实世界中左右相机的光轴不可能绝对平行我们需要在数学上把左右图像强行“拉”到严格水平对齐让左右图同一个物体在同一水平线上。这里我们引入一个关键参数重投影矩阵QQ矩阵不仅包含了焦距和基线还包含了校正后的主点偏移和左右目之间的微小旋转残差。求解Q矩阵的原因在于我们需要得到立体校正后的坐标具体操作如下# 校正变换 R_L, R_R, P_L, P_R, Q, _, _ cv2.fisheye.stereoRectify( K_L, D_L, K_R, D_R, (1280, 720), R, T, flagscv2.fisheye.CALIB_ZERO_DISPARITY, # 使校正后主点对齐 alpha0.5 # 保留多少原始视野 ) print(重投影矩阵 Q:\n, Q)需要注意的是我们这里求解的外参是为了得到校正旋转矩阵R_LR_R和新的投影矩阵P_LP_R以及重投影矩阵Q他与我们真正产线上运行外参不一致。这里可以思考一下为什么不一致好哒至此你已经得到了我们在推理时所有需要用到的标定参数calibration.yamlimport yaml calib_data { image_width: 1280, image_height: 720, K_L: K_L.tolist(), D_L: D_L.tolist(), K_R: K_R.tolist(), D_R: D_R.tolist(), R: R.tolist(), # 立体标定外参 T: T.tolist(), # 立体标定外参 R_L: R_L.tolist(), # 立体校正旋转 R_R: R_R.tolist(), P_L: P_L.tolist(), # 校正后的投影矩阵 (内参 平移) P_R: P_R.tolist(), Q: Q.tolist() } with open(calibration.yaml, w) as f: yaml.dump(calib_data, f)太好了现在你已经攻克了最枯燥的标定理论基础这是最难的部分恭喜你案例这里我们举一个案例辅助我们更好的学习。案例我现在手中拥有照片上某个对象的目标框的坐标已经通过目标检测得到我现在希望将其映射到三维点云的坐标系上。我们可以将整个过程梳理成五个核心阶段阶段一运行时初始化将车间标定好的数学关系载入内存也就是calibration.yaml文件为每一帧图像处理做准备。import cv2 import numpy as np import yaml # 1. 加载 calibration.yaml with open(calibration.yaml, r) as f: calib yaml.safe_load(f) # 2. 提取校正后的内参与映射参数 P_L np.array(calib[P_L]) # 3x4 左目投影矩阵 P_R np.array(calib[P_R]) # 3x4 右目投影矩阵 Q np.array(calib[Q]) # 4x4 重投影矩阵 K_rect P_L[:3, :3] # 提取校正后内参 (用于后续3D-2D投影) # 3. 预计算校正映射表 (鱼眼模型专用) map1_l, map2_l cv2.fisheye.initUndistortRectifyMap( K_L, D_L, R_L, P_L, (1280, 720), cv2.CV_32FC1) map1_r, map2_r cv2.fisheye.initUndistortRectifyMap( K_R, D_R, R_R, P_R, (1280, 720), cv2.CV_32FC1) # 4. 初始化立体匹配算法 (以SGBM为例) stereo cv2.StereoSGBM_create( minDisparity0, numDisparities64, # 必须是16的倍数 blockSize11, P18 * 3 * 11 ** 2, P232 * 3 * 11 ** 2, disp12MaxDiff1, uniquenessRatio10, speckleWindowSize100, speckleRange32 )这一步的关键在于我们需要得到矫正映射表map1_l, map2_l, map1_r, map2_r。map1存储的是“校正后的新像素 (u, v)应该去原始图像的哪个 x 坐标取值map2存储的是“应该去原始图像的哪个 y 坐标取值而初始化立体匹配算法是为了建立起双目测距的“搜索规则”和“代价函数”用来计算每个像素的视差值。在极线对齐之后左图的像素 (x, y) 对应右图的像素 (x - d, y)。这个差值d就是视差。StereoSGBM就是用来寻找这个d的工程化算法。阶段二图像预处理与立体校正这一阶段的目的在于将原始双目照片“拉直”消除畸变和极线倾斜为立体匹配和OD检测提供“干净”的图像。也就是使用阶段一的map得到行对齐的矫正图。# 输入原始左右目图像 imgL_raw, imgR_raw get_camera_frames() # 应用预计算映射表得到行对齐的校正图 imgL_rect cv2.remap(imgL_raw, map1_l, map2_l, cv2.INTER_LINEAR) imgR_rect cv2.remap(imgR_raw, map1_r, map2_r, cv2.INTER_LINEAR)阶段三稠密点云生成这一阶段的目的在于计算每个像素的深度生成相机坐标系下的3D点云。此时点云位于校正后的左目相机坐标系原点在左目光心。这个坐标系与imgL_rect的像素坐标系具有完美的几何对应关系。# 计算视差图 (输出为16位有符号整数) disparity_raw stereo.compute(imgL_rect, imgR_rect).astype(np.float32) # 关键陷阱SGBM输出的视差需要除以16才是真实视差值 disparity disparity_raw / 16.0 # 利用重投影矩阵Q将视差图转为3D坐标 (单位: 毫米) points_3d_cam cv2.reprojectImageTo3D(disparity, Q) # 输出形状: (H, W, 3)每个像素对应 (X, Y, Z)阶段四2D检测与视锥融合目的将2D检测框的类别标签赋给对应空间内的3D点云。# 关键必须在校正图 imgL_rect 上运行以保证与点云对齐 detections od_model.infer(imgL_rect) # 格式: [[x1, y1, x2, y2, class_id, score], ...]然后我们利用2D检测框作为“空间掩膜”从3D点云中“裁剪”出属于该障碍物的点并赋予其类别标签。最后得到的final_cloud是形状为N4 的 NumPy 数组每一行格式为[X, Y, Z, class_id]即带语义标签的 3D 障碍物点云可直接用于下游规划模块。points_3d_cam_raw points_3d_cam # 保留原始 (H, W, 3) for det in detections: x1, y1, x2, y2, cls, _ det # 1. 先裁剪出该框对应的 3D 区域 (H_box, W_box, 3) roi_3d points_3d_cam_raw[y1:y2, x1:x2] # 2. 在这个 roi 内部再展平并过滤无效点 roi_flat roi_3d.reshape(-1, 3) # 展平为 (N_box, 3) mask_roi (roi_flat[:, 2] 0) np.isfinite(roi_flat[:, 0]) valid_pts roi_flat[mask_roi] # 3. 如果框内没有有效点跳过比如误检或太远 if valid_pts.shape[0] 0: continue # 4. 赋予标签并存储 labels np.full((valid_pts.shape[0], 1), cls) semantic_points.append(np.hstack([valid_pts, labels])) # 5. 合并所有检测框 if semantic_points: final_cloud np.vstack(semantic_points) else: final_cloud np.empty((0, 4)) # 返回空数组防止程序崩溃阶段五坐标变换与输出目的把带着语义标签的目标点云从“以左目光心为原点”的局部坐标系搬移到“以车辆后轴中心或地面投影”的全局坐标系在车辆坐标系原点上的世界坐标系供规划控制模块使用。# 读取车间标定好的车辆外参 (R_veh, T_veh) R_veh np.array(calib[R_veh]) T_veh np.array(calib[T_veh]) # 将点云从相机坐标系转到世界坐标系 # 注意这里的外参是 世界-相机因此需要取逆变换 points_world (R_veh.T (final_cloud[:, :3].T - T_veh.reshape(3,1))).T # 最终输出结构: [X_world, Y_world, Z_world, class_id] output np.hstack([points_world, final_cloud[:, 3:4]]) 双目相机求解内参（K、D）、外参（R、T）
专业术语光心、焦距、光轴、内参、外参光心Optical Center你可以把它理解为镜头组的“中心点”或者小孔成像里的那个“小孔”。所有光线在进入相机时都必须穿过这个点。在数学上它是相机坐标系的原点。光轴Optical Axis穿过光心且垂直于成像平面的那条虚拟直线。它代表了镜头最正、畸变最小的那个朝向。如何从三维世界坐标转换到图像上的像素点坐标为什么需要相机内外参相机坐标PXYZ-- 像素坐标uv相机坐标是以光心为原点的一个三维坐标而图像的像素坐标是以左上角为原心的二维坐标。因此我们首先需要先将三维坐标投影到一个与像素坐标平行的平面利用相似三角形原理我们不难得出先投影到归一化平面穿过P点距离光心1m远平面由于存在焦距为什么焦距会有两个参数因为CMOS传感器上的单个像素通常不是完美的正方形宽高比可能不一致而且工厂在生产时横向和纵向的缩放倍率略有不同平面转化完后我们还要平移原点引入主点此时整个过程如下求解过程中的关键参数我们可以把他写作 此时我们就引入了内参的第一个关键参数。但是上述的步骤是基于完美的“针孔模型”但实际镜头是凸透镜光线在边缘会弯曲产生畸变。因此我们还要引入畸变参数这是我们内参的第二个关键参数畸变分为径向畸变由ki修正和切向畸变由pi修正。世界坐标 -- 相机坐标世界坐标和相机坐标同样是三维的区别在与他们的原点位置不同旋转角度不同。因此我们需要先将相机原点转移到世界坐标原点也就是说你需要把世界上所有的点减去相机的位置。即先计算然后将相机的坐标轴通过旋转矩阵把坐标系对齐这样就是我们外参的第一个关键参数。此时我们可以写作然后我们将括号拆开我们把后面的记作这也就是我们外参的第二个关键参数。整个过程如下。有了理论依据以后我们不难发现我们需要得到相机的内外参才能实现坐标之间的转换这也是跨越两个不同坐标系之间的关键鸿沟。如何求解相机内参和外参通过之前的理论推导聪明的你肯定能发现诶如果我相机摆放的位置姿态不一样那么我的旋转矩阵R和平移向量T也会不一样是的你的直觉很灵敏。因此我们在使用相机作业之前都需要固定相机的位置然后在这个位置上进行相机的标定来求解最佳的外参。当然啦这样你又会思考既然外参会变那么内参呢我们的一台定焦相机的焦距f、主点偏移cx, cy 以及畸变系数k1, k2...在物理出厂后只要你不去暴力撞击或剧烈温度变化它们就是固定的物理属性。但是注意啦敲黑板工厂给的数据比如镜头标称“4mm焦距”是极其粗糙的根本无法用于精确的坐标转换。此外镜片在组装时不可能绝对垂直于CMOS光轴会有细微歪斜导致主点偏移。像素的物理尺寸不可能是完美的正方形导致 fx ! fy。注塑镜头的曲面有微小公差导致独特的多项式畸变系数。那么我要怎么才能求解这些参数呢对啦只要我能知道像素坐标和世界坐标那么这些中间参数不就迎刃而解了。假设你现在手里有一台搭配了双目相机的设备通过拍摄得到的图片可以知道像素坐标你还准备了一张平面图这张图上可以知道世界坐标我们通常把这张平面图用棋盘来替代。当你固定好相机的位置后拍摄得到了一张棋盘格的图像对于世界坐标系我们可以很容易得到棋盘格的每一个坐标是多少。比如每个格子的边长是30毫米你规定棋盘格的左上角第一个角点为原点0,0,0因为棋盘格是绝对平面的所以所有角点的Z 轴坐标永远为 0。于是你完全不需要看照片就能用笔在纸上列出所有角点的三维坐标第 1 个点左上角0,0,0第 2 个点右边相邻30,0,0... 第 2 行第 1 个点0,30,0。这里按照严格的顺序从左到右、从上到下那么像素坐标我们要怎么得到呢这里我们可以借助OpenCV的函数算法在照片里会自动提取出黑白交界的角点像素位置其中corners就是坐标点(u,v)ret, corners cv2.findChessboardCorners(img, pattern_size)如此这样我们是不是就得到了我们想要的世界坐标和像素坐标呢。然后我们只要借助函数cv2.fisheye.calibrate就能得到这里是用于鱼眼/广角相机模型普通相机可以用cv2.calibrateCameraret_L, K_L, D_L, rvecs_L, tvecs_L cv2.fisheye.calibrate( object_points, image_points_L, (1280, 720), K_init, D_init, flagscv2.fisheye.CALIB_RECOMPUTE_EXTRINSIC cv2.fisheye.CALIB_CHECK_COND cv2.fisheye.CALIB_FIX_SKEW )其中1280,720是image_sizeobject_points和imgge_points分别对应多张图像的世界坐标和多张图像的像素坐标。返回值retval一个重投影误差的值用于评估标定质量。K计算出的内参矩阵。D计算出的畸变系数。rvecs一个列表包含每张图片对应的旋转向量外参的一部分。tvecs一个列表包含每张图片对应的平移向量外参的另一部分。这样我们所需左目相机的内参KD就得到了。右目相机的内参求解同样如此。现在我们需要求解外参 R旋转矩阵和 T平移向量。具体来说是右目相机相对于左目相机的空间位置关系。# 注意这里输入的是左、右目的单目标定结果以及它们各自看到的角点 criteria (cv2.TERM_CRITERIA_EPS cv2.TERM_CRITERIA_MAX_ITER, 30, 1e-6) ret_stereo, K_L, D_L, K_R, D_R, R, T, E, F cv2.fisheye.stereoCalibrate( object_points, image_points_L, image_points_R, K_L, D_L, K_R, D_R, (1280, 720), criteriacriteria, flagscv2.fisheye.CALIB_FIX_INTRINSIC # 固定内参只优化外参 ) print(旋转矩阵 R (右目-左目):\n, R) print(平移向量 T (右目-左目):\n, T) # 单位: mm为什么要求解这两个的空间关系呢因为现实世界中左右相机的光轴不可能绝对平行我们需要在数学上把左右图像强行“拉”到严格水平对齐让左右图同一个物体在同一水平线上。这里我们引入一个关键参数重投影矩阵QQ矩阵不仅包含了焦距和基线还包含了校正后的主点偏移和左右目之间的微小旋转残差。求解Q矩阵的原因在于我们需要得到立体校正后的坐标具体操作如下# 校正变换 R_L, R_R, P_L, P_R, Q, _, _ cv2.fisheye.stereoRectify( K_L, D_L, K_R, D_R, (1280, 720), R, T, flagscv2.fisheye.CALIB_ZERO_DISPARITY, # 使校正后主点对齐 alpha0.5 # 保留多少原始视野 ) print(重投影矩阵 Q:\n, Q)需要注意的是我们这里求解的外参是为了得到校正旋转矩阵R_LR_R和新的投影矩阵P_LP_R以及重投影矩阵Q他与我们真正产线上运行外参不一致。这里可以思考一下为什么不一致好哒至此你已经得到了我们在推理时所有需要用到的标定参数calibration.yamlimport yaml calib_data { image_width: 1280, image_height: 720, K_L: K_L.tolist(), D_L: D_L.tolist(), K_R: K_R.tolist(), D_R: D_R.tolist(), R: R.tolist(), # 立体标定外参 T: T.tolist(), # 立体标定外参 R_L: R_L.tolist(), # 立体校正旋转 R_R: R_R.tolist(), P_L: P_L.tolist(), # 校正后的投影矩阵 (内参 平移) P_R: P_R.tolist(), Q: Q.tolist() } with open(calibration.yaml, w) as f: yaml.dump(calib_data, f)太好了现在你已经攻克了最枯燥的标定理论基础这是最难的部分恭喜你案例这里我们举一个案例辅助我们更好的学习。案例我现在手中拥有照片上某个对象的目标框的坐标已经通过目标检测得到我现在希望将其映射到三维点云的坐标系上。我们可以将整个过程梳理成五个核心阶段阶段一运行时初始化将车间标定好的数学关系载入内存也就是calibration.yaml文件为每一帧图像处理做准备。import cv2 import numpy as np import yaml # 1. 加载 calibration.yaml with open(calibration.yaml, r) as f: calib yaml.safe_load(f) # 2. 提取校正后的内参与映射参数 P_L np.array(calib[P_L]) # 3x4 左目投影矩阵 P_R np.array(calib[P_R]) # 3x4 右目投影矩阵 Q np.array(calib[Q]) # 4x4 重投影矩阵 K_rect P_L[:3, :3] # 提取校正后内参 (用于后续3D-2D投影) # 3. 预计算校正映射表 (鱼眼模型专用) map1_l, map2_l cv2.fisheye.initUndistortRectifyMap( K_L, D_L, R_L, P_L, (1280, 720), cv2.CV_32FC1) map1_r, map2_r cv2.fisheye.initUndistortRectifyMap( K_R, D_R, R_R, P_R, (1280, 720), cv2.CV_32FC1) # 4. 初始化立体匹配算法 (以SGBM为例) stereo cv2.StereoSGBM_create( minDisparity0, numDisparities64, # 必须是16的倍数 blockSize11, P18 * 3 * 11 ** 2, P232 * 3 * 11 ** 2, disp12MaxDiff1, uniquenessRatio10, speckleWindowSize100, speckleRange32 )这一步的关键在于我们需要得到矫正映射表map1_l, map2_l, map1_r, map2_r。map1存储的是“校正后的新像素 (u, v)应该去原始图像的哪个 x 坐标取值map2存储的是“应该去原始图像的哪个 y 坐标取值而初始化立体匹配算法是为了建立起双目测距的“搜索规则”和“代价函数”用来计算每个像素的视差值。在极线对齐之后左图的像素 (x, y) 对应右图的像素 (x - d, y)。这个差值d就是视差。StereoSGBM就是用来寻找这个d的工程化算法。阶段二图像预处理与立体校正这一阶段的目的在于将原始双目照片“拉直”消除畸变和极线倾斜为立体匹配和OD检测提供“干净”的图像。也就是使用阶段一的map得到行对齐的矫正图。# 输入原始左右目图像 imgL_raw, imgR_raw get_camera_frames() # 应用预计算映射表得到行对齐的校正图 imgL_rect cv2.remap(imgL_raw, map1_l, map2_l, cv2.INTER_LINEAR) imgR_rect cv2.remap(imgR_raw, map1_r, map2_r, cv2.INTER_LINEAR)阶段三稠密点云生成这一阶段的目的在于计算每个像素的深度生成相机坐标系下的3D点云。此时点云位于校正后的左目相机坐标系原点在左目光心。这个坐标系与imgL_rect的像素坐标系具有完美的几何对应关系。# 计算视差图 (输出为16位有符号整数) disparity_raw stereo.compute(imgL_rect, imgR_rect).astype(np.float32) # 关键陷阱SGBM输出的视差需要除以16才是真实视差值 disparity disparity_raw / 16.0 # 利用重投影矩阵Q将视差图转为3D坐标 (单位: 毫米) points_3d_cam cv2.reprojectImageTo3D(disparity, Q) # 输出形状: (H, W, 3)每个像素对应 (X, Y, Z)阶段四2D检测与视锥融合目的将2D检测框的类别标签赋给对应空间内的3D点云。# 关键必须在校正图 imgL_rect 上运行以保证与点云对齐 detections od_model.infer(imgL_rect) # 格式: [[x1, y1, x2, y2, class_id, score], ...]然后我们利用2D检测框作为“空间掩膜”从3D点云中“裁剪”出属于该障碍物的点并赋予其类别标签。最后得到的final_cloud是形状为N4 的 NumPy 数组每一行格式为[X, Y, Z, class_id]即带语义标签的 3D 障碍物点云可直接用于下游规划模块。points_3d_cam_raw points_3d_cam # 保留原始 (H, W, 3) for det in detections: x1, y1, x2, y2, cls, _ det # 1. 先裁剪出该框对应的 3D 区域 (H_box, W_box, 3) roi_3d points_3d_cam_raw[y1:y2, x1:x2] # 2. 在这个 roi 内部再展平并过滤无效点 roi_flat roi_3d.reshape(-1, 3) # 展平为 (N_box, 3) mask_roi (roi_flat[:, 2] 0) np.isfinite(roi_flat[:, 0]) valid_pts roi_flat[mask_roi] # 3. 如果框内没有有效点跳过比如误检或太远 if valid_pts.shape[0] 0: continue # 4. 赋予标签并存储 labels np.full((valid_pts.shape[0], 1), cls) semantic_points.append(np.hstack([valid_pts, labels])) # 5. 合并所有检测框 if semantic_points: final_cloud np.vstack(semantic_points) else: final_cloud np.empty((0, 4)) # 返回空数组防止程序崩溃阶段五坐标变换与输出目的把带着语义标签的目标点云从“以左目光心为原点”的局部坐标系搬移到“以车辆后轴中心或地面投影”的全局坐标系在车辆坐标系原点上的世界坐标系供规划控制模块使用。# 读取车间标定好的车辆外参 (R_veh, T_veh) R_veh np.array(calib[R_veh]) T_veh np.array(calib[T_veh]) # 将点云从相机坐标系转到世界坐标系 # 注意这里的外参是 世界-相机因此需要取逆变换 points_world (R_veh.T (final_cloud[:, :3].T - T_veh.reshape(3,1))).T # 最终输出结构: [X_world, Y_world, Z_world, class_id] output np.hstack([points_world, final_cloud[:, 3:4]])