PCL——Kinect v2 保存点云数据

最新推荐文章于 2024-01-02 22:30:45 发布

原创最新推荐文章于 2024-01-02 22:30:45 发布 · 921 阅读

5 ·

CC 4.0 BY-SA版权

文章标签：

#c++

PCL 专栏收录该内容

4 篇文章

订阅专栏

该博客介绍了如何使用VTK库结合Windows API从Kinect获取深度数据，通过PCL进行点云处理，包括噪声去除、数据保存，并展示了文件管理和用户交互。

摘要生成于 C知道，由 DeepSeek-R1 满血版支持，前往体验 >

参考：https://www.cnblogs.com/yhlx125/p/6643821.html

#include <vtkAutoInit.h>
#include <Windows.h>
#include <iostream>
#include <kinect.h>
#include <pcl/io/pcd_io.h> 
#include <pcl/point_types.h> 
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/filters/radius_outlier_removal.h>  
#include <pcl/filters/conditional_removal.h>
#include <pcl/filters/statistical_outlier_removal.h> 
#include <string.h> 
#include <direct.h>    //头文件  
#include <io.h>  
#include <AR_Medical.h>

using namespace std;
typedef pcl::PointXYZ  MyPointDataType;



template<class Interface>
//安全释放指针
inline void SafeRelease(Interface *& pInterfaceToRelease)
{
	if (pInterfaceToRelease != NULL)
	{
		pInterfaceToRelease->Release();
		pInterfaceToRelease = NULL;
	}
}

