41 #ifndef PCL_REGISTRATION_SAMPLE_CONSENSUS_PREREJECTIVE_HPP_
42 #define PCL_REGISTRATION_SAMPLE_CONSENSUS_PREREJECTIVE_HPP_
45 template <
typename Po
intSource,
typename Po
intTarget,
typename FeatureT>
void
48 if (features == NULL || features->empty ())
50 PCL_ERROR (
"[pcl::%s::setSourceFeatures] Invalid or empty point cloud dataset given!\n", getClassName ().c_str ());
53 input_features_ = features;
57 template <
typename Po
intSource,
typename Po
intTarget,
typename FeatureT>
void
60 if (features == NULL || features->empty ())
62 PCL_ERROR (
"[pcl::%s::setTargetFeatures] Invalid or empty point cloud dataset given!\n", getClassName ().c_str ());
65 target_features_ = features;
66 feature_tree_->setInputCloud (target_features_);
70 template <
typename Po
intSource,
typename Po
intTarget,
typename FeatureT>
void
73 std::vector<int> &sample_indices)
75 if (nr_samples > static_cast<int> (cloud.points.size ()))
77 PCL_ERROR (
"[pcl::%s::selectSamples] ", getClassName ().c_str ());
78 PCL_ERROR (
"The number of samples (%d) must not be greater than the number of points (%zu)!\n",
79 nr_samples, cloud.points.size ());
84 sample_indices.clear ();
85 std::vector<bool> sampled_indices (cloud.points.size (),
false);
86 while (static_cast<int> (sample_indices.size ()) < nr_samples)
92 sample_index = getRandomIndex (static_cast<int> (cloud.points.size ()));
94 while (sampled_indices[sample_index]);
97 sampled_indices[sample_index] =
true;
100 sample_indices.push_back (sample_index);
105 template <
typename Po
intSource,
typename Po
intTarget,
typename FeatureT>
void
107 const FeatureCloud &input_features,
const std::vector<int> &sample_indices,
108 std::vector<int> &corresponding_indices)
110 std::vector<int> nn_indices (k_correspondences_);
111 std::vector<float> nn_distances (k_correspondences_);
113 corresponding_indices.resize (sample_indices.size ());
114 for (
size_t i = 0; i < sample_indices.size (); ++i)
117 feature_tree_->nearestKSearch (input_features, sample_indices[i], k_correspondences_, nn_indices, nn_distances);
120 if (k_correspondences_ == 1)
122 corresponding_indices[i] = nn_indices[0];
126 int random_correspondence = getRandomIndex (k_correspondences_);
127 corresponding_indices[i] = nn_indices[random_correspondence];
133 template <
typename Po
intSource,
typename Po
intTarget,
typename FeatureT>
void
137 if (!input_features_)
139 PCL_ERROR (
"[pcl::%s::computeTransformation] ", getClassName ().c_str ());
140 PCL_ERROR (
"No source features were given! Call setSourceFeatures before aligning.\n");
143 if (!target_features_)
145 PCL_ERROR (
"[pcl::%s::computeTransformation] ", getClassName ().c_str ());
146 PCL_ERROR (
"No target features were given! Call setTargetFeatures before aligning.\n");
150 if (input_->size () != input_features_->size ())
152 PCL_ERROR (
"[pcl::%s::computeTransformation] ", getClassName ().c_str ());
153 PCL_ERROR (
"The source points and source feature points need to be in a one-to-one relationship! Current input cloud sizes: %ld vs %ld.\n",
154 input_->size (), input_features_->size ());
158 if (target_->size () != target_features_->size ())
160 PCL_ERROR (
"[pcl::%s::computeTransformation] ", getClassName ().c_str ());
161 PCL_ERROR (
"The target points and target feature points need to be in a one-to-one relationship! Current input cloud sizes: %ld vs %ld.\n",
162 target_->size (), target_features_->size ());
166 if (inlier_fraction_ < 0.0f || inlier_fraction_ > 1.0f)
168 PCL_ERROR (
"[pcl::%s::computeTransformation] ", getClassName ().c_str ());
169 PCL_ERROR (
"Illegal inlier fraction %f, must be in [0,1]!\n",
174 const float similarity_threshold = correspondence_rejector_poly_->getSimilarityThreshold ();
175 if (similarity_threshold < 0.0f || similarity_threshold >= 1.0f)
177 PCL_ERROR (
"[pcl::%s::computeTransformation] ", getClassName ().c_str ());
178 PCL_ERROR (
"Illegal prerejection similarity threshold %f, must be in [0,1[!\n",
179 similarity_threshold);
184 correspondence_rejector_poly_->setInputSource (input_);
185 correspondence_rejector_poly_->setInputTarget (target_);
186 correspondence_rejector_poly_->setCardinality (nr_samples_);
187 std::vector<bool> accepted (input_->size (),
false);
188 int num_rejections = 0;
191 final_transformation_ = guess;
193 float lowest_error = std::numeric_limits<float>::max ();
197 std::vector<int> inliers;
198 float inlier_fraction;
202 if (!guess.isApprox (Eigen::Matrix4f::Identity (), 0.01f))
204 getFitness (inliers, error);
205 inlier_fraction =
static_cast<float> (inliers.size ()) / static_cast<float> (input_->size ());
206 error /=
static_cast<float> (inliers.size ());
208 if (inlier_fraction >= inlier_fraction_ && error < lowest_error)
211 lowest_error = error;
217 for (
int i = 0; i < max_iterations_; ++i)
220 std::vector<int> sample_indices (nr_samples_);
221 std::vector<int> corresponding_indices (nr_samples_);
224 selectSamples (*input_, nr_samples_, sample_indices);
227 bool samples_accepted =
true;
228 for (
unsigned int j = 0; j < sample_indices.size(); ++j) {
230 samples_accepted =
false;
236 if (samples_accepted)
240 findSimilarFeatures (*input_features_, sample_indices, corresponding_indices);
243 if (!correspondence_rejector_poly_->thresholdPolygon (sample_indices, corresponding_indices)) {
249 transformation_estimation_->estimateRigidTransformation (*input_, sample_indices, *target_, corresponding_indices, transformation_);
252 const Matrix4 final_transformation_prev = final_transformation_;
255 final_transformation_ = transformation_;
258 getFitness (inliers, error);
261 final_transformation_ = final_transformation_prev;
264 inlier_fraction =
static_cast<float> (inliers.size ()) / static_cast<float> (input_->size ());
266 if (inlier_fraction >= inlier_fraction_) {
268 for (
int j = 0; j < nr_samples_; ++j)
272 if (error < lowest_error) {
274 lowest_error = error;
276 final_transformation_ = transformation_;
286 PCL_DEBUG(
"[pcl::%s::computeTransformation] Rejected %i out of %i generated pose hypotheses.\n",
287 getClassName ().c_str (), num_rejections, max_iterations_);
291 template <
typename Po
intSource,
typename Po
intTarget,
typename FeatureT>
void
296 inliers.reserve (input_->size ());
297 fitness_score = 0.0f;
300 const float max_range = corr_dist_threshold_ * corr_dist_threshold_;
304 input_transformed.resize (input_->size ());
308 for (
size_t i = 0; i < input_transformed.points.size (); ++i)
311 std::vector<int> nn_indices (1);
312 std::vector<float> nn_dists (1);
313 tree_->nearestKSearch (input_transformed.points[i], 1, nn_indices, nn_dists);
316 if (nn_dists[0] < max_range)
319 const float dx = input_transformed.points[i].x - target_->points[nn_indices[0]].x;
320 const float dy = input_transformed.points[i].y - target_->points[nn_indices[0]].y;
321 const float dz = input_transformed.points[i].z - target_->points[nn_indices[0]].z;
324 inliers.push_back (static_cast<int> (i));
327 fitness_score += dx*dx + dy*dy + dz*dz;
332 if (inliers.size () > 0)
333 fitness_score /= static_cast<float> (inliers.size ());
335 fitness_score = std::numeric_limits<float>::max ();