生成3D點雲

本文在上一教程座標對映基礎上，構建3D點雲並顯示出來。
1. 獲得深度幀和彩色幀：

hr = m_pDepthFrameReader->AcquireLatestFrame(&m_pDepthFrame);
hr = m_pColorFrameReader->AcquireLatestFrame(&m_pColorFrame);

1. 儲存深度資料和顏色資料到陣列

UINT16 *depthData = new UINT16[424*512];
hr = m_pDepthFrame->CopyFrameDataToArray(424
 
 * 512, depthData);

Mat i_rgb(1080  , 1920, CV_8UC4);
UINT nColorBufferSize = 1920 * 1080 * 4;
if (SUCCEEDED(hr))
    hr = m_pColorFrame->CopyConvertedFrameDataToArray(nColorBufferSize, i_rgb.data, ColorImageFormat::ColorImageFormat_Bgra);

1. 獲得座標對映器：

ICoordinateMapper*      m_pCoordinateMapper;
hr 
 
= m_pKinectSensor->get_CoordinateMapper(&m_pCoordinateMapper);

1. 深度圖對映到彩色圖：

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 < 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];
            }
        }
    }
}

1. 深度圖對映到相機空間

if (SUCCEEDED(hr))
{
    HRESULT hr = m_pCoordinateMapper->MapDepthFrameToCameraSpace(512 * 424, depthData, 512*424, m_pCameraCoordinates);
}

1. 顯示（這裡用的是opengl）

if (SUCCEEDED(hr))
{
    for (int i = 0; i < 512 * 424; i++)
    {
        CameraSpacePoint p = m_pCameraCoordinates[i];
        if (p.X != -std::numeric_limits<float>::infinity() && p.Y != -std::numeric_limits<float>::infinity() && p.Z != -std::numeric_limits<float>::infinity())
        {
            float cameraX = static_cast<float>(p.X);
            float cameraY = static_cast<float>(p.Y);
            float cameraZ = static_cast<float>(p.Z);

            //cout << "x: " << cameraX << "y: " << cameraY << "z: " << cameraZ << endl;
            GLubyte *rgb = new GLubyte();
            rgb[2] = i_depthToRgb.data[i * 4 + 0];
            rgb[1] = i_depthToRgb.data[i * 4 + 1];
            rgb[0] = i_depthToRgb.data[i * 4 + 2];
            // 顯示點
            glColor3ubv(rgb);
            glVertex3f(cameraX, -cameraY, cameraZ);
        }
    }
}

顯示效果如下：

完整程式碼