4
$\begingroup$

This question is strongly related to my other question over here.

I am estimating 6-DOF poses $x_{i}$ of a trajectory using a graph-based SLAM approach. The estimation is based on 6-DOF transformation measurements $z_{ij}$ with uncertainty $\Sigma_{ij}$ which connect the poses.

To avoid singularities I represent both poses and transforms with a 7x1 vector consisting of a 3D-vector and a unit-quaternion:

$$x_{i} = \left( \begin{matrix} t \\ q \end{matrix} \right)$$

The optimization yields 6x1 manifold increment vectors

$$ \Delta \tilde{x}_i = \left( \begin{matrix} t \\ log(q) \end{matrix} \right)$$

which are applied to the pose estimates after each optimization iteration:

$$ x_i \leftarrow x_i \boxplus \Delta \tilde{x}_i$$

The uncertainty gets involved during the hessian update in the optimization step:

$$ \tilde{H}_{[ii]} += \tilde{A}_{ij}^T \Sigma_{ij}^{-1} \tilde{A}_{ij} $$

where

$$ \tilde{A}_{ij} \leftarrow A_{ij} M_{i} = \frac{\partial e_{ij}(x)}{\partial x_i} \frac{\partial x_i \boxplus \Delta \tilde{x}_i}{\partial \Delta x_i} |_{\Delta \tilde{x}_i = 0}$$

and

$$ e_{ij} = log \left( (x_{j} \ominus x_{i}) \ominus z_{ij} \right) $$

is the error function between a measurement $z_{ij}$ and its estimate $\hat{z}_{ij} = x_j \ominus x_i$. Since $\tilde{A}_{ij}$ is a 6x6 matrix and we're optimizing for 6-DOF $\Sigma_{ij}$ is also a 6x6 matrix.

Based on IMU measurements of acceleration $a$ and rotational velocity $\omega$ one can build up a 6x6 sensor noise matrix

$$ \Sigma_{sensor} = \left( \begin{matrix} \sigma_{a}^2 & 0 \\ 0 & \sigma_{\omega}^2 \end{matrix} \right) $$

Further we have a process model which integrates acceleration twice and rotational velocity once to obtain a pose measurement.

To properly model the uncertainty both sensor noise and integration noise have to be considered (anything else?). Thus, I want to calculate the uncertainty as

$$ \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_{i}^{t}}{\partial x_{i}^{t-1}}$ and $J_{process} = \frac{\partial x_{i}^{t}}{\partial \xi_{i}^{t}}$ and current measurement $\xi{i}^{t} = [a,\omega]$.

According to this formula $\Sigma_{ij}$ is a 7x7 matrix, but I need a 6x6 matrix instead. I think I have to include a manifold projection somewhere, but how?

For further details take a look at the following publication, especially at their algorithm 2:

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.

For a similar calculation of the uncertainty take a look at the end of section III A. in:

Corominas Murtra, Andreu, and Josep M. Mirats Tur. "IMU and cable encoder data fusion for in-pipe mobile robot localization." Technologies for Practical Robot Applications (TePRA), 2013 IEEE International Conference on. IEEE, 2013.

.. or section III A. and IV A. in:

Ila, Viorela, Josep M. Porta, and Juan Andrade-Cetto. "Information-based compact Pose SLAM." Robotics, IEEE Transactions on 26.1 (2010): 78-93.

Improve this question
$\endgroup$
6
  • $\begingroup$ Each edge in the graph has an error function associated with a measurement. In your question, what is the error function, and what is the measurement? Are you integrating the IMU data for a while to get a change in pose, and then using that as the measurement? That is, what is $z_{ij}$ in your question? $\endgroup$
    – kamek
    May 23, 2016 at 18:15
  • $\begingroup$ @kamek I updated the question: $z_{ij}$ is a measurement of the pose. And yes, it is calculated based on an odometry measurement $\xi_{i}$ of acceleration and rotaional velocity which is integrated twice or once, respectively. $\endgroup$
    – morph
    May 23, 2016 at 20:33
  • $\begingroup$ Still not sure I understand. Is there a new $z_{ij}$ each time there is an IMU measurement? If so, what is $\Sigma_{ij}^{t-1}$? Put differently, at what point do you add a new node to the graph? Are you integrating the IMU data for, say, a few seconds and then adding a node to the graph? If so, are you resetting the covariance back to zero each time after you add a node? Do you have another sensor or are you just integrating IMU data. If that's all your doing graph-based SLAM isn't going to improve the dead-reckoning uncertainty (i.e., without loop closures). $\endgroup$
    – kamek
    May 23, 2016 at 21:29
  • $\begingroup$ @kamek good question! since I estimate the pose of an imaging device I create a new $z_{ij}$ for every new image frame. During the frames I integrate a pose $x^t$ based on the IMU measurements in between (IMU@100 Hz, images@25 Hz). The $z_{ij}$ is the transformation between $x^t$ and the current reference pose $x_{ref}$. $x_{ref}$ is initialized with $0$ and replaced with $x^t$ after a new $z_{ij}$ was calculated. $\Sigma_{ij}^{t}$ is updated with every new IMU measurement, just like $x^t$ and thereby also describes the covariance of a measurement $z_{ij}$ calculated from $x^t \ominus x_{ref}$ $\endgroup$
    – morph
    May 23, 2016 at 21:52
  • $\begingroup$ @kamek and yes, basically this behavior won't improve the dead-reckoning, until I detect a loop closure using the image data. And that's exactly what I'm doing. $\endgroup$
    – morph
    May 23, 2016 at 21:53

1 Answer 1

Reset to default
1
$\begingroup$

Because a the pose has six degrees-of-freedom, the covariance matrix representing its uncertainty should be $6 \times 6$. To determine how the covariance matrix propagates after each new IMU measurement, you must determine the error state propagation equation, where the error is a minimal parameterization of the error in your state. How to do this for a rotation parameterized as a unit quaternion is explained in detail in Section 2 of Indirect Kalman Filter for 3D Attitude Estimation by Trawny. Note that this implementation explicitly estimates the gyroscope bias as part of the state vector. You would need to replace that with the position and add your accelerometer measurements. In fact, I recommend reading that document (or at least the first two sections) very carefully, I learned a lot the first time I read it.

Improve this answer
$\endgroup$
2
  • $\begingroup$ I have just read your recommended document. I still don't understand the difference between their approach and mine (which is inspired by Murtra). The calculation of $\Sigma_{ij}^t$ has the same structure for both approaches, as it has a "running" component and a fixed one, i.e. the sensor noise. Murtra explicitly makes use of Jacobians. As Trawny's calculations are based on the error development, i.e. he analyses the derivative of the error, it seems to me like he's working with jacobians as well, but he doesn't name it. Do both actually follow the same approach using different "vocabulary"? $\endgroup$
    – morph
    May 24, 2016 at 10:45
  • $\begingroup$ It is very important to note that the error parameterization by Trawny is a minimal parameterization. From skimming Mutra, it looks like Jacobians are taken with respect to the quaternion? As in, with respect to each of the four terms in the quaternion? That does not take into account that these four terms only represent three degrees of freedom. If you consider four quaternion parameters independent and their uncertainty with a $4 \times 4$ covariance matrix, you will run into numerical issues (the covariance matrix may be singular). $\endgroup$
    – kamek
    May 24, 2016 at 11:37

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.