Kinectで取得した点群をPCLで描画する

KinectやPCLの導入・設定方法に関しては他の解説記事をご覧ください！

ソースコード

main.cpp

// Cのmin,maxマクロを無効にする
#define NOMINMAX
// 安全でないメソッドの呼び出しでの警告を無効にする
#define _SCL_SECURE_NO_WARNINGS
#define _CRT_SECURE_NO_WARNINGS

#include <iostream>
#include <Windows.h>
#include <pcl/visualization/cloud_viewer.h>
#include <pcl/features/integral_image_normal.h>
#include "kinect2_grabber.h"

// 点群の型を定義しておく
typedef pcl::PointXYZ PointType;

// プロトタイプ宣言
void estimateNormal( pcl::PointCloud<PointType>::Ptr cloud, pcl::PointCloud<pcl::Normal>::Ptr &cloud_normals );

void main()
{
	try{
		// ビューア
		pcl::visualization::PCLVisualizer viewer( "Point Cloud Viewer" );

		// 点群
		pcl::PointCloud<PointType>::Ptr cloud( new pcl::PointCloud<PointType> );

		// 点群の排他処理
		boost::mutex mutex;

		// 法線
		pcl::PointCloud<pcl::Normal>::Ptr cloud_normals( new pcl::PointCloud < pcl::Normal > );

		// データの更新ごとに呼ばれる関数（オブジェクト）
		boost::function<void( const pcl::PointCloud<PointType>::ConstPtr& )> function
			= [&cloud, &mutex]( const pcl::PointCloud<PointType>::ConstPtr &new_cloud ){
			boost::mutex::scoped_lock lock( mutex );
			pcl::copyPointCloud( *new_cloud, *cloud );
		};

		// Kinect2Grabberを開始する
		pcl::Kinect2Grabber grabber;
		grabber.registerCallback( function );
		grabber.start();

		// ビューアーが終了されるまで動作する
		while ( !viewer.wasStopped() ){
			// 表示を更新する
			viewer.spinOnce();

			// 点群がある場合
			boost::mutex::scoped_try_lock lock( mutex );
			if ( ( cloud->size() != 0 ) && lock.owns_lock() ){
				// 点群を更新する
				auto ret = viewer.updatePointCloud( cloud, "cloud" );
				if ( !ret ){
					// 更新がエラーになった場合は、未作成なので新しい点群として追加する
					viewer.addPointCloud( cloud, "cloud" );
				}

				// 法線を推定する
				estimateNormal( cloud, cloud_normals );
				// 法線を更新する
				viewer.removePointCloud( "normals" );
				viewer.addPointCloudNormals<PointType, pcl::Normal>( cloud, cloud_normals, 100, 0.05, "normals" );
			}

			// エスケープキーが押されたら終了する
			if ( GetKeyState( VK_ESCAPE ) < 0 ){
				break;
			}
		}
	}

	catch ( std::exception& ex ){
		std::cout << ex.what() << std::endl;
	}
}

// 法線を推定する
void estimateNormal( pcl::PointCloud<PointType>::Ptr cloud, pcl::PointCloud<pcl::Normal>::Ptr &cloud_normals )
{
	// 法線推定クラス
	pcl::IntegralImageNormalEstimation<PointType, pcl::Normal>  ne;

	ne.setNormalEstimationMethod( ne.AVERAGE_DEPTH_CHANGE );
	ne.setMaxDepthChangeFactor( 0.01 );
	ne.setNormalSmoothingSize( 5.0 );
	ne.setInputCloud( cloud );
	ne.compute( *cloud_normals );
}

kinect2_grabber.hは以下のリンク先から頂きました。
https://github.com/UnaNancyOwen/KinectGrabber

実行結果例がこちら

にょろにょろ出ている線が推定された法線です。