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.