企业官网建设流程全解析

做视觉定位的时候，总会碰到一个问题：目标明明就在图像里，我也知道它在图像上落在哪个像素，可机器人或者上位机要的不是像素坐标，而是一组空间三维坐标。这篇就是来解决这个问题的——环境是Ubuntu，相机是Intel RealSense D435i，主题非常聚焦：给定一个二维像素点，怎么拿到它在空间中的三维坐标。

这是系列的第二篇。上一篇讲完了驱动安装和基础取流之后，这一步相当于把相机从“能成像的设备”变成“能给坐标的传感器”。做机械臂抓取、AGV避障、物体测量、SLAM标定，都会用到这一段，属于视觉定位里绕不过去的核心环节。我会把原理、Python和C++两种实现、常见坑全部过一遍，尽量让看完的人能直接在自己项目里用起来。

1. 需求拆解：从“鼠标点一下”到“输出一组XYZ”要解决什么问题

1.1 为什么需要像素坐标转三维坐标

先明确一个概念：普通RGB图像本身是没有空间信息的。摄像头把三维世界压缩成二维图像的一瞬间，深度信息就丢了。你在图像上看到一个杯子，知道它大概在第200列、第150行，但你不知道它离相机是30厘米还是3米，更不知道它在相机坐标系下的具体位置。

D435i的价值在于它同时提供彩色图和深度图。深度图每个像素存的是那个位置到相机的距离，单位通常是毫米。有了“像素位置 + 该像素处的深度值”，再结合相机内参，就能通过反投影公式还原出空间点的三维坐标，这本质上是针孔相机成像模型的逆运算。

实际项目里这个需求太常见了：机械臂抓取时，目标检测算法输出目标的中心像素坐标，抓取系统需要的是目标在机械臂基座坐标系下的XYZ；做体积测量时，你需要在图上点两个点，然后算出这两个点在空间中的欧式距离；做多传感器融合时，也需要把视觉检测结果从图像坐标系投影到世界坐标系。所以这个功能不是某个场景的偏门需求，而是几乎所有视觉引导类项目的地基。

1.2 三条实现路线怎么选

拿到需求之后，先别急着写代码。实现“点击图像取三维坐标”这个功能，至少有三种常见路线，各有适用场景。

路线A：RealSense Viewer手动测量。SDK安装完成后，终端里运行rs-viewer，左侧打开彩色图和深度图，鼠标悬停在画面上会直接显示“像素坐标 + 三维坐标 + 深度值”，还能用Measure工具测两点距离。这个方式零代码，适合现场验证和快速查看，但无法集成进自己的程序，不可能让机械臂每次都靠人点一下。

路线B：ROS + realsense-ros + rviz。如果你已经在用ROS做机器人开发，realsense-ros驱动会发布点云话题/camera/depth/color/points，在rviz里可以用PointHead插件点击点云中的点，直接读取XYZ。这个方案对ROS用户非常方便，但依赖较重，而且它是“查看”而非“编程接口”，想要灵活控制还是得看SDK。

路线C：librealsense2 SDK直接写代码。这是我最推荐的方式。Python和C++都有完整API，可以自己控制对齐、反投影、坐标变换全流程，想集成到检测算法里、想批量计算、想离线处理都行。调试也直观，出问题很容易定位是内参问题还是对齐问题。

三条路线的对比如下：

本文后续内容全部围绕路线C展开，语言用Python为主，同时给出C++核心片段。

2. 原理先搞懂：深度图、相机内参和反投影公式

2.1 深度图到底存的是什么

在写代码之前，必须把深度图这件事说清楚。D435i的深度流默认格式是Z16，意思是每个像素用一个16位无符号整数存储深度值。但这个整数不是直接以米为单位的，它要乘以一个缩放因子depth_scale才是真实距离。D435i默认的depth_scale是0.001，也就是存储值乘以0.001得到米，精度1毫米。

