Kinect基于微软SDK-彩图与深度图对齐

其实就是彩图映射到深度图，我建议你以后修改下代码，比如：不显示红外图，加快运行。

#include<opencv2/opencv.hpp>
#include <iostream>
#include<iomanip>
#include <Kinect.h>  

using namespace cv;
using namespace std;

//****************************************************
int i = 0;//拍照命名计数
bool flag;// 如果为真，i进行计数自增一

stringstream strColor;//保存彩图名称
stringstream strDepth;//保存深度图名称
stringstream strIR;   //保存红外图名称
                      //****************************************************

UINT16 *depthData = new UINT16[424 * 512];
Mat Temp;
Mat Temp_;
// 安全释放指针
template<class Interface>
inline void SafeRelease(Interface *& pInterfaceToRelease)
{
    if (pInterfaceToRelease != NULL)
    {
        pInterfaceToRelease->Release();
        pInterfaceToRelease = NULL;
    }
}

int main()
{

    IKinectSensor *kinect;           //创建一个感应器
    HRESULT hr;     //用于检测操作是否成功
    hr = GetDefaultKinectSensor(&kinect);
    if (FAILED(hr))
    {
        return hr;
    }
    IMultiSourceFrameReader* m_pMultiFrameReader = nullptr;//下面的指针都初始化了，避免由于SDL检测，不能通过编译
    if (kinect)
    {
        hr = kinect->Open();     //打开感应器
        if (SUCCEEDED(hr))
        {
            hr = kinect->OpenMultiSourceFrameReader(
                FrameSourceTypes::FrameSourceTypes_Color |
                FrameSourceTypes::FrameSourceTypes_Infrared |
                FrameSourceTypes::FrameSourceTypes_Depth,
                &m_pMultiFrameReader
            );
        }
    }

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

    // 三个数据帧及引用
    IDepthFrameReference* m_pDepthFrameReference = nullptr;
    IColorFrameReference* m_pColorFrameReference = nullptr;
    IInfraredFrameReference* m_pInfraredFrameReference = nullptr;
    IInfraredFrame* m_pInfraredFrame = nullptr;
    IDepthFrame* m_pDepthFrame = nullptr;
    IColorFrame* m_pColorFrame = nullptr;

    // 三个图片格式
    Mat i_rgb(1080, 1920, CV_8UC4);      //注意：这里必须为4通道的图，Kinect的数据只能以Bgra格式传出
    Mat i_depth(424, 512, CV_16UC1);
    Mat i_ir(424, 512, CV_16UC1);

    IMultiSourceFrame* m_pMultiFrame = nullptr;



    while (true)
    {
        // <4>获取新的一个多源数据帧  如果没有意外 以下 hr 都会 Returns S_OK 
        hr = m_pMultiFrameReader->AcquireLatestFrame(&m_pMultiFrame);
        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);

        if (SUCCEEDED(hr))
        {
            hr = m_pColorFrame->CopyConvertedFrameDataToArray(1080 * 1920 * 4, reinterpret_cast<BYTE*>(i_rgb.data), ColorImageFormat::ColorImageFormat_Bgra);   //传出数据
        }
        if (SUCCEEDED(hr))
        {
            hr = m_pDepthFrame->CopyFrameDataToArray(424 * 512, reinterpret_cast<UINT16*>(i_depth.data));
            /*
            hr = m_pDepthFrame->CopyFrameDataToArray(424 * 512, depthData);
            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;
            }
            */
        }
        if (SUCCEEDED(hr))
        {
            hr = m_pInfraredFrame->CopyFrameDataToArray(424 * 512, reinterpret_cast<UINT16*>(i_ir.data));
        }
        
        namedWindow("color", WINDOW_NORMAL);
        imshow("color", i_rgb);

        //i_depth.convertTo(Temp, CV_8UC1);//便于肉眼可见
        //imshow("depth", Temp);
        //imshow("ir", i_ir);

        for (int i = 0; i < (424 * 512); i++)
        {
            depthData[i] = (i_depth.data)[i];
        }
        /*
        // 数据传递测试
            cv::Mat imageXX = cv::Mat::zeros(424,512,CV_16UC1);
        for (int i = 0; i < (424 * 512); i++)
        {
            (imageXX.data)[i] = depthData[i];
        }

        */
        //****************************************************
        //彩色图映射到深度【核心代码】
        int colorHeight = 1080;
        int colorWidth = 1920;
        //Mat coordinateMapperMat_(colorHeight, colorWidth, CV_16UC1);//映射后的深度图
        Mat coordinateMapperMat_ =    Mat::ones(colorHeight, colorWidth, CV_16UC1);


        ICoordinateMapper*      m_pCoordinateMapper_;
        hr = kinect->get_CoordinateMapper(&m_pCoordinateMapper_);

        std::vector<DepthSpacePoint> depthSpacePoints(colorHeight * colorWidth );
        hr = m_pCoordinateMapper_->MapColorFrameToDepthSpace(424 * 512 , reinterpret_cast<UINT16*>(i_depth.data), colorHeight * colorWidth, &depthSpacePoints[0]);

        if (SUCCEEDED(hr))
        {
            //coordinateMapperMat_ = cv::Scalar(0);//？？？？
            for (int y = 0; y < colorHeight; y++)
            {
                for (int x = 0; x < colorWidth; x++)
                {
                    unsigned int index_ = y*colorWidth + x;
                    DepthSpacePoint point_ = depthSpacePoints[index_];
                    int depthX_ = static_cast<int>(std::floor(point_.X + 0.5));
                    int depthY_ = static_cast<int>(std::floor(point_.Y + 0.5));
                    if ((depthX_ >= 0) && (depthX_ < 512) && (depthY_ >= 0) && (depthY_ < 424)) //&& (depth_ >= 800) && (depth_ <= 5000))
                    {
                        coordinateMapperMat_.at<unsigned short>(y, x) = i_depth.at<unsigned short>(depthY_, depthX_);
                    }
                }
            }
        }

        coordinateMapperMat_.convertTo(Temp_, CV_8UC1);
        namedWindow("coordinateMapperMat_", cv::WINDOW_NORMAL);
        imshow("coordinateMapperMat_", Temp_);
        if (waitKey(1) == VK_ESCAPE)
            break;

        //****************************************************
        //拍照
        flag = 0;
        int key = cv::waitKey(30);//获取键盘 顺便延时
        if ((key & 255) == 32)//空格
        {
            //【注】png是无损格式，同时 imread(depth.png, -1);后面的 “-1”是无损读取原图
            strColor << "D:\\Temp\\color\\color_" << setw(2) << setfill('0') << i << ".png"; 
            strDepth << "D:\\Temp\\depth\\depth_" << setw(2) << setfill('0') << i << ".png";
            strIR << "D:\\Temp\\ir\\ir_" << setw(2) << setfill('0') << i << ".png";

            cout << strColor.str() << "||" << strDepth.str() << "||" << strIR.str() << endl;

            imwrite(strColor.str(), i_rgb);//【注：】这里改为：红外图映射后的彩图
            imwrite(strDepth.str(), coordinateMapperMat_);
            imwrite(strIR.str(), i_ir);

            strColor.str("");
            strDepth.str("");
            strIR.str("");

            flag = 1;
        }
        if (flag) i++;
        //****************************************************

        // 释放资源
        SafeRelease(m_pColorFrame);
        SafeRelease(m_pDepthFrame);
        SafeRelease(m_pInfraredFrame);
        SafeRelease(m_pColorFrameReference);
        SafeRelease(m_pDepthFrameReference);
        SafeRelease(m_pInfraredFrameReference);
        SafeRelease(m_pMultiFrame);
    }

    // 关闭窗口，设备
    cv::destroyAllWindows();
    kinect->Close();
    std::system("pause");
    destroyAllWindows();
}

 

转载于:https://www.cnblogs.com/winslam/p/8999592.html