Week #6: Mapping
How to represent maps
Probabilistic occupancy grid mapping
Advantages:
• O(1) occupancy lookup and update
• Supports image operations
Disadvantages:
• Doesn’t scale well in higher dimensions
Each cell contains either:
Each node represents a square. If the node is fully empty or fully occupied it has no children.
If it is partially occupied it has four children. Subdivision stops after some minimal square size.
Each node represents a cube. If the node is fully empty or fully occupied it has no children.
If it is partially occupied it has eight children. Subdivision stops after some minimal cube size.
Problem 1: quadtrees and octrees are not balanced trees. So, in the worst case an occupancy query could be O(n) in the number of nodes.
Each node represents a cube. If the node is fully empty or fully occupied it has no children.
If it is partially occupied it has eight children. Subdivision stops after some minimal cube size.
Problem 1: quadtrees and octrees are not balanced trees. So, in the worst case an occupancy query could be O(n) in the number of nodes.
Problem 2: quadtrees and octrees are sensitive to small changes in the location of obstacles.
Each node represents a cube. If the node is fully empty or fully occupied it has no children.
If it is partially occupied it has eight children. Subdivision stops after some minimal cube size.
This distance function
is defined over any point
in 3D space.
Advantages:
Disadvantages:
\(\qquad\qquad\quad\) GVG nodes: points that are equidistant to 3 or more obstacle points
Retractions are
also called
roadmaps.
Turns comparison between pixels to comparison between graphs.
The skeleton is sensitive to small changes in the object’s boundary.
Can specify whether we pass on the “left” or “right” of each obstacle
on our way to the goal.
Note: winding angle of a path
can be more than 360 degrees
Two paths with the same endpoints are homotopic or belong to the same homotopy class iff one can be deformed continuously (without hitting obstacles) into the other. Formally, the paths:
with
\[ \begin{align} \tau_1 : [0, T] &\to \mathbb{R}^2 \\ \tau_2 : [0, T] &\to \mathbb{R}^2 \\ \tau_1(0) &= \tau_2(0) \\ \tau_1(T) &= \tau_2(T) \end{align} \]
are homotopic iff there exists a continuous function
\[H : [0, 1] \times [0, T] \to \mathbb{R}^2\]
such that for any time t:
\[ \begin{align} H(0, t) &= \tau_1(t) \\ H(1, t) &= \tau_2(t) \end{align} \]
Two paths with the same endpoints are homotopic or belong to the same homotopy class iff one can be deformed continuously (without hitting obstacles) into the other. Formally, the paths:
with
\[ \begin{align} \tau_1 : [0, T] &\to \mathbb{R}^2 \\ \tau_2 : [0, T] &\to \mathbb{R}^2 \\ \tau_1(0) &= \tau_2(0) \\ \tau_1(T) &= \tau_2(T) \end{align} \]
are homotopic iff there exists a continuous function
\[H : [0, 1] \times [0, T] \to \mathbb{R}^2\]
such that for any time t:
\[ \begin{align} H(0, t) &= \tau_1(t) \\ H(1, t) &= \tau_2(t) \end{align} \]
Occupancy grid
Topological map
Topometric map
Edges: rotations and
translations between
local maps, but also topological
connectivity
Main advantage:
allows us combine accurate local
maps into a global, typically
inconsistent map that nevertheless
provides sufficient navigation information.
If the correct correspondences are known, the correct relative rotation/translation can be calculated in closed form.
When correct correspondences
are known we say that data
association is known/unambiguous.
In general, data association is a real
and hairy problem in robotics.
Find the 3D rotation matrix R and the 3D translation vector t that will best align the corresponding points
\[ \text{error}(R, t) = \frac{1}{N} \sum_{i=1}^{N} ||p_i - (Rq_i + t)||^2 \]
\[ R^*, t^* = \operatorname*{argmin}_{R,t} \text{error}(R, t) \]
Q: How do we minimize this error?
A: Turns out it has a closed-form solution.
Find the 3D rotation matrix R and the 3D translation vector t that will best align the corresponding points
\[ \text{error}(R, t) = \frac{1}{N} \sum_{i=1}^{N} ||p_i - (Rq_i + t)||^2 \]
\[ R^*, t^* = \operatorname*{argmin}_{R,t} \text{error}(R, t) \]
Step 1: compute the means of the two scans
\[ \mu_p = \frac{1}{N} \sum_{i=1}^{N} p_i \qquad\qquad \mu_q = \frac{1}{N} \sum_{i=1}^{N} q_i \]
Step 2: subtract the means from the scans
\(\qquad \bar{p}_i = p_i - \mu_p \qquad\qquad \bar{q}_i = q_i - \mu_q\)
Step 3: form the matrix
\[W = \sum_{i=1}^{N} \bar{p}_i \bar{q}_i^T\]
Find the 3D rotation matrix R and the 3D translation vector t that will best align the corresponding points
\[ \text{error}(R, t) = \frac{1}{N} \sum_{i=1}^{N} ||\mathbf{p}_i - (R\mathbf{q}_i + t)||^2 \]
\[ R^*, t^* = \operatorname*{argmin}_{R,t} \text{error}(R, t) \]
Step 1: compute the means of the two scans
\[ \mu_p = \frac{1}{N} \sum_{i=1}^{N} \mathbf{p}_i \qquad\qquad \mu_q = \frac{1}{N} \sum_{i=1}^{N} \mathbf{q}_i \]
Step 2: subtract the means from the scans
\(\qquad \bar{\mathbf{p}}_i = \mathbf{p}_i - \mu_p \qquad\qquad \bar{\mathbf{q}}_i = \mathbf{q}_i - \mu_q\)
Step 3: form the matrix
\[W = \sum_{i=1}^{N} \bar{\mathbf{p}}_i \bar{\mathbf{q}}_i^T\]
Step 4: compute the SVD of the matrix W
\(\qquad W = U\Sigma V^T\)
Step 5: if rank(W)=3, optimal solution is unique:
\(\qquad R^* = UV^T \qquad\qquad t^* = \mu_p - R^* \mu_q\)
Find the 3D rotation matrix R and the 3D translation vector t that will best align the corresponding points
\[ \text{error}(R, t) = \frac{1}{N} \sum_{i=1}^{N} ||p_i - (Rp_i + t)||^2 \]
\[ R^*, t^* = \operatorname*{argmin}_{R,t} \text{error}(R, t) \]
If you’re interested, the proof of the closed-form solution can be found in:
K. S. Arun, T. S. Huang, and S. D. Blostein. Least square fitting of two 3-d point sets.
IEEE Transactions on Pattern Analysis and Machine Intelligence, 9(5):698 – 700, 1987
If correct correspondences are not known, it is generally impossible to determine the optimal relative rotation/ translation in one step
Main idea for data association:
associate each point in the source scan to its nearest neighbor in the target scan
Find optimal rotation and translation for this correspondence.
Repeat until no significant drop in error.
How to represent maps
Probabilistic occupancy grid mapping
\(x_{t+1} = f(x_t, u_t)\)
\(x_{t+1} = f(x_t, u_t) + w_t\)
Sensor
Measurements
\(z_t = h(x_t)\)
e.g. GPS (simplified)
\(z_t = x_t\) 
\(z_t = x_t + v_t, \quad v_t \sim \mathcal{N}(0, \sigma^2I)\)
Dynamics
Sensor Measurements
e.g. GPS (simplified)
\(x_{t+1} = f(x_t, u_t)\)
\(z_t = h(x_t)\)
\(z_t = x_t\)
\(p(x_{t+1} | x_t, u_t)\)
probabilistic dynamics model
\(p(z_t| x_t)\)
probabilistic measurement model
Don’t we have all the information we need to build a map?
\(p(x_{t+1} | x_t, u_t)\)
Uncertainty in the dynamics compounds into increasing uncertainty in odometry, as time passes.
Don’t we have all the information we need to build a map?
\[\mathbf{m} = \{m_{ij}\} \in \{0,1\}^{W \times H}\]
width = #columns
height = #rows
of the occupancy grid
Odometry coordinates
K = #beams, or #points in the scan
(range, angle) in
the laser’s local
coordinates
\[ belief_{t}(\mathbf{m}) = p(\mathbf{m}|\mathbf{z}_{1:t}, \mathbf{x}_{1:t}) \]
Sensor
Measurements
Odometry / Robot Poses
Scenario
Nearby
measurements
Resulting map when
considering cells
independently
Resulting map when
considering cells
jointly
How do we evaluate \(p(m_{ij} = 1 | z_{1:t}, x_{1:t})\) ?
Using conditional Bayes’ rule we get
\(p(m_{ij} = 1|\mathbf{z}_{1:t}, \mathbf{x}_{1:t}) = \frac{p(\mathbf{z}_t|\mathbf{z}_{1:t-1}, \mathbf{x}_{1:t}, m_{ij} = 1)p(m_{ij} = 1|\mathbf{z}_{1:t-1}, \mathbf{x}_{1:t})}{p(\mathbf{z}_t|\mathbf{z}_{1:t-1}, \mathbf{x}_{1:t})}\)
\(p(m_{ij} = 1|\mathbf{z}_{1:t}, \mathbf{x}_{1:t}) = \frac{p(\mathbf{z}_t|\mathbf{x}_t, m_{ij} = 1)p(m_{ij} = 1|\mathbf{z}_{1:t-1}, \mathbf{x}_{1:t-1})}{p(\mathbf{z}_t|\mathbf{z}_{1:t-1}, \mathbf{x}_{1:t})}\)
If C is independent of A
given B, then C provides
no extra information
about A after we know B
\(p(A|B, C) = p(A | B)\)
How do we evaluate \(p(m_{ij} = 1 | z_{1:t}, x_{1:t})\) ?
Using conditional Bayes’ rule we get
\(p(m_{ij} = 1|\mathbf{z}_{1:t}, \mathbf{x}_{1:t}) = \frac{p(\mathbf{z}_t|\mathbf{z}_{1:t-1}, \mathbf{x}_{1:t}, m_{ij} = 1)p(m_{ij} = 1|\mathbf{z}_{1:t-1}, \mathbf{x}_{1:t})}{p(\mathbf{z}_t|\mathbf{z}_{1:t-1}, \mathbf{x}_{1:t})}\)
\(p(m_{ij} = 1|\mathbf{z}_{1:t}, \mathbf{x}_{1:t}) = \frac{p(\mathbf{z}_t|\mathbf{x}_t, m_{ij} = 1)p(m_{ij} = 1|\mathbf{z}_{1:t-1}, \mathbf{x}_{1:t-1})}{p(\mathbf{z}_t|\mathbf{z}_{1:t-1}, \mathbf{x}_{1:t})}\)
Current measurement
only depends on current
state and map cell
Current state without
current measurement provides
no additional information
about the occupancy of the map cell
\(p(m_{ij} = 1|\mathbf{z}_{1:t}, \mathbf{x}_{1:t}) = \frac{p(\mathbf{z}_t|\mathbf{x}_t, m_{ij} = 1)p(m_{ij} = 1|\mathbf{z}_{1:t-1}, \mathbf{x}_{1:t-1})}{p(\mathbf{z}_t|\mathbf{z}_{1:t-1}, \mathbf{x}_{1:t})}\)
\(belief_t(m_{ij} = 1) = \eta \, p(\mathbf{z}_t|\mathbf{x}_t, m_{ij} = 1) \, belief_{t-1}(m_{ij} = 1)\)
So, as long as we can evaluate the measurement likelihood \(\color{black}p(\mathbf{z}_t | \mathbf{x}_t, m_{ij} = 1)\)
and the normalization factor \(\color{black}\eta = 1/p(\mathbf{z}_t | \mathbf{z}_{1: t-1}, \mathbf{x}_{1:t})\)
we can do the belief update.
Problem: this is hard to
compute. How can we avoid it?
We want to compute \(\log \frac{p(\mathbf{z}_t | \mathbf{x}_t, m_{ij} = 1)}{p(\mathbf{z}_t | \mathbf{x}_t, m_{ij} = 0)}\) to do the belief update
We apply conditional Bayes’ rule again: \(\quad p(\mathbf{z}_t | \mathbf{x}_t, m_{ij} = 1) = \frac{p(m_{ij} = 1 | \mathbf{z}_t, \mathbf{x}_t) p(\mathbf{z}_t | \mathbf{x}_t)}{p(m_{ij} = 1 | \mathbf{x}_t)}\)
Knowing the current state provides no
information about whether cell is
occupied, if there are no observations
Prior probability of cell being occupied.
Can choose uniform distribution, for example.
Inverse measurement model:
what is the likelihood of the map
cell being occupied given the current
state and current measurement?
We want to compute \(\log \frac{p(\mathbf{z}_t | \mathbf{x}_t, m_{ij} = 1)}{p(\mathbf{z}_t | \mathbf{x}_t, m_{ij} = 0)}\) but it’s hard
Instead, we can compute the log-odds ratio of the measurement likelihood in terms of the inverse measurement model:
\[\log \frac{p(\mathbf{z}_t | \mathbf{x}_t, m_{ij} = 1)}{p(\mathbf{z}_t | \mathbf{x}_t, m_{ij} = 0)} = \log\frac{p(m_{ij}=1|\mathbf{z}_{t},\mathbf{x}_{t})}{p(m_{ij}=0|\mathbf{z}_{t},\mathbf{x}_{t})}+\log\frac{p(m_{ij}=0)}{p(m_{ij}=1)}\]
Given map cell \((i, j)\) , the robot’s state \(\mathbf{x} = (x, y, \theta)\), and beams \(\mathbf{z} = {(r_k, \psi_k)}\)
Find index k of sensor beam that is closest in heading to the cell \((i,j)\)
If the cell \((i, j)\) is sufficiently farther than \(r_k\) or out of the field of view
// We don’t have enough information to decide whether cell is occupied
Return prior occupation probability \(p(m_{ij} = 1)\)
If the cell \((i, j)\) is nearly as far as the measurement \(r_k\)
// Cell is most likely occupied
Return \(p_{\text{occupied}}\) that is well above 0.5
If the cell \((i, j)\) is sufficiently closer than \(r_k\)
// Cell is most likely free
Return \(p_{\text{occupied}}\) that is well below 0.5
The maximum likelihood map is obtained by clipping the occupancy grid map at a threshold of 0.5