举个例子：深度图上某个像素的存储值是855，乘以0.001后得到0.855米，说明这个位置的空间点距离相机光心沿光轴方向约85.5厘米。这个逻辑一定要刻在脑子里，否则你很容易拿到一个看起来很大的数字，然后开始怀疑人生。

另外，深度图像素值为0的情况非常常见。0在深度图里表示“这个位置没有有效深度数据”。原因可能是物体太近或太远超出了有效量程，可能是表面反光导致红外结构光无法正确匹配，也可能是物体太薄、太透明。写代码时一定要对0值做处理，否则反投影出来的坐标全是0。

2.2 相机内参与反投影：就一个公式的事

相机成像可以用针孔模型描述，核心是4个内参：fx、fy是焦距（以像素为单位），ppx、ppy是光心在图像上的投影位置。D435i在640x480分辨率下，fx、fy通常在385左右，ppx、ppy接近320和240，但每个相机出厂标定值都有细微不同，不要写死，运行时从SDK里读取最稳妥。

反投影公式其实很简单：

Z = depth(u, v) X = (u - ppx) / fx * Z Y = (v - ppy) / fy * Z

这里u是像素列坐标，v是像素行坐标，depth(u, v)是这个像素位置对应的深度值（单位米）。得到的(X, Y, Z)就是以相机光心为原点、Z轴指向相机前方的三维坐标，单位米。这个公式把二维像素坐标还原成了三维空间点，逻辑上就是相机成像的逆过程。

实际代码里通常不手写这套公式，而是直接用SDK提供的rs2_deproject_pixel_to_point()。但公式本身必须懂，因为如果你用了错误的坐标系、错误的深度单位，反投影结果会非常离谱，这时候没有原理知识根本不知道从哪里排查。

2.3 彩色图和深度图视角不同：为什么要做对齐

D435i的彩色相机和深度相机不是同一个传感器，两者在硬件上有物理间距，视角也存在差异。这就导致同一个空间点，在彩色图上的像素位置和在深度图上的像素位置并不重合。如果你拿着一幅彩色图和一幅深度图，直接用彩色图上的像素坐标去深度图里取深度值，算出来的坐标必然有偏差，距离越近偏差越明显。

解决这个问题的方法叫做“对齐”（align），也就是把深度图重投影到彩色图的视角下，让两幅图像素一一对应。SDK里的rs2::align就是干这个的。对齐之后，彩色图上任意一个(u, v)位置，都能直接用get_distance(u, v)取到对应空间点的深度，非常方便。

需要说明一个细节：对齐后的深度帧，其分辨率、内参都与彩色流一致，反投影得到的坐标系是“与彩色流对应的相机坐标系”。如果项目里需要的是原始深度相机坐标系，可以通过传感器间的外参做一次变换，或者干脆不align、直接在原始深度帧上用深度内参反投影。对绝大多数抓取和测量场景，用对齐后的坐标系完全够用，后续反正都要统一变换到机械臂或世界坐标系。

3. Ubuntu实操：Python + pyrealsense2 点击图像取三维坐标

3.1 环境准备

我假设你已经装好了 librealsense2 SDK。如果没有，先跑一遍官方安装脚本，确认rs-enumerate-devices能看到D435i设备信息。Python绑定pyrealsense2通常随SDK一起安装，也可以单独用pip安装：

pip install pyrealsense2

建议顺手把OpenCV也装好，因为下面要用它显示图像和处理鼠标点击事件：

pip install opencv-python

相机通过USB 3.0接口连接，插好后在终端里敲下面命令确认设备状态：

rs-enumerate-devices | grep "Camera name"

能看到Intel RealSense D435I字样就说明设备正常。

3.2 完整可运行的Python脚本

直接上代码。这个脚本的逻辑是：启动D435i的彩色流和深度流，把深度对齐到彩色，然后用OpenCV显示彩色画面，鼠标左键点击图像任意像素，立即输出该像素对应的三维坐标。

