41#ifndef PCL_REGISTRATION_SAMPLE_CONSENSUS_PREREJECTIVE_HPP_
42#define PCL_REGISTRATION_SAMPLE_CONSENSUS_PREREJECTIVE_HPP_
46template <
typename Po
intSource,
typename Po
intTarget,
typename FeatureT>
51 if (features ==
nullptr || features->empty()) {
53 "[pcl::%s::setSourceFeatures] Invalid or empty point cloud dataset given!\n",
54 getClassName().
c_str());
57 input_features_ = features;
60template <
typename Po
intSource,
typename Po
intTarget,
typename FeatureT>
65 if (features ==
nullptr || features->empty()) {
67 "[pcl::%s::setTargetFeatures] Invalid or empty point cloud dataset given!\n",
68 getClassName().
c_str());
71 target_features_ = features;
72 feature_tree_->setInputCloud(target_features_);
75template <
typename Po
intSource,
typename Po
intTarget,
typename FeatureT>
80 if (
nr_samples >
static_cast<int>(cloud.size())) {
81 PCL_ERROR(
"[pcl::%s::selectSamples] ", getClassName().
c_str());
82 PCL_ERROR(
"The number of samples (%d) must not be greater than the number of "
85 static_cast<std::size_t
>(cloud.size()));
95 sample_indices[i] = getRandomIndex(
static_cast<int>(cloud.size()) - i);
98 for (
int j = 0; j < i; j++) {
108 for (
int k = i; k > j; k--)
118template <
typename Po
intSource,
typename Po
intTarget,
typename FeatureT>
137 feature_tree_->nearestKSearch(*input_features_,
144 if (k_correspondences_ == 1)
152template <
typename Po
intSource,
typename Po
intTarget,
typename FeatureT>
158 if (!input_features_) {
159 PCL_ERROR(
"[pcl::%s::computeTransformation] ", getClassName().
c_str());
161 "No source features were given! Call setSourceFeatures before aligning.\n");
164 if (!target_features_) {
165 PCL_ERROR(
"[pcl::%s::computeTransformation] ", getClassName().
c_str());
167 "No target features were given! Call setTargetFeatures before aligning.\n");
171 if (input_->size() != input_features_->size()) {
172 PCL_ERROR(
"[pcl::%s::computeTransformation] ", getClassName().
c_str());
173 PCL_ERROR(
"The source points and source feature points need to be in a one-to-one "
174 "relationship! Current input cloud sizes: %ld vs %ld.\n",
176 input_features_->size());
180 if (target_->size() != target_features_->size()) {
181 PCL_ERROR(
"[pcl::%s::computeTransformation] ", getClassName().
c_str());
182 PCL_ERROR(
"The target points and target feature points need to be in a one-to-one "
183 "relationship! Current input cloud sizes: %ld vs %ld.\n",
185 target_features_->size());
190 PCL_ERROR(
"[pcl::%s::computeTransformation] ", getClassName().
c_str());
191 PCL_ERROR(
"Illegal inlier fraction %f, must be in [0,1]!\n", inlier_fraction_);
196 correspondence_rejector_poly_->getSimilarityThreshold();
198 PCL_ERROR(
"[pcl::%s::computeTransformation] ", getClassName().
c_str());
199 PCL_ERROR(
"Illegal prerejection similarity threshold %f, must be in [0,1[!\n",
204 if (k_correspondences_ <= 0) {
205 PCL_ERROR(
"[pcl::%s::computeTransformation] ", getClassName().
c_str());
206 PCL_ERROR(
"Illegal correspondence randomness %d, must be > 0!\n",
213 correspondence_rejector_poly_->setInputSource(input_);
214 correspondence_rejector_poly_->setInputTarget(target_);
215 correspondence_rejector_poly_->setCardinality(nr_samples_);
219 final_transformation_ =
guess;
230 if (!
guess.isApprox(Eigen::Matrix4f::Identity(), 0.01f)) {
233 static_cast<float>(
inliers.
size()) /
static_cast<float>(input_->size());
246 for (
int i = 0; i < max_iterations_; ++i) {
258 if (!correspondence_rejector_poly_->thresholdPolygon(
sample_indices,
265 transformation_estimation_->estimateRigidTransformation(
272 final_transformation_ = transformation_;
282 static_cast<float>(
inliers.
size()) /
static_cast<float>(input_->size());
289 final_transformation_ = transformation_;
298 PCL_DEBUG(
"[pcl::%s::computeTransformation] Rejected %i out of %i generated pose "
300 getClassName().
c_str(),
305template <
typename Po
intSource,
typename Po
intTarget,
typename FeatureT>
312 inliers.reserve(input_->size());
313 fitness_score = 0.0f;
316 const float max_range = corr_dist_threshold_ * corr_dist_threshold_;
342 fitness_score /=
static_cast<float>(
inliers.
size());
344 fitness_score = std::numeric_limits<float>::max();
Iterator class for point clouds with or without given indices.
ConstCloudIterator(const PointCloud< PointT > &cloud)
std::size_t size() const
Size of the range the iterator is going through.
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.
typename FeatureCloud::ConstPtr FeatureCloudConstPtr
typename Registration< PointSource, PointTarget >::PointCloudSource PointCloudSource
void findSimilarFeatures(const pcl::Indices &sample_indices, std::vector< pcl::Indices > &similar_features, pcl::Indices &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 selectSamples(const PointCloudSource &cloud, int nr_samples, pcl::Indices &sample_indices)
Select nr_samples sample points from cloud while making sure that their pairwise distances are greate...
typename Registration< PointSource, PointTarget >::Matrix4 Matrix4
void getFitness(pcl::Indices &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::Matrix< Scalar, 4, 4 > &transform, bool copy_all_fields)
Apply a rigid transform defined by a 4x4 matrix.
IndicesAllocator<> Indices
Type used for indices in PCL.