基于誤差分量模型的兩階段深度校正算法

摘 要：為了提高消費(fèi)級RGB-D相機(jī)的深度精度，提出了一種基于誤差分量模型的兩階段深度校正算法。該算法根據(jù)誤差特性建立誤差分量模型，引入從短距離到長距離迭代計算思想，設(shè)計了兩階段深度校正算法。探究了像素離散化對校正效果的影響，將算法應(yīng)用于洗浴機(jī)器人中，并與其他深度校正算法進(jìn)行對比。結(jié)果表明，該算法可減小深度誤差，且像素離散化采樣越密集，效果越好。在實際應(yīng)用中，該算法仍能有效減小深度誤差，與其他校正算法相比，在遠(yuǎn)距離處具有優(yōu)勢。該算法能有效提升消費(fèi)級RGB-D相機(jī)的深度精度，且數(shù)據(jù)收集場景簡單，適用于可以產(chǎn)生RGB圖像、深度圖像和點(diǎn)云的傳感器。

關(guān)鍵詞：消費(fèi)級RGB-D相機(jī)；深度校正；局部誤差；全局誤差；像素離散化；深度質(zhì)量評價

中圖分類號：TP391 文獻(xiàn)標(biāo)志碼：A 文章編號：1001-3695（2025）02-028-0523-07

doi：10.19734/j.issn.1001-3695.2024.06.0189

Two-stage depth correction algorithm based on error component modeling

Li Ping^1，2，Yu Hongliu^1’

（1.Institute of Rehabilitation Engineering amp; Technology，University of Shanghai for Science amp; Technology，Shanghai 200093，China;2.Dept.of Biomedical Engineering，Changzhi Medical College，Changzhi Shanxi 046000，China）

Abstract：To improve the depth accuracy of consumer-grade RGB-D cameras，this paper proposed a two-stage depth correction algorithm based on an error component model.The algorithm established an error component model by analyzing error characteristics，employed an iterative calculation approach that progressed from short to long distances，and designed a two-stage depth correction algorithm.This paper explored the effect of pixel discretization on correction accuracy，applied the algorithm to a bathing robot，and compared its performance with that of another depth correction algorithm.The results indicate that this method effectively reduces depth errors，with better performance observed as pixel discretization becomes denser.In practical applications，the algorithm consistently minimizes depth errors and demonstrates superior performance at longer distances compared to alternative correction methods.The algorithm not only enhances the depth accuracy of consumer-grade RGB-D cameras but also features a simple data collection setup，making it suitable for sensors that generate RGB images，depth images，and point clouds.

Key words：consumer-grade RGB-D cameras;depth correction;local errors;global errors;pixel discretization;depth quality evaluation

0 引言

消費(fèi)級RGB-D相機(jī)得到的深度數(shù)據(jù)具有噪聲大、精度較低和存在異常點(diǎn)等缺點(diǎn)^[1]，RGB-D設(shè)備出廠時的校準(zhǔn)參數(shù)質(zhì)量用于游戲和虛擬現(xiàn)實等應(yīng)用場景是足夠的，但對于機(jī)器人應(yīng)用的高精度定位要求是不足夠的。研究用于洗浴機(jī)器人中的消費(fèi)級RGB-D相機(jī)D455的深度校正算法，對實現(xiàn)洗浴機(jī)器人的高精度目標(biāo)定位具有重要意義。

Yamazoe等人^[2]提出了針對Kinect的深度測量誤差模型，基于參數(shù)估計方法進(jìn)行了誤差校正，并考慮了投影儀的失真問題。Jung等人^[3]設(shè)計了2.5D帶孔標(biāo)定板，進(jìn)行距離偏差校正，但適合校準(zhǔn)基于飛行時間法的RGB-D相機(jī)。Herrera等人^[4]提出了一種深度傳感器深度失真模型，提取了深度圖像中棋盤格標(biāo)定平面的四個角，實現(xiàn)了比制造商提供的更好的校準(zhǔn)精度，但存在視差到深度轉(zhuǎn)換中的漂移現(xiàn)象。Darwish等人^[5]采用組合畸變模型對紅外相機(jī)和投影儀產(chǎn)生的畸變和系統(tǒng)誤差進(jìn)行補(bǔ)償，但是只適用于結(jié)構(gòu)光型的RGB-D相機(jī)。Zhang等人^[6]基于攝影測量領(lǐng)域的空間切除率和共線方程建立了Kinect v2相機(jī)的標(biāo)定模型，基于深度圖像中像素的畸變對深度測量值進(jìn)行了校正，但適合大于2 m的遠(yuǎn)距離深度校正。Chen等人^[7]采用稀疏異方差高斯過程來估計深度誤差相對于測量距離的概率分布，利用運(yùn)動捕捉系統(tǒng)對深度進(jìn)行校正，但對設(shè)備要求較高。

目前深度校正工作都是針對Kinect或Kinect v2，而這些產(chǎn)品已經(jīng)停產(chǎn)，機(jī)器人領(lǐng)域目前所使用的消費(fèi)級相機(jī)主要以Intel^? RealSense^TM為主。為了實現(xiàn)面向機(jī)器人應(yīng)用的高定位精度要求，有必要研究Intel^? RealSense^TM消費(fèi)級RGB-D相機(jī)的深度校正算法，以提高其深度值精度。首先，基于深度質(zhì)量評價指標(biāo)，分析RGB-D相機(jī)深度質(zhì)量，并建立包含全局誤差和局部誤差的誤差分量模型；其次，基于誤差隨距離增加而變大的特性，受迭代計算思想啟發(fā)，使用當(dāng)前距離處的校正參數(shù)作為初始值來估計更遠(yuǎn)距離處的誤差校正函數(shù)，分別估計局部校正函數(shù)和全局校正函數(shù)，其中局部校正函數(shù)校正不同像素處的深度誤差，而全局校正函數(shù)校正平均深度值，即可分別校正誤差分量模型中的兩個誤差，同時為了減小計算量進(jìn)行了像素離散化處理；再次，進(jìn)行深度校正實驗，包括收集數(shù)據(jù)、估計函數(shù)、測試校正效果和分析像素離散化對校正效果的影響，并將其應(yīng)用于洗浴機(jī)器人中；最后，與其他深度校正方法進(jìn)行對比。結(jié)果顯示，本文基于誤差分量模型的兩階段深度校正算法是有效的，在實際應(yīng)用場景中也能有效減小深度誤差，與其他深度校正算法相比，本文算法在遠(yuǎn)距離處有更好的校正效果。

