Point Cloud Library (PCL) 1.13.0
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
97namespace 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
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
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 {
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. */
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 {
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
472 {
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
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