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_
detail::int_type_t< detail::index_type_size, detail::index_type_signed > index_t
Type used for an index in PCL.
Define standard C methods to do distance calculations.
void setTargetFeatures(const FeatureCloudConstPtr &features)
Provide a shared pointer to the target point cloud's feature descriptors.
float euclideanDistance(const PointType1 &p1, const PointType2 &p2)
Calculate the euclidean distance between the two given points.
void setSourceFeatures(const FeatureCloudConstPtr &features)
Provide a shared pointer to the source point cloud's feature descriptors.
float computeErrorMetric(const PointCloudSource &cloud, float threshold)
An error metric for that computes the quality of the alignment between the given cloud and the target...
void transformPointCloud(const pcl::PointCloud< PointT > &cloud_in, pcl::PointCloud< PointT > &cloud_out, const Eigen::Transform< Scalar, 3, Eigen::Affine > &transform, bool copy_all_fields)
Apply an affine transform defined by an Eigen Transform.
void selectSamples(const PointCloudSource &cloud, int nr_samples, float min_sample_distance, std::vector< int > &sample_indices)
Select nr_samples sample points from cloud while making sure that their pairwise distances are greate...
typename Registration< PointSource, PointTarget >::PointCloudSource PointCloudSource
void findSimilarFeatures(const FeatureCloud &input_features, const std::vector< int > &sample_indices, std::vector< int > &corresponding_indices)
For each of the sample points, find a list of points in the target cloud whose features are similar t...
void computeTransformation(PointCloudSource &output, const Eigen::Matrix4f &guess) override
Rigid transformation computation method.
typename FeatureCloud::ConstPtr FeatureCloudConstPtr