1 誤差分量模型

1.1 深度質(zhì)量評價指標(biāo)

深度質(zhì)量評價指標(biāo)是進(jìn)行深度質(zhì)量分析、研究深度校正算法和評價校正結(jié)果的基礎(chǔ)。用Z-accuracy評價深度數(shù)據(jù)的精度，用填充率評估圖像深度覆蓋的百分比，用均方根誤差（root mean square error，RMSE）評價空間噪聲。Z-accuracy表征測量深度值與實際距離GT的接近程度，用每個像素或者ROI上面的平均深度值和GT間的差異來度量。填充率由有效像素的數(shù)目占總像素數(shù)目的百分比表征。RMSE表征深度值的內(nèi)在變化，RMSE計算每個有效像素與最佳擬合平面的均方根誤差^[8]。在三個指標(biāo)中，填充率可以通過在Intel^? RealSense^TM Viewer中手動設(shè)置，主要以指標(biāo)Z-accuracy和RMSE作為深度校正研究中的深度質(zhì)量評價指標(biāo)。

1.2 誤差分量模型

選用測距誤差為1～2 mm的高精度激光測距儀進(jìn)行GT的獲取。將Intel^? RealSense^TM消費(fèi)級RGB-D相機(jī)D455平行于平面墻壁，并不斷增加距離，采集不同距離處的點(diǎn)云以分析深度質(zhì)量。使用兩個高精度激光測距儀測量真實距離，并基于距離相等保證D455和所拍攝墻壁的嚴(yán)格平行。采集的點(diǎn)云示例如圖1所示。

計算同一個GT處采集到點(diǎn)云的Z-accuracy，如式（1）所示，其中i表示點(diǎn)云中點(diǎn)的索引，n₀為點(diǎn)云中點(diǎn)的數(shù)目，j_i為每個點(diǎn)處的距離值，GT表示距離真值，表征深度數(shù)據(jù)平均值的準(zhǔn)確程度，將Z-accuracy建模為全局平均誤差，簡稱全局誤差。通過比較點(diǎn)云中點(diǎn)與擬合點(diǎn)云得到的平面之間的差異，計算平面與點(diǎn)之間距離的RMSE，如式（2）所示，d_0i為點(diǎn)云中第i個點(diǎn)距離擬合平面的距離，與像素位于點(diǎn)云中的位置有關(guān)^[9]，表征每個像素處深度值的錯誤程度，將RMSE建模為局部誤差。此為根據(jù)誤差特性建立的包括全局誤差和局部誤差的誤差分量模型。

Z-accuracy=∑n₀i=1|j_i-GT|n₀（1）

RMSE=∑n₀j=1d_0i²n₀（2）

以GT值為橫軸、全局平均誤差和RMSE為縱軸，得到誤差圖，如圖2和3所示。全局平均誤差和RMSE均隨著距離增加而逐步增大，和GT值呈非線性關(guān)系，且同一距離處全局平均誤差大于RMSE。

2 基于誤差分量模型的兩階段深度校正算法

假設(shè)深度圖像大小為W_d×H_d，[u，v]像素處采集的深度值為z_cj，真實深度值為z_true。建模z_cj和相應(yīng)z_true如式（3）所示，其中函數(shù)f（u，v）需要考慮深度值的全局和局部誤差校正。

z_true=f_{（u，v）}（z_cj）（3）

