Kinect 获取深度图计算距离，并进行彩色图和深度图之间的映射

Kinect 获取深度图计算距离，并进行彩色图和深度图之间的映射

最近所进行的项目需要利用KINECT获取深度距离，需要得到彩色图中某一点的位置，在网上找了很多资料，都不是很好，碰了很多坑，这里我整理了一下发给大家，大家有需求可以自取。

---

获取深度图计算距离

KINECT安装openCv安装这里就不说了呢，网上一大堆。
直接上代码：

注意这里是初始化的一些代码，有些头文件是之后做通信用的，不需要的话可以删掉呢，比如Winsock2 wstcpip等

初始化的一些操作这里就不说了

下面是循环读取最近的一帧：

    while (1) {
        // Color Frame
        IColorFrame* pColorFrame = nullptr;
        hResult = pColorReader->AcquireLatestFrame(&pColorFrame);
        if (SUCCEEDED(hResult)) {
            hResult = pColorFrame->CopyConvertedFrameDataToArray(colorBufferSize, reinterpret_cast<BYTE*>(colorBufferMat.data), ColorImageFormat::ColorImageFormat_Bgra);
            if (SUCCEEDED(hResult)) {
            //  cv::resize(colorBufferMat, colorMat, cv::Size(), 0.5, 0.5);
            }
        }
        //SafeRelease( pColorFrame );

        // Depth Frame
        IDepthFrame* pDepthFrame = nullptr;
        hResult = pDepthReader->AcquireLatestFrame(&pDepthFrame);
        if (SUCCEEDED(hResult)) {
            hResult = pDepthFrame->AccessUnderlyingBuffer(&depthBufferSize, reinterpret_cast<UINT16**>(&depthBufferMat.data));
            pDepthFrame->CopyFrameDataToArray(depthHeight * depthWidth, (UINT16 *)temp.data); //先把数据存入16位的图像矩阵中
            if (SUCCEEDED(hResult)) {
                depthBufferMat.convertTo(depthMat, CV_8U, -255.0f / 8000.0f, 255.0f);
            }
        }
        //SafeRelease( pDepthFrame );

        // Mapping (Depth to Color)
        if (SUCCEEDED(hResult)) {
            std::vector<ColorSpacePoint> colorSpacePoints(depthWidth * depthHeight);
            hResult = pCoordinateMapper->MapDepthFrameToColorSpace(depthWidth * depthHeight, reinterpret_cast<UINT16*>(depthBufferMat.data), depthWidth * depthHeight, &colorSpacePoints[0]);
            if (SUCCEEDED(hResult)) {
                coordinateMapperMat = cv::Scalar(0, 0, 0, 0);
                for (int y = 0; y < depthHeight; y++) {
                    for (int x = 0; x < depthWidth; x++) {
                        unsigned int index = y * depthWidth + x;
                        ColorSpacePoint point = colorSpacePoints[index];
                        int colorX = static_cast<int>(std::floor(point.X + 0.5));
                        int colorY = static_cast<int>(std::floor(point.Y + 0.5));
                        unsigned short depth = depthBufferMat.at<unsigned short>(y, x);
                        if ((colorX >= 0) && (colorX < colorWidth) && (colorY >= 0) && (colorY < colorHeight)/* && ( depth >= minDepth ) && ( depth <= maxDepth )*/) {
                            coordinateMapperMat.at<cv::Vec4b>(y, x) = colorBufferMat.at<cv::Vec4b>(colorY, colorX);
                        }

                    }
                }
            }
        }
        //反转图像

        cv::flip(coordinateMapperMat, coordinateMapperMatFlip, 1);
        cv::flip(temp, tempFlip, 1);

        std::string Img_Name = "D:\\git_project\\yolo\\darknetpro\\darknet\\build\\darknet\\x64\\testpic\\test_" + std::to_string(k) + ".jpg";
        std::string txt_addr = "D:\\git_project\\yolo\\darknetpro\\darknet\\build\\darknet\\x64\\picpath\\test_" + std::to_string(k) + ".txt";
        std::string buff = "testpic/test_" + std::to_string(k) + ".jpg";

        // 这里是得到深度图中的距离，一定要注意这里XY是反的， 例如你在映射后的图像中取到（1，2），在这里应该输入
        //（2，1）才能得到对应的深度， 切记
        std::cout << "1:  " << tempFlip.at<UINT16>(183, 237) << "\t";
        std::cout << "2:  " << tempFlip.at<UINT16>(187, 227) << "\t";
        std::cout << "3:  " << tempFlip.at<UINT16>(194, 230) << "\t";
        std::cout << "4:  " << tempFlip.at<UINT16>(250, 334) << "\t";
        std::cout << "5:  " << tempFlip.at<UINT16>(254, 326) << "\t";
        std::cout << "6:  " << tempFlip.at<UINT16>(259, 335) << "\n";


        temp.convertTo(img, CV_8UC1, 255.0 / 4500);   //再把16位转换为8位
        //cv::imwrite("D:\\Projects-2018\\Robotic_Sorting_7_13\\Kinect\\TestKinect\\DepthImage\\test_1.jpg", coordinateMapperMat);
        Sleep(1000);
        SafeRelease(pColorFrame);
        SafeRelease(pDepthFrame);

        //cv::imshow("Color", colorMat);
        //cv::imshow("Depth", depthMat);
        //cv::imshow("CoordinateMapper", coordinateMapperMat);

        /*cv::imshow("CoordinateMapper", coordinateMapperMatFlip);
        if (cv::waitKey(30) == VK_ESCAPE) {
            break;
        }*/
        k++;
    }

    SafeRelease(pColorSource);
    SafeRelease(pDepthSource);
    SafeRelease(pColorReader);
    SafeRelease(pDepthReader);
    SafeRelease(pColorDescription);
    SafeRelease(pDepthDescription);
    SafeRelease(pCoordinateMapper);
    if (pSensor) {
        pSensor->Close();
    }
    SafeRelease(pSensor);
    cv::destroyAllWindows();

    return 0;
}

coordinateMapperMat 是映射之后的图像如下：

注意这里利用OpenCv对图像进行了反转 Cv::flip，很大的坑，没有反转后的图像做坐标转换会有大问题。

并且取深度距离的时候切记：

这里是得到深度图中的距离，一定要注意这里XY是反的， 例如你在映射后的图像中取到（1，2），在这里应该输入（2，1）才能得到对应的深度， 切记

得到深度如下：

还有之前的文章写要去掉后三位是不需要的呢，现在的版本自动给我们转换了呢。

对应的所有源码包括YOLO端识别物体都放在我的Chat上了，有需求可以自取，Chat上还会更详细讲，这里比较忙，就不多说了。

https://gitbook.cn/gitchat/activity/5b8605f23b698170ceac1fc9

在Chat上有微信号，也可以直接加我问的，我如果有空的话会详细告诉你。