VM LAB: 点群データのスムージング

　センサから得られる点群データは、実空間上では平面であってもいざ点群データとして出力された場合は、微妙の誤差があり、まるで凹凸があるかのようになってしまう。勿論これはセンサの原理的に起こりうる事象なので仕方がない。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);
}

実行結果