RealSense D435i像素转三维坐标：原理、对齐与反投影实战

做机器人或者视觉项目的人，八成都会遇到同一个需求：屏幕上鼠标点了一个像素，我想知道这个点在实际空间里的位置。尤其是在Ubuntu下用realsense d435i做机械臂抓取、目标跟随、三维重建的时候，这个“像素点到三维坐标”的转换是绕不开的第一步。这篇文章接着系列往下写，专门讲怎么用librealsense获取某个二维像素点对应的三维坐标，从原理到代码到踩坑一条龙，保证你看完能直接抄作业。

1. 内容整体设计与思路拆解

1.1 这个需求到底在解决什么问题

先把场景说清楚。你在d435i的彩色画面里看到桌面上有个杯子，想控制机械臂去抓它。机械臂需要知道杯子在空间里的坐标，所以你要先把彩色图像里杯子的像素位置，比如(320, 240)，转换成相机坐标系下的三维坐标(x, y, z)，再通过手眼标定变换到机械臂坐标系。这个链条的第一步，就是把像素点“抬升”到三维空间。

d435i这类深度相机的本质，就是在普通RGB相机的基础上多了一个测量深度的通道。每个像素除了颜色信息，还额外携带了一个距离值。有了距离，加上相机自身的几何参数，就能把二维图像上的任意一点映射到三维空间。所谓“获取某二维像素点的三维坐标”，就是干这个事。

很多初学者会卡在一个认知上：觉得realsense SDK里应该有个函数，输入像素坐标就能直接吐出来三维坐标。有的，但是你得理解它背后的参数和坐标系，否则拿到数值也不知道怎么用。这篇文章就是把这条链路完全拆开讲透。

1.2 为什么像素点必须加上深度才有三维坐标

普通相机拍到的是一张平面图像，丢失了深度信息。一幅图里有个人、有棵树、有栋楼，它们在图像里都是像素，但单靠一个像素坐标，你完全无法判断这个点在空间里离相机是1米还是50米。二维像素坐标(u, v)只能确定一条射线，这条射线上所有的点在图像上都会投影到同一个像素位置。

深度相机的价值就在这里：它给射线上唯一的距离值z，于是射线上的某个确定点就被锁定了。有了(u, v, z)，再加上相机内参，就能用针孔相机模型反推出点在相机坐标系里的(x, y, z)坐标。直观地说，像素坐标告诉你方向，深度值告诉你距离，两者一起才构成一个完整的三维空间点。少了任何一个，都是“一条线，不知道在哪”。

这也是为什么在对齐和拿深度值之前，必须先搞明白d435i的两个核心数据流：彩色流和深度流。它们分别来自两个不同的传感器，安装位置有物理偏差，所以同一个空间点在两个画面里的像素坐标不完全一致。你要用彩色图里的像素位置去取深度值，就必须先把两张图对齐，让深度图的坐标系和彩色图的坐标系重合。这一步不做，后面一切都是错的。

1.3 三种实现方案对比，我为什么推荐官方反投影

获取三维坐标，正常情况下有三条路可走。

第一种是使用librealsense自带的点云模块rs2::pointcloud。把每一帧深度图都生成一个点云，然后你想查哪个像素，就去点云里索引。直观，但开销极大——点云是几十万个三维点，你只需要一个点，为了一个点生成全图点云，放在高性能台式机上都浪费，更别提嵌入式平台了。

第二种是手动实现针孔相机反投影公式。先拿到深度相机的内参(焦距fx、fy，主点cx、cy)，再从深度图读取该像素的深度值z，然后套公式x = (u - cx) * z / fx，y = (v - cy) * z / fy。这个办法完全可行，而且吃透了原理。麻烦在于要自己处理内参获取和单位换算，代码可读性一般。

第三种是使用librealsense提供的rs2_deproject_pixel_to_point函数，官方一行API直接完成反投影。它内部做的事和第二种一模一样，只是帮我们把内参管理、公式计算封装好了。

我推荐第三种。原因很简单：官方API经过充分测试，配合pipeline的align机制，代码极简，几乎不会出低级错误。最重要的是，它和我们后续要做的深度图对齐是天然的配合关系，不用自己维护一堆标定参数。这不是说手动实现不好，而是解决实际工程问题时，能用成熟API的地方就不重复造轮子，把精力花在真正要解决的问题上。

2. 核心细节解析与实操要点

2.1 深度图与彩色图对齐：最容易踩的坑

