今後の方針 改訂

　残念ながらKinectの開発環境は未だできておらず、その目処も立っていない。このままではこのブログを腐らせるだけだと判断したので、しばらくはログの解析などに使用できるコードを作っていこうと思う。

　大掛かりな機械制御のソフトや、コード数が膨大なソフトそのものでは、不具合への対処のために一々プログラムの端から端まで見返すことはできない。そこで予めログを吐き出す機能を実装しておくことによって正常な動作を行っているか、異常が発生したのはどの部分であるのか、を見つけやすくするのがログの役割である。無論このログに記載される情報量も膨大で、必要な情報を取り出す処理が必須となる。（データに対して同じ処理を繰り返すソフトや、同じ品を作り続ける機械などは特に）

　このブログでが、このログの解析の際に、特定の情報を得やすいようなプログラムのコードを書いていこうと思う。

　対象となるログは、一行で一つの情報が完結しているものを取り扱おうと思う。簡単に言うと文字列処理である。これに加えてファイルの処理などのコードも作っていこうと思う。

　大原則は変わらず、できるだけ短いコードを書く

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°回転させた。

物事を論じる際に注意すべきこと

　日本人は議論が下手だとよく言われる。それに関しては、我々は議論や討論を行う訓練をつんでいないため仕方がないと思う。議論の際にその議論をより良いものとできるように留意しなければならないと考えたものをいかにまとめてみた。

　議論するときにやめるべきこと

• 意見を述べている人を遮って意見を述べない。
• 当然である。議論は相手の意見がなくてはできない。演説がしたいならば壁にでも向かってやればよい

• 主題と関連しているが、限定的、局所的な例などを用いない、付け足さない。
• これはわき道にそらしていることに他ならない。自身の意見をなんとか通したい場合にありがちの手段である。要は主題と正面から向き合っていない。

•  感情論を持ち込まない。
• 感情的な意見の根拠は自分自身でしかない、つまり相手はそれを観測することは絶対にできない。よってそういった意見を述べられてもなにもすることができない。

• 相手の人格、性格を否定する。
• 主題をはるか遠くへ追いやる行為にである。やる意味がない。意見を交わす議論ではなく単なる悪口になってしまう。

　自分がこれを完璧にできているかと言われればおそらくＮＯであるが、常に心がけることによって議論はよりよいものになると思う。
　よくありがちなのは並行線の議論となってしまった際でもどちらか一方を勝者としてしまおうとすることである。これもまた避けるべきである。時には結論がないという結果もある。
　他にもあればご指摘を賜りたい。

２つの点群データを比較して片方にない点群を取り出す

　PCLではOctreeを使って２つの点群を比較し、その差分（片方に無いもの）を取り出す機能がある。なにが便利かというと、静的な（動きがない）環境で唯一動いているものがあればそれを検出できるということ。絶対に何者かが入って行動することがないような場所だとこのOctreeは使える。もちろん実環境は動的なので使うのは難しい。以下コード

コード

#include "stdafx.h"

#include <pcl/point_cloud.h>
#include <pcl/octree/octree.h>
#include <pcl/io/pcd_io.h>

#include <iostream>
#include <vector>
#include <ctime>


