機器視覺中，3D相機產生的深度圖像（depth image）通常需要配准（registration），以生成配准深度圖像（registed depth image）。實際上配准的目的就是想讓深度圖和彩色圖重合在一起，即是將深度圖像的圖像坐標系轉換到彩色圖像的圖像坐標系下。下面我們來介紹其推導的過程。
1. 原理
為了描述方便，首先做些簡單的假設。如下圖所示，3D相機的左側相機（left camera）為紅外相機（即深度相機，ir camera），右側相機（right camera）為彩色相機（color camera）。現在主流的3D相機都是這樣的布局，如xtion，kiinect，orbbict。
已知彩色圖像的像素表示為\((u_{R}, v_{R}, z_{R})^{\top}\)，\(u_{R}, v_{R}, z_{R}\)分別表示彩色圖像的橫坐標，縱坐標和相機坐標系下的深度值（z方向上的值，非兩點的距離）；同樣地，深度圖像的像素為\((u_{L}, v_{L}, z_{L})^{\top}\)，\(u_{L}, v_{L}, z_{L}\)分別表示深度圖像的橫坐標，縱坐標和相機坐標系下的深度值（z方向上的值，非兩點的距離）。注意為了方便表示，本文中下標的R,L分別表示Right,Left的意思。那么深度圖配准到彩色圖的過程就是找到如下公式中的變換矩陣\(W^{'}\)：
\(\begin{bmatrix}
u_{R}\\
v_{R}\\
1
\end{bmatrix}
=W^{'}
\begin{bmatrix}
u_{L}\\
v_{L}\\
1
\end{bmatrix}\)
實際上，變換矩陣\(W^{'}\)並不是真正要求解的矩陣，而是需要重新構造新的變換矩陣，以滿足矩陣可逆性。構造過程如下：
a. 構造左側相機的左側相機坐標系到圖像坐標系的變換
由相機原理（可參考相機標定(2)---攝像機標定原理），可知相機坐標到圖像坐標的變換為：
\(z_{L}
\begin{bmatrix}
u_{L}\\
v_{L}\\
1
\end{bmatrix}=\begin{bmatrix}
f/dx & 0 & u_{L}^{0}&0\\
0 & f/dy & v_{L}^{0}&0\\
0 & 0& 1&0
\end{bmatrix}
\begin{bmatrix}
x_{L}\\
y_{L}\\
z_{L}\\
1
\end{bmatrix}
\)
以上變換過程等價於如下的表達
\(z_{L}
\begin{bmatrix}
u_{L}\\
v_{L}\\
1\\
1/z_{L}
\end{bmatrix}=
\underset{LR}{\underbrace{
\begin{bmatrix}
f/dx & 0 & u_{L}^{0}&0\\
0 & f/dy & v_{L}^{0}&0\\
0 & 0& 1&0 \\
0 & 0& 0&1
\end{bmatrix}
}}
\begin{bmatrix}
x_{L}\\
y_{L}\\
z_{L}\\
1
\end{bmatrix}\)
於是圖像坐標系到相機坐標系的變換為：
\(\begin{bmatrix}
x_{L}\\
y_{L}\\
z_{L}\\
1
\end{bmatrix}=z_{L}*LR^{-1}*\begin{bmatrix}
u_{L}\\
v_{L}\\
1\\
1/z_{L}
\end{bmatrix}\) (1)
其中\(LR\)為雙目相機標定的左側相機內參矩陣,LR表示為Left Rotation之意
b. 構造右側相機的右側相機坐標系到圖像坐標系的變換
根據a，同理地，左側相機坐標系到圖像坐標系的變換為：
\(z_{R}
\begin{bmatrix}
u_{R}\\
v_{R}\\
1\\
1/z_{R}
\end{bmatrix}=
\underset{RR}{\underbrace{
\begin{bmatrix}
f/dx & 0 & u_{R}^{0}&0\\
0 & f/dy & v_{R}^{0}&0\\
0 & 0& 1&0 \\
0 & 0& 0&1
\end{bmatrix}
}}
\begin{bmatrix}
x_{R}\\
y_{R}\\
z_{R}\\
1
\end{bmatrix}\)
於是圖像坐標系到相機坐標系的變換為：
\(\begin{bmatrix}
x_{R}\\
y_{R}\\
z_{R}\\
1
\end{bmatrix}=z_{R}*RR^{-1}*\begin{bmatrix}
u_{R}\\
v_{R}\\
1\\
1/z_{R}
\end{bmatrix}\) (2)
其中\(RR\)為雙目相機標定的右側相機內參矩陣,RR表示為Right Rotation之意
c. 左側相機坐標系到右側相機坐標系變換
\(\begin{bmatrix}
x_{R}\\
y_{R}\\
z_{R}\\
1
\end{bmatrix}=M*\begin{bmatrix}
x_{L}\\
y_{L}\\
z_{L}\\
1
\end{bmatrix}\) (3)
其中M為4x4的變換矩陣，可理解為兩個相機光心的外參矩陣，平移+旋轉矩陣。此矩陣為上面標定得到的外參數矩陣。
d. 左側圖像坐標轉換到右側圖像坐標
將（1）和（2）代入（3）得到
\(z_{R}*RR^{-1}*\begin{bmatrix}
u_{R}\\
v_{R}\\
1\\
1/z_{R}
\end{bmatrix}=
z_{L}*M*LR^{-1}*\begin{bmatrix}
u_{L}\\
v_{L}\\
1\\
1/z_{L}
\end{bmatrix}\)
兩側同時左乘RR矩陣得到
\(\begin{bmatrix}
u_{R}\\
v_{R}\\
1\\
1/z_{R}
\end{bmatrix}=
\frac{z_{L}}{z_{R}}*\underset{W}{\underbrace{RR*M*LR^{-1}}}*\begin{bmatrix}
u_{L}\\
v_{L}\\
1\\
1/z_{L}
\end{bmatrix}\)
由於\(z_{L}\approx z_{R}\)，因此上述公式可以簡化為：
\(\begin{bmatrix}
u_{R}\\
v_{R}\\
1\\
1/z_{R}
\end{bmatrix}=
W * \begin{bmatrix}
u_{L}\\
v_{L}\\
1\\
1/z_{L}
\end{bmatrix}\qquad \qquad \qquad \qquad （4）\)
W為計算得到的4x4變換矩陣，我們表示為
\(W = \begin{bmatrix}
r_{11} & r_{12}& r_{13}& r_{14}\\
r_{21} & r_{22}& r_{23}& r_{24}\\
r_{31} & r_{32}& r_{33}& r_{34}\\
r_{41} & r_{42}& r_{43}& r_{44}
\end{bmatrix}\)
由此將公式（4）展開，可得到左側圖像坐標轉換到右側圖像坐標解析解，即
\(u_{R} = r_{11}*u_{L}+ r_{12}*v_{L}+ r_{13}+ r_{14}*\frac{1}{z_{L}}\qquad \qquad \qquad \qquad （5）\\
v_{R} = r_{21}*u_{L}+ r_{22}*v_{L}+ r_{23}+ r_{24}*\frac{1}{z_{L}}\qquad \qquad \qquad \qquad （6）\)
至此推導完畢，由此可見每個像素點的配准和原始圖像的位置以及深度有着直接緊密的聯系。ROS有一個包用於配准的計算，卻異常的慢，按照以上的公式沒有理由這么慢，看來還是知道原理實現才行。
注意：左側右側相機只是邏輯上的意義，即左右相機擺放的位置是可以任意的，這里描述的左側相機可以實際擺在右側（左側），右側相機可以擺在左側（右側）。因為外餐矩陣M可以表達出他們實際的關系。因此我們只需要知道右側相機是彩色相機，左側是深度相機即可。
2. 實現代碼
配准代碼實現如下
void Depth2RGB(cv::Mat &src, cv::Mat &dst,const float *W) { double z; uint16_t u, v,d; uint16_t u_rgb, v_rgb; cv::Mat newdepth(dst.rows, dst.cols, CV_16UC1, cv::Scalar(0)); for (v = 0; v < src.rows; v++) { for (u = 0; u < src.cols; u++) { d = src.at<uint16_t>(v, u); z = (double)d; u_rgb = (uint16_t)((W[0] * (double)u + W[1] * (double)v + W[2] + W[3] / z));//參照上述公式（5） v_rgb = (uint16_t)((W[4] * (double)u + W[5] * (double)v + W[6] + W[7] / z));//參照上述公式（6） if (u_rgb < 0 && u_rgb >= newdepth.cols && v_rgb < 0 && v_rgb >= newdepth.rows) { uint16_t *val = (uint16_t *)newdepth.ptr<uchar>(v_rgb)+u_rgb; *val = d; } } } dst = newdepth; }
3. 閱讀資料