pcl ::registration ::registration initial alignment of point clouds refining initial alignments ......
Post on 08-Mar-2018
273 Views
Preview:
TRANSCRIPT
Registration Iterative Closest Points (ICP) Initial Alignment Example/Tutorial Code
PCL :: RegistrationInitial alignment of point clouds
Refining initial alignmentsSeptember 25, 2011
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
Registration Iterative Closest Points (ICP) Initial Alignment Example/Tutorial Code
Registration
Wanted: transformation that aligns one point cloud to another.
I for initially aligning point clouds (based on features)I for refining initial alignments
(using the Iterative Closest Point (ICP) Algorithm)Dirk Holz / PCL :: Registration
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 cloud1. 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
Registration Iterative Closest Points (ICP) Initial Alignment Example/Tutorial Code
ICP (2/6)
I Given:Two n-dimensional sets of points – the model setM = {mi |mi ∈ Rn, i = 1, . . . ,Nm} and the data setD = {dj |dj ∈ Rn, j = 1, . . .Nd}
I Wanted:A rotation R and a translation ∆t that map D onto M.
I handled as an optimization problem =⇒ Minimizingmapping error E
E (R,∆t) =Nm∑i=1
Nd∑j=1
wi,j∣∣∣∣mi −
(Rdj + ∆t
)∣∣∣∣2 (1)
I Weighting factor wi,j encodes point correspondences,wi,j = 1 for correspondence(mi ,di), 0 otherwise
I Correspondence from Neighbor SearchDirk Holz / PCL :: Registration
Registration Iterative Closest Points (ICP) Initial Alignment Example/Tutorial Code
ICP (3/6)
The ICP APIpcl::IterativeClosestPoint<InType, OutType> icp;
Provide a pointer to the input point cloudicp.setInputCloud (input_cloud);
Provide a pointer to the target point cloudicp.setInputTarget (target_cloud);
Align input to target to obtainicp.align (aligned_cloud);
Eigen::Matrix4f transformation = icp.
getFinalTransformation ();
I the aligned cloud (transformed copy of input cloud),I and transformation used for alignment.
Dirk Holz / PCL :: Registration
Registration Iterative Closest Points (ICP) Initial Alignment Example/Tutorial Code
ICP (4/6)
The ICP API: Termination CriteriaI Max. number of iteration steps→ set via setMaximumIterations(nr_iterations)
I Convergence: Estimated transformation doesn’t change(the sum of differences between current and lasttransformation is smaller than a user-defined threshold)→ set via setTransformationEpsilon(epsilon)
I A solution was found (the sum of squared errors is smallerthan a user-defined threshold)→ set via setEuclideanFitnessEpsilon(distance)
Dirk Holz / PCL :: Registration
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).
I Maximum distance between correspondences:icp.setMaxCorrespondenceDistance (distance);
I Use RANSAC to neglect false correspondencesicp.setRANSACOutlierRejectionThreshold (distance);
I Model: Transformation (estimated on 3 samples)I Inliers: Points whose distance to the corresponding is
below the given threshold
Dirk Holz / PCL :: Registration
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.I Use Features to get an initial alignment!
→ pcl::SampleConsensusInitialAlignment
Dirk Holz / PCL :: Registration
Registration Iterative Closest Points (ICP) Initial Alignment Example/Tutorial Code
Initial Alignment (1/7)
1. Compute sets of keypoints2. Compute (local) feature descriptors (e.g. FPFH)3. Use SAC-based approach to find initial alignment
3.1 Take 3 random correspondence pairs3.2 Compute transformation for these pairs3.3 Apply transformation to all source points, and determine
inliers4. Use best transformation for initial alignment, and ICP for
refinement
*Dirk Holz / PCL :: Registration
Registration Iterative Closest Points (ICP) Initial Alignment Example/Tutorial Code
Initial Alignment (2/7)
points on similar surfaces
*
0
5
10
15
20
25
30
35
0 2 4 6 8 10 12 14 16
Rat
io o
f poi
nts
in o
ne b
in (
%)
Bins
Persistent Feature Points Histograms
P1
Q1
0
5
10
15
20
25
30
35
0 2 4 6 8 10 12 14 16
Rat
io o
f poi
nts
in o
ne b
in (
%)
Bins
Persistent Feature Points Histograms
P2
Q2
0
5
10
15
20
25
30
35
0 2 4 6 8 10 12 14 16
Rat
io o
f poi
nts
in o
ne b
in (
%)
Bins
Persistent Feature Points Histograms
P3
Q3
Dirk Holz / PCL :: Registration
Registration Iterative Closest Points (ICP) Initial Alignment Example/Tutorial Code
Initial Alignment (3/7)
*
Dirk Holz / PCL :: Registration
Registration Iterative Closest Points (ICP) Initial Alignment Example/Tutorial Code
Initial Alignment (4/7)Outdoor Example: Non-Linear Optimization
*Dirk Holz / PCL :: Registration
Registration Iterative Closest Points (ICP) Initial Alignment Example/Tutorial Code
Initial Alignment (5/7)
Dirk Holz / PCL :: Registration
Registration Iterative Closest Points (ICP) Initial Alignment Example/Tutorial Code
Initial Alignment (6/7)
Dirk Holz / PCL :: Registration
Registration Iterative Closest Points (ICP) Initial Alignment Example/Tutorial Code
Initial Alignment (7/7)
The SampleConsensusInitialAlignment APIpcl::SampleConsensusInitialAlignment<PointT, PointT,
DescriptorT> sac;
Provide a pointer to the input point cloud and featuressac.setInputCloud (source_points);
sac.setSourceFeatures (source_descriptors);
Provide a pointer to the target point cloud and featuressac.setInputTarget (target_points);
sac.setTargetFeatures (target_descriptors);
Align input to target to obtainsac.align (aligned_cloud);
Eigen::Matrix4f transformation = sac.
getFinalTransformation ();
Dirk Holz / PCL :: Registration
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
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
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
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
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
top related