PCL+kinect2.0实现点云数据的获取及储存

说明

最近也是开始了PCL的学习之路,希望在与大家交流的过程中可以从小白一步步成长。这篇代码也是受一位学长和网上一些相关文章的启发写出来的,下面我会给出源码和一些容易掉进去的坑,希望有志同道合的朋友可以多多指正。那位学长也是很厉害的,大家可以去他的主页看看有没有自己一直想解决却解决不了的问题的方法。他的主页

点云显示+储存 代码:

#include"kinect2_grabber.h"
//---------------------------------
#include 
#include
#include 
#include  
#include 
typedef pcl::PointXYZRGBA PointType;

int main()
{
	int c = 0;
	int total = 0;//文件名
	//pcl::PointCloud cloud_filtered;// 按范围过滤后的点云

	boost::shared_ptr m_viewer;
	
	//显示窗口初始化
	m_viewer.reset(new pcl::visualization::PCLVisualizer("viewer", false));
	m_viewer->setBackgroundColor(0, 0, 0);//设置背景颜色
	m_viewer->initCameraParameters();
	//m_viewer->addCoordinateSystem();//添加红绿蓝坐标轴
	m_viewer->createInteractor();

	                                                                                                                                                                        
	// 获取Kinect设备	
	// Point Cloud
	pcl::PointCloud::ConstPtr pointCloud_XYZRGBA;  //指针变量
															  
	// Retrieved Point Cloud Callback Function 检测到的点云回调函数
	boost::mutex mutex; //并发编程互斥锁
	boost::function::ConstPtr&)> function = [&pointCloud_XYZRGBA, &mutex](const pcl::PointCloud::ConstPtr& ptr) { //只能用ConstPtr,不能用Ptr
		boost::mutex::scoped_lock lock(mutex);
		/* Point Cloud Processing */
		pointCloud_XYZRGBA = ptr->makeShared();
		//ptr->makeShared() = NULL;
	};

	boost::shared_ptr grabberForKinect = boost::shared_ptr(new pcl::Kinect2Grabber);// Kinect2Grabber
	boost::signals2::connection connection = grabberForKinect->registerCallback(function); // Register Callback Function
	grabberForKinect->start();  //Start Grabber

	while (!m_viewer->wasStopped()) {
		// Update Viewer

		c++;

		m_viewer->spinOnce(1); //调用交互程序并更新屏幕一次。
		boost::mutex::scoped_try_lock lock(mutex);

		//cloud_filtered = filterByScope(pointCloud_XYZRGBA, Z_nearby, Z_faraway, X_left, X_right, Y_bottom, Y_top);//截取对应范围图像

		if (lock.owns_lock() && pointCloud_XYZRGBA) {
			//cloud_filtered = filterByScope(pointCloud_XYZRGBA, Z_nearby, Z_faraway, X_left, X_right, Y_bottom, Y_top);
			//5 显示 Point Cloud
			if (!m_viewer->updatePointCloud(pointCloud_XYZRGBA->makeShared(), "cloud")) {//pointCloud_XYZRGBA是指针类型,不能用( . )运算符
				m_viewer->addPointCloud(pointCloud_XYZRGBA->makeShared(), "cloud");
			}
			if (c % 10 == 0) {//每隔一段时间保存一次图片
				//存储图片。
				char name[100];
				sprintf(name, "D:\\%d.pcd", total++);
				pcl::io::savePCDFileASCII(name, *pointCloud_XYZRGBA);//以.pcd的格式保存点云数据到磁盘
			}
		}
	}//while


}

关于代码,其中已经给注释了,还有几个比较容易出问题的地方。

一、

PCL+kinect2.0实现点云数据的获取及储存_第1张图片
这里阴影部分的内容,一定要是 ConstPtr (参照第一篇源码) 否则就会出现下列的错误

PCL+kinect2.0实现点云数据的获取及储存_第2张图片

二、

