pcregistericp

Register two point clouds using ICP algorithm

Syntax

tform = pcregistericp(moving,fixed)

[tform,movingReg] = pcregistericp(moving,fixed)

[___,rmse] = pcregistericp(moving,fixed)

[___] = pcregistericp(moving,fixed,Name=Value)

Description

tform = pcregistericp(moving,fixed) returns a rigid transformation that registers a moving point cloud to a fixed point cloud.

The registration algorithm is based on the iterative closest point (ICP) algorithm. Best performance of this iterative process requires adjusting properties for your data. To improve the accuracy and efficiency of registration, consider downsampling point clouds using pcdownsample before using pcregistericp.

The registration algorithm requires point cloud normals when you select the "pointToPlane" or the "planeToPlane" (also known as Generalized-ICP or G-ICP) metric. If the Normal property of the input point cloud is empty, the function fills it.

[tform,movingReg] = pcregistericp(moving,fixed) additionally returns the transformed point cloud that aligns with the fixed point cloud.

[___,rmse] = pcregistericp(moving,fixed) additionally returns the root mean square error of the Euclidean distance between the aligned points, using any of the preceding syntaxes.

[___] = pcregistericp(moving,fixed,Name=Value) specifies options using one or more name-value arguments in addition to any combination of arguments from previous syntaxes. For example, pcregistericp(moving,fixed,Metric="planeToPlane") sets the minimization metric to "planeToPlane".

Examples

Align Two Point Clouds Using ICP Algorithm

Load point cloud data into the workspace.

ptCloud = pcread("teapot.ply");

Display the loaded point cloud.

pcshow(ptCloud) 
title("Teapot")

Create a transformation object with 30 degree rotation along the Z-axis and with a translation of [5 5 10].

rotationAngles = [0 0 30];
translation = [5 5 10];
tform1 = rigidtform3d(rotationAngles,translation);

Transform the point cloud and visualize the result.

ptCloudTformed = pctransform(ptCloud,tform1);

pcshow(ptCloudTformed)
title("Transformed Teapot")

Register the point clouds.

tform = pcregistericp(ptCloudTformed,ptCloud);

Compare the result with the true transformation.

disp(tform1.A);

    0.8660   -0.5000         0    5.0000
    0.5000    0.8660         0    5.0000
         0         0    1.0000   10.0000
         0         0         0    1.0000

tform2 = invert(tform);
disp(tform2.A);

    0.8660   -0.5000    0.0000    5.0000
    0.5000    0.8660    0.0000    5.0000
   -0.0000   -0.0000    1.0000   10.0000
         0         0         0    1.0000

Align Two Point Clouds Using Plane-to-Plane ICP Registration

Load point cloud data into the workspace.

ld = load("livingRoom.mat");
moving = ld.livingRoomData{1};
fixed  = ld.livingRoomData{2};

Visualize the point cloud and set the direction to display the y-axis.

figure
pcshowpair(moving,fixed,VerticalAxis="Y",VerticalAxisDir="Down")

Downsample the point clouds using nonuniformGridSample method to improve the efficiency and accuracy of registration.

maxNumPoints = 12;
fixedDownsampled = pcdownsample(fixed,"nonuniformGridSample",maxNumPoints);
movingDownsampled = pcdownsample(moving,"nonuniformGridSample",maxNumPoints);

Align the point clouds using plane-to-plane (Generalized-ICP) registration.

[~,movingReg] = pcregistericp(movingDownsampled,fixedDownsampled,Metric="planeToPlane");

Visualize the point cloud alignment and set the direction to display the y-axis.

figure
pcshowpair(movingReg,fixed,VerticalAxis="Y",VerticalAxisDir="Down")

Align Two Colored Point Clouds

Load moving point cloud data having color information into the workspace.

moving = pcread("apartmentMoving.pcd");

Load fixed point cloud data having color information into the workspace.

fixed  = pcread("apartmentFixed.pcd");

Visualize the colored point clouds and set the direction to display the yx-plane.

figure
pcshowpair(moving,fixed,"ViewPlane","YX","ColorSource","Color")

Align the colored point clouds using plane-to-plane (Generalized-ICP) registration and color information. Set the value to the Metric argument as planeToPlaneWithColor.

