\subsection{Mapping}
\bi{Problem Formulations} \textit{Localisation} {\scriptsize (always static given map)}:\\
$\vec{x}^*_{R, k} = \text{argmax}\; \P(x_{R, k} \divider \vec{x}_M, \vec{z}_{1 : k}, \vec{u}_{1 : k})$ (recursive)\\
$\vec{x}^*_{R, 1:k} = \text{argmax}\; \P(x_{R, 1:k} \divider \vec{x}_M, \vec{z}_{1 : k}, \vec{u}_{1 : k})$ (batch)

\textit{SLAM} $\{ \vec{x}^*_{R, k}, \vec{x}^*_M \} \! = \! \text{argmax}\; \P(\vec{x}_R, \vec{x}_M \divider \vec{z}_{1 : k}, \vec{u}_{1 : k})$ (as $\uparrow$)

\textit{Mapp.}: $\vec{x}_M^* \! = \! \text{argmax}\; \P(\vec{x}_M \divider \vec{x}^*_{R, 1:k}, \vec{z}_{1 : k}, \vec{u}_{1 : k})$ with given poses $\vec{x}^*_{R, 1:k}$.
Thus temp. model $\vec{u}_{1:k}$ doesn't matter.

\bi{Prob. Occ. Grid} $\P(f_j) = \P(\neg o_j) = 1 - \P(o_j)$, with $\P(o_j)$ prob. cell $j$ occupied; pairwise independent.

\bi{Occ. Map. w/ depth sensor} $\P(o_j \divider \vec{x}_{R, 1:k}) =$
\[
    \frac{
        {\color{MidnightBlue} \P(o_j \divider \vec{x}_{R, k}, \vec{z}_k)}
        \P(\vec{z}_k \divider \vec{x}_{R, k})
        {\color{ForestGreen} \P(o_j \divider \vec{x}_{R, 1 : k - 1}, \vec{z}_{1 : k - 1})}
    }{{\color{DarkOrchid} \P(o_j)} \P(\vec{z}_k \divider \vec{x}_{R, 1 : k}, \vec{z}_{1 : k - 1})}
\]
{\color{DarkOrchid} map prior}; {\color{ForestGreen} Prev. occ. est.}; {\color{MidnightBlue} Occ. based on curr. range meas.};

\bi{Update function}:
$l(a) := \log(\text{Odds}(a))$ with $\text{Odds}(a) = \frac{\P(a)}{1 - \P(a)}$ (inv. sensor model):
$l(o_j | \vec{x}_{R, 1 : k}, \vec{z}_{1 : k})$ =
\[
    l(o_j | \vec{x}_{R, k}, \vec{z}_k) + l(o_j | \vec{x}_{R, 1 : k - 1}, \vec{z}_{1 : k - 1}) - l(o_j)
\]

\shade{gray}{In 3D} 3D voxel $j$ as signed dist. $s$ and weight $w$, update:
$\displaystyle s_k = \frac{w_{k - 1} s_{k - 1} + \tilde{s}_k}{w_{k - 1} + 1}$ with $w_k = \min(w_{\max}, w_{k - 1} + 1)$

\bi{Implementation} Using Hash maps or octree (dense grid inefficient)


\subsubsection{Iterative Closest Point}
Build \textit{correspondences}: associate all live scan points $l_i$ to closest map points $m_i$. Error term:
$\vec{e} = \vec{T}_{W L_l L_l} m_i - {_W}\vec{l}_i$.
Minimize this via Gauss-Newton, then re-associate, iterate.

\bi{Photometric}: $u_{KF} = \pi(\mat{T}_{WC_{KF}}^{-1} \mat{T}_{WC_l} \pi^{-1}(\vec{u}_{KF}, \mat{D}_{KF}[\vec{u}_{KF}]))$,
error term $\vec{e} = \mat{I}_{KF}[\vec{u}_{KF}] - \mat{I}_L[\vec{u}_{L}]$,
where all subscript $L$ are from live image, all subscript $KF$ key frame.