41 #ifndef PCL_REGISTRATION_SAMPLE_CONSENSUS_PREREJECTIVE_HPP_
42 #define PCL_REGISTRATION_SAMPLE_CONSENSUS_PREREJECTIVE_HPP_
48 template <
typename Po
intSource,
typename Po
intTarget,
typename FeatureT>
void
51 if (features ==
nullptr || features->empty ())
53 PCL_ERROR (
"[pcl::%s::setSourceFeatures] Invalid or empty point cloud dataset given!\n", getClassName ().c_str ());
56 input_features_ = features;
60 template <
typename Po
intSource,
typename Po
intTarget,
typename FeatureT>
void
63 if (features ==
nullptr || features->empty ())
65 PCL_ERROR (
"[pcl::%s::setTargetFeatures] Invalid or empty point cloud dataset given!\n", getClassName ().c_str ());
68 target_features_ = features;
69 feature_tree_->setInputCloud (target_features_);
73 template <
typename Po
intSource,
typename Po
intTarget,
typename FeatureT>
void
75 const PointCloudSource &cloud,
int nr_samples, std::vector<int> &sample_indices)
77 if (nr_samples >
static_cast<int> (cloud.size ()))
79 PCL_ERROR (
"[pcl::%s::selectSamples] ", getClassName ().c_str ());
80 PCL_ERROR(
"The number of samples (%d) must not be greater than the number of "
83 static_cast<std::size_t
>(cloud.size()));
87 sample_indices.resize (nr_samples);
91 for (
int i = 0; i < nr_samples; i++)
94 sample_indices[i] = getRandomIndex (
static_cast<int> (cloud.size ()) - i);
97 for (
int j = 0; j < i; j++)
100 if (sample_indices[i] >= sample_indices[j])
107 temp_sample = sample_indices[i];
108 for (
int k = i; k > j; k--)
109 sample_indices[k] = sample_indices[k - 1];
111 sample_indices[j] = temp_sample;
119 template <
typename Po
intSource,
typename Po
intTarget,
typename FeatureT>
void
121 const std::vector<int> &sample_indices,
122 std::vector<std::vector<int> >& similar_features,
123 std::vector<int> &corresponding_indices)
126 corresponding_indices.resize (sample_indices.size ());
127 std::vector<float> nn_distances (k_correspondences_);
130 for (std::size_t i = 0; i < sample_indices.size (); ++i)
133 const int idx = sample_indices[i];
136 if (similar_features[idx].empty ())
137 feature_tree_->nearestKSearch (*input_features_, idx, k_correspondences_, similar_features[idx], nn_distances);
140 if (k_correspondences_ == 1)
141 corresponding_indices[i] = similar_features[idx][0];
143 corresponding_indices[i] = similar_features[idx][getRandomIndex (k_correspondences_)];
148 template <
typename Po
intSource,
typename Po
intTarget,
typename FeatureT>
void
152 if (!input_features_)
154 PCL_ERROR (
"[pcl::%s::computeTransformation] ", getClassName ().c_str ());
155 PCL_ERROR (
"No source features were given! Call setSourceFeatures before aligning.\n");
158 if (!target_features_)
160 PCL_ERROR (
"[pcl::%s::computeTransformation] ", getClassName ().c_str ());
161 PCL_ERROR (
"No target features were given! Call setTargetFeatures before aligning.\n");
165 if (input_->size () != input_features_->size ())
167 PCL_ERROR (
"[pcl::%s::computeTransformation] ", getClassName ().c_str ());
168 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",
169 input_->size (), input_features_->size ());
173 if (target_->size () != target_features_->size ())
175 PCL_ERROR (
"[pcl::%s::computeTransformation] ", getClassName ().c_str ());
176 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",
177 target_->size (), target_features_->size ());
181 if (inlier_fraction_ < 0.0f || inlier_fraction_ > 1.0f)
183 PCL_ERROR (
"[pcl::%s::computeTransformation] ", getClassName ().c_str ());
184 PCL_ERROR (
"Illegal inlier fraction %f, must be in [0,1]!\n",
189 const float similarity_threshold = correspondence_rejector_poly_->getSimilarityThreshold ();
190 if (similarity_threshold < 0.0f || similarity_threshold >= 1.0f)
192 PCL_ERROR (
"[pcl::%s::computeTransformation] ", getClassName ().c_str ());
193 PCL_ERROR (
"Illegal prerejection similarity threshold %f, must be in [0,1[!\n",
194 similarity_threshold);
198 if (k_correspondences_ <= 0)
200 PCL_ERROR (
"[pcl::%s::computeTransformation] ", getClassName ().c_str ());
201 PCL_ERROR (
"Illegal correspondence randomness %d, must be > 0!\n",
207 correspondence_rejector_poly_->setInputSource (input_);
208 correspondence_rejector_poly_->setInputTarget (target_);
209 correspondence_rejector_poly_->setCardinality (nr_samples_);
210 int num_rejections = 0;
213 final_transformation_ = guess;
215 float lowest_error = std::numeric_limits<float>::max ();
219 std::vector<int> inliers;
220 float inlier_fraction;
224 if (!guess.isApprox (Eigen::Matrix4f::Identity (), 0.01f))
226 getFitness (inliers, error);
227 inlier_fraction =
static_cast<float> (inliers.size ()) /
static_cast<float> (input_->size ());
229 if (inlier_fraction >= inlier_fraction_ && error < lowest_error)
232 lowest_error = error;
238 std::vector<std::vector<int> > similar_features (input_->size ());
241 for (
int i = 0; i < max_iterations_; ++i)
244 std::vector<int> sample_indices;
245 std::vector<int> corresponding_indices;
248 selectSamples (*input_, nr_samples_, sample_indices);
251 findSimilarFeatures (sample_indices, similar_features, corresponding_indices);
254 if (!correspondence_rejector_poly_->thresholdPolygon (sample_indices, corresponding_indices))
261 transformation_estimation_->estimateRigidTransformation (*input_, sample_indices, *target_, corresponding_indices, transformation_);
264 const Matrix4 final_transformation_prev = final_transformation_;
267 final_transformation_ = transformation_;
270 getFitness (inliers, error);
273 final_transformation_ = final_transformation_prev;
276 inlier_fraction =
static_cast<float> (inliers.size ()) /
static_cast<float> (input_->size ());
279 if (inlier_fraction >= inlier_fraction_ && error < lowest_error)
282 lowest_error = error;
284 final_transformation_ = transformation_;
293 PCL_DEBUG(
"[pcl::%s::computeTransformation] Rejected %i out of %i generated pose hypotheses.\n",
294 getClassName ().c_str (), num_rejections, max_iterations_);
298 template <
typename Po
intSource,
typename Po
intTarget,
typename FeatureT>
void
303 inliers.reserve (input_->size ());
304 fitness_score = 0.0f;
307 const float max_range = corr_dist_threshold_ * corr_dist_threshold_;
311 input_transformed.resize (input_->size ());
315 for (std::size_t i = 0; i < input_transformed.size (); ++i)
318 std::vector<int> nn_indices (1);
319 std::vector<float> nn_dists (1);
320 tree_->nearestKSearch (input_transformed[i], 1, nn_indices, nn_dists);
323 if (nn_dists[0] < max_range)
326 inliers.push_back (
static_cast<int> (i));
329 fitness_score += nn_dists[0];
334 if (!inliers.empty ())
335 fitness_score /=
static_cast<float> (inliers.size ());
337 fitness_score = std::numeric_limits<float>::max ();