# Cost functions¶

## Relative Transform Error 2D¶

Given two poses $$\mathbf{p}_i = [\mathbf{x}_i; \theta_i] = [x_i, y_i, \theta_i]^T$$ and $$\mathbf{p}_j = [\mathbf{x}_j; \theta_j] = [x_j, y_j, \theta_j]^T$$ the transformation $$\mathbf T$$ from the coordinate frame $$j$$ to the coordinate frame $$i$$ has the following form

$\begin{split}\mathbf{T}( \mathbf{p}_i,\mathbf{p}_j) = \left[ \begin{array}{c} R(\theta_i)^T (\mathbf x_j - \mathbf x_i) \\ \theta_j-\theta_i \end{array} \right]\end{split}$

where $$R(\theta_i)^T$$ is the rotation matrix of $$\theta_i$$.

The weighted error $$f:\mathbb R^6 \mapsto \mathbb R^3$$ between $$\mathbf T$$ and the measured transformation $$\mathbf T_{ij}^m = [\mathbf x_{ij}^m; \theta_j^m]$$ from the coordinate frame $$j$$ to the coordinate frame $$i$$ can be computed as

$\begin{split}\mathbf f_{\text{relative}}( \mathbf{p}_i,\mathbf{p}_j) = \left[ w_{\text{t}} \; w_{\text{r}} \right] \left( \mathbf T_{ij}^m - \mathbf T( \mathbf{p}_i,\mathbf{p}_j) \right) = \left[ \begin{array}{c} w_{\text{t}}\left( \mathbf x_{ij}^m - R(\theta_i)^T (\mathbf x_j - \mathbf x_i) \right) \\ w_{\text{r}}\left( \mathrm{clamp}(\theta_{ij}^m - (\theta_j-\theta_i)) \right) \end{array} \right]\end{split}$

where $$w_t$$ and $$w_r$$ are weights for translation and rotation respectively and $$\mathrm{clamp}: \mathbb R \mapsto [-\pi, \pi]$$ normalizes the angle difference.

Jacobian matrix $$J_f$$ is given by:

\begin{split}\begin{align} J_f( \mathbf{p}_i,\mathbf{p}_j) &= \left[ \frac{\partial\mathbf f}{\partial x_i} \quad \frac{\partial\mathbf f}{\partial y_i} \quad \frac{\partial\mathbf f}{\partial \theta_i} \quad \frac{\partial\mathbf f}{\partial x_j} \quad \frac{\partial\mathbf f}{\partial y_j} \quad \frac{\partial\mathbf f}{\partial \theta_j} \right] \\ &\mathstrut \\ &= \left[ \begin{array}{cccc} w_{\text{t}} R^T(\theta_i) & -w_{\text{t}} {\frac{\mathrm d R^T(\theta_i)}{\mathrm d \theta}}(\mathbf x_j - \mathbf x_i) & -w_{\text{t}} R^T(\theta_i) & \mathbf{0} \\ \mathbf{0}^T & w_{\text{r}} & \mathbf{0}^T & -w_{\text{r}} \end{array} \right] \end{align}\end{split}

## Landmark Cost Function¶

Let $$\mathbf{p}_o$$ denote the global pose of the SLAM tracking frame at which a landmark with the global pose $$\mathbf{p}_l$$ is observed. The landmark observation itself is the measured transformation $$\mathbf{T}^m_{ol}$$ that was observed at time $$t_o$$.

As the landmark can be observed asynchronously, the pose of observation $$\mathbf{p}_o$$ is modeled in between two regular, consecutive trajectory nodes $$\mathbf{p}_i, \mathbf{p}_j$$. It is interpolated between $$\mathbf{p}_i$$ and $$\mathbf{p}_j$$ at the observation time $$t_o$$ using a linear interpolation for the translation and a quaternion SLERP for the rotation:

$\mathbf{p}_o = \text{interpolate}(\mathbf{p}_i, \mathbf{p}_j, t_o)$

Then, the full weighted landmark cost function can be written as:

\begin{split}\begin{align} \mathbf f_{\text{landmark}}(\mathbf{p}_l, \mathbf{p}_i, \mathbf{p}_j) &= \mathbf f_{\text{relative}}(\mathbf{p}_l, \mathbf{p}_o) \\ &= \left[ w_{\text{t}} \; w_{\text{r}} \right] \left( \mathbf T_{ol}^m - \mathbf T( \mathbf{p}_o,\mathbf{p}_l) \right) \end{align}\end{split}

The translation and rotation weights $$w_{\text{t}}, w_{\text{r}}$$ are part of the landmark observation data that is fed into Cartographer.