The document discusses point cloud registration using iterative closest point (ICP) algorithm. It describes how ICP refines initial alignments between point clouds by finding corresponding points and minimizing distances. Feature-based approaches like FPFH descriptors and sample consensus are used to obtain an initial alignment, which ICP then refines. Example code shows how to perform initial alignment and apply ICP refinement.

1. Registration Iterative Closest Points (ICP) Initial Alignment Example/Tutorial Code
PCL :: Registration
Initial alignment of point clouds
Refining initial alignments
September 25, 2011

2. Registration Iterative Closest Points (ICP) Initial Alignment Example/Tutorial Code
Outline
1. Registration
2. Registration using the Iterative Closest Points (ICP)
3. Feature-Based Initial Alignment
4. Example/Tutorial Code
Dirk Holz / PCL :: Registration

3. Registration Iterative Closest Points (ICP) Initial Alignment Example/Tutorial Code
Registration
Wanted: transformation that aligns one point cloud to another.
for initially aligning point clouds (based on features)
for refining initial alignments
(using the Iterative Closest Point (ICP) Algorithm)
Dirk Holz / PCL :: Registration

4. Registration Iterative Closest Points (ICP) Initial Alignment Example/Tutorial Code
ICP (1/6)
Registration using the Iterative Closest Point (ICP) Algorithm
Given an input point cloud and a target point cloud
1. determine pairs of corresponding points,
2. estimate a transformation that minimizes the distances
between the correspondences,
3. apply the transformation to align input and target.
Dirk Holz / PCL :: Registration

5. Registration Iterative Closest Points (ICP) Initial Alignment Example/Tutorial Code
ICP (2/6)
Given:
Two n-dimensional sets of points – the model set
R
M = {mi |mi ∈ n , i = 1, . . . , Nm } and the data set
R
D = {dj |dj ∈ n , j = 1, . . . Nd }
Wanted:
A rotation R and a translation ∆t that map D onto M.
handled as an optimization problem =⇒ Minimizing
mapping error E
Nm Nd
2
E (R, ∆t) = wi,j mi − Rdj + ∆t (1)
i=1 j=1
Weighting factor wi,j encodes point correspondences,
wi,j = 1 for correspondence(mi , di ), 0 otherwise
Correspondence from Neighbor Search
Dirk Holz / PCL :: Registration

6. Registration Iterative Closest Points (ICP) Initial Alignment Example/Tutorial Code
ICP (3/6)
The ICP API
pcl::IterativeClosestPoint<InType, OutType> icp;
Provide a pointer to the input point cloud
icp.setInputCloud (input_cloud);
Provide a pointer to the target point cloud
icp.setInputTarget (target_cloud);
Align input to target to obtain
icp.align (aligned_cloud);
Eigen::Matrix4f transformation = icp.
getFinalTransformation ();
the aligned cloud (transformed copy of input cloud),
and transformation used for alignment.
Dirk Holz / PCL :: Registration

7. Registration Iterative Closest Points (ICP) Initial Alignment Example/Tutorial Code
ICP (4/6)
The ICP API: Termination Criteria
Max. number of iteration steps
→ set via setMaximumIterations(nr_iterations)
Convergence: Estimated transformation doesn’t change
(the sum of differences between current and last
transformation is smaller than a user-defined threshold)
→ set via setTransformationEpsilon(epsilon)
A solution was found (the sum of squared errors is smaller
than a user-defined threshold)
→ set via setEuclideanFitnessEpsilon(distance)
Dirk Holz / PCL :: Registration

8. Registration Iterative Closest Points (ICP) Initial Alignment Example/Tutorial Code
ICP (5/6)
The ICP API: Problems with ICP
False correspondences negatively affect the alignment
(the algorithms gets caught in local minima).
Maximum distance between correspondences:
icp.setMaxCorrespondenceDistance (distance);
Use RANSAC to neglect false correspondences
icp.setRANSACOutlierRejectionThreshold (distance);
Model: Transformation (estimated on 3 samples)
Inliers: Points whose distance to the corresponding is
below the given threshold
Dirk Holz / PCL :: Registration

9. Registration Iterative Closest Points (ICP) Initial Alignment Example/Tutorial Code
ICP (6/6)
The ICP API: Problems with ICP
The ICP algorithms needs a rough initial alignment.
Use Features to get an initial alignment!
→ pcl::SampleConsensusInitialAlignment
Dirk Holz / PCL :: Registration

10. Registration Iterative Closest Points (ICP) Initial Alignment Example/Tutorial Code
Initial Alignment (1/7)
1. Compute sets of keypoints
2. Compute (local) feature descriptors (e.g. FPFH)
3. Use SAC-based approach to find initial alignment
3.1 Take 3 random correspondence pairs
3.2 Compute transformation for these pairs
3.3 Apply transformation to all source points, and determine
inliers
4. Use best transformation for initial alignment, and ICP for
refinement
*
Dirk Holz / PCL :: Registration

