Point Cloud Library (PCL)  1.6.0
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Groups Pages
organized.hpp
Go to the documentation of this file.
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 Willow Garage, Inc. 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: organized.hpp 6124 2012-07-03 19:04:27Z aichim $
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 
49 template<typename PointT> int
50 pcl::search::OrganizedNeighbor<PointT>::radiusSearch (const PointT &query,
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  register 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  squared_distance = (input_->points[idx].getVector3fMap () - query.getVector3fMap ()).squaredNorm ();
92  if (squared_distance <= squared_radius)
93  {
94  k_indices.push_back (idx);
95  k_sqr_distances.push_back (squared_distance);
96  // already done ?
97  if (k_indices.size () == max_nn)
98  {
99  if (sorted_results_)
100  this->sortResults (k_indices, k_sqr_distances);
101  return (max_nn);
102  }
103  }
104  }
105  }
106  if (sorted_results_)
107  this->sortResults (k_indices, k_sqr_distances);
108  return (static_cast<int> (k_indices.size ()));
109 }
110 
112 template<typename PointT> int
113 pcl::search::OrganizedNeighbor<PointT>::nearestKSearch (const PointT &query,
114  int k,
115  std::vector<int> &k_indices,
116  std::vector<float> &k_sqr_distances) const
117 {
118  assert (isFinite (query) && "Invalid (NaN, Inf) point coordinates given to nearestKSearch!");
119  if (k < 1)
120  {
121  k_indices.clear ();
122  k_sqr_distances.clear ();
123  return (0);
124  }
125 
126  // project query point on the image plane
127  Eigen::Vector3f q = KR_ * query.getVector3fMap () + projection_matrix_.block <3, 1> (0, 3);
128  int xBegin = int(q [0] / q [2] + 0.5f);
129  int yBegin = int(q [1] / q [2] + 0.5f);
130  int xEnd = xBegin + 1; // end is the pixel that is not used anymore, like in iterators
131  int yEnd = yBegin + 1;
132 
133  // the search window. This is supposed to shrink within the iterations
134  unsigned left = 0;
135  unsigned right = input_->width - 1;
136  unsigned top = 0;
137  unsigned bottom = input_->height - 1;
138 
139  std::priority_queue <Entry> results;
140  //std::vector<Entry> k_results;
141  //k_results.reserve (k);
142  // add point laying on the projection of the query point.
143  if (xBegin >= 0 &&
144  xBegin < static_cast<int> (input_->width) &&
145  yBegin >= 0 &&
146  yBegin < static_cast<int> (input_->height))
147  testPoint (query, k, results, yBegin * input_->width + xBegin);
148  else // point lys
149  {
150  // find the box that touches the image border -> dont waste time evaluating boxes that are completely outside the image!
151  int dist = std::numeric_limits<int>::max ();
152 
153  if (xBegin < 0)
154  dist = -xBegin;
155  else if (xBegin >= static_cast<int> (input_->width))
156  dist = xBegin - static_cast<int> (input_->width);
157 
158  if (yBegin < 0)
159  dist = std::min (dist, -yBegin);
160  else if (yBegin >= static_cast<int> (input_->height))
161  dist = std::min (dist, yBegin - static_cast<int> (input_->height));
162 
163  xBegin -= dist;
164  xEnd += dist;
165 
166  yBegin -= dist;
167  yEnd += dist;
168  }
169 
170 
171  // stop used as isChanged as well as stop.
172  bool stop = false;
173  do
174  {
175  // increment box size
176  --xBegin;
177  ++xEnd;
178  --yBegin;
179  ++yEnd;
180 
181  // the range in x-direction which intersects with the image width
182  int xFrom = xBegin;
183  int xTo = xEnd;
184  clipRange (xFrom, xTo, 0, input_->width);
185 
186  // if x-extend is not 0
187  if (xTo > xFrom)
188  {
189  // if upper line of the rectangle is visible and x-extend is not 0
190  if (yBegin >= 0 && yBegin < static_cast<int> (input_->height))
191  {
192  int idx = yBegin * input_->width + xFrom;
193  int idxTo = idx + xTo - xFrom;
194  for (; idx < idxTo; ++idx)
195  stop = testPoint (query, k, results, idx) || stop;
196  }
197 
198 
199  // the row yEnd does NOT belong to the box -> last row = yEnd - 1
200  // if lower line of the rectangle is visible
201  if (yEnd > 0 && yEnd <= static_cast<int> (input_->height))
202  {
203  int idx = (yEnd - 1) * input_->width + xFrom;
204  int idxTo = idx + xTo - xFrom;
205 
206  for (; idx < idxTo; ++idx)
207  stop = testPoint (query, k, results, idx) || stop;
208  }
209 
210  // skip first row and last row (already handled above)
211  int yFrom = yBegin + 1;
212  int yTo = yEnd - 1;
213  clipRange (yFrom, yTo, 0, input_->height);
214 
215  // if we have lines in between that are also visible
216  if (yFrom < yTo)
217  {
218  if (xBegin >= 0 && xBegin < static_cast<int> (input_->width))
219  {
220  int idx = yFrom * input_->width + xBegin;
221  int idxTo = yTo * input_->width + xBegin;
222 
223  for (; idx < idxTo; idx += input_->width)
224  stop = testPoint (query, k, results, idx) || stop;
225  }
226 
227  if (xEnd > 0 && xEnd <= static_cast<int> (input_->width))
228  {
229  int idx = yFrom * input_->width + xEnd - 1;
230  int idxTo = yTo * input_->width + xEnd - 1;
231 
232  for (; idx < idxTo; idx += input_->width)
233  stop = testPoint (query, k, results, idx) || stop;
234  }
235 
236  }
237  // stop here means that the k-nearest neighbor changed -> recalculate bounding box of ellipse.
238  if (stop)
239  getProjectedRadiusSearchBox (query, results.top ().distance, left, right, top, bottom);
240 
241  }
242  // now we use it as stop flag -> if bounding box is completely within the already examined search box were done!
243  stop = (static_cast<int> (left) >= xBegin && static_cast<int> (left) < xEnd &&
244  static_cast<int> (right) >= xBegin && static_cast<int> (right) < xEnd &&
245  static_cast<int> (top) >= yBegin && static_cast<int> (top) < yEnd &&
246  static_cast<int> (bottom) >= yBegin && static_cast<int> (bottom) < yEnd);
247 
248  } while (!stop);
249 
250 
251  k_indices.resize (results.size ());
252  k_sqr_distances.resize (results.size ());
253  size_t idx = results.size () - 1;
254  while (!results.empty ())
255  {
256  k_indices [idx] = results.top ().index;
257  k_sqr_distances [idx] = results.top ().distance;
258  results.pop ();
259  --idx;
260  }
261 
262  return (static_cast<int> (k_indices.size ()));
263 }
264 
266 template<typename PointT> void
267 pcl::search::OrganizedNeighbor<PointT>::getProjectedRadiusSearchBox (const PointT& point,
268  float squared_radius,
269  unsigned &minX,
270  unsigned &maxX,
271  unsigned &minY,
272  unsigned &maxY) const
273 {
274  Eigen::Vector3f q = KR_ * point.getVector3fMap () + projection_matrix_.block <3, 1> (0, 3);
275 
276  float a = squared_radius * KR_KRT_.coeff (8) - q [2] * q [2];
277  float b = squared_radius * KR_KRT_.coeff (7) - q [1] * q [2];
278  float c = squared_radius * KR_KRT_.coeff (4) - q [1] * q [1];
279  int min, max;
280  // a and c are multiplied by two already => - 4ac -> - ac
281  float det = b * b - a * c;
282  if (det < 0)
283  {
284  minY = 0;
285  maxY = input_->height - 1;
286  }
287  else
288  {
289  float y1 = (b - sqrt (det)) / a;
290  float y2 = (b + sqrt (det)) / a;
291 
292  min = std::min (static_cast<int> (floor (y1)), static_cast<int> (floor (y2)));
293  max = std::max (static_cast<int> (ceil (y1)), static_cast<int> (ceil (y2)));
294  minY = static_cast<unsigned> (std::min (static_cast<int> (input_->height) - 1, std::max (0, min)));
295  maxY = static_cast<unsigned> (std::max (std::min (static_cast<int> (input_->height) - 1, max), 0));
296  }
297 
298  b = squared_radius * KR_KRT_.coeff (6) - q [0] * q [2];
299  c = squared_radius * KR_KRT_.coeff (0) - q [0] * q [0];
300 
301  det = b * b - a * c;
302  if (det < 0)
303  {
304  minX = 0;
305  maxX = input_->width - 1;
306  }
307  else
308  {
309  float x1 = (b - sqrt (det)) / a;
310  float x2 = (b + sqrt (det)) / a;
311 
312  min = std::min (static_cast<int> (floor (x1)), static_cast<int> (floor (x2)));
313  max = std::max (static_cast<int> (ceil (x1)), static_cast<int> (ceil (x2)));
314  minX = static_cast<unsigned> (std::min (static_cast<int> (input_->width)- 1, std::max (0, min)));
315  maxX = static_cast<unsigned> (std::max (std::min (static_cast<int> (input_->width) - 1, max), 0));
316  }
317 }
318 
320 template<typename PointT> template <typename MatrixType> void
321 pcl::search::OrganizedNeighbor<PointT>::makeSymmetric (MatrixType& matrix, bool use_upper_triangular) const
322 {
323  if (use_upper_triangular && (MatrixType::Flags & Eigen::RowMajorBit) )
324  {
325  matrix.coeffRef (4) = matrix.coeff (1);
326  matrix.coeffRef (8) = matrix.coeff (2);
327  matrix.coeffRef (9) = matrix.coeff (6);
328  matrix.coeffRef (12) = matrix.coeff (3);
329  matrix.coeffRef (13) = matrix.coeff (7);
330  matrix.coeffRef (14) = matrix.coeff (11);
331  }
332  else
333  {
334  matrix.coeffRef (1) = matrix.coeff (4);
335  matrix.coeffRef (2) = matrix.coeff (8);
336  matrix.coeffRef (6) = matrix.coeff (9);
337  matrix.coeffRef (3) = matrix.coeff (12);
338  matrix.coeffRef (7) = matrix.coeff (13);
339  matrix.coeffRef (11) = matrix.coeff (14);
340  }
341 }
342 
344 template<typename PointT> void
345 pcl::search::OrganizedNeighbor<PointT>::computeCameraMatrix (Eigen::Matrix3f& camera_matrix) const
346 {
347  Eigen::Matrix3f cam = KR_KRT_ / KR_KRT_.coeff (8);
348 
349  memset (&(camera_matrix.coeffRef (0)), 0, sizeof (Eigen::Matrix3f::Scalar) * 9);
350  camera_matrix.coeffRef (8) = 1.0;
351 
352  if (camera_matrix.Flags & Eigen::RowMajorBit)
353  {
354  camera_matrix.coeffRef (2) = cam.coeff (2);
355  camera_matrix.coeffRef (5) = cam.coeff (5);
356  camera_matrix.coeffRef (4) = sqrt (cam.coeff (4) - cam.coeff (5) * cam.coeff (5));
357  camera_matrix.coeffRef (1) = (cam.coeff (1) - cam.coeff (2) * cam.coeff (5)) / camera_matrix.coeff (4);
358  camera_matrix.coeffRef (0) = sqrt (cam.coeff (0) - camera_matrix.coeff (1) * camera_matrix.coeff (1) - cam.coeff (2) * cam.coeff (2));
359  }
360  else
361  {
362  camera_matrix.coeffRef (6) = cam.coeff (2);
363  camera_matrix.coeffRef (7) = cam.coeff (5);
364  camera_matrix.coeffRef (4) = sqrt (cam.coeff (4) - cam.coeff (5) * cam.coeff (5));
365  camera_matrix.coeffRef (3) = (cam.coeff (1) - cam.coeff (2) * cam.coeff (5)) / camera_matrix.coeff (4);
366  camera_matrix.coeffRef (0) = sqrt (cam.coeff (0) - camera_matrix.coeff (3) * camera_matrix.coeff (3) - cam.coeff (2) * cam.coeff (2));
367  }
368 }
369 
371 template<typename PointT> void
372 pcl::search::OrganizedNeighbor<PointT>::estimateProjectionMatrix ()
373 {
374  // internally we calculate with double but store the result into float matrices.
375  typedef double Scalar;
376  projection_matrix_.setZero ();
377  if (input_->height == 1 || input_->width == 1)
378  {
379  PCL_ERROR ("[pcl::%s::estimateProjectionMatrix] Input dataset is not organized!\n", this->getName ().c_str ());
380  return;
381  }
382 
383  const unsigned ySkip = (input_->height >> pyramid_level_);
384  const unsigned xSkip = (input_->width >> pyramid_level_);
385  Eigen::Matrix<Scalar, 4, 4, Eigen::RowMajor> A = Eigen::Matrix<Scalar, 4, 4, Eigen::RowMajor>::Zero ();
386  Eigen::Matrix<Scalar, 4, 4, Eigen::RowMajor> B = Eigen::Matrix<Scalar, 4, 4, Eigen::RowMajor>::Zero ();
387  Eigen::Matrix<Scalar, 4, 4, Eigen::RowMajor> C = Eigen::Matrix<Scalar, 4, 4, Eigen::RowMajor>::Zero ();
388  Eigen::Matrix<Scalar, 4, 4, Eigen::RowMajor> D = Eigen::Matrix<Scalar, 4, 4, Eigen::RowMajor>::Zero ();
389 
390  for (unsigned yIdx = 0, idx = 0; yIdx < input_->height; yIdx += ySkip, idx += input_->width * (ySkip - 1))
391  {
392  for (unsigned xIdx = 0; xIdx < input_->width; xIdx += xSkip, idx += xSkip)
393  {
394  if (!mask_ [idx])
395  continue;
396 
397  const PointT& point = input_->points[idx];
398  if (pcl_isfinite (point.x))
399  {
400  Scalar xx = point.x * point.x;
401  Scalar xy = point.x * point.y;
402  Scalar xz = point.x * point.z;
403  Scalar yy = point.y * point.y;
404  Scalar yz = point.y * point.z;
405  Scalar zz = point.z * point.z;
406  Scalar xx_yy = xIdx * xIdx + yIdx * yIdx;
407 
408  A.coeffRef (0) += xx;
409  A.coeffRef (1) += xy;
410  A.coeffRef (2) += xz;
411  A.coeffRef (3) += point.x;
412 
413  A.coeffRef (5) += yy;
414  A.coeffRef (6) += yz;
415  A.coeffRef (7) += point.y;
416 
417  A.coeffRef (10) += zz;
418  A.coeffRef (11) += point.z;
419  A.coeffRef (15) += 1.0;
420 
421  B.coeffRef (0) -= xx * xIdx;
422  B.coeffRef (1) -= xy * xIdx;
423  B.coeffRef (2) -= xz * xIdx;
424  B.coeffRef (3) -= point.x * static_cast<double>(xIdx);
425 
426  B.coeffRef (5) -= yy * xIdx;
427  B.coeffRef (6) -= yz * xIdx;
428  B.coeffRef (7) -= point.y * static_cast<double>(xIdx);
429 
430  B.coeffRef (10) -= zz * xIdx;
431  B.coeffRef (11) -= point.z * static_cast<double>(xIdx);
432 
433  B.coeffRef (15) -= xIdx;
434 
435  C.coeffRef (0) -= xx * yIdx;
436  C.coeffRef (1) -= xy * yIdx;
437  C.coeffRef (2) -= xz * yIdx;
438  C.coeffRef (3) -= point.x * static_cast<double>(yIdx);
439 
440  C.coeffRef (5) -= yy * yIdx;
441  C.coeffRef (6) -= yz * yIdx;
442  C.coeffRef (7) -= point.y * static_cast<double>(yIdx);
443 
444  C.coeffRef (10) -= zz * yIdx;
445  C.coeffRef (11) -= point.z * static_cast<double>(yIdx);
446 
447  C.coeffRef (15) -= yIdx;
448 
449  D.coeffRef (0) += xx * xx_yy;
450  D.coeffRef (1) += xy * xx_yy;
451  D.coeffRef (2) += xz * xx_yy;
452  D.coeffRef (3) += point.x * xx_yy;
453 
454  D.coeffRef (5) += yy * xx_yy;
455  D.coeffRef (6) += yz * xx_yy;
456  D.coeffRef (7) += point.y * xx_yy;
457 
458  D.coeffRef (10) += zz * xx_yy;
459  D.coeffRef (11) += point.z * xx_yy;
460 
461  D.coeffRef (15) += xx_yy;
462  }
463  }
464  }
465 
466  makeSymmetric(A);
467  makeSymmetric(B);
468  makeSymmetric(C);
469  makeSymmetric(D);
470 
471  Eigen::Matrix<Scalar, 12, 12, Eigen::RowMajor> X = Eigen::Matrix<Scalar, 12, 12, Eigen::RowMajor>::Zero ();
472  X.topLeftCorner<4,4> () = A;
473  X.block<4,4> (0, 8) = B;
474  X.block<4,4> (8, 0) = B;
475  X.block<4,4> (4, 4) = A;
476  X.block<4,4> (4, 8) = C;
477  X.block<4,4> (8, 4) = C;
478  X.block<4,4> (8, 8) = D;
479 
480  Eigen::SelfAdjointEigenSolver<Eigen::Matrix<Scalar, 12, 12, Eigen::RowMajor> > ei_symm(X);
481  Eigen::Matrix<Scalar, 12, 12, Eigen::RowMajor> eigen_vectors = ei_symm.eigenvectors();
482 
483  // check whether the residual MSE is low. If its high, the cloud was not captured from a projective device.
484  Eigen::Matrix<Scalar, 1, 1> residual_sqr = eigen_vectors.col (0).transpose () * X * eigen_vectors.col (0);
485  if ( fabs (residual_sqr.coeff (0)) > eps_ * A.coeff (15))
486  {
487  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.coeff (0) / A.coeff (15), static_cast<int> (A.coeff (15)));
488  return;
489  }
490 
491  projection_matrix_.coeffRef (0) = static_cast <float> (eigen_vectors.coeff (0));
492  projection_matrix_.coeffRef (1) = static_cast <float> (eigen_vectors.coeff (12));
493  projection_matrix_.coeffRef (2) = static_cast <float> (eigen_vectors.coeff (24));
494  projection_matrix_.coeffRef (3) = static_cast <float> (eigen_vectors.coeff (36));
495  projection_matrix_.coeffRef (4) = static_cast <float> (eigen_vectors.coeff (48));
496  projection_matrix_.coeffRef (5) = static_cast <float> (eigen_vectors.coeff (60));
497  projection_matrix_.coeffRef (6) = static_cast <float> (eigen_vectors.coeff (72));
498  projection_matrix_.coeffRef (7) = static_cast <float> (eigen_vectors.coeff (84));
499  projection_matrix_.coeffRef (8) = static_cast <float> (eigen_vectors.coeff (96));
500  projection_matrix_.coeffRef (9) = static_cast <float> (eigen_vectors.coeff (108));
501  projection_matrix_.coeffRef (10) = static_cast <float> (eigen_vectors.coeff (120));
502  projection_matrix_.coeffRef (11) = static_cast <float> (eigen_vectors.coeff (132));
503 
504  if (projection_matrix_.coeff (0) < 0)
505  projection_matrix_ *= -1.0;
506 
507  // get left 3x3 sub matrix, which contains K * R, with K = camera matrix = [[fx s cx] [0 fy cy] [0 0 1]]
508  // and R being the rotation matrix
509  KR_ = projection_matrix_.topLeftCorner <3, 3> ();
510 
511  // precalculate KR * KR^T needed by calculations during nn-search
512  KR_KRT_ = KR_ * KR_.transpose ();
513 }
514 
516 template<typename PointT> bool
517 pcl::search::OrganizedNeighbor<PointT>::projectPoint (const PointT& point, pcl::PointXY& q) const
518 {
519  Eigen::Vector3f projected = KR_ * point.getVector3fMap () + projection_matrix_.block <3, 1> (0, 3);
520  q.x = projected [0] / projected [2];
521  q.y = projected [1] / projected [2];
522  return (projected[2] != 0);
523 }
524 #define PCL_INSTANTIATE_OrganizedNeighbor(T) template class PCL_EXPORTS pcl::search::OrganizedNeighbor<T>;
525 
526 #endif
Generated on Sat Aug 24 2013 17:28:37 for Point Cloud Library (PCL) by   doxygen 1.8.4