41 #ifndef PCL_REGISTRATION_SAMPLE_CONSENSUS_PREREJECTIVE_HPP_
42 #define PCL_REGISTRATION_SAMPLE_CONSENSUS_PREREJECTIVE_HPP_
48 template <
typename Po
intSource,
typename Po
intTarget,
typename FeatureT>
void
51 if (features ==
nullptr || features->empty ())
53 PCL_ERROR (
"[pcl::%s::setSourceFeatures] Invalid or empty point cloud dataset given!\n", getClassName ().c_str ());
56 input_features_ = features;
60 template <
typename Po
intSource,
typename Po
intTarget,
typename FeatureT>
void
63 if (features ==
nullptr || features->empty ())
65 PCL_ERROR (
"[pcl::%s::setTargetFeatures] Invalid or empty point cloud dataset given!\n", getClassName ().c_str ());
68 target_features_ = features;
69 feature_tree_->setInputCloud (target_features_);
73 template <
typename Po
intSource,
typename Po
intTarget,
typename FeatureT>
void
75 const PointCloudSource &cloud,
int nr_samples, std::vector<int> &sample_indices)
77 if (nr_samples >
static_cast<int> (cloud.
points.size ()))
79 PCL_ERROR (
"[pcl::%s::selectSamples] ", getClassName ().c_str ());
80 PCL_ERROR (
"The number of samples (%d) must not be greater than the number of points (%lu)!\n",
81 nr_samples, cloud.
points.size ());
85 sample_indices.resize (nr_samples);
89 for (
int i = 0; i < nr_samples; i++)
92 sample_indices[i] = getRandomIndex (
static_cast<int> (cloud.
points.size ()) - i);
95 for (
int j = 0; j < i; j++)
98 if (sample_indices[i] >= sample_indices[j])
105 temp_sample = sample_indices[i];
106 for (
int k = i; k > j; k--)
107 sample_indices[k] = sample_indices[k - 1];
109 sample_indices[j] = temp_sample;
117 template <
typename Po
intSource,
typename Po
intTarget,
typename FeatureT>
void
119 const std::vector<int> &sample_indices,
120 std::vector<std::vector<int> >& similar_features,
121 std::vector<int> &corresponding_indices)
124 corresponding_indices.resize (sample_indices.size ());
125 std::vector<float> nn_distances (k_correspondences_);
128 for (std::size_t i = 0; i < sample_indices.size (); ++i)
131 const int idx = sample_indices[i];
134 if (similar_features[idx].empty ())
135 feature_tree_->nearestKSearch (*input_features_, idx, k_correspondences_, similar_features[idx], nn_distances);
138 if (k_correspondences_ == 1)
139 corresponding_indices[i] = similar_features[idx][0];
141 corresponding_indices[i] = similar_features[idx][getRandomIndex (k_correspondences_)];
146 template <
typename Po
intSource,
typename Po
intTarget,
typename FeatureT>
void
150 if (!input_features_)
152 PCL_ERROR (
"[pcl::%s::computeTransformation] ", getClassName ().c_str ());
153 PCL_ERROR (
"No source features were given! Call setSourceFeatures before aligning.\n");
156 if (!target_features_)
158 PCL_ERROR (
"[pcl::%s::computeTransformation] ", getClassName ().c_str ());
159 PCL_ERROR (
"No target features were given! Call setTargetFeatures before aligning.\n");
163 if (input_->size () != input_features_->size ())
165 PCL_ERROR (
"[pcl::%s::computeTransformation] ", getClassName ().c_str ());
166 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",
167 input_->size (), input_features_->size ());
171 if (target_->size () != target_features_->size ())
173 PCL_ERROR (
"[pcl::%s::computeTransformation] ", getClassName ().c_str ());
174 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",
175 target_->size (), target_features_->size ());
179 if (inlier_fraction_ < 0.0f || inlier_fraction_ > 1.0f)
181 PCL_ERROR (
"[pcl::%s::computeTransformation] ", getClassName ().c_str ());
182 PCL_ERROR (
"Illegal inlier fraction %f, must be in [0,1]!\n",
187 const float similarity_threshold = correspondence_rejector_poly_->getSimilarityThreshold ();
188 if (similarity_threshold < 0.0f || similarity_threshold >= 1.0f)
190 PCL_ERROR (
"[pcl::%s::computeTransformation] ", getClassName ().c_str ());
191 PCL_ERROR (
"Illegal prerejection similarity threshold %f, must be in [0,1[!\n",
192 similarity_threshold);
196 if (k_correspondences_ <= 0)
198 PCL_ERROR (
"[pcl::%s::computeTransformation] ", getClassName ().c_str ());
199 PCL_ERROR (
"Illegal correspondence randomness %d, must be > 0!\n",
205 correspondence_rejector_poly_->setInputSource (input_);
206 correspondence_rejector_poly_->setInputTarget (target_);
207 correspondence_rejector_poly_->setCardinality (nr_samples_);
208 int num_rejections = 0;
211 final_transformation_ = guess;
213 float lowest_error = std::numeric_limits<float>::max ();
217 std::vector<int> inliers;
218 float inlier_fraction;
222 if (!guess.isApprox (Eigen::Matrix4f::Identity (), 0.01f))
224 getFitness (inliers, error);
225 inlier_fraction =
static_cast<float> (inliers.size ()) /
static_cast<float> (input_->size ());
227 if (inlier_fraction >= inlier_fraction_ && error < lowest_error)
230 lowest_error = error;
236 std::vector<std::vector<int> > similar_features (input_->size ());
239 for (
int i = 0; i < max_iterations_; ++i)
242 std::vector<int> sample_indices;
243 std::vector<int> corresponding_indices;
246 selectSamples (*input_, nr_samples_, sample_indices);
249 findSimilarFeatures (sample_indices, similar_features, corresponding_indices);
252 if (!correspondence_rejector_poly_->thresholdPolygon (sample_indices, corresponding_indices))
259 transformation_estimation_->estimateRigidTransformation (*input_, sample_indices, *target_, corresponding_indices, transformation_);
262 const Matrix4 final_transformation_prev = final_transformation_;
265 final_transformation_ = transformation_;
268 getFitness (inliers, error);
271 final_transformation_ = final_transformation_prev;
274 inlier_fraction =
static_cast<float> (inliers.size ()) /
static_cast<float> (input_->size ());
277 if (inlier_fraction >= inlier_fraction_ && error < lowest_error)
280 lowest_error = error;
282 final_transformation_ = transformation_;
291 PCL_DEBUG(
"[pcl::%s::computeTransformation] Rejected %i out of %i generated pose hypotheses.\n",
292 getClassName ().c_str (), num_rejections, max_iterations_);
296 template <
typename Po
intSource,
typename Po
intTarget,
typename FeatureT>
void
301 inliers.reserve (input_->size ());
302 fitness_score = 0.0f;
305 const float max_range = corr_dist_threshold_ * corr_dist_threshold_;
309 input_transformed.
resize (input_->size ());
313 for (std::size_t i = 0; i < input_transformed.
points.size (); ++i)
316 std::vector<int> nn_indices (1);
317 std::vector<float> nn_dists (1);
318 tree_->nearestKSearch (input_transformed.
points[i], 1, nn_indices, nn_dists);
321 if (nn_dists[0] < max_range)
324 inliers.push_back (
static_cast<int> (i));
327 fitness_score += nn_dists[0];
332 if (!inliers.empty ())
333 fitness_score /=
static_cast<float> (inliers.size ());
335 fitness_score = std::numeric_limits<float>::max ();
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
void resize(std::size_t n)
Resize the cloud.
Eigen::Matrix< Scalar, 4, 4 > Matrix4
void setTargetFeatures(const FeatureCloudConstPtr &features)
Provide a boost shared pointer to the target point cloud's feature descriptors.
void computeTransformation(PointCloudSource &output, const Eigen::Matrix4f &guess) override
Rigid transformation computation method.
void selectSamples(const PointCloudSource &cloud, int nr_samples, std::vector< int > &sample_indices)
Select nr_samples sample points from cloud while making sure that their pairwise distances are greate...
typename FeatureCloud::ConstPtr FeatureCloudConstPtr
void findSimilarFeatures(const std::vector< int > &sample_indices, std::vector< std::vector< int > > &similar_features, 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 setSourceFeatures(const FeatureCloudConstPtr &features)
Provide a boost shared pointer to the source point cloud's feature descriptors.
void getFitness(std::vector< int > &inliers, float &fitness_score)
Obtain the fitness of a transformation The following metrics are calculated, based on final_transform...
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.