Point Cloud Registration and Mapping Overview

A point cloud is a set of points in 3-D space. Point clouds are typically obtained from 3-D scanners, such as a lidar or Kinect^® device. They have applications in robot navigation and perception, depth estimation, stereo vision, visual registration, and advanced driver assistance systems (ADAS).

Point cloud registration is the process of aligning two or more 3-D point clouds of the same scene into a common coordinate system. Mapping is the process of building a map of the environment around a robot or a sensor. Registration and mapping can be used to reconstruct a 3-D scene or build a map of a roadway for localization. While registration is commonly followed by mapping, there are other applications using registration, such as deformable motion tracking, which may not require mapping. Computer Vision Toolbox™ algorithms provide functions for performing point cloud registration and mapping. The workflow consists of preprocessing, registration, drift correction, and alignment of point clouds.

Registration and Mapping Workflow

Follow these steps to perform point cloud registration and mapping on a sequence of point clouds.

Preprocess Point Clouds — To prepare the point clouds for registration, downsample them and remove unwanted features and noise.
Register Point Clouds — Register each point cloud against the one preceding it. These registrations are used in Odometry, which is the process of accumulating a registration estimate over successive frames. Using odometry alone can lead to drift between the measured and ground truth poses.
Detect Loops — To minimize drift, you must identify the return of the sensor to a previously visited location, forming a loop in the trajectory of the sensor. This is referred to as loop closure detection.
Correct Drift — Use the detected loops to minimize drift through pose graph optimization, which consists of incrementally building a pose graph by adding nodes and edges, and then optimizing the pose graph once sufficient loops are found. The result of pose graph optimization is a set of optimized absolute poses.
Assemble Map — Assemble a map by aligning the registered point clouds using their optimized absolute poses.

Manage Point Cloud Registration and Mapping Data

Use these objects to manage data associated with the point cloud registration and mapping workflow:

pointCloud object — The point cloud object stores a set of points located in 3-D space. It uses efficient indexing strategies to accomplish nearest neighbor searches, which are leveraged by point cloud preprocessing and registration functions.
rigid3d object — The rigid 3-D object stores a 3-D rigid geometric transformation. In this workflow, it represents the relative and absolute poses.
pcviewset object — The point cloud view set object manages the data associated with the odometry and mapping process. It organizes data as a set of views and pairwise connections between views. It also builds and updates a pose graph.
- Each view consists of a point cloud and the associated absolute pose transformation. Each view has a unique identifier within the view set and forms a node of the pose graph.
- Each connection stores information that links one view to another view. This includes the relative transformation between the connected views and the uncertainty involved in computing the measurement. Each connection forms an edge in the pose graph.

Preprocess Point Clouds

Preprocessing includes removing unwanted features and noise from the point clouds, as well segmenting or downsampling them. Preprocessing can include these functions:

pcsegdist or segmentLidarData — Segment the point cloud data into clusters, then use the select function to select the desired points.
pcfitplane or segmentGroundFromLidarData — Segment the ground plane, then use the select function to select the desired points.
pcdenoise — Remove unwanted noise from the point cloud.

Register Point Clouds

You can use the pcregistericp, pcregistercpd, pcregisterndt, or pcregistercorr function to register a moving point cloud to a fixed point cloud. The registration algorithms used by these functions are based on the iterative closest point (ICP) algorithm, the coherent point drift (CPD) algorithm, the normal-distributions transform (NDT) algorithm, and a phase correlation algorithm, respectively. For more information on these algorithms, see References.

When registering a point clouds, choose the type of transformation that represents how objects in the scene change between them.

Transformation	Description
Rigid	The rigid transformation preserves the shape and size of objects in the scene. Objects in the scene can undergo translations, rotations, or both. The same transformation applies to all points.
Affine	The affine transformation allows the objects to shear and change scale in addition to translations and rotations.
Nonrigid	The nonrigid transformation allows the shape of objects in the scene to change. Points undergo distinct transformations. A displacement field represents the transformation.

This table compares the point cloud registration function options, their transformation types, and their performance characteristics. Use this table to help you select the appropriate registration function for your use case.

