Coordinate gradient method for point cloud registration for autonomous vehicles
In one embodiment, a system and method for partitioning a region for point cloud registration of LIDAR poses of an autonomous driving vehicle (ADV) using a regional iterative closest point (ICP) algorithm is disclosed. The method determines the frame pair size of one or more pairs of related LIDAR poses of a region of an HD map to be constructed. If the frame pair size is greater than a threshold, the region is further divided into multiple clusters. The method may perform the ICP algorithm for each cluster. Inside a cluster, the ICP algorithm focuses on a partial subset of the decision variables and assumes the rest of the decision variables are fixed. To construct the HD map, the method may determine if the results of the ICP algorithms from the clusters converge. If the solutions converge, a solution to the point cloud registration for the region is found.
1. A computer-implemented method to register point clouds for an autonomous driving vehicles (ADV), the method comprising:
determining a frame pair size of one or more pairs of related poses of a plurality of poses of a plurality of point clouds of a region to be mapped;
separating the region containing the plurality of point clouds and the corresponding poses into a plurality of clusters in response to the frame pair size being greater than a threshold;
applying separately a first optimization model to the plurality of point clouds corresponding to the plurality of clusters to register the point clouds;
merging the registered point clouds from the plurality of clusters to perceive a driving environment of the region surrounding the ADV; and
repeatedly separating the registered point clouds from the plurality of clusters into an additional plurality of clusters and applying the first optimization model to the plurality of point clouds corresponding to the additional plurality of clusters until the registered point clouds converge.
2. The method of claim 1 , further comprising applying a second optimization model to the plurality of points clouds of the region in response to the frame pair size being determined to be smaller or equal to the threshold.
3. The method of claim 1 , wherein the frame pair size of a related pair of the plurality of poses of the region comprises a geometric distance between the related pair.
4. The method of claim 1 , wherein applying separately a first optimization model to the plurality of point clouds corresponding to the plurality of clusters comprises solving a bundle adjustment equation of an iterative closest point (ICP) model of one of the plurality of clusters by focusing on a partial subset of a plurality of decision variables in the one cluster and fixing the decision variables other than the partial subset.
5. The method of claim 4 , wherein solving a bundle adjustment equation of an ICP model of one of the plurality of clusters by focusing on a partial subset of a plurality of decision variables in the one cluster and fixing the decision variables other than the partial subset comprises:
determining if one pose of a pair of poses of the plurality of poses in the one cluster is outside of the one cluster; and
treating the one pose of the pair of poses as a constant when solving the bundle adjustment equation of the one cluster in response to the one pose of the pair of poses being determined to be outside of the one cluster.
6. The method of claim 1 , wherein repeatedly separating the registered point clouds from the plurality of clusters into an additional plurality of clusters and applying the first optimization model to the plurality of point clouds corresponding to the additional plurality of clusters until the registered point clouds converge comprises:
determining if the registered point clouds from the plurality of clusters converge; and
iteratively separating the region containing the registered point clouds from the plurality of clusters into a second plurality of clusters in response to the registered point clouds from the plurality of clusters are determined not to converge, and applying separately the first optimization model to the plurality of point clouds corresponding to the second plurality of clusters to register the point clouds until the registered point clouds from the second plurality of clusters converge.
7. The method of claim 1 , wherein merging the registered point clouds from the plurality of clusters comprises merging using a pose graph algorithm.
8. The method of claim 1 , wherein the sensor comprises a light detection and range (LIDAR) sensor.
9. A non-transitory machine-readable medium having instructions stored therein, which when executed by a processor, cause the processor to perform operations to register points clouds for an autonomous driving vehicles (ADV), the operations comprising:
determining a frame pair size of one or more pairs of related poses of a plurality of poses of a plurality of point clouds of a region to be mapped;
separating the region containing the plurality of point clouds and the corresponding poses into a plurality of clusters in response to the frame pair size being greater than a threshold;
applying separately a first optimization model to the plurality of point clouds corresponding to the plurality of clusters to register the point clouds;
merging the registered point clouds from the plurality of clusters to perceive a driving environment of the region surrounding the ADV; and
repeatedly separating the registered point clouds from the plurality of clusters into an additional plurality of clusters and applying the first optimization model to the plurality of point clouds corresponding to the additional plurality of clusters until the registered point clouds converge.
10. The non-transitory machine-readable medium of claim 9 , further comprising applying a second optimization model to the plurality of points clouds of the region in response to the frame pair size being determined to be smaller or equal to the threshold.
11. The non-transitory machine-readable medium of claim 9 , wherein the frame pair size of a related pair of the plurality of poses of the region comprises a geometric distance between the related pair.
12. The non-transitory machine-readable medium of claim 9 , wherein applying separately a first optimization model to the plurality of point clouds corresponding to the plurality of clusters comprises solving a bundle adjustment equation of an iterative closest point (ICP) model of one of the plurality of clusters by focusing on a partial subset of a plurality of decision variables in the one cluster and fixing the decision variables other than the partial subset.
13. The non-transitory machine-readable medium of claim 12 , wherein solving a bundle adjustment equation of an ICP model of one of the plurality of clusters by focusing on a partial subset of a plurality of decision variables in the one cluster and fixing the decision variables other than the partial subset comprises:
determining if one pose of a pair of poses of the plurality of poses in the one cluster is outside of the one cluster; and
treating the one pose of the pair of poses as a constant when solving the bundle adjustment equation of the one cluster in response to the one pose of the pair of poses being determined to be outside of the one cluster.
14. The non-transitory machine-readable medium of claim 9 , wherein repeatedly separating the registered point clouds from the plurality of clusters into an additional plurality of clusters and applying the first optimization model to the plurality of point clouds corresponding to the additional plurality of clusters until the registered point clouds converge comprises:
determining if the registered point clouds from the plurality of clusters converge; and
iteratively separating the region containing the registered point clouds from the plurality of clusters into a second plurality of clusters in response to the registered point clouds from the plurality of clusters are determined not to converge, and applying separately the first optimization model to the plurality of point clouds corresponding to the second plurality of clusters to register the point clouds until the registered point clouds from the second plurality of clusters converge.
15. A data processing system, comprising:
a processor; and
a memory coupled to the processor to store instructions, which when executed by the processor, cause the processor to perform operations to register point clouds for an autonomous driving vehicles (ADV), the operations including:
determining a frame pair size of one or more pairs of related poses of a plurality of poses of a plurality of point clouds of a region to be mapped,
separating the region containing the plurality of point clouds and the corresponding poses into a plurality of clusters in response to the frame pair size being greater than a threshold,
applying separately a first optimization model to the plurality of point clouds corresponding to the plurality of clusters to register the point clouds,
merging the registered point clouds from the plurality of clusters to perceive a driving environment of the region surrounding the ADV, and
repeatedly separating the registered point clouds from the plurality of clusters into an additional plurality of clusters and applying the first optimization model to the plurality of point clouds corresponding to the additional plurality of clusters until the registered point clouds converge.
16. The data processing system of claim 15 , wherein the operations further comprise applying a second optimization model to the plurality of points clouds of the region in response to the frame pair size being determined to be smaller or equal to the threshold.
17. The data processing system of claim 15 , wherein the frame pair size of a related pair of the plurality of poses of the region comprises a geometric distance between the related pair.
18. The data processing system of claim 15 , wherein the operations for applying separately a first optimization model to the plurality of point clouds corresponding to the plurality of clusters comprises operations for solving a bundle adjustment equation of an iterative closest point (ICP) model of one of the plurality of clusters by focusing on a partial subset of a plurality of decision variables in the one cluster and fixing the decision variables other than the partial subset.
19. The data processing system of claim 18 , wherein the operations for solving a bundle adjustment equation of an iterative closest point (ICP) model of one of the plurality of clusters by focusing on a partial subset of a plurality of decision variables in the one cluster and fixing the decision variables other than the partial subset comprises operations for:
determining if one pose of a pair of poses of the plurality of poses in the one cluster is outside of the one cluster; and
treating the one pose of the pair of poses as a constant when solving the bundle adjustment equation of the one cluster in response to the one pose of the pair of poses being determined to be outside of the one cluster.
20. The data processing system of claim 15 , wherein the operations for repeatedly separating the registered point clouds from the plurality of clusters into an additional plurality of clusters and applying the first optimization model to the plurality of point clouds corresponding to the additional plurality of clusters until the registered point clouds converge comprise:
determining if the registered point clouds from the plurality of clusters converge; and
iteratively separating the region containing the registered point clouds from the plurality of clusters into a second plurality of clusters in response to the registered point clouds from the plurality of clusters are determined not to converge, and applying separately the first optimization model to the plurality of point clouds corresponding to the second plurality of clusters to register the point clouds until the registered point clouds from the second plurality of clusters converge.