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

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.

## 1 Answer

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.

• I am searching for a solution to determine poses for a kind of free moving camera but without using visual odometry. I thougt a composition of poses related in a graph could be understood as a map. I read that one can construct pose graphs both with poses $x$ and landmarks $l$ or only with poses $x$. That's why I tried to implement the whole SLAM approach. Can you explain why pose graph SLAM won't work? The information I insert into the system (the graph) is always a pose, no matter if I compute it using an imu or visual odometry, isn't it? Is there a difference for the system? Mar 19, 2016 at 15:05
• Another reason for using SLAM is that I want to make use of loop closures to correct my estimates - this time using cross-correlation between images from my camera-like device. If I have to abandon the SLAM approach, is it possible to do localization based on a graph to make use of loop closures? Mar 19, 2016 at 15:07
• a note on my 5th question: in the mrpt-graph-slam implementation I saw that it is possible to either represent edge constraints explicitly as transforms or as means of a PDF (compare classes CPose3DQuat and CPose3DQuatPDFGausianInf) Mar 19, 2016 at 15:43
• and overall I was thinking about how the SLAM frontend to my system acutally looks like if the backend is the optimization you explained in your answer of question 5 Mar 19, 2016 at 15:44
• @morph Implementing a SLAM strategy using only an IMU sensor doesn't sound realistic, since this doesn't give you any information about the environment that the robot operates in (therefore no data available for the mapping procedure). Indeed one can implement pose-graph only (without keeping track of landmark positions and covariances) but this involves storing sensor information in the keyframes/nodes of the graph, e.g. keep the 2DRangeScan recorded in each node. Aligning these observations from the prior robot positions (using ICP?) one can add edges between (nearby) nodes/close loops etc. Jul 20, 2016 at 7:43