3次元情報を取得できるセンサの紹介 Leap motion

前回に引き続き3次元情報の取得ができるセンサの紹介を行う

今回は どちらかというと入力装置としての側面が強いがLeap Motionについて
赤外を照射して物体までの距離情報を取得する。10本の指をそれぞれ認識することが可能で、検知範囲は半径50cmほど、きわめて精度の高い情報を取得することが可能

検知範囲が半径50cmと狭いので環境情報を取得するには厳しいセンサである。しかしこの精度はきわめて魅力的である。さらに価格もKinect並みなので手ごろ、人間の入力機器以外に使うことはできないかとアイディアをずっと考えている。

生存報告、加えて3次元情報の取得ができるセンサの紹介 Kinect

かなり久しぶりの更新で申し訳ない

　相変わらず新しい開発環境の構築はできていないのでＰＣＬを用いた点群処理についての更新は行えない。
　なので今回は現在存在する3次元情報を取得できるセンサについて、今後のためにもまとめておこうと思う。
 　対象はあくまでも3次元なので2次元平面を取得するLaser Range Finder（LRF）のようなセンサは対象外とする。（LRFも複数、もしくは使い方によっては3次元情報は取得可能ではある）

　Kinect

  もっとも安価な3次元点群を取得可能なセンサー、取得可能な点の数は307200個、各点の色の情報もそれぞれ取得可能である。安価な故精度が若干他のセンサよりも劣る。また307200個の点を取ることは可能だがその約3割～4割は使用不能であることが多い（反射などの関係）。形はまったく同じだがVersion2が存在しており、Version1よりもより近距離の対象の撮影を行うことができる。3次元点群に触れてみたい方にはもっとも安価で手に入れることができるものであると思う。当然Kinectを用いた研究も多くなされており、SLAM、物体認識、人検知、その他、無数の対象に装備されたりしている。

　今回はこれまで

　近々今一度引っ越すのでそこでは開発環境の再構築を行いたい

点群データから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つ同時に出力できる。

座標軸を回転させる

　点群データは、時に移動させたり回転させたりしなければならないことが多々ある。今回は座標軸を回転させるプログラムを作る。尚PCLでは赤の座標軸がX軸、緑がY軸、青がZ軸を表している

コード

#include "stdafx.h"

#include <iostream>

#include <boost/thread/thread.hpp>
#include <pcl/common/common_headers.h>
#include <pcl/features/normal_3d.h>
#include <pcl/io/pcd_io.h>
#include <stdlib.h>


void centroid(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud)
{
    Eigen::Vector4f xyz_centroid;
    pcl::compute3DCentroid(*cloud, xyz_centroid);//重心を計算

    for(size_t i = 0; i < cloud->points.size(); i++){
        cloud->points[i].x = cloud->points[i].x - xyz_centroid[0];//X座標の移動
        cloud->points[i].y = cloud->points[i].y - xyz_centroid[1];//Y座標の移動
        cloud->points[i].z = cloud->points[i].z - xyz_centroid[2];//Z座標の移動
    }
}

pcl::PointCloud<pcl::PointXYZ> rotation_x(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, double theta)///rotate point cloud by Y axiz
{

    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_centroid (new pcl::PointCloud<pcl::PointXYZ>);
    cloud_centroid = cloud;

    centroid(cloud_centroid);//点群の重心位置求めを（0,0,0）に移動する

    Eigen::Matrix4f rot_x;
    pcl::PointCloud<pcl::PointXYZ> cloud_out;
    cloud_out = *cloud;
    double theta_x = theta * 3.1415926 / 180.0;//角度の変換
    rot_x(0,0) = 1.0;
    rot_x(1,0) = 0.0;
    rot_x(2,0) = 0.0;
    rot_x(3,0) = 0.0;
    rot_x(0,1) = 0.0;
    rot_x(1,1) = cos(theta_x);
    rot_x(2,1) = -sin(theta_x);
    rot_x(3,1) = 0.0;
    rot_x(0,2) = 0.0;
    rot_x(1,2) = sin(theta_x);
    rot_x(2,2) = cos(theta_x);
    rot_x(3,2) = 0.0;
    rot_x(0,3) = 0.0;
    rot_x(1,3) = 0.0;
    rot_x(2,3) = 0.0;
    rot_x(3,3) = 1.0;

    for(size_t i = 0; i < cloud->points.size(); ++i){
        cloud_out.points[i].x = rot_x(0,0) * cloud->points[i].x + rot_x(0,1) * cloud->points[i].y + rot_x(0,2) * cloud->points[i].z + rot_x(0,3) * 1;
        cloud_out.points[i].y = rot_x(1,0) * cloud->points[i].x + rot_x(1,1) * cloud->points[i].y + rot_x(1,2) * cloud->points[i].z + rot_x(1,3) * 1;
        cloud_out.points[i].z = rot_x(2,0) * cloud->points[i].x + rot_x(2,1) * cloud->points[i].y + rot_x(2,2) * cloud->points[i].z + rot_x(2,3) * 1;
    }

    return cloud_out;
}