Registration Method (function)	Transformation Type	Description	Performance Characteristics
`pcregisterndt`	Rigid	Local registration method that relies on an initial transform estimate Robust to outliers Better with point clouds of differing resolutions and densities	Fast registration method, but generally slower than ICP
`pcregistericp`	Rigid	Local registration method that relies on an initial transform estimate	Fastest registration method
`pcregistercpd`	Rigid, affine, and nonrigid	Global method that does not rely on an initial transformation estimate	Slowest registration method
`pcregistercorr`	Rigid	Registration method that relies on an occupancy grid, assigning probability values to the grid based on the Z-coordinate values of points within each grid cell.	Best suited for ground vehicle navigation Increasing the size of the occupancy grids increases the computational requirements of the function.

Registering the current (moving) point cloud against the previous (fixed) point cloud returns a rigid3d transformation that represents the estimated relative pose of the moving point cloud in the frame of the fixed point cloud. Composing this relative pose transformation with all previously accumulated relative pose transformations gives an estimate of the absolute pose transformation.

Add the view formed by the moving point cloud and its absolute pose transformation. You can add the view to the pcviewset object using the addView function.

Add the odometry edge, an edge defined by the connection between successive views, formed by the relative pose transformation between the fixed and moving point clouds to the pcviewset object using the addConnection function.

Detect Loops

Using odometry alone leads to drift due to an accumulation of errors. These errors can result in severe inaccuracies over long distances. Using graph-based simultaneous localization and mapping (SLAM) corrects the drift. To do this, detect loop closures by finding a location visited in a previous point cloud using descriptor matching. Close the loop to correct the drift. Follow these steps for loop detection and closure:

Use the scanContextDescriptor function to extract scan context descriptors, which capture the distinctiveness of a view, from two point clouds in the view set.
Use the scanContextDistance function to compute the descriptor distance between the two scan context descriptors. If the distance between two descriptors is below a specified threshold, then it is a potential loop closure.
Register the point clouds to determine the relative pose transformation between the views and the root mean square error (RMSE) of the Euclidean distance between the aligned point clouds. Use the RMSE to filter invalid loop closures. The relative pose transformation represents a connection between the two views. An edge formed by a connection between nonsuccessive views is called a loop closure edge. You can add the connection to the pcviewset object using the addConnection function.

Correct Drift

The pcviewset object internally updates the pose graph as views and connections are added. To minimize drift, perform pose graph optimization by using the optimizePoses function, once sufficient loop closure. The optimizePoses function returns a pcviewset object with the optimized absolute pose transformations for each view.

You can use the createPoseGraph function to return the pose graph as a MATLAB^® digraph object. You can use graph algorithms in MATLAB to inspect, view, or modify the pose graph. Use the optimizePoseGraph (Navigation Toolbox) function from the Navigation Toolbox™ to optimize the modified pose graph, and then use the updateView function to update the poses in the view set.

Assemble Map

Use the pcalign function to build a point cloud map using the point clouds from the view set and their optimized absolute pose transformations.

Tips

Local registration methods, such as those that use NDT or ICP (pcregisterndt or pcregistericp, respectively), require initial estimates. To obtain an initial estimate, use another sensor such as an inertial measurement unit (IMU) or other forms of odometry. Improving the initial estimate helps the registration algorithm converge faster.
Specifying the higher values for the 'MaxIterations' argument or lower values for the 'Tolerance' property for more accurate registration results, but slower registration speeds.

References

[1] Myronenko, Andriy and Xubo Song. “Point Set Registration: Coherent Point Drift.” IEEE Transactions on Pattern Analysis and Machine Intelligence 32, no. 12 (December 2010): 2262–75.

[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–55.

[3] 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–56.

[4] Biber, P., and W. Strasser. “The Normal Distributions Transform: A New Approach to Laser Scan Matching.” In Proceedings 2003 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS 2003) (Cat. No.03CH37453), 3:2743–48. Las Vegas, Nevada, USA: IEEE, 2003.

[5] Magnusson, Martin. “The Three-Dimensional Normal-Distributions Transform: An Efficient Representation for Registration, Surface Analysis, and Loop Detection.” Örebro University, 2009.

[6] Dimitrievski, Martin, David Van Hamme, Peter Veelaert, and Wilfried Philips. “Robust Matching of Occupancy Maps for Odometry in Autonomous Vehicles:” In Proceedings of the 11th Joint Conference on Computer Vision, Imaging and Computer Graphics Theory and Applications, 626–33. Rome, Italy: SCITEPRESS - Science and and Technology Publications, 2016.

Documentation