D455不僅可以拍攝到場景的RGB圖像、深度圖像，還能同時輸出點(diǎn)云信息，點(diǎn)云和深度圖像可以相互轉(zhuǎn)換。由圖2和3可知，局部誤差的存在導(dǎo)致每個像素處的深度測量值偏離GT值，全局誤差的存在會導(dǎo)致平面的深度測量平均值偏離GT值。當(dāng)拍攝平整墻面時，局部誤差體現(xiàn)在每個三維點(diǎn)偏離正確平面的程度，而全局誤差體現(xiàn)在擬合平面與實際平面的偏離。當(dāng)校正上述兩個誤差時，需將每個三維點(diǎn)糾正至正確的平面上，并修正擬合平面和實際平面的偏離。引入棋盤格，角點(diǎn)檢測算法發(fā)展成熟，有助于識別棋盤格及其所在平面。由于誤差隨著距離的增加而變大，引入迭代思想，從短距離開始，使用當(dāng)前距離處的校正參數(shù)作為初始值來估計更遠(yuǎn)距離處的誤差校正函數(shù)?；谡`差分量模型的深度校正算法，分兩個階段完成：a）局部校正函數(shù)f_l用于校正不同像素處的深度誤差，每個像素處都應(yīng)該計算一個f_l，使平整墻面中的三維點(diǎn)糾正至一個平面上；b）全局校正函數(shù)f_g用于校正平均深度值，上述平面被修正至正確的位置處。棋盤格在理論下應(yīng)形成一個平面^[10]，匹配RGB相機(jī)視野中的棋盤平面和第一步中校正過的點(diǎn)云平面，用來估計全局誤差。但RGB圖和深度圖像所在的參考系不同，兩個參考系間存在剛性變換關(guān)系，需要獲知RGB圖像與點(diǎn)云之間的變換矩陣^C_DM才能進(jìn)行函數(shù)擬合，同時計算外參的過程依賴于準(zhǔn)確的內(nèi)參^[11]。為了解決互為依賴的問題，采取同時優(yōu)化f_g和^C_DM的方案。

基于誤差分量模型的兩階段深度校正算法如圖4所示。從RGB圖像中提取棋盤格角點(diǎn)，從深度圖像中生成點(diǎn)云。使用棋盤格角點(diǎn)、相應(yīng)點(diǎn)云和出廠的粗略變換矩陣^C_DM₀估計f_l。估計出f_l后，將f_l作用于點(diǎn)云以校正局部誤差，并將校正過后的點(diǎn)云、棋盤格角點(diǎn)、墻面點(diǎn)云子集聯(lián)合用于計算f_g和^C_DM。

2.1 局部校正函數(shù)估計

局部校正函數(shù)估計流程如圖5所示。在平整墻面上粘貼棋盤格，基于墻面到相機(jī)的距離升序排列點(diǎn)云，迭代處理每一個點(diǎn)云。使用f_l的當(dāng)前估計值作用于初始點(diǎn)云進(jìn)行局部誤差校正。從校正后的點(diǎn)云中提取墻點(diǎn)，得到一個點(diǎn)云的子集并擬合一個平面plane，以加快運(yùn)算效率^[12]，平面plane用來更新f_l?；谝暰嗤队疤崛〕鲇糜跀M合f_l的數(shù)據(jù)，每處理一次校正，點(diǎn)云便產(chǎn)生一組用于擬合f_l的數(shù)據(jù)。迭代處理所有的點(diǎn)云，實現(xiàn)f_l的迭代更新。迭代結(jié)束后，便得到最終的f_l。同時進(jìn)行深度圖像像素離散化以減少需計算的f_l數(shù)量。

2.1.1 提取墻面點(diǎn)子集

提取RGB圖像中棋盤格的角點(diǎn)，以確定正確的平面^[13]，并基于^C_DM₀定位局部誤差校正后點(diǎn)云中的正確平面?；赗ANSAC方法提取校正點(diǎn)云中正確平面的子集。RANSAC是一種隨機(jī)估計參數(shù)的方法^[14～16]，用于提取平面的流程如圖6所示。

2.1.2 更新f_l

設(shè)平面plane方程如式（4）所示，其中n=[A₁，B₁，C₁]^T為平面法向量，d_N表示視點(diǎn)到平面plane的距離。點(diǎn)云中任一三維點(diǎn)沿視距投影到平面plane上，如圖7所示，O表示視點(diǎn)，A表示點(diǎn)云中任一三維點(diǎn)，連接A、O并與平面plane交于B點(diǎn)，過A點(diǎn)作平面plane的法線，與平面交于C點(diǎn)。過O點(diǎn)作平行于線段BC的直線并與AC交于點(diǎn)D?？梢?，CD長度為d_N。從圖7可知式（5）成立，其中X=[x，y，z]^T，X_N=[x_plane，y_plane，z_plane]^T?；喌玫绞剑?），由此可知z_plane的值。由原始的z和z_Plane組成一組數(shù)據(jù)進(jìn)行曲線擬合得到f_l，此f_l在下一次迭代中被用來校正初始點(diǎn)云，下一輪迭代將按照同樣的方法重新產(chǎn)生一組數(shù)據(jù)對（z，z_plane）再次實現(xiàn)對f_l的估計，從而實現(xiàn)f_l的迭代更新。f_l的作用是將圖7中錯誤的A點(diǎn)糾正到正確的平面上。

n^TX-d_N=0（4）

XX_N=n^TXd_N（5）

X_N=d_NXn^TX（6）

2.1.3 深度圖像像素離散化

假設(shè)深度圖像大小為640×480，理論上為每個像素計算一個f_l，產(chǎn)生很多時間消耗，而且部分像素處的噪聲導(dǎo)致對f_l估計的誤差增大，魯棒性降低。引入系數(shù)Ω_h和Ω_w，表征H_D和W_D方向離散采樣后得到的像素大小的個數(shù)，如圖8所示，只給采樣的像素估計f_l。深度圖像H_D×W_D中未被采樣到的像素點(diǎn)處的f_l由采樣到像素處的f_l計算。假設(shè)深度圖像中的一個像素是[u_d，v_d]，它的f_l函數(shù)由緊鄰其四周的已經(jīng)被采樣到的四個像素處的f_l進(jìn)行線性組合而得到，如式（7）所示，其中S（u_d，v_d）是所有被采樣到的像素的集合，r_W和r_H的定義如式（8）所示，且滿足式（9）的條件。

f_l（u_d，v_d）=∑（s_d，t_d）∈S（u_d，v_d）r_W（u_d，s_d）r_H（v_d，t_d）f_l（s_d，t_d）（7）

r_W（u_d，s_d）=1-|u_d-s_d|W_D/Ω_w

r_H（v_d，t_d）=1-|v_d-t_d|H_D/Ω_h（8）

∑（s_d，t_d）∈S（u_d，v_d）r_W（u_d，s_d）r_H（v_d，t_d）=1（9）

由以上分析可知，W_D×H_D大小的深度圖像，在像素離散化處理之前需要計算（W_D×H_D）個f_l，處理之后需要計算的f_l個數(shù)GS如式（10）所示。由圖8可知，Ω_h的最小值和最大值分別是1和H_D，Ω_w的最小值和最大值分別是1和W_D，極端情況下才能取到最小和最大值?？梢?，經(jīng)過對像素的離散化采樣后，需要計算的函數(shù)量變少，可以有效提升算法的時間利用率。

GS=Ω_h×Ω_w（10）

校正函數(shù)的選擇參考了文獻(xiàn)[17]，如式（11）所示。對每個采樣到的像素，需要準(zhǔn)確計算出三個參數(shù)c_i（i=0，1，2），故離散化后需要估計的參數(shù)個數(shù)為GS×3。為了正確估計出系數(shù)c_i，抽象為非線性最小二乘問題^[18]，如式（12）所示，其中z和z_plane是離散化采樣后得到的像素中的相應(yīng)數(shù)據(jù)對。

f_l（u，v）=c₀z²+c₁z+c₂（11）

argminc₀，c₁，c₂∑（z，z_plane）‖c₀z²+c₁z+c₂-z_plane‖²（12）

經(jīng)過f_l處理后，將點(diǎn)云中所有錯誤的點(diǎn)糾正到了擬合平面上。顯然，經(jīng)過f_l處理后，擬合平面與點(diǎn)云中點(diǎn)之間距離的RMSE減小。由誤差分量模型的建立過程可知，這意味著起到了減小局部誤差的作用，即達(dá)到了校正局部誤差的目的。

2.2 全局校正函數(shù)估計

全局校正函數(shù)估計流程如圖9所示。同一場景下RGB圖像中、對應(yīng)點(diǎn)云中的平面在經(jīng)過一定的旋轉(zhuǎn)和平移后可以重合，且旋轉(zhuǎn)矩陣和平移向量也是RGB圖像坐標(biāo)系和深度圖像坐標(biāo)系間的剛性變換關(guān)系^[19]?；诖?，利用RGB圖像和相應(yīng)點(diǎn)云中的平面方程來估計此變換關(guān)系，并將此變換作用于RGB圖像坐標(biāo)系中的平面方程，從而得到深度圖像坐標(biāo)系下的平面方程P，以此實現(xiàn)對f_g的更新。和估計f_l一樣，基于視距投影從平面P和真實校正點(diǎn)云中提取用于估計f_g的數(shù)據(jù)對，將校正點(diǎn)云中點(diǎn)的錯誤深度值修正為正確的深度值。為了減少計算量，同樣進(jìn)行了像素離散化采樣。

2.2.1 計算^C_DM₀

計算f_g時使用了^C_DM₀。提取RGB圖像中棋盤格平面P₁，提取深度圖像坐標(biāo)系下校正點(diǎn)云中的平面P₂，從兩個平面的信息標(biāo)定出深度圖像坐標(biāo)系和RGB圖像坐標(biāo)系的初始變換矩陣^C_DM₀。

如圖10所示，給定n₂個棋盤格上的3D空間參考點(diǎn)及相應(yīng)在相機(jī)圖像上對應(yīng)的投射點(diǎn)，求解棋盤格坐標(biāo)系與相機(jī)的空間關(guān)系，一般可以利用棋盤格并通過OpenCV提供的solvePnP函數(shù)來求解，是典型的PnP問題^[20～22]。將棋盤格坐標(biāo)系中的三維空間點(diǎn)變換到RGB相機(jī)坐標(biāo)系下，通過平面擬合得到P₁的方程^[23～25]。三個非共線點(diǎn)就能決定一個平面，所以計算P₁方程時只使用棋盤格上三個非共線角點(diǎn)?？紤]到點(diǎn)云是三維空間中的點(diǎn)，使用奇異值分解（singular value decomposition，SVD）擬合三維空間平面來求解P₂方程^[26]。

設(shè)點(diǎn)云中有n₀個三維點(diǎn)，P₂方程如式（13）所示。設(shè)p₀是三維點(diǎn)集合的重心，其坐標(biāo)為全部點(diǎn)的三維坐標(biāo)平均值，則p₀位于平面P₂上，有式（14）成立。p₀到點(diǎn)集中其他點(diǎn)p_i形成的向量位于平面P₂內(nèi)，與法向量垂直，如式（15）所示。并不是點(diǎn)集中所有的點(diǎn)都嚴(yán)格位于平面P₂上，所以以點(diǎn)到P₂的距離之和最小建立目標(biāo)函數(shù)^[27，28]，如式（16）所示，其中F為式（15）中的系數(shù)矩陣，取L₁范數(shù)。對F進(jìn)行奇異值分解，將分解結(jié)果帶入目標(biāo)函數(shù)，式（16）變?yōu)槭剑?7）。Σ的對角元素為奇異值，若最后一個對角元素為最小奇異值，當(dāng)且僅當(dāng)式（18）成立時目標(biāo)函數(shù)最小。此時F的最小奇異值對應(yīng)的奇異向量就是方程的最優(yōu)解，如式（19）所示。d的計算方法如式（20）所示。根據(jù)法向量n、原點(diǎn)到平面的距離d便獲得平面P₂的方程。

確定變換矩陣^C_DM₀采用兩階段算法。假設(shè)共有n₃對平面，P_1，i和相應(yīng)的P_2，i（i=1，2，…，n₃）的法向量和原點(diǎn)到平面的距離分別為n_c、n_d、d_c、d_d。在第一個階段，將平移和旋轉(zhuǎn)解耦。兩者之間的平移變換如式（21）所示。在兩個坐標(biāo)系下，通過最小化原點(diǎn)到對應(yīng)平面的法線之間的差異確定旋轉(zhuǎn)變換，如式（22）所示，其中V₁和U₁滿足式（23）。在第二個階段，通過最小化點(diǎn)云中的點(diǎn)到P_1，i的距離，將第一階段求解出來的值作為初值進(jìn)行迭代優(yōu)化求解。

T₁=（n^T_cn_c）^-1n^T_c（d_c-d_d）（21）

R₁=V₁U^T₁（22）

n_dn^T_c=U₁Σ₁V^T₁（23）

假設(shè)P_d，i為第i個點(diǎn)云中相關(guān)點(diǎn)組成的矩陣，如式（24）所示，m為第i個點(diǎn)云中點(diǎn)的數(shù)量。將旋轉(zhuǎn)和平移關(guān)系的求解看作式（25）所示的最小化問題，取L₂范數(shù)。假設(shè)優(yōu)化后的解是R₂和T₂，此時由R₂和T₂組成^C_DM₀。

P_d，i=x⁰_d，ix¹_d，i…x^m_d，i

y⁰_d，iy¹_d，i…y^m_d，i

z⁰_d，iz¹_d，i…z^m_d，i（24）

argminR，T=∑n₂i=1∑mj=1‖n^T_c，i（RP^j_d，i+T）-d_ci‖（25）

使用^C_DM₀作用于P_1，i將其轉(zhuǎn)換到深度圖像坐標(biāo)系下，結(jié)合初始平面P_2，i計算曲線擬合的數(shù)據(jù)對，生成數(shù)據(jù)對的原理與估計f_l一樣。

2.2.2 聯(lián)合優(yōu)化f_g和^C_DM

將^C_DM₀作為初值，經(jīng)過非線性優(yōu)化后輸出最終的全局校正函數(shù)f_g和變換矩陣^C_DM。為了解決彼此之間的依賴關(guān)系，將f_g和^C_DM放在同一框架內(nèi)進(jìn)行優(yōu)化，還同時優(yōu)化了棋盤格位姿。設(shè)有n₄對數(shù)據(jù)，一對數(shù)據(jù)包括一張含有棋盤格的RGB圖像、同一場景的校正點(diǎn)云。在求解棋盤格位姿時，將世界坐標(biāo)系原點(diǎn)建立在棋盤格的左上角第一個角點(diǎn)上，沿棋盤格的邊長方向分別建立x軸和y軸，以右手定則確定z軸，得到棋盤格上角點(diǎn)集合^BP，B表示在棋盤格坐標(biāo)系下，進(jìn)而得到RGB圖像坐標(biāo)系下對應(yīng)投影點(diǎn)的集合^（u，v）P。結(jié)合相機(jī)內(nèi)參可以得到棋盤格位姿^C_BM，則有式（26）成立，其中^cP表示RGB相機(jī)坐標(biāo)系下棋盤格上角點(diǎn)集合。結(jié)合RGB圖像和深度圖像坐標(biāo)系之間的轉(zhuǎn)換矩陣^C_DM，有式（27）成立，其中^DP表示深度圖像坐標(biāo)系下棋盤格上角點(diǎn)集合。

^cP=^C_BM^BP（26）

^DP=^D_CM^cP（27）

通過最小化兩個誤差實現(xiàn)對f_g和^C_DM的聯(lián)合優(yōu)化，如式（28）所示，E_p表示由RGB圖中棋盤格定義的平面和校正點(diǎn)云中擬合的平面之間的誤差，E_r表示RGB圖像坐標(biāo)系下棋盤格上角點(diǎn)的重投影誤差^[29～31]。E_p用當(dāng)前f_g修正過的點(diǎn)云和它在由^DP定義的平面上的視線投影之間的距離來度量，如式（29）所示，P-c表示使用f_l進(jìn)行局部誤差校正后的點(diǎn)云，f_g（§）表示用當(dāng)前f_g的值校正點(diǎn)云§，p_plane（§）表示將§正交投影到棋盤格平面plane上，Π_k表示第k個數(shù)據(jù)對中的所有棋盤格角點(diǎn)^{（u，v）}P。E_r指三維空間點(diǎn)在圖像平面上的投影即圖像上真實像素點(diǎn)坐標(biāo)和基于當(dāng)前估計位姿計算值投影得到的虛擬像素點(diǎn)之間的誤差，如式（30）所示，^IP表征圖像平面上的真實投影像素點(diǎn)，I_B表征所有基于二維像素坐標(biāo)的棋盤格角點(diǎn)，proj是與相機(jī)的物理特性相關(guān)的投影函數(shù)。

minf_g，DCM∑n₄k=1E_p（k）+E_r（k）（28）

E_p（k）=∑（u，v）∈Π_k‖p_plane（f_g（P-c（u，v）））-f_g（P-c（u，v））‖²（29）

E_r（k）=∑（q，r）∈I_B‖proj（^C_BkM^BP_k（q，r））-^IP_k（q，r）‖²（30）

2.2.3 深度圖像像素離散化

參照f_l離散化過程，在估計f_g時也進(jìn)行了像素離散化。與f_l不同，由于所需函數(shù)數(shù)量大量減少，只取深度圖像四個角點(diǎn)處的像素。為了防止過約束的出現(xiàn)，實際只使用其中三個像素，第四個像素處的函數(shù)由這四個像素的共面約束來推導(dǎo)^[32]。如圖11所示，紅色圓點(diǎn)表示選擇的像素點(diǎn)，橙色圓點(diǎn)表示由共面約束推導(dǎo)的像素點(diǎn)，黃色圓點(diǎn)表示除這四個特殊像素點(diǎn)外其余任一像素點(diǎn)（參見電子版）。橙色圓點(diǎn)像素處的f_g由式（31）估計。黃色圓點(diǎn)像素處的f_g是角點(diǎn)像素f_g的線性組合。考慮到圖2中誤差是非線性的且和圖3中曲線走向具有一定的相似性，參考f_l函數(shù)的選擇，也選擇f_g為二次多項式函數(shù)。

f_g（0，0）（d）+f_{g（WD，HD）}（d）2=f_{g（WD，0）}（d）+f_{g（0，HD）}（d）2（31）

3 深度校正實驗及結(jié)果

3.1 數(shù)據(jù)收集

使用D455在不同的距離和方向上收集實驗場景的數(shù)據(jù)，場景包括一堵粘貼有棋盤格的平整墻面?；贒455的驅(qū)動包和相應(yīng)的ROS包，通過launch文件啟動相機(jī)，并通過設(shè)置true/1選擇所需要的話題進(jìn)行發(fā)布，采集視野中粘貼有棋盤格的平整墻壁，數(shù)據(jù)包括RGB圖像、深度圖像和點(diǎn)云。為了測試算法性能，還收集了用于測試深度校正性能的數(shù)據(jù)。收集數(shù)據(jù)的裝置如圖12所示，包括D455和兩個高精度激光測距儀^[33]，激光測距儀用來測量D455距離墻面的距離并保證墻面與D455的平行。在同一個距離處，將棋盤格粘貼于墻面中的不同位置處，采集相應(yīng)數(shù)據(jù)對，并且保證收集到的數(shù)據(jù)中只包括墻面和棋盤格^[34]，而不包括地板、天花板或其他任何物體。采集時，保證D455采集到的各個數(shù)據(jù)對是同時采集同一個場景的，而且棋盤格和相機(jī)之間不能發(fā)生相對運(yùn)動。收集的RGB圖像和深度圖像尺寸均為640×480。距離取值范圍根據(jù)使用D455的機(jī)器人工作空間及目標(biāo)與D455間的距離范圍來確定，D455和平整墻面的距離信息如表1所示，數(shù)據(jù)來源于兩個激光測距儀的測量值。

3.2 校正實驗結(jié)果

采用C++編寫深度校正程序代碼，其中使用PCL處理點(diǎn)云^[35]。建立工作空間，在工作空間src文件夾下添加程序代碼，并編寫launch文件。程序編譯通過后，運(yùn)行l(wèi)aunch文件，輸出校正算法所需要的f_l和f_g中的各系數(shù)。若H_D和W_D分別為480、640，Ω_h和Ω_w分別取60和80，此時每個像素塊大小為8×8，原來需要估計的f_l數(shù)量為640×480，現(xiàn)在需要估計的f_l數(shù)量為80×60，算法輸出f_l中80×60×3個系數(shù)，而估計f_g共需輸出系數(shù)3×3個。第四個角點(diǎn)像素處的f_g根據(jù)平面約束進(jìn)行計算，算法會輸出第四個像素處f_g的系數(shù)，最終輸出4×3個系數(shù)。

對640×480的深度圖像進(jìn)行不同離散化采樣，參數(shù)取值如表2所示，Ω_w和Ω_h表征W_D和H_D方向離散采樣后得到的像素塊的個數(shù)，對應(yīng)離散化像素塊大小依次為64×48、32×32、16×16、8×8、4×4、2×2、1×1。為了評估校正性能，對每一種離散化采樣配置，利用式（1）（2）對f_l校正前后、f_g校正前后數(shù)據(jù)的全局平均誤差和RMSE進(jìn)行了計算，結(jié)果如圖13～16所示。

如圖13和14所示，對于所有的離散化采樣配置，經(jīng)f_l校正后全局平均誤差均減小，再經(jīng)f_g校正后進(jìn)一步減小，且取不同的像素離散化采樣參數(shù)，將有不同的校正效果。Ω_w和Ω_h越大，像素采樣越密集，全局平均誤差越小，校正效果越好，但伴隨著估計參數(shù)的增多及計算效率的降低。如圖15和16所示，對于所有的離散化采樣配置，經(jīng)f_l校正后RMSE均減小，再經(jīng)f_g校正后進(jìn)一步減小，且取不同的像素離散化采樣參數(shù)，將有不同的校正效果。Ω_w和Ω_h越大，像素采樣越密集，RMSE越小，校正效果越好，但伴隨著估計參數(shù)的增多及計算效率降低。當(dāng)Ω_w和Ω_h分別取80和60時，即像素塊大小為8×8時，與1×1像素塊具有相似的校正效果，但是估計參數(shù)少，能有效提升計算效率?？梢?，在兩個階段的校正過程中，8×8的像素塊是較佳的配置。

基于誤差分量模型的深度校正是有效的，能有效減小深度誤差，提高深度值的精度。但深度誤差值并沒有降為0，可能存在偶然或無法預(yù)測的因素導(dǎo)致存在不能被完全消除的隨機(jī)誤差。為了平衡校正效果和計算效率，可取Ω_w和Ω_h為80和60，即像素塊為8×8的離散化設(shè)置，它既能有效減少估計的函數(shù)參數(shù)，又可以達(dá)到較好的深度校正效果。

3.3 在洗浴機(jī)器人中的應(yīng)用實驗

在洗浴機(jī)器人應(yīng)用實驗中，D455安裝在基座處，通過YOLOv5s識別相機(jī)視野中人體模型的背部區(qū)域，如圖17所示，取紅色背部區(qū)域中心點(diǎn)為目標(biāo)點(diǎn)，獲取此點(diǎn)深度值，并使機(jī)器人定位至目標(biāo)點(diǎn)，如圖18所示。深度校正算法使用Ω_w和Ω_h分別為80和60的參數(shù)配置，從深度實際位置和深度理想位置的差異度量校正算法的性能。實際位置取機(jī)器人定位后的示教器讀數(shù)，而理想位置取末端六維力傳感器為零時的示教器讀數(shù)，誤差取兩者之差的絕對值。深度校正前后誤差結(jié)果如表3所示，深度校正前后誤差均值分別為18.329 mm和1.111 mm。實驗結(jié)果表明在實際應(yīng)用中，深度校正算法仍能有效減小深度誤差。

4 與其他深度校正方法對比

本文方法使用的數(shù)據(jù)包括RGB圖像、深度圖像和點(diǎn)云，場景包括粘貼了棋盤格的墻面。為了更好地評估本文算法的性能，與文獻(xiàn)[4]中的深度校正方法進(jìn)行比較。與本文相似，它開發(fā)了基于棋盤格的校正程序。深度校正算法仍使用Ω_w和Ω_h分別為80和60的參數(shù)配置，并選擇全局平均誤差和RMSE作為性能評估指標(biāo)。結(jié)果如圖19和20所示。

由圖19和20可知，兩種方法在近距離處具有相似的校正效果，而本文算法在遠(yuǎn)距離處的效果更好，可能是因為迭代計算的方式使得遠(yuǎn)距離處的校正過程擁有了較好的初值。

5 結(jié)束語

基于深度質(zhì)量評價指標(biāo)，建立誤差分量模型，將Z-accuracy建模為全局誤差，表征輸出平均深度值的錯誤程度，將RMSE建模為局部誤差，表征每個像素處深度值的錯誤程度?；谡`差分量模型建立了兩階段深度校正方法，通過局部校正函數(shù)f_l和全局校正函數(shù)f_g對局部誤差和全局誤差進(jìn)行校正。經(jīng)過兩個階段的校正函數(shù)處理，誤差分量模型中的全局和局部誤差均減小，像素離散化采樣參數(shù)對校正效果有直接影響，Ω_w和Ω_h越大，采樣越密集，校正效果越好，計算效率越低。應(yīng)用于洗浴機(jī)器人中時，算法仍能較好地校正深度誤差。與其他校正算法相比，本文算法在遠(yuǎn)距離處具有較好的性能。校正場景簡單，只包括張貼一張棋盤格的平整墻面。算法設(shè)計思想并未考慮相機(jī)型號，適用于可以產(chǎn)生RGB圖像、深度圖像和點(diǎn)云的傳感器。不足之處是，f_l和f_g均采用了二次多項式函數(shù)，校正不同型號RGB-D相機(jī)時，算法代碼可能需要調(diào)整。

