Point Cloud ICP

About This Module

Point cloud ICP module provides an API to align 3D points from a pair of lidar spins via Point-Plane Iterative Closest Points implementation. Its usage is shown in ICP (Iterative Closest Points) Sample. This module assumes that only a small translation and rotation exists between the two point clouds (relative to a prior pose provided); Example of a common type of input include lidar sweeps captured less than 2 meters apart from a moving vehicle.

Terminology in the API

The two point clouds input to the the ICP module are named Target and Source point clouds respectively. The output of the alignment is the transform that must be applied to Source points so that they align to the target point cloud. As this module uses an iterative approach, a Convergence criteria is used to test if iterations need to stop. Cost of the iteration is a measure of average distance between point and their target planes (please refer to literature on ICP process).

Principles of implementation.

In the process of obtaining a temporally-fused point cloud comprising of the sweeps of lidar data captured from a moving lidar unit, one would require to compute the relative pose of the lidar unit between each sweep. This set of poses (described as transforms between successive sweeps of lidar), is such that a measure of mis-alignment is minimum between the two sweeps.

A perfectly well aligned pair of sweeps would be such that the distance between a point on a surface in source sweep lies perfectly flush on the same surface in the target sweep. In other words, sum of distances from a point (in source point cloud) to the plane of it nearest neighbor point (in target cloud) is at its minimum. Iterative Closest Point algorithm finds the transform that that sets this measure to its minimum.

The transform thus generated could be an indicator of the motion of the lidar unit, or could be used to align the point cloud to generated a map of the surrounding, among other uses.

The implemented system this document accompanies, repeatedly moves the source points towards the appropriate plane in the target plane over many iterations. The user can control how many such iterations can take plane and what tolerances of alignments are admissible.

This has been tested with lidar consisting of 64 beams, rotating at 10Hz mounted on vehicle that translates less than approx 2 meters between two spins (nominally, 2Km/h)

Restrictions on the input data

ICP Module places the following major restrictions on data input to the module:

• The number of points may not exceed 32767 points in either source or target clouds.
• There is a small rotation/translation relation between the two point clouds, e.g. a translation of no more than 2-meters converges to acceptable results.
• Lastly, the point cloud data input via a pointer is arranged in such a way that points in close proximity to one another in 3D space are nearby in the laid out memory.

Iteration Stopping Criteria

Two criteria control the stopping point of the iteration in this module and consequently the total run time and the accuracy of the results. These criteria are defined in terms of:

• Rotation (in radians) and translation (in meters) tolerances or
• Maximum number of iterations

When the difference between results of two iterative steps is smaller than the rotation/translation tolerances, the control returns to the application. The process terminates after a set number of iterations are complete, which is intended to produce a more uniform distribution of run times.

Alternatively, the application can provide a callback for the convergence test. This callback can compare two of the latest transforms and return a value that indicates whether the iterative process should conclude.

Result Statistics

ICP Module provides some basic statistics about the last iteration performed. The statistics are a quantitative indicator of the alignment between the point clouds.

typedef struct
{
    uint16_t actualNumIterations;
    float32_t rmsCost;
    float32_t inlierFraction;
    uint32_t numCorrespondences;
} dwPointCloudICPResultStats;

• rmsCost is the main indicator of the alignment, giving the rms of the distance between points in source point clouds to the planes assigned to the corresponding target points.
• inlierFraction indicates the fraction (range 0..1) of points that had a point-to-plane error less than or equal to a defined threshold (0.1m).
• numCorrespondences is the total number of point-to-plane correspondences used in in the optimization.
• actualNumIterations is the actual number of iterations used before terminating.

Workflow

The module offers a variant of ICP to be used with structured point clouds only, i.e. there is a correlation of the points to their indices. To initialize the module,

dwPointCloudICPParams params{};
dwPointCloudICPHandle_t icp;
dwPointCloudICP_getDefaultParams(&params);
params.icpType = DW_POINT_CLOUD_ICP_TYPE_DEPTH_MAP;
dwPointCloudICP_initialize(&icp, &params, context);
dwPointCloudICP_setCUDAStream(stream, icp);

In this particular example, we set params.icpType = DW_POINT_CLOUD_ICP_TYPE_DEPTH_MAP.

Warning

Please ensure the input point clouds to the ICP are organized if user selects DW_POINT_CLOUD_ICP_TYPE_DEPTH_MAP as the icp type. See Point Cloud Memory Management and Point Cloud Accumulator for point cloud memory allocation and accumulation.

dwPointCloudICP_bindInput(sourcePointCloud, targetPointCloud, initialSourceToTargetTransformation, icp);
dwPointCloudICP_bindOutput(optimizedSourceToTargetTransformation, icp);
dwPointCloudICP_process(icp);

API allows user to provide initial guess on the transformation between the source and target point clouds. If nullptr is provided, ICP will use identity transformation as initial guess.

To release the module,

dwPointCloudICP_release(icp);

For more details see Point Cloud Processing Sample