★ FEATURED ARTICLE 1. 项目概述接触 Intel Realsense D435i 有一段时间了这相机在我手里已经配合机械臂做过好几种抓取方案也搭过三维重建的采集系统。先说结论D435i 最大的价值不是“能测距”而是它把深度获取的门槛降到了“拿来就能用”的程度但真正落地时大多数人卡住的往往是最后一公里——怎么把深度图里的一个像素变成真实世界里的一个三维坐标点。这正是这篇文章想解决的问题。如果你在使用 D435i 做机械臂抓取、目标定位、三维重建或者 AR 应用一定遇到过这种情况在彩色图里框选了一个物体中心拿到像素坐标 (u, v)然后呢这个点在空间里到底在哪机械臂该往哪儿抓如果对坐标转换的理解只停留在“调用某个函数”或者“复制某段代码”一旦换一台相机、换一个安装位置代码立刻失效定位精度也会变得没法看。读完全文你会掌握三件事第一从像素坐标到三维空间坐标的数学原理知道每个参数从哪来、为什么需要它第二在 D435i 上用 Python 和 pyrealsense2 完整实现坐标转换的代码流程包括单点反投影和批量点云生成第三我在实际项目中踩过的坑尤其是对齐、内参和坐标系外参那些容易出错的地方。这篇文章适合的读者刚入手 D435i 但看不懂官方例程的初学者正在做机械臂视觉抓取或机器人导航的开发者以及想把 RGB-D 数据用于三维重建但搞不清坐标关系的人。已经有了基础经验的话可以直接跳到第 4 节看实现或者第 5 节看避坑。2. 从像素到三维空间必须吃透的数学底子2.1 针孔相机模型要搞懂坐标转换务必先理解相机是怎么把人眼看到的世界变成一张照片的。所有相机不管多贵核心都抽象成针孔模型三维空间中的点通过光心投射到成像平面上画面因此是倒立的但算法里通常会做一次翻转方便处理。先记住几个关键坐标系坐标系符号原点位置单位像素坐标系(u, v)图像左上角pixel图像坐标系(x, y)光轴与成像平面交点mm相机坐标系(Xc, Yc, Zc)相机光心mm 或 m世界坐标系(Xw, Yw, Zw)自定义mm 或 mD435i 输出的深度图每个像素值代表的是该像素位置对应空间点到相机平面的距离严格说是到相机光心所在平面的深度值 Zc单位是毫米。深度图本质上就是一张记录了 Zc 值的二维矩阵。像素坐标到三维坐标的核心公式如下。已知像素坐标 (u, v) 和深度值 Zc可以反投影得到相机坐标系下的三维点Xc (u - cx) * Zc / fx Yc (v - cy) * Zc / fy Zc depth_value其中 fx、fy 是焦距单位是像素cx、cy 是主点坐标也叫光心像素坐标。这一组参数就叫相机内参。注意公式里有个细节Xc、Yc 的单位由 Zc 决定。如果 Zc 单位是米那算出来的 Xc、Yc 就是米如果 Zc 单位是毫米算出来就是毫米。做机械臂抓取时我一般统一用米省得后续转换出错。2.2 为什么需要内参标定很多人以为 D435i 出厂就能直接用不用标定。这话对了一半D435i 确实出厂时做过标定相机内部存了一套工厂标定参数直接用 SDK 读就行。但工厂标定解决的是“这台相机自己的焦距和光心是多少”不解决“相机装在机械臂上、装在车上的位置姿态是什么”。如果把深度相机装到机械臂上采集到的三维点是在相机坐标系下的。机械臂要抓东西需要的是物体在机械臂基坐标系或工具坐标系下的坐标。这就需要一个手眼标定过程求相机坐标系到机械臂坐标系的旋转矩阵 R 和平移向量 T。这部分的坑我后面会详细讲先记住一个核心结论像素坐标到相机坐标靠内参相机坐标到机器人坐标靠外参。二者缺一不可。2.3 D435i的深度测量原理D435i 属于主动双目立体视觉传感器。它的正面有两个红外摄像头和一个红外点阵投射器。工作时投射器发射不可见红外光斑给墙面、桌面这些没有纹理的区域“人为制造”特征点两个红外摄像头从不同角度拍摄这些光斑通过三角测量原理计算视差从而得到每个像素的深度。这解释了 D435i 为什么在有光照变化的室外场景容易翻车——强红外干扰会让光斑识别变得困难。但在室内、近距离0.2m~3m场景下它的精度表现非常稳定这也是它成为机械臂抓取热门选择的原因。D435i 的深度传感器分辨率最高支持 1280x720但实际项目中我更常用 640x480帧率 30fps。分辨率越高单帧数据量越大USB 带宽占用越高反而可能造成帧率下降或数据丢帧。3. 实战准备D435i环境搭建与数据获取3.1 安装 pyrealsense2官方提供的 Python 包是 pyrealsense2安装非常简单pip install pyrealsense2如果你的 Python 环境是 3.8 以上一般直接装就行。但这些年我发现Windows 下有时会遇到 DLL 加载失败多半是 Intel Realsense SDK Runtime 版本和 Python 包版本不匹配去官网下载对应版本的 Intel.RealSense.SDK 安装一次就好了。Linux 环境下建议从源码编译 pyrealsense2或者直接安装官方发布的 deb 包再 pip 安装。Ubuntu 20.04 下直接用 pip 装通常也行前提是内核支持 UVC 设备。3.2 打开相机并获取对齐的深度图与彩色图D435i 同时具备深度传感器和 RGB 传感器但这两个传感器在硬件上有个物理距离导致同一时刻它们拍到的图像视角略有差异。直接用原始深度图和彩色图做像素级对应误差会比较大。所以需要做对齐align操作将深度图映射到彩色图的视角下或者反过来。在 pyrealsense2 中用rs.align实现import pyrealsense2 as rs pipeline rs.pipeline() config rs.config() config.enable_stream(rs.stream.depth, 640, 480, rs.format.z16, 30) config.enable_stream(rs.stream.color, 640, 480, rs.format.bgr8, 30) profile pipeline.start(config) # 对齐将深度图对齐到彩色图 align_to rs.stream.color align rs.align(align_to)这里有个关键概念对齐后的深度图其像素坐标系与彩色图完全一致。也就是说彩色图里 (u, v) 位置的颜色对应深度图里 (u, v) 位置的深度值描述的是同一个空间点。这正是后续做像素坐标转三维坐标的前提。3.3 获取深度传感器的内参内参怎么读官方 SDK 提供了接口depth_sensor profile.get_device().first_depth_sensor() depth_scale depth_sensor.get_depth_scale() print(depth_scale , depth_scale) # 等待获取一帧数据 frames pipeline.wait_for_frames() depth_frame frames.get_depth_frame() # 获取深度内参 depth_profile depth_frame.get_profile() depth_intrinsics depth_profile.as_video_stream_profile().get_intrinsics() print(depth_intrinsics)打印出来的depth_intrinsics包含 fx fy ppx ppy也就是 cx cy以及畸变系数。这就是我们要用的内参。depth_scale这个值非常关键。D435i 默认深度图每个像素存储的是 unsigned int 16 位整数但这个整数不一定直接就是毫米数而需要乘以 depth_scale 才能换算成米。不同固件版本、不同配置下depth_scale 通常是 0.001表示每个单位对应 0.001 米1 毫米。如果用depth_frame.get_distance(u, v)这个方法它返回的直接就是单位为米的值SDK 内部已经帮你做了换算。但如果你直接操作 numpy 数组取深度值就要手动乘以 depth_scale。我在实际项目里遇到过因为忘记乘 depth_scale导致计算出物体距离差了 1000 倍的情况整个机械臂差点往天上抓这种低级错误踩过一次就不会忘了。4. 像素坐标到三维坐标的实现与代码解析4.1 单点反投影从 (u, v) 到 (X, Y, Z)下面实现完整的从单个像素坐标到三维坐标的转换。假设我们已经拿到对齐后的深度帧和彩色帧并且想要知道彩色图中点 (320, 240) 对应的三维坐标。import pyrealsense2 as rs import numpy as np # 流水线初始化 pipeline rs.pipeline() config rs.config() config.enable_stream(rs.stream.depth, 640, 480, rs.format.z16, 30) config.enable_stream(rs.stream.color, 640, 480, rs.format.bgr8, 30) profile pipeline.start(config) align rs.align(rs.stream.color) try: while True: frames pipeline.wait_for_frames() aligned_frames align.process(frames) aligned_depth_frame aligned_frames.get_depth_frame() color_frame aligned_frames.get_color_frame() if not aligned_depth_frame or not color_frame: continue depth_intrinsics aligned_depth_frame.profile.as_video_stream_profile().get_intrinsics() # 像素坐标 u, v 320, 240 # 获取该像素的深度值单位是米 depth_value aligned_depth_frame.get_distance(u, v) # 判断深度值是否有效0表示该位置没有测量到深度 if depth_value 0: print(该像素点没有有效的深度数据) continue # 核心反投影像素坐标到相机坐标 # SDK 提供内置方法 rs2_deproject_pixel_to_point point_3d rs.rs2_deproject_pixel_to_point( depth_intrinsics, [u, v], depth_value ) print(f像素坐标 ({u}, {v}), 深度 {depth_value:.3f} m) print(f相机坐标系下三维坐标: X{point_3d[0]:.3f}, Y{point_3d[1]:.3f}, Z{point_3d[2]:.3f}) finally: pipeline.stop()rs.rs2_deproject_pixel_to_point输入的参数分别是内参、像素坐标列表、深度值米。返回的point_3d是一个三维坐标单位与深度值一致。如果你不用 SDK 内置函数手写公式也是分分钟的事fx depth_intrinsics.fx fy depth_intrinsics.fy cx depth_intrinsics.ppx cy depth_intrinsics.ppy Zc depth_value # 单位米 Xc (u - cx) * Zc / fx Yc (v - cy) * Zc / fy两种方式结果一致。理解公式的好处是当遇到 SDK 版本变化、不同编程语言、甚至换用其他品牌相机时你都能快速迁移。4.2 批量转换从深度图生成三维点云单个点坐标在很多场景不够比如三维重建、点云处理需要把整张深度图全部转换成三维点云。方法就是遍历每个像素做反投影但 Python 逐像素 for 循环效率太低一帧 640x480 的图要跑很久。正确的做法是用 NumPy 实现向量化运算def depth_image_to_point_cloud(depth_image, intrinsics, depth_scale): h, w depth_image.shape fx intrinsics.fx fy intrinsics.fy cx intrinsics.ppx cy intrinsics.ppy # 构建像素坐标网格 v, u np.mgrid[0:h, 0:w] depth depth_image.astype(np.float32) * depth_scale # 无效深度点深度值为0或距离太远 mask depth 0 X (u - cx) * depth / fx Y (v - cy) * depth / fy Z depth points np.stack((X, Y, Z), axis-1) # 返回每个像素对应的三维坐标无效点标记为 [0,0,0] return points, mask这段代码只做了很少的操作但速度比 for 循环快几个数量级。实测 640x480 的深度图逐像素循环大概要 5 秒向量化以后不到 50 毫秒。如果你想要更省事pyrealsense2 还内置了点云生成功能pc rs.pointcloud() points pc.calculate(aligned_depth_frame) vtx np.asanyarray(points.get_vertices()).view(np.float32).reshape(-1, 3)用这种方式生成的点云坐标单位同样是米。点云数据可以直接配 Open3D 做可视化import open3d as o3d pcd o3d.geometry.PointCloud() pcd.points o3d.utility.Vector3dVector(vtx) o3d.visualization.draw_geometries([pcd])4.3 彩色图与点云的颜色绑定只有三维坐标还不够直观把彩色信息贴到点云上看起来才像回事。Open3D 提供了方便的做法color_image np.asanyarray(color_frame.get_data()) color_image cv2.cvtColor(color_image, cv2.COLOR_BGR2RGB) colors color_image.reshape(-1, 3).astype(np.float32) / 255.0 pcd.colors o3d.utility.Vector3dVector(colors)前提是深度图已经对齐到彩色图否则坐标和颜色对应不上。4.4 像素坐标到世界坐标引入外参上面所有转换结果都在相机坐标系下。如果只是看看点云、测距离、做三维重建这个坐标系完全够用。但一旦涉及机器人抓取就必须把点转换到机械臂坐标系。设相机坐标系到机械臂基坐标系的变换为旋转矩阵 $R$ 和平移向量 $T$则P_robot R * P_camera T$P_camera$ 是相机坐标系下的三维点$P_robot$ 是机械臂坐标系下的三维点。R 和 T 怎么求有两种常见方式第一种手眼标定。用棋盘格或 ArUco 标定板同时获取机械臂末端位姿和标定板在相机坐标系下的位姿通过 AXXB 方程求解。这个方法精度高但实现起来有一定工作量。D435i 的手眼标定可以参考 eye-in-hand 的标准流程常用的视觉库是 OpenCV 的cv2.calibrateHandEye。第二种多点拟合。让机械臂末端分别移动到几个已知坐标点同时在相机图像里识别末端标记点并反投影出相机坐标。拿到至少 3 个不共线的对应点对后用 SVD 分解求 R 和 T。这个方法简单直接适合精度要求不高的场景。我在实际项目中通常先用第二种方法快速验证整个流程确定可行后再实施完整的手眼标定以免一开始就陷入标定的复杂度里。4.5 一个完整例子识别物体并输出可抓取坐标把前面的内容串起来写一个最常见的应用场景识别桌面上的物体输出其中心点的相机坐标。完整流程使用 OpenCV 的 HSV 颜色分割或者用目标检测模型在彩色图里找到目标的像素坐标通过深度图获取该像素的深度值反投影得到相机坐标系下的三维坐标通过外参转换到机械臂坐标系核心代码片段如下import cv2 import numpy as np # 假设已经在彩色图 frame 中检测到目标 bounding box # bbox [x, y, w, h] center_u int(x w / 2) center_v int(y h / 2) # 取 center 附近 3x3 深度中值减少单点噪声 depth_values [] for du in range(-1, 2): for dv in range(-1, 2): d aligned_depth_frame.get_distance(center_u du, center_v dv) if d 0: depth_values.append(d) if len(depth_values) 0: print(中心区域无深度) continue center_depth np.median(depth_values) point_camera rs.rs2_deproject_pixel_to_point( depth_intrinsics, [center_u, center_v], center_depth ) print(f物体中心在相机坐标: {point_camera}) # 假设外参矩阵已经标定好了 point_robot R np.array(point_camera) T print(f物体中心在机械臂坐标: {point_robot})这里为什么要做 3x3 中值滤波因为 D435i 的深度图在物体边缘和反光表面容易产生孤立噪声点直接取单点深度经常会出现明显跳变。取邻域中值能有效滤除这些毛刺而且不影响中心位置精度。这个技巧是我在实践中感觉性价比最高的一个小操作。5. D435i使用时最容易踩的坑与排查方法5.1 深度值返回 0 但认为没数据D435i 返回 0.0 的深度值通常有四种原因测量距离超出有效范围太近或太远物体表面吸收红外光黑色吸光材质、镜面反射场景中红外干扰过强对齐后深度的边缘区域没有有效数据排查路径先用 Realsense Viewer 官方工具确认场景深度是否正常如果 Viewer 里能看到完整深度图但程序里 get_distance 返回 0多半是程序里深度内参用错了。代码层面对深度值为 0 的像素要做过滤尤其在批量点云生成时不然(0 - cx) * 0 / fx的结果是 0会把一堆没有任何意义的三维点塞进点云里。5.2 对齐后图像“花”了深度图对齐到彩色图后尤其是物体边缘部分经常出现黑色边框或深度空洞这是正常的。因为深度传感器和 RGB 传感器物理位置不同彩色图里看到的区域深度传感器不一定能看到所以对齐后边缘无可避免地产生空洞。处理方案适当缩小感兴趣区域避开边缘使用rs.temporal_filter或rs.hole_filling_filter填充空洞如果应用对深度完整性要求高考虑用更高分辨率的深度流重新对齐5.3 手眼标定后位置仍偏移外部参数标定完成后发现机械臂抓取位置仍有偏移这个问题最常见的原因是相机标定板平面没有覆盖工作空间。如果你的工作范围是机械臂前方的 400x400mm 区域但标定时标定板只在 100x100mm 的区域里移动外参在这些位置之外的外推误差会很大。另一个容易忽略的点手眼标定求解出的 R、T 与参考坐标系定义有关。如果机械臂用的是右手坐标系而相机 SDK 用的是左手坐标系即使标定正确坐标转换后符号也容易反。解决方式是先打印几个点的相机坐标和机械臂坐标对照确认坐标轴方向一致后再接完整流程。5.4 性能不够用USB 3.0 接口是 D435i 的基本要求插在 USB 2.0 上会掉帧严重。如果确认接口没问题还是掉帧降低深度流分辨率到 480x270或者改用 15fps 帧率。另外No-Multi-Camera 模式下运行 D435i 不需要额外配置但如果多个 D435i 同时使用需要设置相机同步并给不同相机分配不同的 USB 控制器否则带宽抢占会让所有相机的帧率一起崩掉。5.5 深度误差随距离变化D435i 的深度误差与距离不是线性的通常来说在 0.5m~1m 范围内精度最高误差在 1%~2% 左右。距离超过 3m 后误差会明显增大。如果你的应用精度要求高建议让目标物体尽量落在 0.5m~1.5m 区间里。还可以用官方工具 Dynamic Calibration 对相机做二次标定减少温度漂移带来的深度误差。相机长时间工作发热后深度精度会略有下降这是硬件特性做高精度测量时尽量提前开机预热 10 分钟再开始采集数据。6. 实操心得与进阶建议D435i 用久了我最大的体会是硬件本身很皮实真正的门槛都在软件和数学上。像素坐标到三维坐标的转换原理上就是针孔模型的逆变换但只有亲自把内参、深度尺度、对齐、外参这些环节一步不落地走通才会真正理解每个环节存在的意义。个人经验里有几个小点很值得提醒第一个永远使用对齐后的深度图不要用原始深度图做彩色图和深度图的像素级匹配。哪怕你在画面上看起来差不多边缘的偏差在机械臂抓取时会被放大成为抓偏的原因。第二个深度值获取用get_distance()还是读取 numpy 数组要分清楚。get_distance()返回单位是米numpy 数组里的原始 Z16 值需要乘以depth_scale才是米。混用这两个单位很容易搞错。第三个外参不是标定一次永久有效的。相机和机械臂安装结构受震动、温度影响会产生微小位移定期重标定尤其是经过运输或长时间运行后偏差可能会超出可接受范围。后续想在这个基础上做进阶可以研究深度图滤波算法比如双边滤波、时间滤波能明显提升边缘深度质量再往下就是结合 IMU 数据做相机位姿跟踪D435i 内置的 IMU 正好能派上用场做视觉惯性里程计或者运动补偿这些都是很有意思的扩展方向。说到底像素到三维空间的一小步是视觉应用落地的一大步把它彻底搞明白后面那些更复杂的视觉系统设计都会顺很多。
阅读完成 · 觉得有帮助？