d435i的深度传感器和RGB传感器在硬件上是两个独立的模组，物理位置不同，视场角也不同。因此，同一时刻，同一个空间点，在深度图里的像素坐标和彩色图里的像素坐标并不相同，两者之间存在一个固定的平移和缩放关系，专业叫法叫extrinsics，即外参。

如果你从彩色图像里取了一个像素点(320, 240)，然后直接用这个坐标去原始深度图上取深度值，拿到的极有可能是旁边物体的深度，甚至是空洞。目标边缘越复杂、物体距离越近，误差越明显。

解决办法是使用rs2::align先把深度图对齐到彩色图的坐标系下。对齐之后，深度图会生成一张新的、尺寸和彩色图一致的深度图，这张图上每一个像素(u, v)存的就是“彩色图上(u, v)这个位置对应的真实深度”。这一步做完，前面说的错位问题就彻底解决了。

2.2 理解相机内参与坐标变换公式

要做反投影，必须得知道相机内参。realsense的内参包含四个关键值：fx、fy、cx、cy。

fx和fy是焦距在像素单位下的表示。通俗地说，fx反映了一个物体在水平方向上的像素尺寸与它在空间中的水平距离之间的比例关系；fy同理，是竖直方向的。cx和cy是成像平面主点的像素坐标，正常情况下接近图像中心，但因为传感器装配和镜头畸变，会有少量偏移。

当你知道(u, v)是像素坐标、z是深度传感器测到的那个点到相机平面的距离时，相机坐标系下的三维坐标由针孔模型给出：

• X = (u - cx) * z / fx
• Y = (v - cy) * z / fy
• Z = z

其中X轴指向相机右方，Y轴指向相机下方，Z轴指向相机正前方。原点是相机光心。这个坐标系的定义很多人一开始不习惯，因为Y轴是朝下的，和常见的机器人坐标系（Z轴朝上）不一样，后续做坐标变换要记住这一点。

这里有个很容易误解的点：这个z，和三维坐标里的Z，在数值上是同一个。因为深度相机测到的距离，本来就是点到相机平面的垂直距离，而不是点到光心的直线距离。所以反投影公式里，Z直接等于深度值。理解了这一点，你就明白为什么反投影只需要一个深度值加内参就够了，不需要额外求解。

2.3 像素坐标的方向、单位与有效范围

用d435i做开发，单位问题一直是个坑。深度图原始值的单位是毫米（z16格式，16位无符号整数，范围一般是0到65535毫米），但librealsense的get_distance()函数返回的是米，是浮点数。反投影函数rs2_deproject_pixel_to_point接受的深度参数也是米，返回的三维坐标同样是米。如果你习惯用毫米，一定要在代码里做好换算，否则坐标直接差出1000倍。

像素坐标(u, v)方面，需要注意它是以图像左上角为原点、u向右、v向下的坐标。图像尺寸640x480时，(0, 0)是左上角，(639, 479)是右下角。(320, 240)正好是正中心。许多视觉库比如OpenCV在使用像素坐标时也是这个约定，因此两者配合起来基本零障碍。

还有一个关键约束是深度有效范围。d435i的标称深度范围是0.2米到10米左右，但真正精度可靠、适合做机械臂抓取等精密操作的区间是0.3米到3米。太近，红外投影的散斑图案无法形成足够的视差；太远，红外信号衰减严重，噪声急剧上升。所以，不论你的应用是什么，尽量把目标摆在这个范围内，深度值的可信度才有保证。

3. 实操过程与核心代码实现

3.1 环境准备与相机初始化

在开始写代码前，假设你已经装好了librealsense和pyrealsense2。这篇文章是系列第二篇，默认环境已经就绪，如果你还没装好，建议先看一下系列第一篇的环境配置部分，在Ubuntu下安装librealsense和pyrealsense2的步骤都有详细说明。

python版本的pyrealsense2是最方便的，导入很直接：

import pyrealsense2 as rs import numpy as np

初始化pipeline并配置两个数据流。深度流用z16格式，彩色流用bgr8格式，分辨率统一用640x480，帧率30。这个分辨率适合大多数场景，帧率30也能满足实时抓取的需求：

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)

启动pipeline后，相机就开始工作了。但此时深度流和彩色流还是两套独立的坐标系，直接取深度会错位，所以下一步必须创建对齐对象。

获取某个像素的三维坐标，最核心的流程就是下面这几步，我在代码里用注释标出来了。

3.2 对齐与提取单个像素点的深度值

先创建rs2.align对象，目标流设为彩色流。这一步的意思是：把深度图重投影到彩色相机的坐标系下，让深度图像素坐标和彩色图像素坐标一一对应：

