octree_search.cpp
Go to the documentation of this file.
00001 #include <pcl/point_cloud.h>
00002 #include <pcl/octree/octree.h>
00003 
00004 #include <iostream>
00005 #include <vector>
00006 #include <ctime>
00007 
00008 int
00009 main (int argc, char** argv)
00010 {
00011   srand ((unsigned int) 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   float resolution = 128.0f;
00028 
00029   pcl::octree::OctreePointCloudSearch<pcl::PointXYZ> octree (resolution);
00030 
00031   octree.setInputCloud (cloud);
00032   octree.addPointsFromInputCloud ();
00033 
00034   pcl::PointXYZ searchPoint;
00035 
00036   searchPoint.x = 1024.0f * rand () / (RAND_MAX + 1.0f);
00037   searchPoint.y = 1024.0f * rand () / (RAND_MAX + 1.0f);
00038   searchPoint.z = 1024.0f * rand () / (RAND_MAX + 1.0f);
00039 
00040   // Neighbors within voxel search
00041 
00042   std::vector<int> pointIdxVec;
00043 
00044   if (octree.voxelSearch (searchPoint, pointIdxVec))
00045   {
00046     std::cout << "Neighbors within voxel search at (" << searchPoint.x 
00047      << " " << searchPoint.y 
00048      << " " << searchPoint.z << ")" 
00049      << std::endl;
00050               
00051     for (size_t i = 0; i < pointIdxVec.size (); ++i)
00052    std::cout << "    " << cloud->points[pointIdxVec[i]].x 
00053        << " " << cloud->points[pointIdxVec[i]].y 
00054        << " " << cloud->points[pointIdxVec[i]].z << std::endl;
00055   }
00056 
00057   // K nearest neighbor search
00058 
00059   int K = 10;
00060 
00061   std::vector<int> pointIdxNKNSearch;
00062   std::vector<float> pointNKNSquaredDistance;
00063 
00064   std::cout << "K nearest neighbor search at (" << searchPoint.x 
00065             << " " << searchPoint.y 
00066             << " " << searchPoint.z
00067             << ") with K=" << K << std::endl;
00068 
00069   if (octree.nearestKSearch (searchPoint, K, pointIdxNKNSearch, pointNKNSquaredDistance) > 0)
00070   {
00071     for (size_t i = 0; i < pointIdxNKNSearch.size (); ++i)
00072       std::cout << "    "  <<   cloud->points[ pointIdxNKNSearch[i] ].x 
00073                 << " " << cloud->points[ pointIdxNKNSearch[i] ].y 
00074                 << " " << cloud->points[ pointIdxNKNSearch[i] ].z 
00075                 << " (squared distance: " << pointNKNSquaredDistance[i] << ")" << std::endl;
00076   }
00077 
00078   // Neighbors within radius search
00079 
00080   std::vector<int> pointIdxRadiusSearch;
00081   std::vector<float> pointRadiusSquaredDistance;
00082 
00083   float radius = 256.0f * rand () / (RAND_MAX + 1.0f);
00084 
00085   std::cout << "Neighbors within radius search at (" << searchPoint.x 
00086       << " " << searchPoint.y 
00087       << " " << searchPoint.z
00088       << ") with radius=" << radius << std::endl;
00089 
00090 
00091   if (octree.radiusSearch (searchPoint, radius, pointIdxRadiusSearch, pointRadiusSquaredDistance) > 0)
00092   {
00093     for (size_t i = 0; i < pointIdxRadiusSearch.size (); ++i)
00094       std::cout << "    "  <<   cloud->points[ pointIdxRadiusSearch[i] ].x 
00095                 << " " << cloud->points[ pointIdxRadiusSearch[i] ].y 
00096                 << " " << cloud->points[ pointIdxRadiusSearch[i] ].z 
00097                 << " (squared distance: " << pointRadiusSquaredDistance[i] << ")" << std::endl;
00098   }
00099 
00100 }


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