Create a gist now

Instantly share code, notes, and snippets.

What would you like to do?
Kinectから取得した点群の法線を推定する
// 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 );
}
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment