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
00037
00038
00039 #ifndef PCL_SEARCH_OCTREE_H
00040 #define PCL_SEARCH_OCTREE_H
00041
00042 #include <pcl/search/search.h>
00043 #include <pcl/octree/octree_search.h>
00044
00045 namespace pcl
00046 {
00047 namespace search
00048 {
00065 template<typename PointT, typename LeafTWrap = pcl::octree::OctreeContainerDataTVector<int>, typename BranchTWrap = pcl::octree::OctreeContainerEmpty<int>,
00066 typename OctreeT = pcl::octree::OctreeBase<int, LeafTWrap, BranchTWrap > >
00067 class Octree: public Search<PointT>
00068 {
00069 public:
00070
00071 typedef boost::shared_ptr<std::vector<int> > IndicesPtr;
00072 typedef boost::shared_ptr<const std::vector<int> > IndicesConstPtr;
00073
00074 typedef pcl::PointCloud<PointT> PointCloud;
00075 typedef boost::shared_ptr<PointCloud> PointCloudPtr;
00076 typedef boost::shared_ptr<const PointCloud> PointCloudConstPtr;
00077
00078
00079 typedef boost::shared_ptr<pcl::octree::OctreePointCloudSearch<PointT, LeafTWrap, BranchTWrap> > Ptr;
00080 typedef boost::shared_ptr<const pcl::octree::OctreePointCloudSearch<PointT, LeafTWrap, BranchTWrap> > ConstPtr;
00081 Ptr tree_;
00082
00083 using pcl::search::Search<PointT>::input_;
00084 using pcl::search::Search<PointT>::indices_;
00085 using pcl::search::Search<PointT>::sorted_results_;
00086
00090 Octree (const double resolution)
00091 : Search<PointT> ("Octree")
00092 , tree_ (new pcl::octree::OctreePointCloudSearch<PointT, LeafTWrap, BranchTWrap> (resolution))
00093 {
00094 }
00095
00097 virtual
00098 ~Octree ()
00099 {
00100 }
00101
00105 inline void
00106 setInputCloud (const PointCloudConstPtr &cloud)
00107 {
00108 tree_->deleteTree ();
00109 tree_->setInputCloud (cloud);
00110 tree_->addPointsFromInputCloud ();
00111 input_ = cloud;
00112 }
00113
00118 inline void
00119 setInputCloud (const PointCloudConstPtr &cloud, const IndicesConstPtr& indices)
00120 {
00121 tree_->deleteTree ();
00122 tree_->setInputCloud (cloud, indices);
00123 tree_->addPointsFromInputCloud ();
00124 input_ = cloud;
00125 indices_ = indices;
00126 }
00127
00137 inline int
00138 nearestKSearch (const PointCloud &cloud, int index, int k, std::vector<int> &k_indices,
00139 std::vector<float> &k_sqr_distances) const
00140 {
00141 return (tree_->nearestKSearch (cloud, index, k, k_indices, k_sqr_distances));
00142 }
00143
00152 inline int
00153 nearestKSearch (const PointT &point, int k, std::vector<int> &k_indices,
00154 std::vector<float> &k_sqr_distances) const
00155 {
00156 return (tree_->nearestKSearch (point, k, k_indices, k_sqr_distances));
00157 }
00158
00170 inline int
00171 nearestKSearch (int index, int k, std::vector<int> &k_indices, std::vector<float> &k_sqr_distances) const
00172 {
00173 return (tree_->nearestKSearch (index, k, k_indices, k_sqr_distances));
00174 }
00175
00185 inline int
00186 radiusSearch (const PointCloud &cloud,
00187 int index,
00188 double radius,
00189 std::vector<int> &k_indices,
00190 std::vector<float> &k_sqr_distances,
00191 unsigned int max_nn = 0) const
00192 {
00193 tree_->radiusSearch (cloud, index, radius, k_indices, k_sqr_distances, max_nn);
00194 if (sorted_results_)
00195 this->sortResults (k_indices, k_sqr_distances);
00196 return (static_cast<int> (k_indices.size ()));
00197 }
00198
00207 inline int
00208 radiusSearch (const PointT &p_q,
00209 double radius,
00210 std::vector<int> &k_indices,
00211 std::vector<float> &k_sqr_distances,
00212 unsigned int max_nn = 0) const
00213 {
00214 tree_->radiusSearch (p_q, radius, k_indices, k_sqr_distances, max_nn);
00215 if (sorted_results_)
00216 this->sortResults (k_indices, k_sqr_distances);
00217 return (static_cast<int> (k_indices.size ()));
00218 }
00219
00229 inline int
00230 radiusSearch (int index, double radius, std::vector<int> &k_indices,
00231 std::vector<float> &k_sqr_distances, unsigned int max_nn = 0) const
00232 {
00233 tree_->radiusSearch (index, radius, k_indices, k_sqr_distances, max_nn);
00234 if (sorted_results_)
00235 this->sortResults (k_indices, k_sqr_distances);
00236 return (static_cast<int> (k_indices.size ()));
00237 }
00238
00239
00247 inline void
00248 approxNearestSearch (const PointCloudConstPtr &cloud, int query_index, int &result_index,
00249 float &sqr_distance)
00250 {
00251 return (tree_->approxNearestSearch (cloud->points[query_index], result_index, sqr_distance));
00252 }
00253
00259 inline void
00260 approxNearestSearch (const PointT &p_q, int &result_index, float &sqr_distance)
00261 {
00262 return (tree_->approxNearestSearch (p_q, result_index, sqr_distance));
00263 }
00264
00272 inline void
00273 approxNearestSearch (int query_index, int &result_index, float &sqr_distance)
00274 {
00275 return (tree_->approxNearestSearch (query_index, result_index, sqr_distance));
00276 }
00277
00278 };
00279 }
00280 }
00281
00282 #define PCL_INSTANTIATE_Octree(T) template class PCL_EXPORTS pcl::search::Octree<T>;
00283
00284 #endif // PCL_SEARCH_OCTREE_H