| 56 | //=============================================================================================================================================== |
| 57 | |
| 58 | void |
| 59 | pcl::recognition::ObjRecRANSAC::recognize (const PointCloudIn& scene, const PointCloudN& normals, std::list<ObjRecRANSAC::Output>& recognized_objects, double success_probability) |
| 60 | { |
| 61 | // Clear some stuff |
| 62 | this->clearTestData (); |
| 63 | |
| 64 | // Build the scene octree |
| 65 | scene_octree_.build(scene, voxel_size_, &normals); |
| 66 | // Project it on the xy-plane (which roughly corresponds to the projection plane of the scanning device) |
| 67 | scene_octree_proj_.build(scene_octree_, abs_zdist_thresh_, abs_zdist_thresh_); |
| 68 | |
| 69 | // Needed only if icp hypotheses refinement is to be performed |
| 70 | scene_octree_points_ = PointCloudIn::Ptr (new PointCloudIn ()); |
| 71 | // First, get the scene octree points |
| 72 | scene_octree_.getFullLeavesPoints (*scene_octree_points_); |
| 73 | |
| 74 | if ( do_icp_hypotheses_refinement_ ) |
| 75 | { |
| 76 | // Build the ICP instance with the scene points as the target |
| 77 | trimmed_icp_.init (scene_octree_points_); |
| 78 | trimmed_icp_.setNewToOldEnergyRatio (0.99f); |
| 79 | } |
| 80 | |
| 81 | if ( success_probability >= 1.0 ) |
| 82 | success_probability = 0.99; |
| 83 | |
| 84 | // Compute the number of iterations |
| 85 | std::vector<ORROctree::Node*>& full_leaves = scene_octree_.getFullLeaves(); |
| 86 | int num_iterations = this->computeNumberOfIterations(success_probability), num_full_leaves = static_cast<int> (full_leaves.size ()); |
| 87 | |
| 88 | // Make sure that there are not more iterations than full leaves |
| 89 | if ( num_iterations > num_full_leaves ) |
| 90 | num_iterations = num_full_leaves; |
| 91 | |
| 92 | #ifdef OBJ_REC_RANSAC_VERBOSE |
| 93 | printf("ObjRecRANSAC::%s(): recognizing objects [%i iteration(s)]\n", __func__, num_iterations); |
| 94 | #endif |
| 95 | |
| 96 | // First, sample oriented point pairs (opps) |
| 97 | this->sampleOrientedPointPairs (num_iterations, full_leaves, sampled_oriented_point_pairs_); |
| 98 | |
| 99 | // Leave if we are in the SAMPLE_OPP test mode |
| 100 | if ( rec_mode_ == ObjRecRANSAC::SAMPLE_OPP ) |
| 101 | return; |
| 102 | |
| 103 | // Generate hypotheses from the sampled opps |
| 104 | std::list<HypothesisBase> pre_hypotheses; |
| 105 | int num_hypotheses = this->generateHypotheses (sampled_oriented_point_pairs_, pre_hypotheses); |
| 106 | |
| 107 | // Cluster the hypotheses |
| 108 | HypothesisOctree grouped_hypotheses; |
| 109 | this->groupHypotheses (pre_hypotheses, num_hypotheses, transform_space_, grouped_hypotheses); |
| 110 | pre_hypotheses.clear (); |
| 111 | |
| 112 | // The last graph-based steps in the algorithm |
| 113 | ORRGraph<Hypothesis> graph_of_close_hypotheses; |
| 114 | this->buildGraphOfCloseHypotheses (grouped_hypotheses, graph_of_close_hypotheses); |
| 115 | this->filterGraphOfCloseHypotheses (graph_of_close_hypotheses, accepted_hypotheses_); |