import cv2 import numpy as np import pyrealsense2 as rs # 初始化 pipeline 并配置彩色流和深度流 pipeline = rs.pipeline() config = rs.config() config.enable_stream(rs.stream.color, 640, 480, rs.format.bgr8, 30) config.enable_stream(rs.stream.depth, 640, 480, rs.format.z16, 30) profile = pipeline.start(config) # 获取深度缩放因子，默认 0.001（单位：米/单位） depth_sensor = profile.get_device().first_depth_sensor() depth_scale = depth_sensor.get_depth_scale() print("depth_scale:", depth_scale) # 将对齐对象绑定到彩色流 align = rs.align(rs.stream.color) # 全局变量，供鼠标回调使用 current_depth_frame = None def on_mouse(event, x, y, flags, param): global current_depth_frame if event != cv2.EVENT_LBUTTONDOWN: return if current_depth_frame is None: return # 1. 获取该像素位置的深度值，单位是米 depth_value = current_depth_frame.get_distance(x, y) if depth_value == 0: print(f"像素点 ({x}, {y}) 没有有效深度值，请换一个点试试") return # 2. 取对齐后深度帧的内参，用于反投影 depth_intrin = current_depth_frame.profile.as_video_stream_profile().intrinsics # 3. 反投影：像素坐标 + 深度值 -> 相机坐标系三维坐标 point = rs.rs2_deproject_pixel_to_point(depth_intrin, [x, y], depth_value) print(f"像素坐标 ({x}, {y}) -> 三维坐标 " f"(x={point[0]:.4f}, y={point[1]:.4f}, z={point[2]:.4f}) 米") cv2.namedWindow("color") cv2.setMouseCallback("color", on_mouse) 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 # 保存对齐后的深度帧，供鼠标回调使用 current_depth_frame = aligned_depth_frame # 显示彩色图像 color_image = np.asanyarray(color_frame.get_data()) cv2.imshow("color", color_image) # 按 q 退出 if cv2.waitKey(1) & 0xFF == ord('q'): break finally: pipeline.stop() cv2.destroyAllWindows()

脚本运行后，屏幕上会弹出彩色画面窗口，鼠标左键点击任意位置，终端里就会打印出那个像素对应的三维坐标。

3.3 关键代码逐段解读

这段代码看似简单，但每一行背后都有讲究。

配置彩色流和深度流分辨率。我这里彩色和深度都是640x480，30帧。如果你关注更高精度，可以把深度调成1280x720，但要清楚分辨率越高，对齐和反投影的耗时也会增加。对实时点击取坐标这种场景来说，640x480完全够用。

rs.align(rs.stream.color)。这是整个流程的核心，作用就是把深度图重投影到彩色图的视角，让两幅图像素对齐。如果没有这一步，彩色图上点的坐标直接拿去取深度值，结果会偏。这个偏不是几毫米的小问题，近距离时可能偏几厘米甚至更多。

current_depth_frame.get_distance(x, y)。这里的(x, y)就是鼠标回调里的像素坐标。注意顺序是“列、行”，也就是说第一个参数是u，第二个参数是v。很多人第一次写的时候会把行列弄反，导致取到的深度值张冠李戴。

rs.rs2_deproject_pixel_to_point(depth_intrin, [x, y], depth_value)。这是反投影的核心API，传入内参、像素坐标、深度值，返回一个三维点。返回值的单位是米，坐标系是相机坐标系。值得注意的是，对齐后的深度帧的profile与彩色流一致，所以这里的内参等同于彩色内参。

为什么要在回调里用全局变量保存深度帧。鼠标回调是OpenCV事件触发，跟主循环的取帧是异步的。如果把深度帧放在回调外面直接用，很可能取到的是旧帧或者空帧。用全局变量保存当前最新帧，回调触发时读取，简单可靠。

3.4 C++版核心代码片段

虽然Python演示起来最直观，但很多工程落地还是要用C++。这里给出一段等价的核心逻辑，方便需要集成到C++项目里的朋友参考。