align_to = rs.stream.color align = rs.align(align_to)

在循环里取帧，先用align.process处理原始帧，得到对齐后的frameset，再从对齐后的frameset里分别取深度帧和彩色帧。注意，这里取深度帧的顺序很重要——必须在对齐之后取，而且要用对齐后frameset里的深度帧，而不是原始帧里的深度帧：

frames = pipeline.wait_for_frames() aligned_frames = align.process(frames) depth_frame = aligned_frames.get_depth_frame() color_frame = aligned_frames.get_color_frame() if not depth_frame or not color_frame: return

之后就可以随意指定一个像素坐标(u, v)去取深度值了：

u, v = 320, 240 depth = depth_frame.get_distance(u, v)

如果该像素位置没有有效的深度测量值，get_distance返回0。这时候你要处理这个特殊情况，否则后面反投影会得到(0, 0, 0)。产生0值的原因比较多，最常见的是目标太近、太远、表面反光、纯黑吸收红外光、或者目标处于深度图边缘区域。

取到深度值后，还要获取深度传感器的内参。注意，反投影时用的内参必须是对齐后深度帧的内参，是通过depth_frame.profile拿到的，不是彩色帧的内参：

depth_intrinsics = depth_frame.profile.as_video_stream_profile().get_intrinsics()

这个地方我单独强调一下，因为很多教程在这里含糊带过，直接用彩色内参反投影，结果坐标偏得离谱。对齐后的深度图虽然在像素坐标上和彩色图重合，但它的几何投影关系仍然是深度传感器自身的，所以反投影必须用深度内参。

3.3 完整可运行的Python示例

把以上零散代码拼接起来，就是一个完整的获取像素点三维坐标的最小程序。下面这段代码我实测可以跑，逻辑上覆盖了初始化、对齐、取深度、反投影、打印坐标全过程：

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) try: while True: frames = pipeline.wait_for_frames() aligned_frames = align.process(frames) depth_frame = aligned_frames.get_depth_frame() color_frame = aligned_frames.get_color_frame() if not depth_frame or not color_frame: continue u, v = 320, 240 # 你要查询的像素坐标 depth = depth_frame.get_distance(u, v) if depth == 0: print("({}, {}) 处无有效深度数据".format(u, v)) continue depth_intrinsics = depth_frame.profile.as_video_stream_profile().get_intrinsics() # 反投影：像素坐标 + 深度 -> 相机坐标系三维坐标(米) point = rs.deproject_pixel_to_point(depth_intrinsics, [u, v], depth) print("像素 ({}, {}) -> 三维坐标: x={:.3f}m, y={:.3f}m, z={:.3f}m".format( u, v, point[0], point[1], point[2] )) finally: pipeline.stop()

运行这段代码，终端会持续打印出你指定的像素点对应的三维坐标。数值单位是米，坐标系是相机坐标系，X向右、Y向下、Z向前。

如果你想把程序封成一个方便调用的函数，可以这样写：

def get_3d_point(depth_frame, u, v): depth = depth_frame.get_distance(u, v) if depth == 0: return None intrinsics = depth_frame.profile.as_video_stream_profile().get_intrinsics() point = rs.deproject_pixel_to_point(intrinsics, [u, v], depth) return point

返回的是一个长度为3的列表，或者直接numpy数组去处理也行。这样后续做起连续取点、多点标定都方便。

3.4 实际操作中的几个关键检查项

代码写完后，建议做一次“可信度验证”。找一个有明确尺寸的物体，比如一张A4纸，把相机固定好，用上面的代码分别取纸面左上角和右下角的三维坐标，计算两点间的欧氏距离，看看是不是和真实尺寸接近。这个验证能帮你一次性排掉内参错误、对齐错误、单位错误、坐标系搞混等问题。我每次换新相机或者换新环境，都会先做这一步，耗时不长，但能省大量排查时间。

还有就是要区分帧率的影响。d435i在30fps下工作时，深度图每一帧都会有一定噪声，反映到三维坐标上就是坐标轻微抖动。如果你只是取一个静态点，多帧取平均即可；如果你需要实时连续输出坐标给机械臂做闭环，建议再加上一个一阶低通滤波或者卡尔曼滤波，否则机械臂末端容易抖动。这在后面的章节里继续展开。

4. 常见问题与排查技巧实录

4.1 深度值一直返回0，哪里出了问题

这是新手遇到最多的问题。u, v坐标明明是画面里的物体位置，但get_distance返回0。

