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 isMuchSmallerThan(float x,float y)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 unitOrthogonal(const float3 & src)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 computeRoots2(const float & b,const float & c,float3 & roots)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 swap(float & a,float & b)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 computeRoots(const CovarianceMatrix & m,float3 & roots)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 eigen33(const CovarianceMatrix & mat,CovarianceMatrix & evecs,float3 & evals)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 operatorAddPoints434 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 operatorAddCovariances445 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 operatorconvert_point_to_float3459 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__ ComputeCovarianceForPointComputeCovarianceForPoint467 ComputeCovarianceForPoint (const float3& centroid) : centroid_ (centroid) {} 468 469 __inline__ __host__ __device__ CovarianceMatrix operatorComputeCovarianceForPoint470 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> compute3DCentroid(IteratorT begin,IteratorT end,float3 & centroid)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> computeCovariance(IteratorT begin,IteratorT end,CovarianceMatrix & cov,float3 centroid)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: OrganizedRadiusSearch(const CloudPtr & input,float focalLength,float sqr_radius)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 getProjectedRadiusSearchBox(const float3 & point_arg)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__ radiusSearch(const float3 & query_pt,int k_indices[],int max_nnn)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__ computeCovarianceOnline(const float3 & query_pt,CovarianceMatrix & cov,float sqrt_desired_nr_neighbors)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__ computeCentroid(const float3 & query_pt,CovarianceMatrix & cov,float sqrt_desired_nr_neighbors)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 772