Pose-Graph-SLAM: How to create edges if only IMU-odometry is given?

Question

I want to estimate the poses of a vehicle at certain key frames. The only sensor information I can use is from an IMU which yields translational acceleration and orientation measurments. I obtain a 7D pose, i.e. 3D position vector + unit quaternion orientation, if I integrate the translational acceleration twice and propagate the orientation measurements.

If I want to add a new edge to the graph I need a constraint for that edge. In general, for pose graphs this constraint represents a relational transformation $z_{ij}$ between the vertex positions $x_i$ and $x_j$ that are connected by the edge.

Comparing my case to the literature the following questions arised:

1. How do I calculate a prediction $\hat{z}_{ij}$ which I can compare to a measurement $z_{ij}$ when computing the edge error? Initially, I understood that graph slam models the vertex poses as gaussian distributed variables and thus a prediction is simply calculated by $\hat{z}_{ij}=x_i^{-1} x_j$.

2. How do I calculate the information (preferred) or covariance matrix?

3. How and when do I update the information matrices? During optimization? Or only at edge creation? At loop closure?

4. I read about the chi-square distribution and how it relates to the Mahalanobis distance. But how is it involved in the above steps?

5. Studying current implementations (e.g. mrpt-graph-slam or g2o) I didn't really discover how predictions (or any probability density function) is involved. In contrast, I was even more confused when reading the mrpt-graph-slam example where one can choose between raw poses and poses which are treated as means of a probability distribution.

Answer 1

First off, it doesn't sound like you're actually doing SLAM. You didn't mention an exteroceptive sensor (e.g., laser, camera) that actually maps the environment. With just an IMU, you are doing localization, or more specifically, dead-reckoning. With just an IMU, there is no way to actually implement pose-graph SLAM in its usual formulation.

That being said, I'll try to answer your questions pretending you have some exteroceptive sensor that is measuring the environment, with which you can generate pose-transformation measurements.

1. Let's say the nodes in the graph are $x=(x_1,...x_n)$, where $x_i = (p_i, q_i)$ is the position $p_i$ and orientation $q_i$ of the node (6 DOF). Then the predicted observation between them is simply their relative position and relative orientation. For example, if the orientation is parameterized as a unit quaternion, $\hat{z}_{ij} = (p_j - p_i, \log(q^{-1}_i q_j)).$ For an explanation as to why the $\log$ is there, see one of my other answers.

2. In pose-graph SLAM, the information matrix is the "Hessian" $J^\top \Sigma J$ (not actually the Hessian, just an approximation), where $J$ is the Jacobian of your errors with respect to your state, and $\Sigma$ is the information matrix of your measurements.

3. The information matrix changes every time you add a node, every time you add an edge, and every time you optimize the pose-graph. You can calculate it at any time by calculating $J^\top \Sigma J$.

4. Your cost function that you are trying to minimize represents the Mahalanobis distance between your predicted and actual measurements, which has a chi-square distribution.

5. Not really sure exactly what you mean here. Note that you are trying to maximize the likelihood of your observations, which means you are trying to find the state the maximizes the PDFs of your measurements. How do we do this? We take the negative log of the Gaussian PDF, which forms our cost function. For example, for a 1-D linear system, the cost of a measurement is the negative log of the PDF of the measurement:

$$ f(z|x,\sigma) = \frac{1}{\sigma\sqrt{2\pi}}\exp\left(-\frac{\left(z-Hx\right)^2}{2\sigma^2}\right) $$

$$ -\ln(f(z|x,\sigma)) = \eta\frac{\left(z-Hx\right)^2}{2\sigma^2} $$

Note that $\eta$ is a normalization constant that does not depend on $x$, therefore it has no effect on the optimization. The goal of pose-graph SLAM is to find $x$ that minimizes $-\ln(f(z|x,\sigma))$ for measurements simultaneously. If you consider the measurements independent, then the cost is just the sum of the negative log-likelihoods.