From 30bc6bd97aea9947a655ccd427fd68257ddd5f7e Mon Sep 17 00:00:00 2001
From: Michael Grupp <grupp@magazino.eu>
Date: Thu, 12 Jul 2018 13:25:57 +0200
Subject: [PATCH] Add equations for landmark cost function. (#1254)

---
 docs/source/cost_functions.rst | 48 +++++++++++++++++++++++++++++-----
 1 file changed, 42 insertions(+), 6 deletions(-)

diff --git a/docs/source/cost_functions.rst b/docs/source/cost_functions.rst
index d49969a..58ad253 100644
--- a/docs/source/cost_functions.rst
+++ b/docs/source/cost_functions.rst
@@ -20,13 +20,13 @@ Relative Transform Error 2D
 ===========================
 
 Given two poses
-: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`
+: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`
 the transformation :math:`\mathbf T` from the coordinate frame :math:`j` to the
 coordinate frame :math:`i` has the following form
 
 .. math::
- \mathbf{T}( \mathbf{p_i},\mathbf{p_j}) =
+ \mathbf{T}( \mathbf{p}_i,\mathbf{p}_j) =
  \left[
    \begin{array}{c}
         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
 
 .. math::
- \mathbf f( \mathbf{p_i},\mathbf{p_j}) =
+ \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})
+   \mathbf T_{ij}^m - \mathbf T( \mathbf{p}_i,\mathbf{p}_j)
  \right) =
  \left[
    \begin{array}{c}
@@ -68,7 +68,7 @@ Jacobian matrix  :math:`J_f` is given by:
 
 .. math::
  \begin{align}
-   J_f( \mathbf{p_i},\mathbf{p_j}) &=
+   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
@@ -92,3 +92,39 @@ Jacobian matrix  :math:`J_f` is given by:
      \end{array}
    \right]
  \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.