点群データからHarris特徴量を抽出する

VFH、SIFTに続いて今回はHarris特徴量を点群から抽出する。Harris特徴量（特徴点とも）は簡単に言えば入力されたデータの角を検出する特徴量である。、このHarris特徴量もSIFT同様PCLでは機能が実装されているので簡単に使うことができる。

コード

#include "stdafx.h"

#include <iostream>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/common/io.h>
#include <pcl/keypoints/harris_keypoint3D.h>
#include <pcl/visualization/pcl_visualizer.h>


int _tmain(int argc, _TCHAR* argv[])

{
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
    pcl::io::loadPCDFile<pcl::PointXYZ> (argv[1], *cloud);

    pcl::HarrisKeypoint3D<pcl::PointXYZ,pcl::PointXYZI> detector;
    detector.setNonMaxSupression (true);
    detector.setRadius (0.01);
    //detector.setRadiusSearch (100);
    detector.setInputCloud(cloud);

    pcl::PointCloud<pcl::PointXYZI>::Ptr keypoints(new pcl::PointCloud<pcl::PointXYZI>());
    detector.compute(*keypoints);

    std::cout << "keypoints detected: " << keypoints->size() << std::endl;

    pcl::PointCloud<pcl::PointXYZ>::Ptr keypoints3D(new pcl::PointCloud<pcl::PointXYZ>());
    pcl::PointXYZ tmp;
    double max = 0,min=0;

    for(pcl::PointCloud<pcl::PointXYZI>::iterator i = keypoints->begin(); i!= keypoints->end(); i++){
        tmp = pcl::PointXYZ((*i).x,(*i).y,(*i).z);
        if ((*i).intensity>max ){
            std::cout << (*i) << " coords: " << (*i).x << ";" << (*i).y << ";" << (*i).z << std::endl;
            max = (*i).intensity;
        }
        if ((*i).intensity<min){
            min = (*i).intensity;
        }
        keypoints3D->push_back(tmp);
    }

    std::cout << "maximal responce: "<< max << " min responce:  "<< min<<std::endl;

    //show point cloud
    pcl::visualization::PCLVisualizer viewer ("3D Viewer");
    pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> pccolor(cloud, 255, 255, 255);
    pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> kpcolor(keypoints3D, 255, 0, 0);
    viewer.addPointCloud(cloud,pccolor,"testimg.png");
    viewer.addPointCloud(keypoints3D,kpcolor,"keypoints.png");
    viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 7, "keypoints.png");
    while (!viewer.wasStopped ())
    {
        viewer.spinOnce();
        pcl_sleep (0.01);
    }
    return (0); 

}


実行結果

入力点群

出力された点群＋特徴点

　画像では少々わかりにくいが、いわゆる角のような場所に点が加わっているのがわかると思う。

　一ヶ月ほど更新を休む必要ができたため次回の更新は未定とする（もともと不定期な更新であるが）

点群データからSIFT特徴量を抽出する

　以前記事にしたのは３次元点群のために作られた特徴量VFH（Viewpoint Feature Histogram）だったが、今回はSIFT（Scale-Invariant Feature Transform）特徴量の取得を行う。このSIFT特徴量の計算を行う機能もVFH同様PCLで実装されている。

コード


#include "stdafx.h"

#include <iostream>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/common/io.h>
#include <pcl/keypoints/sift_keypoint.h>
#include <pcl/features/normal_3d.h>
#include <pcl/visualization/pcl_visualizer.h>

pcl::PointCloud<pcl::PointNormal>::Ptr Surface_normals(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud)
{
    pcl::NormalEstimation<pcl::PointXYZ, pcl::PointNormal> ne;
    ne.setInputCloud (cloud);//法線の計算を行いたい点群を指定する

    pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ> ());//KDTREEを作る
    ne.setSearchMethod (tree);//検索方法にKDTREEを指定する

    pcl::PointCloud<pcl::PointNormal>::Ptr cloud_normals (new pcl::PointCloud<pcl::PointNormal>);//法線情報を入れる変数

    ne.setRadiusSearch (0.5);//検索する半径を指定する

    ne.compute (*cloud_normals);//法線情報の出力先を指定する

    return cloud_normals;
}