[~,movingReg] = pcregistericp(moving,fixed,Metric="planeToPlaneWithColor");

Visualize the point cloud alignment and set the direction to display the yx-plane.

figure
pcshowpair(movingReg,fixed,"ViewPlane","YX","ColorSource","Color")

Input Arguments

moving — Moving point cloud

pointCloud object

Moving point cloud, specified as a pointCloud object.

fixed — Fixed point cloud

pointCloud object

Fixed point cloud, specified as a pointCloud object.

Name-Value Arguments

Specify optional pairs of arguments as Name1=Value1,...,NameN=ValueN, where Name is the argument name and Value is the corresponding value. Name-value arguments must appear after other arguments, but the order of the pairs does not matter.

Before R2021a, use commas to separate each name and value, and enclose Name in quotes.

Example: Metric="planeToPlane" sets the minimization metric for the ICP algorithm to "planeToPlane".

Metric — Minimization metric

"pointToPoint" (default) | "pointToPlane" | "planeToPlane" | "pointToPlaneWithColor" | "planeToPlaneWithColor"

Minimization metric, specified as "pointToPoint", "pointToPlane", "planeToPlane", "planeToPlaneWithColor", or "pointToPlaneWithColor". The iterative closest point (ICP) algorithm estimates the rigid transformation between the moving and fixed point clouds by minimizing the distance between the two point clouds according to the specified metric.

Specifying Metric as "pointToPlane" or "planeToPlane" (also known as the Generalized-ICP algorithm) can reduce the number of iterations required for registration. However, these metrics require extra algorithmic steps within each iteration.

Specify Metric as "pointToPlane" or "planeToPlane" to improve the registration of planar surfaces. Use the "pointToPlaneWithColor" or "planeToPlaneWithColor" metric to improve the registration of point clouds with color information.

Downsample Method Selection
Downsample the point clouds using the pcdownsample function. The registration algorithm requires point cloud normals when you select the "pointToPoint" metric. The "nonuniformGridSample" algorithm computes the normals on the original data prior to downsampling, resulting in more accurate normals. Therefore, using the "nonuniformGridSample" downsampling method can result in a more accurate "pointToPoint" registration. Use the "random", "gridAverage", or "nonuniformGridSample" method for the pcdownsample function according to this table.

Metric	Moving Point Cloud Downsample Method	Fixed Point Cloud Downsample Method
"pointToPoint"	"random"	"random"
	"gridAverage"	"gridAverage"
"pointToPlane"	"gridAverage"	"gridAverage"
	"random"	"nonuniformGridSample"
"planeToPlane"	"gridAverage"	"gridAverage"
	"nonuniformGridSample"	"nonuniformGridSample"
"planeToPlaneWithColor"	"gridAverage"	"gridAverage"
	"nonuniformGridSample"	"nonuniformGridSample"

The "pointToPlaneWithColor" metric uses a multiscale registration approach where the point clouds are registered from coarser registration to finer registration. The result of the coarser registration is fed to the next finer registration step as the initial transformation. Therefore, the pcregistericp function does not use the normals in the input point clouds and thus the downsampling method does not affect its registration accuracy.

Extrapolate — Extrapolation

false or 0 (default) | true or 1

Extrapolation, specified as a logical 0 (false) or 1 (true). When you specify this argument as true, the function adds an extrapolation step that traces out a path in the registration state space, as described in A Method for Registration of 3-D Shapes. Enabling extrapolation can reduce the number of iterations required for the point clouds to converge.

You cannot specify the Extrapolate name-value argument when using the "planeToPlane", "pointToPlaneWithColor", or "planeToPlaneWithColor" metrics.

Warning

The Extrapolate name-value argument will be removed in a future release. You can increase the MaxIterations or use the "pointToPlane" or "planeToPlane" values for Metric name-value argument for similar accuracy.

InlierRatio — Inlier percentage of matched pairs

1 (default) | scalar in the range (0, 1]

Inlier percentage of matched pairs, specified as a scalar between (0, 1]. This value specifies the percentage of matched pairs to use as inliers. The function considers a pair of matched points to be an inlier if the Euclidean distance between the two points is less than the specified percentage of the maximum Euclidean distance between any two points in a matched pair of the point clouds. By default, the function uses all matching pairs as inliers. To instead specify the inliers using distance use the InlierDistance name-value argument.