PCL+kinect2.0实现点云数据的获取及储存_第3张图片
调试——>项目属性——>VC++ 目录——>包含目录 ,里不要出现这样一项,我就是因为配置的问题,搞了好几天,脑袋都大了,最后是这里有问题。如果带上这一项的话,会出现无法解析的外部符号。

另外可能有的朋友没有kinect2_grabber.h这个头文件的源码,下面给出:

#pragma once
#pragma once
// Kinect2Grabber is pcl::Grabber to retrieve the point cloud data from Kinect v2 using Kinect for Windows SDK 2.x.
// This source code is licensed under the MIT license. Please see the License in License.txt.

#ifndef KINECT2_GRABBER
#define KINECT2_GRABBER

#define NOMINMAX
#include 
#include 

#include 
#include 
#include 
#include 


namespace pcl
{
	struct pcl::PointXYZ;
	struct pcl::PointXYZI;
	struct pcl::PointXYZRGB;
	struct pcl::PointXYZRGBA;
	template  class pcl::PointCloud;

	template
	inline void SafeRelease(Interface *& IRelease)
	{
		if (IRelease != NULL) {
			IRelease->Release();
			IRelease = NULL;
		}
	}

	class Kinect2Grabber : public pcl::Grabber
	{
	public:
		Kinect2Grabber();
		virtual ~Kinect2Grabber() throw ();
		virtual void start();
		virtual void stop();
		virtual bool isRunning() const;
		virtual std::string getName() const;
		virtual float getFramesPerSecond() const;

		typedef void (signal_Kinect2_PointXYZ)(const boost::shared_ptr>&);
		typedef void (signal_Kinect2_PointXYZI)(const boost::shared_ptr>&);
		typedef void (signal_Kinect2_PointXYZRGB)(const boost::shared_ptr>&);
		typedef void (signal_Kinect2_PointXYZRGBA)(const boost::shared_ptr>&);

	protected:
		boost::signals2::signal* signal_PointXYZ;
		boost::signals2::signal* signal_PointXYZI;
		boost::signals2::signal* signal_PointXYZRGB;
		boost::signals2::signal* signal_PointXYZRGBA;

		pcl::PointCloud::Ptr convertDepthToPointXYZ(UINT16* depthBuffer);
		pcl::PointCloud::Ptr convertInfraredDepthToPointXYZI(UINT16* infraredBuffer, UINT16* depthBuffer);
		pcl::PointCloud::Ptr convertRGBDepthToPointXYZRGB(RGBQUAD* colorBuffer, UINT16* depthBuffer);
		pcl::PointCloud::Ptr convertRGBADepthToPointXYZRGBA(RGBQUAD* colorBuffer, UINT16* depthBuffer);

		boost::thread thread;
		mutable boost::mutex mutex;

		void threadFunction();

		bool quit;
		bool running;

		HRESULT result;
		IKinectSensor* sensor;
		ICoordinateMapper* mapper;
		IColorFrameSource* colorSource;
		IColorFrameReader* colorReader;
		IDepthFrameSource* depthSource;
		IDepthFrameReader* depthReader;
		IInfraredFrameSource* infraredSource;
		IInfraredFrameReader* infraredReader;

		int colorWidth;
		int colorHeight;
		std::vector colorBuffer;

		int depthWidth;
		int depthHeight;
		std::vector depthBuffer;

