RGB-D SLAM - Compute Information Matrix

Question

currently im working on a RGB-D SLAM with a Kinect v1 Camera. In the front-end the SLAM estimates the pose with Ransac as an initial guess for the ICP. With the pose estimation i transform the pointcloud to a pointcloud-scene which represents my map.

To smooth the map im trying to implement a graph optimizing algorithm (g2o). Until now, there is no graph representation in my frontend, so i started to integrate that.

Im trying to build a .g2o file with the following fromat:

VERTEX_SE3 i x y z qx qy qz qw

where x, y, z is the translation and qx, qy, qz, qw ist the Rotation in respect to the initial coordinate system. And,

EDGE_SE3 observed_vertex_id observing_vertex_id x y z qx, qy, qz, qw inf_11 inf_12 .. inf_16 inf_22 .. inf_66

Translation and rotation for the edge is the pose estimate that i compute with Ransac and ICP (visual odometry).

Now im getting stuck with the information matrix. I read the chapter 3.4 THE INFORMATION FILTER in Thrun's Probabolistic Robotics and several threads in this forum, such as:

The relationship between point cloud maps and graph maps

and

information filter instead of kalman filter approach

From the second link, i got this here.

The covariance update $$P_{+} = (I-KH)P$$ can be expanded by the definition of K to be

$$ P_{+} = P - KHP$$ $$ P_{+} = P - PH^T (HPH^T+R)^{-1} HP$$

Now apply the matrix inversion lemma, and we have:

$$P_{+} = P - PH^T (HPH^T+R)^{-1} HP$$ $$ P_{+} = (P^{-1} + H^TR^{-1}H)^{-1}$$

Which implies: $$ P_{+}^{-1} = P^{-1} + H^TR^{-1}H$$

The term $P^{-1}$ is called the prior information,$$H^TR^{-1}H$$ is the sensor information (inverse of sensor variance), and this gives us $P^{-1}_+$, which is the posterior information.

Could you please point this out for me. What data do i need to compute the information matrix?

Answer 1

The information matrix is the inverse of the covariance matrix. In this case, the covariance is over the variables (x,y,z,qx,qy,qz). It is assumed that your quaternion is normalized to be unit magnitude.

You should be able to get an estimate of the information matrix from the ICP.

Edit:

In general the covariance estimate can be found by the following procedure.

• Set I = 0 (I will be the information matrix for the system)
• For each input.
  • Get the covariance (must be estimated somehow empirically).
  • Invert the covariance to get the information W
  • Calculate the Jacobian J of your error in the output space with respect to the input (see http://en.wikipedia.org/wiki/Jacobian_matrix_and_determinant)
  • Transform the information into the error space via X = J^T * W * J
  • Set I = I + X
• Invert I to get the covariance S of the output

Edit 2:

The covariance formula that you linked to is the covariance of the points in the point cloud. For the mapping, you want the covariance estimate for the estimated transform which is not at all the same thing. It is a little difficult to explain. Try looking at http://en.wikipedia.org/wiki/Non-linear_least_squares . In your case the x_i would be points in P_A, y_i would be points in P_B, f would be the function that transforms points from P_A to P_B (i.e. f(x_i, T) = T(x_i) where T(x) is applying T to x), and beta would be the transform parameters. In the last equation in the theory section, the J^TWJ term is the estimated information for the parameters. Inverting this provides a covariance estimate (this is based on the Cramer-Rao lower bound, see http://en.wikipedia.org/wiki/Cram%C3%A9r%E2%80%93Rao_bound). For the typical case of independent measurements, the J^T * W * J term ends up just being a sum of J_i^T * W_i * J_i for each measurement i.

Answer 2

From the wiki of the g2o file format:

The information matrix or precision matrix which represent the uncertainty of the measurement error is the inverse of the covariance matrix. Hence, it is symmetric and positive semi-definite.

Unfortunately there is no mentioning of the representation for the covariance matrix for that file format, that I could find. My guess is that it is a compact representation, so either a $6\times6$ matrix using $[x, y, z, \psi, \phi, \theta]$ (euler angles) or $[x, y, z, r_1, r_2, r_3]$ (scaled axis of rotation).

Answer 3

Your sensor does not measure state

Kinect does not provide any "measurements" of state. It scans the environment and provides a point cloud. To use this to localize a robot, you must compare currently-sensed point clouds to past-sensed point clouds to provide a "correction" term. ICP (as mentioned) will provide such a "correction" term. That correction is the "innovation" in the EKF/EIF. To calculate the covariance / information matrix, you need, at minimum, the variance (if one-dimensional) or covariance (if 2-or-more-dimensional) of the innovation.

But you don't want to do that, actually. You want to do pose-graph optimization to smooth the maps. For that, you simply need to provide a state estimate of the position of the kinect sensor at the time of each scan. Imagine an interative process like this:

1. Take the first scan, save it with sensor position $x_0=0,0$
2. Take another scan, and use ICP to figure out how the sensor has moved between scans $\delta x$
3. Save that scan, and set $x_1=\delta x+x_{0}$

Now, repeat 2 and 3, taking scans, calculating $\delta_x$ and setting $x_i=\delta x + x_{i-1}$.

You have a pose graph / set of key frames. The positions are the poses, and the features in the environment are the ... features. Pose graph optimization requires at minimum these two things plus some measure of uncertainty of the poses. You can easily make up these values to get a working implementation (e.g., $\Sigma_i=c\cdot I$). Then, the information matrix is .... $\frac{1}{c}I$. Is this OK? well, the only better way to do it is to calculate the variance of the innovation above, and somehow "calibrate" the "position sensor" that is given by the ICP scan registration process.

Repeating again

The optimization problem is to find the correct set of $\delta x$ values for all $i=1...n$, but, you already did that in 2-3. So your pose graph optimization will not do anything new unless you iteratively match all pairwise scans and update the correction factors. This will only work if subsequent scans overlap scans from more than one step in the prior set of scans ... e.g., if you have a loop closure.

Repeating another way

Ultimately, the "right" way to do this is to form a giant optimization problem where you find the set of all $\delta x$ to minimize the error in the scan matches, subject to not straying "too far" from the initial pose estimates. Here "too far" is encoded by your "made up" uncertainty on the poses, and you'll have to tune how much you weight priors.

you may find this restatement of the above details better