Point Cloud Library (PCL)  1.9.1-dev
organized.hpp
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_IMPL_ORGANIZED_NEIGHBOR_SEARCH_H_
41 #define PCL_SEARCH_IMPL_ORGANIZED_NEIGHBOR_SEARCH_H_
42 
43 #include <pcl/search/organized.h>
44 #include <pcl/common/eigen.h>
45 #include <pcl/common/time.h>
46 #include <Eigen/Eigenvalues>
47 
48 //////////////////////////////////////////////////////////////////////////////////////////////
49 template<typename PointT> int
51  const double radius,
52  std::vector<int> &k_indices,
53  std::vector<float> &k_sqr_distances,
54  unsigned int max_nn) const
55 {
56  // NAN test
57  assert (isFinite (query) && "Invalid (NaN, Inf) point coordinates given to nearestKSearch!");
58 
59  // search window
60  unsigned left, right, top, bottom;
61  //unsigned x, y, idx;
62  float squared_distance;
63  double squared_radius;
64 
65  k_indices.clear ();
66  k_sqr_distances.clear ();
67 
68  squared_radius = radius * radius;
69 
70  this->getProjectedRadiusSearchBox (query, static_cast<float> (squared_radius), left, right, top, bottom);
71 
72  // iterate over search box
73  if (max_nn == 0 || max_nn >= static_cast<unsigned int> (input_->points.size ()))
74  max_nn = static_cast<unsigned int> (input_->points.size ());
75 
76  k_indices.reserve (max_nn);
77  k_sqr_distances.reserve (max_nn);
78 
79  unsigned yEnd = (bottom + 1) * input_->width + right + 1;
80  unsigned idx = top * input_->width + left;
81  unsigned skip = input_->width - right + left - 1;
82  unsigned xEnd = idx - left + right + 1;
83 
84  for (; xEnd != yEnd; idx += skip, xEnd += input_->width)
85  {
86  for (; idx < xEnd; ++idx)
87  {
88  if (!mask_[idx] || !isFinite (input_->points[idx]))
89  continue;
90 
91  float dist_x = input_->points[idx].x - query.x;
92  float dist_y = input_->points[idx].y - query.y;
93  float dist_z = input_->points[idx].z - query.z;
94  squared_distance = dist_x * dist_x + dist_y * dist_y + dist_z * dist_z;
95  //squared_distance = (input_->points[idx].getVector3fMap () - query.getVector3fMap ()).squaredNorm ();
96  if (squared_distance <= squared_radius)
97  {
98  k_indices.push_back (idx);
99  k_sqr_distances.push_back (squared_distance);
100  // already done ?
101  if (k_indices.size () == max_nn)
102  {
103  if (sorted_results_)
104  this->sortResults (k_indices, k_sqr_distances);
105  return (max_nn);
106  }
107  }
108  }
109  }
110  if (sorted_results_)
111  this->sortResults (k_indices, k_sqr_distances);
112  return (static_cast<int> (k_indices.size ()));
113 }
114 
115 //////////////////////////////////////////////////////////////////////////////////////////////
116 template<typename PointT> int
118  int k,
119  std::vector<int> &k_indices,
120  std::vector<float> &k_sqr_distances) const
121 {
122  assert (isFinite (query) && "Invalid (NaN, Inf) point coordinates given to nearestKSearch!");
123  if (k < 1)
124  {
125  k_indices.clear ();
126  k_sqr_distances.clear ();
127  return (0);
128  }
129 
130  Eigen::Vector3f queryvec (query.x, query.y, query.z);
131  // project query point on the image plane
132  //Eigen::Vector3f q = KR_ * query.getVector3fMap () + projection_matrix_.block <3, 1> (0, 3);
133  Eigen::Vector3f q (KR_ * queryvec + projection_matrix_.block <3, 1> (0, 3));
134  int xBegin = int(q [0] / q [2] + 0.5f);
135  int yBegin = int(q [1] / q [2] + 0.5f);
136  int xEnd = xBegin + 1; // end is the pixel that is not used anymore, like in iterators
137  int yEnd = yBegin + 1;
138 
139  // the search window. This is supposed to shrink within the iterations
140  unsigned left = 0;
141  unsigned right = input_->width - 1;
142  unsigned top = 0;
143  unsigned bottom = input_->height - 1;
144 
145  std::priority_queue <Entry> results;
146  //std::vector<Entry> k_results;
147  //k_results.reserve (k);
148  // add point laying on the projection of the query point.
149  if (xBegin >= 0 &&
150  xBegin < static_cast<int> (input_->width) &&
151  yBegin >= 0 &&
152  yBegin < static_cast<int> (input_->height))
153  testPoint (query, k, results, yBegin * input_->width + xBegin);
154  else // point lys
155  {
156  // find the box that touches the image border -> don't waste time evaluating boxes that are completely outside the image!
157  int dist = std::numeric_limits<int>::max ();
158 
159  if (xBegin < 0)
160  dist = -xBegin;
161  else if (xBegin >= static_cast<int> (input_->width))
162  dist = xBegin - static_cast<int> (input_->width);
163 
164  if (yBegin < 0)
165  dist = std::min (dist, -yBegin);
166  else if (yBegin >= static_cast<int> (input_->height))
167  dist = std::min (dist, yBegin - static_cast<int> (input_->height));
168 
169  xBegin -= dist;
170  xEnd += dist;
171 
172  yBegin -= dist;
173  yEnd += dist;
174  }
175 
176 
177  // stop used as isChanged as well as stop.
178  bool stop = false;
179  do
180  {
181  // increment box size
182  --xBegin;
183  ++xEnd;
184  --yBegin;
185  ++yEnd;
186 
187  // the range in x-direction which intersects with the image width
188  int xFrom = xBegin;
189  int xTo = xEnd;
190  clipRange (xFrom, xTo, 0, input_->width);
191 
192  // if x-extend is not 0
193  if (xTo > xFrom)
194  {
195  // if upper line of the rectangle is visible and x-extend is not 0
196  if (yBegin >= 0 && yBegin < static_cast<int> (input_->height))
197  {
198  int idx = yBegin * input_->width + xFrom;
199  int idxTo = idx + xTo - xFrom;
200  for (; idx < idxTo; ++idx)
201  stop = testPoint (query, k, results, idx) || stop;
202  }
203 
204 
205  // the row yEnd does NOT belong to the box -> last row = yEnd - 1
206  // if lower line of the rectangle is visible
207  if (yEnd > 0 && yEnd <= static_cast<int> (input_->height))
208  {
209  int idx = (yEnd - 1) * input_->width + xFrom;
210  int idxTo = idx + xTo - xFrom;
211 
212  for (; idx < idxTo; ++idx)
213  stop = testPoint (query, k, results, idx) || stop;
214  }
215 
216  // skip first row and last row (already handled above)
217  int yFrom = yBegin + 1;
218  int yTo = yEnd - 1;
219  clipRange (yFrom, yTo, 0, input_->height);
220 
221  // if we have lines in between that are also visible
222  if (yFrom < yTo)
223  {
224  if (xBegin >= 0 && xBegin < static_cast<int> (input_->width))
225  {
226  int idx = yFrom * input_->width + xBegin;
227  int idxTo = yTo * input_->width + xBegin;
228 
229  for (; idx < idxTo; idx += input_->width)
230  stop = testPoint (query, k, results, idx) || stop;
231  }
232 
233  if (xEnd > 0 && xEnd <= static_cast<int> (input_->width))
234  {
235  int idx = yFrom * input_->width + xEnd - 1;
236  int idxTo = yTo * input_->width + xEnd - 1;
237 
238  for (; idx < idxTo; idx += input_->width)
239  stop = testPoint (query, k, results, idx) || stop;
240  }
241 
242  }
243  // stop here means that the k-nearest neighbor changed -> recalculate bounding box of ellipse.
244  if (stop)
245  getProjectedRadiusSearchBox (query, results.top ().distance, left, right, top, bottom);
246 
247  }
248  // now we use it as stop flag -> if bounding box is completely within the already examined search box were done!
249  stop = (static_cast<int> (left) >= xBegin && static_cast<int> (left) < xEnd &&
250  static_cast<int> (right) >= xBegin && static_cast<int> (right) < xEnd &&
251  static_cast<int> (top) >= yBegin && static_cast<int> (top) < yEnd &&
252  static_cast<int> (bottom) >= yBegin && static_cast<int> (bottom) < yEnd);
253 
254  } while (!stop);
255 
256 
257  k_indices.resize (results.size ());
258  k_sqr_distances.resize (results.size ());
259  std::size_t idx = results.size () - 1;
260  while (!results.empty ())
261  {
262  k_indices [idx] = results.top ().index;
263  k_sqr_distances [idx] = results.top ().distance;
264  results.pop ();
265  --idx;
266  }
267 
268  return (static_cast<int> (k_indices.size ()));
269 }
270 
271 ////////////////////////////////////////////////////////////////////////////////////////////
272 template<typename PointT> void
274  float squared_radius,
275  unsigned &minX,
276  unsigned &maxX,
277  unsigned &minY,
278  unsigned &maxY) const
279 {
280  Eigen::Vector3f queryvec (point.x, point.y, point.z);
281  //Eigen::Vector3f q = KR_ * point.getVector3fMap () + projection_matrix_.block <3, 1> (0, 3);
282  Eigen::Vector3f q (KR_ * queryvec + projection_matrix_.block <3, 1> (0, 3));
283 
284  float a = squared_radius * KR_KRT_.coeff (8) - q [2] * q [2];
285  float b = squared_radius * KR_KRT_.coeff (7) - q [1] * q [2];
286  float c = squared_radius * KR_KRT_.coeff (4) - q [1] * q [1];
287  int min, max;
288  // a and c are multiplied by two already => - 4ac -> - ac
289  float det = b * b - a * c;
290  if (det < 0)
291  {
292  minY = 0;
293  maxY = input_->height - 1;
294  }
295  else
296  {
297  float y1 = static_cast<float> ((b - std::sqrt (det)) / a);
298  float y2 = static_cast<float> ((b + std::sqrt (det)) / a);
299 
300  min = std::min (static_cast<int> (std::floor (y1)), static_cast<int> (std::floor (y2)));
301  max = std::max (static_cast<int> (std::ceil (y1)), static_cast<int> (std::ceil (y2)));
302  minY = static_cast<unsigned> (std::min (static_cast<int> (input_->height) - 1, std::max (0, min)));
303  maxY = static_cast<unsigned> (std::max (std::min (static_cast<int> (input_->height) - 1, max), 0));
304  }
305 
306  b = squared_radius * KR_KRT_.coeff (6) - q [0] * q [2];
307  c = squared_radius * KR_KRT_.coeff (0) - q [0] * q [0];
308 
309  det = b * b - a * c;
310  if (det < 0)
311  {
312  minX = 0;
313  maxX = input_->width - 1;
314  }
315  else
316  {
317  float x1 = static_cast<float> ((b - std::sqrt (det)) / a);
318  float x2 = static_cast<float> ((b + std::sqrt (det)) / a);
319 
320  min = std::min (static_cast<int> (std::floor (x1)), static_cast<int> (std::floor (x2)));
321  max = std::max (static_cast<int> (std::ceil (x1)), static_cast<int> (std::ceil (x2)));
322  minX = static_cast<unsigned> (std::min (static_cast<int> (input_->width)- 1, std::max (0, min)));
323  maxX = static_cast<unsigned> (std::max (std::min (static_cast<int> (input_->width) - 1, max), 0));
324  }
325 }
326 
327 
328 //////////////////////////////////////////////////////////////////////////////////////////////
329 template<typename PointT> void
331 {
332  pcl::getCameraMatrixFromProjectionMatrix (projection_matrix_, camera_matrix);
333 }
334 
335 //////////////////////////////////////////////////////////////////////////////////////////////
336 template<typename PointT> void
338 {
339  // internally we calculate with double but store the result into float matrices.
340  projection_matrix_.setZero ();
341  if (input_->height == 1 || input_->width == 1)
342  {
343  PCL_ERROR ("[pcl::%s::estimateProjectionMatrix] Input dataset is not organized!\n", this->getName ().c_str ());
344  return;
345  }
346 
347  const unsigned ySkip = (std::max) (input_->height >> pyramid_level_, unsigned (1));
348  const unsigned xSkip = (std::max) (input_->width >> pyramid_level_, unsigned (1));
349 
350  std::vector<int> indices;
351  indices.reserve (input_->size () >> (pyramid_level_ << 1));
352 
353  for (unsigned yIdx = 0, idx = 0; yIdx < input_->height; yIdx += ySkip, idx += input_->width * ySkip)
354  {
355  for (unsigned xIdx = 0, idx2 = idx; xIdx < input_->width; xIdx += xSkip, idx2 += xSkip)
356  {
357  if (!mask_ [idx2])
358  continue;
359 
360  indices.push_back (idx2);
361  }
362  }
363 
364  double residual_sqr = pcl::estimateProjectionMatrix<PointT> (input_, projection_matrix_, indices);
365 
366  if (std::abs (residual_sqr) > eps_ * float (indices.size ()))
367  {
368  PCL_ERROR ("[pcl::%s::radiusSearch] Input dataset is not from a projective device!\nResidual (MSE) %f, using %d valid points\n", this->getName ().c_str (), residual_sqr / double (indices.size()), indices.size ());
369  return;
370  }
371 
372  // get left 3x3 sub matrix, which contains K * R, with K = camera matrix = [[fx s cx] [0 fy cy] [0 0 1]]
373  // and R being the rotation matrix
374  KR_ = projection_matrix_.topLeftCorner <3, 3> ();
375 
376  // precalculate KR * KR^T needed by calculations during nn-search
377  KR_KRT_ = KR_ * KR_.transpose ();
378 }
379 
380 //////////////////////////////////////////////////////////////////////////////////////////////
381 template<typename PointT> bool
383 {
384  Eigen::Vector3f projected = KR_ * point.getVector3fMap () + projection_matrix_.block <3, 1> (0, 3);
385  q.x = projected [0] / projected [2];
386  q.y = projected [1] / projected [2];
387  return (projected[2] != 0);
388 }
389 #define PCL_INSTANTIATE_OrganizedNeighbor(T) template class PCL_EXPORTS pcl::search::OrganizedNeighbor<T>;
390 
391 #endif
bool isFinite(const PointT &pt)
Tests if the 3D components of a point are all finite param[in] pt point to be tested return true if f...
Definition: point_tests.h:55
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
bool projectPoint(const PointT &p, pcl::PointXY &q) const
projects a point into the image
Definition: organized.hpp:382
void estimateProjectionMatrix()
estimated the projection matrix from the input cloud.
Definition: organized.hpp:337
A 2D point structure representing Euclidean xy coordinates.
void computeCameraMatrix(Eigen::Matrix3f &camera_matrix) const
Compute the camera matrix.
Definition: organized.hpp:330
A point structure representing Euclidean xyz coordinates, and the RGB color.
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 override
Search for all neighbors of query point that are within a given radius.
Definition: organized.hpp:50
int nearestKSearch(const PointT &p_q, int k, std::vector< int > &k_indices, std::vector< float > &k_sqr_distances) const override
Search for the k-nearest neighbors for a given query point.
Definition: organized.hpp:117
Define methods for measuring time spent in code blocks.
PCL_EXPORTS void getCameraMatrixFromProjectionMatrix(const Eigen::Matrix< float, 3, 4, Eigen::RowMajor > &projection_matrix, Eigen::Matrix3f &camera_matrix)
Determines the camera matrix from the given projection matrix.