# Covariance and optimization

I am trying to build a map containing lamps as landmarks. I drive around with a robot and a monocular camera looking to the ceiling.

The first step is detect the edges of each observed rectangular lamp and save the position in pixels and also the current position from odometry of the robot. After the lamp disappears from the field of view, there is enough base-line to do a 3D reconstruction based on structure from motion. Once this reconstruction is done there will be uncertainty in the position of the lamps that can be modelled by covariance. Imagine if the robot was driving for a while, its own position estimated from odometry will also have a relatively high incertitude, how can I integrate all of those incertitudes together in the final covariance matrix of the position of each lamp?

If I understand well there would be the following covariances:

• noise from camera
• Inaccurate camera calibration matrix
• inaccurate result from optimization
• drift in odometry

My goal is to manually do loop closure using for example g2o (graph optimization) and for that I think correct covariances are needed for each point.

This is the basic slam problem. You have to find out (model) how the uncertainty of the robot affects the uncertainty of the landmarks, and visa-versa. This is done using the cross-correlation terms of the uncertainty. Or, the covariance. Those four things you referred to as covariances are actually variances. The covariance describes how the uncertainties in one thing bleed over to create uncertainty in the other things. This sounds terrible, but actually, if you reduce uncertainty in one thing, then the fact that two things co-vary allows you to reduce uncertainty in two things.

See, when you discuss covariance of the robot's position, what you mean is something like a 2x2 matrix. The matrix has multiple terms, two variances in the principal x-y directions, and the covariance of the x uncertainty and the y uncertainty. So you're already doing SLAM. Sorta.

What you have to do for $N$ lamps in $2D$ space and 1 robot moving in $2D$, is construct the giant $2\times (N+1)$ by $2\times (N+1)$ matrix. How to do this is mechanically straightforward, but an intuitive understanding is difficult to build up. I'm sure someone will come along that has a better mechanical answer for you. But until then, you might check out Probabilistic Robotics, by Thrun.

UPDATE:

For example. Let's say you have one landmark, and one robot. Both are on the line, and so the state is simply their $x$ position. (This greatly simplifies the problem). Then, let's say the robot measures the distance (with direction) to the landmark. The the measurement function (which describes what the robot measures) is $$h=x_l - x_r$$.

We'll use the EKF update equations to find the covariance of the robot-landmark state. Note, this will be a $2\times 2$ matrix, given the discussion above. We're not really limited to the EKF update equations. Most estimators use the same equation to estimate the covariance of the state. If you really want to know more about this, read up on the Fisher Information Matrix, which is the gold standard for covariance estimation.

Back to the topic at hand.

The full state covariance matrix after the robot moves with some uncertain actuation is given by:

$$P = F^T P F + G$$

and after the measurement, is given by

$$P = \left( P^{-1} + H^T R^{-1} H \right)^{-1}$$

Here, $R$ is the measurement noise, $H$ is the jacobian of the measurement function with respect to the state, $F$ is the jacobian of the update function (the kinematics) with respect to the state, and $G$ is the noise matrix for the controls.

In our case,

$$H =\begin{bmatrix} \frac{\partial h}{x_r}, \frac{\partial h}{x_l} \end{bmatrix}\\ =\begin{bmatrix} -1, 1 \end{bmatrix}\\$$

Suppose the robot's position is known up to variance $\sigma_r^2$ and the measurement is corrupted by zero-mean noise with variance $\sigma_s^2$. Then, when the landmark is measured, (assuming a diagonal starting covariance)

$$P^{-1} = \begin{bmatrix} \frac{1}{\sigma_r^2}, 0\\ 0, \frac{1}{\sigma_l^2} \end{bmatrix} + \begin{bmatrix} -1\\ 1 \end{bmatrix} \frac{1}{\sigma_s^2} \begin{bmatrix} -1, 1 \end{bmatrix}$$

Simplifying, $$P^{-1}= \begin{bmatrix} \frac{1}{\sigma_r^2}, 0\\ 0, \frac{1}{\sigma_l^2} \end{bmatrix}+ \frac{1}{\sigma_s^2} \begin{bmatrix} 1, -1\\-1,1 \end{bmatrix}$$ Implying, $$P= \begin{bmatrix} \frac{1}{\sigma_r^2} +\frac{1}{\sigma_s^2}& -\frac{1}{\sigma_s^2}\\ -\frac{1}{\sigma_s^2}& \frac{1}{\sigma_l^2}+\frac{1}{\sigma_s^2} \end{bmatrix}^{-1}$$

The previous defines a computational way to calculate the covariance after a measurement. A similar method using the control function to get the matrix $F$ will get you the update step.

To get this in closed form, just carry the partial derivatives through.

Or, a better way: In general, the matrix on the right is known as the Fisher Information, with the caveat that the matrix $H$ is not linearized at the true target location. To construct an arbitrary covariance matrix from scratch, you can do so by noting that the FIM has each i,j element given by (assuming gaussian noise), for state vector $x$.

$$\begin{bmatrix} \frac{\partial h}{x(i)}\ln \mathcal{L}(x) \frac{\partial h}{x(j)}\ln \mathcal{L}(x) \end {bmatrix}$$

In the previous, $\mathcal{L}(\cdot)$ is the liklihood function of the measurements. In this case, a Gaussian. This derivation is covered in this thesis and also this one (with the full disclosure that the second one is mine).

• Thanks for the explanation. I have been reading the book again and again, it is very helpful but it doesn't really show how to construct such matrices. As for me it is not yet mechanically straightforward, do you have any resources that help understanding these matrices? Sep 28 '15 at 11:51
• Yeah sure! I'll update the answer shortly Sep 28 '15 at 20:17
• Thanks. In my case, I can't really observe directly the position of the marker and that is why I observe it from multiple positions and then do structure from motion to get the depth information and then finally x and y. How could I fit this extra step into the whole thing. Or should I just ignore it for now and try to describe it implicitely as extra noise in R? Oct 14 '15 at 15:56
• Of course you can't observe directly. Structure from motion should output co variances. If not, use the selected viewpoints and some estimate of sensor noise to estimate the resulting co variances. Oct 14 '15 at 19:27