Ground intensity LIDAR localizer
A system for determining a pose of a vehicle and building maps from vehicle priors processes received ground intensity LIDAR data including intensity data for points believed to be on the ground and height information to form ground intensity LIDAR (GIL) images including pixels in 2D coordinates where each pixel contains an intensity value, a height value, and x- and y-gradients of intensity and height. The GIL images are formed by filtering aggregated ground intensity LIDAR data falling into a same spatial bin on the ground and using a registration algorithm to align two GIL images relative to one another by estimating a 6-degree-of-freedom pose with associated uncertainty that minimizes error between the two GIL images. The aligned GIL images are provided as a pose estimate to a localizer. The system may provide online localization and pose estimation, prior building, and prior to prior alignment pose estimation using image-based techniques.
1. A system for determining a pose of a vehicle, comprising:
at least one processor; and
a non-transitory, machine-readable medium comprising instructions thereon that, when executed by the at least one processor, cause the at least one processor to perform operations comprising:
receiving ground intensity LIDAR data describing a first ground point, the ground intensity LIDAR data comprising first intensity data describing the first ground point and height data describing the first ground point;
forming a first Ground Intensity LIDAR (GIL) image comprising a plurality of pixels arranged on a two-dimensional grid, a first pixel of the plurality of pixels comprising: a height value for the first ground point and an intensity value for the first ground point;
estimating a 6-degree-of-freedom pose that minimizes an error between the first GIL image and an image prior; and
returning the 6-degree-of-freedom pose as a pose estimate for the vehicle.
2. The system of claim 1 , further comprising receiving an initial estimate of vehicle pose parameters, the estimating of the 6-degree-of-freedom pose being based at least in part on the initial estimate of vehicle pose parameters.
3. The system of claim 1 , wherein the estimating of the 6-degree-of-freedom pose comprises aligning the first GIL image and the image prior using at least one of a Gauss-Newton framework or a Levenberg-Marquardt framework for solving a nonlinear optimization problem that generates the 6-degree-of-freedom pose.
4. The system of claim 1 , wherein receiving the ground intensity LIDAR data comprises:
receiving a 3D point cloud including intensity values and uncertainty estimates;
receiving a pose initialization estimate; and
combining the 3D point cloud and pose initialization estimate.
5. The system of claim 4 , the 3D point cloud comprising a plurality of LIDAR points, the operations further comprising:
transforming the plurality of LIDAR points with the pose initialization estimate to bring the plurality of LIDAR points into a consistent coordinate system; and
storing the transformed plurality of LIDAR points in a rolling buffer that accumulates LIDAR points over a period of time.
6. The system of claim 5 , the operations further comprising applying a spatial filter to the plurality of LIDAR points to generate a set of filtered LIDAR points, the spatial filter configured to remove LIDAR points outside a radius from the vehicle using a range estimate of each respective lidar point.
7. The system of claim 6 , the operations further comprising:
determining, from a ground plane, a plurality of partially overlapping cells having a specified size;
selecting, from the set of filtered LIDAR points, a set of candidate ground points associated with a first cell of the plurality of partially overlapping cells, the set of candidate ground points being selected based at least in part on height estimates for the set of filtered LIDAR points and an expected ground height within a specified LIDAR range;
determining that a first candidate ground point of the set of candidate ground points belongs to the ground plane, the determining based at least in part on a plane fitting using the first candidate ground point and the ground plane; and
assigning the first candidate ground point to an XY cell grid position in the ground plane.
8. The system of claim 7 , further comprising determining that the first candidate ground point fits into the ground plane with a normal of the first candidate ground point pointing predominantly along a z-axis when the first candidate ground point satisfies a minimum number of inliers per cell.
9. The system of claim 7 , the estimating of the 6-degree-of-freedom pose comprising:
aligning the first GIL image and the image prior at least in part by registering the first candidate ground point with a 2.5D image prior using a registration algorithm comprising at least one of a Gauss-Newton framework or a Levenberg-Marquardt framework to generate the 6-degree-of-freedom pose, the 2.5D image prior comprising pixels that encode height values, the height values comprising the height value for the first ground point.
10. The system of claim 1 , the estimating of the 6-degree-of-freedom pose comprising aligning the first GIL image and the image prior in scale-space.
11. The system of claim 1 , the operations further comprising aligning the first GIL image and the image prior.
12. The system of claim 11 , the first pixel of the plurality of pixels further comprising a height gradient value for the first ground point, the operations further comprising before aligning the first GIL image and the image prior, removing from the first GIL image at least one pixel of the plurality of pixels having a vanishing height gradient.
13. The system of claim 11 , the aligning the first GIL image and the image prior being based at least in part on an Inverse Compositional alignment of the first GIL image and the image prior.
14. A method for determining a pose of a vehicle, the method comprising:
receiving ground intensity LIDAR data describing a first ground point, the ground intensity LIDAR data comprising first intensity data describing the first ground point and height data describing the first ground point;
forming a first Ground Intensity LIDAR (GIL) image comprising a plurality of pixels arranged on a two-dimensional grid, a first pixel of the plurality of pixels comprising: a height value for the first ground point and an intensity value for the first ground point;
estimating a 6-degree-of-freedom pose that minimizes an error between the first GIL image and an image prior; and
returning the 6-degree-of-freedom pose as a pose estimate for the vehicle.
15. The method of claim 14 , further comprising receiving an initial estimate of vehicle pose parameters, the estimating of the 6-degree-of-freedom pose being based at least in part on the initial estimate of vehicle pose parameters.
16. The method of claim 14 , wherein the estimating of the 6-degree-of-freedom pose comprises aligning the first GIL image and the image prior using at least one of a Gauss-Newton framework or a Levenberg-Marquardt framework for solving a nonlinear optimization problem that generates the 6-degree-of-freedom pose.
17. The method of claim 14 , wherein receiving the ground intensity LIDAR data comprises:
receiving a 3D point cloud including intensity values and uncertainty estimates;
receiving a pose initialization estimate; and
combining the 3D point cloud and pose initialization estimate.
18. The method of claim 17 , the 3D point cloud comprising a plurality of LIDAR points, further comprising:
transforming the plurality of LIDAR points with the pose initialization estimate to bring the plurality of LIDAR points into a consistent coordinate system; and
storing the transformed LIDAR points in a rolling buffer that accumulates LIDAR points over a period of time.
19. The method of claim 18 , further comprising applying a spatial filter to the plurality of LIDAR points to generate a set of filtered LIDAR points, the spatial filter configured to remove LIDAR points outside a radius from the vehicle using a range estimate of each respective lidar point.
20. A non-transitory computer-readable medium comprising instructions thereon that when executed by at least one processor, because the at least one processor to perform operations comprising:
receiving ground intensity LIDAR data describing a first ground point, the ground intensity LIDAR data comprising first intensity data describing the first ground point and height data describing the first ground point;
forming a first Ground Intensity LIDAR (GIL) image comprising a plurality of pixels arranged on a two-dimensional grid, a first pixel of the plurality of pixels comprising: a height value for the first ground point and an intensity value for the first ground point;
estimating a 6-degree-of-freedom pose that minimizes an error between the first GIL image and an image prior; and
returning the 6-degree-of-freedom pose as a pose estimate for a vehicle.