Point Cloud Library (PCL)  1.14.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 
97 namespace pcl
98 {
99  namespace cuda
100  {
101 
102  inline __host__ __device__ bool isMuchSmallerThan (float x, float y)
103  {
104  // inspired by Eigen's implementation
105  float prec_sqr =
106  std::numeric_limits<float>::epsilon() * std::numeric_limits<float>::epsilon();
107  return x * x <= prec_sqr * y * y;
108  }
109 
110  inline __host__ __device__ float3 unitOrthogonal (const float3& src)
111  {
112  float3 perp;
113  /* Let us compute the crossed product of *this with a vector
114  * that is not too close to being colinear to *this.
115  */
116 
117  /* unless the x and y coords are both close to zero, we can
118  * simply take ( -y, x, 0 ) and normalize it.
119  */
120  if((!isMuchSmallerThan(src.x, src.z))
121  || (!isMuchSmallerThan(src.y, src.z)))
122  {
123  float invnm = 1.0f / sqrtf (src.x*src.x + src.y*src.y);
124  perp.x = -src.y*invnm;
125  perp.y = src.x*invnm;
126  perp.z = 0.0f;
127  }
128  /* if both x and y are close to zero, then the vector is close
129  * to the z-axis, so it's far from colinear to the x-axis for instance.
130  * So we take the crossed product with (1,0,0) and normalize it.
131  */
132  else
133  {
134  float invnm = 1.0f / sqrtf (src.z*src.z + src.y*src.y);
135  perp.x = 0.0f;
136  perp.y = -src.z*invnm;
137  perp.z = src.y*invnm;
138  }
139 
140  return perp;
141  }
142 
143  inline __host__ __device__ void computeRoots2 (const float& b, const float& c, float3& roots)
144  {
145  roots.x = 0.0f;
146  float d = b * b - 4.0f * c;
147  if (d < 0.0f) // no real roots!!!! THIS SHOULD NOT HAPPEN!
148  d = 0.0f;
149 
150  float sd = sqrt (d);
151 
152  roots.z = 0.5f * (b + sd);
153  roots.y = 0.5f * (b - sd);
154  }
155 
156  inline __host__ __device__ void swap (float& a, float& b)
157  {
158  float c(a); a=b; b=c;
159  }
160 
161 
162  // template<typename Matrix, typename Roots>
163  inline __host__ __device__ void
164  computeRoots (const CovarianceMatrix& m, float3& roots)
165  {
166  // The characteristic equation is x^3 - c2*x^2 + c1*x - c0 = 0. The
167  // eigenvalues are the roots to this equation, all guaranteed to be
168  // real-valued, because the matrix is symmetric.
169  float c0 = m.data[0].x*m.data[1].y*m.data[2].z
170  + 2.0f * m.data[0].y*m.data[0].z*m.data[1].z
171  - m.data[0].x*m.data[1].z*m.data[1].z
172  - m.data[1].y*m.data[0].z*m.data[0].z
173  - m.data[2].z*m.data[0].y*m.data[0].y;
174  float c1 = m.data[0].x*m.data[1].y -
175  m.data[0].y*m.data[0].y +
176  m.data[0].x*m.data[2].z -
177  m.data[0].z*m.data[0].z +
178  m.data[1].y*m.data[2].z -
179  m.data[1].z*m.data[1].z;
180  float c2 = m.data[0].x + m.data[1].y + m.data[2].z;
181 
182 
183  if (std::abs(c0) < std::numeric_limits<float>::epsilon()) // one root is 0 -> quadratic equation
184  computeRoots2 (c2, c1, roots);
185  else
186  {
187  const float s_inv3 = 1.0f/3.0f;
188  const float s_sqrt3 = sqrtf (3.0f);
189  // Construct the parameters used in classifying the roots of the equation
190  // and in solving the equation for the roots in closed form.
191  float c2_over_3 = c2 * s_inv3;
192  float a_over_3 = (c1 - c2 * c2_over_3) * s_inv3;
193  if (a_over_3 > 0.0f)
194  a_over_3 = 0.0f;
195 
196  float half_b = 0.5f * (c0 + c2_over_3 * (2.0f * c2_over_3 * c2_over_3 - c1));
197 
198  float q = half_b * half_b + a_over_3 * a_over_3 * a_over_3;
199  if (q > 0.0f)
200  q = 0.0f;
201 
202  // Compute the eigenvalues by solving for the roots of the polynomial.
203  float rho = sqrtf (-a_over_3);
204  float theta = std::atan2 (sqrtf (-q), half_b) * s_inv3;
205  float cos_theta = std::cos (theta);
206  float sin_theta = sin (theta);
207  roots.x = c2_over_3 + 2.f * rho * cos_theta;
208  roots.y = c2_over_3 - rho * (cos_theta + s_sqrt3 * sin_theta);
209  roots.z = c2_over_3 - rho * (cos_theta - s_sqrt3 * sin_theta);
210 
211  // Sort in increasing order.
212  if (roots.x >= roots.y)
213  swap (roots.x, roots.y);
214  if (roots.y >= roots.z)
215  {
216  swap (roots.y, roots.z);
217  if (roots.x >= roots.y)
218  swap (roots.x, roots.y);
219  }
220 
221  if (roots.x <= 0.0f) // eigenval for symmetric positive semi-definite matrix can not be negative! Set it to 0
222  computeRoots2 (c2, c1, roots);
223  }
224  }
225 
226  inline __host__ __device__ void
227  eigen33 (const CovarianceMatrix& mat, CovarianceMatrix& evecs, float3& evals)
228  {
229  evals = evecs.data[0] = evecs.data[1] = evecs.data[2] = make_float3 (0.0f, 0.0f, 0.0f);
230 
231  // Scale the matrix so its entries are in [-1,1]. The scaling is applied
232  // only when at least one matrix entry has magnitude larger than 1.
233 
234  //Scalar scale = mat.cwiseAbs ().maxCoeff ();
235  float3 scale_tmp = fmaxf (fmaxf (fabs (mat.data[0]), fabs (mat.data[1])), fabs (mat.data[2]));
236  float scale = fmaxf (fmaxf (scale_tmp.x, scale_tmp.y), scale_tmp.z);
237  if (scale <= std::numeric_limits<float>::min())
238  scale = 1.0f;
239 
240  CovarianceMatrix scaledMat;
241  scaledMat.data[0] = mat.data[0] / scale;
242  scaledMat.data[1] = mat.data[1] / scale;
243  scaledMat.data[2] = mat.data[2] / scale;
244 
245  // Compute the eigenvalues
246  computeRoots (scaledMat, evals);
247 
248  if ((evals.z - evals.x) <= std::numeric_limits<float>::epsilon())
249  {
250  // all three equal
251  evecs.data[0] = make_float3 (1.0f, 0.0f, 0.0f);
252  evecs.data[1] = make_float3 (0.0f, 1.0f, 0.0f);
253  evecs.data[2] = make_float3 (0.0f, 0.0f, 1.0f);
254  }
255  else if ((evals.y - evals.x) <= std::numeric_limits<float>::epsilon())
256  {
257  // first and second equal
258  CovarianceMatrix tmp;
259  tmp.data[0] = scaledMat.data[0];
260  tmp.data[1] = scaledMat.data[1];
261  tmp.data[2] = scaledMat.data[2];
262 
263  tmp.data[0].x -= evals.z;
264  tmp.data[1].y -= evals.z;
265  tmp.data[2].z -= evals.z;
266 
267  float3 vec1 = cross (tmp.data[0], tmp.data[1]);
268  float3 vec2 = cross (tmp.data[0], tmp.data[2]);
269  float3 vec3 = cross (tmp.data[1], tmp.data[2]);
270 
271  float len1 = dot (vec1, vec1);
272  float len2 = dot (vec2, vec2);
273  float len3 = dot (vec3, vec3);
274 
275  if (len1 >= len2 && len1 >= len3)
276  evecs.data[2] = vec1 / sqrtf (len1);
277  else if (len2 >= len1 && len2 >= len3)
278  evecs.data[2] = vec2 / sqrtf (len2);
279  else
280  evecs.data[2] = vec3 / sqrtf (len3);
281 
282  evecs.data[1] = unitOrthogonal (evecs.data[2]);
283  evecs.data[0] = cross (evecs.data[1], evecs.data[2]);
284  }
285  else if ((evals.z - evals.y) <= std::numeric_limits<float>::epsilon())
286  {
287  // second and third equal
288  CovarianceMatrix tmp;
289  tmp.data[0] = scaledMat.data[0];
290  tmp.data[1] = scaledMat.data[1];
291  tmp.data[2] = scaledMat.data[2];
292  tmp.data[0].x -= evals.x;
293  tmp.data[1].y -= evals.x;
294  tmp.data[2].z -= evals.x;
295 
296  float3 vec1 = cross (tmp.data[0], tmp.data[1]);
297  float3 vec2 = cross (tmp.data[0], tmp.data[2]);
298  float3 vec3 = cross (tmp.data[1], tmp.data[2]);
299 
300  float len1 = dot (vec1, vec1);
301  float len2 = dot (vec2, vec2);
302  float len3 = dot (vec3, vec3);
303 
304  if (len1 >= len2 && len1 >= len3)
305  evecs.data[0] = vec1 / sqrtf (len1);
306  else if (len2 >= len1 && len2 >= len3)
307  evecs.data[0] = vec2 / sqrtf (len2);
308  else
309  evecs.data[0] = vec3 / sqrtf (len3);
310 
311  evecs.data[1] = unitOrthogonal (evecs.data[0]);
312  evecs.data[2] = cross (evecs.data[0], evecs.data[1]);
313  }
314  else
315  {
316  CovarianceMatrix tmp;
317  tmp.data[0] = scaledMat.data[0];
318  tmp.data[1] = scaledMat.data[1];
319  tmp.data[2] = scaledMat.data[2];
320  tmp.data[0].x -= evals.z;
321  tmp.data[1].y -= evals.z;
322  tmp.data[2].z -= evals.z;
323 
324  float3 vec1 = cross (tmp.data[0], tmp.data[1]);
325  float3 vec2 = cross (tmp.data[0], tmp.data[2]);
326  float3 vec3 = cross (tmp.data[1], tmp.data[2]);
327 
328  float len1 = dot (vec1, vec1);
329  float len2 = dot (vec2, vec2);
330  float len3 = dot (vec3, vec3);
331 
332  float mmax[3];
333  unsigned int min_el = 2;
334  unsigned int max_el = 2;
335  if (len1 >= len2 && len1 >= len3)
336  {
337  mmax[2] = len1;
338  evecs.data[2] = vec1 / sqrtf (len1);
339  }
340  else if (len2 >= len1 && len2 >= len3)
341  {
342  mmax[2] = len2;
343  evecs.data[2] = vec2 / sqrtf (len2);
344  }
345  else
346  {
347  mmax[2] = len3;
348  evecs.data[2] = vec3 / sqrtf (len3);
349  }
350 
351  tmp.data[0] = scaledMat.data[0];
352  tmp.data[1] = scaledMat.data[1];
353  tmp.data[2] = scaledMat.data[2];
354  tmp.data[0].x -= evals.y;
355  tmp.data[1].y -= evals.y;
356  tmp.data[2].z -= evals.y;
357 
358  vec1 = cross (tmp.data[0], tmp.data[1]);
359  vec2 = cross (tmp.data[0], tmp.data[2]);
360  vec3 = cross (tmp.data[1], tmp.data[2]);
361 
362  len1 = dot (vec1, vec1);
363  len2 = dot (vec2, vec2);
364  len3 = dot (vec3, vec3);
365  if (len1 >= len2 && len1 >= len3)
366  {
367  mmax[1] = len1;
368  evecs.data[1] = vec1 / sqrtf (len1);
369  min_el = len1 <= mmax[min_el]? 1: min_el;
370  max_el = len1 > mmax[max_el]? 1: max_el;
371  }
372  else if (len2 >= len1 && len2 >= len3)
373  {
374  mmax[1] = len2;
375  evecs.data[1] = vec2 / sqrtf (len2);
376  min_el = len2 <= mmax[min_el]? 1: min_el;
377  max_el = len2 > mmax[max_el]? 1: max_el;
378  }
379  else
380  {
381  mmax[1] = len3;
382  evecs.data[1] = vec3 / sqrtf (len3);
383  min_el = len3 <= mmax[min_el]? 1: min_el;
384  max_el = len3 > mmax[max_el]? 1: max_el;
385  }
386 
387  tmp.data[0] = scaledMat.data[0];
388  tmp.data[1] = scaledMat.data[1];
389  tmp.data[2] = scaledMat.data[2];
390  tmp.data[0].x -= evals.x;
391  tmp.data[1].y -= evals.x;
392  tmp.data[2].z -= evals.x;
393 
394  vec1 = cross (tmp.data[0], tmp.data[1]);
395  vec2 = cross (tmp.data[0], tmp.data[2]);
396  vec3 = cross (tmp.data[1], tmp.data[2]);
397 
398  len1 = dot (vec1, vec1);
399  len2 = dot (vec2, vec2);
400  len3 = dot (vec3, vec3);
401  if (len1 >= len2 && len1 >= len3)
402  {
403  mmax[0] = len1;
404  evecs.data[0] = vec1 / sqrtf (len1);
405  min_el = len3 <= mmax[min_el]? 0: min_el;
406  max_el = len3 > mmax[max_el]? 0: max_el;
407  }
408  else if (len2 >= len1 && len2 >= len3)
409  {
410  mmax[0] = len2;
411  evecs.data[0] = vec2 / sqrtf (len2);
412  min_el = len3 <= mmax[min_el]? 0: min_el;
413  max_el = len3 > mmax[max_el]? 0: max_el;
414  }
415  else
416  {
417  mmax[0] = len3;
418  evecs.data[0] = vec3 / sqrtf (len3);
419  min_el = len3 <= mmax[min_el]? 0: min_el;
420  max_el = len3 > mmax[max_el]? 0: max_el;
421  }
422 
423  unsigned mid_el = 3 - min_el - max_el;
424  evecs.data[min_el] = normalize (cross (evecs.data[(min_el+1)%3], evecs.data[(min_el+2)%3] ));
425  evecs.data[mid_el] = normalize (cross (evecs.data[(mid_el+1)%3], evecs.data[(mid_el+2)%3] ));
426  }
427  // Rescale back to the original size.
428  evals *= scale;
429  }
430 
431  /** \brief Simple kernel to add two points. */
432  struct AddPoints
433  {
434  __inline__ __host__ __device__ float3
435  operator () (float3 lhs, float3 rhs)
436  {
437  return lhs + rhs;
438  }
439  };
440 
441  /** \brief Adds two matrices element-wise. */
443  {
444  __inline__ __host__ __device__
447  {
448  CovarianceMatrix ret;
449  ret.data[0] = lhs.data[0] + rhs.data[0];
450  ret.data[1] = lhs.data[1] + rhs.data[1];
451  ret.data[2] = lhs.data[2] + rhs.data[2];
452  return ret;
453  }
454  };
455 
456  /** \brief Simple kernel to convert a PointXYZRGB to float3. Relies the cast operator of PointXYZRGB. */
458  {
459  __inline__ __host__ __device__ float3
460  operator () (const PointXYZRGB& pt) {return pt;}
461  };
462 
463  /** \brief Kernel to compute a ``covariance matrix'' for a single point. */
465  {
466  float3 centroid_;
467  __inline__ __host__ __device__
468  ComputeCovarianceForPoint (const float3& centroid) : centroid_ (centroid) {}
469 
470  __inline__ __host__ __device__ CovarianceMatrix
471  operator () (const PointXYZRGB& point)
472  {
473  CovarianceMatrix cov;
474  float3 pt = point - centroid_;
475  cov.data[1].y = pt.y * pt.y;
476  cov.data[1].z = pt.y * pt.z;
477  cov.data[2].z = pt.z * pt.z;
478 
479  pt *= pt.x;
480  cov.data[0].x = pt.x;
481  cov.data[0].y = pt.y;
482  cov.data[0].z = pt.z;
483  return cov;
484  }
485  };
486 
487  /** \brief Computes a centroid for a given range of points. */
488  template <class IteratorT>
489  void compute3DCentroid (IteratorT begin, IteratorT end, float3& centroid)
490  {
491  // Compute Centroid:
492  centroid.x = centroid.y = centroid.z = 0;
493  // we need a way to iterate over the inliers in the point cloud.. permutation_iterator to the rescue
494  centroid = transform_reduce (begin, end, convert_point_to_float3 (), centroid, AddPoints ());
495  centroid /= (float) (end - begin);
496  }
497 
498  /** \brief Computes a covariance matrix for a given range of points. */
499  template <class IteratorT>
500  void computeCovariance (IteratorT begin, IteratorT end, CovarianceMatrix& cov, float3 centroid)
501  {
502  cov.data[0] = make_float3 (0.0f, 0.0f, 0.0f);
503  cov.data[1] = make_float3 (0.0f, 0.0f, 0.0f);
504  cov.data[2] = make_float3 (0.0f, 0.0f, 0.0f);
505 
506  cov = transform_reduce (begin, end,
507  ComputeCovarianceForPoint (centroid),
508  cov,
509  AddCovariances ());
510 
511  // fill in the lower triangle (symmetry)
512  cov.data[1].x = cov.data[0].y;
513  cov.data[2].x = cov.data[0].z;
514  cov.data[2].y = cov.data[1].z;
515 
516  // divide by number of inliers
517  cov.data[0] /= (float) (end - begin);
518  cov.data[1] /= (float) (end - begin);
519  cov.data[2] /= (float) (end - begin);
520  }
521 
522  /** Kernel to compute a radius neighborhood given a organized point cloud (aka range image cloud) */
523  template <typename CloudPtr>
525  {
526  public:
527  OrganizedRadiusSearch (const CloudPtr &input, float focalLength, float sqr_radius)
528  : points_(thrust::raw_pointer_cast (&input->points[0]))
529  , focalLength_(focalLength)
530  , width_ (input->width)
531  , height_ (input->height)
532  , sqr_radius_ (sqr_radius)
533  {}
534 
535  //////////////////////////////////////////////////////////////////////////////////////////////
536  // returns float4: .x = min_x, .y = max_x, .z = min_y, .w = max_y
537  // note: assumes the projection of a point falls onto the image lattice! otherwise, min_x might be bigger than max_x !!!
538  inline __host__ __device__
539  int4
540  getProjectedRadiusSearchBox (const float3& point_arg)
541  {
542  int4 res;
543  float r_quadr, z_sqr;
544  float sqrt_term_y, sqrt_term_x, norm;
545  float x_times_z, y_times_z;
546 
547  // 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
548  // where b = p_q_arg.y, c = p_q_arg.z, r = radius_arg, f = focalLength_
549 
550  r_quadr = sqr_radius_ * sqr_radius_;
551  z_sqr = point_arg.z * point_arg.z;
552 
553  sqrt_term_y = sqrt (point_arg.y * point_arg.y * sqr_radius_ + z_sqr * sqr_radius_ - r_quadr);
554  sqrt_term_x = sqrt (point_arg.x * point_arg.x * sqr_radius_ + z_sqr * sqr_radius_ - r_quadr);
555  //sqrt_term_y = sqrt (point_arg.y * point_arg.y * sqr_radius_ + z_sqr * sqr_radius_ - r_quadr);
556  //sqrt_term_x = sqrt (point_arg.x * point_arg.x * sqr_radius_ + z_sqr * sqr_radius_ - r_quadr);
557  norm = 1.0f / (z_sqr - sqr_radius_);
558 
559  x_times_z = point_arg.x * point_arg.z;
560  y_times_z = point_arg.y * point_arg.z;
561 
562  float4 bounds;
563  bounds.x = (x_times_z - sqrt_term_x) * norm;
564  bounds.y = (x_times_z + sqrt_term_x) * norm;
565  bounds.z = (y_times_z - sqrt_term_y) * norm;
566  bounds.w = (y_times_z + sqrt_term_y) * norm;
567 
568  // determine 2-D search window
569  bounds *= focalLength_;
570  bounds.x += width_ / 2.0f;
571  bounds.y += width_ / 2.0f;
572  bounds.z += height_ / 2.0f;
573  bounds.w += height_ / 2.0f;
574 
575  res.x = (int)std::floor (bounds.x);
576  res.y = (int)std::ceil (bounds.y);
577  res.z = (int)std::floor (bounds.z);
578  res.w = (int)std::ceil (bounds.w);
579 
580  // clamp the coordinates to fit to depth image size
581  res.x = clamp (res.x, 0, width_-1);
582  res.y = clamp (res.y, 0, width_-1);
583  res.z = clamp (res.z, 0, height_-1);
584  res.w = clamp (res.w, 0, height_-1);
585  return res;
586  }
587 
588  //////////////////////////////////////////////////////////////////////////////////////////////
589  inline __host__ __device__
590  int radiusSearch (const float3 &query_pt, int k_indices[], int max_nnn)
591  {
592  // bounds.x = min_x, .y = max_x, .z = min_y, .w = max_y
593  int4 bounds = getProjectedRadiusSearchBox(query_pt);
594 
595  int nnn = 0;
596  // iterate over all pixels in the rectangular region
597  for (int x = bounds.x; (x <= bounds.y) & (nnn < max_nnn); ++x)
598  {
599  for (int y = bounds.z; (y <= bounds.w) & (nnn < max_nnn); ++y)
600  {
601  int idx = y * width_ + x;
602 
603  if (isnan (points_[idx].x))
604  continue;
605 
606  float3 point_dif = points_[idx] - query_pt;
607 
608  // check distance and add to results
609  if (dot (point_dif, point_dif) <= sqr_radius_)
610  {
611  k_indices[nnn] = idx;
612  ++nnn;
613  }
614  }
615  }
616 
617  return nnn;
618  }
619 
620  //////////////////////////////////////////////////////////////////////////////////////////////
621  inline __host__ __device__
622  int computeCovarianceOnline (const float3 &query_pt, CovarianceMatrix &cov, float sqrt_desired_nr_neighbors)
623  {
624  // bounds.x = min_x, .y = max_x, .z = min_y, .w = max_y
625  //
626  //sqr_radius_ = query_pt.z * (0.2f / 4.0f);
627  //sqr_radius_ *= sqr_radius_;
628  int4 bounds = getProjectedRadiusSearchBox(query_pt);
629 
630  // This implements a fixed window size in image coordinates (pixels)
631  //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);
632  //int window_size = 1;
633  //int4 bounds = make_int4 (
634  // proj_point.x - window_size,
635  // proj_point.x + window_size,
636  // proj_point.y - window_size,
637  // proj_point.y + window_size
638  // );
639 
640  // clamp the coordinates to fit to depth image size
641  bounds.x = clamp (bounds.x, 0, width_-1);
642  bounds.y = clamp (bounds.y, 0, width_-1);
643  bounds.z = clamp (bounds.z, 0, height_-1);
644  bounds.w = clamp (bounds.w, 0, height_-1);
645  //int4 bounds = getProjectedRadiusSearchBox(query_pt);
646 
647  // number of points in rectangular area
648  //int boundsarea = (bounds.y-bounds.x) * (bounds.w-bounds.z);
649  //float skip = max (sqrtf ((float)boundsarea) / sqrt_desired_nr_neighbors, 1.0);
650  float skipX = max (sqrtf ((float)bounds.y-bounds.x) / sqrt_desired_nr_neighbors, 1.0f);
651  float skipY = max (sqrtf ((float)bounds.w-bounds.z) / sqrt_desired_nr_neighbors, 1.0f);
652  skipX = 1;
653  skipY = 1;
654 
655  cov.data[0] = make_float3(0,0,0);
656  cov.data[1] = make_float3(0,0,0);
657  cov.data[2] = make_float3(0,0,0);
658  float3 centroid = make_float3(0,0,0);
659  int nnn = 0;
660  // iterate over all pixels in the rectangular region
661  for (float y = (float) bounds.z; y <= bounds.w; y += skipY)
662  {
663  for (float x = (float) bounds.x; x <= bounds.y; x += skipX)
664  {
665  // find index in point cloud from x,y pixel positions
666  int idx = ((int)y) * width_ + ((int)x);
667 
668  // ignore invalid points
669  if (isnan (points_[idx].x) | isnan (points_[idx].y) | isnan (points_[idx].z))
670  continue;
671 
672  float3 point_dif = points_[idx] - query_pt;
673 
674  // check distance and update covariance matrix
675  if (dot (point_dif, point_dif) <= sqr_radius_)
676  {
677  ++nnn;
678  float3 demean_old = points_[idx] - centroid;
679  centroid += demean_old / (float) nnn;
680  float3 demean_new = points_[idx] - centroid;
681 
682  cov.data[1].y += demean_new.y * demean_old.y;
683  cov.data[1].z += demean_new.y * demean_old.z;
684  cov.data[2].z += demean_new.z * demean_old.z;
685 
686  demean_old *= demean_new.x;
687  cov.data[0].x += demean_old.x;
688  cov.data[0].y += demean_old.y;
689  cov.data[0].z += demean_old.z;
690  }
691  }
692  }
693 
694  cov.data[1].x = cov.data[0].y;
695  cov.data[2].x = cov.data[0].z;
696  cov.data[2].y = cov.data[1].z;
697  cov.data[0] /= (float) nnn;
698  cov.data[1] /= (float) nnn;
699  cov.data[2] /= (float) nnn;
700  return nnn;
701  }
702 
703  //////////////////////////////////////////////////////////////////////////////////////////////
704  inline __host__ __device__
705  float3 computeCentroid (const float3 &query_pt, CovarianceMatrix &cov, float sqrt_desired_nr_neighbors)
706  {
707  // bounds.x = min_x, .y = max_x, .z = min_y, .w = max_y
708  //
709  //sqr_radius_ = query_pt.z * (0.2f / 4.0f);
710  //sqr_radius_ *= sqr_radius_;
711  int4 bounds = getProjectedRadiusSearchBox(query_pt);
712 
713  // This implements a fixed window size in image coordinates (pixels)
714  //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);
715  //int window_size = 1;
716  //int4 bounds = make_int4 (
717  // proj_point.x - window_size,
718  // proj_point.x + window_size,
719  // proj_point.y - window_size,
720  // proj_point.y + window_size
721  // );
722 
723  // clamp the coordinates to fit to depth image size
724  bounds.x = clamp (bounds.x, 0, width_-1);
725  bounds.y = clamp (bounds.y, 0, width_-1);
726  bounds.z = clamp (bounds.z, 0, height_-1);
727  bounds.w = clamp (bounds.w, 0, height_-1);
728 
729  // number of points in rectangular area
730  //int boundsarea = (bounds.y-bounds.x) * (bounds.w-bounds.z);
731  //float skip = max (sqrtf ((float)boundsarea) / sqrt_desired_nr_neighbors, 1.0);
732  float skipX = max (sqrtf ((float)bounds.y-bounds.x) / sqrt_desired_nr_neighbors, 1.0f);
733  float skipY = max (sqrtf ((float)bounds.w-bounds.z) / sqrt_desired_nr_neighbors, 1.0f);
734 
735  skipX = 1;
736  skipY = 1;
737  float3 centroid = make_float3(0,0,0);
738  int nnn = 0;
739  // iterate over all pixels in the rectangular region
740  for (float y = (float) bounds.z; y <= bounds.w; y += skipY)
741  {
742  for (float x = (float) bounds.x; x <= bounds.y; x += skipX)
743  {
744  // find index in point cloud from x,y pixel positions
745  int idx = ((int)y) * width_ + ((int)x);
746 
747  // ignore invalid points
748  if (isnan (points_[idx].x) | isnan (points_[idx].y) | isnan (points_[idx].z))
749  continue;
750 
751  float3 point_dif = points_[idx] - query_pt;
752 
753  // check distance and update covariance matrix
754  if (dot (point_dif, point_dif) <= sqr_radius_)
755  {
756  centroid += points_[idx];
757  ++nnn;
758  }
759  }
760  }
761 
762  return centroid / (float) nnn;
763  }
764 
768  float sqr_radius_;
769  };
770 
771  } // namespace
772 } // namespace
Kernel to compute a radius neighborhood given a organized point cloud (aka range image cloud)
Definition: eigen.h:525
__host__ __device__ int radiusSearch(const float3 &query_pt, int k_indices[], int max_nnn)
Definition: eigen.h:590
__host__ __device__ float3 computeCentroid(const float3 &query_pt, CovarianceMatrix &cov, float sqrt_desired_nr_neighbors)
Definition: eigen.h:705
OrganizedRadiusSearch(const CloudPtr &input, float focalLength, float sqr_radius)
Definition: eigen.h:527
__host__ __device__ int computeCovarianceOnline(const float3 &query_pt, CovarianceMatrix &cov, float sqrt_desired_nr_neighbors)
Definition: eigen.h:622
__host__ __device__ int4 getProjectedRadiusSearchBox(const float3 &point_arg)
Definition: eigen.h:540
const PointXYZRGB * points_
Definition: eigen.h:766
__host__ __device__ float3 unitOrthogonal(const float3 &src)
Definition: eigen.h:110
void computeCovariance(IteratorT begin, IteratorT end, CovarianceMatrix &cov, float3 centroid)
Computes a covariance matrix for a given range of points.
Definition: eigen.h:500
__host__ __device__ void eigen33(const CovarianceMatrix &mat, CovarianceMatrix &evecs, float3 &evals)
Definition: eigen.h:227
__host__ __device__ void computeRoots(const CovarianceMatrix &m, float3 &roots)
Definition: eigen.h:164
__host__ __device__ void swap(float &a, float &b)
Definition: eigen.h:156
__host__ __device__ bool isMuchSmallerThan(float x, float y)
Definition: eigen.h:102
__host__ __device__ void computeRoots2(const float &b, const float &c, float3 &roots)
Definition: eigen.h:143
void compute3DCentroid(IteratorT begin, IteratorT end, float3 &centroid)
Computes a centroid for a given range of points.
Definition: eigen.h:489
Adds two matrices element-wise.
Definition: eigen.h:443
__inline__ __host__ __device__ CovarianceMatrix operator()(CovarianceMatrix lhs, CovarianceMatrix rhs)
Definition: eigen.h:446
Simple kernel to add two points.
Definition: eigen.h:433
__inline__ __host__ __device__ float3 operator()(float3 lhs, float3 rhs)
Definition: eigen.h:435
Kernel to compute a `‘covariance matrix’' for a single point.
Definition: eigen.h:465
__inline__ __host__ __device__ CovarianceMatrix operator()(const PointXYZRGB &point)
Definition: eigen.h:471
__inline__ __host__ __device__ ComputeCovarianceForPoint(const float3 &centroid)
Definition: eigen.h:468
misnamed class holding a 3x3 matrix
Definition: point_cloud.h:50
Default point xyz-rgb structure.
Definition: point_types.h:49
Simple kernel to convert a PointXYZRGB to float3.
Definition: eigen.h:458
__inline__ __host__ __device__ float3 operator()(const PointXYZRGB &pt)
Definition: eigen.h:460