Kinect三维重建1111----获取数据

版权声明：本文为CSDN博主「悄然的我-粤Y」的原创文章，遵循CC 4.0 BY-SA版权协议，转载请附上原文出处链接及本声明。
原文链接：https://blog.csdn.net/u010848251/article/details/70992345

———————————————————————————————

废话不多说，我采用的是Kinect v2 + PCL 的方法对模型奶牛进行重建。关键的想法如下图所示：

Kinect摄像机固定不动，从6个角度去拍摄模型奶牛，每个角度拍一帧（当然，想要有好的细节应该每个角度多拍几帧。这里假设只拍一帧），拍摄顺序为0-1-2-3-4-5，先右转60度后左转60度。获取到点云后，把前面3帧（即0-1-2）的点云融合，然后把后面3帧（即3-4-5）的点云融合，最后把前面的点云与后面的点云进行融合。大概的想法为这样子。

这篇文章先介绍如何获取点云数据。我使用的是二代Kinect，PCL1.8.0（没有用到太高级的库，所以个人觉得1.6以上的都行），事先要装Kinect SDK和配置好PCL环境。（这些 配置网上已经有很多教程，这里不叙述）。这里只获取深度数据，然后转化为点云。完整代码会在最后面给出。

首先看一下拍摄环境：

如左图所示，摄像机底下放了3个Kinect盒子，模型奶牛底下也放了1个Kinect盒子。模型奶牛到摄像机的距离大概在0.8米~1米之间。由于地板上有那些线，我利用这个十字点来当成旋转的中心，如右图所示。

生成点云的时候，我使用了PCL中“条件与”的方法过滤了一些点云（或者说只提取感兴趣或范围内的点云）。如下例代码所示：

//-----------------------提取范围内的点------------------------
pcl::ConditionAnd<MyPointDataType>::Ptr range_cond(new pcl::ConditionAnd<MyPointDataType>());
range_cond->addComparison(pcl::FieldComparison<MyPointDataType>::ConstPtr(new	pcl::FieldComparison<MyPointDataType>("z", pcl::ComparisonOps::GT, 0.001)));
range_cond->addComparison(pcl::FieldComparison<MyPointDataType>::ConstPtr(new	pcl::FieldComparison<MyPointDataType>("z", pcl::ComparisonOps::LT, 2.0)));
range_cond->addComparison(pcl::FieldComparison<MyPointDataType>::ConstPtr(new	pcl::FieldComparison<MyPointDataType>("x", pcl::ComparisonOps::GT, -0.5)));
range_cond->addComparison(pcl::FieldComparison<MyPointDataType>::ConstPtr(new	pcl::FieldComparison<MyPointDataType>("x", pcl::ComparisonOps::LT, 0.5)));
//range_cond->addComparison(pcl::FieldComparison<MyPointDataType>::ConstPtr(new	pcl::FieldComparison<MyPointDataType>("y", pcl::ComparisonOps::GT, -0.85)));
pcl::ConditionalRemoval<MyPointDataType> condrem(range_cond, false);
condrem.setInputCloud(cloud);
condrem.setKeepOrganized(false);
condrem.filter(*cloud_filtered);
//--------------------------------------------------------------

这里，对于Z值来说，0.001<Z<2.0，是我想要的点云。因为深度图里有很多深度值为0的数据，所以我只提取有深度值的点云，所以要大于0.001。然后我只要距摄像机2米之内的点云，这样可以过滤掉后面背景的那些点云。（Tips：这里的浮点数可以大概看成是现实中以米为单位的数值，如2.0大概表示现实中的2米）。而对于X值来说，-0.5<X<0.5，同样也是为了过滤掉左右两边不想要的点云。而对Y值有点特殊，这里不作处理（我注释掉了）。对于实验不同的情况，可以调节你想要的范围内的点云。 

拍摄的时候，有一个非常重要的前提——摄像机一定要水平放，这是非常重要的事情，接下来会解释。

从想法上看，我们应该要拍6帧数据。但是，我这里还要先额外拍2帧数据。

首先第一帧数据：

 拍摄的是地面的点云数据。这是为了后面去除地面点云数据而准备的。想法很简单：因为我们假设摄像机是水平放置的，那么生成的地面点云大概会在一个平面上（某一个xz平面上），那么我们只要获取这些地面点云中最大的Y值坐标（yMax1），就可以用来去除地面点云。即Y值小于yMax1的点云为地面点云，或者说Y值大于yMax1的点云为非地面点云。

