12345qiupeng
/
cartographer
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Add equations for landmark cost function. (#1254)

master
Michael Grupp 2018-07-12 13:25:57 +02:00 committed by Wally B. Feed
parent 1b88fb8e90
commit 30bc6bd97a
1 changed files with 42 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

48
docs/source/cost_functions.rst
View File

@ -20,13 +20,13 @@ Relative Transform Error 2D
=========================== ===========================
Given two poses Given two poses
:math:`\mathbf{p_i} = [\mathbf{x_i}; \theta_i] = [x_i, y_i, \theta_i]^T` :math:`\mathbf{p}_i = [\mathbf{x}_i; \theta_i] = [x_i, y_i, \theta_i]^T`
and :math:`\mathbf{p_j} = [\mathbf{x_j}; \theta_j] = [x_j, y_j, \theta_j]^T` and :math:`\mathbf{p}_j = [\mathbf{x}_j; \theta_j] = [x_j, y_j, \theta_j]^T`
the transformation :math:`\mathbf T` from the coordinate frame :math:`j` to the the transformation :math:`\mathbf T` from the coordinate frame :math:`j` to the
coordinate frame :math:`i` has the following form coordinate frame :math:`i` has the following form
.. math:: .. math::
\mathbf{T}( \mathbf{p_i},\mathbf{p_j}) = \mathbf{T}( \mathbf{p}_i,\mathbf{p}_j) =
\left[ \left[
\begin{array}{c} \begin{array}{c}
R(\theta_i)^T (\mathbf x_j - \mathbf x_i) \\ R(\theta_i)^T (\mathbf x_j - \mathbf x_i) \\
@ -42,12 +42,12 @@ The weighted error :math:`f:\mathbb R^6 \mapsto \mathbb R^3` between
coordinate frame :math:`i` can be computed as coordinate frame :math:`i` can be computed as
.. math:: .. math::
\mathbf f( \mathbf{p_i},\mathbf{p_j}) = \mathbf f_{\text{relative}}( \mathbf{p}_i,\mathbf{p}_j) =
\left[ \left[
w_{\text{t}} \; w_{\text{r}} w_{\text{t}} \; w_{\text{r}}
\right] \right]
\left( \left(
\mathbf T_{ij}^m - \mathbf T( \mathbf{p_i},\mathbf{p_j}) \mathbf T_{ij}^m - \mathbf T( \mathbf{p}_i,\mathbf{p}_j)
\right) = \right) =
\left[ \left[
\begin{array}{c} \begin{array}{c}
@ -68,7 +68,7 @@ Jacobian matrix :math:`J_f` is given by:
.. math:: .. math::
\begin{align} \begin{align}
J_f( \mathbf{p_i},\mathbf{p_j}) &= J_f( \mathbf{p}_i,\mathbf{p}_j) &=
\left[ \left[
\frac{\partial\mathbf f}{\partial x_i} \quad \frac{\partial\mathbf f}{\partial x_i} \quad
\frac{\partial\mathbf f}{\partial y_i} \quad \frac{\partial\mathbf f}{\partial y_i} \quad
@ -92,3 +92,39 @@ Jacobian matrix :math:`J_f` is given by:
\end{array} \end{array}
\right] \right]
\end{align} \end{align}
Landmark Cost Function
======================
Let :math:`\mathbf{p}_o` denote the global pose of the SLAM tracking frame at
which a landmark with the global pose :math:`\mathbf{p}_l` is observed.
The landmark observation itself is the measured transformation
:math:`\mathbf{T}^m_{ol}` that was observed at time :math:`t_o`.
As the landmark can be observed asynchronously, the pose of observation
:math:`\mathbf{p}_o` is modeled in between two regular, consecutive trajectory
nodes :math:`\mathbf{p}_i, \mathbf{p}_j`.
It is interpolated between :math:`\mathbf{p}_i` and
:math:`\mathbf{p}_j` at the observation time :math:`t_o` using a linear
interpolation for the translation and a quaternion SLERP for the rotation:
.. math::
\mathbf{p}_o = \text{interpolate}(\mathbf{p}_i, \mathbf{p}_j, t_o)
Then, the full weighted landmark cost function can be written as:
.. math::
\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}
The translation and rotation weights :math:`w_{\text{t}}, w_{\text{r}}` are
part of the landmark observation data that is fed into Cartographer.