normalWriter

#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>
float rad;
using namespace std;

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

int main(int argc, char** argv)
{
    pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZRGB>);

    //PCDファイルを読み込む
    cout << "PCDファイルを読み込みます" << endl;
    if(pcl::io::loadPCDFile<pcl::PointXYZRGB> (getFileName().c_str(), *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 (0.4);

    //法線の計算結果取得
    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(getFileName().c_str());

    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 << 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;
}