pcl::PointCloud<pcl::PointXYZ> difference_extraction(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_base, pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_test)
{
    //cloud_baseは元となる点群
    //cloud_testは比較対象の点群
    //cloud_diffは比較の結果差分とされた点群

    double resolution = 0.00001;//Octreeの解像度を指定
   
    pcl::octree::OctreePointCloudChangeDetector<pcl::PointXYZ> octree (resolution);//Octreeを作成
   
    octree.setInputCloud (cloud_base);//元となる点群を入力
    octree.addPointsFromInputCloud ();

    octree.switchBuffers ();//バッファの切り替え

    octree.setInputCloud (cloud_test);//比較対象の点群を入力
    octree.addPointsFromInputCloud ();

    std::vector<int> newPointIdxVector;//

    octree.getPointIndicesFromNewVoxels (newPointIdxVector);//比較の結果差分と判断された点郡の情報を保管
   
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_diff (new pcl::PointCloud<pcl::PointXYZ> );//出力先


    //保管先のサイズの設定
    cloud_diff->width = cloud_base->points.size() + cloud_test->points.size();
    cloud_diff->height = 1;
    cloud_diff->points.resize (cloud_diff->width * cloud_diff->height);   

    int n = 0;//差分点群の数を保存する
    for(size_t i = 0; i < newPointIdxVector.size (); i++)
    {
        cloud_diff->points[i].x = cloud_test->points[newPointIdxVector[i]].x;
        cloud_diff->points[i].y = cloud_test->points[newPointIdxVector[i]].y;
        cloud_diff->points[i].z = cloud_test->points[newPointIdxVector[i]].z;
        n++;
    }
    //差分点群のサイズの再設定
    cloud_diff->width = n;
    cloud_diff->height = 1;
    cloud_diff->points.resize (cloud_diff->width * cloud_diff->height);

    return *cloud_diff;
}

int _tmain(int argc, _TCHAR* argv[])
{
    srand ((unsigned int) time (NULL));

    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_base (new pcl::PointCloud<pcl::PointXYZ> );
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_test (new pcl::PointCloud<pcl::PointXYZ> );

    pcl::io::loadPCDFile(argv[1],*cloud_base);
    pcl::io::loadPCDFile(argv[2], *cloud_test);

    pcl::io::savePCDFileBinary("difference.pcd", difference_extraction(cloud_base, cloud_test));

    return (0);
}

実行結果

基とするデータ

比較対象

差分取得後

　今回resolutionを極めて小さい値にしているが本来はもっと大きくしておかないと全て差分として検出されてしまうから注意が必要（センサから得られる情報はたとえ同じ地点の情報でも若干上下したりする）。resolutionを小さくしているのは用意したデータがどちらも静的なものであるから。


　このOctreeは一応警備システムなどにも使えるのではないかと思う。人の活動がなくなる時間帯に起動しておいて、なにもない状況を登録→変化があればその場面を保存。みたいな流れ。

点群の数を減少させる

　法線の推定などは入力された点群データの全ての点について、一定範囲内の近隣の点を探し、それらを基に法線を推定していた。この処理は点の数が多いほど時間が増大する。Kinectを使用する場合だと得られる点群の数が理論上307200個なので膨大な時間を要し、リアルタイムに処理を行う必要がある場面では、なにかしらの前処理を行って、点群の数を減らす必要がある。
 　PCLではダウンサンプリングと呼ばれる手法が用いられる。このダウンサンプリングは一定の範囲内にある点の中心に新たな点を配し、それ以外の点は全て削除する機能である。ちなみにこのダウンサンプリングの正式な名前はVoxelGridFilterという。点群の密度は状況によって大きく変わるのでここでは点群情報の体積から範囲を決定する。

コード

#include "stdafx.h"

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

pcl::PointCloud<pcl::PointXYZ> voxel_grid(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud)
{
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered (new pcl::PointCloud<pcl::PointXYZ>);

    double max_height = 0, max_width = 0, max_length = 0;
    double min_height = 0, min_width = 0, min_length = 0;
    double height = 0, width = 0, length = 0;

    for(size_t i = 0; i < cloud->points.size(); i++){//入力点群の縦横高さの最大値と最小値を計算する
        if(max_height > cloud->points[i].z)
            max_height = max_height;
        else
            max_height = cloud->points[i].z;

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

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

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

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

        if(min_length < cloud->points[i].x)
            min_length = min_length;
        else
            min_length = cloud->points[i].x;

    }

    height = fabs(max_height) + fabs(min_height);
    width = fabs(max_width) + fabs(min_width);
    length = fabs(max_length) + fabs(min_length);

    double model_volume = 0, voxel_volume, voxel_length = 0;
    model_volume = height * width * length;
    voxel_volume = model_volume / 150000;

    voxel_length = pow(voxel_volume, 1.0 / 3.0);

    pcl::VoxelGrid<pcl::PointXYZ> sor;
    sor.setInputCloud (cloud);
    //sor.setLeafSize (0.01f, 0.01f, 0.01f);
    sor.setLeafSize(voxel_length, voxel_length, voxel_length);
    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;
    std::string name = argv[1];
    name.erase(name.length()-4);
    Filename << name << "_voxel.pcd";

    pcl::io::savePCDFileBinary(Filename.str(), voxel_grid(cloud));

    return (0); 
}