第二帧数据：

        拍摄的是只有盒子的点云数据。同上面一样，因为我们假设摄像机是水平放置的，那么我们获取这些点云中最大的Y值坐标（yMax2），即为盒子上表面的坐标。因为模型奶牛是放在盒子上面的，那么对于去除了地面点云的数据，只剩下盒子和模型奶牛的数据，我们可以通过这个yMax2来分开盒子与模型奶牛的数据，即Y值小于yMax2的点云为盒子点云，Y值大于yMax2的点云为模型奶牛点云。
        这里区分盒子与奶牛点云数据是非常重要的，这里简单讲一下。在最后面——前后点云融合的时候，我们是想把奶牛的前后点云数据进行融合，然而这是一件非常困难的事情，因为前后点云数据之间重叠的点云非常少，或者说其对应点（Corresponding Points）少得可怜，难以进行配准。因此，我们改为对奶牛底下的盒子进行配准。因为盒子与奶牛之间相对固定的，拍摄的时候我没有去移动过奶牛，只对盒子进行了移动，那么奶牛也会跟随盒子进行移动。如果我们能够对盒子进行前后配准，那么相当于把奶牛前后配准好了。

下面是从6个不同角度去拍摄，其顺序在最开始的图已经说明了，其旋转角度大概在60度左右就进行了，不一定要很准确。

下面按照0-1-2-3-4-5的顺序进行拍摄，先右转后左转：

 

 

 

 

 于是，我们总共生成了8个PCD文件：

这些文件都只是点云数据，即XYZ数据。下面是完整代码： 

#define vtkRenderingCore_AUTOINIT 4(vtkInteractionStyle,vtkRenderingFreeType,vtkRenderingFreeType,vtkRenderingOpenGL) 
#define vtkRenderingVolume_AUTOINIT 1(vtkRenderingVolumeOpenGL)
#include <Windows.h>
#include <iostream>
#include <kinect.h>
#include <opencv2/core/core.hpp>  
#include <opencv2/highgui/highgui.hpp>  
#include <pcl/io/pcd_io.h> //PCL的PCD格式文件的输入输出头文件
#include <pcl/point_types.h> //PCL对各种格式的点的支持头文件
#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 <pcl/registration/icp.h> //ICP(iterative closest point)配准
#include <pcl/console/parse.h>  //pcl控制台解析
//kd树
#include <pcl/kdtree/kdtree_flann.h>
//特征提取
#include <pcl/features/normal_3d_omp.h>
#include <pcl/features/normal_3d.h>
//重构
#include <pcl/surface/gp3.h>
#include <pcl/surface/poisson.h>
 
