您的位置：首页 > 运维架构

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

2016-11-08 22:23 465 查看

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

显示效果如下：

完整代码

内容来自用户分享和网络整理，不保证内容的准确性，如有侵权内容，可联系管理员处理

标签： kinect 点云 OpenGL

相关文章推荐

新的分享

章节导航