Kinect2点云配准

使用opencv对Kinect2相机采集的深度图像和彩色图像实现配准

使用opencv对Kinect2采集的深度图像和彩色图像进行配准结果进行显示。

opencv的数据结构

在进行kinect2相机实现点云的配准过程中，使用opencv创建了Mat类型的数据，mat类型数据创建过程中，Mat i_depthToRgb(424, 512, CV_8UC4);//高宽
需要指定图像的宽（512）和高（424），同时还需要指定数据类型，本例中指定的数据类型为CV_8UC4。本例中使用的数据类型为CV_8UC4，8U表示无符号整型，使用Kinect2获得的彩色图像为4通道（ColorImageFormat_Bgra），其中C4表示的就是四个通道。在进行点云映射过程中使用函数MapDepthFrameToColorSpace实现，进行显示配准之后的点云显示过程中使用代码以下进行显示。对于i_depthToRgb.data和ColorImg.data之间的转换使用Mat::data指针实现。通过遍历的方式实现。同时在进行图像数据转换的过程中也可以使用data 指针的复制实现ma类型的初始化， cv::Mat mcolorImage(cColorHeight, cColorWidth, CV_8UC4, pBuffer);// pBuffer为Kinect相机采集的RGBQUAD结构的颜色图像，通过以上方式实现imshow的显示。Opencv中的数据类型文档中介绍的共有30种。同时也可以使用宏定义自行增加数据类型，增加数据类型的方法可以查看链接。可以使用转换函数converto实现数据类型的转换。数据类型的含义和具体转换方式可通过链接查看OpenCV中Mat数据类型及相互转换使用imshow进行显示图像时，需要将像素值映射到[0，255]，不同数据类型显示时需要进行转换。

实现采集和映射的代码

#include <stdio.h>
#include <Kinect.h>
#include <windows.h>
#include <opencv2/opencv.hpp>
#include <stdlib.h>
#include <time.h>
#include <string.h>
#include <strstream>
using namespace cv;
using namespace std;
void app()
{
    IKinectSensor* m_pKinectSensor;
    IDepthFrameReader* m_pDepthFrameReader;
    IDepthFrameSource* pDepthFrameSource = NULL;
    IColorFrameReader* ColorFrameReader = NULL;
    IColorFrameSource* ColorFrameSource = NULL;
    IFrameDescription* FrameDescription = NULL;
    GetDefaultKinectSensor(&m_pKinectSensor);
    //打开传感器
    m_pKinectSensor->Open();
    m_pKinectSensor->get_DepthFrameSource(&pDepthFrameSource);
    m_pKinectSensor->get_ColorFrameSource(&ColorFrameSource);
    pDepthFrameSource->OpenReader(&m_pDepthFrameReader);
    ColorFrameSource->OpenReader(&ColorFrameReader);

    while (true)
    {
        IDepthFrame* pDepthFrame = NULL;
        IColorFrame* pColorFrame = NULL;
        while (pDepthFrame == NULL) {
            m_pDepthFrameReader->AcquireLatestFrame(&pDepthFrame);
        }
        pDepthFrameSource->get_FrameDescription(&FrameDescription);
        int depth_width, depth_height;
        FrameDescription->get_Width(&depth_width);
        FrameDescription->get_Height(&depth_height);
        Mat DepthImg(depth_height, depth_width, CV_16UC1);
        pDepthFrame->CopyFrameDataToArray(depth_height * depth_width, (UINT16*)DepthImg.data);

        //转换为8位图像
        DepthImg.convertTo(DepthImg, CV_8UC1, 255.0 / 4500);
        //均衡化
        //equalizeHist(DepthImg, DepthImg);
        while (pColorFrame == NULL) {
            ColorFrameReader->AcquireLatestFrame(&pColorFrame);
        }
        pColorFrame->get_FrameDescription(&FrameDescription);
        int CWidth, CHeight;
        FrameDescription->get_Width(&CWidth);
        FrameDescription->get_Height(&CHeight);
        Mat ColorImg(CHeight, CWidth, CV_8UC4);
        pColorFrame->CopyConvertedFrameDataToArray(CWidth * CHeight * 4, (BYTE*)ColorImg.data, ColorImageFormat_Bgra);
        //显示图像
        imshow("depthImg", DepthImg);
        cv::namedWindow("ColorImg",0);
        cv::resizeWindow("ColorImg",CWidth/3 ,CHeight/3);
        imshow("ColorImg", ColorImg);
        //实现匹配并显示
        UINT16* depthData = new UINT16[424 * 512];
        pDepthFrame->CopyFrameDataToArray(424 * 512, depthData);
        ICoordinateMapper* m_pCoordinateMapper;
        m_pKinectSensor->get_CoordinateMapper(&m_pCoordinateMapper);
        ColorSpacePoint* m_pColorCoordinates = new ColorSpacePoint[ 512* 424];
        HRESULT hr = m_pCoordinateMapper->MapDepthFrameToColorSpace(512 * 424, depthData, 512 * 424, m_pColorCoordinates);
        Mat i_depthToRgb(424, 512, CV_8UC4);//高*宽
        if (SUCCEEDED(hr))
        {
            for (int i = 0; i < 512 * 424; 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] = ColorImg.data[(colorY * 1920 + colorX) * 4];
                        i_depthToRgb.data[i * 4 + 1] = ColorImg.data[(colorY * 1920 + colorX) * 4 + 1];
                        i_depthToRgb.data[i * 4 + 2] = ColorImg.data[(colorY * 1920 + colorX) * 4 + 2];
                        i_depthToRgb.data[i * 4 + 3] = ColorImg.data[(colorY * 1920+ colorX) * 4 + 3];
                    }
                }
            }
        }
        imshow("rgb2depth", i_depthToRgb);

        DepthImg.release();
        ColorImg.release();

        if (pDepthFrame)
            pDepthFrame->Release();
        if (pColorFrame)
            pColorFrame->Release();

        if (27 == waitKey(1))
            break;
    }
}

int main()
{
    app();
}

ColorImg.data中像素储存的方式可以认为是一个向量，向量的长度为512 * 424，从而实现索引。代码内容参考了以下博主

https://blog.csdn.net/tengfei461807914/article/details/80615721
https://blog.csdn.net/jiaojialulu/article/details/53088170