生成3D点云
本文在上一教程坐标映射基础上,构建3D点云并显示出来。
1. 获得深度帧和彩色帧:
hr = m_pDepthFrameReader->AcquireLatestFrame(&m_pDepthFrame);
hr = m_pColorFrameReader->AcquireLatestFrame(&m_pColorFrame);
- 保存深度数据和颜色数据到数组
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);
- 获得坐标映射器:
ICoordinateMapper* m_pCoordinateMapper;
hr = m_pKinectSensor->get_CoordinateMapper(&m_pCoordinateMapper);
- 深度图映射到彩色图:
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];
}
}
}
}
- 深度图映射到相机空间
if (SUCCEEDED(hr))
{
HRESULT hr = m_pCoordinateMapper->MapDepthFrameToCameraSpace(512 * 424, depthData, 512*424, m_pCameraCoordinates);
}
- 显示(这里用的是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);
}
}
}
显示效果如下:
完整代码