Point Cloud Library (PCL)  1.9.1-dev
eigen.h
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2010, Willow Garage, Inc.
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions
9  * are met:
10  *
11  * * Redistributions of source code must retain the above copyright
12  * notice, this list of conditions and the following disclaimer.
13  * * Redistributions in binary form must reproduce the above
14  * copyright notice, this list of conditions and the following
15  * disclaimer in the documentation and/or other materials provided
16  * with the distribution.
17  * * Neither the name of Willow Garage, Inc. nor the names of its
18  * contributors may be used to endorse or promote products derived
19  * from this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32  * POSSIBILITY OF SUCH DAMAGE.
33  *
34  * $Id$
35  *
36  */
37 // This file is part of Eigen, a lightweight C++ template library
38 // for linear algebra.
39 //
40 // Copyright (C) 2010 Gael Guennebaud <gael.guennebaud@inria.fr>
41 //
42 // Eigen is free software; you can redistribute it and/or
43 // modify it under the terms of the GNU Lesser General Public
44 // License as published by the Free Software Foundation; either
45 // version 3 of the License, or (at your option) any later version.
46 //
47 // Alternatively, you can redistribute it and/or
48 // modify it under the terms of the GNU General Public License as
49 // published by the Free Software Foundation; either version 2 of
50 // the License, or (at your option) any later version.
51 //
52 // Eigen is distributed in the hope that it will be useful, but WITHOUT ANY
53 // WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
54 // FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License or the
55 // GNU General Public License for more details.
56 //
57 // You should have received a copy of the GNU Lesser General Public
58 // License and a copy of the GNU General Public License along with
59 // Eigen. If not, see <http://www.gnu.org/licenses/>.
60 
61 // The computeRoots function included in this is based on materials
62 // covered by the following copyright and license:
63 //
64 // Geometric Tools, LLC
65 // Copyright (c) 1998-2010
66 // Distributed under the Boost Software License, Version 1.0.
67 //
68 // Permission is hereby granted, free of charge, to any person or organization
69 // obtaining a copy of the software and accompanying documentation covered by
70 // this license (the "Software") to use, reproduce, display, distribute,
71 // execute, and transmit the Software, and to prepare derivative works of the
72 // Software, and to permit third-parties to whom the Software is furnished to
73 // do so, all subject to the following:
74 //
75 // The copyright notices in the Software and this entire statement, including
76 // the above license grant, this restriction and the following disclaimer,
77 // must be included in all copies of the Software, in whole or in part, and
78 // all derivative works of the Software, unless such copies or derivative
79 // works are solely in the form of machine-executable object code generated by
80 // a source language processor.
81 //
82 // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
83 // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
84 // FITNESS FOR A PARTICULAR PURPOSE, TITLE AND NON-INFRINGEMENT. IN NO EVENT
85 // SHALL THE COPYRIGHT HOLDERS OR ANYONE DISTRIBUTING THE SOFTWARE BE LIABLE
86 // FOR ANY DAMAGES OR OTHER LIABILITY, WHETHER IN CONTRACT, TORT OR OTHERWISE,
87 // ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
88 // DEALINGS IN THE SOFTWARE.
89 
90 #pragma once
91 
92 #include <pcl/cuda/point_cloud.h>
93 #include <pcl/cuda/cutil_math.h>
94 
95 #include <limits>
96 #include <float.h>
97 
98 namespace pcl
99 {
100  namespace cuda
101  {
102 
103  inline __host__ __device__ bool isMuchSmallerThan (float x, float y)
104  {
105  float prec_sqr = FLT_EPSILON * FLT_EPSILON; // copied from <eigen>/include/Eigen/src/Core/NumTraits.h
106  return x * x <= prec_sqr * y * y;
107  }
108 
109  inline __host__ __device__ float3 unitOrthogonal (const float3& src)
110  {
111  float3 perp;
112  /* Let us compute the crossed product of *this with a vector
113  * that is not too close to being colinear to *this.
114  */
115 
116  /* unless the x and y coords are both close to zero, we can
117  * simply take ( -y, x, 0 ) and normalize it.
118  */
119  if((!isMuchSmallerThan(src.x, src.z))
120  || (!isMuchSmallerThan(src.y, src.z)))
121  {
122  float invnm = 1.0f / sqrtf (src.x*src.x + src.y*src.y);
123  perp.x = -src.y*invnm;
124  perp.y = src.x*invnm;
125  perp.z = 0.0f;
126  }
127  /* if both x and y are close to zero, then the vector is close
128  * to the z-axis, so it's far from colinear to the x-axis for instance.
129  * So we take the crossed product with (1,0,0) and normalize it.
130  */
131  else
132  {
133  float invnm = 1.0f / sqrtf (src.z*src.z + src.y*src.y);
134  perp.x = 0.0f;
135  perp.y = -src.z*invnm;
136  perp.z = src.y*invnm;
137  }
138 
139  return perp;
140  }
141 
142  inline __host__ __device__ void computeRoots2 (const float& b, const float& c, float3& roots)
143  {
144  roots.x = 0.0f;
145  float d = b * b - 4.0f * c;
146  if (d < 0.0f) // no real roots!!!! THIS SHOULD NOT HAPPEN!
147  d = 0.0f;
148 
149  float sd = sqrt (d);
150 
151  roots.z = 0.5f * (b + sd);
152  roots.y = 0.5f * (b - sd);
153  }
154 
155  inline __host__ __device__ void swap (float& a, float& b)
156  {
157  float c(a); a=b; b=c;
158  }
159 
160 
161  // template<typename Matrix, typename Roots>
162  inline __host__ __device__ void
163  computeRoots (const CovarianceMatrix& m, float3& roots)
164  {
165  // The characteristic equation is x^3 - c2*x^2 + c1*x - c0 = 0. The
166  // eigenvalues are the roots to this equation, all guaranteed to be
167  // real-valued, because the matrix is symmetric.
168  float c0 = m.data[0].x*m.data[1].y*m.data[2].z
169  + 2.0f * m.data[0].y*m.data[0].z*m.data[1].z
170  - m.data[0].x*m.data[1].z*m.data[1].z
171  - m.data[1].y*m.data[0].z*m.data[0].z
172  - m.data[2].z*m.data[0].y*m.data[0].y;
173  float c1 = m.data[0].x*m.data[1].y -
174  m.data[0].y*m.data[0].y +
175  m.data[0].x*m.data[2].z -
176  m.data[0].z*m.data[0].z +
177  m.data[1].y*m.data[2].z -
178  m.data[1].z*m.data[1].z;
179  float c2 = m.data[0].x + m.data[1].y + m.data[2].z;
180 
181 
182  if (std::abs(c0) < FLT_EPSILON) // one root is 0 -> quadratic equation
183  computeRoots2 (c2, c1, roots);
184  else
185  {
186  const float s_inv3 = 1.0f/3.0f;
187  const float s_sqrt3 = sqrtf (3.0f);
188  // Construct the parameters used in classifying the roots of the equation
189  // and in solving the equation for the roots in closed form.
190  float c2_over_3 = c2 * s_inv3;
191  float a_over_3 = (c1 - c2 * c2_over_3) * s_inv3;
192  if (a_over_3 > 0.0f)
193  a_over_3 = 0.0f;
194 
195  float half_b = 0.5f * (c0 + c2_over_3 * (2.0f * c2_over_3 * c2_over_3 - c1));
196 
197  float q = half_b * half_b + a_over_3 * a_over_3 * a_over_3;
198  if (q > 0.0f)
199  q = 0.0f;
200 
201  // Compute the eigenvalues by solving for the roots of the polynomial.
202  float rho = sqrtf (-a_over_3);
203  float theta = std::atan2 (sqrtf (-q), half_b) * s_inv3;
204  float cos_theta = std::cos (theta);
205  float sin_theta = sin (theta);
206  roots.x = c2_over_3 + 2.f * rho * cos_theta;
207  roots.y = c2_over_3 - rho * (cos_theta + s_sqrt3 * sin_theta);
208  roots.z = c2_over_3 - rho * (cos_theta - s_sqrt3 * sin_theta);
209 
210  // Sort in increasing order.
211  if (roots.x >= roots.y)
212  swap (roots.x, roots.y);
213  if (roots.y >= roots.z)
214  {
215  swap (roots.y, roots.z);
216  if (roots.x >= roots.y)
217  swap (roots.x, roots.y);
218  }
219 
220  if (roots.x <= 0.0f) // eigenval for symmetric positive semi-definite matrix can not be negative! Set it to 0
221  computeRoots2 (c2, c1, roots);
222  }
223  }
224 
225  inline __host__ __device__ void
226  eigen33 (const CovarianceMatrix& mat, CovarianceMatrix& evecs, float3& evals)
227  {
228  evals = evecs.data[0] = evecs.data[1] = evecs.data[2] = make_float3 (0.0f, 0.0f, 0.0f);
229 
230  // Scale the matrix so its entries are in [-1,1]. The scaling is applied
231  // only when at least one matrix entry has magnitude larger than 1.
232 
233  //Scalar scale = mat.cwiseAbs ().maxCoeff ();
234  float3 scale_tmp = fmaxf (fmaxf (fabs (mat.data[0]), fabs (mat.data[1])), fabs (mat.data[2]));
235  float scale = fmaxf (fmaxf (scale_tmp.x, scale_tmp.y), scale_tmp.z);
236  if (scale <= FLT_MIN)
237  scale = 1.0f;
238 
239  CovarianceMatrix scaledMat;
240  scaledMat.data[0] = mat.data[0] / scale;
241  scaledMat.data[1] = mat.data[1] / scale;
242  scaledMat.data[2] = mat.data[2] / scale;
243 
244  // Compute the eigenvalues
245  computeRoots (scaledMat, evals);
246 
247  if ((evals.z-evals.x) <= FLT_EPSILON)
248  {
249  // all three equal
250  evecs.data[0] = make_float3 (1.0f, 0.0f, 0.0f);
251  evecs.data[1] = make_float3 (0.0f, 1.0f, 0.0f);
252  evecs.data[2] = make_float3 (0.0f, 0.0f, 1.0f);
253  }
254  else if ((evals.y-evals.x) <= FLT_EPSILON)
255  {
256  // first and second equal
257  CovarianceMatrix tmp;
258  tmp.data[0] = scaledMat.data[0];
259  tmp.data[1] = scaledMat.data[1];
260  tmp.data[2] = scaledMat.data[2];
261 
262  tmp.data[0].x -= evals.z;
263  tmp.data[1].y -= evals.z;
264  tmp.data[2].z -= evals.z;
265 
266  float3 vec1 = cross (tmp.data[0], tmp.data[1]);
267  float3 vec2 = cross (tmp.data[0], tmp.data[2]);
268  float3 vec3 = cross (tmp.data[1], tmp.data[2]);
269 
270  float len1 = dot (vec1, vec1);
271  float len2 = dot (vec2, vec2);
272  float len3 = dot (vec3, vec3);
273 
274  if (len1 >= len2 && len1 >= len3)
275  evecs.data[2] = vec1 / sqrtf (len1);
276  else if (len2 >= len1 && len2 >= len3)
277  evecs.data[2] = vec2 / sqrtf (len2);
278  else
279  evecs.data[2] = vec3 / sqrtf (len3);
280 
281  evecs.data[1] = unitOrthogonal (evecs.data[2]);
282  evecs.data[0] = cross (evecs.data[1], evecs.data[2]);
283  }
284  else if ((evals.z-evals.y) <= FLT_EPSILON)
285  {
286  // second and third equal
287  CovarianceMatrix tmp;
288  tmp.data[0] = scaledMat.data[0];
289  tmp.data[1] = scaledMat.data[1];
290  tmp.data[2] = scaledMat.data[2];
291  tmp.data[0].x -= evals.x;
292  tmp.data[1].y -= evals.x;
293  tmp.data[2].z -= evals.x;
294 
295  float3 vec1 = cross (tmp.data[0], tmp.data[1]);
296  float3 vec2 = cross (tmp.data[0], tmp.data[2]);
297  float3 vec3 = cross (tmp.data[1], tmp.data[2]);
298 
299  float len1 = dot (vec1, vec1);
300  float len2 = dot (vec2, vec2);
301  float len3 = dot (vec3, vec3);
302 
303  if (len1 >= len2 && len1 >= len3)
304  evecs.data[0] = vec1 / sqrtf (len1);
305  else if (len2 >= len1 && len2 >= len3)
306  evecs.data[0] = vec2 / sqrtf (len2);
307  else
308  evecs.data[0] = vec3 / sqrtf (len3);
309 
310  evecs.data[1] = unitOrthogonal (evecs.data[0]);
311  evecs.data[2] = cross (evecs.data[0], evecs.data[1]);
312  }
313  else
314  {
315  CovarianceMatrix tmp;
316  tmp.data[0] = scaledMat.data[0];
317  tmp.data[1] = scaledMat.data[1];
318  tmp.data[2] = scaledMat.data[2];
319  tmp.data[0].x -= evals.z;
320  tmp.data[1].y -= evals.z;
321  tmp.data[2].z -= evals.z;
322 
323  float3 vec1 = cross (tmp.data[0], tmp.data[1]);
324  float3 vec2 = cross (tmp.data[0], tmp.data[2]);
325  float3 vec3 = cross (tmp.data[1], tmp.data[2]);
326 
327  float len1 = dot (vec1, vec1);
328  float len2 = dot (vec2, vec2);
329  float len3 = dot (vec3, vec3);
330 
331  float mmax[3];
332  unsigned int min_el = 2;
333  unsigned int max_el = 2;
334  if (len1 >= len2 && len1 >= len3)
335  {
336  mmax[2] = len1;
337  evecs.data[2] = vec1 / sqrtf (len1);
338  }
339  else if (len2 >= len1 && len2 >= len3)
340  {
341  mmax[2] = len2;
342  evecs.data[2] = vec2 / sqrtf (len2);
343  }
344  else
345  {
346  mmax[2] = len3;
347  evecs.data[2] = vec3 / sqrtf (len3);
348  }
349 
350  tmp.data[0] = scaledMat.data[0];
351  tmp.data[1] = scaledMat.data[1];
352  tmp.data[2] = scaledMat.data[2];
353  tmp.data[0].x -= evals.y;
354  tmp.data[1].y -= evals.y;
355  tmp.data[2].z -= evals.y;
356 
357  vec1 = cross (tmp.data[0], tmp.data[1]);
358  vec2 = cross (tmp.data[0], tmp.data[2]);
359  vec3 = cross (tmp.data[1], tmp.data[2]);
360 
361  len1 = dot (vec1, vec1);
362  len2 = dot (vec2, vec2);
363  len3 = dot (vec3, vec3);
364  if (len1 >= len2 && len1 >= len3)
365  {
366  mmax[1] = len1;
367  evecs.data[1] = vec1 / sqrtf (len1);
368  min_el = len1 <= mmax[min_el]? 1: min_el;
369  max_el = len1 > mmax[max_el]? 1: max_el;
370  }
371  else if (len2 >= len1 && len2 >= len3)
372  {
373  mmax[1] = len2;
374  evecs.data[1] = vec2 / sqrtf (len2);
375  min_el = len2 <= mmax[min_el]? 1: min_el;
376  max_el = len2 > mmax[max_el]? 1: max_el;
377  }
378  else
379  {
380  mmax[1] = len3;
381  evecs.data[1] = vec3 / sqrtf (len3);
382  min_el = len3 <= mmax[min_el]? 1: min_el;
383  max_el = len3 > mmax[max_el]? 1: max_el;
384  }
385 
386  tmp.data[0] = scaledMat.data[0];
387  tmp.data[1] = scaledMat.data[1];
388  tmp.data[2] = scaledMat.data[2];
389  tmp.data[0].x -= evals.x;
390  tmp.data[1].y -= evals.x;
391  tmp.data[2].z -= evals.x;
392 
393  vec1 = cross (tmp.data[0], tmp.data[1]);
394  vec2 = cross (tmp.data[0], tmp.data[2]);
395  vec3 = cross (tmp.data[1], tmp.data[2]);
396 
397  len1 = dot (vec1, vec1);
398  len2 = dot (vec2, vec2);
399  len3 = dot (vec3, vec3);
400  if (len1 >= len2 && len1 >= len3)
401  {
402  mmax[0] = len1;
403  evecs.data[0] = vec1 / sqrtf (len1);
404  min_el = len3 <= mmax[min_el]? 0: min_el;
405  max_el = len3 > mmax[max_el]? 0: max_el;
406  }
407  else if (len2 >= len1 && len2 >= len3)
408  {
409  mmax[0] = len2;
410  evecs.data[0] = vec2 / sqrtf (len2);
411  min_el = len3 <= mmax[min_el]? 0: min_el;
412  max_el = len3 > mmax[max_el]? 0: max_el;
413  }
414  else
415  {
416  mmax[0] = len3;
417  evecs.data[0] = vec3 / sqrtf (len3);
418  min_el = len3 <= mmax[min_el]? 0: min_el;
419  max_el = len3 > mmax[max_el]? 0: max_el;
420  }
421 
422  unsigned mid_el = 3 - min_el - max_el;
423  evecs.data[min_el] = normalize (cross (evecs.data[(min_el+1)%3], evecs.data[(min_el+2)%3] ));
424  evecs.data[mid_el] = normalize (cross (evecs.data[(mid_el+1)%3], evecs.data[(mid_el+2)%3] ));
425  }
426  // Rescale back to the original size.
427  evals *= scale;
428  }
429 
430  /** \brief Simple kernel to add two points. */
431  struct AddPoints
432  {
433  __inline__ __host__ __device__ float3
434  operator () (float3 lhs, float3 rhs)
435  {
436  return lhs + rhs;
437  }
438  };
439 
440  /** \brief Adds two matrices element-wise. */
442  {
443  __inline__ __host__ __device__
446  {
447  CovarianceMatrix ret;
448  ret.data[0] = lhs.data[0] + rhs.data[0];
449  ret.data[1] = lhs.data[1] + rhs.data[1];
450  ret.data[2] = lhs.data[2] + rhs.data[2];
451  return ret;
452  }
453  };
454 
455  /** \brief Simple kernel to convert a PointXYZRGB to float3. Relies the cast operator of PointXYZRGB. */
457  {
458  __inline__ __host__ __device__ float3
459  operator () (const PointXYZRGB& pt) {return pt;}
460  };
461 
462  /** \brief Kernel to compute a ``covariance matrix'' for a single point. */
464  {
465  float3 centroid_;
466  __inline__ __host__ __device__
467  ComputeCovarianceForPoint (const float3& centroid) : centroid_ (centroid) {}
468 
469  __inline__ __host__ __device__ CovarianceMatrix
470  operator () (const PointXYZRGB& point)
471  {
472  CovarianceMatrix cov;
473  float3 pt = point - centroid_;
474  cov.data[1].y = pt.y * pt.y;
475  cov.data[1].z = pt.y * pt.z;
476  cov.data[2].z = pt.z * pt.z;
477 
478  pt *= pt.x;
479  cov.data[0].x = pt.x;
480  cov.data[0].y = pt.y;
481  cov.data[0].z = pt.z;
482  return cov;
483  }
484  };
485 
486  /** \brief Computes a centroid for a given range of points. */
487  template <class IteratorT>
488  void compute3DCentroid (IteratorT begin, IteratorT end, float3& centroid)
489  {
490  // Compute Centroid:
491  centroid.x = centroid.y = centroid.z = 0;
492  // we need a way to iterate over the inliers in the point cloud.. permutation_iterator to the rescue
493  centroid = transform_reduce (begin, end, convert_point_to_float3 (), centroid, AddPoints ());
494  centroid /= (float) (end - begin);
495  }
496 
497  /** \brief Computes a covariance matrix for a given range of points. */
498  template <class IteratorT>
499  void computeCovariance (IteratorT begin, IteratorT end, CovarianceMatrix& cov, float3 centroid)
500  {
501  cov.data[0] = make_float3 (0.0f, 0.0f, 0.0f);
502  cov.data[1] = make_float3 (0.0f, 0.0f, 0.0f);
503  cov.data[2] = make_float3 (0.0f, 0.0f, 0.0f);
504 
505  cov = transform_reduce (begin, end,
506  ComputeCovarianceForPoint (centroid),
507  cov,
508  AddCovariances ());
509 
510  // fill in the lower triangle (symmetry)
511  cov.data[1].x = cov.data[0].y;
512  cov.data[2].x = cov.data[0].z;
513  cov.data[2].y = cov.data[1].z;
514 
515  // divide by number of inliers
516  cov.data[0] /= (float) (end - begin);
517  cov.data[1] /= (float) (end - begin);
518  cov.data[2] /= (float) (end - begin);
519  }
520 
521  /** Kernel to compute a radius neighborhood given a organized point cloud (aka range image cloud) */
522  template <typename CloudPtr>
524  {
525  public:
526  OrganizedRadiusSearch (const CloudPtr &input, float focalLength, float sqr_radius)
527  : points_(thrust::raw_pointer_cast (&input->points[0]))
528  , focalLength_(focalLength)
529  , width_ (input->width)
530  , height_ (input->height)
531  , sqr_radius_ (sqr_radius)
532  {}
533 
534  //////////////////////////////////////////////////////////////////////////////////////////////
535  // returns float4: .x = min_x, .y = max_x, .z = min_y, .w = max_y
536  // note: assumes the projection of a point falls onto the image lattice! otherwise, min_x might be bigger than max_x !!!
537  inline __host__ __device__
538  int4
539  getProjectedRadiusSearchBox (const float3& point_arg)
540  {
541  int4 res;
542  float r_quadr, z_sqr;
543  float sqrt_term_y, sqrt_term_x, norm;
544  float x_times_z, y_times_z;
545 
546  // see http://www.wolframalpha.com/input/?i=solve+%5By%2Fsqrt%28f^2%2By^2%29*c-f%2Fsqrt%28f^2%2By^2%29*b%2Br%3D%3D0%2C+f%3D1%2C+y%5D
547  // where b = p_q_arg.y, c = p_q_arg.z, r = radius_arg, f = focalLength_
548 
549  r_quadr = sqr_radius_ * sqr_radius_;
550  z_sqr = point_arg.z * point_arg.z;
551 
552  sqrt_term_y = sqrt (point_arg.y * point_arg.y * sqr_radius_ + z_sqr * sqr_radius_ - r_quadr);
553  sqrt_term_x = sqrt (point_arg.x * point_arg.x * sqr_radius_ + z_sqr * sqr_radius_ - r_quadr);
554  //sqrt_term_y = sqrt (point_arg.y * point_arg.y * sqr_radius_ + z_sqr * sqr_radius_ - r_quadr);
555  //sqrt_term_x = sqrt (point_arg.x * point_arg.x * sqr_radius_ + z_sqr * sqr_radius_ - r_quadr);
556  norm = 1.0f / (z_sqr - sqr_radius_);
557 
558  x_times_z = point_arg.x * point_arg.z;
559  y_times_z = point_arg.y * point_arg.z;
560 
561  float4 bounds;
562  bounds.x = (x_times_z - sqrt_term_x) * norm;
563  bounds.y = (x_times_z + sqrt_term_x) * norm;
564  bounds.z = (y_times_z - sqrt_term_y) * norm;
565  bounds.w = (y_times_z + sqrt_term_y) * norm;
566 
567  // determine 2-D search window
568  bounds *= focalLength_;
569  bounds.x += width_ / 2.0f;
570  bounds.y += width_ / 2.0f;
571  bounds.z += height_ / 2.0f;
572  bounds.w += height_ / 2.0f;
573 
574  res.x = (int)std::floor (bounds.x);
575  res.y = (int)std::ceil (bounds.y);
576  res.z = (int)std::floor (bounds.z);
577  res.w = (int)std::ceil (bounds.w);
578 
579  // clamp the coordinates to fit to depth image size
580  res.x = clamp (res.x, 0, width_-1);
581  res.y = clamp (res.y, 0, width_-1);
582  res.z = clamp (res.z, 0, height_-1);
583  res.w = clamp (res.w, 0, height_-1);
584  return res;
585  }
586 
587  //////////////////////////////////////////////////////////////////////////////////////////////
588  inline __host__ __device__
589  int radiusSearch (const float3 &query_pt, int k_indices[], int max_nnn)
590  {
591  // bounds.x = min_x, .y = max_x, .z = min_y, .w = max_y
592  int4 bounds = getProjectedRadiusSearchBox(query_pt);
593 
594  int nnn = 0;
595  // iterate over all pixels in the rectangular region
596  for (int x = bounds.x; (x <= bounds.y) & (nnn < max_nnn); ++x)
597  {
598  for (int y = bounds.z; (y <= bounds.w) & (nnn < max_nnn); ++y)
599  {
600  int idx = y * width_ + x;
601 
602  if (isnan (points_[idx].x))
603  continue;
604 
605  float3 point_dif = points_[idx] - query_pt;
606 
607  // check distance and add to results
608  if (dot (point_dif, point_dif) <= sqr_radius_)
609  {
610  k_indices[nnn] = idx;
611  ++nnn;
612  }
613  }
614  }
615 
616  return nnn;
617  }
618 
619  //////////////////////////////////////////////////////////////////////////////////////////////
620  inline __host__ __device__
621  int computeCovarianceOnline (const float3 &query_pt, CovarianceMatrix &cov, float sqrt_desired_nr_neighbors)
622  {
623  // bounds.x = min_x, .y = max_x, .z = min_y, .w = max_y
624  //
625  //sqr_radius_ = query_pt.z * (0.2f / 4.0f);
626  //sqr_radius_ *= sqr_radius_;
627  int4 bounds = getProjectedRadiusSearchBox(query_pt);
628 
629  // This implements a fixed window size in image coordinates (pixels)
630  //int2 proj_point = make_int2 ( query_pt.x/(query_pt.z/focalLength_)+width_/2.0f, query_pt.y/(query_pt.z/focalLength_)+height_/2.0f);
631  //int window_size = 1;
632  //int4 bounds = make_int4 (
633  // proj_point.x - window_size,
634  // proj_point.x + window_size,
635  // proj_point.y - window_size,
636  // proj_point.y + window_size
637  // );
638 
639  // clamp the coordinates to fit to depth image size
640  bounds.x = clamp (bounds.x, 0, width_-1);
641  bounds.y = clamp (bounds.y, 0, width_-1);
642  bounds.z = clamp (bounds.z, 0, height_-1);
643  bounds.w = clamp (bounds.w, 0, height_-1);
644  //int4 bounds = getProjectedRadiusSearchBox(query_pt);
645 
646  // number of points in rectangular area
647  //int boundsarea = (bounds.y-bounds.x) * (bounds.w-bounds.z);
648  //float skip = max (sqrtf ((float)boundsarea) / sqrt_desired_nr_neighbors, 1.0);
649  float skipX = max (sqrtf ((float)bounds.y-bounds.x) / sqrt_desired_nr_neighbors, 1.0f);
650  float skipY = max (sqrtf ((float)bounds.w-bounds.z) / sqrt_desired_nr_neighbors, 1.0f);
651  skipX = 1;
652  skipY = 1;
653 
654  cov.data[0] = make_float3(0,0,0);
655  cov.data[1] = make_float3(0,0,0);
656  cov.data[2] = make_float3(0,0,0);
657  float3 centroid = make_float3(0,0,0);
658  int nnn = 0;
659  // iterate over all pixels in the rectangular region
660  for (float y = (float) bounds.z; y <= bounds.w; y += skipY)
661  {
662  for (float x = (float) bounds.x; x <= bounds.y; x += skipX)
663  {
664  // find index in point cloud from x,y pixel positions
665  int idx = ((int)y) * width_ + ((int)x);
666 
667  // ignore invalid points
668  if (isnan (points_[idx].x) | isnan (points_[idx].y) | isnan (points_[idx].z))
669  continue;
670 
671  float3 point_dif = points_[idx] - query_pt;
672 
673  // check distance and update covariance matrix
674  if (dot (point_dif, point_dif) <= sqr_radius_)
675  {
676  ++nnn;
677  float3 demean_old = points_[idx] - centroid;
678  centroid += demean_old / (float) nnn;
679  float3 demean_new = points_[idx] - centroid;
680 
681  cov.data[1].y += demean_new.y * demean_old.y;
682  cov.data[1].z += demean_new.y * demean_old.z;
683  cov.data[2].z += demean_new.z * demean_old.z;
684 
685  demean_old *= demean_new.x;
686  cov.data[0].x += demean_old.x;
687  cov.data[0].y += demean_old.y;
688  cov.data[0].z += demean_old.z;
689  }
690  }
691  }
692 
693  cov.data[1].x = cov.data[0].y;
694  cov.data[2].x = cov.data[0].z;
695  cov.data[2].y = cov.data[1].z;
696  cov.data[0] /= (float) nnn;
697  cov.data[1] /= (float) nnn;
698  cov.data[2] /= (float) nnn;
699  return nnn;
700  }
701 
702  //////////////////////////////////////////////////////////////////////////////////////////////
703  inline __host__ __device__
704  float3 computeCentroid (const float3 &query_pt, CovarianceMatrix &cov, float sqrt_desired_nr_neighbors)
705  {
706  // bounds.x = min_x, .y = max_x, .z = min_y, .w = max_y
707  //
708  //sqr_radius_ = query_pt.z * (0.2f / 4.0f);
709  //sqr_radius_ *= sqr_radius_;
710  int4 bounds = getProjectedRadiusSearchBox(query_pt);
711 
712  // This implements a fixed window size in image coordinates (pixels)
713  //int2 proj_point = make_int2 ( query_pt.x/(query_pt.z/focalLength_)+width_/2.0f, query_pt.y/(query_pt.z/focalLength_)+height_/2.0f);
714  //int window_size = 1;
715  //int4 bounds = make_int4 (
716  // proj_point.x - window_size,
717  // proj_point.x + window_size,
718  // proj_point.y - window_size,
719  // proj_point.y + window_size
720  // );
721 
722  // clamp the coordinates to fit to depth image size
723  bounds.x = clamp (bounds.x, 0, width_-1);
724  bounds.y = clamp (bounds.y, 0, width_-1);
725  bounds.z = clamp (bounds.z, 0, height_-1);
726  bounds.w = clamp (bounds.w, 0, height_-1);
727 
728  // number of points in rectangular area
729  //int boundsarea = (bounds.y-bounds.x) * (bounds.w-bounds.z);
730  //float skip = max (sqrtf ((float)boundsarea) / sqrt_desired_nr_neighbors, 1.0);
731  float skipX = max (sqrtf ((float)bounds.y-bounds.x) / sqrt_desired_nr_neighbors, 1.0f);
732  float skipY = max (sqrtf ((float)bounds.w-bounds.z) / sqrt_desired_nr_neighbors, 1.0f);
733 
734  skipX = 1;
735  skipY = 1;
736  float3 centroid = make_float3(0,0,0);
737  int nnn = 0;
738  // iterate over all pixels in the rectangular region
739  for (float y = (float) bounds.z; y <= bounds.w; y += skipY)
740  {
741  for (float x = (float) bounds.x; x <= bounds.y; x += skipX)
742  {
743  // find index in point cloud from x,y pixel positions
744  int idx = ((int)y) * width_ + ((int)x);
745 
746  // ignore invalid points
747  if (isnan (points_[idx].x) | isnan (points_[idx].y) | isnan (points_[idx].z))
748  continue;
749 
750  float3 point_dif = points_[idx] - query_pt;
751 
752  // check distance and update covariance matrix
753  if (dot (point_dif, point_dif) <= sqr_radius_)
754  {
755  centroid += points_[idx];
756  ++nnn;
757  }
758  }
759  }
760 
761  return centroid / (float) nnn;
762  }
763 
766  int width_, height_;
767  float sqr_radius_;
768  };
769 
770  } // namespace
771 } // namespace
__host__ __device__ int4 getProjectedRadiusSearchBox(const float3 &point_arg)
Definition: eigen.h:539
__host__ __device__ int radiusSearch(const float3 &query_pt, int k_indices[], int max_nnn)
Definition: eigen.h:589
misnamed class holding a 3x3 matrix
Definition: point_cloud.h:49
This file defines compatibility wrappers for low level I/O functions.
Definition: convolution.h:45
__host__ __device__ bool isMuchSmallerThan(float x, float y)
Definition: eigen.h:103
Simple kernel to add two points.
Definition: eigen.h:431
void computeCovariance(IteratorT begin, IteratorT end, CovarianceMatrix &cov, float3 centroid)
Computes a covariance matrix for a given range of points.
Definition: eigen.h:499
__host__ __device__ void eigen33(const CovarianceMatrix &mat, CovarianceMatrix &evecs, float3 &evals)
Definition: eigen.h:226
__host__ __device__ float3 unitOrthogonal(const float3 &src)
Definition: eigen.h:109
OrganizedRadiusSearch(const CloudPtr &input, float focalLength, float sqr_radius)
Definition: eigen.h:526
Adds two matrices element-wise.
Definition: eigen.h:441
__host__ __device__ void computeRoots2(const float &b, const float &c, float3 &roots)
Definition: eigen.h:142
__host__ __device__ float3 computeCentroid(const float3 &query_pt, CovarianceMatrix &cov, float sqrt_desired_nr_neighbors)
Definition: eigen.h:704
void compute3DCentroid(IteratorT begin, IteratorT end, float3 &centroid)
Computes a centroid for a given range of points.
Definition: eigen.h:488
Simple kernel to convert a PointXYZRGB to float3.
Definition: eigen.h:456
__inline__ __host__ __device__ ComputeCovarianceForPoint(const float3 &centroid)
Definition: eigen.h:467
__host__ __device__ void computeRoots(const CovarianceMatrix &m, float3 &roots)
Definition: eigen.h:163
__host__ __device__ int computeCovarianceOnline(const float3 &query_pt, CovarianceMatrix &cov, float sqrt_desired_nr_neighbors)
Definition: eigen.h:621
Kernel to compute a radius neighborhood given a organized point cloud (aka range image cloud) ...
Definition: eigen.h:523
Kernel to compute a ``covariance matrix&#39;&#39; for a single point.
Definition: eigen.h:463
Default point xyz-rgb structure.
Definition: point_types.h:48
__inline__ __host__ __device__ float3 operator()(float3 lhs, float3 rhs)
Definition: eigen.h:434
const PointXYZRGB * points_
Definition: eigen.h:765
__host__ __device__ void swap(float &a, float &b)
Definition: eigen.h:155