watonomous.github.io
[ Software Division : HD Map Lookup ]
Created by [ Ankil Patel], last modified by [ Henry Wang] on Feb 13, 2020
The objective of this concept is to provide fast lookups of relevant local static environment objects described in HD Maps, given the ego pose (and possibly the uncertainty thereof). In particular the objects of interest are nearby lanes that the vehicle is on or may route to.
The HD Map lookup process can be broken down into a stack or pipeline of 3 layers.
- Query Handler
- Query Implementation
- AST Transformation
The HD Map first parsed into an AST, represented in program memory. This initial AST is then transformed into another format that's more amenable to querying, for example a rectangle tree (rtree). The query implementation contains the code to actually perform the query, for example traversing the rtree. The query handler is the interface that allows other programs or external processes to make queries.
There are talks of implementing a next pointer for each lane that would allow you to query lanelets in O(1) time instead of O(log(n)) time. That implementation needs to be hidden behind the query handler.
[Query Implementation]
The query implementation uses a rtree internally.
\
The following document informed the rtree implementation used: https://blog.mapbox.com/a-dive-into-spatial-search-algorithms-ebd0c5e39d2a?gi=6fded0f9d4dc.
From the document, it's clear that another data structure could have also been used: kd-trees. While easier to implement and relatively fast, the kd-tree does not handle adding and removing points (without recomputing the tree). Moreover, it also doesn't handle range searches.
A rtree is a self-balancing 2d-(it could be more dimensions) tree that supports range queries. The objects of interest are placed in the leaves of the tree. The root represents a range that encompasses all the objects in the tree. As we move from one level to the next, the range represented by each non-leaf node encompasses all the ranges of its children (and so on). Since the tree is self-balancing, each lookup is guaranteed to occur in O(log(n)).
[
{.confluence-embedded-image
height=”250”}]{.confluence-embedded-file-wrapper
.confluence-embedded-manual-size}
We traverse the rtree with a given pose:
Pose -> x,y, optional uncertainty (assumed to be unit distance if not given)
Traversals start at the root. We determine whether the ranges of the children intersect with the pose range/point. We enter the children and repeat the process until we get to the leaf node.
We retrieve all the lane objects that intersect with the pose range given. Since there might be lane objects returned that might not be relevant, further filtering would be required (e.g. filtering for the direction of the lane (although you can encode that into the tree with negative numbers as well)).
The naive implementation involves going through each lane object and determining which lane object is closest to the given pose. There are several issues with this implementation:
- The closest lane object doesn't always refer to the car's actual
lane object
- You would need to use convex hulls to determine which lane segment was closer to your pose: a process that takes O(n log(n)) time
- It's a linear search - this could be problematic for large numbers of nodes ( I hear this might be a year 4 issue)
\
\
Attachments:
rangetree.png (image/png)\
Document generated by Confluence on Dec 10, 2021 04:02