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
72 const PointCloudSource &cloud,
int nr_samples, std::vector<int> &sample_indices)
74 if (nr_samples > static_cast<int> (cloud.points.size ()))
76 PCL_ERROR (
"[pcl::%s::selectSamples] ", getClassName ().c_str ());
77 PCL_ERROR (
"The number of samples (%d) must not be greater than the number of points (%lu)!\n",
78 nr_samples, cloud.points.size ());
82 sample_indices.resize (nr_samples);
86 for (
int i = 0; i < nr_samples; i++)
89 sample_indices[i] = getRandomIndex (static_cast<int> (cloud.points.size ()) - i);
92 for (
int j = 0; j < i; j++)
95 if (sample_indices[i] >= sample_indices[j])
102 temp_sample = sample_indices[i];
103 for (
int k = i; k > j; k--)
104 sample_indices[k] = sample_indices[k - 1];
106 sample_indices[j] = temp_sample;
114 template <
typename Po
intSource,
typename Po
intTarget,
typename FeatureT>
void
116 const std::vector<int> &sample_indices,
117 std::vector<std::vector<int> >& similar_features,
118 std::vector<int> &corresponding_indices)
121 corresponding_indices.resize (sample_indices.size ());
122 std::vector<float> nn_distances (k_correspondences_);
125 for (
size_t i = 0; i < sample_indices.size (); ++i)
128 const int idx = sample_indices[i];
131 if (similar_features[idx].empty ())
132 feature_tree_->nearestKSearch (*input_features_, idx, k_correspondences_, similar_features[idx], nn_distances);
135 if (k_correspondences_ == 1)
136 corresponding_indices[i] = similar_features[idx][0];
138 corresponding_indices[i] = similar_features[idx][getRandomIndex (k_correspondences_)];
143 template <
typename Po
intSource,
typename Po
intTarget,
typename FeatureT>
void
147 if (!input_features_)
149 PCL_ERROR (
"[pcl::%s::computeTransformation] ", getClassName ().c_str ());
150 PCL_ERROR (
"No source features were given! Call setSourceFeatures before aligning.\n");
153 if (!target_features_)
155 PCL_ERROR (
"[pcl::%s::computeTransformation] ", getClassName ().c_str ());
156 PCL_ERROR (
"No target features were given! Call setTargetFeatures before aligning.\n");
160 if (input_->size () != input_features_->size ())
162 PCL_ERROR (
"[pcl::%s::computeTransformation] ", getClassName ().c_str ());
163 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",
164 input_->size (), input_features_->size ());
168 if (target_->size () != target_features_->size ())
170 PCL_ERROR (
"[pcl::%s::computeTransformation] ", getClassName ().c_str ());
171 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",
172 target_->size (), target_features_->size ());
176 if (inlier_fraction_ < 0.0f || inlier_fraction_ > 1.0f)
178 PCL_ERROR (
"[pcl::%s::computeTransformation] ", getClassName ().c_str ());
179 PCL_ERROR (
"Illegal inlier fraction %f, must be in [0,1]!\n",
184 const float similarity_threshold = correspondence_rejector_poly_->getSimilarityThreshold ();
185 if (similarity_threshold < 0.0f || similarity_threshold >= 1.0f)
187 PCL_ERROR (
"[pcl::%s::computeTransformation] ", getClassName ().c_str ());
188 PCL_ERROR (
"Illegal prerejection similarity threshold %f, must be in [0,1[!\n",
189 similarity_threshold);
193 if (k_correspondences_ <= 0)
195 PCL_ERROR (
"[pcl::%s::computeTransformation] ", getClassName ().c_str ());
196 PCL_ERROR (
"Illegal correspondence randomness %d, must be > 0!\n",
202 correspondence_rejector_poly_->setInputSource (input_);
203 correspondence_rejector_poly_->setInputTarget (target_);
204 correspondence_rejector_poly_->setCardinality (nr_samples_);
205 int num_rejections = 0;
208 final_transformation_ = guess;
210 float lowest_error = std::numeric_limits<float>::max ();
214 std::vector<int> inliers;
215 float inlier_fraction;
219 if (!guess.isApprox (Eigen::Matrix4f::Identity (), 0.01f))
221 getFitness (inliers, error);
222 inlier_fraction =
static_cast<float> (inliers.size ()) / static_cast<float> (input_->size ());
223 error /=
static_cast<float> (inliers.size ());
225 if (inlier_fraction >= inlier_fraction_ && error < lowest_error)
228 lowest_error = error;
234 std::vector<std::vector<int> > similar_features (input_->size ());
237 for (
int i = 0; i < max_iterations_; ++i)
240 std::vector<int> sample_indices;
241 std::vector<int> corresponding_indices;
244 selectSamples (*input_, nr_samples_, sample_indices);
247 findSimilarFeatures (sample_indices, similar_features, corresponding_indices);
250 if (!correspondence_rejector_poly_->thresholdPolygon (sample_indices, corresponding_indices))
257 transformation_estimation_->estimateRigidTransformation (*input_, sample_indices, *target_, corresponding_indices, transformation_);
260 const Matrix4 final_transformation_prev = final_transformation_;
263 final_transformation_ = transformation_;
266 getFitness (inliers, error);
269 final_transformation_ = final_transformation_prev;
272 inlier_fraction =
static_cast<float> (inliers.size ()) / static_cast<float> (input_->size ());
275 if (inlier_fraction >= inlier_fraction_ && error < lowest_error)
278 lowest_error = error;
280 final_transformation_ = transformation_;
289 PCL_DEBUG(
"[pcl::%s::computeTransformation] Rejected %i out of %i generated pose hypotheses.\n",
290 getClassName ().c_str (), num_rejections, max_iterations_);
294 template <
typename Po
intSource,
typename Po
intTarget,
typename FeatureT>
void
299 inliers.reserve (input_->size ());
300 fitness_score = 0.0f;
303 const float max_range = corr_dist_threshold_ * corr_dist_threshold_;
307 input_transformed.resize (input_->size ());
311 for (
size_t i = 0; i < input_transformed.points.size (); ++i)
314 std::vector<int> nn_indices (1);
315 std::vector<float> nn_dists (1);
316 tree_->nearestKSearch (input_transformed.points[i], 1, nn_indices, nn_dists);
319 if (nn_dists[0] < max_range)
322 inliers.push_back (static_cast<int> (i));
325 fitness_score += nn_dists[0];
330 if (inliers.size () > 0)
331 fitness_score /= static_cast<float> (inliers.size ());
333 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 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 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 setSourceFeatures(const FeatureCloudConstPtr &features)
Provide a boost shared pointer to the source point cloud's feature descriptors.