81 unsigned int nr_samples,
82 float min_sample_distance,
85 if (nr_samples > cloud.size()) {
86 PCL_ERROR(
"[pcl::%s::selectSamples] ", getClassName().c_str());
87 PCL_ERROR(
"The number of samples (%u) must not be greater than the number of "
90 static_cast<std::size_t
>(cloud.size()));
95 index_t iterations_without_a_sample = 0;
96 const auto max_iterations_without_a_sample = 3 * cloud.size();
97 sample_indices.clear();
98 while (sample_indices.size() < nr_samples) {
100 const auto sample_index = getRandomIndex(cloud.size());
103 bool valid_sample =
true;
104 for (
const auto& sample_idx : sample_indices) {
105 float distance_between_samples =
108 if (sample_index == sample_idx ||
109 distance_between_samples < min_sample_distance) {
110 valid_sample =
false;
117 sample_indices.push_back(sample_index);
118 iterations_without_a_sample = 0;
121 ++iterations_without_a_sample;
124 if (
static_cast<std::size_t
>(iterations_without_a_sample) >=
125 max_iterations_without_a_sample) {
126 PCL_WARN(
"[pcl::%s::selectSamples] ", getClassName().c_str());
127 PCL_WARN(
"No valid sample found after %zu iterations. Relaxing "
128 "min_sample_distance_ to %f\n",
129 static_cast<std::size_t
>(iterations_without_a_sample),
130 0.5 * min_sample_distance);
132 min_sample_distance_ *= 0.5f;
133 min_sample_distance = min_sample_distance_;
134 iterations_without_a_sample = 0;
147 std::vector<float> nn_distances(k_correspondences_);
149 corresponding_indices.resize(sample_indices.size());
150 for (std::size_t i = 0; i < sample_indices.size(); ++i) {
152 feature_tree_->nearestKSearch(input_features,
159 const auto random_correspondence = getRandomIndex(k_correspondences_);
160 corresponding_indices[i] = nn_indices[random_correspondence];
192 if (!input_features_) {
193 PCL_ERROR(
"[pcl::%s::computeTransformation] ", getClassName().c_str());
195 "No source features were given! Call setSourceFeatures before aligning.\n");
198 if (!target_features_) {
199 PCL_ERROR(
"[pcl::%s::computeTransformation] ", getClassName().c_str());
201 "No target features were given! Call setTargetFeatures before aligning.\n");
205 if (input_->size() != input_features_->size()) {
206 PCL_ERROR(
"[pcl::%s::computeTransformation] ", getClassName().c_str());
207 PCL_ERROR(
"The source points and source feature points need to be in a one-to-one "
208 "relationship! Current input cloud sizes: %ld vs %ld.\n",
210 input_features_->size());
214 if (target_->size() != target_features_->size()) {
215 PCL_ERROR(
"[pcl::%s::computeTransformation] ", getClassName().c_str());
216 PCL_ERROR(
"The target points and target feature points need to be in a one-to-one "
217 "relationship! Current input cloud sizes: %ld vs %ld.\n",
219 target_features_->size());
224 error_functor_.reset(
new TruncatedError(
static_cast<float>(corr_dist_threshold_)));
229 float lowest_error(0);
231 final_transformation_ = guess;
234 if (!guess.isApprox(Eigen::Matrix4f::Identity(), 0.01f)) {
238 computeErrorMetric(input_transformed,
static_cast<float>(corr_dist_threshold_));
242 for (; i_iter < max_iterations_; ++i_iter) {
244 selectSamples(*input_, nr_samples_, min_sample_distance_, sample_indices);
247 findSimilarFeatures(*input_features_, sample_indices, corresponding_indices);
250 transformation_estimation_->estimateRigidTransformation(
251 *input_, sample_indices, *target_, corresponding_indices, transformation_);
256 computeErrorMetric(input_transformed,
static_cast<float>(corr_dist_threshold_));
259 if (i_iter == 0 || error < lowest_error) {
260 lowest_error = error;
261 final_transformation_ = transformation_;
void selectSamples(const PointCloudSource &cloud, unsigned int nr_samples, float min_sample_distance, pcl::Indices &sample_indices)
Select nr_samples sample points from cloud while making sure that their pairwise distances are greate...
void findSimilarFeatures(const FeatureCloud &input_features, const pcl::Indices &sample_indices, pcl::Indices &corresponding_indices)
For each of the sample points, find a list of points in the target cloud whose features are similar t...
void transformPointCloud(const pcl::PointCloud< PointT > &cloud_in, pcl::PointCloud< PointT > &cloud_out, const Eigen::Matrix< Scalar, 4, 4 > &transform, bool copy_all_fields)
Apply a rigid transform defined by a 4x4 matrix.