Fast Point Cloud Registration for Bootstrapping Localization in a Map

1 . A method, comprising:

deriving, by one or more computing devices, a query point cloud from a sweep of a light detection and ranging (lidar) sensor device of a vehicle and a reference point cloud from a high-definition (HD) map;

extracting, by the one or more computing devices, a first set of features from the query point cloud and a second set of features from the reference point cloud

determining, by the one or more computing devices, a coarse alignment based on a plurality of matches between the first set of features from the query point cloud and the second set of features from the reference point cloud; and

estimating, by the one or more computing devices, a position-orientation pose of the vehicle by refining the coarse alignment using an iterative closest point (ICP) algorithm.

2 . The method of claim 1 , wherein the deriving the reference point cloud from the HD map further comprises:

selecting, by the one or more computing devices and based on geolocation data obtained from one or more geolocation sensor devices of the vehicle at commencement of an activation sequence of the vehicle, a map tile comprising a predefined geographical area that includes an initial location of the vehicle from a plurality of map tiles in the HD map; and

obtaining, by the one or more computing devices, the reference point cloud from the selected map tile in the HD map.

3 . The method of claim 1 , further comprising:

downsampling, by the one or more computing devices, voxels in the query point cloud and the reference point cloud; and

deriving, by the one or more computing devices, a surface normal for each of the query point cloud and the reference point cloud.

4 . The method of claim 1 , wherein the extracting the first set of features from the query point cloud and the second set of features from the reference point cloud further comprises:

deriving, by the one or more computing devices, at least one fast point feature histogram (FPFH) descriptor for every point in the query point cloud and the reference point cloud.

5 . The method of claim 1 , further comprising:

generating, by the one or more computing devices, the plurality of matches between the first set of features from the query point cloud and the second set of features from the reference point cloud by pairing each point in the query point cloud with a closest neighboring point in the reference point cloud.

6 . The method of claim 1 , wherein the determining the coarse alignment further comprises:

generating, by the one or more computing devices, a plurality of solutions by applying a predetermined number of distinct graduated non-convexity (GNC) optimizations with uniformly sampled initial vehicle headings to the plurality of matches between the first set of features from the query point cloud and the second set of features from the reference point cloud;

determining, by the one or more computing devices, a number of inlier feature matches for each solution in the plurality of solutions; and

identifying, by the one or more computing devices, the solution with the highest number of inlier feature matches as the coarse alignment.

7 . The method of claim 6 , wherein each inlier feature match comprises a feature correspondence pair having a first point in the query point cloud and a second point in the reference point cloud that are within a predetermined distance threshold of each other.

8 . A non-transitory computer readable medium storing instructions that, when executed by one or more processors, causes the one or more processors to perform operations comprising:

deriving a query point cloud from a sweep of a light detection and ranging (lidar) sensor device of a vehicle and a reference point cloud from a high-definition (HD) map;

extracting a first set of features from the query point cloud and a second set of features from the reference point cloud;

determining a coarse alignment based on a plurality of matches between the first set of features from the query point cloud and the second set of features from the reference point cloud; and

estimating a position-orientation pose of the vehicle by refining the coarse alignment using an iterative closest point (ICP) algorithm.

9 . The non-transitory computer readable medium of claim 8 , wherein the operations further comprise:

selecting, based on geolocation data obtained from one or more geolocation sensor devices of the vehicle at commencement of an activation sequence of the vehicle, a map tile comprising a predefined geographical area that includes an initial location of the vehicle from a plurality of map tiles in the HD map; and

obtaining the reference point cloud from the selected map tile in the HD map.

10 . The non-transitory computer-readable medium of claim 8 , wherein the operations further comprise:

downsampling voxels in the query point cloud and the reference point cloud; and

deriving a surface normal for each of the query point cloud and the reference point cloud.

11 . The non-transitory computer-readable medium of claim 8 , wherein the extracting the first set of features from the query point cloud and the second set of features from the reference point cloud further comprises:

deriving at least one fast point feature histogram (FPFH) descriptor for every point in the query point cloud and the reference point cloud.

12 . The non-transitory computer-readable medium of claim 8 , wherein the operations further comprise:

generating the plurality of matches between the first set of features from the query point cloud and the second set of features from the reference point cloud by pairing each point in the query point cloud with a closest neighboring point in the reference point cloud.

13 . The non-transitory computer-readable medium of claim 8 , wherein the determining the coarse alignment further comprises:

generating a plurality of solutions by applying a predetermined number of distinct graduated non-convexity (GNC) optimizations with uniformly sampled initial vehicle headings to the plurality of matches between the first set of features from the query point cloud and the second set of features from the reference point cloud;

determining a number of inlier feature matches for each solution in the plurality of solutions; and

identifying the solution with the highest number of inlier feature matches as the coarse alignment.

14 . The non-transitory computer-readable medium of claim 13 , wherein each inlier feature match comprises a feature correspondence pair having a first point in the query point cloud and a second point in the reference point cloud that are within a predetermined distance threshold of each other.

15 . A system, comprising:

one or more processors; and

a memory communicatively coupled to the one or more processors, wherein the memory stores instructions that, when executed by the one or more processors, cause the one or more processors to perform operations comprising:

deriving a query point cloud from a sweep of a light detection and ranging (lidar) sensor device of a vehicle and a reference point cloud from a high-definition (HD) map;

extracting a first set of features from the query point cloud and a second set of features from the reference point cloud

determining a coarse alignment based on a plurality of matches between the first set of features from the query point cloud and the second set of features from the reference point cloud; and

estimating a position-orientation pose of the vehicle by refining the coarse alignment using an iterative closest point (ICP) algorithm.

16 . The system of claim 15 , wherein the operations further comprise:

selecting, based on geolocation data obtained from one or more geolocation sensor devices of the vehicle at commencement of an activation sequence of the vehicle, a map tile comprising a predefined geographical area that includes an initial location of the vehicle from a plurality of map tiles in the HD map; and

obtaining the reference point cloud from the selected map tile in the HD map.

17 . The system of claim 15 , wherein the operations further comprise:

downsampling voxels in the query point cloud and the reference point cloud; and

deriving a surface normal for each of the query point cloud and the reference point cloud.

18 . The system of claim 15 , wherein the extracting the first set of features from the query point cloud and the second set of features from the reference point cloud further comprises:

deriving at least one fast point feature histogram (FPFH) descriptor for every point in the query point cloud and the reference point cloud.

19 . The system of claim 15 , wherein the operations further comprise:

generating the plurality of matches between the first set of features from the query point cloud and the second set of features from the reference point cloud by pairing each point in the query point cloud with a closest neighboring point in the reference point cloud.

20 . The system of claim 15 , wherein the determining the coarse alignment further comprises:

generating a plurality of solutions by applying a predetermined number of distinct graduated non-convexity (GNC) optimizations with uniformly sampled initial vehicle headings to the plurality of matches between the first set of features from the query point cloud and the second set of features from the reference point cloud;

determining a number of inlier feature matches for each solution in the plurality of solutions, wherein each inlier feature match comprises a feature correspondence pair having a first point in the query point cloud and a second point in the reference point cloud that are within a predetermined distance threshold of each other; and

identifying the solution with the highest number of inlier feature matches as the coarse alignment.