PCL（Point Cloud Library）

第13回 名古屋CV・PRML勉強会

野田 雅文@miyabiarts

発表の流れ

• 紹介編

• 導入編

• 基礎編

• 応用編

2

紹介編

3

PCLの紹介

• Point Cloud Library – 点群（ポントクラウド）処理のためのラ

ブラリ

• PCL開発チーム

– スポンサー：Willow Garage, NVidia, Google,

Toyota, Trimble, Urban Robotics.

4

姉妹プロジェクト

• OpenCV

5

ポントクラウド（点群）データ

• 3次元の点の集合

– 深度（距離）センサから取得

– （ステレオカメラ）

6

どんなことができるか

• filters • features • keypoints • registration • kdtree • octree • segmentation • sample_consensus • surface • range_data • io • visualization

7

どんなことができるか

8

フゖルタリング

• 点群の削減

• 外れ値・無効値の除去

• 値の平滑化

9

削減前 削減後

特徴量

• 3次元の特徴量計算

– 任意の点周辺の点群から算出

10

特徴点

• 対応付けなどに利用するユニークな点の検出

– SIFT、NARF

11

レジストレーション

• 複数の点群間の位置合わせ

– ICPゕルゴリズム

12

kd木

• 空間構造

• 最近傍探索

13

八分木

• 空間構造（グリッド構造）

• 点群データの圧縮などに利用

14

セグメンテーション

• モデルフゖッテゖング

– モデル：平面、シリンダーなど

• 領域分割

15

サンプリングコンセンサス

• モデルフゖッテゖングなどにおけるサンプリング

– RANSACなど

16

直線フゖッテゖング

球面フゖッテゖング

表面データ

• 点群データからの面の再構成

17

レンジデータ

• 点群データからレンジデータを復元

18

入出力

• Kinect/Xtionからの入力

• フゔル入出力

– PCD（点群）、OBJ・PLY（メッシュ）

19

可視化

• VTK（Visual Toolkit）

– 点群・レンジ・メッシュデータ可視化

– GUI表示

20

応用ゕプリケーション

• 3次元再構成

• SLAM

• ロボットの自律制御

21

導入編

22

PCLの仕様

• プログラミング言語：C++

• 対応OS：Windows、MacOS、Linux

• ラセンス：BSD

• 公式サト：

– http://pointclouds.org/

23

ドキュメント&チュートリゕル

• http://pointclouds.org/documentation/

24

その他の情報

• ICCV2011・IROS2011 チュートリゕル

25

ダウンロード

• http://pointclouds.org/download

26

導入

• 今回は64bit 、Visual Studio 2010を対象

• 最新版：PCL 1.4.0（2012/1/1）

• ンストーラで関連するラブラリを一括でンストール可能

– OpenNIだけは別途ンストール

• デフォルトのンストールデゖレクトリ

– C:¥Program files¥PCL1.4.0

27

関連するラブラリ

• Boost：C++準標準ラブラリ

– スマートポンタ、関数オブジェクト、スレッド、シグナルなど

• VTK：GUIツールキット

• Eigen：行列ラブラリ

• FLANN：最近傍探索ラブラリ

• qHull：凸包、三角面化、ドロネー分割

• OpenNI：Kinect

28

ンクルードパス

• C:¥Program files¥PCL1.4.0¥include¥pcl-1.4

• C:¥Program files¥PCL1.4.0¥3rdParty¥Boost¥include

• C:¥Program files¥PCL1.4.0¥3rdParty¥Eigen¥include

• C:¥Program files¥PCL1.4.0¥3rdParty¥flann¥include

• C:¥Program files¥PCL1.4.0¥3rdParty¥qhull¥include

• C:¥Program files¥PCL1.4.0¥3rdParty¥VTK 5.8.0¥include¥vtk-5.8

• C:¥Program files¥OpenNI¥include

29

ラブラリパス

• C:¥Program files¥PCL1.4.0¥lib

• C:¥Program files¥PCL1.4.0¥3rdParty¥Boost¥lib

• C:¥Program files¥PCL1.4.0¥3rdParty¥flann¥lib

• C:¥Program files¥PCL1.4.0¥3rdParty¥qhull¥lib

• C:¥Program files¥PCL1.4.0¥3rdParty¥VTK 5.8.0¥lib¥vtk-5.8

• C:¥Program files¥OpenNI¥lib

30

リンクするラブラリ（Release版）

31

openNI64.lib OpenNI.jni64.lib NiSampleModule64.lib NiSampleExtensionModule64.lib

