Point Cloud Library (PCL) 1.12.1
Loading...
Searching...
No Matches
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
98namespace 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. */
441 struct AddCovariances
442 {
443 __inline__ __host__ __device__
444 CovarianceMatrix
445 operator () (CovarianceMatrix lhs, CovarianceMatrix rhs)
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. */
456 struct convert_point_to_float3
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. */
463 struct ComputeCovarianceForPoint
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>
523 class OrganizedRadiusSearch
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
764 float focalLength_;
765 const PointXYZRGB *points_;
766 int width_, height_;
767 float sqr_radius_;
768 };
769
770 } // namespace
771} // namespace
pcl::cuda::OrganizedRadiusSearch::radiusSearch
__host__ __device__ int radiusSearch(const float3 &query_pt, int k_indices[], int max_nnn)
Definition eigen.h:589
pcl::cuda::OrganizedRadiusSearch::computeCentroid
__host__ __device__ float3 computeCentroid(const float3 &query_pt, CovarianceMatrix &cov, float sqrt_desired_nr_neighbors)
Definition eigen.h:704
pcl::cuda::OrganizedRadiusSearch::OrganizedRadiusSearch
OrganizedRadiusSearch(const CloudPtr &input, float focalLength, float sqr_radius)
Definition eigen.h:526
pcl::cuda::OrganizedRadiusSearch::computeCovarianceOnline
__host__ __device__ int computeCovarianceOnline(const float3 &query_pt, CovarianceMatrix &cov, float sqrt_desired_nr_neighbors)
Definition eigen.h:621
pcl::cuda::OrganizedRadiusSearch::getProjectedRadiusSearchBox
__host__ __device__ int4 getProjectedRadiusSearchBox(const float3 &point_arg)
Definition eigen.h:539
pcl::cuda::OrganizedRadiusSearch::points_
const PointXYZRGB * points_
Definition eigen.h:765
pcl::cuda::unitOrthogonal
__host__ __device__ float3 unitOrthogonal(const float3 &src)
Definition eigen.h:109
pcl::cuda::computeCovariance
void computeCovariance(IteratorT begin, IteratorT end, CovarianceMatrix &cov, float3 centroid)
Computes a covariance matrix for a given range of points.
Definition eigen.h:499
pcl::cuda::eigen33
__host__ __device__ void eigen33(const CovarianceMatrix &mat, CovarianceMatrix &evecs, float3 &evals)
Definition eigen.h:226
pcl::cuda::computeRoots
__host__ __device__ void computeRoots(const CovarianceMatrix &m, float3 &roots)
Definition eigen.h:163
pcl::cuda::swap
__host__ __device__ void swap(float &a, float &b)
Definition eigen.h:155
pcl::cuda::isMuchSmallerThan
__host__ __device__ bool isMuchSmallerThan(float x, float y)
Definition eigen.h:103
pcl::cuda::computeRoots2
__host__ __device__ void computeRoots2(const float &b, const float &c, float3 &roots)
Definition eigen.h:142
pcl::cuda::compute3DCentroid
void compute3DCentroid(IteratorT begin, IteratorT end, float3 &centroid)
Kernel to compute a ``covariance matrix'' for a single point.
Definition eigen.h:488
pcl::cuda::AddCovariances
Adds two matrices element-wise.
Definition eigen.h:442
pcl::cuda::AddCovariances::operator()
__inline__ __host__ __device__ CovarianceMatrix operator()(CovarianceMatrix lhs, CovarianceMatrix rhs)
Definition eigen.h:445
pcl::cuda::AddPoints
Simple kernel to add two points.
Definition eigen.h:432
pcl::cuda::AddPoints::operator()
__inline__ __host__ __device__ float3 operator()(float3 lhs, float3 rhs)
Definition eigen.h:434
pcl::cuda::CovarianceMatrix
misnamed class holding a 3x3 matrix
Definition point_cloud.h:50
pcl::cuda::PointXYZRGB
Default point xyz-rgb structure.
Definition point_types.h:49
pcl::cuda::convert_point_to_float3
Simple kernel to convert a PointXYZRGB to float3.
Definition eigen.h:457
pcl::cuda::convert_point_to_float3::operator()
__inline__ __host__ __device__ float3 operator()(const PointXYZRGB &pt)
Definition eigen.h:459