kinect 2.0 SDK学习笔记（三）--生成3D点云

生成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);
        }
    }
}

显示效果如下：

完整代码