pcl_common.lib pcl_features.lib pcl_filters.lib pcl_io.lib pcl_kdtree.lib pcl_keypoints.lib pcl_octree.lib pcl_range_image.lib pcl_range_image_border_extractor.lib pcl_registration.lib pcl_sample_consensus.lib pcl_segmentation.lib pcl_surface.lib pcl_visualization.lib

vtkIO.lib vtkjpeg.lib vtklibxml2.lib vtkmetaio.lib vtkNetCDF.lib vtkNetCDF_cxx.lib vtkpng.lib vtkproj4.lib vtkRendering.lib vtksqlite.lib vtksys.lib vtktiff.lib vtkverdict.lib vtkViews.lib vtkVolumeRendering.lib vtkWidgets.lib vtkzlib.lib opengl32.lib

MapReduceMPI.lib mpistubs.lib vtkalglib.lib vtkCharts.lib vtkCommon.lib vtkDICOMParser.lib vtkexoIIc.lib vtkexpat.lib vtkFiltering.lib vtkfreetype.lib vtkftgl.lib vtkGenericFiltering.lib vtkGeovis.lib vtkGraphics.lib vtkHybrid.lib vtkImaging.lib vtkInfovis.lib

flann.lib flann_s.lib flann_cpp_s.lib qhull6.lib qhullcpp.lib qhullstatic.lib qhullstatic_p.lib

リンクするラブラリ

• Debug版では「ラブラリ名-gd.lib」

– PCL関連のラブラリのみ

• OpenNIは32bit版だとラブラリ名が変わるので注意

32

基礎編

33

サンプル

• 点群をフゔルから読み込んで画面に表示

34

サンプルコード

35

int main() { pcl::PointCloud<pcl::PointXYZ>::Ptr data( new pcl::PointCloud<pcl::PointXYZ>() ); pcl::io::loadPCDFile( “filename.pcd", *data ); pcl::visualization::CloudViewer *viewer = new pcl::visualization::CloudViewer( “Sample" ); viewer->showCloud( cloud_filtered ); while( !viewer->wasStopped() ) { boost::this_thread::sleep (boost::posix_time::microseconds (100000)); } return 0; }

点群データ

36

pcl::PointXYZ ：位置 pcl::PointXYZI ：位置+輝度 pcl::PointXYZRGB ：位置+輝度（RGB） pcl::PointNormal ：位置+法線 ・・・

pcl::PointCloud< PointT >::Ptr cloud( new PointCloud< PointT >() );

点群データクラス

PointT：点の型

スマートポンタ

スマートポンタ

• pcl::PointCloud< PointT >::Ptr

• pcl::PointCloud< PointT >::ConstPtr – 中身はboost::shared_ptr

• 通常のポンタ

– newしたら必ずdeleteする必要がある

• スマートポンタ

– deleteをスマートポンタが担当

37

フゔル入力

• フゔルフォーマット：.pcd

• 戻り値：-1で読み込み失敗

38

