34 #include <pcl/registration/icp.h>
35 #include <pcl/registration/transformation_estimation_2D.h>
36 #include <pcl/registration/transformation_estimation_svd.h>
37 #include <pcl/sample_consensus/sac_model_registration.h>
38 #include <pcl/sample_consensus/ransac.h>
39 #include <pcl/common/common.h>
51 const pcl::PointCloud<pcl::PointXYZ> & cloud1,
52 const pcl::PointCloud<pcl::PointXYZ> & cloud2)
54 pcl::registration::TransformationEstimationSVD<pcl::PointXYZ, pcl::PointXYZ>
svd;
58 svd.estimateRigidTransformation(cloud1, cloud2,
matrix);
64 const pcl::PointCloud<pcl::PointXYZ>::ConstPtr & cloud1,
65 const pcl::PointCloud<pcl::PointXYZ>::ConstPtr & cloud2,
66 double inlierThreshold,
70 std::vector<int> * inliersOut,
79 *covariance = cv::Mat::eye(6,6,CV_64FC1);
82 if(cloud1->size() >=3 && cloud1->size() == cloud2->size())
85 UDEBUG(
"iterations=%d inlierThreshold=%f", iterations, inlierThreshold);
86 std::vector<int> source_indices (cloud2->size());
87 std::vector<int> target_indices (cloud1->size());
90 for (
int i = 0;
i < (
int)cloud1->size(); ++
i)
92 source_indices[
i] =
i;
93 target_indices[
i] =
i;
98 pcl::SampleConsensusModelRegistration<pcl::PointXYZ>::Ptr
model;
99 model.reset(
new pcl::SampleConsensusModelRegistration<pcl::PointXYZ>(cloud2, source_indices));
101 model->setInputTarget (cloud1, target_indices);
103 pcl::RandomSampleConsensus<pcl::PointXYZ> sac (
model, inlierThreshold);
104 sac.setMaxIterations(iterations);
107 if(sac.computeModel())
109 std::vector<int> inliers;
110 Eigen::VectorXf model_coefficients;
112 sac.getInliers(inliers);
113 sac.getModelCoefficients (model_coefficients);
115 if (refineIterations>0)
117 double error_threshold = inlierThreshold;
118 int refine_iterations = 0;
119 bool inlier_changed =
false, oscillating =
false;
120 std::vector<int> new_inliers, prev_inliers = inliers;
121 std::vector<size_t> inliers_sizes;
122 Eigen::VectorXf new_model_coefficients = model_coefficients;
126 model->optimizeModelCoefficients (prev_inliers, new_model_coefficients, new_model_coefficients);
127 inliers_sizes.push_back (prev_inliers.size ());
130 model->selectWithinDistance (new_model_coefficients, error_threshold, new_inliers);
131 UDEBUG(
"RANSAC refineModel: Number of inliers found (before/after): %d/%d, with an error threshold of %f.",
132 (
int)prev_inliers.size (), (
int)new_inliers.size (), error_threshold);
134 if (new_inliers.empty ())
137 if (refine_iterations >= refineIterations)
145 double variance =
model->computeVariance ();
146 error_threshold =
std::min (inlierThreshold, refineSigma *
sqrt(variance));
148 UDEBUG (
"RANSAC refineModel: New estimated error threshold: %f (variance=%f) on iteration %d out of %d.",
149 error_threshold, variance, refine_iterations, refineIterations);
150 inlier_changed =
false;
154 if (new_inliers.size () != prev_inliers.size ())
157 if (inliers_sizes.size () >= 4)
159 if (inliers_sizes[inliers_sizes.size () - 1] == inliers_sizes[inliers_sizes.size () - 3] &&
160 inliers_sizes[inliers_sizes.size () - 2] == inliers_sizes[inliers_sizes.size () - 4])
166 inlier_changed =
true;
171 for (
size_t i = 0;
i < prev_inliers.size (); ++
i)
174 if (prev_inliers[
i] != new_inliers[
i])
176 inlier_changed =
true;
181 while (inlier_changed && ++refine_iterations < refineIterations);
184 if (new_inliers.empty ())
186 UWARN (
"RANSAC refineModel: Refinement failed: got an empty set of inliers!");
191 UDEBUG(
"RANSAC refineModel: Detected oscillations in the model refinement.");
195 model_coefficients = new_model_coefficients;
198 if (inliers.size() >= 3)
202 *inliersOut = inliers;
206 double variance =
model->computeVariance();
208 *covariance *= variance;
212 Eigen::Matrix4f bestTransformation;
213 bestTransformation.row (0) = model_coefficients.segment<4>(0);
214 bestTransformation.row (1) = model_coefficients.segment<4>(4);
215 bestTransformation.row (2) = model_coefficients.segment<4>(8);
216 bestTransformation.row (3) = model_coefficients.segment<4>(12);
219 UDEBUG(
"RANSAC inliers=%d/%d tf=%s", (
int)inliers.size(), (
int)cloud1->size(),
transform.prettyPrint().c_str());
225 UDEBUG(
"RANSAC: Model with inliers < 3");
230 UDEBUG(
"RANSAC: Failed to find model");
235 UDEBUG(
"Not enough points to compute the transform");
240 template<
typename Po
intNormalT>
242 const typename pcl::PointCloud<PointNormalT>::ConstPtr & cloudA,
243 const typename pcl::PointCloud<PointNormalT>::ConstPtr & cloudB,
244 double maxCorrespondenceDistance,
245 double maxCorrespondenceAngle,
247 int & correspondencesOut,
251 correspondencesOut = 0;
252 typename pcl::registration::CorrespondenceEstimation<PointNormalT, PointNormalT>::Ptr est;
253 est.reset(
new pcl::registration::CorrespondenceEstimation<PointNormalT, PointNormalT>);
254 const typename pcl::PointCloud<PointNormalT>::ConstPtr &
target = cloudA->size()>cloudB->size()?cloudA:cloudB;
255 const typename pcl::PointCloud<PointNormalT>::ConstPtr &
source = cloudA->size()>cloudB->size()?cloudB:cloudA;
256 est->setInputTarget(
target);
257 est->setInputSource(
source);
258 pcl::Correspondences correspondences;
259 est->determineReciprocalCorrespondences(correspondences, maxCorrespondenceDistance);
261 if(correspondences.size())
263 std::vector<double> distances(correspondences.size());
264 correspondencesOut = 0;
265 for(
unsigned int i=0;
i<correspondences.size(); ++
i)
267 distances[
i] = correspondences[
i].distance;
268 if(maxCorrespondenceAngle <= 0.0)
270 ++correspondencesOut;
275 target->at(correspondences[
i].index_match).normal_x,
276 target->at(correspondences[
i].index_match).normal_y,
277 target->at(correspondences[
i].index_match).normal_z,
280 source->at(correspondences[
i].index_query).normal_x,
281 source->at(correspondences[
i].index_query).normal_y,
282 source->at(correspondences[
i].index_query).normal_z,
285 if(
angle < maxCorrespondenceAngle)
287 ++correspondencesOut;
291 if(correspondencesOut)
293 distances.resize(correspondencesOut);
296 std::sort(distances.begin (), distances.end ());
297 double median_error_sqr = distances[distances.size () >> 1];
298 variance = (2.1981 * median_error_sqr);
304 const pcl::PointCloud<pcl::PointNormal>::ConstPtr & cloudA,
305 const pcl::PointCloud<pcl::PointNormal>::ConstPtr & cloudB,
306 double maxCorrespondenceDistance,
307 double maxCorrespondenceAngle,
309 int & correspondencesOut,
312 computeVarianceAndCorrespondencesImpl<pcl::PointNormal>(cloudA, cloudB, maxCorrespondenceDistance, maxCorrespondenceAngle, variance, correspondencesOut, reciprocal);
316 const pcl::PointCloud<pcl::PointXYZINormal>::ConstPtr & cloudA,
317 const pcl::PointCloud<pcl::PointXYZINormal>::ConstPtr & cloudB,
318 double maxCorrespondenceDistance,
319 double maxCorrespondenceAngle,
321 int & correspondencesOut,
324 computeVarianceAndCorrespondencesImpl<pcl::PointXYZINormal>(cloudA, cloudB, maxCorrespondenceDistance, maxCorrespondenceAngle, variance, correspondencesOut, reciprocal);
327 template<
typename Po
intT>
329 const typename pcl::PointCloud<PointT>::ConstPtr & cloudA,
330 const typename pcl::PointCloud<PointT>::ConstPtr & cloudB,
331 double maxCorrespondenceDistance,
333 int & correspondencesOut,
337 correspondencesOut = 0;
338 typename pcl::registration::CorrespondenceEstimation<PointT, PointT>::Ptr est;
339 est.reset(
new pcl::registration::CorrespondenceEstimation<PointT, PointT>);
340 est->setInputTarget(cloudA->size()>cloudB->size()?cloudA:cloudB);
341 est->setInputSource(cloudA->size()>cloudB->size()?cloudB:cloudA);
342 pcl::Correspondences correspondences;
343 est->determineReciprocalCorrespondences(correspondences, maxCorrespondenceDistance);
345 if(correspondences.size()>=3)
347 std::vector<double> distances(correspondences.size());
348 for(
unsigned int i=0;
i<correspondences.size(); ++
i)
350 distances[
i] = correspondences[
i].distance;
354 std::sort(distances.begin (), distances.end ());
355 double median_error_sqr = distances[distances.size () >> 1];
356 variance = (2.1981 * median_error_sqr);
359 correspondencesOut = (
int)correspondences.size();
363 const pcl::PointCloud<pcl::PointXYZ>::ConstPtr & cloudA,
364 const pcl::PointCloud<pcl::PointXYZ>::ConstPtr & cloudB,
365 double maxCorrespondenceDistance,
367 int & correspondencesOut,
370 computeVarianceAndCorrespondencesImpl<pcl::PointXYZ>(cloudA, cloudB, maxCorrespondenceDistance, variance, correspondencesOut, reciprocal);
374 const pcl::PointCloud<pcl::PointXYZI>::ConstPtr & cloudA,
375 const pcl::PointCloud<pcl::PointXYZI>::ConstPtr & cloudB,
376 double maxCorrespondenceDistance,
378 int & correspondencesOut,
381 computeVarianceAndCorrespondencesImpl<pcl::PointXYZI>(cloudA, cloudB, maxCorrespondenceDistance, variance, correspondencesOut, reciprocal);
385 template<
typename Po
intT>
387 const typename pcl::PointCloud<PointT>::ConstPtr & cloud_target,
388 double maxCorrespondenceDistance,
389 int maximumIterations,
391 pcl::PointCloud<PointT> & cloud_source_registered,
395 pcl::IterativeClosestPoint<PointT, PointT>
icp;
397 icp.setInputTarget (cloud_target);
398 icp.setInputSource (cloud_source);
402 typename pcl::registration::TransformationEstimation2D<PointT, PointT>::Ptr est;
403 est.reset(
new pcl::registration::TransformationEstimation2D<PointT, PointT>);
404 icp.setTransformationEstimation(est);
408 icp.setMaxCorrespondenceDistance (maxCorrespondenceDistance);
410 icp.setMaximumIterations (maximumIterations);
418 icp.align (cloud_source_registered);
419 hasConverged =
icp.hasConverged();
424 Transform icp(
const pcl::PointCloud<pcl::PointXYZ>::ConstPtr & cloud_source,
425 const pcl::PointCloud<pcl::PointXYZ>::ConstPtr & cloud_target,
426 double maxCorrespondenceDistance,
427 int maximumIterations,
429 pcl::PointCloud<pcl::PointXYZ> & cloud_source_registered,
433 return icpImpl(cloud_source, cloud_target, maxCorrespondenceDistance, maximumIterations, hasConverged, cloud_source_registered,
epsilon, icp2D);
437 Transform icp(
const pcl::PointCloud<pcl::PointXYZI>::ConstPtr & cloud_source,
438 const pcl::PointCloud<pcl::PointXYZI>::ConstPtr & cloud_target,
439 double maxCorrespondenceDistance,
440 int maximumIterations,
442 pcl::PointCloud<pcl::PointXYZI> & cloud_source_registered,
446 return icpImpl(cloud_source, cloud_target, maxCorrespondenceDistance, maximumIterations, hasConverged, cloud_source_registered,
epsilon, icp2D);
450 template<
typename Po
intNormalT>
452 const typename pcl::PointCloud<PointNormalT>::ConstPtr & cloud_source,
453 const typename pcl::PointCloud<PointNormalT>::ConstPtr & cloud_target,
454 double maxCorrespondenceDistance,
455 int maximumIterations,
457 pcl::PointCloud<PointNormalT> & cloud_source_registered,
461 pcl::IterativeClosestPoint<PointNormalT, PointNormalT>
icp;
463 icp.setInputTarget (cloud_target);
464 icp.setInputSource (cloud_source);
466 typename pcl::registration::TransformationEstimationPointToPlaneLLS<PointNormalT, PointNormalT>::Ptr est;
467 est.reset(
new pcl::registration::TransformationEstimationPointToPlaneLLS<PointNormalT, PointNormalT>);
468 icp.setTransformationEstimation(est);
471 icp.setMaxCorrespondenceDistance (maxCorrespondenceDistance);
473 icp.setMaximumIterations (maximumIterations);
481 icp.align (cloud_source_registered);
482 hasConverged =
icp.hasConverged();
496 const pcl::PointCloud<pcl::PointNormal>::ConstPtr & cloud_source,
497 const pcl::PointCloud<pcl::PointNormal>::ConstPtr & cloud_target,
498 double maxCorrespondenceDistance,
499 int maximumIterations,
501 pcl::PointCloud<pcl::PointNormal> & cloud_source_registered,
505 return icpPointToPlaneImpl(cloud_source, cloud_target, maxCorrespondenceDistance, maximumIterations, hasConverged, cloud_source_registered,
epsilon, icp2D);
509 const pcl::PointCloud<pcl::PointXYZINormal>::ConstPtr & cloud_source,
510 const pcl::PointCloud<pcl::PointXYZINormal>::ConstPtr & cloud_target,
511 double maxCorrespondenceDistance,
512 int maximumIterations,
514 pcl::PointCloud<pcl::PointXYZINormal> & cloud_source_registered,
518 return icpPointToPlaneImpl(cloud_source, cloud_target, maxCorrespondenceDistance, maximumIterations, hasConverged, cloud_source_registered,
epsilon, icp2D);