実行結果

処理前

処理後

　このプログラムによって点群の数を減らし、処理速度の向上などを見込む事ができる。しかし、この処理はPassThroughFilterと同様、点が有用であるかどうか問わずに削除を行うので注意が必要。もしそれが嫌だったら高機能なコンピュータを容易するかGPUに処理を手伝わせるなどする必要がある。自分はGPUに処理の一部を投げる方法はまだ知らない。

２つの点郡データを１つに統合する

　これはチュートリアルにあるのをそのまま使えばいい話だが一応記事にしておく
　引数に与えられた２つのPCDファイルをひとつにまとめて表示＋保存するプログラム。

コード

#include "stdafx.h"

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

pcl::PointCloud<pcl::PointXYZ> merge_cloud(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud1, pcl::PointCloud<pcl::PointXYZ>::Ptr cloud2)
{
    pcl::PointCloud<pcl::PointXYZ> cloud_merged;//２つの点群をまとめて格納する
    cloud_merged = *cloud1;//１つ目の点群を統合先に格納する
    cloud_merged += *cloud2;//２つ目の点群を統合先に格納する。

    return cloud_merged;
}

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

    pcl::io::savePCDFileBinary("merged.pcd" , merge_cloud(cloud1, cloud2));

    return (0); 
}



　まぁ大して面白くもない、ただただ２つの点郡を１つのファイルにまとめるだけ。

各点の法線の計算とその法線の表示

　３次元の点郡を扱う上で重要になってくるのが各点が持つ法線である。３次元点郡を扱う研究では、頻繁にこの法線を使う。ものによってはこの法線情報のみを特徴として人間の検知をやってしまっている研究さえある。今回は入力された点郡の法線を取得して表示するプログラムを作る。
　今のところPCLのチュートリアルの機能を関数化したりしてるだけだが、ここで作った関数を後で切り貼りすると大きな機能を含んだプログラムを作るのが容易になるはず。

コード

#include "stdafx.h"

#include <iostream>
#include <pcl/io/pcd_io.h>
#include <pcl/io/io.h>
#include <pcl/point_types.h>
#include <pcl/features/normal_3d.h>
#include <pcl/visualization/pcl_visualizer.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;
}

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

    surface_normals(cloud);

    std::stringstream Filename;
    std::string name;
    name = argv[1];
    name.erase(name.length()-4);
    Filename << name << "_normal.pcd";

    pcl::io::savePCDFileBinary(Filename.str() , *surface_normals(cloud));

    pcl::visualization::PCLVisualizer viewer ("3D Viewer");
    viewer.setBackgroundColor(1.0,0.5,1.0);
    viewer.addPointCloud<pcl::PointXYZ> (cloud, "Input cloud");
    viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE,3,"Input cloud");
    viewer.addPointCloudNormals<pcl::PointXYZ,pcl::Normal>(cloud,surface_normals(cloud),10,0.05,"normals");

    while(!viewer.wasStopped())
    {
        viewer.spinOnce (100);
    }
    return (0); 
}

実行結果

以前作ったViewerでは法線の表示はできないので今回は法線を表示できるViewerをつけている。
　この法線推定プログラムの処理にはそこそこの時間を要する、この理由は各点の周辺（指定した半径）の点の数から法線を求める処理を全ての入力点に対して行っているためである。なので処理を短くしたい場合は、点群の数を減らすなどの工夫が必要。