41 #ifndef IA_RANSAC_HPP_
42 #define IA_RANSAC_HPP_
50 template <
typename Po
intSource,
typename Po
intTarget,
typename FeatureT>
void
53 if (features ==
nullptr || features->empty ())
55 PCL_ERROR (
"[pcl::%s::setSourceFeatures] Invalid or empty point cloud dataset given!\n", getClassName ().c_str ());
58 input_features_ = features;
62 template <
typename Po
intSource,
typename Po
intTarget,
typename FeatureT>
void
65 if (features ==
nullptr || features->empty ())
67 PCL_ERROR (
"[pcl::%s::setTargetFeatures] Invalid or empty point cloud dataset given!\n", getClassName ().c_str ());
70 target_features_ = features;
71 feature_tree_->setInputCloud (target_features_);
75 template <
typename Po
intSource,
typename Po
intTarget,
typename FeatureT>
void
78 std::vector<int> &sample_indices)
80 if (nr_samples >
static_cast<int> (cloud.size ()))
82 PCL_ERROR (
"[pcl::%s::selectSamples] ", getClassName ().c_str ());
83 PCL_ERROR(
"The number of samples (%d) must not be greater than the number of "
86 static_cast<std::size_t
>(cloud.size()));
91 index_t iterations_without_a_sample = 0;
92 const auto max_iterations_without_a_sample = 3 * cloud.size ();
93 sample_indices.clear ();
94 while (
static_cast<int> (sample_indices.size ()) < nr_samples)
97 int sample_index = getRandomIndex (
static_cast<int> (cloud.size ()));
100 bool valid_sample =
true;
101 for (
const int &sample_idx : sample_indices)
103 float distance_between_samples =
euclideanDistance (cloud[sample_index], cloud[sample_idx]);
105 if (sample_index == sample_idx || distance_between_samples < min_sample_distance)
107 valid_sample =
false;
115 sample_indices.push_back (sample_index);
116 iterations_without_a_sample = 0;
119 ++iterations_without_a_sample;
122 if (
static_cast<std::size_t
>(iterations_without_a_sample) >= max_iterations_without_a_sample)
124 PCL_WARN (
"[pcl::%s::selectSamples] ", getClassName ().c_str ());
125 PCL_WARN (
"No valid sample found after %zu iterations. Relaxing min_sample_distance_ to %f\n",
126 static_cast<std::size_t
>(iterations_without_a_sample), 0.5*min_sample_distance);
128 min_sample_distance_ *= 0.5f;
129 min_sample_distance = min_sample_distance_;
130 iterations_without_a_sample = 0;
136 template <
typename Po
intSource,
typename Po
intTarget,
typename FeatureT>
void
138 const FeatureCloud &input_features,
const std::vector<int> &sample_indices,
139 std::vector<int> &corresponding_indices)
141 std::vector<int> nn_indices (k_correspondences_);
142 std::vector<float> nn_distances (k_correspondences_);
144 corresponding_indices.resize (sample_indices.size ());
145 for (std::size_t i = 0; i < sample_indices.size (); ++i)
148 feature_tree_->nearestKSearch (input_features, sample_indices[i], k_correspondences_, nn_indices, nn_distances);
151 int random_correspondence = getRandomIndex (k_correspondences_);
152 corresponding_indices[i] = nn_indices[random_correspondence];
157 template <
typename Po
intSource,
typename Po
intTarget,
typename FeatureT>
float
161 std::vector<int> nn_index (1);
162 std::vector<float> nn_distance (1);
167 for (
int i = 0; i < static_cast<int> (cloud.size ()); ++i)
170 tree_->nearestKSearch (cloud, i, 1, nn_index, nn_distance);
173 error += compute_error (nn_distance[0]);
179 template <
typename Po
intSource,
typename Po
intTarget,
typename FeatureT>
void
183 if (!input_features_)
185 PCL_ERROR (
"[pcl::%s::computeTransformation] ", getClassName ().c_str ());
186 PCL_ERROR (
"No source features were given! Call setSourceFeatures before aligning.\n");
189 if (!target_features_)
191 PCL_ERROR (
"[pcl::%s::computeTransformation] ", getClassName ().c_str ());
192 PCL_ERROR (
"No target features were given! Call setTargetFeatures before aligning.\n");
196 if (input_->size () != input_features_->size ())
198 PCL_ERROR (
"[pcl::%s::computeTransformation] ", getClassName ().c_str ());
199 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",
200 input_->size (), input_features_->size ());
204 if (target_->size () != target_features_->size ())
206 PCL_ERROR (
"[pcl::%s::computeTransformation] ", getClassName ().c_str ());
207 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",
208 target_->size (), target_features_->size ());
213 error_functor_.reset (
new TruncatedError (
static_cast<float> (corr_dist_threshold_)));
216 std::vector<int> sample_indices (nr_samples_);
217 std::vector<int> corresponding_indices (nr_samples_);
219 float lowest_error (0);
221 final_transformation_ = guess;
224 if (!guess.isApprox (Eigen::Matrix4f::Identity (), 0.01f))
228 lowest_error = computeErrorMetric (input_transformed,
static_cast<float> (corr_dist_threshold_));
232 for (; i_iter < max_iterations_; ++i_iter)
235 selectSamples (*input_, nr_samples_, min_sample_distance_, sample_indices);
238 findSimilarFeatures (*input_features_, sample_indices, corresponding_indices);
241 transformation_estimation_->estimateRigidTransformation (*input_, sample_indices, *target_, corresponding_indices, transformation_);
245 float error = computeErrorMetric (input_transformed,
static_cast<float> (corr_dist_threshold_));
248 if (i_iter == 0 || error < lowest_error)
250 lowest_error = error;
251 final_transformation_ = transformation_;
262 #endif //#ifndef IA_RANSAC_HPP_