參考文獻(xiàn)：

［1］Wu Jian，Wang Lili.Panoramic ray tracing for interactive mixed reality rendering based on 360° RGBD video[J].IEEE Computer Gra-phics and Applications，2024，44（1）：62-75.

[2]Yamazoe H，Habe H，Mitsugami I，et al.Depth error correction for projector-camera based consumer depth cameras[J].Computational Visual Media，2018，4（2）：103-111.

[3]Jung J，Lee J Y，Jeong Y，et al.Time-of-flight sensor calibration for a color and depth camera pair[J].IEEE Trans on Pattern Analysis and Machine Intelligence，2015，37（7）：1501-1513.

[4]Herrera D C，Kannala J，Heikkila J.Joint depth and color camera calibration with distortion correction[J].IEEE Trans on Pattern Analysis and Machine Intelligence，2012，34（10）：2058-2064.

[5]Darwish W，Tang Shenjun，Li Wenbin，et al.A new calibration method for commercial RGB-D sensors[J].Sensors，2017，17（6）：article ID 1204.

[6]Zhang Chenyang，Huang Teng，Zhao Qiang.A new model of RGB-D camera calibration based on 3D control field[J].Sensors，2019，19（23）：article ID 5082.

[7]Chen Guangda，Cui Guowei，Jin Zhongxiao，et al.Accurate intrinsic and extrinsic calibration of RGB-D cameras with GP-based depth correction[J].IEEE Sensors Journal，2019，19（7）：2685-2694.

