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;

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

相关文章推荐

使用OpenCV显示KinectV2数据

若有问题请去原文链接留言,csdn已经很少上了:http://guoming.me/use-opencv-display-kinectv2-data 这次给大家分享下我自己写的KinectV...

Kinect v2深度测量原理

【翻译】Kinect v2程序设计(C++) Depth编 Kinect SDK v2预览版,取得Depth数据的方法说明。 上一节,介绍了通过使用Kinect for Windows SDK...

kinect v2 标定

Kinect2 Calibration Maintainer Thiemo Wiedemeyer wiedemeyer@cs.uni-bremen.de>, Institute for Ar...

Baxter学习笔记5-Kinect摄像头标定(内参和外参)篇

1: Kinect内参标定参考:https://github.com/code-iai/iai_kinect2/tree/master/kinect2_calibration1.1 注意事项:1. ...

Kinect深度图与彩色图的坐标校准

kinect的深度数据和彩色数据的分辨率以及视场大小都不一样,不能直接对应起来,想要把深度和彩色融合起来就要费一番周折了。 看了MSDN中kinectSDK的介绍,发现了一个ICoordinateM...

图像复原之维纳滤波

维纳滤波也称最小均方误差滤波,它能处理被退化函数退化和噪声污染的图像。该滤波方法建立在图像和噪声都是随机变量的基础之上,目标是找到未污染图像f(x,y)的一个估计,使它们之间的均方误差最小,即,其中E...

维纳滤波在图像复原中的应用

图像退化/复原模型      g(x,y) = h(x,y)*f(x,y)+n(x,y)     频域:G(u,v) = H(u,v)F(u,v) +N(u,v)     ...

图像去模糊(维纳滤波)

在数学应用上,对于运动引起的图像模糊,最简单的方法是直接做逆滤波,但是逆滤波对加性噪声特别敏感,使得回复的图像几乎不可用。最小均方差(维纳)滤波用来去除含有噪声的模糊图像,其目标是找到未污染图像的一个...

维纳滤波器(Wiener Filter)在图像处理中的应用(一)

维纳滤波器是一种自适应的滤波器,在数字信号处理中有着广泛的应用。ispforfun会在从今天开始定期给大家带来维纳滤波器在图像处理中应用。本节讲诉维纳滤波器在图像去噪中的简单应用。        让...
内容举报
返回顶部
收藏助手
不良信息举报
您举报文章:深度学习:神经网络中的前向传播和反向传播算法推导
举报原因:
原因补充:

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