☆normalwrite4.cpp

/********************************************************************
Normal Writer
Input file : PCD file (XYZRGB)
Processing : generate surface normal
Output file : PCD file (XYZRGBNormal)
argv[0]: executable filename
argv[1]:input pcd (XYZRGB) filename
argv[2]:output pcd (XYZRGBNormal) filename
argv[3]: value of radius

********************************************************************/
#include <iostream>
#include <string>
#include <fstream>
#include <iomanip>

#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/features/normal_3d.h>

using namespace std;

//関数のプロトタイプ宣言
string getFileName();

int main(int argc, char** argv)
{
    int r,g,b;

    float rad;
    rad=atof(argv[3]);

    pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZRGB>);

    //PCDファイルを読み込む (Input PCD file:XYZRGB)
    cout << "PCDファイルを読み込みます" << endl;

    if(pcl::io::loadPCDFile<pcl::PointXYZRGB> (argv[1], *cloud) == -1)
    {
        PCL_ERROR("Coudn't read file\n");
        return(-1);
    }

    //法線の計算
    cout << "法線を計算します" << endl;

    pcl::NormalEstimation <pcl::PointXYZRGB, pcl::Normal> ne;
    ne.setInputCloud (cloud);

    pcl::search::KdTree <pcl::PointXYZRGB>::Ptr tree (new pcl::search::KdTree <pcl::PointXYZRGB> ());
    ne.setSearchMethod (tree);
    ne.setRadiusSearch (rad);

    //法線の計算結果取得
    pcl::PointCloud <pcl::Normal>::Ptr normals (new pcl::PointCloud <pcl::Normal>);
        ne.compute (*normals);

    /*
        法線の計算結果の書き出し
        ファイルフォーマットは
        x y z r g b normal_x normal_y normal_z
    */

    cout << "計算結果を書き出します" << endl;

     ofstream ofs;
        ofs.open(argv[2]);

        for(size_t i=0; i < cloud->points.size() ; i++)
        {
                ofs << setprecision(10)
                        << fixed << cloud->points[i].x << " "
                        << fixed << cloud->points[i].y << " "
                        << fixed << cloud->points[i].z << " "
                        << (int)cloud->points[i].r << " "
                        << (int)cloud->points[i].g << " "
                        << (int)cloud->points[i].b << " "
                        << fixed << normals->points[i].normal_x << " "
                        << fixed << normals->points[i].normal_y << " "
                        << fixed << normals->points[i].normal_z << " "
                        << fixed << normals->points[i].curvature << endl;
        }

         ofs.close();

        return(0);
}


string getFileName()
{
        fflush(stdin);
        std::cout << "Input file name:";
        std::string file_name;
        std::getline( std::cin, file_name );
        return file_name;
}