ait-survey

normal point cloud

Sep 5th, 2013
181
0
Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
C++ 2.02 KB | None | 0 0
  1. /******************
  2. Normal Writer
  3.  
  4. Input file:
  5. Output file:
  6. ********************/
  7.  
  8.  
  9. #include <iostream>
  10. #include <string>
  11. #include <fstream>
  12. #include <iomanip>
  13.  
  14. #include <pcl/io/pcd_io.h>
  15. #include <pcl/point_types.h>
  16. #include <pcl/features/normal_3d.h>
  17.  
  18. using namespace std;
  19.  
  20. string getFileName();
  21.  
  22. int main(int argc, char** argv)
  23. {
  24.     pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZRGB>);
  25.  
  26.     //PCDÉtÉ@ÉCÉãÇì«Ç›çûÇfi
  27.     cout << "Load PCD File Format" << endl;
  28.  
  29.     if(pcl::io::loadPCDFile<pcl::PointXYZRGB> (getFileName().c_str(), *cloud) == -1)
  30.     {
  31.         PCL_ERROR("Coudn't read file\n");
  32.         return(-1);
  33.     }
  34.  
  35.     //ñ@ê¸ÇÃåvéZ
  36.     //cout << "ñ@ê¸ÇåvéZǵNjÇ∑" << endl;
  37.  
  38.     pcl::NormalEstimation <pcl::PointXYZRGB, pcl::Normal> ne;
  39.     ne.setInputCloud (cloud);
  40.  
  41.     pcl::search::KdTree <pcl::PointXYZRGB>::Ptr tree (new pcl::search::KdTree <pcl::PointXYZRGB> ());
  42.     ne.setSearchMethod (tree);
  43.     ne.setRadiusSearch (0.03);
  44.  
  45.     //ñ@ê¸ÇÃåvéZåãâ éÊìæ
  46.     pcl::PointCloud <pcl::Normal>::Ptr normals (new pcl::PointCloud <pcl::Normal>);
  47.     ne.compute (*normals);
  48.  
  49.     /*
  50.         ñ@ê¸ÇÃåvéZåãâ ÇÃèëÇ´èoǵ
  51.         ÉtÉ@ÉCÉãÉtÉHÅ[É}ÉbÉgÇÕ
  52.         x y z r g b normal_x normal_y normal_z
  53.     */
  54.    
  55.     cout << "Output filename" << endl;
  56.     ofstream ofs;
  57.     ofs.open(getFileName().c_str());
  58.  
  59.     for(size_t i=0; i < cloud->points.size() ; i++)
  60.     {
  61.         ofs << setprecision(10)
  62.             << fixed << cloud->points[i].x << " "
  63.             << fixed << cloud->points[i].y << " "
  64.             << fixed << cloud->points[i].z << " "
  65.             << (int)cloud->points[i].r << " "
  66.             << (int)cloud->points[i].g << " "
  67.             << (int)cloud->points[i].b << " "
  68.             << fixed << normals->points[i].normal_x << " "
  69.             << fixed << normals->points[i].normal_y << " "
  70.             << fixed << normals->points[i].normal_z << endl;
  71.     }
  72.  
  73.     ofs.close();
  74.  
  75.     return(0);
  76. }
  77.  
  78.  
  79. string getFileName()
  80. {
  81.     fflush(stdin);
  82.     std::cout << "Input file name:";
  83.     std::string file_name;
  84.     std::getline( std::cin, file_name );
  85.     return file_name;
  86. }
Advertisement
Add Comment
Please, Sign In to add comment