kinect 2.0 SDK学习笔记（七）--matlab自带相机标定程序对kinect进行简单标定

相机标定是完成很多任务之前很有必要的一项工作，标定要做的就是得到相机内外参数。kinect有两个相机，一个深度相机，一个彩色相机，两个相机的内外参我们都要得到，具体什么是相机标定，大家可以百度。kinect 2.0 SDK中虽然可以查找到一部分标定信息，但是感觉不够准确，所以我们自己来试一试！

SDK中的数据

通过代码我们可以获得深度相机内参，下面写出主要的几行代码：

CameraIntrinsics* m_pCameraIntrinsics = new CameraIntrinsics();

hr = m_pKinectSensor->get_CoordinateMapper(&m_pCoordinateMapper);

hr = m_pKinectSensor->get_CoordinateMapper(&m_pCoordinateMapper);

// 获取深度相机内参并打印
m_pCoordinateMapper->GetDepthCameraIntrinsics(m_pCameraIntrinsics);
cout << "FocalLengthX : " << m_pCameraIntrinsics->FocalLengthX << endl;
cout << "FocalLengthY : " << m_pCameraIntrinsics->FocalLengthY << endl;
cout << "PrincipalPointX : " << m_pCameraIntrinsics->PrincipalPointX << endl;
cout << "PrincipalPointY : " << m_pCameraIntrinsics->PrincipalPointY << endl;
cout << "RadialDistortionFourthOrder : " << m_pCameraIntrinsics->RadialDistortionFourthOrder << endl;
cout << "RadialDistortionSecondOrder : " << m_pCameraIntrinsics->RadialDistortionSecondOrder << endl;
cout << "RadialDistortionSixthOrder : " << m_pCameraIntrinsics->RadialDistortionSixthOrder << endl;

我们就可以得到kinect自带的出厂测试的数据，如下：

FocalLengthX : 367.749
FocalLengthY : 367.749
PrincipalPointX : 259.896
PrincipalPointY : 206.745
RadialDistortionFourthOrder : -0.272151
RadialDistortionSecondOrder : 0.0900451
RadialDistortionSixthOrder : 0.0964618

没有找到获取彩色相机内参的函数（不知道为什么），因此如果要用到彩色相机内参或者彩色相机坐标系和深度相机坐标系之间的刚体变换矩阵（R，T），即两个相机之间的相对位置关系，我们就要自己进行标定了。下面我们就开始标定工作！

---

标定步骤

一、获取原始图片

基本上matlab自带的标定程序还是使用的张正友标定法。因此，我们需要拿着棋盘格标定板，不断改变标定板与相机的相对位置，同时用相机拍摄下来，要让棋盘格上的每个方格都清晰可见。
这里我们写一个采集图片的小程序，因为深度图（Depth）对于棋盘格上的方格不可见，所以我们用红外图（Infrared）来代替，因为两种数据源都是深度相机获取的，因此可以这么做。

/*
获取kinect原始图片序列并依时间保存，以100张为单位，获取的图片可用于kinect标定
*/

#include "kinect.h"
#include <iostream>
#include <opencv2/core/core.hpp>  
#include <opencv2/highgui/highgui.hpp>  
#include <time.h>
#include <vector>

using namespace cv;
using namespace std;

// 安全释放指针
template<class Interface>
inline void SafeRelease(Interface *& pInterfaceToRelease)
{
    if (pInterfaceToRelease != NULL)
    {
        pInterfaceToRelease->Release();
        pInterfaceToRelease = NULL;
    }
}

// 保存所需数据的结构体
struct eachFrame
{
    string depth_name;
    string rgb_name;
    cv::Mat tmp_itD1;
    cv::Mat tmp_itRGB1;
};

