座標軸を回転させる

　点群データは、時に移動させたり回転させたりしなければならないことが多々ある。今回は座標軸を回転させるプログラムを作る。尚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をつけている。
　この法線推定プログラムの処理にはそこそこの時間を要する、この理由は各点の周辺（指定した半径）の点の数から法線を求める処理を全ての入力点に対して行っているためである。なので処理を短くしたい場合は、点群の数を減らすなどの工夫が必要。