Localization using 2D maps which capture vertical structures in 3D point data
View Patent ↗Autonomous vehicles require precise localization to support safe and reliable operation. Current systems aim to localize in 6DOF based on observations from a combination of cameras and 3D LiDAR, matching against dense, 3D prior maps. These maps are quite large and complex, presenting both computational and physical challenges in terms of matching, storage, and retrieval. Most of the environments where vehicles operate in contain frequent and distinct vertical structure sufficient for 2D localization, while state-of-the-art IMUs can be used to recover roll and pitch. This disclosure introduces a fast method for constructing 2D maps summarizing the vertical structure in the environment and demonstrate that it can be used to localize accurately in vehicular and other applications.
1. A method for generating a two-dimensional representation of
structures in a scene, comprising:
receiving three-dimensional point data of a scene;
partitioning ground of the scene into a plurality of cells;
projecting points from the three-dimensional point data into the plurality of cells;
recording, for each cell in the plurality of cells, heights at which the points occur within the cell, wherein recording heights at which the points occur in the cell comprises quantizing heights into elements of a column vector and thereby forming a plurality of vector columns, where each element in the column vector corresponds to a different range of heights above a reference point;
determining, for each cell in the plurality of cells, a property of the points within the cell; and
generating a two-dimensional representation of the scene from the properties determined in each cell in the plurality of cells.
2. The method of claim 1 further comprises capturing the three-dimensional point data from a LiDAR sensor.
3. The method of claim 1 wherein quantizing heights in elements of a column vector further comprises encoding presence or absence of a point in a given element as a binary number.
4. The method of claim 1 wherein a number of elements in a column vector is less than or equal to a size of an integer data type of a processing unit.
5. The method of claim 1 wherein each cell includes a height offset value that shifts the meaning of each bit in the column vector.
6. The method of claim 1 further comprises determining a property of the points in a given cell by performing a population count on the given cell.
7. The method of claim 1 further comprises determining a property of the points in a given cell through a sequence of bit-wise AND and population counts on the given cells.
8. The method of claim 1 wherein recording heights at which the point occurs further comprises receiving a new point that falls outside the range of heights defined by a given column vector and adjusting the range of heights by bit shifting elements in the given column vector based on the new point.
9. The method of claim 1 further comprises determining a property of the points in a cell by applying a rule set to the plurality of cells, where the rule set distinguishes immovable structures from other objects detected in the scene.
10. A method for determining a location of a movable object, comprising:
collecting three-dimensional point data of a scene surrounding the movable object;
transforming the three-dimensional point data to a current two-dimensional map of structures in the scene by representing the verticality of any structure detected in the scene with a column vector, where each element in the column vector corresponds to a different range of heights above a reference point;
matching the current two-dimensional map to one or more of a set of two-dimensional candidate maps, where each two-dimensional candidate map in the set of two-dimensional candidate maps includes global coordinates; and
determining the location of the movable object using one or more matched two-dimensional candidate maps.
11. The method of claim 10 wherein transforming the three-dimensional point data to a two-dimensional map includes
partitioning ground of the scene into a plurality of cells;
projecting points from the three-dimensional point data into the plurality of cells;
recording, for each cell in the plurality of cells, heights at which the points occur within the cell;
determining, for each cell in the plurality of cells, a property of the points within the cell; and
generating a two-dimensional representation of the scene from the properties determined in each cell in the plurality of cells.
12. The method of claim 11 wherein recording heights at which the points occur in the cell comprises quantizing heights into elements of a column vector and thereby forming a plurality of vector columns, where each element in the column vector corresponds to a different range of heights above a reference point.
13. The method of claim 10 wherein matching the current two-dimensional map to one or more of a set of two-dimensional candidate maps includes
estimating a current location of the movable object;
identifying the set of two-dimensional candidate maps from a plurality of predetermined two-dimensional maps using the current location of the movable object.
14. The method of claim 10 wherein matching the current two-dimensional map to one or more of a set of two-dimensional candidate maps includes aligning the current two-dimensional map with each two-dimensional candidate map in the set of two-dimensional candidate maps and thereby determine a position estimate for the current two-dimensional map in relation to each two-dimensional candidate map in the set of two-dimensional candidate maps.
15. The method of claim 14 wherein aligning the current two-dimensional map with each two-dimensional candidate map in the set of two-dimensional candidate maps includes computing a rigid transformation between the current two-dimensional map and each two-dimensional candidate map in the set of two-dimensional candidate maps.
16. The method of claim 15 further comprises filtering the set of two-dimensional candidate maps based on alignment with the current two-dimensional map and prior to the step of determining location of the movable object.
17. The method of claim 14 further comprises determining location of the movable object using particle filters.
18. The method of claim 14 wherein determining location of the movable object includes
constructing a factor graph using the position estimates for the current two-dimensional map in relation to each two-dimensional candidate map in the set of two-dimensional candidate maps, where nodes in the factor graph represent known and unknown locations of the movable object and edges in the factor graph represent observations that relate values of nodes to each other; and
solving the factor graph using a nonlinear least squares method.
19. The method of claim 10 further comprises collecting three-dimensional point data of the scene at a different location and repeating the steps to determine a location of the movable object at the different location.