Go to the documentation of this file.00001
00002 #include <pcl/point_types.h>
00003 #include <pcl/io/pcd_io.h>
00004 #include <pcl/filters/filter_indices.h>
00005
00006
00007 #include <pcl/search/kdtree.h>
00008
00009
00010
00011
00012 #include <pcl/common/time.h>
00013 #include <pcl/console/parse.h>
00014
00015 #include <vector>
00016 #include <limits>
00017 typedef typename pcl::PointXYZ PointIn;
00018 typedef typename pcl::PointXYZI PointOut;
00019 typedef pcl::PointCloud<PointIn> CloudIn;
00020 typedef typename CloudIn::Ptr CloudInPtr;
00021 typedef pcl::PointCloud<PointOut> CloudOut;
00022 typedef typename CloudOut::Ptr CloudOutPtr;
00023
00024 struct CloudLabelPair {
00025 std::string name;
00026 int label;
00027 } ;
00028
00029 int
00030 main (int argc, char** argv)
00031 {
00032
00033 if (argc == 1)
00034 {
00035 pcl::console::print_info ("Syntax is: %s <-in scene_in.pcd> <-r radius> <-out scene_out.pcd> <-n subscenes in argv table> <subscene1.pcd label1 ...> \n", argv[0]);
00036 exit(0);
00037 }
00038
00039 double radius;
00040 pcl::console::parse_argument (argc, argv, "-r", radius);
00041
00042 std::string in_cloud;
00043 pcl::console::parse_argument (argc, argv, "-in", in_cloud);
00044
00045 std::string out_cloud;
00046 pcl::console::parse_argument (argc, argv, "-out", out_cloud);
00047
00048 int subscene_argc;
00049 pcl::console::parse_argument (argc, argv, "-n", subscene_argc);
00050
00051 std::vector<CloudLabelPair> all_clouds_and_labels;
00052 CloudLabelPair pair;
00053 for (int i = subscene_argc; i< argc; i=i+2)
00054 {
00055 pair.name = std::string(argv[i]);
00056 pair.label = atoi(argv[i+1]);
00057 all_clouds_and_labels.push_back(pair);
00058 }
00059
00060 double start, stop;
00061
00062 pcl::PCDReader reader;
00063 CloudInPtr cloud (new CloudIn), query_cloud (new CloudIn);
00064 CloudOutPtr cloud_out (new CloudOut);
00065 reader.read (in_cloud, *cloud);
00066 pcl::copyPointCloud(*cloud, *cloud_out);
00067 for (uint i = 0; i < cloud_out->points.size(); i++)
00068 {
00069 cloud_out->points[i].intensity = -1;
00070 }
00071
00072
00073 pcl::search::KdTree<PointIn>::Ptr tree (new pcl::search::KdTree<PointIn>);
00074 std::vector< int > cloud_non_nan;
00075 std::cerr << "removing NaN points: " << cloud->points.size() << std::endl;
00076 pcl::removeNaNFromPointCloud (*cloud, cloud_non_nan);
00077 std::cerr << "removed NaN points: " << cloud_non_nan.size() << std::endl;
00078 pcl::IndicesPtr cloud_non_nan_ind (new std::vector<int>);
00079 *cloud_non_nan_ind = cloud_non_nan;
00080 tree->setInputCloud (cloud, cloud_non_nan_ind);
00081 std::vector<int> kd_indices;
00082 std::vector<float> kd_distances;
00083 PointIn query;
00084 int knn = 0;
00085 double dist;
00086 int closest;
00087
00088 for (uint k = 0; k < all_clouds_and_labels.size(); k++)
00089 {
00090 reader.read (all_clouds_and_labels[k].name, *query_cloud);
00091 int label = all_clouds_and_labels[k].label;
00092 start = pcl::getTime ();
00093 std::cerr << "starting radiusSearch for label: " << label << " , query_cloud size: " << query_cloud->points.size() << std::endl;
00094 for (uint i = 0; i < query_cloud->points.size(); i++)
00095 {
00096 query = query_cloud->points[i];
00097 if( !pcl_isfinite( query.x ) ||
00098 !pcl_isfinite( query.y ) ||
00099 !pcl_isfinite( query.z ) )
00100 continue;
00101 tree->radiusSearch (query, radius, kd_indices, kd_distances, knn);
00102 dist = std::numeric_limits<double>::max();
00103 closest = -1;
00104 if (kd_indices.size() != 0)
00105 {
00106 for (uint j = 0; j < kd_indices.size(); j++)
00107 {
00108 if (kd_distances[j] < dist)
00109 {
00110 dist = kd_distances[j];
00111 closest = kd_indices[j];
00112 }
00113 }
00114 }
00115 if (closest != -1)
00116 {
00117 cloud_out->points[closest].intensity = label;
00118 }
00119 }
00120 stop = pcl::getTime ();
00121 std::cerr << "search query: " << stop - start << std::endl;
00122 }
00123
00124 pcl::PCDWriter writer;
00125 writer.write (out_cloud.c_str(), *cloud_out, false);
00126
00127 return (0);
00128 }