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. Krgb=fx_rgb000fy_rgb0cx_rgbcy_rgb1 K r g b = [ f x _ r g b 0 c x _ r g b 0 f y _ r g b c y _ r g b 0 0 1 ]
因此对于各自相机坐标系下的齐次的三维点( P=[X  Y  Z  1]T P = [ X     Y     Z     1 ] T )到各自图片上齐次表示的像素坐标( p=[u  v  1]T p = [ u     v     1 ] T )的映射关系如下:
对彩色相机而言,有:

Zrgbprgb=Krgb[I|0]Prgb Z r g b ∗ p r g b = K r g b ∗ [ I | 0 ] P r g b

展开如下,

其中, 齐次坐标 Prgb=[Xrgb Yrgb Zrgb 1]T P r g b = [ X r g b   Y r g b   Z r g b   1 ] T 我们可以用 非齐次坐标 Prgb¯¯¯¯¯¯¯¯=[Xrgb Yrgb Zrgb]T P r g b ¯ = [ X r g b   Y r g b   Z r g b ] T 来表示,就有如下的形式:
Zrgbprgb=KrgbPrgb¯¯¯¯¯¯¯¯ Z r g b ∗ p r g b = K r g b ∗ P r g b ¯

同理,可得到深度相机的映射公式:
Zirpir=KirPir¯¯¯¯¯¯ Z i r ∗ p i r = K i r ∗ P i r ¯

2. 针对于同一幅棋盘格的外参:
彩色相机: Rrgb R r g b Trgb T r g b
深度相机: Rir R i r Tir T i r
因此,两个相机有如下刚体变换关系:
Rir2rgb=RrgbR1ir R i r 2 r g b = R r g b ∗ R i r − 1
Tir2rgb=TrgbRir2rgbTir T i r 2 r g b = T r g b − R i r 2 r g b ∗ T i r
对于 非齐次坐标表示的各自 相机坐标系下的三维点 Prgb¯¯¯¯¯¯¯¯ P r g b ¯ Pir¯¯¯¯¯¯ P i r ¯ 来说,有如下关系:
Prgb¯¯¯¯¯¯¯¯=Rir2rgbPir¯¯¯¯¯¯+Tir2rgb P r g b ¯ = R i r 2 r g b ∗ P i r ¯ + T i r 2 r g b

3. 最后我们可以得到如下公式:
Zrgbprgb=KrgbRir2rgbK1irZirpir+KrgbTir2rgb Z r g b ∗ p r g b = K r g b ∗ R i r 2 r g b ∗ K i r − 1 ∗ Z i r ∗ p i r + K r g b ∗ T i r 2 r g b

这样就把 prgb p r g b pir p i r 联系起来了。
为了简化表示,我们令:
R=KrgbRir2rgbK1ir R = K r g b ∗ R i r 2 r g b ∗ K i r − 1

T=KrgbTir2rgb T = K r g b ∗ T i r 2 r g b

则有,
Zrgbprgb=RZirpir+T Z r g b ∗ p r g b = R ∗ Z i r ∗ p i r + T

代码实现

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

/*
将深度图映射到彩色图上,生成和深度图匹配的对齐彩色图
*/
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 

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

你可能感兴趣的:(kinect,Kinect2)