41 #ifndef IA_RANSAC_HPP_ 42 #define IA_RANSAC_HPP_ 44 #include <pcl/common/distances.h> 47 template <
typename Po
intSource,
typename Po
intTarget,
typename FeatureT>
void 50 if (features == NULL || features->empty ())
52 PCL_ERROR (
"[pcl::%s::setSourceFeatures] Invalid or empty point cloud dataset given!\n", getClassName ().c_str ());
55 input_features_ = features;
59 template <
typename Po
intSource,
typename Po
intTarget,
typename FeatureT>
void 62 if (features == NULL || features->empty ())
64 PCL_ERROR (
"[pcl::%s::setTargetFeatures] Invalid or empty point cloud dataset given!\n", getClassName ().c_str ());
67 target_features_ = features;
68 feature_tree_->setInputCloud (target_features_);
72 template <
typename Po
intSource,
typename Po
intTarget,
typename FeatureT>
void 75 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 ());
86 int iterations_without_a_sample = 0;
87 int max_iterations_without_a_sample =
static_cast<int> (3 * cloud.points.size ());
88 sample_indices.clear ();
89 while (static_cast<int> (sample_indices.size ()) < nr_samples)
92 int sample_index = getRandomIndex (static_cast<int> (cloud.points.size ()));
95 bool valid_sample =
true;
96 for (
size_t i = 0; i < sample_indices.size (); ++i)
98 float distance_between_samples =
euclideanDistance (cloud.points[sample_index], cloud.points[sample_indices[i]]);
100 if (sample_index == sample_indices[i] || distance_between_samples < min_sample_distance)
102 valid_sample =
false;
110 sample_indices.push_back (sample_index);
111 iterations_without_a_sample = 0;
114 ++iterations_without_a_sample;
117 if (iterations_without_a_sample >= max_iterations_without_a_sample)
119 PCL_WARN (
"[pcl::%s::selectSamples] ", getClassName ().c_str ());
120 PCL_WARN (
"No valid sample found after %d iterations. Relaxing min_sample_distance_ to %f\n",
121 iterations_without_a_sample, 0.5*min_sample_distance);
123 min_sample_distance_ *= 0.5f;
124 min_sample_distance = min_sample_distance_;
125 iterations_without_a_sample = 0;
131 template <
typename Po
intSource,
typename Po
intTarget,
typename FeatureT>
void 133 const FeatureCloud &input_features,
const std::vector<int> &sample_indices,
134 std::vector<int> &corresponding_indices)
136 std::vector<int> nn_indices (k_correspondences_);
137 std::vector<float> nn_distances (k_correspondences_);
139 corresponding_indices.resize (sample_indices.size ());
140 for (
size_t i = 0; i < sample_indices.size (); ++i)
143 feature_tree_->nearestKSearch (input_features, sample_indices[i], k_correspondences_, nn_indices, nn_distances);
146 int random_correspondence = getRandomIndex (k_correspondences_);
147 corresponding_indices[i] = nn_indices[random_correspondence];
152 template <
typename Po
intSource,
typename Po
intTarget,
typename FeatureT>
float 156 std::vector<int> nn_index (1);
157 std::vector<float> nn_distance (1);
162 for (
int i = 0; i < static_cast<int> (cloud.points.size ()); ++i)
165 tree_->nearestKSearch (cloud, i, 1, nn_index, nn_distance);
168 error += compute_error (nn_distance[0]);
174 template <
typename Po
intSource,
typename Po
intTarget,
typename FeatureT>
void 178 if (!input_features_)
180 PCL_ERROR (
"[pcl::%s::computeTransformation] ", getClassName ().c_str ());
181 PCL_ERROR (
"No source features were given! Call setSourceFeatures before aligning.\n");
184 if (!target_features_)
186 PCL_ERROR (
"[pcl::%s::computeTransformation] ", getClassName ().c_str ());
187 PCL_ERROR (
"No target features were given! Call setTargetFeatures before aligning.\n");
191 if (input_->size () != input_features_->size ())
193 PCL_ERROR (
"[pcl::%s::computeTransformation] ", getClassName ().c_str ());
194 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",
195 input_->size (), input_features_->size ());
199 if (target_->size () != target_features_->size ())
201 PCL_ERROR (
"[pcl::%s::computeTransformation] ", getClassName ().c_str ());
202 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",
203 target_->size (), target_features_->size ());
208 error_functor_.reset (
new TruncatedError (static_cast<float> (corr_dist_threshold_)));
211 std::vector<int> sample_indices (nr_samples_);
212 std::vector<int> corresponding_indices (nr_samples_);
214 float error, lowest_error (0);
216 final_transformation_ = guess;
219 if (!guess.isApprox (Eigen::Matrix4f::Identity (), 0.01f))
223 lowest_error = computeErrorMetric (input_transformed, static_cast<float> (corr_dist_threshold_));
227 for (; i_iter < max_iterations_; ++i_iter)
230 selectSamples (*input_, nr_samples_, min_sample_distance_, sample_indices);
233 findSimilarFeatures (*input_features_, sample_indices, corresponding_indices);
236 transformation_estimation_->estimateRigidTransformation (*input_, sample_indices, *target_, corresponding_indices, transformation_);
240 error = computeErrorMetric (input_transformed, static_cast<float> (corr_dist_threshold_));
243 if (i_iter == 0 || error < lowest_error)
245 lowest_error = error;
246 final_transformation_ = transformation_;
255 #endif //#ifndef IA_RANSAC_HPP_ 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.
void setTargetFeatures(const FeatureCloudConstPtr &features)
Provide a boost shared pointer to the target point cloud's feature descriptors.
float euclideanDistance(const PointType1 &p1, const PointType2 &p2)
Calculate the euclidean distance between the two given points.
void setSourceFeatures(const FeatureCloudConstPtr &features)
Provide a boost shared pointer to the source point cloud's feature descriptors.
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...
Registration< PointSource, PointTarget >::PointCloudSource PointCloudSource
void selectSamples(const PointCloudSource &cloud, int nr_samples, float min_sample_distance, std::vector< int > &sample_indices)
Select nr_samples sample points from cloud while making sure that their pairwise distances are greate...
virtual void computeTransformation(PointCloudSource &output, const Eigen::Matrix4f &guess)
Rigid transformation computation method.
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...
FeatureCloud::ConstPtr FeatureCloudConstPtr