先确认这个位置是不是真的在有效深度范围内。d435i太近测不到，通常小于0.2米就是0；太远测不到，超过10米基本也是0。再看看目标表面材质，纯黑色物体吸收红外光、镜面反射物体把红外光弹飞、透明物体直接穿透，这些都会导致该像素无有效深度。

如果上述情况都排除了，那大概率是目标处于深度图边缘或者遮挡区域。d435i的深度计算基于红外散斑图案，物体边缘会出现“过渡区”，过渡区里的像素深度值往往是0。这时候可以尝试换一个像素点采样，或者使用深度图中的邻域均值来替代单点深度值，比如取以(u, v)为中心的3x3窗口里所有非0深度的平均值。

4.2 对齐后图像还是错位，坐标依然不准

代码里已经用了align.process，但深度和彩色看起来仍然对不上。这种情况通常是帧不同步造成的。d435i的深度帧和彩色帧曝光时间不同，室内光线暗的时候彩色帧曝光时间可能很长，导致深度帧和彩色帧在时间轴上不完全对齐，运动物体上尤其明显。

解决方法是打开realsense的自动曝光并让相机预热几分钟。另外，librealsense支持基于时间戳的帧同步机制，在pipeline启动后，深度帧和彩色帧会被尽可能对齐到同一时刻。如果你观察到的错位只出现在运动物体上，那基本是时间同步问题而非空间对齐问题，这种情况下只能靠提高环境亮度来缩短曝光时间，把运动模糊和帧间延迟降到最低。

对于静态场景，错位往往是因为忘了从对齐后的frameset里取深度帧。直接从原始frames里get_depth_frame()，那个深度图还没有经过对齐，像素位置和彩色图当然对不上。这个错误很隐蔽，因为代码不会报错，数值也不会是0，但坐标就是偏。

4.3 反投影坐标在静止时仍然上下左右乱跳

深度噪声是主要原因。d435i虽然精度不错，但也不是激光级的。在1米距离处，深度值的典型噪声约在正负几毫米到一两厘米之间，具体取决于环境光照、目标表面反射率和距离。这个噪声经过反投影放大，在X和Y方向上都会表现出来。

降低噪声的办法有几个。第一，提高深度图的滤波强度，librealsense自带空间滤波和时间滤波，可以对depth_frame做后处理。第二，多帧平均，取同一个像素在10帧里的平均坐标，基本能把抖动抑制到很小的范围。第三是前面提到过的低通滤波，把坐标变化率限制住，适合实时控制场景。机械臂抓取这种对稳定性要求高的场景，我通常建议时间滤波加空间滤波一起上。

4.4 这个Z值到底准不准，精度有没有参考

D435i在0.5米到1米这个区间内，深度精度标称在约正负1%到2%之间，测距越远误差越大。到了3米开外，误差可能已经到好几厘米。所以做精密抓取时，尽量把目标控制在1.5米以内。另外，深度精度还受到环境红外光干扰的影响，户外阳光直射下d435i会明显变差，室内普通照明反而表现最好。

还有一个容易忽略的点：开机后的热漂移。d435i内部有红外激光发射器，工作一段时间后会发热，导致传感器微小的几何形变，深度值因此会产生少量漂移。精度要求高的应用，建议开机后先让相机稳定运行5分钟以上再做数据采集，或者做完手眼标定后期校正。

4.5 常见问题速查表

5. 一些值得继续深挖的扩展方向

获取单个像素的三维坐标只是第一步，实际项目里它会被反复使用，组合成各种更高级的功能。

最常见的扩展就是“点选物体”。在彩色画面上点击一下，程序立刻返回物体的三维位置，然后机械臂跟踪过去抓取。这个交互逻辑在很多视觉抓取demo里都有，核心原理就是这篇文章讲的内容。进一步的，可以把连续的几个三维点连起来做空间尺寸测量，比如测工件长度，误差可以控制在毫米级。

再往下走，就是手眼标定了。相机坐标系的坐标要变成机械臂基座坐标系的坐标，需要一个变换矩阵，这个矩阵通常通过眼在手外（相机固定）或眼在手上（相机装在机械臂末端）的标定流程来求。如果你做机械臂抓取，这篇文章的三维坐标到手眼标定这个环节，是绕不开的一站。

我个人的经验是，把像素点反投影这个功能封装成独立的工具函数，放在工程的基础模块里，所有需要用到三维定位的地方统一调用。这样做最大的好处是，后续如果换了相机型号、改了分辨率或是需要加入滤波，只需改这一个函数，全工程受益。实际项目里我基本都是这么组织的，维护成本低很多。