読者です 読者をやめる 読者になる 読者になる

Natural Software

KinectなどのDepthセンサーを中心に活動しています

PCLで検出した平面でフィルタする

PCL

前回検出した平面を使ってフィルタします。平面を使ったフィルタは2種類あり、「平面を残す」か「平面を除去する」かです。

元の画像(床面検出したもの)

検出された床面のみを残したもの

検出された床面を除去したもの

さて、コードを見てみましょう。大きな流れは、セグメンテーション→フィルターになります。

今回のfilterのために、前回のsegmentate を少し変えました(一部省略しています)。

pcl::PointIndices::Ptr 

segmentate(pcl::PointCloud<pcl::PointXYZRGB>::Ptr& cloud, double threshould)

{

pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients);

pcl::PointIndices::Ptr inliers (new pcl::PointIndices);

// Create the segmentation object

pcl::SACSegmentation<pcl::PointXYZRGB> seg;

...

seg.segment (*inliers, *coefficients);

return inliers;

}

一つ目は、引数を pcl::PointCloud<pcl::PointXYZRGB>::Ptr& にしました。どうも pcl::PointCloud よりも pcl::PointCloud::Ptr のほうが扱いやすい感じなので。

二つ目は、戻り値を pcl::PointIndices::Ptr にしました。このデータは filter の入力にします。

で、filterはというと、こんな感じです。フィルタリングは pcl::ExtractIndices で行います。このクラスにポイントクラウドと、先ほどセグメンテーション結果を渡します。さらに除去する方法を選びフィルタします。フィルタ後のポイントクラウドを新しい pcl::PointCloud::Ptr に入れて返します。

除去する方法は pcl::ExtractIndices::setNegative() で設定します。true にすると inliers が除去され、false にすると inliers 以外が除去されます。

pcl::PointCloud<pcl::PointXYZRGB>::Ptr 

filter( pcl::PointCloud<pcl::PointXYZRGB>::Ptr& cloud,

pcl::PointIndices::Ptr inliers, bool isNegatibe )

{

pcl::PointCloud<pcl::PointXYZRGB>::Ptr result =

pcl::PointCloud<pcl::PointXYZRGB>::Ptr( new pcl::PointCloud<pcl::PointXYZRGB>() );

//フィルタリング

pcl::ExtractIndices<pcl::PointXYZRGB> extract;

extract.setInputCloud( cloud );

extract.setIndices( inliers );

// true にすると平面を除去、false にすると平面以外を除去

extract.setNegative( isNegatibe );

extract.filter( *result );

return result;

}

全体のコード

// thanks http://ros-robot.blogspot.jp/2011/08/pclapi-point-cloud-library-pcl-pcl-api.html

#include <iostream>

#include <pcl/ModelCoefficients.h>

#include <pcl/io/openni_grabber.h>

#include <pcl/io/pcd_io.h>

#include <pcl/point_types.h>

#include <pcl/sample_consensus/method_types.h>

#include <pcl/sample_consensus/model_types.h>

#include <pcl/segmentation/sac_segmentation.h>

#include <pcl/visualization/cloud_viewer.h>

#include <pcl/filters/extract_indices.h>

pcl::PointIndices::Ptr

segmentate(pcl::PointCloud<pcl::PointXYZRGB>::Ptr& cloud, double threshould)

{

pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients);

pcl::PointIndices::Ptr inliers (new pcl::PointIndices);

// Create the segmentation object

pcl::SACSegmentation<pcl::PointXYZRGB> seg;

// Optional

seg.setOptimizeCoefficients (true);

// Mandatory

seg.setModelType (pcl::SACMODEL_PLANE);

seg.setMethodType (pcl::SAC_RANSAC);

seg.setDistanceThreshold (threshould);

seg.setInputCloud (cloud);

seg.segment (*inliers, *coefficients);

for (size_t i = 0; i < inliers->indices.size (); ++i) {

cloud->points[inliers->indices[i]].r = 255;

cloud->points[inliers->indices[i]].g = 0;

cloud->points[inliers->indices[i]].b = 0;

}

return inliers;

}

pcl::PointCloud<pcl::PointXYZRGB>::Ptr

filter( pcl::PointCloud<pcl::PointXYZRGB>::Ptr& cloud,

pcl::PointIndices::Ptr inliers, bool isNegatibe )

{

pcl::PointCloud<pcl::PointXYZRGB>::Ptr result =

pcl::PointCloud<pcl::PointXYZRGB>::Ptr( new pcl::PointCloud<pcl::PointXYZRGB>() );

//フィルタリング

pcl::ExtractIndices<pcl::PointXYZRGB> extract;

extract.setInputCloud( cloud );

extract.setIndices( inliers );

// true にすると平面を除去、false にすると平面以外を除去

extract.setNegative( isNegatibe );

extract.filter( *result );

return result;

}

void main()

{

// viewerを作る

pcl::visualization::CloudViewer viewer( "OpenNI Viewer" );

// データ更新のコールバック関数

boost::function<void (const pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr&)> f =

[&viewer](const pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr &cloud) {

if (!viewer.wasStopped()) {

pcl::PointCloud<pcl::PointXYZRGB>::Ptr copy_cloud(

new pcl::PointCloud<pcl::PointXYZRGB>( *cloud ) );

auto inliers = segmentate( copy_cloud, 0.01 );

//auto filtered = filter( copy_cloud, inliers, true );

auto filtered = filter( copy_cloud, inliers, false );

viewer.showCloud( filtered );

}

};

// OpenNIの入力を作る

pcl::Grabber* interface = new pcl::OpenNIGrabber();

interface->registerCallback( f );

interface->start();

while ( !viewer.wasStopped() ) {

Sleep( 0 );

}

interface->stop();

}

プロパティファイルを含めたプロジェクト全体はこちらに置いてあります。