Probabilistic OGM

What is it?

Probabilistic Occupancy Grid Mapping (OGM) is a technique used to overcome certain limitations of Binary OGM. With Binary OGM, cells are labeled as either occupied (1) or unoccupied (0), which leaves no room for uncertainty. This rigidity makes the algorithm impractical in real-world applications due to noisy sensors. Probabilistic OGM overcomes this obstacle by using individual probabilities for each cell to represent how certain we are that it is filled.

U
properties — An application of these algorithms is present in Waymo!

Why Probabilistic?

In the real world, sensors are not always accurate. Depending on what sensor you are using and what environment your robot is operating in, your certainty in sensor readings can vary quite a bit from numbers as low as 70% on the low end and 99% on the high end. Let's take a look at how this affects Binary OGM.

Imagine our robot is looking at a wall. It has a LiDAR sensor with a sensor accuracy of 80%. So on average, for every four correct readings we get, we will also get an incorrect reading. Sometimes we may get lucky and receive a completely accurate set of readings.

But on average we will often find ourselves with a slightly incorrect set of readings.

In the worst case, we may find ourselves with a completely wrong set of readings.

This is the weakness of Binary OGM: we change our beliefs about the state of our environment based on only our newest data. This leads to a consistently prominent amount of inaccuracies in our beliefs. This can also cause problems for other areas such as planning. Considering how frequent most sensors take measurements, different paths may appear and disappear quickly from constantly changing data, leading to a confused movement of our robot.

To address this issue, Probabilistic OGM takes into account past data and slowly updates its beliefs with new data. Let's look at an example to see how this works. Imagine our LiDAR is taking measurements at a block which is the size of a cell. We will again assume a sensor accuracy of 80%. Our LiDAR takes 5 measurements and from its data we record: Occupied, Occupied, Occupied, Occupied, Free.

If we only consider the newest data like Binary OGM, after the fifth scan we will believe that cell is empty. Instead Probabilistic OGM considers our past data. After the fifth scan, we have recorded a state of occupied 4 times and free 1 time. Since the cell has a long history of being occupied that is much larger than our single measurement of free, our belief will still be that the cell is occupied despite our new data.

Applying this on a broader range to all cells, we can achieve a much more accurate representation of our environment that filters out the noise of our sensor. This is a surface level look that demonstrates a rough idea of what Probabilistic OGM does. To more deeply understand how Probabilistic OGM works and is implemented, let's take a closer look at log-odds and the algorithm.

Bayes' Theorem in Mapping

Applying Bayes' Theorem to mapping, we can derive an equation:

\[P_{occ} = \frac{P(z_t | \text{occ}) P_{prev}}{P(z_t)}\]

where:

$P_{occ}$ is the probability that a cell is occupied (posterior):
$P(z_t | \text{occ})$ is the probability of the sensor measurement $z_t$ given that the cell is occupied (likelihood).
$P_{prev}$ is the probability that the cell was occupied in the previous time step (prior).
$P(z_t)$ is the probability of the sensor measurement $z_t$ at time $t$ (marginal).

This Bayesian update ensures that uncertainties in sensor measurements and temporary changes in the environment are handled more gracefully. It can be a little difficult to understand how to use this formula to make calculations for OGM, especially for those unfamiliar with probability theory. Thankfully, there is a much easier way to apply Bayesian updates that also has advantages for mapping.

Log-odds

Log-odds (or logit) and odds are a way of representing probablity similar to how percentages and decimals are ways of representing probability. Log-odds are how probabilities are commonly calculated in mapping, but to understand log-odds, one must first understand odds.

Odds, similar to percent probabilities, are simply just ratios. The key difference lies in what they are the ratio of:

Percent probabilities are the ratio of the how often a measured event occurs to the total probability (1 or 100%).
Odds are the ratio of how often a measured event occurs to how often that measured event doesn't occur.

For example, take flipping a coin. On average, for every $1$ head flipped, there should also be $1$ tail flipped. To measure the odds of flipping heads, just divide the number of heads by tails, which gives us an odds of $1$. For a dice, there are $6$ possible outcomes—each as likely as the other. To find the odds of rolling anything but 6, consider how many outcomes satisfy the constraint and how many don't. There are $5$ numbers that aren't 6 and only $1$ number that is 6. So dividing $5$ by $1$, we find that the ratio is an odds of $5$.

