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
00040 #ifndef PCL_SEARCH_ORGANIZED_NEIGHBOR_SEARCH_H_
00041 #define PCL_SEARCH_ORGANIZED_NEIGHBOR_SEARCH_H_
00042
00043 #include <pcl/point_cloud.h>
00044 #include <pcl/point_types.h>
00045 #include <pcl/search/search.h>
00046 #include <pcl/common/eigen.h>
00047
00048 #include <algorithm>
00049 #include <queue>
00050 #include <vector>
00051
00052 namespace pcl
00053 {
00054 namespace search
00055 {
00060 template<typename PointT>
00061 class OrganizedNeighbor : public pcl::search::Search<PointT>
00062 {
00063
00064 public:
00065
00066 typedef pcl::PointCloud<PointT> PointCloud;
00067 typedef boost::shared_ptr<PointCloud> PointCloudPtr;
00068
00069 typedef boost::shared_ptr<const PointCloud> PointCloudConstPtr;
00070 typedef boost::shared_ptr<const std::vector<int> > IndicesConstPtr;
00071
00072 typedef boost::shared_ptr<pcl::search::OrganizedNeighbor<PointT> > Ptr;
00073 typedef boost::shared_ptr<const pcl::search::OrganizedNeighbor<PointT> > ConstPtr;
00074
00075 using pcl::search::Search<PointT>::indices_;
00076 using pcl::search::Search<PointT>::sorted_results_;
00077 using pcl::search::Search<PointT>::input_;
00078
00087 OrganizedNeighbor (bool sorted_results = false, float eps = 1e-4f, unsigned pyramid_level = 5)
00088 : Search<PointT> ("OrganizedNeighbor", sorted_results)
00089 , projection_matrix_ (Eigen::Matrix<float, 3, 4, Eigen::RowMajor>::Zero ())
00090 , KR_ (Eigen::Matrix<float, 3, 3, Eigen::RowMajor>::Zero ())
00091 , KR_KRT_ (Eigen::Matrix<float, 3, 3, Eigen::RowMajor>::Zero ())
00092 , eps_ (eps)
00093 , pyramid_level_ (pyramid_level)
00094 , mask_ ()
00095 {
00096 }
00097
00099 virtual ~OrganizedNeighbor () {}
00100
00106 bool
00107 isValid () const
00108 {
00109
00110
00111
00112 float min_f = 0.043744332f * static_cast<float>(input_->width);
00113
00114 return (determinant3x3Matrix<Eigen::Matrix3f> (KR_ / sqrt (KR_KRT_.coeff (8))) >= (min_f * min_f));
00115 }
00116
00120 void
00121 computeCameraMatrix (Eigen::Matrix3f& camera_matrix) const;
00122
00127 virtual void
00128 setInputCloud (const PointCloudConstPtr& cloud, const IndicesConstPtr &indices = IndicesConstPtr ())
00129 {
00130 input_ = cloud;
00131
00132 mask_.resize (input_->size ());
00133 input_ = cloud;
00134 indices_ = indices;
00135
00136 if (indices_.get () != NULL && indices_->size () != 0)
00137 {
00138 mask_.assign (input_->size (), 0);
00139 for (std::vector<int>::const_iterator iIt = indices_->begin (); iIt != indices_->end (); ++iIt)
00140 mask_[*iIt] = 1;
00141 }
00142 else
00143 mask_.assign (input_->size (), 1);
00144
00145 estimateProjectionMatrix ();
00146 }
00147
00158 int
00159 radiusSearch (const PointT &p_q,
00160 double radius,
00161 std::vector<int> &k_indices,
00162 std::vector<float> &k_sqr_distances,
00163 unsigned int max_nn = 0) const;
00164
00166 void
00167 estimateProjectionMatrix ();
00168
00178 int
00179 nearestKSearch (const PointT &p_q,
00180 int k,
00181 std::vector<int> &k_indices,
00182 std::vector<float> &k_sqr_distances) const;
00183
00189 bool projectPoint (const PointT& p, pcl::PointXY& q) const;
00190
00191 protected:
00192
00193 struct Entry
00194 {
00195 Entry (int idx, float dist) : index (idx), distance (dist) {}
00196 Entry () : index (0), distance (0) {}
00197 unsigned index;
00198 float distance;
00199
00200 inline bool
00201 operator < (const Entry& other) const
00202 {
00203 return (distance < other.distance);
00204 }
00205 };
00206
00214 inline bool
00215 testPoint (const PointT& query, unsigned k, std::priority_queue<Entry>& queue, unsigned index) const
00216 {
00217 const PointT& point = input_->points [index];
00218 if (mask_ [index] && pcl_isfinite (point.x))
00219 {
00220 float squared_distance = (point.getVector3fMap () - query.getVector3fMap ()).squaredNorm ();
00221 if (queue.size () < k)
00222 queue.push (Entry (index, squared_distance));
00223 else if (queue.top ().distance > squared_distance)
00224 {
00225 queue.pop ();
00226 queue.push (Entry (index, squared_distance));
00227 return true;
00228 }
00229 }
00230 return false;
00231 }
00232
00233 inline void
00234 clipRange (int& begin, int &end, int min, int max) const
00235 {
00236 begin = std::max (std::min (begin, max), min);
00237 end = std::min (std::max (end, min), max);
00238 }
00239
00248 void
00249 getProjectedRadiusSearchBox (const PointT& point, float squared_radius, unsigned& minX, unsigned& minY,
00250 unsigned& maxX, unsigned& maxY) const;
00251
00252
00254 template <typename MatrixType> void
00255 makeSymmetric (MatrixType& matrix, bool use_upper_triangular = true) const;
00256
00258 Eigen::Matrix<float, 3, 4, Eigen::RowMajor> projection_matrix_;
00259
00261 Eigen::Matrix<float, 3, 3, Eigen::RowMajor> KR_;
00262
00264 Eigen::Matrix<float, 3, 3, Eigen::RowMajor> KR_KRT_;
00265
00267 const float eps_;
00268
00270 const unsigned pyramid_level_;
00271
00273 std::vector<unsigned char> mask_;
00274 public:
00275 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
00276 };
00277 }
00278 }
00279
00280 #endif
00281