今後の方針 改訂

　残念ながら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を使用したものと変化が無いように感じられるが、このプログラムの場合入力するファイルはひとつで良い。