		int infraredWidth;
		int infraredHeight;
		std::vector infraredBuffer;
	};
	pcl::Kinect2Grabber::Kinect2Grabber()
		: sensor(nullptr)
		, mapper(nullptr)
		, colorSource(nullptr)
		, colorReader(nullptr)
		, depthSource(nullptr)
		, depthReader(nullptr)
		, infraredSource(nullptr)
		, infraredReader(nullptr)
		, result(S_OK)
		, colorWidth(1920)
		, colorHeight(1080)
		, colorBuffer()
		, depthWidth(512)
		, depthHeight(424)
		, depthBuffer()
		, infraredWidth(512)
		, infraredHeight(424)
		, infraredBuffer()
		, running(false)
		, quit(false)
		, signal_PointXYZ(nullptr)
		, signal_PointXYZI(nullptr)
		, signal_PointXYZRGB(nullptr)
		, signal_PointXYZRGBA(nullptr)
	{
		// Create Sensor Instance
		
		result = GetDefaultKinectSensor(&sensor);
		if (FAILED(result)) {
			throw std::exception("Exception : GetDefaultKinectSensor()");
		}

		// Open Sensor
		result = sensor->Open();
		if (FAILED(result)) {
			throw std::exception("Exception : IKinectSensor::Open()");
		}

		// Retrieved Coordinate Mapper
		result = sensor->get_CoordinateMapper(&mapper);
		if (FAILED(result)) {
			throw std::exception("Exception : IKinectSensor::get_CoordinateMapper()");
		}

		// Retrieved Color Frame Source
		result = sensor->get_ColorFrameSource(&colorSource);
		if (FAILED(result)) {
			throw std::exception("Exception : IKinectSensor::get_ColorFrameSource()");
		}

		// Retrieved Depth Frame Source
		result = sensor->get_DepthFrameSource(&depthSource);
		if (FAILED(result)) {
			throw std::exception("Exception : IKinectSensor::get_DepthFrameSource()");
		}

		// Retrieved Infrared Frame Source
		result = sensor->get_InfraredFrameSource(&infraredSource);
		if (FAILED(result)) {
			throw std::exception("Exception : IKinectSensor::get_InfraredFrameSource()");
		}

		// Retrieved Color Frame Size
		IFrameDescription* colorDescription;
		result = colorSource->get_FrameDescription(&colorDescription);
		if (FAILED(result)) {
			throw std::exception("Exception : IColorFrameSource::get_FrameDescription()");
		}

		result = colorDescription->get_Width(&colorWidth); // 1920
		if (FAILED(result)) {
			throw std::exception("Exception : IFrameDescription::get_Width()");
		}

		result = colorDescription->get_Height(&colorHeight); // 1080
		if (FAILED(result)) {
			throw std::exception("Exception : IFrameDescription::get_Height()");
		}

		SafeRelease(colorDescription);

		// To Reserve Color Frame Buffer
		colorBuffer.resize(colorWidth * colorHeight);

		// Retrieved Depth Frame Size
		IFrameDescription* depthDescription;
		result = depthSource->get_FrameDescription(&depthDescription);
		if (FAILED(result)) {
			throw std::exception("Exception : IDepthFrameSource::get_FrameDescription()");
		}

		result = depthDescription->get_Width(&depthWidth); // 512
		if (FAILED(result)) {
			throw std::exception("Exception : IFrameDescription::get_Width()");
		}

		result = depthDescription->get_Height(&depthHeight); // 424
		if (FAILED(result)) {
			throw std::exception("Exception : IFrameDescription::get_Height()");
		}

		SafeRelease(depthDescription);

		// To Reserve Depth Frame Buffer
		depthBuffer.resize(depthWidth * depthHeight);

		// Retrieved Infrared Frame Size
		IFrameDescription* infraredDescription;
		result = infraredSource->get_FrameDescription(&infraredDescription);
		if (FAILED(result)) {
			throw std::exception("Exception : IInfraredFrameSource::get_FrameDescription()");
		}

		result = infraredDescription->get_Width(&infraredWidth); // 512
		if (FAILED(result)) {
			throw std::exception("Exception : IFrameDescription::get_Width()");
		}

		result = infraredDescription->get_Height(&infraredHeight); // 424
		if (FAILED(result)) {
			throw std::exception("Exception : IFrameDescription::get_Height()");
		}

		SafeRelease(infraredDescription);

		// To Reserve Infrared Frame Buffer
		infraredBuffer.resize(infraredWidth * infraredHeight);

		signal_PointXYZ = createSignal();
		signal_PointXYZI = createSignal();
		signal_PointXYZRGB = createSignal();
		signal_PointXYZRGBA = createSignal();
	}