#include <librealsense2/rs.hpp> #include <opencv2/opencv.hpp> rs2::depth_frame aligned_depth; // 全局保存对齐后的深度帧 void onMouse(int event, int x, int y, int, void*) { if (event != cv::EVENT_LBUTTONDOWN) return; if (!aligned_depth) return; // 获取对齐后深度帧的内参 auto intrin = aligned_depth.get_profile() .as<rs2::video_stream_profile>().get_intrinsics(); // 获取像素处的深度值，单位米 float depth_value = aligned_depth.get_distance(x, y); if (depth_value == 0.f) { std::cout << "No valid depth at (" << x << ", " << y << ")" << std::endl; return; } // 反投影得到三维坐标 rs2::vertex point = rs2::deproject_pixel_to_point( intrin, { (float)x, (float)y }, depth_value); std::cout << "Pixel (" << x << ", " << y << ") -> 3D (" << point.x << ", " << point.y << ", " << point.z << ")" << std::endl; } int main() { rs2::pipeline pipe; rs2::config cfg; cfg.enable_stream(RS2_STREAM_COLOR, 640, 480, RS2_FORMAT_BGR8, 30); cfg.enable_stream(RS2_STREAM_DEPTH, 640, 480, RS2_FORMAT_Z16, 30); rs2::pipeline_profile profile = pipe.start(cfg); rs2::align align_to_color(RS2_STREAM_COLOR); cv::namedWindow("color"); cv::setMouseCallback("color", onMouse); while (cv::waitKey(1) != 'q') { auto frames = pipe.wait_for_frames(); auto aligned = align_to_color.process(frames); aligned_depth = aligned.get_depth_frame(); auto color = aligned.get_color_frame(); cv::Mat color_image( cv::Size(640, 480), CV_8UC3, (void*)color.get_data(), cv::Mat::AUTO_STEP); cv::imshow("color", color_image); } pipe.stop(); return 0; }

C++版本的思路和Python完全一致，就是取帧、对齐、反投影三步。注意get_distance的参数顺序同样是(u, v)，不要写反。

3.5 拿到坐标之后怎么办

刚才输出的三维坐标有多少参考价值，取决于你接下来怎么用它。我这里给出几个常见走向。

用于测量距离。对同一场景中的两个点分别取三维坐标，然后计算欧式距离sqrt((x1-x2)^2 + (y1-y2)^2 + (z1-z2)^2)，就能得到空间两点的实际距离。这个做物体尺寸测量非常好用，前提是相机正对物体、深度值可靠。

用于机械臂抓取。拿到的是相机坐标系下的坐标，机械臂认的是它自己基座坐标系下的坐标。中间还差一个“手眼标定”。这个标定要解相机到机械臂基座的外参矩阵，是另一个话题，但如果你不想自己推矩阵，最简单的做法是采用眼在手上（eye-in-hand）或眼在手外（eye-to-hand）两种经典方案配合标定板去解。

用于视觉引导。如果你要做目标检测加定位，流程就是把鼠标点击替换成检测模型输出的目标中心像素坐标，然后自动取深度、反投影，剩下的逻辑完全一样。

我把这三类走向用了三个小节来写，但选哪个走向取决于你的项目。从效率角度说，先把“像素到相机坐标”这段跑通，后面接任何下游任务都有了一个稳固的入口。

4. 常见问题与排查记录

这段应该是最值钱的。我自己在这个流程上踩过不少坑，也帮别人排查过，把高频问题整理成了一张速查表。

下面挑几个重点展开说。

深度值为0是最大头的坑。D435i的有效深度范围有限，默认最小工作距离大约0.28米，最大在3米左右（不同模式和分辨率会有差异）。太近、太远、表面反光、透明物体，都会导致深度值为0。程序里如果不判断depth_value == 0，反投影出来的点就是(0,0,0)，下游任务拿到这个点直接崩。我在代码里加了判断，但实际项目中你还要决定是跳帧、重试还是用周边有效深度插值。