InlierDistance — Inlier distance threshold

scalar

Inlier distance threshold, specified as a scalar. This value sets the maximum distance between a point in the fixed point cloud and a matched point in the moving point cloud for the function to consider them as inliers. Decrease this value when you have a more accurate initial transformation, InitialTransform. Increase this value when there is more uncertainty in the initial transformation. Note that, increasing this value can decrease the speed of registration. To instead specify the inliers as a ratio of the matched pairs of points between the point clouds, use the InlierRatio name-value argument.

MaxIterations — Maximum number of iterations

30 (default) | positive integer

Maximum number of iterations, specified as a positive integer. This value specifies the maximum number of iterations over which the function attempts to make the two point clouds converge.

Tolerance — Tolerance between consecutive ICP iterations

[0.01 0.5] (default) | two-element vector

Tolerance between consecutive ICP iterations, specified as a two-element vector of the form [Tdiff Rdiff], where Tdiff and Rdiff represent the tolerances of absolute difference in translation and in rotation, respectively, estimated in consecutive ICP iterations. Tdiff measures the Euclidean distance between two translation vectors. Rdiff measures the angular difference in degrees. The algorithm stops when the average difference between estimated rigid transformations in the three most recent consecutive iterations is less than the specified tolerance values.

InitialTransform — Initial rigid transformation

rigidtform3d object

Initial rigid transformation, specified as a rigidtform3d object. The default value is a rigidtform3d object that contains the translation between the centroids of the moving and the fixed point clouds. The initial rigid transformation is useful when you have an external coarse estimation.

Note

For the "pointToPlaneWithColor" metric, the default value of the InitialTransform name-value argument is a rigidtform3d object that performs an identity transformation.

Verbose — Display progress information

false or 0 (default) | true or 1

Display progress information, specified as a logical 0 (false) or 1 (true). Specify Verbose as true to display progress information.

Output Arguments

tform — Rigid transformation

rigidtform3d object

Rigid transformation, returned as a rigidtform3d object. The rigid transformation registers a moving point cloud to a fixed point cloud. The rigidtform3d object describes the rigid 3-D transform. The iterative closest point (ICP) algorithm estimates the rigid transformation between the moving and fixed point clouds.

movingReg — Transformed point cloud

pointCloud object

Transformed point cloud, returned as a pointCloud object. The transformed point cloud is aligned with the fixed point cloud.

rmse — Root mean square error

positive numeric scalar

Root mean square error, returned as a positive numeric scalar that represents the Euclidean distance between the aligned points. The aligned points is a result of the moving point cloud transformed by the tform output and the fixed point cloud. For each point in the fixed point cloud, the algorithm finds the closest point in the transformed point cloud, movingReg, then computes the Euclidean distance between the points, and then computes the rmse. A low rmse value indicates a more accurate registration. The rmse calculation is:

rmse = sqrt(sum(distance.^2,'all')/numel(distance));

Algorithms

Tips

• For ground vehicle point clouds, you can improve performance and accuracy by removing the ground using the pcfitplane or segmentGroundFromLidarData function before registration. For details on how to do this, see the helperProcessPointCloud helper function in the Build a Map from Lidar Data (Automated Driving Toolbox) example.

• To merge more than two point clouds, you can use the pccat function instead of the pcmerge function.

References

[1] Besl, P.J., and Neil D. McKay. “A Method for Registration of 3-D Shapes.” IEEE Transactions on Pattern Analysis and Machine Intelligence 14, no. 2 (February 1992): 239–256. https://doi.org/10.1109/34.121791.

[2] Chen, Yang, and Gérard Medioni. “Object Modelling by Registration of Multiple Range Images.” Image and Vision Computing 10, no. 3 (April 1992): 145–155. https://doi.org/10.1016/0262-8856(92)90066-C.

[3] Segal, A., Haehnel, D. and S. Thrun. "Generalized-ICP". Robotics: Science and Systems V, Robotics: Science and Systems Foundation,. (June 2009): 435-442. https://doi.org/10.15607/RSS.2009.V.021.

