Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00045 #include "pcl/io/pcd_io.h"
00046 #include "pcl/point_types.h"
00047 #include <pcl/features/normal_3d.h>
00048
00049 int
00050 main (int argc, char** argv)
00051 {
00052
00053 pcl::PCDReader reader;
00054 pcl::PointCloud<pcl::PointXYZ> cloud_xyz;
00055 if (argc != 2)
00056 {
00057 ROS_ERROR("Usage: %s <input_cloud.pcd>", argv[0]);
00058 exit(2);
00059 }
00060 reader.read (argv[1], cloud_xyz);
00061 ROS_INFO ("PointCloud has: %zu data points.", cloud_xyz.points.size ());
00062
00063
00064
00065 pcl::PointCloud<pcl::PointXYZI> cloud_a;
00066 cloud_a.points.resize(cloud_xyz.points.size ());
00067 for (unsigned int i = 0; i < cloud_xyz.points.size(); i++)
00068 {
00069 cloud_a.points[i].x = cloud_xyz.points[i].x;
00070 cloud_a.points[i].y = cloud_xyz.points[i].y;
00071 cloud_a.points[i].z = cloud_xyz.points[i].z;
00072 cloud_a.points[i].intensity = 0;
00073 }
00074
00075
00076
00077 pcl::PointCloud<pcl::Normal> cloud_b;
00078 pcl::NormalEstimation<pcl::PointXYZI, pcl::Normal> ne;
00079 pcl::search::KdTree<pcl::PointXYZI>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZI> ());
00080 ne.setSearchMethod (tree);
00081 ne.setInputCloud (cloud_a.makeShared());
00082 ne.setKSearch (10);
00083 ne.compute (cloud_b);
00084
00085
00086
00087 pcl::PointCloud<pcl::PointXYZINormal> cloud_c;
00088 pcl::concatenateFields (cloud_a, cloud_b, cloud_c);
00089
00090
00091
00092 pcl::PCDWriter writer;
00093 ROS_INFO ("Writing PointCloud with: %zu data points.", cloud_c.points.size ());
00094 writer.write (std::string(argv[1]) + "_normals.pcd", cloud_c, false);
00095 return (0);
00096 }