kinect 2.0 SDK学习笔记（四）--深度图与彩色图对齐

深度图与彩色图对齐

如果用kinect SDK的话只需要一个函数就好了：

MapDepthFrameToColorSpace(512 * 424, depthData, 512 * 424, m_pColorCoordinates);

depthData表示深度图的数据，m_pColorCoordinates表示一个数组，每个元素代表深度图中每个元素对应的1920*1080彩色图的像素坐标。
我们深度图与彩色图的对齐也就是为了达到这样一个目的：找到深度图上每个像素点对应的RGB值。

理论实现

相机标定

如果我们想自己实现这个目的，那么应该如何去做呢？
最终要的就是相机标定！
我用的是张正友标定法，工具就是常用的 Camera Calibration Toolbox for Matlab
分别对彩色摄像头和深度摄像头进行标定，求出内参和外参。

对齐推导

假设我们在上一步的标定中得到了如下的参数：
彩色相机内参：
1. $K_{rgb} = \left[{ \begin{matrix} f_{x\_rgb} & 0 & c_{x\_rgb}\\ 0 & f_{y\_rgb} & c_{y\_rgb}\\ 0 & 0 & 1 \end{matrix} }\right]$
因此对于各自相机坐标系下的齐次的三维点（$P=[ X\ \ Y\ \ Z\ \ 1]^T$）到各自图片上齐次表示的像素坐标（$p=[u\ \ v\ \ 1]^T$）的映射关系如下：
对彩色相机而言，有：

$$Z_{rgb}*p_{rgb}=K_{rgb}*[I|0]{P_{rgb}}$$
展开如下，

其中， 齐次坐标 $P_{rgb}= [ X_{rgb} \ Y_{rgb} \ Z_{rgb} \ 1]^T$我们可以用 非齐次坐标 $\overline{P_{rgb}} = [ X_{rgb} \ Y_{rgb} \ Z_{rgb} ]^T$来表示，就有如下的形式：
$$Z_{rgb}*p_{rgb}=K_{rgb}*\overline{P_{rgb}}$$
同理，可得到深度相机的映射公式：
$$Z_{ir}*p_{ir}=K_{ir}*\overline{P_{ir}}$$
2. 针对于同一幅棋盘格的外参：
彩色相机： $R_{rgb}$和 $T_{rgb}$
深度相机： $R_{ir}$和 $T_{ir}$
因此，两个相机有如下刚体变换关系：
$R_{ir2rgb}=R_{rgb}*R_{ir}^{-1}$
$T_{ir2rgb}=T_{rgb}-R_{ir2rgb}*T_{ir}$
对于 非齐次坐标表示的各自 相机坐标系下的三维点 $\overline{P_{rgb}}$和 $\overline{P_{ir}}$来说，有如下关系：
$$\overline{P_{rgb}}=R_{ir2rgb}*\overline{P_{ir}}+T_{ir2rgb}$$
3. 最后我们可以得到如下公式：
$$Z_{rgb}*p_{rgb}=K_{rgb}*R_{ir2rgb}*K_{ir}^{-1}*Z_{ir}*p_{ir}+{K_{rgb}*T_{ir2rgb}}$$
这样就把 $p_{rgb}$和 $p_{ir}$联系起来了。
为了简化表示，我们令：
$$R = K_{rgb}*R_{ir2rgb}*K_{ir}^{-1}$$
$$T = K_{rgb}*T_{ir2rgb}$$
则有，
$$Z_{rgb}*p_{rgb}=R*Z_{ir}*p_{ir}+T$$

代码实现

为了方便，我事先将kinect v2采集的原始数据以图片的形式保存了下来，这里要注意一点，深度图因为是16位的，所以保存格式必须是PNG，彩色图以jpg的形式保存的下来，但是在读取图片时发现读取之后图片格式变为了CV_8UC3，当然这并不影响我们从中读取RGB数据。
代码如下：

/*
将深度图映射到彩色图上，生成和深度图匹配的对齐彩色图
*/
#include <opencv2\core\core.hpp>
#include <opencv2\highgui\highgui.hpp>
#include <fstream>
#include <iostream>
#include <sstream>
#include <Eigen/Core>
#include <Eigen/LU>
#include <thread>

using namespace cv;
using namespace std;

struct KinectParm
{
    float fx_rgb;
    float fy_rgb;
    float cx_rgb;
    float cy_rgb;

