From Coursera, State Estimation and Localization for Self-Driving Cars by University of Toronto

https://www.coursera.org/learn/motion-planning-self-driving-cars

## Mapping for Planning

two environmental maps: the occupancy grid map and the high-definition road map.

### Occupancy Grids

- discretized fine grain grid which surrounds the current ego vehicle position.
- can be 2D or 3D

- Each grid square of the occupancy grid indicates if a static or stationary object is present in that grid location. If so, that grid location is classified as occupied.
- Trees and buildings
- Curbs and other non drivable surfaces: lawns or sidewalks.

- Each cell is a binary value denoted by $m^i \in {0,1}$, where 1 indicates that the square is occupied by a static object, and 0 indicates that it is not.
- Assumption of Occupancy Grid
- Static environment: all dynamic objects or moving objects must be removed from the sensor data before it is used for occupancy grid mapping
- Independence of each cell: to simplify the update functions needed to create the occupancy grid
- Known vehicle state at each time step

- LIDAR sensor is used most frequently
- Several components of the LIDAR data need to be filtered out before this data can be used to construct an occupancy grid.
- filter all LIDAR points which comprise the ground plane. In this case, the ground plane is the road surface which the autonomous car can safely drive on.
- filter all points which appear above the highest point of the vehicle: they can be ignored as they will not impede the progression of the autonomous vehicle.
- filter all non-static objects which had been captured by the LIDAR. This includes all vehicles, pedestrians, bicycles, and animals.
- Once all filtering of the LIDAR data is complete, the 3D LIDAR data will need to be projected down to a 2D plane to be used to construct our occupancy grid.

- To handle the environmental and sensor noise, the occupancy grid will be modified to be probabilistic.
- the occupancy grid can now be represented as a belief map: $bel_t(m^i) = p(m^i|(y,x))$
- $m^i$ represents a single square of the occupancy grid, where i can be constructed from measurements y, and the vehicle location x.
- $m^i$ is equal to the probability that the current cell $m^i$ is occupied given the sensor measurements gathered for that cell location.
- Threshold of certainty will be used to establish occupancy to get the binary map

- Bayesian Update of the Occupancy Grid
- To improve robustness multiple timesteps are used to produce the current map: $bel_t(m^i) = p(m^i|(y,x)_{1:t})$
- we can update beliefs in a recursive manner so that at each time step t, we use all prior information from time one onwards to define our belief.
- Bayes’theorem is applied for at each update step for each cell: $bel_t(m^i) = \eta p(y_t|m^i) bel_{t-1}(m^i)$
- The distribution $p(y_t|m^i)$, is the probability of getting a particular measurement given a cell $m^i$ is occupied. This is known as the measurement model
- The belief at time t-1 $bel_{t-1}(m^i)$ corresponds to the prior belief stored in our occupancy grid from the previous time step.
- rely on the Markov assumption, that all necessary information for estimating cell occupancy is captured in the belief map at each time step. So no earlier history needs to be considered in the cell update equations.
- $\eta$ in this case corresponds to a normalizing constant applied to the belief map.

### Populating Occupancy Grids from LIDAR Scan Data (Part 1)

**lssue With Standard Bayesian Update**

- Multiplication of numbers close to zero is hard for computers: lead to significant rounding error when multiplying small numbers, which in turn can lead to instability in the estimate of the probabilities.
- The multiplication of probabilities turns out to be an inefficient way to perform the belief update.
- Solution: Instead of storing the belief map with the values ranging from 0-1, we can convert the beliefs into log odds probabilities using the logit function: $log(\frac{p}{1-p})$
- This leads to cell values ranging from $-/infty$ to $+/infty$ avoiding the issue with numbers close to zero.

**Bayesian Log Odds Single Cell Update Derivation**

- Applying Bayes’rule: $$p(m^i|y_{1:t})$$
- where $m^i$ is the current occupancy grid map square at location $i$ and $y_{1:t}$ are the sensor measurements of that cell from time one to time t.

