98 int num_trimmed_source_points = num_source_points_to_use, num_source_points =
static_cast<int> (source_points.size ());
100 if ( num_trimmed_source_points >= num_source_points )
102 printf (
"WARNING in 'TrimmedICP::%s()': the user-defined number of source points of interest is greater or equal to "
103 "the total number of source points. Trimmed ICP will work correctly but won't be very efficient. Either set "
104 "the number of source points to use to a lower value or use standard ICP.\n", __func__);
105 num_trimmed_source_points = num_source_points;
109 pcl::Correspondences full_src_to_tgt (num_source_points), trimmed_src_to_tgt (num_trimmed_source_points);
114 std::vector<float> sqr_dist_to_target (1);
115 float old_energy, energy = std::numeric_limits<float>::max ();
122 for (
int i = 0 ; i < num_source_points ; ++i )
125 aux::transform (guess_and_result, source_points[i], transformed_source_point);
128 kdtree_.nearestKSearch (transformed_source_point, 1, target_index, sqr_dist_to_target);
131 full_src_to_tgt[i].index_query = i;
132 full_src_to_tgt[i].index_match = target_index[0];
133 full_src_to_tgt[i].distance = sqr_dist_to_target[0];
137 std::sort (full_src_to_tgt.begin (), full_src_to_tgt.end (), TrimmedICP::compareCorrespondences);
143 for (
int i = 0 ; i < num_trimmed_source_points ; ++i )
145 trimmed_src_to_tgt[i].index_query = full_src_to_tgt[i].index_query;
146 trimmed_src_to_tgt[i].index_match = full_src_to_tgt[i].index_match;
147 energy += full_src_to_tgt[i].distance;
150 this->estimateRigidTransformation (source_points, *target_points_, trimmed_src_to_tgt, guess_and_result);
154 while ( energy/old_energy < new_to_old_energy_ratio_ );