深度图像配准（Registration）原理

机器视觉中，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})^{ op})，(u_{R}, v_{R}, z_{R})分别表示彩色图像的横坐标，纵坐标和相机坐标系下的深度值（z方向上的值，非两点的距离）；同样地，深度图像的像素为((u_{L}, v_{L}, z_{L})^{ op})，(u_{L}, v_{L}, z_{L})分别表示深度图像的横坐标，纵坐标和相机坐标系下的深度值（z方向上的值，非两点的距离）。注意为了方便表示，本文中下标的R,L分别表示Right,Left的意思。那么深度图配准到彩色图的过程就是找到如下公式中的变换矩阵(W^{'})：

(egin{bmatrix}
u_{R}\
v_{R}\
1
end{bmatrix}
=W^{'}
egin{bmatrix}
u_{L}\
v_{L}\
1
end{bmatrix})

实际上，变换矩阵(W^{'})并不是真正要求解的矩阵，而是需要重新构造新的变换矩阵，以满足矩阵可逆性。构造过程如下：

a. 构造左侧相机的左侧相机坐标系到图像坐标系的变换

由相机原理（可参考相机标定(2)---摄像机标定原理），可知相机坐标到图像坐标的变换为：

(z_{L}
egin{bmatrix}
 u_{L}\ 
 v_{L}\ 
1
end{bmatrix}=egin{bmatrix}
f/dx & 0 & u_{L}^{0}&0\ 
0 & f/dy & v_{L}^{0}&0\
0 & 0& 1&0
end{bmatrix}
egin{bmatrix}
x_{L}\ 
y_{L}\ 
z_{L}\ 
1
end{bmatrix}
)

 以上变换过程等价于如下的表达

(z_{L}
egin{bmatrix}
u_{L}\
v_{L}\
1\
1/z_{L}
end{bmatrix}=
underset{LR}{underbrace{
egin{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}
}}
egin{bmatrix}
x_{L}\
y_{L}\
z_{L}\
1
end{bmatrix})

 于是图像坐标系到相机坐标系的变换为：

(egin{bmatrix}
x_{L}\
y_{L}\
z_{L}\
1
end{bmatrix}=z_{L}*LR^{-1}*egin{bmatrix}
u_{L}\
v_{L}\
1\
1/z_{L}
end{bmatrix})                             (1)

 其中(LR)为双目相机标定的左侧相机内参矩阵,LR表示为Left Rotation之意

b. 构造右侧相机的右侧相机坐标系到图像坐标系的变换

根据a，同理地，左侧相机坐标系到图像坐标系的变换为：

 (z_{R}
egin{bmatrix}
u_{R}\ 
v_{R}\ 
1\ 
1/z_{R}
end{bmatrix}=
underset{RR}{underbrace{
egin{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}
}}
egin{bmatrix}
x_{R}\ 
y_{R}\ 
z_{R}\ 
1
end{bmatrix})

于是图像坐标系到相机坐标系的变换为：

(egin{bmatrix}
x_{R}\ 
y_{R}\ 
z_{R}\ 
1
end{bmatrix}=z_{R}*RR^{-1}*egin{bmatrix}
u_{R}\ 
v_{R}\ 
1\ 
1/z_{R}
end{bmatrix})                                (2)

 其中(RR)为双目相机标定的右侧相机内参矩阵,RR表示为Right Rotation之意

c. 左侧相机坐标系到右侧相机坐标系变换

(egin{bmatrix}
x_{R}\
y_{R}\
z_{R}\
1
end{bmatrix}=M*egin{bmatrix}
x_{L}\
y_{L}\
z_{L}\
1
end{bmatrix})                                     (3)

 其中M为4x4的变换矩阵，可理解为两个相机光心的外参矩阵，平移+旋转矩阵。此矩阵为上面标定得到的外参数矩阵。 

d. 左侧图像坐标转换到右侧图像坐标

将（1）和（2）代入（3）得到

(z_{R}*RR^{-1}*egin{bmatrix}
u_{R}\
v_{R}\
1\
1/z_{R}
end{bmatrix}=
z_{L}*M*LR^{-1}*egin{bmatrix}
u_{L}\
v_{L}\
1\
1/z_{L}
end{bmatrix})

两侧同时左乘RR矩阵得到

(egin{bmatrix}
u_{R}\
v_{R}\
1\
1/z_{R}
end{bmatrix}=
frac{z_{L}}{z_{R}}*underset{W}{underbrace{RR*M*LR^{-1}}}*egin{bmatrix}
u_{L}\
v_{L}\
1\
1/z_{L}
end{bmatrix})

由于(z_{L}approx z_{R})，因此上述公式可以简化为：

(egin{bmatrix}
u_{R}\
v_{R}\
1\
1/z_{R}
end{bmatrix}=
W * egin{bmatrix}
u_{L}\
v_{L}\
1\
1/z_{L}
end{bmatrix}qquad qquad qquad qquad （4）)

W为计算得到的4x4变换矩阵，我们表示为

(W = egin{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. 阅读资料

[1]. Kinect深度图与摄像头RGB的标定与配