pcl::PointCloud<pcl::PointWithScale> Extract_SIFT(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, pcl::PointCloud<pcl::PointNormal>::Ptr cloud_normals)
{   
    // SIFT特徴量計算のためのパラメータ
    const float min_scale = 0.01f;
    const int n_octaves = 3;
    const int n_scales_per_octave = 4;
    const float min_contrast = 0.001f;
    pcl::SIFTKeypoint<pcl::PointNormal, pcl::PointWithScale> sift;
    pcl::PointCloud<pcl::PointWithScale> result;
    pcl::search::KdTree<pcl::PointNormal>::Ptr tree(new pcl::search::KdTree<pcl::PointNormal> ());
    sift.setSearchMethod(tree);
    sift.setScales(min_scale, n_octaves, n_scales_per_octave);
    sift.setMinimumContrast(0.00);
    sift.setInputCloud(cloud_normals);
    sift.compute(result);
    std::cout << "No of SIFT points in the result are " << result.points.size () << std::endl;

    return result;
}


int _tmain(int argc, _TCHAR* argv[])

{
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
    pcl::io::loadPCDFile<pcl::PointXYZ> (argv[1], *cloud);

    pcl::PointCloud<pcl::PointNormal>::Ptr cloud_normals (new pcl::PointCloud<pcl::PointNormal>);
    cloud_normals = Surface_normals(cloud);

    //　XYZの情報をcloudからSurface_normals(cloud)にXYZとして加える
    for(size_t i = 0; i < cloud_normals->points.size(); ++i)
    {
        cloud_normals->points[i].x = cloud->points[i].x;
        cloud_normals->points[i].y = cloud->points[i].y;
        cloud_normals->points[i].z = cloud->points[i].z;
    }

    // 視覚化のためSIFT計算の結果をcloud_tempにコピー
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_temp (new pcl::PointCloud<pcl::PointXYZ>);
    copyPointCloud(Extract_SIFT(cloud, cloud_normals), *cloud_temp);
    std::cout << "SIFT points in the cloud_temp are " << cloud_temp->points.size () << std::endl;
   
    // 入力点群と計算された特徴点を表示
    pcl::visualization::PCLVisualizer viewer("PCL Viewer");
    pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> keypoints_color_handler (cloud_temp, 0, 255, 0);
    pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> cloud_color_handler (cloud, 255, 0, 0);
    viewer.setBackgroundColor( 0.0, 0.0, 0.0 );
    viewer.addPointCloud(cloud, cloud_color_handler, "cloud");
    viewer.addPointCloud(cloud_temp, keypoints_color_handler, "keypoints");
    viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 7, "keypoints");
    while(!viewer.wasStopped ())
    {
        viewer.spinOnce ();
    }
    return (0); 
}

実行結果

 

　赤の点が入力した点群、緑の点がSIFT特徴量として計算された点である。本来ならば色の情報も使うべきであるが現在色情報のついたデータがないため後日実行することにする。

入力された点群の各座標の最大値と最小値を求める

　題意の通り、極めて簡単なものなので必要がないと言われればその通りかもしれないが、なにかの機会に使用することがあるかもしれない。

コード

#include "stdafx.h"

#include <iostream>
#include <pcl/io/io.h>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <stdlib.h>

class Size
{
public:
    double min_x;
    double min_y;
    double min_z;
    double max_x;
    double max_y;
    double max_z;
};

Size Min_Max(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud)
{
    Size size;
    size.max_x = 0;
    size.max_y = 0;
    size.max_z = 0;
    size.min_x = 0;
    size.min_y = 0;
    size.min_z = 0;

    for(size_t i = 0; i < cloud->points.size(); i++){
        if(size.max_z > cloud->points[i].z)
            size.max_z = size.max_z;
        else
            size.max_z = cloud->points[i].z;

        if(size.max_y > cloud->points[i].y)
            size.max_y = size.max_y;
        else
            size.max_y = cloud->points[i].y;

        if(size.max_x > cloud->points[i].x)
            size.max_x = size.max_x;
        else
            size.max_x = cloud->points[i].x;

        if(size.min_z < cloud->points[i].z)
            size.min_z = size.min_z;
        else
            size.min_z = cloud->points[i].z;

        if(size.min_y < cloud->points[i].y)
            size.min_y = size.min_y;
        else
            size.min_y = cloud->points[i].y;

        if(size.min_x < cloud->points[i].x)
            size.min_x = size.min_x;
        else
            size.min_x = cloud->points[i].x;
    }
    return size;
}


