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
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
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:
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:
Then, the full weighted landmark cost function can be written as:
The translation and rotation weights \(w_{\text{t}}, w_{\text{r}}\) are part of the landmark observation data that is fed into Cartographer.