| 45 | |
| 46 | ////////////////////////////////////////////////////////////////////////// |
| 47 | template <template <typename> class Storage> bool |
| 48 | pcl_cuda::MultiRandomSampleConsensus<Storage>::computeModel (int debug_verbosity_level) |
| 49 | { |
| 50 | // Warn and exit if no threshold was set |
| 51 | if (threshold_ == std::numeric_limits<double>::max()) |
| 52 | { |
| 53 | std::cerr << "[pcl_cuda::MultiRandomSampleConsensus::computeModel] No threshold set!" << std::endl; |
| 54 | return (false); |
| 55 | } |
| 56 | |
| 57 | // compute number of points |
| 58 | int nr_points = sac_model_->getIndices ()->size (); |
| 59 | int nr_remaining_points = nr_points; |
| 60 | //std::cerr << "nr_points = " << nr_points << std::endl; |
| 61 | // number of total iterations |
| 62 | unsigned int cur_iteration = 0; |
| 63 | // number of valid iterations |
| 64 | int valid_iterations = 0; |
| 65 | // each batch has a vector of plane coefficients (float4) |
| 66 | std::vector<Hypotheses> h(max_batches_); |
| 67 | std::vector<typename Storage<int>::type> h_samples (max_batches_); |
| 68 | std::vector<float3> centroids (max_batches_ * iterations_per_batch_); |
| 69 | // current batch number |
| 70 | int cur_batch = 0; |
| 71 | //// stencil vector that holds the current inliers |
| 72 | std::vector<IndicesPtr> hypotheses_inliers_stencils (max_batches_ * iterations_per_batch_); |
| 73 | std::vector<int> hypotheses_inlier_count (max_batches_ * iterations_per_batch_); |
| 74 | // initialize some things |
| 75 | all_inliers_.clear (); |
| 76 | all_model_coefficients_.clear (); |
| 77 | all_model_centroids_.clear (); |
| 78 | int n_inliers_count = 0; |
| 79 | int n_best_inliers_count = 0; |
| 80 | int good_coeff = -1; |
| 81 | float k = max_batches_ * iterations_per_batch_; |
| 82 | |
| 83 | //thrust::host_vector<float3> host_points = sac_model_->getInputCloud()->points; |
| 84 | //std::cerr << "Input Points:" << std::endl; |
| 85 | //for (unsigned int print_iter = 0; print_iter < nr_points; ++print_iter) |
| 86 | //{ |
| 87 | // std::cerr << print_iter << " : [ " |
| 88 | // << host_points[print_iter].x << ", " |
| 89 | // << host_points[print_iter].y << ", " |
| 90 | // << host_points[print_iter].z << " ]" << std::endl; |
| 91 | //} |
| 92 | |
| 93 | ScopeTime t ("ALLLLLLLLLLL"); |
| 94 | do // multiple models .. |
| 95 | { |
| 96 | thrust::host_vector<int> host_samples; |
| 97 | thrust::host_vector<float4> host_coeffs; |
| 98 | // make sure that sac_model_->indices_ only contains remaining point indices |
| 99 | sac_model_->getIndices (); |
| 100 | |
| 101 | // generate a new batch of hypotheses |
| 102 | { |
| 103 | ScopeTime t ("generateModelHypotheses"); |
| 104 | sac_model_->generateModelHypotheses (h[cur_batch], h_samples[cur_batch], iterations_per_batch_); |