int _tmain(int argc, _TCHAR* argv[])
{
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::io::loadPCDFile(argv[1], *cloud);

    std::cout << Min_Max(cloud).max_x << std::endl;
    std::cout << Min_Max(cloud).max_y << std::endl;
    std::cout << Min_Max(cloud).max_z << std::endl;
    std::cout << Min_Max(cloud).min_x << std::endl;
    std::cout << Min_Max(cloud).min_y << std::endl;
    std::cout << Min_Max(cloud).min_z << std::endl;

    return (0); 

}

平面の検出

　センサーから取得される点群データには、特に設定をしなければ壁などの構造物のデータも当然含まれる。こういったデータは目的によっては使われることがないので除去されるのが望ましい場合があったり、むしろ壁情報の方が重要である場合も当然あるので、いずれにせよ検知する必要がある。

コード
#include "stdafx.h"

#include <iostream>
#include <pcl/ModelCoefficients.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/filters/extract_indices.h>


pcl::PointCloud<pcl::PointXYZ>::Ptr Detect_wall(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, bool TF)
{
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_output (new pcl::PointCloud<pcl::PointXYZ>);

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

    pcl::SACSegmentation<pcl::PointXYZ> seg;

    seg.setOptimizeCoefficients (true);
    // Mandatory
    seg.setModelType (pcl::SACMODEL_PLANE);//検出するモデルのタイプを指定
    seg.setMethodType (pcl::SAC_RANSAC);//検出に使用する方法を指定
    seg.setDistanceThreshold (0.01);//

    seg.setInputCloud (cloud);
    seg.segment (*inliers, *coefficients);

    pcl::ExtractIndices<pcl::PointXYZ> extract;
    extract.setInputCloud(cloud);
    extract.setIndices(inliers);
    extract.setNegative(TF);//Trueの場合出力は検出された壁以外のデータ falseの場合は壁のデータ
    extract.filter(*cloud_output);
   
    return cloud_output;//出力
}


int _tmain(int argc, _TCHAR* argv[])
{
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);   
    pcl::io::loadPCDFile(argv[1], *cloud);

    std::stringstream Filename;
    std::string name = argv[1];
    name.erase(name.length() - 4);
    Filename << name << "_wall.pcd";
    pcl::io::savePCDFileBinary(Filename.str(), *Detect_wall(cloud, false));

    Filename.str("");
    Filename << name << "_remaining.pcd";
    pcl::io::savePCDFileBinary(Filename.str(), *Detect_wall(cloud, true));

    return (0);
}


実行結果

入力データ

出力データ

　無人空間などでSLAMを行う際にはこの平面の検出が重要になってくることがある。今回は壁の検出に関連する機能以外は使用していないが、処理の高速化、軽量化のためにダウンサンプリングやスムージングを行うのも良いかもしれない。実行結果のみからではOctreeを使用したものと変化が無いように感じられるが、このプログラムの場合入力するファイルはひとつで良い。

点群データからメッシュモデルを生成する

　点群データはあくまでも点の集まりにすぎないので、今回は点のそれぞれを結びメッシュデータを作成する。


コード
#include "stdafx.h"

#include <pcl/point_types.h>
#include <pcl/io/pcd_io.h>
#include <pcl/kdtree/kdtree_flann.h>
#include <pcl/features/normal_3d.h>
#include <pcl/surface/gp3.h>
#include <pcl/io/vtk_io.h>

pcl::PointCloud<pcl::Normal>::Ptr surface_normals(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud)
{
    pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> ne;
    ne.setInputCloud (cloud);//法線の計算を行いたい点群を指定する

    pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ> ());//KDTREEを作る
    ne.setSearchMethod (tree);//検索方法にKDTREEを指定する

    pcl::PointCloud<pcl::Normal>::Ptr cloud_normals (new pcl::PointCloud<pcl::Normal>);//法線情報を入れる変数

    ne.setRadiusSearch (0.005);//検索する半径を指定する

    ne.compute (*cloud_normals);//法線情報の出力先を指定する

    return cloud_normals;
}

