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 ==
nullptr || 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 ==
nullptr || 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
72 const PointCloudSource &cloud,
int nr_samples, std::vector<int> &sample_indices)
74 if (nr_samples >
static_cast<int> (cloud.points.size ()))
76 PCL_ERROR (
"[pcl::%s::selectSamples] ", getClassName ().c_str ());
77 PCL_ERROR (
"The number of samples (%d) must not be greater than the number of points (%lu)!\n",
78 nr_samples, cloud.points.size ());
82 sample_indices.resize (nr_samples);
86 for (
int i = 0; i < nr_samples; i++)
89 sample_indices[i] = getRandomIndex (
static_cast<int> (cloud.points.size ()) - i);
92 for (
int j = 0; j < i; j++)
95 if (sample_indices[i] >= sample_indices[j])
102 temp_sample = sample_indices[i];
103 for (
int k = i; k > j; k--)
104 sample_indices[k] = sample_indices[k - 1];
106 sample_indices[j] = temp_sample;
114 template <
typename Po
intSource,
typename Po
intTarget,
typename FeatureT>
void
116 const std::vector<int> &sample_indices,
117 std::vector<std::vector<int> >& similar_features,
118 std::vector<int> &corresponding_indices)
121 corresponding_indices.resize (sample_indices.size ());
122 std::vector<float> nn_distances (k_correspondences_);
125 for (std::size_t i = 0; i < sample_indices.size (); ++i)
128 const int idx = sample_indices[i];
131 if (similar_features[idx].empty ())
132 feature_tree_->nearestKSearch (*input_features_, idx, k_correspondences_, similar_features[idx], nn_distances);
135 if (k_correspondences_ == 1)
136 corresponding_indices[i] = similar_features[idx][0];
138 corresponding_indices[i] = similar_features[idx][getRandomIndex (k_correspondences_)];
143 template <
typename Po
intSource,
typename Po
intTarget,
typename FeatureT>
void
147 if (!input_features_)
149 PCL_ERROR (
"[pcl::%s::computeTransformation] ", getClassName ().c_str ());
150 PCL_ERROR (
"No source features were given! Call setSourceFeatures before aligning.\n");
153 if (!target_features_)
155 PCL_ERROR (
"[pcl::%s::computeTransformation] ", getClassName ().c_str ());
156 PCL_ERROR (
"No target features were given! Call setTargetFeatures before aligning.\n");
160 if (input_->size () != input_features_->size ())
162 PCL_ERROR (
"[pcl::%s::computeTransformation] ", getClassName ().c_str ());
163 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",
164 input_->size (), input_features_->size ());
168 if (target_->size () != target_features_->size ())
170 PCL_ERROR (
"[pcl::%s::computeTransformation] ", getClassName ().c_str ());
171 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",
172 target_->size (), target_features_->size ());
176 if (inlier_fraction_ < 0.0f || inlier_fraction_ > 1.0f)
178 PCL_ERROR (
"[pcl::%s::computeTransformation] ", getClassName ().c_str ());
179 PCL_ERROR (
"Illegal inlier fraction %f, must be in [0,1]!\n",
184 const float similarity_threshold = correspondence_rejector_poly_->getSimilarityThreshold ();
185 if (similarity_threshold < 0.0f || similarity_threshold >= 1.0f)
187 PCL_ERROR (
"[pcl::%s::computeTransformation] ", getClassName ().c_str ());
188 PCL_ERROR (
"Illegal prerejection similarity threshold %f, must be in [0,1[!\n",
189 similarity_threshold);
193 if (k_correspondences_ <= 0)
195 PCL_ERROR (
"[pcl::%s::computeTransformation] ", getClassName ().c_str ());
196 PCL_ERROR (
"Illegal correspondence randomness %d, must be > 0!\n",
202 correspondence_rejector_poly_->setInputSource (input_);
203 correspondence_rejector_poly_->setInputTarget (target_);
204 correspondence_rejector_poly_->setCardinality (nr_samples_);
205 int num_rejections = 0;
208 final_transformation_ = guess;
210 float lowest_error = std::numeric_limits<float>::max ();
214 std::vector<int> inliers;
215 float inlier_fraction;
219 if (!guess.isApprox (Eigen::Matrix4f::Identity (), 0.01f))
221 getFitness (inliers, error);
222 inlier_fraction =
static_cast<float> (inliers.size ()) /
static_cast<float> (input_->size ());
224 if (inlier_fraction >= inlier_fraction_ && error < lowest_error)
227 lowest_error = error;
233 std::vector<std::vector<int> > similar_features (input_->size ());
236 for (
int i = 0; i < max_iterations_; ++i)
239 std::vector<int> sample_indices;
240 std::vector<int> corresponding_indices;
243 selectSamples (*input_, nr_samples_, sample_indices);
246 findSimilarFeatures (sample_indices, similar_features, corresponding_indices);
249 if (!correspondence_rejector_poly_->thresholdPolygon (sample_indices, corresponding_indices))
256 transformation_estimation_->estimateRigidTransformation (*input_, sample_indices, *target_, corresponding_indices, transformation_);
259 const Matrix4 final_transformation_prev = final_transformation_;
262 final_transformation_ = transformation_;
265 getFitness (inliers, error);
268 final_transformation_ = final_transformation_prev;
271 inlier_fraction =
static_cast<float> (inliers.size ()) /
static_cast<float> (input_->size ());
274 if (inlier_fraction >= inlier_fraction_ && error < lowest_error)
277 lowest_error = error;
279 final_transformation_ = transformation_;
288 PCL_DEBUG(
"[pcl::%s::computeTransformation] Rejected %i out of %i generated pose hypotheses.\n",
289 getClassName ().c_str (), num_rejections, max_iterations_);
293 template <
typename Po
intSource,
typename Po
intTarget,
typename FeatureT>
void
298 inliers.reserve (input_->size ());
299 fitness_score = 0.0f;
302 const float max_range = corr_dist_threshold_ * corr_dist_threshold_;
306 input_transformed.resize (input_->size ());
310 for (std::size_t i = 0; i < input_transformed.points.size (); ++i)
313 std::vector<int> nn_indices (1);
314 std::vector<float> nn_dists (1);
315 tree_->nearestKSearch (input_transformed.points[i], 1, nn_indices, nn_dists);
318 if (nn_dists[0] < max_range)
321 inliers.push_back (
static_cast<int> (i));
324 fitness_score += nn_dists[0];
329 if (!inliers.empty ())
330 fitness_score /=
static_cast<float> (inliers.size ());
332 fitness_score = std::numeric_limits<float>::max ();