	pcl::Kinect2Grabber::~Kinect2Grabber() throw()
	{
		stop();

		disconnect_all_slots();
		disconnect_all_slots();
		disconnect_all_slots();
		disconnect_all_slots();

		thread.join();

		// End Processing
		if (sensor) {
			sensor->Close();
		}
		SafeRelease(sensor);
		SafeRelease(mapper);
		SafeRelease(colorSource);
		SafeRelease(colorReader);
		SafeRelease(depthSource);
		SafeRelease(depthReader);
		SafeRelease(infraredSource);
		SafeRelease(infraredReader);
	}

	void pcl::Kinect2Grabber::start()
	{
		// Open Color Frame Reader
		result = colorSource->OpenReader(&colorReader);
		if (FAILED(result)) {
			throw std::exception("Exception : IColorFrameSource::OpenReader()");
		}

		// Open Depth Frame Reader
		result = depthSource->OpenReader(&depthReader);
		if (FAILED(result)) {
			throw std::exception("Exception : IDepthFrameSource::OpenReader()");
		}

		// Open Infrared Frame Reader
		result = infraredSource->OpenReader(&infraredReader);
		if (FAILED(result)) {
			throw std::exception("Exception : IInfraredFrameSource::OpenReader()");
		}

		running = true;

		thread = boost::thread(&Kinect2Grabber::threadFunction, this);
	}

	void pcl::Kinect2Grabber::stop()
	{
		boost::unique_lock lock(mutex);

		quit = true;
		running = false;

		lock.unlock();
	}

	bool pcl::Kinect2Grabber::isRunning() const
	{
		boost::unique_lock lock(mutex);

		return running;

		lock.unlock();
	}

	std::string pcl::Kinect2Grabber::getName() const
	{
		return std::string("Kinect2Grabber");
	}

	float pcl::Kinect2Grabber::getFramesPerSecond() const
	{
		return 30.0f;
	}

	void pcl::Kinect2Grabber::threadFunction()
	{
		while (!quit) {
			boost::unique_lock lock(mutex);

			// Acquire Latest Color Frame
			IColorFrame* colorFrame = nullptr;
			result = colorReader->AcquireLatestFrame(&colorFrame);
			if (SUCCEEDED(result)) {
				// Retrieved Color Data
				result = colorFrame->CopyConvertedFrameDataToArray(colorBuffer.size() * sizeof(RGBQUAD), reinterpret_cast(&colorBuffer[0]), ColorImageFormat::ColorImageFormat_Bgra);
				if (FAILED(result)) {
					throw std::exception("Exception : IColorFrame::CopyConvertedFrameDataToArray()");
				}
			}
			SafeRelease(colorFrame);

			// Acquire Latest Depth Frame
			IDepthFrame* depthFrame = nullptr;
			result = depthReader->AcquireLatestFrame(&depthFrame);
			if (SUCCEEDED(result)) {
				// Retrieved Depth Data
				result = depthFrame->CopyFrameDataToArray(depthBuffer.size(), &depthBuffer[0]);
				if (FAILED(result)) {
					throw std::exception("Exception : IDepthFrame::CopyFrameDataToArray()");
				}
			}
			SafeRelease(depthFrame);

			// Acquire Latest Infrared Frame
			IInfraredFrame* infraredFrame = nullptr;
			result = infraredReader->AcquireLatestFrame(&infraredFrame);
			if (SUCCEEDED(result)) {
				// Retrieved Infrared Data
				result = infraredFrame->CopyFrameDataToArray(infraredBuffer.size(), &infraredBuffer[0]);
				if (FAILED(result)) {
					throw std::exception("Exception : IInfraredFrame::CopyFrameDataToArray()");
				}
			}
			SafeRelease(infraredFrame);

			lock.unlock();

			if (signal_PointXYZ->num_slots() > 0) {
				signal_PointXYZ->operator()(convertDepthToPointXYZ(&depthBuffer[0]));
			}

			if (signal_PointXYZI->num_slots() > 0) {
				signal_PointXYZI->operator()(convertInfraredDepthToPointXYZI(&infraredBuffer[0], &depthBuffer[0]));
			}

			if (signal_PointXYZRGB->num_slots() > 0) {
				signal_PointXYZRGB->operator()(convertRGBDepthToPointXYZRGB(&colorBuffer[0], &depthBuffer[0]));
			}

			if (signal_PointXYZRGBA->num_slots() > 0) {
				signal_PointXYZRGBA->operator()(convertRGBADepthToPointXYZRGBA(&colorBuffer[0], &depthBuffer[0]));
			}
		}
	}

