41 #ifndef IA_RANSAC_HPP_
42 #define IA_RANSAC_HPP_
48 template <
typename Po
intSource,
typename Po
intTarget,
typename FeatureT>
53 if (features ==
nullptr || features->empty()) {
55 "[pcl::%s::setSourceFeatures] Invalid or empty point cloud dataset given!\n",
56 getClassName().c_str());
59 input_features_ = features;
62 template <
typename Po
intSource,
typename Po
intTarget,
typename FeatureT>
67 if (features ==
nullptr || features->empty()) {
69 "[pcl::%s::setTargetFeatures] Invalid or empty point cloud dataset given!\n",
70 getClassName().c_str());
73 target_features_ = features;
74 feature_tree_->setInputCloud(target_features_);
77 template <
typename Po
intSource,
typename Po
intTarget,
typename FeatureT>
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;
139 template <
typename Po
intSource,
typename Po
intTarget,
typename FeatureT>
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];
164 template <
typename Po
intSource,
typename Po
intTarget,
typename FeatureT>
170 std::vector<float> nn_distance(1);
175 for (
const auto& point : cloud) {
178 tree_->nearestKSearch(point, 1, nn_index, nn_distance);
181 error += compute_error(nn_distance[0]);
186 template <
typename Po
intSource,
typename Po
intTarget,
typename FeatureT>
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 computeTransformation(PointCloudSource &output, const Eigen::Matrix4f &guess) override
Rigid transformation computation method.
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...
typename Registration< PointSource, PointTarget >::PointCloudSource PointCloudSource
void setTargetFeatures(const FeatureCloudConstPtr &features)
Provide a shared pointer to the target point cloud's feature descriptors.
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...
typename FeatureCloud::ConstPtr FeatureCloudConstPtr
void setSourceFeatures(const FeatureCloudConstPtr &features)
Provide a shared pointer to the source point cloud's feature descriptors.
Define standard C methods to do distance calculations.
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.
detail::int_type_t< detail::index_type_size, detail::index_type_signed > index_t
Type used for an index in PCL.
float euclideanDistance(const PointType1 &p1, const PointType2 &p2)
Calculate the euclidean distance between the two given points.
IndicesAllocator<> Indices
Type used for indices in PCL.