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

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