pcl::io::loadPCDFile( “filename.pcd", *cloud );

フゔル出力

39

pcl::io::savePCDFile( “filename.pcd", *cloud );

点群データの表示

• pcl::visualization::CloudViewerを利用

40

pcl::visualization::CloudViewer viewer( "Sample" ); viewer.showCloud( data ); while( !viewer.wasStopped() ) { boost::this_thread::sleep (boost::posix_time::microseconds (100000)); }

Viewer

41

Viewer

• pcl::visualization名前空間内に存在

– CloudViewer：点群データのみを表示

– PCLVisualizer：点群・法線・メッシュデータなどを表示

• 操作方法自体はどれも同じ

42

Viewerの操作方法

• 左ドラッグ：視点の回転

• Shift+左ドラッグ：視点の平行移動

• Ctrl+左ドラッグ：画面上の回転

• 右ドラッグ：ズーム

• g：メジャーの表示

• j：スクリーンショットの保存

43

Viewerの機能拡張

• コールバック関数を登録することで機能を拡張できる

– registerKeyboardCallback

– registerMouseCallback

– registerMousePickCallback

44

pcl::visualization::PCLVisualizer

45

pcl::visualization::PCLVisualizer *viewer = new pcl::visualization::PCLVisualizer(); viewer->setBackgroundColor (0, 0, 0); viewer->addCoordinateSystem (0.1); viewer->initCameraParameters (); viewer->addPointCloud( cloud ); while( !viewer->wasStopped() ) { viewer->spinOnce(); boost::this_thread::sleep (boost::posix_time::microseconds( 100000 ) ); }

点群の作成

• テスト用などに点群を作成したい場合

46

pcl::PointCloud< PointT >::Ptr cloud( new PointCloud< PointT >() ); for( int i = 0 ; i < 10000 ; ++i ) { // PointT = PointXYZの場合

PointXYZ point; point.x = rand(); point.y = rand(); point.z = rand(); cloud->points.push_back( point ); } cloud->width = cloud->points.size(); cloud->height = 1;

boost::random

• C標準のrand関数は線形合同法

– 高次元で乱数に周期が発生

• 任意の乱数生成器、分布を設定可能

47

boost::mt19937 gen( static_cast<unsigned long>(time(0)) ); boost::uniform_real<> dst( -0.1, 0.1 ); boost::variate_generator< boost::mt19937&, boost::uniform_real<> > rand( gen, dst ); // 乱数生成

rand()

Kinectからの入力

• 内部でOpenNIを利用

48

pcl::Grabber* interface = new pcl::OpenNIGrabber(); boost::function<void (const pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr&)> func = boost::bind ( capture, _1); interface->registerCallback ( func ); interface->start (); while (!viewer.wasStopped()) { sleep (1); } interface->stop ();

void capture(const pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr &cloud ) { viewer.showCloud( cloud ); }

コールバック関数

無効値の除去

• Kinectからの入力には、NaNが含まれる

– NaNが存在すると、レジストレーションなどの結果がおかしくなる

49

pcl::PassThrough<pcl::PointXYZRGB> pass; pass.setInputCloud( input ); pass.filter( output );

pass.setFilterLimits( 0.0, 1.0 )とすることで、特定の範囲に制限することも可能

応用編

50

応用編の流れ

• 2つの点群間を位置合わせし、面を再構成

– ICCV2011 チュートリゕルに基づく

– 画像処理で言うと、モザキング

51

統合

処理の流れ

52

平面検出と除去

クラスタリング

特徴点検出

特徴量記述

初期対応付け

対応付けの高精度化

点群の統合

メッシュ化

複数の点群データ

メッシュデータ

対象物体の抽出

平面の検出と除去

• 平面推定x2

• 平面に属する点群の除去

53

平面フゖッテゖング

• 点群を平面にフゖッテゖング

– 平面のパラメータの推定

– 平面に所属する点群の推定

54

平面フゖッテゖング

55

pcl::SACSegmentation< pcl::PointXYZ > seg; seg.setInputCloud ( data ); seg.setOptimizeCoefficients ( true ); seg.setModelType ( pcl::SACMODEL_PLANE ); seg.setMethodType ( pcl::SAC_RANSAC ); seg.setDistanceThreshold ( 0.02 ); pcl::ModelCoefficients::Ptr coefficients ( new pcl::ModelCoefficients() ); pcl::PointIndices::Ptr inliers ( new pcl::PointIndices() ); seg.segment ( *inliers, *coefficients );

点群の抽出（除去）

56

pcl::ExtractIndices< pcl::PointXYZRGB > extract; extract.setInputCloud( cloud ); extract.setIndices( inliers ); extract.setNegative( true ); extract.filter( *segmented );

これを2回繰り返す

クラスタリングによる対象領域の抽出

• 大きな点群クラスタのみを対象領域として残し、ノズを除去する

57

クラスタリング

58

pcl::PointCloud< pcl::PointXYZRGB >::Ptr tmp( new pcl::PointCloud< pcl::PointXYZRGB >() ); pcl::PointCloud< pcl::PointXYZRGB >::Ptr segmented( new pcl::PointCloud< pcl::PointXYZRGB >() ); removePlanar( cloud, tmp ); removePlanar( tmp, segmented ); pcl::search::KdTree<pcl::PointXYZRGB>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZRGB>); tree->setInputCloud (segmented); std::vector< pcl::PointIndices > cluster_indices; pcl::EuclideanClusterExtraction<pcl::PointXYZRGB> clustering; clustering.setClusterTolerance( 0.03 ); clustering.setMinClusterSize( 1000 ); clustering.setMaxClusterSize( 300000 ); clustering.setSearchMethod( tree ); clustering.setInputCloud( segmented ); clustering.extract( cluster_indices );

対象領域の抽出

59

pcl::ExtractIndices< pcl::PointXYZRGB > extract; extract.setInputCloud( segmented ); pcl::IndicesPtr indices (new std::vector<int>); *indices = cluster_indices[0].indices; extract.setIndices( indices ); extract.setNegative( false ); extract.filter( *result );

レジストレーションの流れ

60

特徴点検出

特徴量記述

初期対応付け

対応付けの高精度化

特徴点検出

• 点群から特徴的な点（Keypoint）を検出

61

特徴点検出

• いくつかの特徴点検出手法が存在

– SIFT

– Harris（Harris, Tomasi, Noble, Lowe）

– Curvature（曲率）

62

特徴点検出

• SIFT特徴点検出

63

pcl::SIFTKeypoint<pcl::PointXYZRGB, pcl::PointXYZI> sift3D; sift3D.setScales( 0.01, 3, 2 ); sift3D.setMinimumContrast( 0.0 ); pcl::PointCloud<pcl::PointXYZI>::Ptr keypoints( new pcl::PointCloud<pcl::PointXYZI>() ); sift3D.setInputCloud( cloud ); sift3D.compute(*keypoints);

特徴量抽出

• いくつかの特徴量記述手法が存在

– PFH（Point Feature Histograms）

– PFH（RGB）

– FPFH（faster approximation of）

– SHOT（RGB）

64

特徴量抽出

• キーポント周辺からFPFHを算出

• FPFHを算出するためには、点の法線が必要

– 法線の算出法は、後のスラドで説明

65

pcl::Feature<pcl::PointXYZRGB, pcl::FPFHSignature33> feature_extractor ; feature_extractor.setSearchMethod( pcl::search::Search<pcl::PointXYZRGB>::Ptr( new pcl::search::KdTree<pcl::PointXYZRGB>() ) ); feature_extractor.setRadiusSearch( 0.05 ); pcl::PointCloud<pcl::PointXYZRGB>::Ptr kpts(new pcl::PointCloud<pcl::PointXYZRGB>); kpts.points.resize(keypoints->points.size()); pcl::copyPointCloud(*keypoints, *kpts); feature_from_normals->setInputNormals( normals ); pcl::PointCloud<pcl::FPFHSignature33>::Ptr features( new pcl::PointCloud<pcl::FPFHSignature33>() ); feature_extractor->compute (*features);

初期対応付け

• 点群1 -> 点群2の対応付けを算出

• 点群2 -> 点群1の対応付けを算出

• 以上2つ対応付けのうち、共通する対応のみを取得

66

初期対応付け

• 点群1から点群2への対応を計算

– 反対も計算

67

correspondences.resize(source->size() ); pcl::KdTreeFLANN<FeatureType> descriptor_kdtree; descriptor_kdtree.setInputCloud ( cloud2 ); const int k = 1; std::vector<int> k_indices (k); std::vector<float> k_squared_distances (k); for (size_t i = 0; i < source->size (); ++i) { descriptor_kdtree.nearestKSearch (*cloud1, i, k, k_indices, k_squared_distances); correspondences[i] = k_indices[0]; }

初期対応付け

• 両方向に共通する対応のみを残す

– RANSACを行うことにより、一部の対応を除去

68

std::vector<std::pair<unsigned, unsigned> > c; for (unsigned cIdx = 0; cIdx < c1.size (); ++cIdx) { if ( c2[c1[cIdx]] == cIdx) { c.push_back( std::make_pair( cIdx, c1[cIdx] ) ); } } correspondences->resize( c.size()); for (unsigned cIdx = 0; cIdx < c.size(); ++cIdx) { (*correspondences)[cIdx].index_query = c[cIdx].first; (*correspondences)[cIdx].index_match = c[cIdx].second; } pcl::registration::CorrespondenceRejectorSampleConsensus<pcl::PointXYZI> rejector; rejector.setInputCloud( keypoints1 ); rejector.setTargetCloud( keypoints2 ); rejector.setInputCorrespondences( correspondences ); rejector.getCorrespondences( *correspondences );

初期対応付け

• 2つの点群間の幾何関係を計算

69

pcl::registration::TransformationEstimation<pcl::PointXYZI, pcl::PointXYZI>::Ptr transformation_estimation(new pcl::registration::TransformationEstimationSVD<pcl::PointXYZI, pcl::PointXYZI>); Eigen::Matrix4f initial_transformation_matrix_; transformation_estimation->estimateRigidTransformation( *keypoints1, *keypoints2, *correspondences, initial_transformation_matrix_); pcl::transformPointCloud( *cloud1, *transformed1, initial_transformation_matrix_);

対応付けの高精度化

• ICP（Iterative Closest Point）ゕルゴリズム

– 距離が最小となる点同士を対応付ける

– 点群間の距離の和を最小化

70

対応付けの高精度化

71

pcl::IterativeClosestPoint<pcl::PointXYZRGB, pcl::PointXYZRGB> icp; icp.setInputCloud( cloud1 ); icp.setInputTarget( cloud2 ); icp.setTransformationEpsilon( 1e-6 ); icp.setMaxCorrespondenceDistance( 5.0 ); icp.setMaximumIterations( 200 ); icp.setEuclideanFitnessEpsilon( 1.0 ); icp.setRANSACOutlierRejectionThreshold( 1.0 ); pcl::PointCloud<pcl::PointXYZRGB> final; icp.align(final);

点群の統合

• 2つの点群をマージ

72

*merged = *cloud1 + *cloud2

ここでは、

*merged = *cloud1_final + *cloud2

点群間の重なりがあるので除去する必要がある

点群の削減（ダウンサンプリング）

• 不必要な点（同じ座標の点）の除去

• 後の処理の計算コストの削減

• 点群の分布の均一化

73

削減前 削減後

点群の削減（ダウンサンプリング）

• VoxelGridによる削減手法

– 点間の距離が一定以上になるように削減

74

pcl::VoxelGrid< pcl::PointXYZ > vg; vg.setInputCloud( cloud ); vg.setLeafSize( 0.01f, 0.01f, 0.01f ); vg.setDownsampleAllData(true); vg.filter( *cloud_filtered );

他に、pcl::ApproximateVoxelGridがある

メッシュ化

75

メッシュ化の流れ

76

点の法線を計算

メッシュ化

法線計算

• 任意の点中心から一定の範囲の点群を用いて法線方向を算出

77

pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> ne; ne.setInputCloud (data); pcl::search::KdTree<pcl::PointXYZ >::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ > ()); ne.setSearchMethod (tree); pcl::PointCloud<pcl::Normal>::Ptr normals (new pcl::PointCloud<pcl::Normal>); ne.setRadiusSearch (1.0); ne.compute( *normals );

