Point Cloud Library (PCL)  1.9.1
registration.h
1 #ifndef REGISTRATION_H
2 #define REGISTRATION_H
3 
4 #include "typedefs.h"
5 
6 #include <pcl/registration/ia_ransac.h>
7 #include <pcl/registration/icp.h>
8 
9 /* Use SampleConsensusInitialAlignment to find a rough alignment from the source cloud to the target cloud by finding
10  * correspondences between two sets of local features
11  * Inputs:
12  * source_points
13  * The "source" points, i.e., the points that must be transformed to align with the target point cloud
14  * source_descriptors
15  * The local descriptors for each source point
16  * target_points
17  * The "target" points, i.e., the points to which the source point cloud will be aligned
18  * target_descriptors
19  * The local descriptors for each target point
20  * min_sample_distance
21  * The minimum distance between any two random samples
22  * max_correspondence_distance
23  * The
24  * nr_interations
25  * The number of RANSAC iterations to perform
26  * Return: A transformation matrix that will roughly align the points in source to the points in target
27  */
28 Eigen::Matrix4f
29 computeInitialAlignment (const PointCloudPtr & source_points, const LocalDescriptorsPtr & source_descriptors,
30  const PointCloudPtr & target_points, const LocalDescriptorsPtr & target_descriptors,
31  float min_sample_distance, float max_correspondence_distance, int nr_iterations)
32 {
34  sac_ia.setMinSampleDistance (min_sample_distance);
35  sac_ia.setMaxCorrespondenceDistance (max_correspondence_distance);
36  sac_ia.setMaximumIterations (nr_iterations);
37 
38  sac_ia.setInputCloud (source_points);
39  sac_ia.setSourceFeatures (source_descriptors);
40 
41  sac_ia.setInputTarget (target_points);
42  sac_ia.setTargetFeatures (target_descriptors);
43 
44  PointCloud registration_output;
45  sac_ia.align (registration_output);
46 
47  return (sac_ia.getFinalTransformation ());
48 }
49 
50 /* Use IterativeClosestPoint to find a precise alignment from the source cloud to the target cloud,
51  * starting with an initial guess
52  * Inputs:
53  * source_points
54  * The "source" points, i.e., the points that must be transformed to align with the target point cloud
55  * target_points
56  * The "target" points, i.e., the points to which the source point cloud will be aligned
57  * intial_alignment
58  * An initial estimate of the transformation matrix that aligns the source points to the target points
59  * max_correspondence_distance
60  * A threshold on the distance between any two corresponding points. Any corresponding points that are further
61  * apart than this threshold will be ignored when computing the source-to-target transformation
62  * outlier_rejection_threshold
63  * A threshold used to define outliers during RANSAC outlier rejection
64  * transformation_epsilon
65  * The smallest iterative transformation allowed before the algorithm is considered to have converged
66  * max_iterations
67  * The maximum number of ICP iterations to perform
68  * Return: A transformation matrix that will precisely align the points in source to the points in target
69  */
70 Eigen::Matrix4f
71 refineAlignment (const PointCloudPtr & source_points, const PointCloudPtr & target_points,
72  const Eigen::Matrix4f & initial_alignment, float max_correspondence_distance,
73  float outlier_rejection_threshold, float transformation_epsilon, float max_iterations)
74 {
75 
77  icp.setMaxCorrespondenceDistance (max_correspondence_distance);
78  icp.setRANSACOutlierRejectionThreshold (outlier_rejection_threshold);
79  icp.setTransformationEpsilon (transformation_epsilon);
80  icp.setMaximumIterations (max_iterations);
81 
82  PointCloudPtr source_points_transformed (new PointCloud);
83  pcl::transformPointCloud (*source_points, *source_points_transformed, initial_alignment);
84 
85  icp.setInputCloud (source_points_transformed);
86  icp.setInputTarget (target_points);
87 
88  PointCloud registration_output;
89  icp.align (registration_output);
90 
91  return (icp.getFinalTransformation () * initial_alignment);
92 }
93 
94 #endif
pcl::Registration< PointSource, PointTarget >::setInputTarget
virtual void setInputTarget(const PointCloudTargetConstPtr &cloud)
Provide a pointer to the input target (e.g., the point cloud that we want to align the input source t...
Definition: registration.hpp:43
pcl::Registration< PointSource, PointTarget >::align
void align(PointCloudSource &output)
Call the registration algorithm which estimates the transformation and returns the transformed source...
Definition: registration.hpp:154
pcl::SampleConsensusInitialAlignment
SampleConsensusInitialAlignment is an implementation of the initial alignment algorithm described in ...
Definition: ia_ransac.h:54
pcl::IterativeClosestPoint
IterativeClosestPoint provides a base implementation of the Iterative Closest Point algorithm.
Definition: icp.h:94
pcl::IterativeClosestPoint::setInputTarget
virtual void setInputTarget(const PointCloudTargetConstPtr &cloud)
Provide a pointer to the input target (e.g., the point cloud that we want to align to the target)
Definition: icp.h:213
pcl::PointCloud
PointCloud represents the base class in PCL for storing collections of 3D points.
Definition: projection_matrix.h:53
pcl::PCLBase< PointSource >::setInputCloud
virtual void setInputCloud(const PointCloudConstPtr &cloud)
Provide a pointer to the input dataset.
Definition: pcl_base.hpp:66
pcl::Registration< PointSource, PointTarget, float >::setRANSACOutlierRejectionThreshold
void setRANSACOutlierRejectionThreshold(double inlier_threshold)
Set the inlier distance threshold for the internal RANSAC outlier rejection loop.
Definition: registration.h:295
pcl::SampleConsensusInitialAlignment::setMinSampleDistance
void setMinSampleDistance(float min_sample_distance)
Set the minimum distances between samples.
Definition: ia_ransac.h:171
pcl::Registration< PointSource, PointTarget >::getFinalTransformation
Matrix4 getFinalTransformation()
Get the final transformation matrix estimated by the registration method.
Definition: registration.h:261
pcl::transformPointCloud
void transformPointCloud(const pcl::PointCloud< PointT > &cloud_in, pcl::PointCloud< PointT > &cloud_out, const Eigen::Transform< Scalar, 3, Eigen::Affine > &transform, bool copy_all_fields=true)
Apply an affine transform defined by an Eigen Transform.
Definition: transforms.hpp:215
pcl::SampleConsensusInitialAlignment::setTargetFeatures
void setTargetFeatures(const FeatureCloudConstPtr &features)
Provide a boost shared pointer to the target point cloud's feature descriptors.
Definition: ia_ransac.hpp:60
pcl::Registration< PointSource, PointTarget >::setMaximumIterations
void setMaximumIterations(int nr_iterations)
Set the maximum number of iterations the internal optimization should run for.
Definition: registration.h:271
pcl::SampleConsensusInitialAlignment::setSourceFeatures
void setSourceFeatures(const FeatureCloudConstPtr &features)
Provide a boost shared pointer to the source point cloud's feature descriptors.
Definition: ia_ransac.hpp:48
pcl::Registration< PointSource, PointTarget >::setMaxCorrespondenceDistance
void setMaxCorrespondenceDistance(double distance_threshold)
Set the maximum distance threshold between two correspondent points in source <-> target.
Definition: registration.h:307
pcl::Registration< PointSource, PointTarget, float >::setTransformationEpsilon
void setTransformationEpsilon(double epsilon)
Set the transformation epsilon (maximum allowable translation squared difference between two consecut...
Definition: registration.h:322