pcl::PointCloud<pcl::PointXYZ> rotation_y(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, double theta)///rotate point cloud by Y axiz
{

    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_centroid (new pcl::PointCloud<pcl::PointXYZ>);
    cloud_centroid = cloud;

    centroid(cloud_centroid);//点群の重心位置求めを（0,0,0）に移動する

    Eigen::Matrix4f rot_y;
    pcl::PointCloud<pcl::PointXYZ> cloud_out;
    cloud_out = *cloud;
    double theta_y = theta * 3.1415926 / 180.0;//角度の変換
    rot_y(0,0) = cos(theta_y);
    rot_y(1,0) = 0.0;
    rot_y(2,0) = sin(theta_y);
    rot_y(3,0) = 0.0;
    rot_y(0,1) = 0.0;
    rot_y(1,1) = 1.0;
    rot_y(2,1) = 0.0;
    rot_y(3,1) = 0.0;
    rot_y(0,2) = -sin(theta_y);
    rot_y(1,2) = 0.0;
    rot_y(2,2) = cos(theta_y);
    rot_y(3,2) = 0.0;
    rot_y(0,3) = 0.0;
    rot_y(1,3) = 0.0;
    rot_y(2,3) = 0.0;
    rot_y(3,3) = 1.0;

    for(size_t i = 0; i < cloud->points.size(); ++i){
        cloud_out.points[i].x = rot_y(0,0) * cloud->points[i].x + rot_y(0,1) * cloud->points[i].y + rot_y(0,2) * cloud->points[i].z + rot_y(0,3) * 1;
        cloud_out.points[i].y = rot_y(1,0) * cloud->points[i].x + rot_y(1,1) * cloud->points[i].y + rot_y(1,2) * cloud->points[i].z + rot_y(1,3) * 1;
        cloud_out.points[i].z = rot_y(2,0) * cloud->points[i].x + rot_y(2,1) * cloud->points[i].y + rot_y(2,2) * cloud->points[i].z + rot_y(2,3) * 1;
    }

    return cloud_out;
}


pcl::PointCloud<pcl::PointXYZ> rotation_z(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, double theta)///rotate point cloud by Y axiz
{

    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_centroid (new pcl::PointCloud<pcl::PointXYZ>);
    cloud_centroid = cloud;

    centroid(cloud_centroid);//点群の重心位置求めを（0,0,0）に移動する

    Eigen::Matrix4f rot_z;
    pcl::PointCloud<pcl::PointXYZ> cloud_out;
    cloud_out = *cloud;
    double theta_z = theta * 3.1415926 / 180.0;//角度の変換
    rot_z(0,0) = cos(theta_z);
    rot_z(1,0) = -sin(theta_z);
    rot_z(2,0) = 0.0;
    rot_z(3,0) = 0.0;
    rot_z(0,1) = sin(theta_z);
    rot_z(1,1) = cos(theta_z);
    rot_z(2,1) = 0.0;
    rot_z(3,1) = 0.0;
    rot_z(0,2) = 0.0;
    rot_z(1,2) = 0.0;
    rot_z(2,2) = 1.0;
    rot_z(3,2) = 0.0;
    rot_z(0,3) = 0.0;
    rot_z(1,3) = 0.0;
    rot_z(2,3) = 0.0;
    rot_z(3,3) = 1.0;

    for(size_t i = 0; i < cloud->points.size(); ++i){
        cloud_out.points[i].x = rot_z(0,0) * cloud->points[i].x + rot_z(0,1) * cloud->points[i].y + rot_z(0,2) * cloud->points[i].z + rot_z(0,3) * 1;
        cloud_out.points[i].y = rot_z(1,0) * cloud->points[i].x + rot_z(1,1) * cloud->points[i].y + rot_z(1,2) * cloud->points[i].z + rot_z(1,3) * 1;
        cloud_out.points[i].z = rot_z(2,0) * cloud->points[i].x + rot_z(2,1) * cloud->points[i].y + rot_z(2,2) * cloud->points[i].z + rot_z(2,3) * 1;
    }

    return cloud_out;
}

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

    pcl::io::loadPCDFile(argv[1],*cloud);
   
    Eigen::Vector4f xyz_centroid;
    pcl::compute3DCentroid(*cloud, xyz_centroid);//元の点群の重心位置を記録

    *rotated_cloud = rotation_x(cloud, -90.0);//回転
    //*rotated_cloud = rotation_y(cloud, -90.0);//回転
    //*rotated_cloud = rotation_z(cloud, -90.0);//回転
   
    std::stringstream Filename;
    string name = argv[1];
    name.erase(name.length()-4);
    Filename << name << "_rotated.pcd";

    pcl::io::savePCDFileBinary(Filename.str(), *rotated_cloud);
    return 0;
}


結果

実行前

実行後

　1つの関数にまとめてしまっても良かったが、後の混乱を回避するためそれぞれの回転に分けた。実行に際しては回転する角度を-180～180でしていする。今回はX軸を-90°回転させた。