1
$\begingroup$

Given a pose $x_i = (t_i, q_i)$ with translation vector $t_i$ and rotation quaternion $q_i$ and a transform between poses $x_i$ and $x_j$ as $z_{ij} = (t_{ij}, q_{ij})$ I want to compute the error function $e(x_i, x_j) = e_{ij}$, which has to be minimized like this to yield the optimal poses $X^* = \{ x_i \}$:

$$X^* = argmin_X \sum_{ij} e_{ij}^T \Sigma^{-1}_{ij} e_{ij}$$

A naive approach would look like this:

$$ e_{ij} = z_{ij} - f(x_i,x_j) $$

where $z_{ij}$ is the current measurement of the transform between $x_i$ and $x_j$ and $f$ calculates an estimate for the same transform. Thus, $e_{ij}$ simply computes the difference of translations and difference of turning angles:

$$ e_{ij} = \begin{pmatrix} t_{ij} - t_j - t_i \\\ q_{ij} (q_j q_i^{-1})^{-1} \end{pmatrix} $$

Is there anything wrong with this naive approach? Am I missing something?

Improve this question
$\endgroup$

1 Answer 1

Reset to default
1
$\begingroup$

Yes, you have an issue, and it has to do with the parameterization of rotations. As you have it, your $e_{ij}$ will be a $7\times 1$ column, but your pose has only six degrees of freedom (three for translation, three for rotation). There is actually a constraint that isn't properly represented in your optimization function (the norm of a unit quaternion must be one). This means that when you try to do the optimization, it will not "know" about this redundancy and solving for $X^*$ will actually optimize a degree of freedom that doesn't exist.

So how do we deal with this? It's a good idea to use unit quaternions because they avoid the singularities present in all minimal parameterizations (i.e., three parameter representations) of rotations. A good approach to this problem is using a dual parameterization of rotations. First, we use a singularity-free parameterization of the rotation in the state (e.g., your use of unit quaternions is fine), but we represent rotation errors using a minimal parameterization. If the differences between your measurements $z_{ij}$ and predicted measurements $f$ are relatively small, you can safely convert the error rotation to a minimal parameterization without fear of it encountering a singularity (depending on your choice of minimal parameterization).

For example, let's use a rotation vector $\mathbf{r} = \theta\mathbf{a}$ for the minimal parameterization (where $\theta$ is the angle of rotation and $\mathbf{a}$ is the axis of rotation). Your new error vector is

$$ e_{ij} = \begin{bmatrix} t_{ij} - t_j - t_i \\ \log\left(q_{ij}(q_jq_i^{-1})^{-1}\right) \end{bmatrix} $$

where $\log(q)$ converts a quaternion to a rotation vector. Note that you should also be representing the uncertainty of your rotation measurement using a minimal parameterization (i.e., $\Sigma_{ij}$ is a $6\times 6$ matrix).

After you solve for the optimal $6 \times 1$ perturbation of the state $\Delta\mathbf{x}^*$, you need to apply it to your state by converting the rotation vector back to a unit quaternion; i.e.,

$$ q \gets q\, \exp(\Delta \mathbf{r}^*) $$

where $\Delta \mathbf{r}^*$ is the optimal perturbation of one of the state's quaternions (i.e., it's part of $\Delta\mathbf{x}^*$), and $\exp$ converts a rotation vector to a unit quaternion.

I didn't really go into too much detail here, and skipped some mathematical rigour for the sake of keeping things brief. For more information on this approach, see the below references. This approach is not limited to rotations.

C. Hertzberg, R. Wagner, U. Frese, and L. Schröder, “Integrating generic sensor fusion algorithms with sound state representations through encapsulation of manifolds,” Information Fusion, vol. 14, no. 1, pp. 57–77, Jan. 2013.

G. Grisetti, R. Kümmerle, C. Stachniss, and W. Burgard, “A tutorial on graph-based SLAM,” IEEE Intelligent Transportation Systems Maga- zine, vol. 2, no. 4, pp. 31–43, 2010.

Improve this answer
$\endgroup$
6
  • $\begingroup$ a big thank you! It helps me a lot to see everything explained in a little different way. I actually came up with the question when reading your second reference but didn't really understand the motion composition operator and thought about an easier solution. $\endgroup$
    – morph
    Feb 19, 2016 at 15:48
  • $\begingroup$ when I calculate $\Sigma_{ij}$ this leads to mismatching matrix dimensions:$$\Sigma_{ij}^{t}=J_{iterate} \Sigma_{ij}^{t-1} J_{iterate}^T + J_{process} \Sigma_{sensor} J_{process}^T$$ where $J_{iterate}=\frac{\partial x^{t}}{\partial x^{t-1}}$, $J_{process}=\frac{\partial x^{t}}{\partial \xi^{t}}$ and $\xi^{t} = [a,\omega]$. my process model is $x^{t} = f(x^{t-1},a,\omega)$ where $a$ and $\omega$ are acceleration and rotational velocity both measured by an IMU and $f$ integrates the acceleration twice and the rotational velocity once to obtain position and orientation. $\endgroup$
    – morph
    May 22, 2016 at 11:07
  • $\begingroup$ $\Sigma_{sensor}$ is a matrix with the acceleration and rotational velocity variances on its diagonal which makes a 6x6 matrix. But since $x$ has the dimension 7x1 the calculation finally yields a $\Sigma_{ij}^{t}$ of dimension 7x7 so I guess I'd somehow have to replace $x$ by its minimal parametrization $\log{x}$ which is 6x1. But how do I adapt the process model then? $\endgroup$
    – morph
    May 22, 2016 at 11:19
  • $\begingroup$ @morph In the original question, you were asking about graph-based SLAM. However, your new question seems like it asking about a Kalman filter. If this is the case, perhaps you should ask it as a new question because it is related but not directly a follow up to the original question. $\endgroup$
    – kamek
    May 23, 2016 at 7:46
  • $\begingroup$ Is that true? I thought that I am talking about the SLAM-frontend, while graph-based SLAM relates to the SLAM-backend, doesn't it? I also think that my question in the comment is also strongly related to the manifold topic, that's why I asked it here. $\endgroup$
    – morph
    May 23, 2016 at 8:11

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge that you have read and understand our privacy policy and code of conduct.

Not the answer you're looking for? Browse other questions tagged or ask your own question.