kdtree_search.cpp
Go to the documentation of this file.
00001 #include <pcl/point_cloud.h>
00002 #include <pcl/kdtree/kdtree_flann.h>
00003 
00004 #include <iostream>
00005 #include <vector>
00006 #include <ctime>
00007 
00008 int
00009 main (int argc, char** argv)
00010 {
00011   srand (time (NULL));
00012 
00013   pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
00014 
00015   // Generate pointcloud data
00016   cloud->width = 1000;
00017   cloud->height = 1;
00018   cloud->points.resize (cloud->width * cloud->height);
00019 
00020   for (size_t i = 0; i < cloud->points.size (); ++i)
00021   {
00022     cloud->points[i].x = 1024.0f * rand () / (RAND_MAX + 1.0f);
00023     cloud->points[i].y = 1024.0f * rand () / (RAND_MAX + 1.0f);
00024     cloud->points[i].z = 1024.0f * rand () / (RAND_MAX + 1.0f);
00025   }
00026 
00027   pcl::KdTreeFLANN<pcl::PointXYZ> kdtree;
00028 
00029   kdtree.setInputCloud (cloud);
00030 
00031   pcl::PointXYZ searchPoint;
00032 
00033   searchPoint.x = 1024.0f * rand () / (RAND_MAX + 1.0f);
00034   searchPoint.y = 1024.0f * rand () / (RAND_MAX + 1.0f);
00035   searchPoint.z = 1024.0f * rand () / (RAND_MAX + 1.0f);
00036 
00037   // K nearest neighbor search
00038 
00039   int K = 10;
00040 
00041   std::vector<int> pointIdxNKNSearch(K);
00042   std::vector<float> pointNKNSquaredDistance(K);
00043 
00044   std::cout << "K nearest neighbor search at (" << searchPoint.x 
00045             << " " << searchPoint.y 
00046             << " " << searchPoint.z
00047             << ") with K=" << K << std::endl;
00048 
00049   if ( kdtree.nearestKSearch (searchPoint, K, pointIdxNKNSearch, pointNKNSquaredDistance) > 0 )
00050   {
00051     for (size_t i = 0; i < pointIdxNKNSearch.size (); ++i)
00052       std::cout << "    "  <<   cloud->points[ pointIdxNKNSearch[i] ].x 
00053                 << " " << cloud->points[ pointIdxNKNSearch[i] ].y 
00054                 << " " << cloud->points[ pointIdxNKNSearch[i] ].z 
00055                 << " (squared distance: " << pointNKNSquaredDistance[i] << ")" << std::endl;
00056   }
00057 
00058   // Neighbors within radius search
00059 
00060   std::vector<int> pointIdxRadiusSearch;
00061   std::vector<float> pointRadiusSquaredDistance;
00062 
00063   float radius = 256.0f * rand () / (RAND_MAX + 1.0f);
00064 
00065   std::cout << "Neighbors within radius search at (" << searchPoint.x 
00066             << " " << searchPoint.y 
00067             << " " << searchPoint.z
00068             << ") with radius=" << radius << std::endl;
00069 
00070 
00071   if ( kdtree.radiusSearch (searchPoint, radius, pointIdxRadiusSearch, pointRadiusSquaredDistance) > 0 )
00072   {
00073     for (size_t i = 0; i < pointIdxRadiusSearch.size (); ++i)
00074       std::cout << "    "  <<   cloud->points[ pointIdxRadiusSearch[i] ].x 
00075                 << " " << cloud->points[ pointIdxRadiusSearch[i] ].y 
00076                 << " " << cloud->points[ pointIdxRadiusSearch[i] ].z 
00077                 << " (squared distance: " << pointRadiusSquaredDistance[i] << ")" << std::endl;
00078   }
00079 
00080 
00081   return 0;
00082 }


pcl
Author(s): Open Perception
autogenerated on Wed Aug 26 2015 15:25:05