[4] Korn, Michael, Martin Holzkothen, and Josef Pauli. "Color supported generalized-ICP." In 2014 International Conference on Computer Vision Theory and Applications (VISAPP), 592–599. Lisbon, Portugal: IEEE, 2014. https://doi.org/10.5220/0004692805920599.

[5] Park, Jaesik, Qian-Yi Zhou, and Vladlen Koltun. "Colored point cloud registration revisited." In Proceedings of the IEEE International Conference on Computer Vision (ICCV), 143-152. Venice, Italy: IEEE, 2017. htttps://doi.org/10.1109/ICCV.2017.25.

Extended Capabilities

C/C++ Code Generation

Generate C and C++ code using MATLAB® Coder™.

Usage notes and limitations:

• Values to Extrapolate and Metric arguments must be compile time constants.

GPU Code Generation

Generate CUDA® code for NVIDIA® GPUs using GPU Coder™.

Usage notes and limitations:

• The "pointToPlaneWithColor" and "planeToPlaneWithColor" algorithm metrics do not support GPU code generation.

• The point cloud output locations of the GPU-generated code differ from simulation by an order of 10e-2, and the RMSE output differs by an order of 10e-4, due to precision errors. However, the simulation and GPU code generation outputs have no notable visual differences.

• To ensure similar outputs from GPU code generation and simulation, the Extrapolate argument must be false or 0.

Version History

Introduced in R2018a

R2024a: InitialTransform name-value argument default value changed for "pointToPlaneWithColor" metric

Starting in R2024a, for the "pointToPlaneWithColor" metric, the default value of the InitialTransform name-value argument is changed to a rigidtform3d object that performs an identity transformation. Prior to R2024a, the default value is a rigidtform3d object that contains the translation between the centroids of the moving and the fixed point clouds.

R2023b: Register point clouds with color information

Starting in R2023b, the pcregistericp function now supports the point cloud registration using color information. To include color information for registration, specify the Metric name-value argument as "pointToPlaneWithColor" or "planeToPlaneWithColor". The "pointToPlaneWithColor" and "planeToPlaneWithColor" algorithm metrics do not support code generation.

R2023b: Extrapolate name-value argument will be removed in a future release

The Extrapolate name-value argument of the pcregrigid function will be removed in a future release. You can increase the MaxIterations argument or use the pointToPlane or planeToPlane values for the Metric name-value argument for similar accuracy.

R2023b: MaxIterations name-value argument default value changed

The default value for the MaxIterations name-value argument was changed from 20 to 30 for pointToPoint and pointToPlane metrics.

R2023a: Returns RMSE of all points

Starting in R2023a, the pcregistericp function now returns the RMSE of all points. Previously, the function returned the RMSE of only the inliers. The function still outputs the RMSE of the inliers at each iteration when you set the Verbose argument to true.

R2023a: Default value modified for InitialTransform argument

Starting in R2023a, the pcregistericp function now uses a modified default value for the InitialTransform name-value argument. The default value is now a rigidtform3d object that contains a translation aligning the centroid of the moving point cloud to that of the fixed point cloud. Prior to R2023a, the default value is rigidtform3d().

R2022b: Returns rigidtform3d object

Starting in R2022b, most Computer Vision Toolbox™ functions create and perform geometric transformations using the premultiply convention. Accordingly, the pcregistericp function now returns the output argument tform a rigidtform3d object, which uses the premultiply convention. Before, the function returned tform as a rigid3d object, which uses the postmultiply convention.

You can now specify the InitialTransform name-value argument as a rigidtform3d object. Although you can still specify InitialTransform as a rigid3d object, this object is not recommended because it uses the postmultiply convention. You can streamline your geometric transformation workflows by switching to new premultiply geometric transformation objects. For more information, see Migrate Geometric Transformations to Premultiply Convention.

R2022b: Generalized-ICP registration algorithm

The pcregistericp function now supports the Generalized-ICP (G-ICP), or plane-to-plane, minimization metric. To use this registration algorithm, specify the Metric name-value argument as "planeToPlane". The "planeToPlane" algorithm metric does not support code generation.

R2020a: Returns a rigid3d object

Starting in R2020a, the pcregistericp function returns a rigid3d object. Prior to R2020a, the function returned an affine3d object.