做视觉定位的时候总会碰到一个问题目标明明就在图像里我也知道它在图像上落在哪个像素可机器人或者上位机要的不是像素坐标而是一组空间三维坐标。这篇就是来解决这个问题的——环境是Ubuntu相机是Intel RealSense D435i主题非常聚焦给定一个二维像素点怎么拿到它在空间中的三维坐标。这是系列的第二篇。上一篇讲完了驱动安装和基础取流之后这一步相当于把相机从“能成像的设备”变成“能给坐标的传感器”。做机械臂抓取、AGV避障、物体测量、SLAM标定都会用到这一段属于视觉定位里绕不过去的核心环节。我会把原理、Python和C两种实现、常见坑全部过一遍尽量让看完的人能直接在自己项目里用起来。1. 需求拆解从“鼠标点一下”到“输出一组XYZ”要解决什么问题1.1 为什么需要像素坐标转三维坐标先明确一个概念普通RGB图像本身是没有空间信息的。摄像头把三维世界压缩成二维图像的一瞬间深度信息就丢了。你在图像上看到一个杯子知道它大概在第200列、第150行但你不知道它离相机是30厘米还是3米更不知道它在相机坐标系下的具体位置。D435i的价值在于它同时提供彩色图和深度图。深度图每个像素存的是那个位置到相机的距离单位通常是毫米。有了“像素位置 该像素处的深度值”再结合相机内参就能通过反投影公式还原出空间点的三维坐标这本质上是针孔相机成像模型的逆运算。实际项目里这个需求太常见了机械臂抓取时目标检测算法输出目标的中心像素坐标抓取系统需要的是目标在机械臂基座坐标系下的XYZ做体积测量时你需要在图上点两个点然后算出这两个点在空间中的欧式距离做多传感器融合时也需要把视觉检测结果从图像坐标系投影到世界坐标系。所以这个功能不是某个场景的偏门需求而是几乎所有视觉引导类项目的地基。1.2 三条实现路线怎么选拿到需求之后先别急着写代码。实现“点击图像取三维坐标”这个功能至少有三种常见路线各有适用场景。路线ARealSense Viewer手动测量。SDK安装完成后终端里运行rs-viewer左侧打开彩色图和深度图鼠标悬停在画面上会直接显示“像素坐标 三维坐标 深度值”还能用Measure工具测两点距离。这个方式零代码适合现场验证和快速查看但无法集成进自己的程序不可能让机械臂每次都靠人点一下。路线BROS realsense-ros rviz。如果你已经在用ROS做机器人开发realsense-ros驱动会发布点云话题/camera/depth/color/points在rviz里可以用PointHead插件点击点云中的点直接读取XYZ。这个方案对ROS用户非常方便但依赖较重而且它是“查看”而非“编程接口”想要灵活控制还是得看SDK。路线Clibrealsense2 SDK直接写代码。这是我最推荐的方式。Python和C都有完整API可以自己控制对齐、反投影、坐标变换全流程想集成到检测算法里、想批量计算、想离线处理都行。调试也直观出问题很容易定位是内参问题还是对齐问题。三条路线的对比如下方案适用场景优点缺点rs-viewer手动测量现场验证、临时查看零代码、直观不能编程集成ROS rvizROS项目、点云可视化可视化友好、生态完善依赖重、灵活性一般SDK自写代码检测抓取、测量、集成可控性强、易调试需要理解相机原理本文后续内容全部围绕路线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 关键代码逐段解读这段代码看似简单但每一行背后都有讲究。配置彩色流和深度流分辨率。我这里彩色和深度都是640x48030帧。如果你关注更高精度可以把深度调成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() .asrs2::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该像素处没有深度值换一个点改善光照和反光检查距离是否在0.3m-3m有效量程内坐标数值巨大且不稳定深度单位没用对把z16原始值当成了米乘以 depth_scale通常0.001后再反投影坐标和实际位置偏移明显没做深度对齐彩图和深度图像素不对应启用 rs.align(rs.stream.color)x和y方向反了行列坐标搞混记住 get_distance 参数顺序是 (u, v) (列, 行)点击后程序卡顿鼠标回调里做了耗时操作回调里只做取帧和打印不做图像处理viewer里能看到坐标但程序取不到深度流和彩色流分辨率不一致统一配置分辨率如都设640x480近距离目标深度为0低于D435i最小工作距离D435i最小工作距离约0.28米太近没数据反投影坐标向外飘用了深度内参去投影彩色像素点对齐后取对齐帧自身的profile内参下面挑几个重点展开说。深度值为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里打开同一场景鼠标悬停在目标点上看它自带的坐标显示是多少再跑到自己的程序里点同一个位置对比两者输出。两个数值能对得上说明内参、对齐、单位这些环节都没问题再继续往检测或标定的方向走。这个习惯我到现在还在用每到一个新环境先做这个对照能省下一大半排查时间。你如果第一次跑通之后发现坐标不太对先别急着怀疑代码用这个方法对照一下很多问题当场就能定位。