	pcl::PointCloud::Ptr pcl::Kinect2Grabber::convertDepthToPointXYZ(UINT16* depthBuffer)
	{
		pcl::PointCloud::Ptr cloud(new pcl::PointCloud());

		cloud->width = static_cast(depthWidth);
		cloud->height = static_cast(depthHeight);
		cloud->is_dense = false;

		cloud->points.resize(cloud->height * cloud->width);

		pcl::PointXYZ* pt = &cloud->points[0];
		for (int y = 0; y < depthHeight; y++) {
			for (int x = 0; x < depthWidth; x++, pt++) {
				pcl::PointXYZ point;

				DepthSpacePoint depthSpacePoint = { static_cast(x), static_cast(y) };
				UINT16 depth = depthBuffer[y * depthWidth + x];

				// Coordinate Mapping Depth to Camera Space, and Setting PointCloud XYZ
				CameraSpacePoint cameraSpacePoint = { 0.0f, 0.0f, 0.0f };
				mapper->MapDepthPointToCameraSpace(depthSpacePoint, depth, &cameraSpacePoint);
				point.x = cameraSpacePoint.X;
				point.y = cameraSpacePoint.Y;
				point.z = cameraSpacePoint.Z;

				*pt = point;
			}
		}

		return cloud;
	}

	pcl::PointCloud::Ptr pcl::Kinect2Grabber::convertInfraredDepthToPointXYZI(UINT16* infraredBuffer, UINT16* depthBuffer)
	{
		pcl::PointCloud::Ptr cloud(new pcl::PointCloud());

		cloud->width = static_cast(depthWidth);
		cloud->height = static_cast(depthHeight);
		cloud->is_dense = false;

		cloud->points.resize(cloud->height * cloud->width);

		pcl::PointXYZI* pt = &cloud->points[0];
		for (int y = 0; y < depthHeight; y++) {
			for (int x = 0; x < depthWidth; x++, pt++) {
				pcl::PointXYZI point;

				DepthSpacePoint depthSpacePoint = { static_cast(x), static_cast(y) };
				UINT16 depth = depthBuffer[y * depthWidth + x];

				// Setting PointCloud Intensity
				point.intensity = static_cast(infraredBuffer[y * depthWidth + x]);

				// Coordinate Mapping Depth to Camera Space, and Setting PointCloud XYZ
				CameraSpacePoint cameraSpacePoint = { 0.0f, 0.0f, 0.0f };
				mapper->MapDepthPointToCameraSpace(depthSpacePoint, depth, &cameraSpacePoint);
				point.x = cameraSpacePoint.X;
				point.y = cameraSpacePoint.Y;
				point.z = cameraSpacePoint.Z;

				*pt = point;
			}
		}

		return cloud;
	}

