三维视觉--基于Kinect2.0深度相机的点云获取方案实现（C++版）

上一篇中使用的点云获取设备是Intel Realsense d455相机，这两天接触的相机不少，也发现很多点云检测和分割的论文中使用的都是Kinect相机，今天就再分享一篇使用Kinect2.0获取点云并使用C++实现。

首先还是相机SDK的下载和安装：可以去官网下载，也可以到我之前分享的百度网盘获取。

点云获取（深度相机）方案实现C++版_低调猫熊的博客-CSDN博客

安装后再配置环境：

环境配好后，就是代码编写， 

代码如下：

#include <iostream>
#include <fstream>

#include <Windows.h>
#include <Kinect.h>

#include <opencv2/opencv.hpp>
#include <pcl/io/ply_io.h>
#include <pcl/point_types.h>

using namespace std;
using namespace cv;

template<class Interface>
inline void SafeRelease(Interface*& ptr)
{
    if (ptr)
    {
        ptr->Release();
        ptr = nullptr;
    }
}

int main()
{
    // 初始化Kinect
    IKinectSensor* pKinectSensor = nullptr;
    HRESULT hr = GetDefaultKinectSensor(&pKinectSensor);
    if (FAILED(hr))
    {
        cerr << "Failed to get default Kinect sensor." << endl;
        return -1;
    }

    hr = pKinectSensor->Open();
    if (FAILED(hr))
    {
        cerr << "Failed to open Kinect sensor." << endl;
        return -1;
    }

    // 打开深度数据流和彩色数据流
    IDepthFrameReader* pDepthFrameReader = nullptr;
    IColorFrameReader* pColorFrameReader = nullptr;

    IDepthFrameSource* pDepthFrameSource = nullptr;
    hr = pKinectSensor->get_DepthFrameSource(&pDepthFrameSource);
    if (SUCCEEDED(hr))
    {
        hr = pDepthFrameSource->OpenReader(&pDepthFrameReader);
    }
    SafeRelease(pDepthFrameSource);
    if (FAILED(hr))
    {
        cerr << "Failed to open depth frame reader." << endl;
        return -1;
    }

    IColorFrameSource* pColorFrameSource = nullptr;
    hr = pKinectSensor->get_ColorFrameSource(&pColorFrameSource);
    if (SUCCEEDED(hr))
    {
        hr = pColorFrameSource->OpenReader(&pColorFrameReader);
    }
    SafeRelease(pColorFrameSource);
    if (FAILED(hr))
    {
        cerr << "Failed to open color frame reader." << endl;
        return -1;
    }

    // 打开坐标映射器
    ICoordinateMapper* pCoordinateMapper = nullptr;
    hr = pKinectSensor->get_CoordinateMapper(&pCoordinateMapper);
    if (FAILED(hr))
    {
        cerr << "Failed to get coordinate mapper." << endl;
        return -1;
    }

    // 创建窗口
    namedWindow("Color", WINDOW_AUTOSIZE);
    namedWindow("Depth", WINDOW_AUTOSIZE);

    // 定义缓冲区
    ColorSpacePoint* pColorSpacePoints = new ColorSpacePoint[512 * 424];
    UINT16* pDepthData = new UINT16[512 * 424];

    // 定义点云对象
    pcl::PointCloud<pcl::PointXYZRGB>::Ptr pPointCloud(new pcl::PointCloud<pcl::PointXYZRGB>);
    pPointCloud->width = 512;
    pPointCloud->height = 424;
    pPointCloud->points.resize(512 * 424);

    // 循环读取帧数据
    bool bSavePointCloud = false;
    while (true)
    {
        // 获取彩色帧
        IColorFrame* pColorFrame = nullptr;
        hr = pColorFrameReader->AcquireLatestFrame(&pColorFrame);
        if (SUCCEEDED(hr))
        {
            // 获取彩色帧数据
            UINT colorBufferSize = 1920 * 1080 * 4;
            BYTE* pColorBuffer = new BYTE[colorBufferSize];
            hr = pColorFrame->CopyConvertedFrameDataToArray(colorBufferSize, pColorBuffer, ColorImageFormat_Bgra);
            if (FAILED(hr))
            {
                cerr << "Failed to copy color frame data." << endl;
            }

            // 创建彩色图像
            Mat color(1080, 1920, CV_8UC4, pColorBuffer);
            // 显示彩色图像
            imshow("Color", color);

            // 释放彩色帧
            SafeRelease(pColorFrame);

            // 获取深度帧
            IDepthFrame* pDepthFrame = nullptr;
            hr = pDepthFrameReader->AcquireLatestFrame(&pDepthFrame);
            if (SUCCEEDED(hr))
            {
                // 获取深度帧数据
                hr = pDepthFrame->CopyFrameDataToArray(512 * 424, pDepthData);
                if (FAILED(hr))
                {
                    cerr << "Failed to copy depth frame data." << endl;
                }

                // 将深度图转换为彩色图的空间坐标
                pCoordinateMapper->MapDepthFrameToColorSpace(512 * 424, pDepthData, 512 * 424, pColorSpacePoints);

                // 释放深度帧
                SafeRelease(pDepthFrame);
            }
            else
            {
                cerr << "Failed to acquire latest depth frame." << endl;
            }
            // 显示深度图
            Mat depth(424, 512, CV_16UC1, pDepthData);
            imshow("Depth", depth);

            // 将深度图转换为点云
            for (int i = 0; i < 512 * 424; ++i)
            {
                pcl::PointXYZRGB& point = pPointCloud->points[i];
                ColorSpacePoint& colorSpacePoint = pColorSpacePoints[i];
                UINT16 depthValue = pDepthData[i];

                if (depthValue > 0 && colorSpacePoint.X >= 0 && colorSpacePoint.X < 1920 && colorSpacePoint.Y >= 0 && colorSpacePoint.Y < 1080)
                {
                    // 计算空间坐标
                    CameraSpacePoint cameraSpacePoint = { 0 };
                    pCoordinateMapper->MapDepthPointToCameraSpace({ (float)(i % 512), (float)(i / 512) }, depthValue, &cameraSpacePoint);

                    // 设置点云坐标和颜色
                    point.x = cameraSpacePoint.X;
                    point.y = cameraSpacePoint.Y;
                    point.z = cameraSpacePoint.Z;
                    point.r = color.at<Vec4b>((int)colorSpacePoint.Y, (int)colorSpacePoint.X)[2];
                    point.g = color.at<Vec4b>((int)colorSpacePoint.Y, (int)colorSpacePoint.X)[1];
                    point.b = color.at<Vec4b>((int)colorSpacePoint.Y, (int)colorSpacePoint.X)[0];
                }
                else
                {
                    // 设置无效点云坐标
                    //point.x = point.y = point.z = numeric_limits<float>::quiet_NaN();
                }
            }

        }
        else
        {
            cerr << "Failed to acquire latest color frame." << endl;
        }
        
        
       
        // 按下空格键保存点云数据为ply文件
        if (waitKey(30) == VK_SPACE)
        {
            if (!bSavePointCloud)
            {
                bSavePointCloud = true;

                // 保存点云数据为ply文件
                pcl::PLYWriter writer;
                
                writer.write("Kinect_point_cloud.ply", *pPointCloud);

                cout << "Saved point cloud data to point_cloud.ply" << endl;
            }
        }
    }

    // 释放资源
    SafeRelease(pDepthFrameReader);
    SafeRelease(pColorFrameReader);
    SafeRelease(pCoordinateMapper);
    if (pKinectSensor)
    {
        pKinectSensor->Close();
    }
    SafeRelease(pKinectSensor);
    delete[] pColorSpacePoints;
    delete[] pDepthData;

    return 0;
}

至此，就可以实现调用Kinect2.0相机获取点云了。

草稿代码可能不是那么严谨，但运行起来问题不大。