    float fx_ir;
    float fy_ir;
    float cx_ir;
    float cy_ir;

    Eigen::Matrix3f R_ir2rgb;
    Eigen::Vector3f T_ir2rgb;
};

bool loadParm(KinectParm* kinectParm)
{
    // 加载参数
    ifstream parm("registration.txt");
    string stringLine;
    if (parm.is_open())
    {
        // rgb相机参数：fx,fy,cx,cy
        getline(parm, stringLine);
        stringstream lin(stringLine);
        string s1, s2, s3, s4, s5, s6, s7, s8, s9;
        lin >> s1 >> s2 >> s3 >> s4;
        kinectParm->fx_rgb = atof(s1.c_str());
        kinectParm->fy_rgb = atof(s2.c_str());
        kinectParm->cx_rgb = atof(s3.c_str());
        kinectParm->cy_rgb = atof(s4.c_str());
        stringLine.clear();
        // ir相机参数：fx,fy,cx,cy
        getline(parm, stringLine);
        stringstream lin2(stringLine);
        lin2 << stringLine;
        lin2 >> s1 >> s2 >> s3 >> s4;
        kinectParm->fx_ir = atof(s1.c_str());
        kinectParm->fy_ir = atof(s2.c_str());
        kinectParm->cx_ir = atof(s3.c_str());
        kinectParm->cy_ir = atof(s4.c_str());
        stringLine.clear();

        // R_ir2rgb
        getline(parm, stringLine);
        stringstream lin3(stringLine);
        lin3 << stringLine;
        lin3 >> s1 >> s2 >> s3 >> s4 >> s5 >> s6 >> s7 >> s8 >> s9;
        kinectParm->R_ir2rgb <<
            atof(s1.c_str()), atof(s2.c_str()), atof(s3.c_str()),
            atof(s4.c_str()), atof(s5.c_str()), atof(s6.c_str()),
            atof(s7.c_str()), atof(s8.c_str()), atof(s9.c_str());
        stringLine.clear();

        // T_ir2rgb
        getline(parm, stringLine);
        stringstream lin4(stringLine);
        lin4 << stringLine;
        lin4 >> s1 >> s2 >> s3;
        kinectParm->T_ir2rgb << atof(s1.c_str()), atof(s2.c_str()), atof(s3.c_str());
    }
    else
    {
        cout << "parm.txt not right!!!";
        return false;
    }
    cout << "******************************************" << endl;

    cout << "fx_rgb:    " << kinectParm->fx_rgb << endl;
    cout << "fy_rgb:    " << kinectParm->fy_rgb << endl;
    cout << "cx_rgb:    " << kinectParm->cx_rgb << endl;
    cout << "cy_rgb:    " << kinectParm->cy_rgb << endl;
    cout << "******************************************" << endl;
    cout << "fx_ir:     " << kinectParm->fx_ir << endl;
    cout << "fy_ir:     " << kinectParm->fy_ir << endl;
    cout << "cx_ir:     " << kinectParm->cx_ir << endl;
    cout << "cy_ir:     " << kinectParm->cy_ir << endl;
    cout << "******************************************" << endl;
    cout << "R_ir2rgb:" << endl << kinectParm->R_ir2rgb << endl;
    cout << "******************************************" << endl;
    cout << "T_ir2rgb:" << endl << kinectParm->T_ir2rgb << endl;
    cout << "******************************************" << endl;
    return true;
}
int main()
{
    // 1. 读取参数
    KinectParm *parm = new KinectParm();
    if(!loadParm(parm))
        return 0;
    // 2. 载入rgb图片和深度图并显示
    Mat bgr(1080, 1920, CV_8UC4);
    bgr = imread("color.jpg");
    Mat depth(424, 512, CV_16UC1);
    depth = imread("depth.png", IMREAD_ANYDEPTH);   // 图片读入后的格式不一定和定义时候的一样，比如这里读入后的格式就是8UC3
    Mat depth2rgb = imread("depth2rgb.jpg");
    // 3. 显示
    thread th = std::thread([&]{
        while (true)
        {
            imshow("原始彩色图", bgr);
            waitKey(1);
            imshow("原始深度图", depth);
            waitKey(1);
            imshow("原始投影图", depth2rgb);
            waitKey(1);
        }
    });
    // 4. 变换

    // 4.1 计算各个矩阵
#pragma region  非齐次
    Eigen::Matrix3f K_ir;           // ir内参矩阵
    K_ir <<
        parm->fx_ir, 0, parm->cx_ir,
        0, parm->fy_ir, parm->cy_ir,
        0, 0, 1;
    Eigen::Matrix3f K_rgb;          // rgb内参矩阵
    K_rgb <<
        parm->fx_rgb, 0, parm->cx_rgb, 
        0, parm->fy_rgb, parm->cy_rgb, 
        0, 0, 1;

    Eigen::Matrix3f R;          
    Eigen::Vector3f T;
    R = K_rgb*parm->R_ir2rgb*K_ir.inverse();
    T = K_rgb*parm->T_ir2rgb;

    cout << "K_rgb:\n" << K_rgb << endl;
    cout << "K_ir:\n" << K_ir << endl;
    cout << "R:\n" << R << endl;
    cout << "T:\n" << T << endl;

    cout << depth.type() << endl;


    // 4.2 计算投影
    Mat result(424, 512, CV_8UC3);
    int i = 0;
    for (int row = 0; row < 424; row++)
    {
        for (int col = 0; col < 512; col++)
        {
            unsigned short* p = (unsigned short*)depth.data;
            unsigned short depthValue = p[row * 512 + col];
            //cout << "depthValue       " << depthValue << endl;
            if (depthValue != -std::numeric_limits<unsigned short>::infinity() && depthValue != -std::numeric_limits<unsigned short>::infinity() && depthValue != 0 && depthValue != 65535)
            {
                // 投影到彩色图上的坐标
                Eigen::Vector3f uv_depth(col, row,1.0f);                            // !!!p_ir
                Eigen::Vector3f uv_color = depthValue/1000.f*R*uv_depth + T/1000;   // !!!Z_rgb*p_rgb=R*Z_ir*p_ir+T; (除以1000，是为了从毫米变米)

                int X = static_cast<int>(uv_color[0] / uv_color[2]);                // !!!Z_rgb*p_rgb -> p_rgb
                int Y = static_cast<int>(uv_color[1] / uv_color[2]);                // !!!Z_rgb*p_rgb -> p_rgb
                //cout << "X:       " << X << "     Y:      " << Y << endl;
                if ((X >= 0 && X < 1920) && (Y >= 0 && Y < 1080))
                {
                    //cout << "X:       " << X << "     Y:      " << Y << endl;
                    result.data[i * 3] = bgr.data[3 * (Y * 1920 + X)];
                    result.data[i * 3 + 1] = bgr.data[3 * (Y * 1920 + X) + 1];
                    result.data[i * 3 + 2] = bgr.data[3 * (Y * 1920 + X) + 2];
                }
                else
                {
                    result.data[i * 3] = 0;
                    result.data[i * 3 + 1] = 0;
                    result.data[i * 3 + 2] = 0;
                }
            }
            else
            {
                result.data[i * 3] = 0;
                result.data[i * 3 + 1] = 0;
                result.data[i * 3 + 2] = 0;
            }
            i++;
        }
    }
    imwrite("registrationResult.png", result);
    thread th2 = std::thread([&]{
        while (true)
        {
            imshow("结果图", result);
            waitKey(1);
        }
    });

    th.join();
    th2.join();
#pragma endregion


    system("pause");
    return 0;
}

实验对比

左边是SDK的结果，右边是我得到的结果，当然标定的如果更加精确的话，结果也就更加符合实际了。
看到图像中有重复纹理，分析出现的原因，是因为深度摄像头和彩色摄像头不重合，导致深度图上的一部分在实际的场景中不能被彩色摄像头捕获，导致附着这当物体的纹理。

参数文件

用Matlab Calibration Toolbox 获取kinect的红外图像和彩色图像进行标定得到的参数，方法比较简单，用的图也比较少，可能不太精确，看看格式就好。
我的配准文件

注意文件名

PS：直接把文件内容粘在这里好了，也没想着赚积分什么了，也不知道怎么改已经上传了的资源。。。
1096.03541 1103.58516 965.49803 526.73740
388.61422 390.31838 245.90673 190.29050
0.9993 -0.0032 -0.0371 0.0026 0.9998 -0.0180 0.0372 0.0179 0.9991
41.1327 -15.8984 -15.1594