pcl::PolygonMesh Generate_mesh(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud)
{
    pcl::PointCloud<pcl::Normal>::Ptr cloud_normals (new pcl::PointCloud<pcl::Normal> ());
    cloud_normals = surface_normals(cloud);

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

    pcl::search::KdTree<pcl::PointNormal>::Ptr tree2 (new pcl::search::KdTree<pcl::PointNormal>);
    tree2->setInputCloud (cloud_with_normals);

    pcl::GreedyProjectionTriangulation<pcl::PointNormal> gp3;
    pcl::PolygonMesh triangles;

    gp3.setSearchRadius (0.025);

    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);
    gp3.reconstruct (triangles);

    std::vector<int> parts = gp3.getPartIDs();
    std::vector<int> states = gp3.getPointStates();

    return triangles;
}

int _tmain(int argc, _TCHAR* argv[])
{
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
    pcl::io::loadPCDFile (argv[1], *cloud);

    std::stringstream Filename;
    string name = argv[1];
    name.erase(name.length() - 4);
    Filename << name << "_mesh.vtk";
    pcl::io::saveVTKFile(Filename.str(), Generate_mesh(cloud));

    return (0);
}


実行結果

入力モデル

出力されたメッシュモデル

　　メッシュデータ（VTK）の表示にはParaView（リンク）というソフトを使用した。
　当然このメッシュモデルは、点群の密度と数が多いほど精巧なモデルが得られる。 処理の内容については後に追記する。

PCD Viewerに機能追加

　以前記事に載せたPCDファイルを表示するプログラムはまさにPCDをファイルしか読み込めないので今回はこのプログラムを改良してVFHファイルも読み込んで表示できるようにする。自分が出力ファイルはひと目で分かるように「元となったファイル＋”_***.pcd”」などの形式となっているのでファイル名によって判断を行う。

コード

#include "stdafx.h"

#include <pcl/visualization/cloud_viewer.h>
#include <iostream>
#include <pcl/io/io.h>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <stdlib.h>
#include <pcl/visualization/histogram_visualizer.h>

void
    viewerOneOff (pcl::visualization::PCLVisualizer& viewer)
{  
    viewer.setBackgroundColor (1.0, 0.5, 1.0);
}

int _tmain(int argc, _TCHAR* argv[])
{
    pcl::PointCloud<pcl::PointXYZRGBA> cloud;

    std::string fname = argv[1];
    unsigned int loc = fname.find("_vfh",0);
    if(loc == std::string::npos)
    {
        pcl::visualization::CloudViewer viewer("Cloud Viewer");
        pcl::io::loadPCDFile (argv[1], cloud);
        viewer.runOnVisualizationThreadOnce (viewerOneOff);
        viewer.showCloud(cloud.makeShared());
        while (!viewer.wasStopped ())
        {
        }
    }
    else{
        pcl::PointCloud<pcl::VFHSignature308>::Ptr vfhs (new pcl::PointCloud<pcl::VFHSignature308> ());
        pcl::io::loadPCDFile(argv[1], *vfhs);
        pcl::visualization::PCLHistogramVisualizer hist;
        hist.addFeatureHistogram(*vfhs,263,"id",640,200);
        std::cout << vfhs->points[0].histogram[308];

        while (1)
        {
            hist.spinOnce (100);
        }
    }
    return (0);
}

おそらくファイル内にある「FIELD」の項目によって機能の切り替えを行うべきなのかもしれないが今回はそれを行っていない。

PLYデータを点群データ（PCD）に変換する

　現在３次元のデータには複数のファイルフォーマットが存在するが、それらの多くは点群データに変換可能である。今回は有名なPLYファイルを点群データファイルPCDに変換する。
　短すぎるので関数化はしていない。

コード

#include "stdafx.h"

#include <iostream>
#include <pcl/io/pcd_io.h>
#include <pcl/io/ply_io.h>
#include <pcl/point_types.h>

