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.points.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 points (%lu)!\n",
84 nr_samples, cloud.points.size ());
89 int iterations_without_a_sample = 0;
90 int max_iterations_without_a_sample =
static_cast<int> (3 * cloud.points.size ());
91 sample_indices.clear ();
92 while (
static_cast<int> (sample_indices.size ()) < nr_samples)
95 int sample_index = getRandomIndex (
static_cast<int> (cloud.points.size ()));
98 bool valid_sample =
true;
99 for (
const int &sample_idx : sample_indices)
101 float distance_between_samples =
euclideanDistance (cloud.points[sample_index], cloud.points[sample_idx]);
103 if (sample_index == sample_idx || distance_between_samples < min_sample_distance)
105 valid_sample =
false;
113 sample_indices.push_back (sample_index);
114 iterations_without_a_sample = 0;
117 ++iterations_without_a_sample;
120 if (iterations_without_a_sample >= max_iterations_without_a_sample)
122 PCL_WARN (
"[pcl::%s::selectSamples] ", getClassName ().c_str ());
123 PCL_WARN (
"No valid sample found after %d iterations. Relaxing min_sample_distance_ to %f\n",
124 iterations_without_a_sample, 0.5*min_sample_distance);
126 min_sample_distance_ *= 0.5f;
127 min_sample_distance = min_sample_distance_;
128 iterations_without_a_sample = 0;
134 template <
typename Po
intSource,
typename Po
intTarget,
typename FeatureT>
void
136 const FeatureCloud &input_features,
const std::vector<int> &sample_indices,
137 std::vector<int> &corresponding_indices)
139 std::vector<int> nn_indices (k_correspondences_);
140 std::vector<float> nn_distances (k_correspondences_);
142 corresponding_indices.resize (sample_indices.size ());
143 for (std::size_t i = 0; i < sample_indices.size (); ++i)
146 feature_tree_->nearestKSearch (input_features, sample_indices[i], k_correspondences_, nn_indices, nn_distances);
149 int random_correspondence = getRandomIndex (k_correspondences_);
150 corresponding_indices[i] = nn_indices[random_correspondence];
155 template <
typename Po
intSource,
typename Po
intTarget,
typename FeatureT>
float
159 std::vector<int> nn_index (1);
160 std::vector<float> nn_distance (1);
165 for (
int i = 0; i < static_cast<int> (cloud.points.size ()); ++i)
168 tree_->nearestKSearch (cloud, i, 1, nn_index, nn_distance);
171 error += compute_error (nn_distance[0]);
177 template <
typename Po
intSource,
typename Po
intTarget,
typename FeatureT>
void
181 if (!input_features_)
183 PCL_ERROR (
"[pcl::%s::computeTransformation] ", getClassName ().c_str ());
184 PCL_ERROR (
"No source features were given! Call setSourceFeatures before aligning.\n");
187 if (!target_features_)
189 PCL_ERROR (
"[pcl::%s::computeTransformation] ", getClassName ().c_str ());
190 PCL_ERROR (
"No target features were given! Call setTargetFeatures before aligning.\n");
194 if (input_->size () != input_features_->size ())
196 PCL_ERROR (
"[pcl::%s::computeTransformation] ", getClassName ().c_str ());
197 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",
198 input_->size (), input_features_->size ());
202 if (target_->size () != target_features_->size ())
204 PCL_ERROR (
"[pcl::%s::computeTransformation] ", getClassName ().c_str ());
205 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",
206 target_->size (), target_features_->size ());
211 error_functor_.reset (
new TruncatedError (
static_cast<float> (corr_dist_threshold_)));
214 std::vector<int> sample_indices (nr_samples_);
215 std::vector<int> corresponding_indices (nr_samples_);
217 float lowest_error (0);
219 final_transformation_ = guess;
222 if (!guess.isApprox (Eigen::Matrix4f::Identity (), 0.01f))
226 lowest_error = computeErrorMetric (input_transformed,
static_cast<float> (corr_dist_threshold_));
230 for (; i_iter < max_iterations_; ++i_iter)
233 selectSamples (*input_, nr_samples_, min_sample_distance_, sample_indices);
236 findSimilarFeatures (*input_features_, sample_indices, corresponding_indices);
239 transformation_estimation_->estimateRigidTransformation (*input_, sample_indices, *target_, corresponding_indices, transformation_);
243 float error = computeErrorMetric (input_transformed,
static_cast<float> (corr_dist_threshold_));
246 if (i_iter == 0 || error < lowest_error)
248 lowest_error = error;
249 final_transformation_ = transformation_;
260 #endif //#ifndef IA_RANSAC_HPP_