#include <boost/thread/thread.hpp>
#include <string.h>
using namespace cv;
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 main()
{
	// 获取Kinect设备
	IKinectSensor* m_pKinectSensor;
	HRESULT hr;
	hr = GetDefaultKinectSensor(&m_pKinectSensor);
	if (FAILED(hr))
	{
		return hr;
	}
 
	IMultiSourceFrameReader* m_pMultiFrameReader;
	if (m_pKinectSensor)
	{
		hr = m_pKinectSensor->Open();
		if (SUCCEEDED(hr))
		{
			// 获取多数据源到读取器  
			hr = m_pKinectSensor->OpenMultiSourceFrameReader(
				//FrameSourceTypes::FrameSourceTypes_Color |
				//FrameSourceTypes::FrameSourceTypes_Infrared |
				FrameSourceTypes::FrameSourceTypes_Depth,
				&m_pMultiFrameReader);
		}
	}
 
	if (!m_pKinectSensor || FAILED(hr))
	{
		return E_FAIL;
	}
	
	UINT16 *depthData = new UINT16[424 * 512];//用于存储深度图数据
	Mat i_rgb(1080, 1920, CV_8UC4);
	Mat i_depthWrite(424, 512, CV_16UC1);
	UINT nColorBufferSize = 1920 * 1080 * 4;
 
	// 三个数据帧及引用
	IDepthFrameReference* m_pDepthFrameReference = nullptr;
	IColorFrameReference* m_pColorFrameReference = nullptr;
	IDepthFrame* m_pDepthFrame = nullptr;
	IColorFrame* m_pColorFrame = nullptr;
	IMultiSourceFrame* m_pMultiFrame = nullptr;
 
	ICoordinateMapper* m_pCoordinateMapper = nullptr;
	
 
	int count = 0;
	while (count <= 30)
	{
		
		Sleep(5000);
		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);
		//if (SUCCEEDED(hr))
		//	hr = m_pMultiFrame->get_ColorFrameReference(&m_pColorFrameReference);
		//if (SUCCEEDED(hr))
		//	hr = m_pColorFrameReference->AcquireFrame(&m_pColorFrame);
 
		hr = m_pKinectSensor->get_CoordinateMapper(&m_pCoordinateMapper);
 
		//if (SUCCEEDED(hr))
		//	hr = m_pColorFrame->CopyConvertedFrameDataToArray(nColorBufferSize, i_rgb.data, ColorImageFormat::ColorImageFormat_Bgra);
 
		// 定义相关变量
		pcl::PointCloud<MyPointDataType>::Ptr cloud(new pcl::PointCloud<MyPointDataType>);
		pcl::PointCloud<MyPointDataType>::Ptr cloud_filtered(new pcl::PointCloud<MyPointDataType>);
		//初始化点云数据PCD文件头
		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);
			/*hr = m_pDepthFrame->CopyFrameDataToArray(424 * 512, reinterpret_cast<UINT16*>(i_depthWrite.data));
			imwrite("depth_" + num2str(count) + ".png", i_depthWrite);*/
			CameraSpacePoint* m_pCameraCoordinates = new CameraSpacePoint[512 * 424];
			hr = m_pCoordinateMapper->MapDepthFrameToCameraSpace(512 * 424, depthData, 512 * 424, m_pCameraCoordinates);
			//ColorSpacePoint* m_pColorCoordinates = new ColorSpacePoint[512 * 424];
			//hr = m_pCoordinateMapper->MapDepthFrameToColorSpace(512 * 424, depthData, 512 * 424, m_pColorCoordinates);
			for (int i = 0; i < 512 * 424; i++)
			{
				//------写入RGB------
				/*ColorSpacePoint colorP = m_pColorCoordinates[i];
				if (colorP.X != -std::numeric_limits<float>::infinity() && colorP.Y != -std::numeric_limits<float>::infinity())
				{
					int colorX = static_cast<int>(colorP.X + 0.5f);
					int colorY = static_cast<int>(colorP.Y + 0.5f);
					if ((colorX >= 0 && colorX < 1920) && (colorY >= 0 && colorY < 1080))
					{
						cloud->points[i].b = i_rgb.data[(colorY * 1920 + colorX) * 4];
						cloud->points[i].g = i_rgb.data[(colorY * 1920 + colorX) * 4 + 1];
						cloud->points[i].r = i_rgb.data[(colorY * 1920 + colorX) * 4 + 2];
					}
				}*/
 
				//------写入XYZ------
				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;
 
				}
			}
		}
		//-----------------------提取范围内的点------------------------
		pcl::ConditionAnd<MyPointDataType>::Ptr range_cond(new pcl::ConditionAnd<MyPointDataType>());
		range_cond->addComparison(pcl::FieldComparison<MyPointDataType>::ConstPtr(new	pcl::FieldComparison<MyPointDataType>("z", pcl::ComparisonOps::GT, 0.001)));
		range_cond->addComparison(pcl::FieldComparison<MyPointDataType>::ConstPtr(new	pcl::FieldComparison<MyPointDataType>("z", pcl::ComparisonOps::LT, 2.0)));
		range_cond->addComparison(pcl::FieldComparison<MyPointDataType>::ConstPtr(new	pcl::FieldComparison<MyPointDataType>("x", pcl::ComparisonOps::GT, -0.5)));
		range_cond->addComparison(pcl::FieldComparison<MyPointDataType>::ConstPtr(new	pcl::FieldComparison<MyPointDataType>("x", pcl::ComparisonOps::LT, 0.5)));
		//range_cond->addComparison(pcl::FieldComparison<MyPointDataType>::ConstPtr(new	pcl::FieldComparison<MyPointDataType>("y", pcl::ComparisonOps::GT, -0.85)));
		pcl::ConditionalRemoval<MyPointDataType> condrem(range_cond, false);
		condrem.setInputCloud(cloud);
		condrem.setKeepOrganized(false);
		condrem.filter(*cloud_filtered);
		//--------------------------------------------------------------
 
		//-----------------------去除离群点------------------------
		//pcl::RadiusOutlierRemoval<MyPointDataType> outrem;
		//outrem.setInputCloud(cloud_filtered);
		//outrem.setRadiusSearch(0.03);
		//outrem.setMinNeighborsInRadius(15);
		//outrem.filter(*cloud_filtered);
 
		//pcl::StatisticalOutlierRemoval<MyPointDataType> sor;
		//sor.setInputCloud(cloud_filtered);
		//sor.setMeanK(10);
		//sor.setStddevMulThresh(1.0);
		//sor.filter(*cloud_filtered);
		//--------------------------------------------------------------
 
		string s = "cow";
		s += num2str(count);
		s += ".pcd";
		pcl::io::savePCDFile(s, *cloud_filtered,false); //将点云保存到PCD文件中
		std::cerr << "Saved " << cloud_filtered->points.size() << " data points." << std::endl;
		s.clear();
 
		//Beep(1046, 1000);
 
		// 显示结果图
		boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer(new pcl::visualization::PCLVisualizer("3D viewer"));
		viewer->addPointCloud(cloud_filtered);
		viewer->resetCamera();
 
		viewer->addCoordinateSystem(0.1);
		viewer->initCameraParameters();
		while (!viewer->wasStopped()){
			viewer->spinOnce();
		}
 
		count++;
		cout << "test" << endl;
 
		// 释放资源
		SafeRelease(m_pDepthFrame);
		SafeRelease(m_pDepthFrameReference);
		SafeRelease(m_pColorFrame);
		SafeRelease(m_pColorFrameReference);
		SafeRelease(m_pMultiFrame);
	}
	
 
	// 关闭窗口，设备
	m_pKinectSensor->Close();
	SafeRelease(m_pKinectSensor);
	std::system("pause");
	return 0;
}

————————————————

版权声明：本文为CSDN博主「悄然的我-粤Y」的原创文章，遵循CC 4.0 BY-SA版权协议，转载请附上原文出处链接及本声明。
原文链接：https://blog.csdn.net/u010848251/article/details/70992345