Edited at

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

More than 1 year has passed since last update.

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

実行結果例がこちら

2017-01-09 (1).png

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