法線の表示

• pcl::visualization::PCLVisualizerを利用

78

pcl::visualization::PCLVisualizer> viewer ("3D Viewer"); viewer.setBackgroundColor (0, 0, 0); viewer.addPointCloud<pcl::PointXY> (cloud, "sample cloud"); viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3, "sample cloud"); viewer.addPointCloudNormals<pcl::PointXYZ, pcl::Normal> (cloud, normals, 10, 0.05, "normals"); viewer.addCoordinateSystem (1.0); viewer.initCameraParameters ();

法線付き点群の計算

• 点群（位置）と点群（法線）をまとめて、一つの点群（位置+法線）に変換

79

pcl::PointCloud<pcl::PointNormal>::Ptr cloud_with_normals (new pcl::PointCloud<pcl::PointNormal>); pcl::concatenateFields (*cloud, *normals, *cloud_with_normals);

位置

位置

位置

位置

法線

法線

法線

法線

+

位置+法線

位置+法線

位置+法線

位置+法線

メッシュ化

• メッシュ化手法がいくつか存在

– pcl::GreedyProjectionTriangulation

– pcl::MarchingCubesGreedy

– ・・・

80

メッシュ化

• pcl::GreedyProjectionTriangulationで再構成

81

pcl::GreedyProjectionTriangulation<pcl::PointNormal> gp3; gp3.setSearchRadius (0.01); gp3.setMu (2.5); gp3.setMaximumNearestNeighbors (100); gp3.setMaximumSurfaceAngle(M_PI/4); gp3.setMinimumAngle(M_PI/18); gp3.setMaximumAngle(2*M_PI/3); gp3.setNormalConsistency(false); gp3.setInputCloud (cloud_with_normals); gp3.setSearchMethod (tree2); pcl::PolygonMesh triangles; gp3.reconstruct (triangles);

面の表示

• pcl::visualization::PCLVisualizerを利用

82

pcl::visualization::PCLVisualizer *viewer = new pcl::visualization::PCLVisualizer; viewer->setBackgroundColor (0, 0, 0); viewer->addCoordinateSystem (0.1); viewer->initCameraParameters (); viewer->addPolygonMesh( surface_ ); while( !viewer->wasStopped() ) { viewer->spinOnce(); boost::this_thread::sleep (boost::posix_time::microseconds (100000)); }

構築結果

• パラメータ調整がマチ？

83