11. Registration Iterative Closest Points (ICP) Initial Alignment Example/Tutorial Code
Initial Alignment (2/7)
points on similar surfaces
*
Persistent Feature Points Histograms Persistent Feature Points Histograms Persistent Feature Points Histograms
35 35 35
P1 P2 P3
Ratio of points in one bin (%)
Ratio of points in one bin (%)
Ratio of points in one bin (%)
30 30 30
25
Q1 25
Q2 25
Q3
20 20 20
15 15 15
10 10 10
5 5 5
0 0 0
0 2 4 6 8 10 12 14 16 0 2 4 6 8 10 12 14 16 0 2 4 6 8 10 12 14 16
Bins Bins Bins
Dirk Holz / PCL :: Registration

12. Registration Iterative Closest Points (ICP) Initial Alignment Example/Tutorial Code
Initial Alignment (3/7)
*
Dirk Holz / PCL :: Registration

13. Registration Iterative Closest Points (ICP) Initial Alignment Example/Tutorial Code
Initial Alignment (4/7)
Outdoor Example: Non-Linear Optimization
*
Dirk Holz / PCL :: Registration

14. Registration Iterative Closest Points (ICP) Initial Alignment Example/Tutorial Code
Initial Alignment (5/7)
Dirk Holz / PCL :: Registration

15. Registration Iterative Closest Points (ICP) Initial Alignment Example/Tutorial Code
Initial Alignment (6/7)
Dirk Holz / PCL :: Registration

16. Registration Iterative Closest Points (ICP) Initial Alignment Example/Tutorial Code
Initial Alignment (7/7)
The SampleConsensusInitialAlignment API
pcl::SampleConsensusInitialAlignment<PointT, PointT,
DescriptorT> sac;
Provide a pointer to the input point cloud and features
sac.setInputCloud (source_points);
sac.setSourceFeatures (source_descriptors);
Provide a pointer to the target point cloud and features
sac.setInputTarget (target_points);
sac.setTargetFeatures (target_descriptors);
Align input to target to obtain
sac.align (aligned_cloud);
Eigen::Matrix4f transformation = sac.
getFinalTransformation ();
Dirk Holz / PCL :: Registration

17. Registration Iterative Closest Points (ICP) Initial Alignment Example/Tutorial Code
Example/Tutorial Code (1/5)
Initial alignment of point clouds
1 pcl::SampleConsensusInitialAlignment<PointT, PointT,
DescriptorT> sac;
2 sac.setMinSampleDistance (min_sample_distance);
3 sac.setMaxCorrespondenceDistance (
max_correspondence_distance);
4 sac.setMaximumIterations (nr_iterations);
5
6 sac.setInputCloud (source_points);
7 sac.setSourceFeatures (source_descriptors);
8
9 sac.setInputTarget (target_points);
10 sac.setTargetFeatures (target_descriptors);
11
12 PointCloudPtr aligned_source(new PointCloud);
13 sac.align (*aligned_source);
14 Eigen::Matrix4f initial_T = sac.getFinalTransformation
());
Dirk Holz / PCL :: Registration

18. Registration Iterative Closest Points (ICP) Initial Alignment Example/Tutorial Code
Example Code (2/5)
Refining initial alignments using ICP
1 pcl::IterativeClosestPoint<PointT, PointT> icp;
2 icp.setMaxCorrespondenceDistance (distance);
3 icp.setRANSACOutlierRejectionThreshold (distance);
4 icp.setTransformationEpsilon (transformation_epsilon);
5 icp.setMaximumIterations (max_iterations);
6
7 icp.setInputCloud (aligned_source); // from (1)
8 icp.setInputTarget (target_points);
9
10 PointCloud registration_output;
11 icp.align (registration_output);
12
13 Eigen::Matrix4f refined_T =
14 icp.getFinalTransformation () * initial_T);
Dirk Holz / PCL :: Registration

19. Registration Iterative Closest Points (ICP) Initial Alignment Example/Tutorial Code
Example Code (3/5)
Example: Correspondences in initial alignment
$ ./correspondence_viewer ../../data/robot/robot0
../../data/robot/robot1 -n 5
Dirk Holz / PCL :: Registration

20. Registration Iterative Closest Points (ICP) Initial Alignment Example/Tutorial Code
Example Code (4/5)
Example: Initial alignment + refinement
$ ./test_registration ../../data/robot/robot0 ../../
data/robot/robot1 -i 0.025,0.01,500 -r 0.05,0.05,0,100
Dirk Holz / PCL :: Registration

21. Registration Iterative Closest Points (ICP) Initial Alignment Example/Tutorial Code
Example Code (5/5)
Example: Final model = robot0 + robot1
$ ./test_registration ... -s robot_registered.pcd
$ pcd_viewer robot_registered.pcd
Dirk Holz / PCL :: Registration