40 #ifndef PCL_SEARCH_ORGANIZED_NEIGHBOR_SEARCH_H_
41 #define PCL_SEARCH_ORGANIZED_NEIGHBOR_SEARCH_H_
43 #include <pcl/point_cloud.h>
44 #include <pcl/point_types.h>
45 #include <pcl/search/search.h>
46 #include <pcl/common/eigen.h>
51 #include <pcl/common/projection_matrix.h>
61 template<
typename Po
intT>
73 typedef boost::shared_ptr<pcl::search::OrganizedNeighbor<PointT> >
Ptr;
74 typedef boost::shared_ptr<const pcl::search::OrganizedNeighbor<PointT> >
ConstPtr;
88 OrganizedNeighbor (
bool sorted_results =
false,
float eps = 1e-4f,
unsigned pyramid_level = 5)
91 ,
KR_ (Eigen::Matrix<float, 3, 3, Eigen::RowMajor>::Zero ())
92 ,
KR_KRT_ (Eigen::Matrix<float, 3, 3, Eigen::RowMajor>::Zero ())
113 float min_f = 0.043744332f *
static_cast<float>(
input_->width);
115 return (determinant3x3Matrix<Eigen::Matrix3f> (
KR_ / sqrtf (
KR_KRT_.coeff (8))) >= (min_f * min_f));
140 for (std::vector<int>::const_iterator iIt =
indices_->begin (); iIt !=
indices_->end (); ++iIt)
162 std::vector<int> &k_indices,
163 std::vector<float> &k_sqr_distances,
164 unsigned int max_nn = 0)
const;
182 std::vector<int> &k_indices,
183 std::vector<float> &k_sqr_distances)
const;
216 testPoint (
const PointT& query,
unsigned k, std::priority_queue<Entry>& queue,
unsigned index)
const
219 if (
mask_ [index] && pcl_isfinite (point.x))
222 float dist_x = point.x - query.x;
223 float dist_y = point.y - query.y;
224 float dist_z = point.z - query.z;
225 float squared_distance = dist_x * dist_x + dist_y * dist_y + dist_z * dist_z;
226 if (queue.size () < k)
227 queue.push (
Entry (index, squared_distance));
228 else if (queue.top ().distance > squared_distance)
231 queue.push (
Entry (index, squared_distance));
239 clipRange (
int& begin,
int &end,
int min,
int max)
const
241 begin = std::max (std::min (begin, max), min);
242 end = std::min (std::max (end, min), max);
255 unsigned& maxX,
unsigned& maxY)
const;
262 Eigen::Matrix<float, 3, 3, Eigen::RowMajor>
KR_;
265 Eigen::Matrix<float, 3, 3, Eigen::RowMajor>
KR_KRT_;
276 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
281 #ifdef PCL_NO_PRECOMPILE
282 #include <pcl/search/impl/organized.hpp>