[8]Gupta H，Lilienthal A J，Andreasson H，et al.NDT-6D for color registration in agri-robotic applications[J].Journal of Field Robotics，2023，40（6）：1603-1619.

[9]Sun Zhiliang，Zhang Rongguo，Hu Jing，et al.Probability re-weighted 3D point cloud registration for missing correspondences[J].Multimedia Tools and Applications，2022，81（8）：11107-11126.

[10]Fryskowska A.An improvement in the identification of the centres of checkerboard targets in point clouds using terrestrial laser scanning[J].Sensors，2019，19（4）：article ID 938.

[11]Zhang Xiaowen，Ren Yongfeng，Zhen Guoyong，et al.Camera calibration method for solid spheres based on triangular primitives[J].Precision Engineering，2020，65：91-102.

[12]Zhu Xianzhang，Liu Xinyi，Zhang Yongjun，et al.Robust 3-D plane segmentation from airborne point clouds based on quasi-a-contrario theory[J].IEEE Journal of Selected Topics in Applied Earth Observations and Remote Sensing，2021，14：7133-7147.

[13]Sang Qiang，Huang Tao，Wang Hongyi.An improved checkerboard detection algorithm based on adaptive filters[J].Pattern Recognition Letters，2023，172：22-28.

[14]Chung K L，Chang W T.Centralized RANSAC-based point cloud re-gistration with fast convergence and high accuracy[J].IEEE Journal of Selected Topics in Applied Earth Observations and Remote Sensing，2024，17：5431-5442.

