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

原创 2017年08月20日 17:23:41

相机标定是完成很多任务之前很有必要的一项工作,标定要做的就是得到相机内外参数。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;

版权声明:本文为博主原创文章,未经博主允许不得转载。

Kinect2.0之使用KinectSDK自带的KinectStudio进行数据采集

去年一直摆弄Kinect2.0,发现二代Kinect和一代存在差别较大,采集程序也大致不一样了。 在这里我主要是介绍两种采集方法,一种是基于KinectSDK驱动直接采集的,能够采集到深度图和彩色图。...
  • csuzhaoqinghui
  • csuzhaoqinghui
  • 2016年11月29日 16:59
  • 1912

kinect 2.0 SDK学习笔记(一)--获得原始数据

工程配置包含目录加上: $(KINECTSDK20_DIR)\inc; 库目录加上: $(KINECTSDK20_DIR)\lib\x64;(注意选择x64/x86) 其中KINECTS...
  • jiaojialulu
  • jiaojialulu
  • 2016年11月08日 20:48
  • 6150

Kinect开发笔记之二Kinect for Windows 2.0新特性

这是本博客的第一篇翻译文档,笔者已经苦逼的竭尽全力的在翻译了,但无奈英语水平也是很有限,有不对或者不妥当不准确的地方必然会有,还恳请大家留言或者邮件我以批评指正,我会虚心接受。谢谢大家。     ...
  • u012200908
  • u012200908
  • 2015年03月19日 11:02
  • 2219

Kinect2.0开发文档

  • 2016年08月27日 15:31
  • 958KB
  • 下载

kinect2.0sdk的安装与配置

在你准备做kinect开发前一定要先注意自己的配置再去购买kinect2.0 免得以后麻烦   kinect2.0 对windows要求在这 Supported Operating Sys...
  • qq_22033759
  • qq_22033759
  • 2015年07月01日 00:10
  • 9448

kinect2.0获取深度图、彩色图,并利用Opencv显示

  • 2016年12月15日 09:59
  • 9.6MB
  • 下载

VS2013+OpenCV3.0.0+Kinect v2.0 安装 (Win10-64位)

因为Kinect 2 需要 VS2012版本及以上的编译器,VS2015太大只,故选择VS2013. 环境为Win10. 此贴仅作为安装记录,备忘。 1.下载并安装VS2013    ...
  • shiny0910
  • shiny0910
  • 2017年01月02日 16:34
  • 789

Kinect for Windows SDK v2.0 开发笔记 (一)环境

嗯,它来了,Windows for Windows v2。 7
  • dustpg
  • dustpg
  • 2014年07月20日 05:56
  • 16941

ROS视觉和图像-Kinect V2使用

Kinect2安装使用
  • x_r_su
  • x_r_su
  • 2016年10月24日 16:24
  • 3208

kinect v2 标定

Kinect2 Calibration Maintainer Thiemo Wiedemeyer wiedemeyer@cs.uni-bremen.de>, Institute for Ar...
  • wuguangbin1230
  • wuguangbin1230
  • 2017年07月16日 18:27
  • 1097
内容举报
返回顶部
收藏助手
不良信息举报
您举报文章:kinect 2.0 SDK学习笔记(七)--matlab自带相机标定程序对kinect进行简单标定
举报原因:
原因补充:

(最多只允许输入30个字)