c_cpp 使用PCLVisualizer和Kinect v2 Grabber绘制点云

Posted 2021-05-14

// Disable Error C4996 that occur when using Boost.Signals2.
#ifdef _DEBUG
	#define _SCL_SECURE_NO_WARNINGS
#endif

#include "kinect2_grabber.h"
#include <pcl/visualization/pcl_visualizer.h>

typedef pcl::PointXYZRGBA PointType;

void keyboard_callback( const pcl::visualization::KeyboardEvent& event, void* )
{
	if( event.getKeyCode() && event.keyDown() ){
		std::cout << "Key : " << event.getKeyCode() << std::endl;
	}
}

void mouse_callback( const pcl::visualization::MouseEvent& event, void* )
{
	if( event.getType() == pcl::visualization::MouseEvent::MouseButtonPress && event.getButton() == pcl::visualization::MouseEvent::LeftButton ){
		std::cout << "Mouse : " << event.getX() << ", " << event.getY() << std::endl;
	}
}

int _tmain( int argc, _TCHAR* argv[] )
{
	boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer( new pcl::visualization::PCLVisualizer( "Point Cloud Viewer" ) );
	viewer->registerKeyboardCallback( &keyboard_callback, "keyboard" );
	viewer->registerMouseCallback( &mouse_callback, "mouse" );
	
	pcl::PointCloud<PointType>::ConstPtr cloud;
	
	boost::mutex mutex;
	boost::function<void( const pcl::PointCloud<PointType>::ConstPtr& )> function =
		[&cloud, &mutex]( const pcl::PointCloud<PointType>::ConstPtr &ptr ){
			boost::mutex::scoped_lock lock( mutex );
			cloud = ptr->makeShared();
		};

	boost::shared_ptr<pcl::Grabber> grabber = boost::make_shared<pcl::Kinect2Grabber>();

	boost::signals2::connection connection = grabber->registerCallback( function );

	grabber->start();

	while( !viewer->wasStopped() ){
		viewer->spinOnce();

		boost::mutex::scoped_try_lock lock( mutex );
		if( lock.owns_lock() && cloud ){
			if( !viewer->updatePointCloud( cloud, "cloud" ) ){
				viewer->addPointCloud( cloud, "cloud" );
			}
		}
	}

	grabber->stop();

	if( connection.connected() ){
		connection.disconnect();
	}

	return 0;
}

// Disable Error C4996 that occur when using Boost.Signals2.
#ifdef _DEBUG
	#define _SCL_SECURE_NO_WARNINGS
#endif

#include "kinect2_grabber.h"
#include <pcl/visualization/pcl_visualizer.h>

template <typename PointType>
class Viewer
{
	typedef pcl::PointCloud<PointType> PointCloud;
	typedef typename PointCloud::ConstPtr ConstPtr;

	public:
		Viewer( pcl::Grabber& grabber )
			: viewer( new pcl::visualization::PCLVisualizer( "Point Cloud Viewer" ) )
			, grabber( grabber )
		{
		}

		void run()
		{
			viewer->registerMouseCallback( &Viewer::mouse_callback, *this );
			viewer->registerKeyboardCallback( &Viewer::keyboard_callback, *this );
			boost::function<void( const ConstPtr& ) > callback = boost::bind( &Viewer::cloud_callback, this, _1 );
			boost::signals2::connection connection = grabber.registerCallback( callback );

			grabber.start();

			while( !viewer->wasStopped() ){
				viewer->spinOnce();

				ConstPtr cloud;

				if( mutex.try_lock() ){
					buffer.swap( cloud );
					mutex.unlock();
				}

				if( cloud ){
					if( !viewer->updatePointCloud( cloud, "Cloud" ) ){
						viewer->addPointCloud( cloud, "Cloud" );
					}
				}
			}

			grabber.stop();

			if( connection.connected() ){
				connection.disconnect();
			}
		}

	private:
		void cloud_callback( const ConstPtr& cloud )
		{
			boost::mutex::scoped_lock lock( mutex );
			buffer = cloud->makeShared();
		}

		void keyboard_callback( const pcl::visualization::KeyboardEvent& event, void* )
		{
			if( event.getKeyCode() && event.keyDown() ){
				std::cout << "Key : " << event.getKeyCode() << std::endl;
			}
		}

		void mouse_callback( const pcl::visualization::MouseEvent& event, void* )
		{
			if( event.getType() == pcl::visualization::MouseEvent::MouseButtonPress && event.getButton() == pcl::visualization::MouseEvent::LeftButton ){
				std::cout << "Mouse : " << event.getX() << ", " << event.getY() << std::endl;
			}
		}

		boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer;
		pcl::Grabber& grabber;
		boost::mutex mutex;
		ConstPtr buffer;
};

int _tmain( int argc, _TCHAR* argv[] )
{
	boost::shared_ptr<pcl::Grabber> grabber = boost::make_shared<pcl::Kinect2Grabber>();
	Viewer<pcl::PointXYZRGB> viewer( *grabber );
	viewer.run();

	return 0;
}