int _tmain(int argc, _TCHAR* argv[])
{
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);

    pcl::PLYReader reader;
    reader.read<pcl::PointXYZ> (argv[1], *cloud);

    std::stringstream Filename;
    std::string name = argv[1];
    name.erase(name.length() - 4);
    Filename << name << ".pcd";
    std::cout << Filename.str() << std::endl;
    pcl::io::savePCDFileBinary(Filename.str(), *cloud);

    return (0);
}
　PLYファイルを表示するプログラムを誤って消してしまったので今回は実行結果を掲載していないが、このプログラムを使用することによって出力される点群データは表示できているので成功していることにする。　前述したが、極めて短いコードなのでプログラムと呼べるのかは疑問である。

点群データのスムージング

　センサから得られる点群データは、実空間上では平面であってもいざ点群データとして出力された場合は、微妙の誤差があり、まるで凹凸があるかのようになってしまう。勿論これはセンサの原理的に起こりうる事象なので仕方がない。PCLではこの微妙な誤差を修正するためにスムージングを行う機能がある。
　

コード

#include "stdafx.h"

#include <pcl/point_types.h>
#include <pcl/io/pcd_io.h>
#include <pcl/kdtree/kdtree_flann.h>
#include <pcl/surface/mls.h>

pcl::PointCloud<pcl::PointNormal> Smoothing(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud)
{
    pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ>);//Kdtreeの作成

    pcl::PointCloud<pcl::PointNormal> mls_points;//出力する点群の格納場所を作成

    pcl::MovingLeastSquares<pcl::PointXYZ, pcl::PointNormal> mls;

    mls.setComputeNormals (true);//法線の計算を行うかどうか

    // 各パラメーターの設定
    mls.setInputCloud (cloud);
    mls.setPolynomialFit (true);
    mls.setSearchMethod (tree);
    mls.setSearchRadius (0.03);

    mls.process (mls_points);//再構築

    return mls_points;//出力
}


int _tmain(int argc, _TCHAR* argv[])
{
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ> ());
    pcl::io::loadPCDFile (argv[1], *cloud);

    std::stringstream Filename;
    string name = argv[1];
    name.erase(name.length() - 4);
    Filename << name << "_smoothed.pcd";
    std::cout << Filename.str() << std::endl;
    pcl::io::savePCDFile (Filename.str(), Smoothing(cloud));
    return (0);
}

実行結果

処理前

処理後

　このスムージング処理を行った後の点群データを使用すると平面の検出をより精度よく行うことができるが構造が複雑なものに対して行うと、その物体の構造を崩してしまうので必要に応じて点群を切り分けるなどの処理が必要である。

点群データの特徴を抽出する Viewpoint Feature Histogram

　物体検出などを行う際にはかならずと言っていいほどデータから特徴を抽出する。２次元においてはそういった特徴は無数に存在するが、比較的最近研究が行われるようになった３次元の点群においてはまだ数えるほどしかない。（２次元で使用される特徴量を３次元点群からも抽出できるようにしたものもある）PCLではいくつか点群から抽出する特徴量があり、その一つがViewpoint Feature Histogramである。PCLが提供している機能には他にもPFH（Point Feature Histogram）やFPFH（Fast Point Feature Histogram）などもある。それらについてはまた記事にする。

コード

#include "stdafx.h"

#include <pcl/point_types.h>
#include <pcl/features/vfh.h>
#include <pcl/io/pcd_io.h>
#include <pcl/features/normal_3d.h>

pcl::PointCloud<pcl::Normal>::Ptr surface_normals(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud)
{
    pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> ne;
    ne.setInputCloud (cloud);//法線の計算を行いたい点群を指定する

    pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ> ());//KDTREEを作る
    ne.setSearchMethod (tree);//検索方法にKDTREEを指定する

    pcl::PointCloud<pcl::Normal>::Ptr cloud_normals (new pcl::PointCloud<pcl::Normal>);//法線情報を入れる変数

    ne.setRadiusSearch (0.005);//検索する半径を指定する

    ne.compute (*cloud_normals);//法線情報の出力先を指定する

    return cloud_normals;
}


