41 #ifndef PCL_REGISTRATION_SAMPLE_CONSENSUS_PREREJECTIVE_HPP_
42 #define PCL_REGISTRATION_SAMPLE_CONSENSUS_PREREJECTIVE_HPP_
45 template <
typename Po
intSource,
typename Po
intTarget,
typename FeatureT>
void
48 if (features == NULL || features->empty ())
50 PCL_ERROR (
"[pcl::%s::setSourceFeatures] Invalid or empty point cloud dataset given!\n", getClassName ().c_str ());
53 input_features_ = features;
57 template <
typename Po
intSource,
typename Po
intTarget,
typename FeatureT>
void
60 if (features == NULL || features->empty ())
62 PCL_ERROR (
"[pcl::%s::setTargetFeatures] Invalid or empty point cloud dataset given!\n", getClassName ().c_str ());
65 target_features_ = features;
66 feature_tree_->setInputCloud (target_features_);
70 template <
typename Po
intSource,
typename Po
intTarget,
typename FeatureT>
void
73 std::vector<int> &sample_indices)
75 if (nr_samples > static_cast<int> (cloud.points.size ()))
77 PCL_ERROR (
"[pcl::%s::selectSamples] ", getClassName ().c_str ());
78 PCL_ERROR (
"The number of samples (%d) must not be greater than the number of points (%zu)!\n",
79 nr_samples, cloud.points.size ());
84 sample_indices.clear ();
85 std::vector<bool> sampled_indices (cloud.points.size (),
false);
86 while (static_cast<int> (sample_indices.size ()) < nr_samples)
92 sample_index = getRandomIndex (static_cast<int> (cloud.points.size ()));
94 while (sampled_indices[sample_index]);
97 sampled_indices[sample_index] =
true;
100 sample_indices.push_back (sample_index);
105 template <
typename Po
intSource,
typename Po
intTarget,
typename FeatureT>
void
107 const FeatureCloud &input_features,
const std::vector<int> &sample_indices,
108 std::vector<int> &corresponding_indices)
110 std::vector<int> nn_indices (k_correspondences_);
111 std::vector<float> nn_distances (k_correspondences_);
113 corresponding_indices.resize (sample_indices.size ());
114 for (
size_t i = 0; i < sample_indices.size (); ++i)
117 feature_tree_->nearestKSearch (input_features, sample_indices[i], k_correspondences_, nn_indices, nn_distances);
120 if (k_correspondences_ == 1)
122 corresponding_indices[i] = nn_indices[0];
126 int random_correspondence = getRandomIndex (k_correspondences_);
127 corresponding_indices[i] = nn_indices[random_correspondence];
133 template <
typename Po
intSource,
typename Po
intTarget,
typename FeatureT>
void
137 if (!input_features_)
139 PCL_ERROR (
"[pcl::%s::computeTransformation] ", getClassName ().c_str ());
140 PCL_ERROR (
"No source features were given! Call setSourceFeatures before aligning.\n");
143 if (!target_features_)
145 PCL_ERROR (
"[pcl::%s::computeTransformation] ", getClassName ().c_str ());
146 PCL_ERROR (
"No target features were given! Call setTargetFeatures before aligning.\n");
150 if (input_->size () != input_features_->size ())
152 PCL_ERROR (
"[pcl::%s::computeTransformation] ", getClassName ().c_str ());
153 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",
154 input_->size (), input_features_->size ());
158 if (target_->size () != target_features_->size ())
160 PCL_ERROR (
"[pcl::%s::computeTransformation] ", getClassName ().c_str ());
161 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",
162 target_->size (), target_features_->size ());
166 if (inlier_fraction_ < 0.0f || inlier_fraction_ > 1.0f)
168 PCL_ERROR (
"[pcl::%s::computeTransformation] ", getClassName ().c_str ());
169 PCL_ERROR (
"Illegal inlier fraction %f, must be in [0,1]!\n",
174 const float similarity_threshold = correspondence_rejector_poly_->getSimilarityThreshold ();
175 if (similarity_threshold < 0.0f || similarity_threshold >= 1.0f)
177 PCL_ERROR (
"[pcl::%s::computeTransformation] ", getClassName ().c_str ());
178 PCL_ERROR (
"Illegal prerejection similarity threshold %f, must be in [0,1[!\n",
179 similarity_threshold);
184 correspondence_rejector_poly_->setInputSource (input_);
185 correspondence_rejector_poly_->setInputTarget (target_);
186 correspondence_rejector_poly_->setCardinality (nr_samples_);
187 int num_rejections = 0;
190 final_transformation_ = guess;
192 float highest_inlier_fraction = inlier_fraction_;
196 std::vector<int> inliers;
197 float inlier_fraction;
201 if (!guess.isApprox (Eigen::Matrix4f::Identity (), 0.01f))
203 getFitness (inliers, error);
204 inlier_fraction =
static_cast<float> (inliers.size ()) / static_cast<float> (input_->size ());
206 if (inlier_fraction > highest_inlier_fraction)
209 highest_inlier_fraction = inlier_fraction;
215 for (
int i = 0; i < max_iterations_; ++i)
218 std::vector<int> sample_indices (nr_samples_);
219 std::vector<int> corresponding_indices (nr_samples_);
222 selectSamples (*input_, nr_samples_, sample_indices);
225 findSimilarFeatures (*input_features_, sample_indices, corresponding_indices);
228 if (!correspondence_rejector_poly_->thresholdPolygon (sample_indices, corresponding_indices)){
234 transformation_estimation_->estimateRigidTransformation (*input_, sample_indices, *target_, corresponding_indices, transformation_);
237 const Matrix4 final_transformation_prev = final_transformation_;
240 final_transformation_ = transformation_;
243 getFitness (inliers, error);
246 const float inlier_fraction =
static_cast<float> (inliers.size ()) / static_cast<float> (input_->size ());
247 if (inlier_fraction > highest_inlier_fraction)
250 highest_inlier_fraction = inlier_fraction;
256 final_transformation_ = final_transformation_prev;
265 PCL_DEBUG(
"[pcl::%s::computeTransformation] Rejected %i out of %i generated pose hypotheses.\n",
266 getClassName ().c_str (), num_rejections, max_iterations_);
270 template <
typename Po
intSource,
typename Po
intTarget,
typename FeatureT>
void
275 inliers.reserve (input_->size ());
276 fitness_score = 0.0f;
279 const float max_range = corr_dist_threshold_ * corr_dist_threshold_;
283 input_transformed.resize (input_->size ());
287 for (
size_t i = 0; i < input_transformed.points.size (); ++i)
290 std::vector<int> nn_indices (1);
291 std::vector<float> nn_dists (1);
292 tree_->nearestKSearch (input_transformed.points[i], 1, nn_indices, nn_dists);
295 if (nn_dists[0] < max_range)
298 const float dx = input_transformed.points[i].x - target_->points[nn_indices[0]].x;
299 const float dy = input_transformed.points[i].y - target_->points[nn_indices[0]].y;
300 const float dz = input_transformed.points[i].z - target_->points[nn_indices[0]].z;
303 inliers.push_back (static_cast<int> (i));
306 fitness_score += dx*dx + dy*dy + dz*dz;
311 if (inliers.size () > 0)
312 fitness_score /= static_cast<float> (inliers.size ());
314 fitness_score = std::numeric_limits<float>::max ();
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 computeTransformation(PointCloudSource &output, const Eigen::Matrix4f &guess)
Rigid transformation computation method.
Registration< PointSource, PointTarget >::PointCloudSource PointCloudSource
void transformPointCloud(const pcl::PointCloud< PointT > &cloud_in, pcl::PointCloud< PointT > &cloud_out, const Eigen::Transform< Scalar, 3, Eigen::Affine > &transform)
Apply an affine transform defined by an Eigen Transform.
Registration< PointSource, PointTarget >::Matrix4 Matrix4
FeatureCloud::ConstPtr FeatureCloudConstPtr
void setTargetFeatures(const FeatureCloudConstPtr &features)
Provide a boost shared pointer to the target point cloud's feature descriptors.
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...
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 setSourceFeatures(const FeatureCloudConstPtr &features)
Provide a boost shared pointer to the source point cloud's feature descriptors.