1 /* 2 * Software License Agreement (BSD License) 3 * 4 * Copyright (c) 2011, 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 38 #include <pcl/pcl_exports.h> 39 40 #include <pcl/cuda/sample_consensus/sac_model_1point_plane.h> 41 #include <pcl/cuda/common/eigen.h> 42 #include <pcl/cuda/cutil_math.h> 43 44 #include <thrust/copy.h> 45 #include <thrust/count.h> 46 #include <thrust/random.h> 47 48 #include <vector_types.h> 49 #include <stdio.h> 50 #include <limits> 51 52 // specify inlier computation method 53 //#define KINECT_NORMALS 54 #define KINECT 55 56 namespace pcl 57 { 58 namespace cuda 59 { 60 61 ////////////////////////////////////////////////////////////////////////// 62 template <template <typename> class Storage> SampleConsensusModel1PointPlane(const PointCloudConstPtr & cloud)63 SampleConsensusModel1PointPlane<Storage>::SampleConsensusModel1PointPlane ( 64 const PointCloudConstPtr &cloud) : 65 SampleConsensusModel<Storage> (cloud) 66 { 67 } 68 69 ////////////////////////////////////////////////////////////////////////// 70 template <template <typename> class Storage> void getSamples(int & iterations,Indices & samples)71 SampleConsensusModel1PointPlane<Storage>::getSamples (int &iterations, Indices &samples) 72 { 73 samples.resize (1); 74 float trand = indices_->size () / (RAND_MAX + 1.0f); 75 int idx = (int)(rngl_ () * trand); 76 samples[0] = (*indices_)[idx]; 77 } 78 79 ////////////////////////////////////////////////////////////////////////// 80 template <template <typename> class Storage> bool computeModelCoefficients(const Indices & samples,Coefficients & model_coefficients)81 SampleConsensusModel1PointPlane<Storage>::computeModelCoefficients ( 82 const Indices &samples, Coefficients &model_coefficients) 83 { 84 if (samples.size () != 1) 85 return (false); 86 87 /* if (isnan ((PointXYZRGB)input_->points[samples[0]]).x || 88 isnan ((PointXYZRGB)input_->points[samples[1]]).x || 89 isnan ((PointXYZRGB)input_->points[samples[2]]).x) 90 return (false);*/ 91 92 float3 normal; 93 normal.x = 0; 94 normal.y = 0; 95 normal.z = -1; 96 97 // Compute the plane coefficients 98 // calculate the plane normal n = (p2-p1) x (p3-p1) = cross (p2-p1, p3-p1) 99 float3 mc = normalize (normal); 100 101 if (model_coefficients.size () != 4) 102 model_coefficients.resize (4); 103 model_coefficients[0] = mc.x; 104 model_coefficients[1] = mc.y; 105 model_coefficients[2] = mc.z; 106 // ... + d = 0 107 model_coefficients[3] = -1 * dot (mc, ((PointXYZRGB)input_->points[samples[0]]).xyz); 108 109 return (true); 110 } 111 112 __host__ __device__ hash(unsigned int a)113 unsigned int hash(unsigned int a) 114 { 115 a = (a+0x7ed55d16) + (a<<12); 116 a = (a^0xc761c23c) ^ (a>>19); 117 a = (a+0x165667b1) + (a<<5); 118 a = (a+0xd3a2646c) ^ (a<<9); 119 a = (a+0xfd7046c5) + (a<<3); 120 a = (a^0xb55a4f09) ^ (a>>16); 121 return a; 122 } 123 124 ////////////////////////////////////////////////////////////////////////// 125 template <template <typename> class Storage> 126 //template <typename Tuple> 127 thrust::tuple <int, float4> operator ()(int t)128 Create1PointPlaneSampleHypothesis<Storage>::operator () (int t) 129 { 130 float4 coeff; 131 coeff.x = coeff.y = coeff.z = coeff.w = 5; 132 133 float trand = (float) nr_indices / (thrust::default_random_engine::max + 1.0f); 134 //rng.seed (hash (t)); 135 136 //int sample_point = indices[(int)(rng () * trand)]; 137 int sample_point = indices[(int)(t * trand)]; 138 139 if (isnan (input[sample_point].x)) 140 return (thrust::make_tuple (sample_point, coeff)); 141 142 #if 0 143 //TODO:: kind of important: get normal! :D 144 int xIdx = sample_point % width_; 145 int yIdx = sample_point / width_; 146 147 //int counter = 1; 148 149 int window_size = 3; 150 int left_index = 0; 151 int top_index = 0; 152 // West 153 if (xIdx >= window_size) 154 { 155 left_index = sample_point - window_size; 156 } 157 else 158 { 159 left_index = sample_point + window_size; 160 } 161 162 // North 163 if (yIdx >= window_size) 164 { 165 top_index = sample_point - window_size * width_; 166 } 167 else 168 { 169 top_index = sample_point + window_size * width_; 170 } 171 172 float3 left_point; 173 174 left_point.x = input[left_index].x - input[sample_point].x; 175 left_point.y = input[left_index].y - input[sample_point].y; 176 left_point.z = input[left_index].z - input[sample_point].z; 177 178 float3 top_point; 179 180 top_point.x = input[top_index].x - input[sample_point].x; 181 top_point.y = input[top_index].y - input[sample_point].y; 182 top_point.z = input[top_index].z - input[sample_point].z; 183 184 float3 normal = cross (top_point, left_point); 185 186 // Compute the plane coefficients from the 3 given points in a straightforward manner 187 // calculate the plane normal n = (p2-p1) x (p3-p1) = cross (p2-p1, p3-p1) 188 float3 mc = normalize (normal); 189 190 if (0 == (normal.x) && 0 == (normal.y) && 0 == (normal.z)) 191 { 192 //mc.x = mc.y = 0; 193 if (top_point.x == 0 && top_point.y == 0 && top_point.z == 0) 194 { 195 mc.x = 999999; 196 mc.y = input[top_index].x; 197 mc.z = input[sample_point].x; 198 //mc.z = top_index - sample_point; 199 //mc.z = 999999; 200 } 201 else 202 { 203 if (left_point.x == 0 && left_point.y == 0 && left_point.z == 0) 204 { 205 mc.x = mc.y = 888888; 206 mc.z = left_index - sample_point; 207 //mc.z = 888888; 208 } 209 } 210 } 211 #else 212 float3 mc = make_float3 (normals_[sample_point].x, normals_[sample_point].y, normals_[sample_point].z); 213 #endif 214 215 coeff.x = mc.x; 216 coeff.y = mc.y; 217 coeff.z = mc.z; 218 // ... + d = 0 219 coeff.w = -1 * dot (mc, input[sample_point].xyz); 220 221 return (thrust::make_tuple (sample_point, coeff)); 222 } 223 224 ////////////////////////////////////////////////////////////////////////// 225 template <template <typename> class Storage> 226 //template <typename Tuple> 227 float4 operator ()(int t)228 Create1PointPlaneHypothesis<Storage>::operator () (int t) 229 { 230 float4 coeff; 231 coeff.x = coeff.y = coeff.z = coeff.w = bad_value; 232 233 float trand = nr_indices / (RAND_MAX + 1.0f); 234 thrust::default_random_engine rng (t); 235 236 int sample_point = indices[(int)(rng () * trand)]; 237 238 if (isnan (input[sample_point].x)) 239 return (coeff); 240 241 //TODO:: kind of important: get normal! :D 242 243 //int xIdx = sample_point % width; 244 //int yIdx = sample_point / width; 245 246 //float3 b = input[sample_point]; 247 //int counter = 1; 248 249 //// West 250 //if (xIdx < width-window_size) 251 //{ 252 // b += input[sample_point + window_size]; 253 // counter += 1 254 //} 255 256 //// North 257 //if (yIdx >= window_size) 258 //{ 259 // b += input[sample_point - window_size * width]; 260 //} 261 262 //// South 263 //if (yIdx < height-window_size) 264 //{ 265 // b += input[sample_point + window_size * width]; 266 //} 267 268 //// East 269 //if (xIdx >= window_size) 270 //{ 271 // b += input[sample_point + window_size]; 272 //} 273 274 //// Estimate the XYZ centroid 275 //compute3DCentroid (cloud, xyz_centroid); 276 277 //// Compute the 3x3 covariance matrix 278 //computeCovarianceMatrix (cloud, xyz_centroid, covariance_matrix); 279 280 //// Get the plane normal and surface curvature 281 //solvePlaneParameters (covariance_matrix, xyz_centroid, plane_parameters, curvature); 282 283 //int[5] idxs; 284 //idxs[0] = sample_point; 285 // west = sample_point - window_size; 286 //else 287 // west = -1; 288 289 290 float3 normal; 291 normal.x = 0; 292 normal.y = 0; 293 normal.z = -1; 294 295 // Compute the plane coefficients from the 3 given points in a straightforward manner 296 // calculate the plane normal n = (p2-p1) x (p3-p1) = cross (p2-p1, p3-p1) 297 float3 mc = normalize (normal); 298 299 coeff.x = mc.x; 300 coeff.y = mc.y; 301 coeff.z = mc.z; 302 // ... + d = 0 303 coeff.w = -1 * dot (mc, input[sample_point].xyz); 304 305 return (coeff); 306 } 307 308 ////////////////////////////////////////////////////////////////////////// 309 template <template <typename> class Storage> bool generateModelHypotheses(Hypotheses & h,int max_iterations)310 SampleConsensusModel1PointPlane<Storage>::generateModelHypotheses ( 311 Hypotheses &h, int max_iterations) 312 { 313 using namespace thrust; 314 315 // Create a vector of how many samples/coefficients do we want to get 316 h.resize (max_iterations); 317 318 typename Storage<int>::type randoms (max_iterations); 319 // a sequence counting up from 0 320 thrust::counting_iterator<int> index_sequence_begin (0); 321 // transform the range [0,1,2,...N] 322 // to a range of random numbers 323 thrust::transform (index_sequence_begin, 324 index_sequence_begin + max_iterations, 325 randoms.begin (), 326 parallel_random_generator (0)); 327 328 //thrust::counting_iterator<int> first (0); 329 // Input: Point Cloud, Indices 330 // Output: Hypotheses 331 transform (//first, first + max_iterations, 332 //index_sequence_begin, 333 //index_sequence_begin + max_iterations, 334 randoms.begin (), randoms.begin () + max_iterations, 335 h.begin (), 336 Create1PointPlaneHypothesis<Storage> (thrust::raw_pointer_cast (&input_->points[0]), 337 thrust::raw_pointer_cast (&(*indices_)[0]), 338 indices_->size (), std::numeric_limits<float>::quiet_NaN ())); 339 return (true); 340 } 341 342 ////////////////////////////////////////////////////////////////////////// 343 template <template <typename> class Storage> bool generateModelHypotheses(Hypotheses & h,Samples & samples,int max_iterations)344 SampleConsensusModel1PointPlane<Storage>::generateModelHypotheses ( 345 Hypotheses &h, Samples &samples, int max_iterations) 346 { 347 using namespace thrust; 348 349 // Create a vector of how many samples/coefficients do we want to get 350 h.resize (max_iterations); 351 samples.resize (max_iterations); 352 353 typename Storage<int>::type randoms (max_iterations); 354 // a sequence counting up from 0 355 thrust::counting_iterator<int> index_sequence_begin (0); 356 // transform the range [0,1,2,...N] 357 // to a range of random numbers 358 thrust::transform (index_sequence_begin, 359 index_sequence_begin + max_iterations, 360 randoms.begin (), 361 parallel_random_generator (0)); 362 363 364 //thrust::counting_iterator<int> first (0); 365 // Input: Point Cloud, Indices 366 // Output: Hypotheses 367 transform (//first, first + max_iterations, 368 //index_sequence_begin, 369 //index_sequence_begin + max_iterations, 370 randoms.begin (), randoms.begin () + max_iterations, 371 //index_sequence_begin, index_sequence_begin + max_iterations, 372 thrust::make_zip_iterator (thrust::make_tuple (samples.begin (), h.begin())), 373 // h.begin (), 374 Create1PointPlaneSampleHypothesis<Storage> (thrust::raw_pointer_cast (&input_->points[0]), 375 thrust::raw_pointer_cast (&(*normals_)[0]), 376 thrust::raw_pointer_cast (&(*indices_)[0]), 377 input_->width, input_->height, 378 indices_->size (), std::numeric_limits<float>::quiet_NaN ())); 379 return (true); 380 } 381 382 ////////////////////////////////////////////////////////////////////////// 383 template <typename Tuple> bool operator ()(const Tuple & t)384 CountPlanarInlier::operator () (const Tuple &t) 385 { 386 if (!isfinite (thrust::raw_reference_cast(thrust::get<0>(t)).x)) 387 return (false); 388 389 //TODO: make threshold adaptive, depending on z 390 391 return (std::abs (thrust::raw_reference_cast(thrust::get<0>(t)).x * coefficients.x + 392 thrust::raw_reference_cast(thrust::get<0>(t)).y * coefficients.y + 393 thrust::raw_reference_cast(thrust::get<0>(t)).z * coefficients.z + coefficients.w) < threshold); 394 } 395 396 ////////////////////////////////////////////////////////////////////////// 397 template <template <typename> class Storage> int operator ()(const int & idx)398 NewCheckPlanarInlier<Storage>::operator () (const int &idx) 399 { 400 if (idx == -1) 401 return -1; 402 403 PointXYZRGB p = input_[idx]; 404 405 if (isnan (p.x)) 406 return -1; 407 408 if (std::abs (p.x * coefficients.x + 409 p.y * coefficients.y + 410 p.z * coefficients.z + coefficients.w) < threshold) 411 // If inlier, return its position in the vector 412 return idx; 413 else 414 // If outlier, return -1 415 return -1; 416 } 417 418 ////////////////////////////////////////////////////////////////////////// 419 template <typename Tuple> int operator ()(const Tuple & t)420 CheckPlanarInlier::operator () (const Tuple &t) 421 { 422 if (thrust::get<1>(t) == -1) 423 return (-1); 424 if (isnan (thrust::get<0>(t).x)) 425 return (-1); 426 // Fill in XYZ (and copy NaNs with it) 427 float4 pt; 428 pt.x = thrust::get<0>(t).x; 429 pt.y = thrust::get<0>(t).y; 430 pt.z = thrust::get<0>(t).z; 431 pt.w = 1; 432 433 //TODO: make threshold adaptive, depending on z 434 435 if (std::abs (dot (pt, coefficients)) < threshold) 436 // If inlier, return its position in the vector 437 return (thrust::get<1>(t)); 438 else 439 // If outlier, return -1 440 return (-1); 441 } 442 operator ()(const PointXYZRGB & pt,const int & idx)443 int CheckPlanarInlierKinectIndices::operator () (const PointXYZRGB &pt, const int &idx) 444 { 445 //if (isnan (pt.x) | isnan (pt.y) | isnan (pt.z) | (idx == -1)) 446 // return (-1); 447 448 const float b = 0.075f; 449 const float f = 580.0f/2.0f; 450 float length_pt = sqrtf (dot (pt, pt)); 451 float dot_n_p = pt.x * coefficients.x + 452 pt.y * coefficients.y + 453 pt.z * coefficients.z; 454 float D = - coefficients.w * length_pt / dot_n_p - length_pt; 455 456 float orig_disparity = b * f / pt.z; 457 float actual_disparity = orig_disparity * length_pt / (length_pt + D); 458 459 if ((std::abs (actual_disparity - orig_disparity) <= 1.0/6.0) & idx != -1) 460 return (idx); 461 else 462 return -1; 463 } 464 465 template <typename Tuple> operator ()(const Tuple & t,const int & idx)466 int CheckPlanarInlierKinectNormalIndices::operator () (const Tuple &t, const int &idx) 467 { 468 //if (isnan (pt.x) | isnan (pt.y) | isnan (pt.z) | (idx == -1)) 469 // return (-1); 470 471 const PointXYZRGB &pt = thrust::get<0>(t); 472 float4 &normal = thrust::get<1>(t); 473 474 const float b = 0.075f; 475 const float f = 580.0f/2.0f; 476 float length_pt = sqrtf (dot (pt, pt)); 477 float dot_n_p = pt.x * coefficients.x + 478 pt.y * coefficients.y + 479 pt.z * coefficients.z; 480 float D = - coefficients.w * length_pt / dot_n_p - length_pt; 481 482 float orig_disparity = b * f / pt.z; 483 float actual_disparity = orig_disparity * length_pt / (length_pt + D); 484 485 if ((std::abs (actual_disparity - orig_disparity) <= 1.0/2.0) & (idx != -1) 486 & 487 ( 488 std::abs (std::acos (normal.x*coefficients.x + normal.y*coefficients.y + normal.z*coefficients.z)) < angle_threshold 489 | 490 std::abs (std::acos (-(normal.x*coefficients.x + normal.y*coefficients.y + normal.z*coefficients.z))) < angle_threshold 491 ) 492 ) 493 return (idx); 494 else 495 return -1; 496 } 497 498 template <typename Tuple> operator ()(const Tuple & t,const int & idx)499 int CheckPlanarInlierNormalIndices::operator () (const Tuple &t, const int &idx) 500 { 501 const PointXYZRGB &pt = thrust::get<0>(t); 502 if (isnan (pt.x) | isnan (pt.y) | isnan (pt.z) | (idx == -1)) 503 return (-1); 504 505 float4 &normal = thrust::get<1>(t); 506 //TODO: make threshold adaptive, depending on z 507 508 if (std::abs (pt.x * coefficients.x + 509 pt.y * coefficients.y + 510 pt.z * coefficients.z + coefficients.w) < threshold 511 & 512 ( 513 std::abs (std::acos (normal.x*coefficients.x + normal.y*coefficients.y + normal.z*coefficients.z)) < angle_threshold 514 | 515 std::abs (std::acos (-(normal.x*coefficients.x + normal.y*coefficients.y + normal.z*coefficients.z))) < angle_threshold 516 ) 517 ) 518 // If inlier, return its position in the vector 519 return (idx); 520 else 521 // If outlier, return -1 522 return (-1); 523 } 524 operator ()(const PointXYZRGB & pt,const int & idx)525 int CheckPlanarInlierIndices::operator () (const PointXYZRGB &pt, const int &idx) 526 { 527 if (idx == -1) 528 return (-1); 529 if (isnan (pt.x) | isnan (pt.y) | isnan (pt.z)) 530 return (-1); 531 532 //TODO: make threshold adaptive, depending on z 533 534 if (std::abs (pt.x * coefficients.x + 535 pt.y * coefficients.y + 536 pt.z * coefficients.z + coefficients.w) < threshold) 537 // If inlier, return its position in the vector 538 return (idx); 539 else 540 // If outlier, return -1 541 return (-1); 542 } 543 544 ////////////////////////////////////////////////////////////////////////// 545 template <template <typename> class Storage> int countWithinDistance(const Coefficients & model_coefficients,float threshold)546 SampleConsensusModel1PointPlane<Storage>::countWithinDistance ( 547 const Coefficients &model_coefficients, float threshold) 548 { 549 using namespace thrust; 550 551 // Needs a valid set of model coefficients 552 if (model_coefficients.size () != 4) 553 { 554 fprintf (stderr, "[pcl::cuda::SampleConsensusModel1PointPlane::countWithinDistance] Invalid number of model coefficients given (%lu)!\n", (unsigned long) model_coefficients.size ()); 555 return 0; 556 } 557 558 float4 coefficients; 559 coefficients.x = model_coefficients[0]; 560 coefficients.y = model_coefficients[1]; 561 coefficients.z = model_coefficients[2]; 562 coefficients.w = model_coefficients[3]; 563 564 return (int) count_if ( 565 make_zip_iterator (make_tuple (input_->points.begin (), indices_->begin ())), 566 make_zip_iterator (make_tuple (input_->points.begin (), indices_->begin ())) + 567 indices_->size (), 568 CountPlanarInlier (coefficients, threshold)); 569 } 570 571 ////////////////////////////////////////////////////////////////////////// 572 template <template <typename> class Storage> int countWithinDistance(const Hypotheses & h,int idx,float threshold)573 SampleConsensusModel1PointPlane<Storage>::countWithinDistance ( 574 const Hypotheses &h, int idx, float threshold) 575 { 576 if (isnan (((float4)h[idx]).x)) 577 return (0); 578 579 return (int) 580 (thrust::count_if ( 581 thrust::make_zip_iterator (thrust::make_tuple (input_->points.begin (), indices_->begin ())), 582 thrust::make_zip_iterator (thrust::make_tuple (input_->points.begin (), indices_->begin ())) + 583 indices_->size (), 584 CountPlanarInlier (h[idx], threshold))); 585 } 586 587 ////////////////////////////////////////////////////////////////////////// 588 template <template <typename> class Storage> int selectWithinDistance(const Coefficients & model_coefficients,float threshold,IndicesPtr & inliers,IndicesPtr & inliers_stencil)589 SampleConsensusModel1PointPlane<Storage>::selectWithinDistance ( 590 const Coefficients &model_coefficients, float threshold, IndicesPtr &inliers, IndicesPtr &inliers_stencil) 591 { 592 using namespace thrust; 593 594 // Needs a valid set of model coefficients 595 if (model_coefficients.size () != 4) 596 { 597 fprintf (stderr, "[SampleConsensusModel1PointPlane::selectWithinDistance] Invalid number of model coefficients given (%lu)!\n", (unsigned long) model_coefficients.size ()); 598 return 0; 599 } 600 601 int nr_points = (int) indices_->size (); 602 { 603 // pcl::ScopeTime t ("Resize inl"); 604 if (!inliers_stencil) 605 inliers_stencil.reset (new Indices()); 606 607 inliers_stencil->resize (nr_points); 608 } 609 610 float4 coefficients; 611 coefficients.x = model_coefficients[0]; 612 coefficients.y = model_coefficients[1]; 613 coefficients.z = model_coefficients[2]; 614 coefficients.w = model_coefficients[3]; 615 616 { 617 // pcl::ScopeTime t ("transform"); 618 // Send the data to the device 619 transform ( 620 make_zip_iterator (make_tuple (input_->points.begin (), indices_->begin ())), 621 make_zip_iterator (make_tuple (input_->points.begin (), indices_->begin ())) + 622 nr_points, 623 inliers_stencil->begin (), 624 CheckPlanarInlier (coefficients, threshold)); 625 } 626 627 { 628 // pcl::ScopeTime t ("Resize all"); 629 if (!inliers) 630 inliers.reset (new Indices()); 631 inliers->resize (nr_points); 632 } 633 typename Indices::iterator it; 634 { 635 // pcl::ScopeTime t ("copy-if"); 636 // Copy data 637 it = copy_if (inliers_stencil->begin (), inliers_stencil->end (), inliers->begin (), isInlier ()); 638 //it = remove_copy (inliers_stencil->begin (), inliers_stencil->end (), inliers->begin (), -1); 639 } 640 { 641 // pcl::ScopeTime t ("Resize"); 642 inliers->resize (it - inliers->begin ()); 643 } 644 return (int) inliers->size(); 645 } 646 647 ////////////////////////////////////////////////////////////////////////// 648 template <template <typename> class Storage> int selectWithinDistance(const Hypotheses & h,int idx,float threshold,IndicesPtr & inliers,IndicesPtr & inliers_stencil)649 SampleConsensusModel1PointPlane<Storage>::selectWithinDistance ( 650 const Hypotheses &h, int idx, float threshold, IndicesPtr &inliers, IndicesPtr &inliers_stencil) 651 { 652 using namespace thrust; 653 654 // Needs a valid set of model coefficients 655 /* if (model_coefficients.size () != 4) 656 { 657 fprintf (stderr, "[SampleConsensusModel1PointPlane::selectWithinDistance] Invalid number of model coefficients given (%lu)!\n", (unsigned long) model_coefficients.size ()); 658 return; 659 }*/ 660 661 int nr_points = (int) indices_->size (); 662 { 663 // pcl::ScopeTime t ("Resize inl"); 664 665 if (!inliers_stencil) 666 inliers_stencil.reset (new Indices()); 667 668 inliers_stencil->resize (nr_points); 669 } 670 671 float4 coefficients; 672 coefficients.x = ((float4)h[idx]).x; 673 coefficients.y = ((float4)h[idx]).y; 674 coefficients.z = ((float4)h[idx]).z; 675 coefficients.w = ((float4)h[idx]).w; 676 677 { 678 // pcl::ScopeTime t ("transform"); 679 // Send the data to the device 680 transform ( 681 make_zip_iterator (make_tuple (input_->points.begin (), indices_->begin ())), 682 make_zip_iterator (make_tuple (input_->points.begin (), indices_->begin ())) + 683 nr_points, 684 inliers_stencil->begin (), 685 CheckPlanarInlier (coefficients, threshold)); 686 } 687 688 { 689 // pcl::ScopeTime t ("Resize all"); 690 if (!inliers) 691 inliers.reset (new Indices()); 692 inliers->resize (nr_points); 693 } 694 typename Indices::iterator it; 695 { 696 // pcl::ScopeTime t ("copy-if"); 697 // Copy data 698 it = copy_if (inliers_stencil->begin (), inliers_stencil->end (), inliers->begin (), isInlier ()); 699 } 700 { 701 // pcl::ScopeTime t ("Resize"); 702 inliers->resize (it - inliers->begin ()); 703 } 704 return (int) inliers->size (); 705 } 706 707 708 709 ////////////////////////////////////////////////////////////////////////// 710 template <template <typename> class Storage> int selectWithinDistance(Hypotheses & h,int idx,float threshold,IndicesPtr & inliers_stencil,float3 & c)711 SampleConsensusModel1PointPlane<Storage>::selectWithinDistance ( 712 Hypotheses &h, int idx, float threshold, IndicesPtr &inliers_stencil, float3 &c) 713 { 714 float angle_threshold = 0.26f; 715 using namespace thrust; 716 717 int nr_points = (int) indices_stencil_->size (); 718 float bad_point = std::numeric_limits<float>::quiet_NaN (); 719 720 if (!inliers_stencil) 721 inliers_stencil.reset (new Indices()); 722 inliers_stencil->resize (nr_points); 723 724 // necessary for the transform_if call below (since not all elements get written, we init with -1).. 725 //inliers_stencil->resize (nr_points, -1); 726 727 float4 coefficients; 728 coefficients.x = ((float4)h[idx]).x; 729 coefficients.y = ((float4)h[idx]).y; 730 coefficients.z = ((float4)h[idx]).z; 731 coefficients.w = ((float4)h[idx]).w; 732 733 if (isnan (coefficients.x) | 734 isnan (coefficients.y) | 735 isnan (coefficients.z) | 736 isnan (coefficients.w) ) 737 { 738 c.x = c.y = c.z = 0; 739 return 0; 740 } 741 742 float3 best_centroid; 743 IndicesPtr best_inliers_stencil; 744 745 float3 centroid; 746 747 centroid.x = centroid.y = centroid.z = 0; 748 best_centroid = centroid; 749 750 //ORIG 751 // transform ( 752 // make_zip_iterator (make_tuple (input_->points.begin (), indices_->begin ())), 753 // make_zip_iterator (make_tuple (input_->points.begin (), indices_->begin ())) + 754 // nr_points, 755 // inliers_stencil->begin (), 756 // CheckPlanarInlier (coefficients, threshold)); 757 758 // this is just as fast as the ORIG version, but requires initialization to -1 (see above) --> much slower 759 // transform_if ( 760 // make_zip_iterator (make_tuple (input_->points.begin (), indices_->begin ())), 761 // make_zip_iterator (make_tuple (input_->points.begin (), indices_->begin ())) + 762 // nr_points, 763 // indices_->begin(), 764 // inliers_stencil->begin (), 765 // CheckPlanarInlier (coefficients, threshold), 766 // isInlier () 767 // ); 768 769 // i forgot why this was slow. but it was. :) 770 // transform ( 771 // indices_stencil_->begin (), 772 // indices_stencil_->end(), 773 // inliers_stencil->begin (), 774 // NewCheckPlanarInlier<Storage> (coefficients, (float)threshold, input_->points)); 775 776 // compute inliers 777 // fastest 778 #ifdef KINECT 779 // NOTE: this performs inlier checks with kinect disparity error model, without normal check 780 transform ( 781 input_->points.begin (), input_->points.end (), 782 indices_stencil_->begin (), 783 inliers_stencil->begin (), 784 CheckPlanarInlierKinectIndices (coefficients, threshold, angle_threshold)); 785 #endif 786 787 #ifdef KINECT_NORMALS 788 // NOTE: this performs inlier checks with kinect disparity error model, with normal check 789 transform ( 790 make_zip_iterator (make_tuple (input_->points.begin (), normals_->begin())), 791 make_zip_iterator (make_tuple (input_->points.begin (), normals_->begin())) + nr_points, 792 indices_stencil_->begin (), 793 inliers_stencil->begin (), 794 CheckPlanarInlierKinectNormalIndices (coefficients, threshold, angle_threshold)); 795 #endif 796 797 // store inliers here 798 Indices inliers; 799 inliers.resize (indices_->size ()); // is this necessary? 800 801 typename Indices::iterator last = thrust::remove_copy (inliers_stencil->begin (), inliers_stencil->end (), inliers.begin (), -1); 802 inliers.erase (last, inliers.end ()); 803 804 if (inliers.size () < 1) 805 return (int) inliers.size (); 806 807 best_inliers_stencil = inliers_stencil; 808 int best_nr_inliers = (int) inliers.size (); 809 810 int nr_inliers_after_refit = (int) inliers.size (); 811 int nr_inliers_before_refit; 812 int nr_refit_iterations = 0; 813 814 do { 815 nr_inliers_before_refit = nr_inliers_after_refit; 816 817 compute3DCentroid (make_permutation_iterator (input_->points.begin (), inliers.begin ()), 818 make_permutation_iterator (input_->points.begin (), inliers.end ()), 819 centroid); 820 821 if (isnan (centroid.x) | isnan (centroid.y) | isnan (centroid.z)) 822 { 823 std::cerr << "Wow, centroid contains nans!" << std::endl; 824 825 inliers_stencil = best_inliers_stencil; 826 c = make_float3 (bad_point, bad_point, bad_point); 827 //best_centroid; 828 return best_nr_inliers; 829 } 830 831 // Note: centroid contains centroid * inliers.size() at this point ! 832 #if 0 833 std::cerr << "----------------------------------------------------------------------------" << std::endl; 834 std::cerr << "inliers before: " << inliers.size () << std::endl; 835 std::cerr << "Centroid: " << 836 centroid.x << ", " << centroid.y << ", " << centroid.z << ", " << std::endl; 837 #endif 838 839 CovarianceMatrix covariance_matrix; 840 841 computeCovariance (make_permutation_iterator (input_->points.begin (), inliers.begin ()), 842 make_permutation_iterator (input_->points.begin (), inliers.end ()), 843 covariance_matrix, centroid); 844 845 if (isnan (covariance_matrix.data[0].x)) 846 { 847 std::cerr << "Wow, covariance matrix contains nans!" << std::endl; 848 inliers_stencil = best_inliers_stencil; 849 c = make_float3 (bad_point, bad_point, bad_point); 850 //best_centroid; 851 return best_nr_inliers; 852 } 853 854 #if 0 855 std::cerr << "Covariance: " << 856 covariance_matrix.data[0].x << ", " << covariance_matrix.data[0].y << ", " << covariance_matrix.data[0].z << std::endl << 857 covariance_matrix.data[1].x << ", " << covariance_matrix.data[1].y << ", " << covariance_matrix.data[1].z << std::endl << 858 covariance_matrix.data[2].x << ", " << covariance_matrix.data[2].y << ", " << covariance_matrix.data[2].z << std::endl; 859 #endif 860 861 CovarianceMatrix evecs; 862 float3 evals; 863 864 // compute eigenvalues and -vectors 865 eigen33 (covariance_matrix, evecs, evals); 866 867 float3 mc = normalize (evecs.data[0]); 868 869 #if 0 870 std::cerr << "Eigenvectors: " << 871 evecs.data[0].x << ", " << evecs.data[0].y << ", " << evecs.data[0].z << std::endl << 872 evecs.data[1].x << ", " << evecs.data[1].y << ", " << evecs.data[1].z << std::endl << 873 evecs.data[2].x << ", " << evecs.data[2].y << ", " << evecs.data[2].z << std::endl; 874 std::cerr << "Coefficients before: " << 875 coefficients.x << ", " << coefficients.y << ", " << coefficients.z << ", " << coefficients.w << ", " << std::endl; 876 #endif 877 878 // compute plane coefficients from eigenvector corr. to smallest eigenvalue and centroid 879 coefficients.x = mc.x; 880 coefficients.y = mc.y; 881 coefficients.z = mc.z; 882 // ... + d = 0 883 coefficients.w = -1 * dot (mc, centroid); 884 885 #if 0 886 std::cerr << "Coefficients after: " << 887 coefficients.x << ", " << coefficients.y << ", " << coefficients.z << ", " << coefficients.w << ", " << std::endl; 888 #endif 889 890 // finally, another inlier check: 891 #ifdef KINECT 892 transform ( 893 input_->points.begin (), input_->points.end (), 894 //make_zip_iterator (make_tuple (input_->points.begin (), normals_.begin())), 895 //make_zip_iterator (make_tuple (input_->points.begin (), normals_.begin())) + nr_points, 896 // input_->points.begin (), 897 // input_->points.end (), 898 indices_stencil_->begin (), 899 inliers_stencil->begin (), 900 CheckPlanarInlierKinectIndices (coefficients, threshold, angle_threshold)); 901 #endif 902 903 #ifdef KINECT_NORMALS 904 transform ( 905 make_zip_iterator (make_tuple (input_->points.begin (), normals_->begin())), 906 make_zip_iterator (make_tuple (input_->points.begin (), normals_->begin())) + nr_points, 907 indices_stencil_->begin (), 908 inliers_stencil->begin (), 909 CheckPlanarInlierKinectNormalIndices (coefficients, threshold, angle_threshold)); 910 #endif 911 912 // copy inliers from stencil to inlier vector 913 inliers.resize (inliers_stencil->size ()); // is this necessary? 914 last = thrust::remove_copy (inliers_stencil->begin (), inliers_stencil->end (), inliers.begin (), -1); 915 inliers.erase (last, inliers.end ()); 916 917 nr_inliers_after_refit = (int) inliers.size (); 918 919 compute3DCentroid (make_permutation_iterator (input_->points.begin (), inliers.begin ()), 920 make_permutation_iterator (input_->points.begin (), inliers.end ()), 921 centroid); 922 923 if (nr_inliers_after_refit > best_nr_inliers) 924 { 925 best_nr_inliers = nr_inliers_after_refit; 926 best_inliers_stencil = inliers_stencil; 927 best_centroid = centroid; 928 h[idx] = coefficients; 929 } 930 931 //fprintf (stderr, "iteration %i: %f, %f, %f, %f ---> %i\n", nr_refit_iterations, coefficients.x, coefficients.y, coefficients.z, coefficients.w, best_nr_inliers); 932 933 } while (nr_inliers_after_refit > nr_inliers_before_refit & ++nr_refit_iterations < 120); 934 935 #if 0 936 std::cerr << "inliers after: " << nr_inliers_after_refit << std::endl; 937 #endif 938 //std::cerr << "--> refitting steps: " << nr_refit_iterations << std::endl; 939 940 inliers_stencil = best_inliers_stencil; 941 c = best_centroid; 942 return best_nr_inliers; 943 } 944 945 946 // explicit template instantiation for device and host 947 template class PCL_EXPORTS SampleConsensusModel1PointPlane<Device>; 948 template class PCL_EXPORTS SampleConsensusModel1PointPlane<Host>; 949 950 } // namespace 951 } // namespace 952