1. 項目概述接觸 Intel Realsense D435i 有一段時間了這相機(jī)在我手里已經(jīng)配合機(jī)械臂做過好幾種抓取方案也搭過三維重建的采集系統(tǒng)。先說結(jié)論D435i 最大的價值不是“能測距”而是它把深度獲取的門檻降到了“拿來就能用”的程度但真正落地時大多數(shù)人卡住的往往是最后一公里——怎么把深度圖里的一個像素變成真實世界里的一個三維坐標(biāo)點。這正是這篇文章想解決的問題。如果你在使用 D435i 做機(jī)械臂抓取、目標(biāo)定位、三維重建或者 AR 應(yīng)用一定遇到過這種情況在彩色圖里框選了一個物體中心拿到像素坐標(biāo) (u, v)然后呢這個點在空間里到底在哪機(jī)械臂該往哪兒抓如果對坐標(biāo)轉(zhuǎn)換的理解只停留在“調(diào)用某個函數(shù)”或者“復(fù)制某段代碼”一旦換一臺相機(jī)、換一個安裝位置代碼立刻失效定位精度也會變得沒法看。讀完全文你會掌握三件事第一從像素坐標(biāo)到三維空間坐標(biāo)的數(shù)學(xué)原理知道每個參數(shù)從哪來、為什么需要它第二在 D435i 上用 Python 和 pyrealsense2 完整實現(xiàn)坐標(biāo)轉(zhuǎn)換的代碼流程包括單點反投影和批量點云生成第三我在實際項目中踩過的坑尤其是對齊、內(nèi)參和坐標(biāo)系外參那些容易出錯的地方。這篇文章適合的讀者剛?cè)胧?D435i 但看不懂官方例程的初學(xué)者正在做機(jī)械臂視覺抓取或機(jī)器人導(dǎo)航的開發(fā)者以及想把 RGB-D 數(shù)據(jù)用于三維重建但搞不清坐標(biāo)關(guān)系的人。已經(jīng)有了基礎(chǔ)經(jīng)驗的話可以直接跳到第 4 節(jié)看實現(xiàn)或者第 5 節(jié)看避坑。2. 從像素到三維空間必須吃透的數(shù)學(xué)底子2.1 針孔相機(jī)模型要搞懂坐標(biāo)轉(zhuǎn)換務(wù)必先理解相機(jī)是怎么把人眼看到的世界變成一張照片的。所有相機(jī)不管多貴核心都抽象成針孔模型三維空間中的點通過光心投射到成像平面上畫面因此是倒立的但算法里通常會做一次翻轉(zhuǎn)方便處理。先記住幾個關(guān)鍵坐標(biāo)系坐標(biāo)系符號原點位置單位像素坐標(biāo)系(u, v)圖像左上角pixel圖像坐標(biāo)系(x, y)光軸與成像平面交點mm相機(jī)坐標(biāo)系(Xc, Yc, Zc)相機(jī)光心mm 或 m世界坐標(biāo)系(Xw, Yw, Zw)自定義mm 或 mD435i 輸出的深度圖每個像素值代表的是該像素位置對應(yīng)空間點到相機(jī)平面的距離嚴(yán)格說是到相機(jī)光心所在平面的深度值 Zc單位是毫米。深度圖本質(zhì)上就是一張記錄了 Zc 值的二維矩陣。像素坐標(biāo)到三維坐標(biāo)的核心公式如下。已知像素坐標(biāo) (u, v) 和深度值 Zc可以反投影得到相機(jī)坐標(biāo)系下的三維點Xc (u - cx) * Zc / fx Yc (v - cy) * Zc / fy Zc depth_value其中 fx、fy 是焦距單位是像素cx、cy 是主點坐標(biāo)也叫光心像素坐標(biāo)。這一組參數(shù)就叫相機(jī)內(nèi)參。注意公式里有個細(xì)節(jié)Xc、Yc 的單位由 Zc 決定。如果 Zc 單位是米那算出來的 Xc、Yc 就是米如果 Zc 單位是毫米算出來就是毫米。做機(jī)械臂抓取時我一般統(tǒng)一用米省得后續(xù)轉(zhuǎn)換出錯。2.2 為什么需要內(nèi)參標(biāo)定很多人以為 D435i 出廠就能直接用不用標(biāo)定。這話對了一半D435i 確實出廠時做過標(biāo)定相機(jī)內(nèi)部存了一套工廠標(biāo)定參數(shù)直接用 SDK 讀就行。但工廠標(biāo)定解決的是“這臺相機(jī)自己的焦距和光心是多少”不解決“相機(jī)裝在機(jī)械臂上、裝在車上的位置姿態(tài)是什么”。如果把深度相機(jī)裝到機(jī)械臂上采集到的三維點是在相機(jī)坐標(biāo)系下的。機(jī)械臂要抓東西需要的是物體在機(jī)械臂基坐標(biāo)系或工具坐標(biāo)系下的坐標(biāo)。這就需要一個手眼標(biāo)定過程求相機(jī)坐標(biāo)系到機(jī)械臂坐標(biāo)系的旋轉(zhuǎn)矩陣 R 和平移向量 T。這部分的坑我后面會詳細(xì)講先記住一個核心結(jié)論像素坐標(biāo)到相機(jī)坐標(biāo)靠內(nèi)參相機(jī)坐標(biāo)到機(jī)器人坐標(biāo)靠外參。二者缺一不可。2.3 D435i的深度測量原理D435i 屬于主動雙目立體視覺傳感器。它的正面有兩個紅外攝像頭和一個紅外點陣投射器。工作時投射器發(fā)射不可見紅外光斑給墻面、桌面這些沒有紋理的區(qū)域“人為制造”特征點兩個紅外攝像頭從不同角度拍攝這些光斑通過三角測量原理計算視差從而得到每個像素的深度。這解釋了 D435i 為什么在有光照變化的室外場景容易翻車——強(qiáng)紅外干擾會讓光斑識別變得困難。但在室內(nèi)、近距離0.2m~3m場景下它的精度表現(xiàn)非常穩(wěn)定這也是它成為機(jī)械臂抓取熱門選擇的原因。D435i 的深度傳感器分辨率最高支持 1280x720但實際項目中我更常用 640x480幀率 30fps。分辨率越高單幀數(shù)據(jù)量越大USB 帶寬占用越高反而可能造成幀率下降或數(shù)據(jù)丟幀。3. 實戰(zhàn)準(zhǔn)備D435i環(huán)境搭建與數(shù)據(jù)獲取3.1 安裝 pyrealsense2官方提供的 Python 包是 pyrealsense2安裝非常簡單pip install pyrealsense2如果你的 Python 環(huán)境是 3.8 以上一般直接裝就行。但這些年我發(fā)現(xiàn)Windows 下有時會遇到 DLL 加載失敗多半是 Intel Realsense SDK Runtime 版本和 Python 包版本不匹配去官網(wǎng)下載對應(yīng)版本的 Intel.RealSense.SDK 安裝一次就好了。Linux 環(huán)境下建議從源碼編譯 pyrealsense2或者直接安裝官方發(fā)布的 deb 包再 pip 安裝。Ubuntu 20.04 下直接用 pip 裝通常也行前提是內(nèi)核支持 UVC 設(shè)備。3.2 打開相機(jī)并獲取對齊的深度圖與彩色圖D435i 同時具備深度傳感器和 RGB 傳感器但這兩個傳感器在硬件上有個物理距離導(dǎo)致同一時刻它們拍到的圖像視角略有差異。直接用原始深度圖和彩色圖做像素級對應(yīng)誤差會比較大。所以需要做對齊align操作將深度圖映射到彩色圖的視角下或者反過來。在 pyrealsense2 中用rs.align實現(xiàn)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)這里有個關(guān)鍵概念對齊后的深度圖其像素坐標(biāo)系與彩色圖完全一致。也就是說彩色圖里 (u, v) 位置的顏色對應(yīng)深度圖里 (u, v) 位置的深度值描述的是同一個空間點。這正是后續(xù)做像素坐標(biāo)轉(zhuǎn)三維坐標(biāo)的前提。3.3 獲取深度傳感器的內(nèi)參內(nèi)參怎么讀官方 SDK 提供了接口depth_sensor profile.get_device().first_depth_sensor() depth_scale depth_sensor.get_depth_scale() print(depth_scale , depth_scale) # 等待獲取一幀數(shù)據(jù) frames pipeline.wait_for_frames() depth_frame frames.get_depth_frame() # 獲取深度內(nèi)參 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以及畸變系數(shù)。這就是我們要用的內(nèi)參。depth_scale這個值非常關(guān)鍵。D435i 默認(rèn)深度圖每個像素存儲的是 unsigned int 16 位整數(shù)但這個整數(shù)不一定直接就是毫米數(shù)而需要乘以 depth_scale 才能換算成米。不同固件版本、不同配置下depth_scale 通常是 0.001表示每個單位對應(yīng) 0.001 米1 毫米。如果用depth_frame.get_distance(u, v)這個方法它返回的直接就是單位為米的值SDK 內(nèi)部已經(jīng)幫你做了換算。但如果你直接操作 numpy 數(shù)組取深度值就要手動乘以 depth_scale。我在實際項目里遇到過因為忘記乘 depth_scale導(dǎo)致計算出物體距離差了 1000 倍的情況整個機(jī)械臂差點往天上抓這種低級錯誤踩過一次就不會忘了。4. 像素坐標(biāo)到三維坐標(biāo)的實現(xiàn)與代碼解析4.1 單點反投影從 (u, v) 到 (X, Y, Z)下面實現(xiàn)完整的從單個像素坐標(biāo)到三維坐標(biāo)的轉(zhuǎn)換。假設(shè)我們已經(jīng)拿到對齊后的深度幀和彩色幀并且想要知道彩色圖中點 (320, 240) 對應(yīng)的三維坐標(biāo)。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() # 像素坐標(biāo) u, v 320, 240 # 獲取該像素的深度值單位是米 depth_value aligned_depth_frame.get_distance(u, v) # 判斷深度值是否有效0表示該位置沒有測量到深度 if depth_value 0: print(該像素點沒有有效的深度數(shù)據(jù)) continue # 核心反投影像素坐標(biāo)到相機(jī)坐標(biāo) # SDK 提供內(nèi)置方法 rs2_deproject_pixel_to_point point_3d rs.rs2_deproject_pixel_to_point( depth_intrinsics, [u, v], depth_value ) print(f像素坐標(biāo) ({u}, {v}), 深度 {depth_value:.3f} m) print(f相機(jī)坐標(biāo)系下三維坐標(biāo): X{point_3d[0]:.3f}, Y{point_3d[1]:.3f}, Z{point_3d[2]:.3f}) finally: pipeline.stop()rs.rs2_deproject_pixel_to_point輸入的參數(shù)分別是內(nèi)參、像素坐標(biāo)列表、深度值米。返回的point_3d是一個三維坐標(biāo)單位與深度值一致。如果你不用 SDK 內(nèi)置函數(shù)手寫公式也是分分鐘的事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兩種方式結(jié)果一致。理解公式的好處是當(dāng)遇到 SDK 版本變化、不同編程語言、甚至換用其他品牌相機(jī)時你都能快速遷移。4.2 批量轉(zhuǎn)換從深度圖生成三維點云單個點坐標(biāo)在很多場景不夠比如三維重建、點云處理需要把整張深度圖全部轉(zhuǎn)換成三維點云。方法就是遍歷每個像素做反投影但 Python 逐像素 for 循環(huán)效率太低一幀 640x480 的圖要跑很久。正確的做法是用 NumPy 實現(xiàn)向量化運(yùn)算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 # 構(gòu)建像素坐標(biāo)網(wǎng)格 v, u np.mgrid[0:h, 0:w] depth depth_image.astype(np.float32) * depth_scale # 無效深度點深度值為0或距離太遠(yuǎn) mask depth 0 X (u - cx) * depth / fx Y (v - cy) * depth / fy Z depth points np.stack((X, Y, Z), axis-1) # 返回每個像素對應(yīng)的三維坐標(biāo)無效點標(biāo)記為 [0,0,0] return points, mask這段代碼只做了很少的操作但速度比 for 循環(huán)快幾個數(shù)量級。實測 640x480 的深度圖逐像素循環(huán)大概要 5 秒向量化以后不到 50 毫秒。如果你想要更省事pyrealsense2 還內(nèi)置了點云生成功能pc rs.pointcloud() points pc.calculate(aligned_depth_frame) vtx np.asanyarray(points.get_vertices()).view(np.float32).reshape(-1, 3)用這種方式生成的點云坐標(biāo)單位同樣是米。點云數(shù)據(jù)可以直接配 Open3D 做可視化import open3d as o3d pcd o3d.geometry.PointCloud() pcd.points o3d.utility.Vector3dVector(vtx) o3d.visualization.draw_geometries([pcd])4.3 彩色圖與點云的顏色綁定只有三維坐標(biāo)還不夠直觀把彩色信息貼到點云上看起來才像回事。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)前提是深度圖已經(jīng)對齊到彩色圖否則坐標(biāo)和顏色對應(yīng)不上。4.4 像素坐標(biāo)到世界坐標(biāo)引入外參上面所有轉(zhuǎn)換結(jié)果都在相機(jī)坐標(biāo)系下。如果只是看看點云、測距離、做三維重建這個坐標(biāo)系完全夠用。但一旦涉及機(jī)器人抓取就必須把點轉(zhuǎn)換到機(jī)械臂坐標(biāo)系。設(shè)相機(jī)坐標(biāo)系到機(jī)械臂基坐標(biāo)系的變換為旋轉(zhuǎn)矩陣 $R$ 和平移向量 $T$則P_robot R * P_camera T$P_camera$ 是相機(jī)坐標(biāo)系下的三維點$P_robot$ 是機(jī)械臂坐標(biāo)系下的三維點。R 和 T 怎么求有兩種常見方式第一種手眼標(biāo)定。用棋盤格或 ArUco 標(biāo)定板同時獲取機(jī)械臂末端位姿和標(biāo)定板在相機(jī)坐標(biāo)系下的位姿通過 AXXB 方程求解。這個方法精度高但實現(xiàn)起來有一定工作量。D435i 的手眼標(biāo)定可以參考 eye-in-hand 的標(biāo)準(zhǔn)流程常用的視覺庫是 OpenCV 的cv2.calibrateHandEye。第二種多點擬合。讓機(jī)械臂末端分別移動到幾個已知坐標(biāo)點同時在相機(jī)圖像里識別末端標(biāo)記點并反投影出相機(jī)坐標(biāo)。拿到至少 3 個不共線的對應(yīng)點對后用 SVD 分解求 R 和 T。這個方法簡單直接適合精度要求不高的場景。我在實際項目中通常先用第二種方法快速驗證整個流程確定可行后再實施完整的手眼標(biāo)定以免一開始就陷入標(biāo)定的復(fù)雜度里。4.5 一個完整例子識別物體并輸出可抓取坐標(biāo)把前面的內(nèi)容串起來寫一個最常見的應(yīng)用場景識別桌面上的物體輸出其中心點的相機(jī)坐標(biāo)。完整流程使用 OpenCV 的 HSV 顏色分割或者用目標(biāo)檢測模型在彩色圖里找到目標(biāo)的像素坐標(biāo)通過深度圖獲取該像素的深度值反投影得到相機(jī)坐標(biāo)系下的三維坐標(biāo)通過外參轉(zhuǎn)換到機(jī)械臂坐標(biāo)系核心代碼片段如下import cv2 import numpy as np # 假設(shè)已經(jīng)在彩色圖 frame 中檢測到目標(biāo) 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(中心區(qū)域無深度) 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物體中心在相機(jī)坐標(biāo): {point_camera}) # 假設(shè)外參矩陣已經(jīng)標(biāo)定好了 point_robot R np.array(point_camera) T print(f物體中心在機(jī)械臂坐標(biāo): {point_robot})這里為什么要做 3x3 中值濾波因為 D435i 的深度圖在物體邊緣和反光表面容易產(chǎn)生孤立噪聲點直接取單點深度經(jīng)常會出現(xiàn)明顯跳變。取鄰域中值能有效濾除這些毛刺而且不影響中心位置精度。這個技巧是我在實踐中感覺性價比最高的一個小操作。5. D435i使用時最容易踩的坑與排查方法5.1 深度值返回 0 但認(rèn)為沒數(shù)據(jù)D435i 返回 0.0 的深度值通常有四種原因測量距離超出有效范圍太近或太遠(yuǎn)物體表面吸收紅外光黑色吸光材質(zhì)、鏡面反射場景中紅外干擾過強(qiáng)對齊后深度的邊緣區(qū)域沒有有效數(shù)據(jù)排查路徑先用 Realsense Viewer 官方工具確認(rèn)場景深度是否正常如果 Viewer 里能看到完整深度圖但程序里 get_distance 返回 0多半是程序里深度內(nèi)參用錯了。代碼層面對深度值為 0 的像素要做過濾尤其在批量點云生成時不然(0 - cx) * 0 / fx的結(jié)果是 0會把一堆沒有任何意義的三維點塞進(jìn)點云里。5.2 對齊后圖像“花”了深度圖對齊到彩色圖后尤其是物體邊緣部分經(jīng)常出現(xiàn)黑色邊框或深度空洞這是正常的。因為深度傳感器和 RGB 傳感器物理位置不同彩色圖里看到的區(qū)域深度傳感器不一定能看到所以對齊后邊緣無可避免地產(chǎn)生空洞。處理方案適當(dāng)縮小感興趣區(qū)域避開邊緣使用rs.temporal_filter或rs.hole_filling_filter填充空洞如果應(yīng)用對深度完整性要求高考慮用更高分辨率的深度流重新對齊5.3 手眼標(biāo)定后位置仍偏移外部參數(shù)標(biāo)定完成后發(fā)現(xiàn)機(jī)械臂抓取位置仍有偏移這個問題最常見的原因是相機(jī)標(biāo)定板平面沒有覆蓋工作空間。如果你的工作范圍是機(jī)械臂前方的 400x400mm 區(qū)域但標(biāo)定時標(biāo)定板只在 100x100mm 的區(qū)域里移動外參在這些位置之外的外推誤差會很大。另一個容易忽略的點手眼標(biāo)定求解出的 R、T 與參考坐標(biāo)系定義有關(guān)。如果機(jī)械臂用的是右手坐標(biāo)系而相機(jī) SDK 用的是左手坐標(biāo)系即使標(biāo)定正確坐標(biāo)轉(zhuǎn)換后符號也容易反。解決方式是先打印幾個點的相機(jī)坐標(biāo)和機(jī)械臂坐標(biāo)對照確認(rèn)坐標(biāo)軸方向一致后再接完整流程。5.4 性能不夠用USB 3.0 接口是 D435i 的基本要求插在 USB 2.0 上會掉幀嚴(yán)重。如果確認(rèn)接口沒問題還是掉幀降低深度流分辨率到 480x270或者改用 15fps 幀率。另外No-Multi-Camera 模式下運(yùn)行 D435i 不需要額外配置但如果多個 D435i 同時使用需要設(shè)置相機(jī)同步并給不同相機(jī)分配不同的 USB 控制器否則帶寬搶占會讓所有相機(jī)的幀率一起崩掉。5.5 深度誤差隨距離變化D435i 的深度誤差與距離不是線性的通常來說在 0.5m~1m 范圍內(nèi)精度最高誤差在 1%~2% 左右。距離超過 3m 后誤差會明顯增大。如果你的應(yīng)用精度要求高建議讓目標(biāo)物體盡量落在 0.5m~1.5m 區(qū)間里。還可以用官方工具 Dynamic Calibration 對相機(jī)做二次標(biāo)定減少溫度漂移帶來的深度誤差。相機(jī)長時間工作發(fā)熱后深度精度會略有下降這是硬件特性做高精度測量時盡量提前開機(jī)預(yù)熱 10 分鐘再開始采集數(shù)據(jù)。6. 實操心得與進(jìn)階建議D435i 用久了我最大的體會是硬件本身很皮實真正的門檻都在軟件和數(shù)學(xué)上。像素坐標(biāo)到三維坐標(biāo)的轉(zhuǎn)換原理上就是針孔模型的逆變換但只有親自把內(nèi)參、深度尺度、對齊、外參這些環(huán)節(jié)一步不落地走通才會真正理解每個環(huán)節(jié)存在的意義。個人經(jīng)驗里有幾個小點很值得提醒第一個永遠(yuǎn)使用對齊后的深度圖不要用原始深度圖做彩色圖和深度圖的像素級匹配。哪怕你在畫面上看起來差不多邊緣的偏差在機(jī)械臂抓取時會被放大成為抓偏的原因。第二個深度值獲取用get_distance()還是讀取 numpy 數(shù)組要分清楚。get_distance()返回單位是米numpy 數(shù)組里的原始 Z16 值需要乘以depth_scale才是米?；煊眠@兩個單位很容易搞錯。第三個外參不是標(biāo)定一次永久有效的。相機(jī)和機(jī)械臂安裝結(jié)構(gòu)受震動、溫度影響會產(chǎn)生微小位移定期重標(biāo)定尤其是經(jīng)過運(yùn)輸或長時間運(yùn)行后偏差可能會超出可接受范圍。后續(xù)想在這個基礎(chǔ)上做進(jìn)階可以研究深度圖濾波算法比如雙邊濾波、時間濾波能明顯提升邊緣深度質(zhì)量再往下就是結(jié)合 IMU 數(shù)據(jù)做相機(jī)位姿跟蹤D435i 內(nèi)置的 IMU 正好能派上用場做視覺慣性里程計或者運(yùn)動補(bǔ)償這些都是很有意思的擴(kuò)展方向。說到底像素到三維空間的一小步是視覺應(yīng)用落地的一大步把它徹底搞明白后面那些更復(fù)雜的視覺系統(tǒng)設(shè)計都會順很多。