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