[15]Wu Qinghua，Liu Jiacheng，Gao Can，et al.Improved RANSAC point cloud spherical target detection and parameter estimation method based on principal curvature constraint[J].Sensors，2022，22（15）：Article ID 5850.

[16]Xu Guangxuan，Pang Yajun，Bai Zhenxu，et al.A fast point clouds re-gistration algorithm for laser scanners[J].Applied Sciences-Basel，2021，11（8）：article ID 3426.

[17]Canessa A，Chessa M，Gibaldi A，et al.Calibrated depth and color cameras for accurate 3D interaction in a stereoscopic augmented reality environment[J].Journal of Visual Communication and Image Representation，2014，25（1）：227-237.

[18]Feng Zhiao，Han Xianfeng.Guided normal filter for 3D point clouds[J].Multimedia Tools and Applications，2023，82（9）：13797-13810.

[19]Yang Liang，Dryanovski I，Valenti R G，et al.RGB-D camera calibration and trajectory estimation for indoor mapping[J].Autonomous Robots，2020，44（8）：1485-1503.

[20]Adli S E，Shoaran M，Noorani S M S.GSPnP：simple and geometric solution for PnP problem[J].Visual Computer，2020，36（8）：1549-1557.

[21]Boixader A，Labella C，Catalan M，et al.A high-accuracy，scalable and affordable indoor positioning system using visible light positioning for automated guided vehicles[J].Electronics，2024，13（1）：article ID 82.