pcl::PointCloud<pcl::VFHSignature308>::Ptr Extract_VFH(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud)
{
    pcl::PointCloud<pcl::Normal>::Ptr cloud_normals (new pcl::PointCloud<pcl::Normal> ());
    pcl::VFHEstimation<pcl::PointXYZ, pcl::Normal, pcl::VFHSignature308> vfh;
    pcl::PointCloud<pcl::VFHSignature308>::Ptr vfhs (new pcl::PointCloud<pcl::VFHSignature308> ());
   
    cloud_normals = surface_normals(cloud);
   
    vfh.setInputCloud (cloud);
    vfh.setInputNormals (cloud_normals);

    pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ> ());
    vfh.setSearchMethod (tree);

   
    vfh.compute (*vfhs);
    return vfhs;
}


int _tmain(int argc, _TCHAR* argv[])
{
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
    pcl::io::loadPCDFile(argv[1], *cloud);
       
    std::stringstream Filename;
    string name = argv[1];
    name.erase(name.length() - 4);
    Filename << name << "_vfh.pcd";
    std::cout << Filename.str() << std::endl;
    pcl::io::savePCDFile(Filename.str(), *Extract_VFH(cloud));

    return (0);
}

 実行結果

ダウンサンプリングを行っていない兎モデル

抽出されたVFH

ダウンサンプリングを行った兎モデル

抽出されたVFH

ダウンサンプリングによって点群の数を約３分の１程度まで減少させた２つのモデルからVFHを抽出しても得られるヒストグラムはまったく一緒である。

点群のクラスタリング

　Kinectから得られる点群データは基本的に背景や、空間内に存在する物体情報が一つのファイルの中に含まれている。物体認識などを点群データを用いて行う場合は、ラスタースキャンなどによって3次元空間中をマッチングしていくことも勿論可能であるが、2次元での探索以上に時間がかかってしまう。そこで入力点群にクラスタリングを行い1つのデータを複数のデータに分割してそれぞれを比較するとにより高速に行う方法がある。

処理の流れ

1. 入力点群PのKd-treeを作る
2. 空のクラスタを作り、検査が必要な点Qを設定する
3. Pに属する全ての点ｐiに対して以下の処理を行う。
1.  ｐiををQに加える
2. Qに属する全ての点ｐiに対して以下の処理を行う
1. 設定半径内に存在するpiの近隣の点ｐnを捜査する
2. 発見された全ての点pnがすでに処理を行われていなければQに加える
3.  Qに保存された全ての点への処理が終了した場合QをCに登録し、Qの点群を空にする
4. 全てのPに属する点pへの処理が完了したら終了とする。Cに登録された各QがPから得られたクラスタである。

コード

#include "stdafx.h"

#include <pcl/ModelCoefficients.h>
#include <pcl/point_types.h>
#include <pcl/io/pcd_io.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/features/normal_3d.h>
#include <pcl/kdtree/kdtree.h>
#include <pcl/sample_consensus/method_types.h>
#include <pcl/sample_consensus/model_types.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/segmentation/extract_clusters.h>
#include <direct.h>

class Size
{
public:
    double length;///x方向
    double width;///y方向
    double height;///z方向
};

Size max_min(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud)//点群の各XYZ方向の最大値と最小値を求める
{
    double min_x, min_y, min_z, max_x, max_y, max_z;
    Size size;
    min_x = 10000;
    min_y = 10000;
    min_z = 100000;
    max_x = -10000;
    max_y = -10000;
    max_z = -10000;

    for(int i = 0; i < cloud->points.size(); i++)
    {
        if(max_x < cloud->points[i].x)
            max_x = cloud->points[i].x;
        if(max_y < cloud->points[i].y)
            max_y = cloud->points[i].y;
        if(max_z < cloud->points[i].z)
            max_z = cloud->points[i].z;

        if(min_x > cloud->points[i].x)
            min_x = cloud->points[i].x;
        if(min_y > cloud->points[i].y)
            min_y = cloud->points[i].y;
        if(min_z > cloud->points[i].z)
            min_z = cloud->points[i].z;
    }

    size.length = abs(max_x) + abs(min_x);
    size.width = abs(max_y) + abs(min_y);
    size.height = abs(max_z) + abs(min_y);
    return size;
}