int main()
{
    // 创建保存目录
    CreateDirectory(L".//images", NULL);

    // 获取Kinect设备
    IKinectSensor* m_pKinectSensor;
    ICoordinateMapper*      m_pCoordinateMapper;
    CameraIntrinsics* m_pCameraIntrinsics = new CameraIntrinsics();
    HRESULT hr;
    hr = GetDefaultKinectSensor(&m_pKinectSensor);

    if (FAILED(hr))
    {
        return hr;
    }

    IMultiSourceFrameReader* m_pMultiFrameReader;
    IBodyFrameSource* m_pBodyFrameSource;
    IBodyFrameReader* m_pBodyFrameReader;
    if (m_pKinectSensor)
    {
        hr = m_pKinectSensor->Open();
        Sleep(1000);
        if (SUCCEEDED(hr))
        {
            m_pKinectSensor->get_BodyFrameSource(&m_pBodyFrameSource);
            // 获取多数据源到读取器  
            hr = m_pKinectSensor->OpenMultiSourceFrameReader(
                FrameSourceTypes::FrameSourceTypes_Color |
                FrameSourceTypes::FrameSourceTypes_Infrared |
                FrameSourceTypes::FrameSourceTypes_Depth,
                &m_pMultiFrameReader);
        }
    }

    if (SUCCEEDED(hr))
    {
        hr = m_pKinectSensor->get_CoordinateMapper(&m_pCoordinateMapper);
    }

    if (!m_pKinectSensor || FAILED(hr))
    {
        return E_FAIL;
    }

    // 获取深度相机内参并打印
    if (SUCCEEDED(hr))
    {
        hr = m_pCoordinateMapper->GetDepthCameraIntrinsics(m_pCameraIntrinsics);
    }
    if (SUCCEEDED(hr))
    {
        cout << "FocalLengthX : " << m_pCameraIntrinsics->FocalLengthX << endl;
        cout << "FocalLengthY : " << m_pCameraIntrinsics->FocalLengthY << endl;
        cout << "PrincipalPointX : " << m_pCameraIntrinsics->PrincipalPointX << endl;
        cout << "PrincipalPointY : " << m_pCameraIntrinsics->PrincipalPointY << endl;
        cout << "RadialDistortionFourthOrder : " << m_pCameraIntrinsics->RadialDistortionFourthOrder << endl;
        cout << "RadialDistortionSecondOrder : " << m_pCameraIntrinsics->RadialDistortionSecondOrder << endl;
        cout << "RadialDistortionSixthOrder : " << m_pCameraIntrinsics->RadialDistortionSixthOrder << endl;
    }


    // 三个数据帧及引用
    IDepthFrameReference* m_pDepthFrameReference;
    IColorFrameReference* m_pColorFrameReference;
    IInfraredFrameReference* m_pInfraredFrameReference;
    IInfraredFrame* m_pInfraredFrame;
    IDepthFrame* m_pDepthFrame;
    IColorFrame* m_pColorFrame;
    // 四个个图片格式
    Mat i_rgb(1080, 1920, CV_8UC4);      //注意：这里必须为4通道的图，Kinect的数据只能以Bgra格式传出
    Mat i_depth(424, 512, CV_8UC1);
    Mat i_depth_raw(424, 512, CV_16UC1);
    Mat i_ir(424, 512, CV_16UC1);

    UINT16 *depthData = new UINT16[424 * 512];
    UINT16 *irData = new UINT16[424 * 512];
    IMultiSourceFrame* m_pMultiFrame = nullptr;

    DepthSpacePoint*        m_pDepthCoordinates;
    ColorSpacePoint*        m_pColorCoordinates;
    CameraSpacePoint*        m_pCameraCoordinates;
    m_pDepthCoordinates = new DepthSpacePoint[1920 * 1080];
    m_pColorCoordinates = new ColorSpacePoint[512 * 424];
    m_pCameraCoordinates = new CameraSpacePoint[512 * 424];

    clock_t start_time;
    vector<eachFrame> framvec;
    SYSTEMTIME sys;
    size_t framecount = 0;

    while (true)
    {
        if (framecount == 0)
        {
            start_time = clock();
        }

        eachFrame thisframe;
        char depth_name[200] = { '\0' };
        char rgb_name[200] = { '\0' };

        // 获取新的一个多源数据帧
        hr = m_pMultiFrameReader->AcquireLatestFrame(&m_pMultiFrame);

         按照时间来保存
        //GetLocalTime(&sys);
        //sprintf(depth_name, "%s%4d%02d%02d%02d_%02d_%02d_%03d%s", "images//depth_", sys.wYear, sys.wMonth, sys.wDay, sys.wHour, sys.wMinute, sys.wSecond, sys.wMilliseconds, ".png");//保存图片名
        //sprintf(rgb_name, "%s%4d%02d%02d%02d_%02d_%02d_%03d%s", "images//rgb_", sys.wYear, sys.wMonth, sys.wDay, sys.wHour, sys.wMinute, sys.wSecond, sys.wMilliseconds, ".png");//保存图片名

        // 按照序号来保存
        GetLocalTime(&sys);
        sprintf(depth_name, "%s%d%s", "images//depth_", framecount, ".tif");//保存图片名
        sprintf(rgb_name, "%s%d%s", "images//rgb_", framecount, ".jpg");//保存图片名

        if (FAILED(hr) || !m_pMultiFrame)
        {
            //cout << "!!!" << endl;
            continue;
        }

        // 从多源数据帧中分离出彩色数据，深度数据和红外数据
        if (SUCCEEDED(hr))
            hr = m_pMultiFrame->get_ColorFrameReference(&m_pColorFrameReference);
        if (SUCCEEDED(hr))
            hr = m_pColorFrameReference->AcquireFrame(&m_pColorFrame);
        if (SUCCEEDED(hr))
            hr = m_pMultiFrame->get_DepthFrameReference(&m_pDepthFrameReference);
        if (SUCCEEDED(hr))
            hr = m_pDepthFrameReference->AcquireFrame(&m_pDepthFrame);
        if (SUCCEEDED(hr))
            hr = m_pMultiFrame->get_InfraredFrameReference(&m_pInfraredFrameReference);
        if (SUCCEEDED(hr))
            hr = m_pInfraredFrameReference->AcquireFrame(&m_pInfraredFrame);

        // color拷贝到图片中
        UINT nColorBufferSize = 1920 * 1080 * 4;
        if (SUCCEEDED(hr))
            hr = m_pColorFrame->CopyConvertedFrameDataToArray(nColorBufferSize, i_rgb.data, ColorImageFormat::ColorImageFormat_Bgra);

        // depth拷贝到图片中
        if (SUCCEEDED(hr))
        {
            hr = m_pDepthFrame->CopyFrameDataToArray(424 * 512, depthData);
            hr = m_pDepthFrame->CopyFrameDataToArray(424 * 512, reinterpret_cast<UINT16*>(i_depth_raw.data));
            for (int i = 0; i < 512 * 424; i++)
            {
                // 0-255深度图，为了显示明显，只取深度数据的低8位
                BYTE intensity = static_cast<BYTE>(depthData[i] % 256);
                reinterpret_cast<BYTE*>(i_depth.data)[i] = intensity;
            }

            // 实际是16位unsigned int数据
            //hr = m_pDepthFrame->CopyFrameDataToArray(424 * 512, reinterpret_cast<UINT16*>(i_depth.data));
        }

        // ir拷贝到图片中
        if (SUCCEEDED(hr))
        {
            hr = m_pInfraredFrame->CopyFrameDataToArray(424 * 512, irData);
            hr = m_pInfraredFrame->CopyFrameDataToArray(424 * 512, reinterpret_cast<UINT16*>(i_ir.data));
        }

        // 深度图映射到彩色图上
        if (SUCCEEDED(hr))
        {
            HRESULT hr = m_pCoordinateMapper->MapDepthFrameToColorSpace(512 * 424, depthData, 512 * 424, m_pColorCoordinates);      // 注意这里的彩色点数要写成424*512，至于为什么。。。可能是为了代表下一个参数colorSpacePoints的大小
        }
        Mat i_depthToRgb(424, 512, CV_8UC4);
        if (SUCCEEDED(hr))
        {
            for (int i = 0; i < 424 * 512; i++)
            {
                ColorSpacePoint p = m_pColorCoordinates[i];
                if (p.X != -std::numeric_limits<float>::infinity() && p.Y != -std::numeric_limits<float>::infinity())
                {
                    int colorX = static_cast<int>(p.X + 0.5f);
                    int colorY = static_cast<int>(p.Y + 0.5f);

                    if ((colorX >= 0 && colorX < 1920) && (colorY >= 0 && colorY < 1080))
                    {
                        i_depthToRgb.data[i * 4] = i_rgb.data[(colorY * 1920 + colorX) * 4];
                        i_depthToRgb.data[i * 4 + 1] = i_rgb.data[(colorY * 1920 + colorX) * 4 + 1];
                        i_depthToRgb.data[i * 4 + 2] = i_rgb.data[(colorY * 1920 + colorX) * 4 + 2];
                        i_depthToRgb.data[i * 4 + 3] = i_rgb.data[(colorY * 1920 + colorX) * 4 + 3];
                    }
                }
            }
        }

        thisframe.depth_name = depth_name;
        thisframe.rgb_name = rgb_name;
        //thisframe.tmp_itD1 = i_depth_raw.clone();
        thisframe.tmp_itD1 = i_ir.clone();
        //thisframe.tmp_itRGB1 = i_depthToRgb.clone();
        thisframe.tmp_itRGB1 = i_rgb.clone();
        framvec.push_back(thisframe);

        // 释放资源
        SafeRelease(m_pColorFrame);
        SafeRelease(m_pDepthFrame);
        SafeRelease(m_pInfraredFrame);
        SafeRelease(m_pColorFrameReference);
        SafeRelease(m_pDepthFrameReference);
        SafeRelease(m_pInfraredFrameReference);
        SafeRelease(m_pMultiFrame);
        framecount++;
        if (100 == framecount)
        {
            clock_t end_time = clock();
            float time = (end_time - start_time) / CLOCKS_PER_SEC;

            //save
            for (int i = 0; i < framvec.size(); i++)
            {
                imwrite(framvec[i].depth_name, framvec[i].tmp_itD1);
                imwrite(framvec[i].rgb_name, framvec[i].tmp_itRGB1);
            }
            framvec.clear();
            std::cout << "fps: " << framecount / time << std::endl;
            framecount = 0;
        }
    }
    // 关闭窗口，设备
    cv::destroyAllWindows();
    SafeRelease(m_pCoordinateMapper);
    m_pKinectSensor->Close();
    std::system("pause");
    return 0;
}

运行程序，可以在framecount = 0;代码处设置断点，这样我们就可以获取100组以序号结尾的原始数据了，如下图所示：

可以看到在相同位置下，红外图和彩色图的序号是一样的。

二、Matlab标定

打开Matlab（我用的2014a），在应用程序中找到Camera Calibrator

以红外相机标定为例，选择Add Images,然后选择图片，我们将100幅红外图全部载入，还要选择棋盘格上方格边长，我这里是19mm。可以看到程序自动帮我们找到了所有角点。找不到的程序会提醒你。

这里有一些选项可以选择，根据实际情况选择，鼠标放在上面有解释

点击Calibrate，标定时间可能有点长，慢慢等待。

点击导出标定结果，我们就可以在命令行窗口看到标定的内外参数了。这里的外参是针对每一幅图片的，根据棋盘格建立世界坐标系得到的外参，因此一幅图片就对应一组外参。如果我们想得到kinect上深度相机和彩色相机之间的位置关系，简单的方法就是利用相同序号的两个图片得到两个相机的外参，再根据变换关系得到它们之间的R，T，变换关系可参考。

R_ir2rgb=R_rgb*R_ir^(-1);
T_ir2rgb=T_rgb-R_ir2rgb*T_ir;