string num2str(int i)
{
	stringstream ss;
	ss << i;
	return ss.str();
}
//主函数
int KinectGetPCD()
{
	int count = 1;//文件序号
	cout << "进入<点云采集系统>" << endl;
	cout << "_____________________________ 正在打开Kinect相机，等待......" ;

	//获取Kinect传感器
	IKinectSensor* m_pKinectSensor = nullptr;
	HRESULT hr;
	hr = GetDefaultKinectSensor(&m_pKinectSensor);
	if (FAILED(hr))
	{
		return hr;
	}
	//多源数据读取
	IMultiSourceFrameReader* m_pMultiFrameReader = nullptr;
	if (m_pKinectSensor)
	{
		//打开相机
		hr = m_pKinectSensor->Open();
		if (SUCCEEDED(hr))
		{
			cout << "已打开！" << endl;
			
			//获取RGB、深度图
			hr = m_pKinectSensor->OpenMultiSourceFrameReader(
				FrameSourceTypes::FrameSourceTypes_Depth ,
				&m_pMultiFrameReader);
		}
	}
	if (!m_pKinectSensor || FAILED(hr))
	{
		return E_FAIL;
	}
	//点云数据存储数组
	UINT16 *depthData = new UINT16[424 * 512];  
	//获取数据引用
	IDepthFrameReference* m_pDepthFrameReference = nullptr;
	//深度图像缓冲区
	IDepthFrame* m_pDepthFrame = nullptr;
	//多源数据缓冲区
	IMultiSourceFrame* m_pMultiFrame = nullptr;
	//坐标映射
	ICoordinateMapper* m_pCoordinateMapper = nullptr;
	
	cout<<"_____________________________ 输入对象名称：" << endl;
	char ModelName[32];
	cin >> ModelName;
	//创建文件保存地址
	string pcdFileDir = (".\\pcdFileDir\\%s", ModelName);//相对路径下创建对象名称的文件夹，放保存的点云数据
	if (_access(pcdFileDir.c_str(), 0) == -1)
	{
		int i = _mkdir(pcdFileDir.c_str());
		cout << "_____________________________ 文件夹创建成功" << endl;
	}
	else
	{
		//文件序号顺接
		count = GetFileNum(pcdFileDir.c_str())+1;
		cout << "_____________________________ 文件夹已存在，且已存在"<< GetFileNum(pcdFileDir.c_str())<<"个数据" << endl;
		
	}
	//对键盘操作做出响应
	while(true)  
	{
		
		//刷新缓冲
		while (true)
		{
			hr = m_pMultiFrameReader->AcquireLatestFrame(&m_pMultiFrame); //最新一帧
			if (FAILED(hr) || !m_pMultiFrame)
			{
				continue;
			}
			break;
		}
		//获取深度数据
		if (SUCCEEDED(hr))
			hr = m_pMultiFrame->get_DepthFrameReference(&m_pDepthFrameReference);
		if (SUCCEEDED(hr))
			hr = m_pDepthFrameReference->AcquireFrame(&m_pDepthFrame);
		hr = m_pKinectSensor->get_CoordinateMapper(&m_pCoordinateMapper); //坐标映射器
		//读取键盘
		char cmd;
		cout << "_____________________________ 【y】--保存第 "<<count<<" 个视角" << endl<<"_____________________________ 【q】--退出<点云采集系统>" << endl;
		cin >> cmd;
		//【y】保存点云数据
		if (cmd == 'y')
		{
				
			//创建点云
			pcl::PointCloud<MyPointDataType>::Ptr cloud(new pcl::PointCloud<MyPointDataType>);
			cloud->width = 512 * 424;//无序
			cloud->height = 1;
			cloud->is_dense = false;
			cloud->points.resize(cloud->width * cloud->height);
			if (SUCCEEDED(hr))
			{
				hr = m_pDepthFrame->CopyFrameDataToArray(424 * 512, depthData);//点云数据存入数组
				CameraSpacePoint* m_pCameraCoordinates = new CameraSpacePoint[512 * 424];//相机空间坐标
				hr = m_pCoordinateMapper->MapDepthFrameToCameraSpace(512 * 424, depthData, 512 * 424, m_pCameraCoordinates);
				//坐标存入点云
				for (int i = 0; i < 512 * 424; i++)
				{
					CameraSpacePoint cameraP = m_pCameraCoordinates[i];
					//去噪点
					if (cameraP.X != -std::numeric_limits<float>::infinity() && cameraP.Y != -std::numeric_limits<float>::infinity() && cameraP.Z != -std::numeric_limits<float>::infinity())
					{
						float cameraX = static_cast<float>(cameraP.X);
						float cameraY = static_cast<float>(cameraP.Y);
						float cameraZ = static_cast<float>(cameraP.Z);
						cloud->points[i].x = cameraX;
						cloud->points[i].y = cameraY;
						cloud->points[i].z = cameraZ;
					}
				}
			}
			//保存文件 命名规则：文件目录-pcdFileDir-（当天日期，如：2022- 3-22）-对象名称-序号
			SYSTEMTIME st;
			GetLocalTime(&st);
			char filename[100];
			sprintf_s(filename, ("%s\\%d.pcd"), pcdFileDir.c_str(), count);//输入都是字符串.c_str()
			pcl::io::savePCDFileBinary(filename, *cloud);
			cout << "_____________________________ 点云文件" << filename << "已保存，保存点数： " << cloud->points.size() << std::endl;
			count++;
			//释放缓存
			SafeRelease(m_pDepthFrame);
			SafeRelease(m_pDepthFrameReference);
			SafeRelease(m_pMultiFrame);
		}
		
		//【q】退出（循环），即退出本函数	
		if (cmd == 'q')
		{
			break;
		}
		
		//cout << ">>>>>>>>>>>>>>>>>>>>>>>>>【ENTER】键浏览点云" << endl;
		//boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer(new pcl::visualization::PCLVisualizer("3D viewer"));
		//// 【ENTER】 浏览
		//if (KeyValue == 13)
		//{
		//	string filename = "Model-1.pcd";
		//	pcl::PointCloud<pcl::PointXYZ>::Ptr Model_cloud(new pcl::PointCloud<pcl::PointXYZ>);
		//	try
		//	{
		//		pcl::io::loadPCDFile(filename, *Model_cloud);
		//	}
		//	catch (const std::exception&)
		//	{
		//		std::cout << "找不到点云文件 \n" << endl; //文件不存在时，返回错误，终止程序。
		//		return (-1);
		//	}
		//	//显示
		//	viewer->addPointCloud(Model_cloud);
		//	viewer->resetCamera();
		//	viewer->addCoordinateSystem(0.1);
		//	viewer->initCameraParameters();
		//	while (!viewer->wasStopped()) {
		//		viewer->spinOnce();
		//	}
		//}
	}
	//退出
	m_pKinectSensor->Close();
	SafeRelease(m_pKinectSensor);
	cout << "退出<点云采集系统>" << endl;
	return 0;
}