void Clustering(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud)
{  
    Size size;
    size = max_min(cloud);
    double min = size.height;
    if(min > size.width)
        min = size.width;
    if(min > size.length)
        min = size.length;
    if(min > size.height)
        min = size.height;

    pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ>);
    tree->setInputCloud (cloud);

    std::vector<pcl::PointIndices> cluster_indices;
    pcl::EuclideanClusterExtraction<pcl::PointXYZ> ec;
    ec.setClusterTolerance (min / 10);
    ec.setMinClusterSize (10);//最小のクラスターの値を設定
    ec.setMaxClusterSize (25000);//最大のクラスターの値を設定
    ec.setSearchMethod (tree);//検索に使用する手法を指定
    ec.setInputCloud (cloud);//点群を入力
    ec.extract (cluster_indices);//クラスター情報を出力

    std::string name;
    name = "cluster";

    int j = 0;
    for (std::vector<pcl::PointIndices>::const_iterator it = cluster_indices.begin (); it != cluster_indices.end (); ++it)//クラスターを1塊ごと出力
    {
        pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_cluster (new pcl::PointCloud<pcl::PointXYZ>);
        for (std::vector<int>::const_iterator pit = it->indices.begin (); pit != it->indices.end (); pit++)
            cloud_cluster->points.push_back (cloud->points[*pit]); //*

        cloud_cluster->width = cloud_cluster->points.size ();
        cloud_cluster->height = 1;
        cloud_cluster->is_dense = true;

        std::cout << "PointCloud representing the Cluster: " << cloud_cluster->points.size () << " data points." << std::endl;
        std::stringstream ss;
        ss << name << "_p_" << j << ".pcd";//クラスター毎に名前を変化
        pcl::io::savePCDFileBinary(ss.str(), *cloud_cluster);
        j++;
    }
}


int _tmain(int argc, _TCHAR* argv[])
{
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
    pcl::io::loadPCDFile(argv[1], *cloud);

    Clustering(cloud);

    return (0);

}


実行結果

入力点群

出力されたクラスター１

出力されたクラスター2

出力されたクラスター3

出力されたクラスター4

出力されたクラスタ－5

　この方法の問題点はクラスタリングが正確にできていない場合が多分に発生することである。なので正確な設定が必要とされる。

Outlier（外れ値、異常値）を除去するプログラム

　Kinectなどの3次元点群を取得するセンサーは便利であるが、度々ある点の座標値が以上であったりすることがある。こういった点のことをよくノイズだとかエラーだとか外れ値という。原理的にこれは発生しうる。またこれらを放置すると、法線の正確な計算など後に適用される処理に悪影響を与えるので除去しなければならない。
　除去の原理は簡単に言うと各点からその近隣の全ての点への平均距離を用いる。

コード

#include "stdafx.h"
#include <iostream>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/filters/statistical_outlier_removal.h>
#include <pcl/filters/radius_outlier_removal.h>
#include <pcl/filters/conditional_removal.h>

pcl::PointCloud<pcl::PointXYZ> Remove_outliers(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud)
{
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered (new pcl::PointCloud<pcl::PointXYZ>);
     pcl::StatisticalOutlierRemoval<pcl::PointXYZ> sor;
    sor.setInputCloud (cloud);//外れ値を除去する点群を入力
    sor.setMeanK (50);//MeanKを設定
    sor.setStddevMulThresh (0.1);
    sor.setNegative (false);//外れ値を出力する場合はtrueにする
    sor.filter (*cloud_filtered);//出力
    return *cloud_filtered;
}


int _tmain(int argc, _TCHAR* argv[])
{

    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
   
    pcl::io::loadPCDFile(argv[1], *cloud);

    std::stringstream Filename;
    string name = argv[1];
    name.erase(name.length()-4);
    Filename << name << "_outlier.pcd";
   
   
    pcl::io::savePCDFileBinary (Filename.str(), Remove_outliers(cloud));

    return (0);
}


　手頃なデータがなかったので実行結果は表示できていない。sor.setNegativeをtrueに設定すれば除去される外れ値の点群を出力できる。もちろん2つ点群の格納場所を作れば2つ同時に出力できる。