[22]陳津平，吳曉亮.參考點(diǎn)共面條件下的穩(wěn)健相機(jī)位姿估計方法[J].激光與光電子學(xué)進(jìn)展，2024，61（4）：147-155.（Chen Jinping，Wu Xiaoliang.Robust camera pose estimation method under coplanar reference points condition[J].Laser and Optoelectronics Progress，2024，61（4）：147-155.）

[23]Xu Yanan，So Y H，Woo S.Plane fitting in 3D reconstruction to preserve smooth homogeneous surfaces[J].Sensors，2022，22（23）：article ID 9391.

[24]李普山，李偉波，馮智莉，等.基于GPU并行的點(diǎn)云數(shù)據(jù)簡化的改進(jìn)算法[J].計算機(jī)應(yīng)用研究，2020，37（9）：2730-2733.（Li Pu-shan，Li Weibo，F(xiàn)eng Zhili，et al.Improved algorithm for point cloud data simplification based on GPU parallel[J].Application Research of Computers，2020，37（9）：2730-2733.）

[25]楊少舟，龍東平，陳繼堯，等.基于M估計算法的三維點(diǎn)云平面擬合方法研究[J].電子測量技術(shù)，2024，47（5）：70-76.（Yang Shaozhou，Long Dongping，Chen Jiyao，et al.Research on three-dimensional point cloud plane fitting method based on M-estimation algorithm[J].Electronic Measurement Technology，2024，47（5）：70-76.）

