Point Cloud Library (PCL)  1.9.0-dev
organized.h
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Point Cloud Library (PCL) - www.pointclouds.org
5  * Copyright (c) 2010-2011, Willow Garage, Inc.
6  *
7  * All rights reserved.
8  *
9  * Redistribution and use in source and binary forms, with or without
10  * modification, are permitted provided that the following conditions
11  * are met:
12  *
13  * * Redistributions of source code must retain the above copyright
14  * notice, this list of conditions and the following disclaimer.
15  * * Redistributions in binary form must reproduce the above
16  * copyright notice, this list of conditions and the following
17  * disclaimer in the documentation and/or other materials provided
18  * with the distribution.
19  * * Neither the name of the copyright holder(s) nor the names of its
20  * contributors may be used to endorse or promote products derived
21  * from this software without specific prior written permission.
22  *
23  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
24  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
25  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
26  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
27  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
28  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
29  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
30  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
31  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
32  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
33  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
34  * POSSIBILITY OF SUCH DAMAGE.
35  *
36  * $Id$
37  *
38  */
39 
40 #ifndef PCL_SEARCH_ORGANIZED_NEIGHBOR_SEARCH_H_
41 #define PCL_SEARCH_ORGANIZED_NEIGHBOR_SEARCH_H_
42 
43 #include <pcl/point_cloud.h>
44 #include <pcl/point_types.h>
45 #include <pcl/search/search.h>
46 #include <pcl/common/eigen.h>
47 
48 #include <algorithm>
49 #include <queue>
50 #include <vector>
51 #include <pcl/common/projection_matrix.h>
52 
53 namespace pcl
54 {
55  namespace search
56  {
57  /** \brief OrganizedNeighbor is a class for optimized nearest neigbhor search in organized point clouds.
58  * \author Radu B. Rusu, Julius Kammerl, Suat Gedikli, Koen Buys
59  * \ingroup search
60  */
61  template<typename PointT>
62  class OrganizedNeighbor : public pcl::search::Search<PointT>
63  {
64 
65  public:
66  // public typedefs
68  typedef boost::shared_ptr<PointCloud> PointCloudPtr;
69 
70  typedef boost::shared_ptr<const PointCloud> PointCloudConstPtr;
71  typedef boost::shared_ptr<const std::vector<int> > IndicesConstPtr;
72 
73  typedef boost::shared_ptr<pcl::search::OrganizedNeighbor<PointT> > Ptr;
74  typedef boost::shared_ptr<const pcl::search::OrganizedNeighbor<PointT> > ConstPtr;
75 
79 
80  /** \brief Constructor
81  * \param[in] sorted_results whether the results should be return sorted in ascending order on the distances or not.
82  * This applies only for radius search, since knn always returns sorted resutls
83  * \param[in] eps the threshold for the mean-squared-error of the estimation of the projection matrix.
84  * if the MSE is above this value, the point cloud is considered as not from a projective device,
85  * thus organized neighbor search can not be applied on that cloud.
86  * \param[in] pyramid_level the level of the down sampled point cloud to be used for projection matrix estimation
87  */
88  OrganizedNeighbor (bool sorted_results = false, float eps = 1e-4f, unsigned pyramid_level = 5)
89  : Search<PointT> ("OrganizedNeighbor", sorted_results)
90  , projection_matrix_ (Eigen::Matrix<float, 3, 4, Eigen::RowMajor>::Zero ())
91  , KR_ (Eigen::Matrix<float, 3, 3, Eigen::RowMajor>::Zero ())
92  , KR_KRT_ (Eigen::Matrix<float, 3, 3, Eigen::RowMajor>::Zero ())
93  , eps_ (eps)
94  , pyramid_level_ (pyramid_level)
95  , mask_ ()
96  {
97  }
98 
99  /** \brief Empty deconstructor. */
100  virtual ~OrganizedNeighbor () {}
101 
102  /** \brief Test whether this search-object is valid (input is organized AND from projective device)
103  * User should use this method after setting the input cloud, since setInput just prints an error
104  * if input is not organized or a projection matrix could not be determined.
105  * \return true if the input data is organized and from a projective device, false otherwise
106  */
107  bool
108  isValid () const
109  {
110  // determinant (KR) = determinant (K) * determinant (R) = determinant (K) = f_x * f_y.
111  // If we expect at max an opening angle of 170degree in x-direction -> f_x = 2.0 * width / tan (85 degree);
112  // 2 * tan (85 degree) ~ 22.86
113  float min_f = 0.043744332f * static_cast<float>(input_->width);
114  //std::cout << "isValid: " << determinant3x3Matrix<Eigen::Matrix3f> (KR_ / sqrt (KR_KRT_.coeff (8))) << " >= " << (min_f * min_f) << std::endl;
115  return (determinant3x3Matrix<Eigen::Matrix3f> (KR_ / std::sqrt (KR_KRT_.coeff (8))) >= (min_f * min_f));
116  }
117 
118  /** \brief Compute the camera matrix
119  * \param[out] camera_matrix the resultant computed camera matrix
120  */
121  void
122  computeCameraMatrix (Eigen::Matrix3f& camera_matrix) const;
123 
124  /** \brief Provide a pointer to the input data set, if user has focal length he must set it before calling this
125  * \param[in] cloud the const boost shared pointer to a PointCloud message
126  * \param[in] indices the const boost shared pointer to PointIndices
127  */
128  virtual void
129  setInputCloud (const PointCloudConstPtr& cloud, const IndicesConstPtr &indices = IndicesConstPtr ())
130  {
131  input_ = cloud;
132 
133  mask_.resize (input_->size ());
134  input_ = cloud;
135  indices_ = indices;
136 
137  if (indices_.get () != NULL && indices_->size () != 0)
138  {
139  mask_.assign (input_->size (), 0);
140  for (std::vector<int>::const_iterator iIt = indices_->begin (); iIt != indices_->end (); ++iIt)
141  mask_[*iIt] = 1;
142  }
143  else
144  mask_.assign (input_->size (), 1);
145 
147  }
148 
149  /** \brief Search for all neighbors of query point that are within a given radius.
150  * \param[in] p_q the given query point
151  * \param[in] radius the radius of the sphere bounding all of p_q's neighbors
152  * \param[out] k_indices the resultant indices of the neighboring points
153  * \param[out] k_sqr_distances the resultant squared distances to the neighboring points
154  * \param[in] max_nn if given, bounds the maximum returned neighbors to this value. If \a max_nn is set to
155  * 0 or to a number higher than the number of points in the input cloud, all neighbors in \a radius will be
156  * returned.
157  * \return number of neighbors found in radius
158  */
159  int
160  radiusSearch (const PointT &p_q,
161  double radius,
162  std::vector<int> &k_indices,
163  std::vector<float> &k_sqr_distances,
164  unsigned int max_nn = 0) const;
165 
166  /** \brief estimated the projection matrix from the input cloud. */
167  void
169 
170  /** \brief Search for the k-nearest neighbors for a given query point.
171  * \note limiting the maximum search radius (with setMaxDistance) can lead to a significant improvement in search speed
172  * \param[in] p_q the given query point (\ref setInputCloud must be given a-priori!)
173  * \param[in] k the number of neighbors to search for (used only if horizontal and vertical window not given already!)
174  * \param[out] k_indices the resultant point indices (must be resized to \a k beforehand!)
175  * \param[out] k_sqr_distances \note this function does not return distances
176  * \return number of neighbors found
177  * @todo still need to implements this functionality
178  */
179  int
180  nearestKSearch (const PointT &p_q,
181  int k,
182  std::vector<int> &k_indices,
183  std::vector<float> &k_sqr_distances) const;
184 
185  /** \brief projects a point into the image
186  * \param[in] p point in 3D World Coordinate Frame to be projected onto the image plane
187  * \param[out] q the 2D projected point in pixel coordinates (u,v)
188  * @return true if projection is valid, false otherwise
189  */
190  bool projectPoint (const PointT& p, pcl::PointXY& q) const;
191 
192  protected:
193 
194  struct Entry
195  {
196  Entry (int idx, float dist) : index (idx), distance (dist) {}
197  Entry () : index (0), distance (0) {}
198  unsigned index;
199  float distance;
200 
201  inline bool
202  operator < (const Entry& other) const
203  {
204  return (distance < other.distance);
205  }
206  };
207 
208  /** \brief test if point given by index is among the k NN in results to the query point.
209  * \param[in] query query point
210  * \param[in] k number of maximum nn interested in
211  * \param[in,out] queue priority queue with k NN
212  * \param[in] index index on point to be tested
213  * \return whether the top element changed or not.
214  */
215  inline bool
216  testPoint (const PointT& query, unsigned k, std::priority_queue<Entry>& queue, unsigned index) const
217  {
218  const PointT& point = input_->points [index];
219  if (mask_ [index] && pcl_isfinite (point.x))
220  {
221  //float squared_distance = (point.getVector3fMap () - query.getVector3fMap ()).squaredNorm ();
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  {
228  queue.push (Entry (index, squared_distance));
229  return queue.size () == k;
230  }
231  else if (queue.top ().distance > squared_distance)
232  {
233  queue.pop ();
234  queue.push (Entry (index, squared_distance));
235  return true; // top element has changed!
236  }
237  }
238  return false;
239  }
240 
241  inline void
242  clipRange (int& begin, int &end, int min, int max) const
243  {
244  begin = std::max (std::min (begin, max), min);
245  end = std::min (std::max (end, min), max);
246  }
247 
248  /** \brief Obtain a search box in 2D from a sphere with a radius in 3D
249  * \param[in] point the query point (sphere center)
250  * \param[in] squared_radius the squared sphere radius
251  * \param[out] minX the min X box coordinate
252  * \param[out] minY the min Y box coordinate
253  * \param[out] maxX the max X box coordinate
254  * \param[out] maxY the max Y box coordinate
255  */
256  void
257  getProjectedRadiusSearchBox (const PointT& point, float squared_radius, unsigned& minX, unsigned& minY,
258  unsigned& maxX, unsigned& maxY) const;
259 
260 
261  /** \brief the projection matrix. Either set by user or calculated by the first / each input cloud */
262  Eigen::Matrix<float, 3, 4, Eigen::RowMajor> projection_matrix_;
263 
264  /** \brief inveser of the left 3x3 projection matrix which is K * R (with K being the camera matrix and R the rotation matrix)*/
265  Eigen::Matrix<float, 3, 3, Eigen::RowMajor> KR_;
266 
267  /** \brief inveser of the left 3x3 projection matrix which is K * R (with K being the camera matrix and R the rotation matrix)*/
268  Eigen::Matrix<float, 3, 3, Eigen::RowMajor> KR_KRT_;
269 
270  /** \brief epsilon value for the MSE of the projection matrix estimation*/
271  const float eps_;
272 
273  /** \brief using only a subsample of points to calculate the projection matrix. pyramid_level_ = use down sampled cloud given by pyramid_level_*/
274  const unsigned pyramid_level_;
275 
276  /** \brief mask, indicating whether the point was in the indices list or not.*/
277  std::vector<unsigned char> mask_;
278  public:
279  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
280  };
281  }
282 }
283 
284 #ifdef PCL_NO_PRECOMPILE
285 #include <pcl/search/impl/organized.hpp>
286 #endif
287 
288 #endif
289 
bool projectPoint(const PointT &p, pcl::PointXY &q) const
projects a point into the image
Definition: organized.hpp:382
Eigen::Matrix< float, 3, 3, Eigen::RowMajor > KR_KRT_
inveser of the left 3x3 projection matrix which is K * R (with K being the camera matrix and R the ro...
Definition: organized.h:268
This file defines compatibility wrappers for low level I/O functions.
Definition: convolution.h:45
bool testPoint(const PointT &query, unsigned k, std::priority_queue< Entry > &queue, unsigned index) const
test if point given by index is among the k NN in results to the query point.
Definition: organized.h:216
PointCloudConstPtr input_
Definition: search.h:403
virtual ~OrganizedNeighbor()
Empty deconstructor.
Definition: organized.h:100
void estimateProjectionMatrix()
estimated the projection matrix from the input cloud.
Definition: organized.hpp:337
Eigen::Matrix< float, 3, 3, Eigen::RowMajor > KR_
inveser of the left 3x3 projection matrix which is K * R (with K being the camera matrix and R the ro...
Definition: organized.h:265
int radiusSearch(const PointT &p_q, double radius, std::vector< int > &k_indices, std::vector< float > &k_sqr_distances, unsigned int max_nn=0) const
Search for all neighbors of query point that are within a given radius.
Definition: organized.hpp:50
Definition: bfgs.h:10
boost::shared_ptr< pcl::search::OrganizedNeighbor< PointT > > Ptr
Definition: organized.h:73
boost::shared_ptr< PointCloud > PointCloudPtr
Definition: organized.h:68
A 2D point structure representing Euclidean xy coordinates.
boost::shared_ptr< const PointCloud > PointCloudConstPtr
Definition: organized.h:70
bool isValid() const
Test whether this search-object is valid (input is organized AND from projective device) User should ...
Definition: organized.h:108
IndicesConstPtr indices_
Definition: search.h:404
const unsigned pyramid_level_
using only a subsample of points to calculate the projection matrix.
Definition: organized.h:274
std::vector< unsigned char > mask_
mask, indicating whether the point was in the indices list or not.
Definition: organized.h:277
Entry()
Definition: organized.h:197
void computeCameraMatrix(Eigen::Matrix3f &camera_matrix) const
Compute the camera matrix.
Definition: organized.hpp:330
boost::shared_ptr< const pcl::search::OrganizedNeighbor< PointT > > ConstPtr
Definition: organized.h:74
OrganizedNeighbor(bool sorted_results=false, float eps=1e-4f, unsigned pyramid_level=5)
Constructor.
Definition: organized.h:88
Eigen::Matrix< float, 3, 4, Eigen::RowMajor > projection_matrix_
the projection matrix.
Definition: organized.h:262
const float eps_
epsilon value for the MSE of the projection matrix estimation
Definition: organized.h:271
void clipRange(int &begin, int &end, int min, int max) const
Definition: organized.h:242
PointCloud represents the base class in PCL for storing collections of 3D points. ...
unsigned index
Definition: organized.h:198
void getProjectedRadiusSearchBox(const PointT &point, float squared_radius, unsigned &minX, unsigned &minY, unsigned &maxX, unsigned &maxY) const
Obtain a search box in 2D from a sphere with a radius in 3D.
Definition: organized.hpp:273
virtual void setInputCloud(const PointCloudConstPtr &cloud, const IndicesConstPtr &indices=IndicesConstPtr())
Provide a pointer to the input data set, if user has focal length he must set it before calling this...
Definition: organized.h:129
OrganizedNeighbor is a class for optimized nearest neigbhor search in organized point clouds...
Definition: organized.h:62
bool operator<(const Entry &other) const
Definition: organized.h:202
A point structure representing Euclidean xyz coordinates, and the RGB color.
int nearestKSearch(const PointT &p_q, int k, std::vector< int > &k_indices, std::vector< float > &k_sqr_distances) const
Search for the k-nearest neighbors for a given query point.
Definition: organized.hpp:117
boost::shared_ptr< const std::vector< int > > IndicesConstPtr
Definition: organized.h:71
float distance
Definition: organized.h:199
Entry(int idx, float dist)
Definition: organized.h:196
Definition: organized.h:194
pcl::PointCloud< PointT > PointCloud
Definition: organized.h:67
Generic search class.
Definition: search.h:74