- The full Bayesian update for incorporating the latest measurements into our occupancy belief: $$p(m^i|y_{1:t}) = \frac{p(y_t|y_{1:t-1},m^i)p(m^i|y_{1:t-1}}{p(y_t|y_{1:t-1})} $$
- Next applying the Markov assumption: $$p(m^i|y_{1:t}) = \frac{p(y_t|m^i)p(m^i|y_{1:t-1})}{p(y_t|y_{1:t-1})}$$
- this ensures the current measurement is independent of previous measurements if the map state $m^i$ is known.

- Next is to expand the measurement model $p(y_t|m^i)$ by the application of Bayes’ rule once again $$p(y_t|m^i) = \frac{p(m^i|y_t)p(y_t)}{p(m^i)} $$
- Substitute the expanded measurement model in blue into the main Bayesian inference equation:

$$p(m^i|y_{1:t}) = \frac{p(m^i|y_t)p(y_t)p(m^i|y_{1:t-1})}{p(m^i)p(y_t|y_{1:t-1})}$$ - After logit function: $$logit(p(m^i|y_{1:t})) = logit(p(m^i|y_t)) + logit(p(m^i|y_{1:t-1})) - logit(p(m^i)) $$
- rewrite it as: $l_{t,i} = logit(p(m^i|y_t)) + l_{t-1,i} - l_{0,i}$
- $l_{t-1,i}$ is the previous belief
- $l_{0,i}$ is the initial belief

- Advantages over directly updating probabilities
- Numerically stable
- Computationally efficient

### Populating Occupancy Grids from LIDAR Scan Data (Part 2)

**Inverse Measurement Module**

- Relative change: $$r^i = \sqrt((m^i_x - x_{1,t})^2 + (m^i_y - x_{2,t})^2)$$
- the Euclidean distance from the sensor to the cell
- where $r^i$ is the range to grid cell i
- $m^i_x$ and $m^i_y$ are the x and y coordinates of the center of the grid cell.
- $x_{1,t}$ and $x_{2,t}$ are the sensor location at the current time t

- Relative bearing: $$\phi^i = \tan^{-1} (\frac{m^i_y - x_{2,t}}{m^i_x - x_{1,t}}) - x_{3,t}$$
- Closest relative bearing: $$k = argmin(|\phi^i - \phi^s_j|)$$
- For each cell, we associate the most relevant lidar beam by finding the measurement with the minimum error between its beam angle and the cell bearing.

- Then define two parameters $\alpha$ and $\beta$, which define a sector around each beam in which cell occupancy should be determined based on the beam range.
- $\alpha$ defines the affected range for high probability
- $\beta$ defines the affected angle for low and high probability
- This essentially creates a region around each beam which will get assigned the measurement information of that particular beam.

- We are now ready to assign a probability that any cell is occupied given the lidar measurements received based on these three types of cells:
- no information zone: if $r^i > min(r^s_{max}) $ or $ |\phi^i -\phi^s_k| > \beta/2$
- high information zone: if $r^s_k < r^s_{max}$ and $|r^i - r^s_k| > \alpha/2$
- low information zone: if $r_i < r^s_k$

**Inverse Measurement Module With Ray Tracing**

- However, this simple inverse measurement model can be computationally expensive:
- It requires a full update of every cell in the measurement map
- and relies on multiple floating-point operations to identify which measurements correspond to which cells.

- Alternative is to use Ray tracing algorithm
- using Bresenham’s line algorithm
- Rasterized line algorithm
- Uses very cheap fixed point operations for fast calculations

### Occupancy Grid Updates for Self-Driving Cars

Downsampling

- Up to ~1.2 million points per second
- Removal of redundant points
- Improves computation

Removal of overhanging objects

- Removing all Lidar points that are above a given threshold of the height limit of the car

Removal of ground plane

- Difficult to estimate due to several complications o Differing road geometries oCurbs,lane boundaries oDon’t want to miss small objects
- Ground plane Classification
- Utilize segmentation to remove points of road elements
- Keep points from no drivable surfaces

Removal of Dynamic Objects

- Not all vehicles are dynamic, so they should be included
- History of dynamic object location can be used to identify parked vehicle
- The dynamic objects are identified from the previous LIDAR frame
- Predicted future location improvement

Projection of LIDAR Onto a 2D Plane

- Collapse all points by Zeroing the Z coordinate
- Sum up the number of LIDAR points in each grid location
- More points indicated greater chance of occupation of that grid cell

### High Definition Road Maps

High Definition Road Maps stores

- the precise road locations including all lanes down to a few centimeters accuracy.
- all of the locations of road signs and signals which might effect the autonomous vehicle.

**Lanelet map**

- Due to the detailed and interconnected nature of the data, an effective method is required to store all information contained within the map.
- Due to this method’s effectiveness in storage and communication of the complex set of information needed for HD mapping, it is widely used
- The lanelet map has two main components
- Lanelet element: stores all information connected to a small longitudinal segment of a lane on a road which it represents
- Intersection element: stores all lanelet elements which are part of a single intersection for simple retrieval during motion planning tasks.

**Lanelet element**

- Defines the following:
- Left and Right Boundaries
- Regulation
- Elements: e.g. a stop sign line or a static sign line
- Attributes: attributes that might affect that segment of the road e.g. speed limit
- Note that we only store a line for any regulatory element as this is the point that the autonomous vehicle treats as the active location for that regulatory element.

- Connectivity to other lanelets
- This allows for easy traversal and calculations through the graph created by the set of lanelets in an HD map.

- A new lanelet is created when a new regulatory element is encountered or ends
- Boundaries of the lanelet are represented as the edges of the lanelet.
- These boundaries capture either marked lane boundaries or curves which inhibit driving.
- Lane boundaries are stored as a set of points creating a continuous polygonal line. Each point stores its x, y, and z GPS coordinates. The distance between points can be as fine as a few centimeters, or as course as a few meters depending on the smoothness of the polyline in question.
- The ordering of the points defines the direction of travel and heading for the lanelet.

- The entire lanelet structure is connected in a directed graph, which is the base structure of the HD map
- Operations Done On Lanelets
- Path planning through complex road networks
- Localize Dynamic Objects
- Interactions with other Dynamic Objects

**Creations Of Lanelets**

3 methods to create the lanelet map

- Offline creation
- Online creation: highly computationally expensive
- Offline creation with online updating