[26]Daghigh H，Tannant D D，Jaberipour M.A computationally efficient approach to automatically extract rock mass discontinuities from 3D point cloud data[J].International Journal of Rock Mechanics and Mining Sciences，2023，172：article ID 105603.

[27]童子良，余學(xué)祥，汪濤，等.一種新的魯棒三維點(diǎn)云平面擬合方法[J].合肥工業(yè)大學(xué)學(xué)報：自然科學(xué)版，2020，43（12）：1684-1688.（Tong Ziliang，Yu Xuexiang，Wang Tao，et al.A new robust 3D point cloud plane fitting method[J].Journal of Hefei University of Technology：Natural Science，2020，43（12）：1684-1688.）

[28]Zhang Wenyuan，Li Zhixin，Shan Jie.Optimal model fitting for buil-ding reconstruction from point clouds[J].IEEE Journal of Selected Topics in Applied Earth Observations and Remote Sensing，2021，14：9636-9650.

[29]賴欣，楊肖，張啟燦.基于自適應(yīng)EKF的攝像機(jī)標(biāo)定優(yōu)化方法[J].光學(xué)學(xué)報，2023，43（23）：199-208.（Lai Xin，Yang Xiao，Zhang Qican.Adaptive EKF-based camera calibration optimization method[J].Acta Optica Sinica，2023，43（23）：199-208.）

[30]張子淼，張昊，許凱，等.基于光偏振信息的攝像機(jī)標(biāo)定方法研究[J].傳感技術(shù)學(xué)報，2023，36（3）：389-396.（Zhang Zimiao，Zhang Hao，Xu Kai，et al.Research on the camera calibration method based on light polarization information[J].Chinese Journal of Sensors and Actuators，2023，36（3）：389-396.）

[31]華楊，黃福珍.基于改良天鷹優(yōu)化器的單目相機(jī)標(biāo)定方法[J].電子測量技術(shù)，2023，46（16）：126-137.（Hua Yang，Huang Fuzhen.Monocular camera calibration method based on modified aquila optimizer[J].Electronic Measurement Technology，2023，46（16）：126-137.）

[32]王耀東，徐金楊，朱力強(qiáng)，等.隧道輪廓激光雷達(dá)點(diǎn)云線特征共面約束標(biāo)定方法[J].光學(xué)精密工程，2024，32（6）：774-784.（Wang Yaodong，Xu Jinyang，Zhu Liqiang，et al.Line feature coplanar constraint calibration method for lidar point cloud of tunnel contour[J].Optics and Precision Engineering，2024，32（6）：774-784.）

[33]Xu Wenbo，Zheng Xinhui，Tian Qiyan，et al.Study of underwater large-target localization based on binocular camera and laser rangefinder[J].Journal of Marine Science and Engineering，2024，12（5）：article ID 734.

[34]Liu Jie，Zhou Dianzhuo，Wang Yifan，et al.A distance measurement approach for large fruit picking with single camera[J].Horticulturae，2023，9（5）：article ID 537.

[35]Li Lin，Chen Xi，Zhang Tie.Pose estimation of metal workpieces based on RPM-Net for robot grasping from point cloud[J].Industrial Robot，2022，49（6）：1178-1189.