	pcl::PointCloud::Ptr pcl::Kinect2Grabber::convertRGBDepthToPointXYZRGB(RGBQUAD* colorBuffer, UINT16* depthBuffer)
	{
		pcl::PointCloud::Ptr cloud(new pcl::PointCloud());

		cloud->width = static_cast(depthWidth);
		cloud->height = static_cast(depthHeight);
		cloud->is_dense = false;

		cloud->points.resize(cloud->height * cloud->width);

		pcl::PointXYZRGB* pt = &cloud->points[0];
		for (int y = 0; y < depthHeight; y++) {
			for (int x = 0; x < depthWidth; x++, pt++) {
				pcl::PointXYZRGB point;

				DepthSpacePoint depthSpacePoint = { static_cast(x), static_cast(y) };
				UINT16 depth = depthBuffer[y * depthWidth + x];

				// Coordinate Mapping Depth to Color Space, and Setting PointCloud RGB
				ColorSpacePoint colorSpacePoint = { 0.0f, 0.0f };
				mapper->MapDepthPointToColorSpace(depthSpacePoint, depth, &colorSpacePoint);
				int colorX = static_cast(std::floor(colorSpacePoint.X + 0.5f));
				int colorY = static_cast(std::floor(colorSpacePoint.Y + 0.5f));
				if ((0 <= colorX) && (colorX < colorWidth) && (0 <= colorY) && (colorY < colorHeight)) {
					RGBQUAD color = colorBuffer[colorY * colorWidth + colorX];
					point.b = color.rgbBlue;
					point.g = color.rgbGreen;
					point.r = color.rgbRed;
				}

				// Coordinate Mapping Depth to Camera Space, and Setting PointCloud XYZ
				CameraSpacePoint cameraSpacePoint = { 0.0f, 0.0f, 0.0f };
				mapper->MapDepthPointToCameraSpace(depthSpacePoint, depth, &cameraSpacePoint);
				if ((0 <= colorX) && (colorX < colorWidth) && (0 <= colorY) && (colorY < colorHeight)) {
					point.x = cameraSpacePoint.X;
					point.y = cameraSpacePoint.Y;
					point.z = cameraSpacePoint.Z;
				}

				*pt = point;
			}
		}

		return cloud;
	}

	pcl::PointCloud::Ptr pcl::Kinect2Grabber::convertRGBADepthToPointXYZRGBA(RGBQUAD* colorBuffer, UINT16* depthBuffer)
	{
		pcl::PointCloud::Ptr cloud(new pcl::PointCloud());

		cloud->width = static_cast(depthWidth);
		cloud->height = static_cast(depthHeight);
		cloud->is_dense = false;

		cloud->points.resize(cloud->height * cloud->width);

		pcl::PointXYZRGBA* pt = &cloud->points[0];
		for (int y = 0; y < depthHeight; y++) {
			for (int x = 0; x < depthWidth; x++, pt++) {
				pcl::PointXYZRGBA point;

				DepthSpacePoint depthSpacePoint = { static_cast(x), static_cast(y) };
				UINT16 depth = depthBuffer[y * depthWidth + x];

				// Coordinate Mapping Depth to Color Space, and Setting PointCloud RGBA
				ColorSpacePoint colorSpacePoint = { 0.0f, 0.0f };
				mapper->MapDepthPointToColorSpace(depthSpacePoint, depth, &colorSpacePoint);
				int colorX = static_cast(std::floor(colorSpacePoint.X + 0.5f));
				int colorY = static_cast(std::floor(colorSpacePoint.Y + 0.5f));
				if ((0 <= colorX) && (colorX < colorWidth) && (0 <= colorY) && (colorY < colorHeight)) {
					RGBQUAD color = colorBuffer[colorY * colorWidth + colorX];
					point.b = color.rgbBlue;
					point.g = color.rgbGreen;
					point.r = color.rgbRed;
					point.a = color.rgbReserved;
				}

				// Coordinate Mapping Depth to Camera Space, and Setting PointCloud XYZ
				CameraSpacePoint cameraSpacePoint = { 0.0f, 0.0f, 0.0f };
				mapper->MapDepthPointToCameraSpace(depthSpacePoint, depth, &cameraSpacePoint);
				if ((0 <= colorX) && (colorX < colorWidth) && (0 <= colorY) && (colorY < colorHeight)) {
					point.x = cameraSpacePoint.X;
					point.y = cameraSpacePoint.Y;
					point.z = cameraSpacePoint.Z;
				}

				*pt = point;
			}
		}

		return cloud;
	}
}

#endif KINECT2_GRABBER


最后希望大家有问题多多留言,我们共同进步。
转载注明出处,方便有问题的朋友交流,谢谢配合!

你可能感兴趣的:(PCL学习)