坐标系方向一定要心里有数。反投影出来的坐标系是：X轴向右，Y轴向下，Z轴指向相机前方。如果你后面要接到机械臂坐标系，这个方向差异会造成负号问题。我见过不少人在这个坐标轴方向上调半天，最后发现是“Y轴向下”没注意。Ubuntu下调试这个很方便，你可以把相机对着自己，点图像中心偏左的位置，看输出的X是正还是负，很快就能验证坐标系方向。

关于内参的一个细节。程序里我直接从对齐后深度帧的profile取内参，不要手动填预置的 fx、fy。因为不同相机出厂标定值有差异，即便同型号也可能差一两个像素，手动填内参很容易导致坐标系统性偏差。这个偏差平时看不出来，一旦做机械臂抓取就会在空间位置精度上暴露。

深度scale一定要取，不要假设。虽然D435i默认depth_scale是0.001，但保险起见还是通过get_depth_scale()读取。万一有人在配置里改了深度单位，你假设的0.001会让所有坐标放大或缩小1000倍，这种错误极难排查，因为逻辑完全没毛病，就是数值不对。

鼠标回调里别做重活。如果你在回调函数里写复杂的计算逻辑，OpenCV的界面线程会被卡住，表现就是鼠标点击之后画面卡死。正确做法是回调里只记录点击坐标和最新帧，计算放到主循环或者单独线程里去跑。我的示例里因为只打印一个点，所以直接在回调里反投影了，项目里建议把get_distance和反投影这部分也放到主循环。

5. 实际项目中的三种扩展用法

5.1 结合目标检测自动定位

把鼠标点击换成目标检测模型的输出，这个功能就从“手动取点”升级成了“自动定位”。流程基本不变：检测模型从彩色图上识别目标并输出边界框，取边界框中心像素作为目标点，然后用对齐后的深度帧取深度值，最后反投影。这里要注意，检测模型输出的坐标要转换成与深度图对齐后的图像坐标，好在你已经用了align到彩色流，所以检测输出坐标可以直接使用。

我实际做机械臂抓取时，流程是：先用YOLO类模型检测物体，取中心点，用get_distance拿到距离，再反投影出三维坐标，最后通过手眼矩阵把相机坐标变换到机械臂基座坐标。整套链路里，今天讲的这段像素到相机坐标是承上启下的关键环节。

5.2 测量任意两点空间距离

假如要测物体高度或者两个点之间的距离，原理就是取两个像素点的三维坐标，算欧式距离。需要注意，取点时尽量选深度值稳定、没有遮挡的位置，否则测出来的距离误差很大。如果物体边缘有深度突变，尽量取中心区域。

这个功能配合D435i做工业測尺寸、物体分拣前测体积，都很实用。测量时如果条件允许，把相机固定在三角架上并尽量正对被测物体，可以减少因视角倾斜带来的测量误差。

5.3 下一步：相机到机械臂的手眼标定

拿到相机坐标系下的三维坐标只是第一步，机械臂抓取时还需要把坐标变换到机械臂基座坐标系。这一步靠的是标定，常用方法有眼在手外（相机固定在支架上）和眼在手上（相机装在机械臂末端）。标定需要用到标定板（棋盘格或者ArUco码），采集多组机械臂末端位姿和标定板角点的像素坐标，求解相机到机械臂基座的变换矩阵。

这部分内容比较多，而且和具体的机械臂型号、标定工具链有关。如果后续大家感兴趣，我可以单独写一篇“三”来详细展开。这里只需要明确一个问题：手眼标定的输入就是今天能稳定输出的像素坐标和三维坐标，所以把这篇文章的基础打牢，后面标定才有意义。

最后再分享一个经验

调试这类程序时，我习惯先在RealSense Viewer里打开同一场景，鼠标悬停在目标点上，看它自带的坐标显示是多少，再跑到自己的程序里点同一个位置，对比两者输出。两个数值能对得上，说明内参、对齐、单位这些环节都没问题，再继续往检测或标定的方向走。这个习惯我到现在还在用，每到一个新环境先做这个对照，能省下一大半排查时间。你如果第一次跑通之后发现坐标不太对，先别急着怀疑代码，用这个方法对照一下，很多问题当场就能定位。