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