This idea of using the number of outcomes to calculate odds can be translated to using percentages with the equation:

\[\text{Odds} = \frac{P}{1-P}\]

where:

$P$ is the probability of an event occuring.

In the numerator is the probability of event A occurring. In the denominator is $1$ minus that value, which is just the probability of event A not occurring. Instead of using the number of occurences of each outcome like before, they can be directly replaced with their respective percentages.

Moving onto log-odds, the conversion from odds to log-odds is very simple. As the name suggest, to get log-odds you just need to put odds into a logarithmic scale. To do that, we simply take the natural logarithm of the odds to get the equation:

\[\text{Log Odds} = \ln(\frac{P}{1-P})\]

Log-odds have unique properties that make them particularly useful for the purposes of mapping. Instead of applying Bayes' Theorem every time an update is needed, the log-odds can be simply added to apply Bayesian updates instead. This has two major benefits:

Simpler calculation that can be integrated into code much more easily.
Removing the need to constantly multiply fractions which can lead to edge cases where long floating point numbers can lead to errors or take up computational power.

Applying Probability in the Occupancy Grid

Key Assumptions:

The robot’s position and orientation (poses) are known.
The probability of individual cells is independent of that of their neighbors.
The environment’s uncertainty is represented by an initial occupancy probability of 50% (which is $0$ in log-odds), as it is unclear whether the cells in the environment are occupied or unoccupied. This is visualized on the occupancy grid map as a grey shading.
The probability values of each cell change in accordance with the robot’s LiDAR measurements. Here we will use assume a confidence of 80% in our LiDAR's measurements, meaning there is a 20% chance that the measurements are inaccurate. Converting this probability to log-odds gives us $ \ln(4) $. In the real world, this percentage needs to be adjusted to fit your confidence in your sensor.

Creating the Algorithm:

Initial State: Each cell starts with an initial probability of $0$ (initial prior) in log-odds or 50% to represent uncertainty.
Sensor Update:
- If the cell senses an obstacle with 80% confidence, it will add $ \ln(4) $ to the cell's current log odds.
- If the cell senses that the cell is unoccupied with 80% confidence, it will add $ -\ln(4) $ to the cell's current log odds.
Resampling:
- Each new sensor reading is used to update the log odds value additively: $$ l_{\text{new}} = l_{\text{old}} + \text{update} $$
- Over time, as multiple measurements are added, the log-odds value will keep increasing if occupancy is consistently observed and decreasing if vacancy is consistently observed. Log-odds can be converted back to a percentage or decimal using the equation:
  $$ P = \frac{1}{1+e^{-l}} $$ where:
  - $P$ is the probability as a decimal.
  - $l$ is the log-odds of the probability.

If done properly, as a robot moves through an environment and scans through the room, the probability of all occupied cells should approach $1$ and the probability of all free cells should approach $0$. If environment is dynamic, cells where the environment is changing will oscillate between values, reflecting the dynamic nature of their states.

Let's say our LiDAR has an accuracy of 70%. For a particular cell we record the data of five scans: Occupied, Free, Occupied, Occupied, Occupied. What will be the probability that our Probabilistic OGM stores for that cell after those five scans? Convert your answer back to a percentage.

A

$ 98.4\text{%} $

B

$ 7.3\text{%} $

C

$ 96.7\text{%} $

D

$ 92.7\text{%} $

$$l = \ln(\frac{P}{1-P})$$

The log-odds probability is calculated with the above equation. Plugging in our probability of $P = 0.70$, we get a log-odds of $l = 0.847...$ for our known accuracy. Our initial condition of unknown always starts out at 50%, which is equivalent to $0$ in log-odds. Looking at our recorded states of the cell, occupied was recorded $4$ times and free $1$ time. Accordingly, we add $l$ four times and subtract $l$ one time to our starting to condition of $0$. This leaves us with a recorded value of $2.541...$

$$P = \frac{1}{1+e^{-l}}$$

Now we just need to convert that number back to a percentage with the above equation to get our answer. We arrive at the answer 92. 7%, which is D

Demo

To try out the OGM algorithm, run this Google Colab notebook: