More accurate displacement covariance by including cross-correlation between poses

[miguelcastillon]

Hi José Luis,

In our lab we have recently come up with a function that allows us to accurately recover the covariance of the displacement between two poses. I'm opening this issue because I thought it might be of interest to the library. Let me know what you think about it.

Context: I'm currently working with pose graph SLAM in underwater scenarios. It is not uncommon that underwater robots have very reliable inertial navigation systems (INSs). These sensors usually have very low drift because of their high-end gyroscopes and because they can directly measure velocity using Doppler Velocity Logs (DVLs). Therefore, we can safely assume that the displacement between two poses calculated by the INS has very low errors.

Taking this into account, a very convenient way of building the graph is adding these displacements as between_factors (without the need of integrating inertial measurements ourselves). However, now the question arises: how do we characterize the covariance matrices of these displacements?

We assume we have two poses (with their covariance matrices) that we have read from the INS: $x_{k-1}$ and $x_k$. We can compute the displacement as:
$\Delta x_k = \ominus x_{k-1} \oplus x_k$

Using MRPT, we could do:

mrpt::poses::CPose3DPDFGaussian pose_k_1, pose_k = ... // Read from INS
mrpt::poses::CPose3DPDFGaussian displacement = - pose_k_1 + pose_k;

However, this is no good, because the resulting covariance of the variable displacement would be larger than any of the matrices of the poses. Instead, the displacement covariance should exactly reflect the increment in uncertainty from $x_{k-1}$ to $x_k$. Said differently, this test should hold true:

mrpt::poses::CPose3DPDFGaussian pose_k_1 = random_initialization;
mrpt::poses::CPose3DPDFGaussian displacement_ground_truth = random_initialization;
mrpt::poses::CPose3DPDFGaussian pose_k = pose_k_1 + displacement_ground_truth;

mrpt::poses::CPose3DPDFGaussian displacement = pose_k_1.relativeDisplacement(pose_k);

Doing this, displacement and displacement_ground_truth should be the same. The reason displacement = - pose_k_1 + pose_k is not correct is that $x_{k-1}$ and $x_k$ are strongly correlated: $x_k= x_{k-1} \oplus \Delta x_k$.

We have found out that we can know exactly the cross-correlation term between pairs of poses, which makes the relativeDisplacement function return the exact value for the displacement covariance. The explanation is a bit too long to write here, but I just wanted to check if you see this as a nice feature to have in the library (maybe it's something that you already do and I'm not aware of). If so, I could do a PR with it.

Best regards,
Miguel

[jlblancoc]

Hi Miguel!
So good to bring that discussion here 👍
Indeed, that's one of the main scopes of mrpt::poses, so probabilistic manipulation of poses totally fit the library.

I still don't get the point of why you say that : $displacement = (\ominus pose_{k-1}) \oplus pose_k$ would be incorrect... if we are assuming Gaussian distributions, and linear propagation, that formula is correct for retrieving the displacement mean of the Gaussian. The covariances / correlations shouldn't play a role there... (a different story is whether the linear approximation is good enough, etc.)

Please, give it a try to this existing method https://github.com/MRPT/mrpt/blob/develop/libs/poses/src/CPose3DPDFGaussian.cpp#L476 which should do what you are looking for, i.e. full covariance of the pose increment between two poses, each with its own full covariance.

Thinking it twice, perhaps your point is that a new -= operator should exist for the case when we knew the cross-correlation block between the two poses, isn't it? In that case, you are right, that case is not implemented here. It was done in mrpt::slam for relative landmarks positions (data-association,...), but not directly for poses.

If that's your idea, then a new function + unit test exposing that functionality would be more than welcome, of course! ;-)
Afterwards, it could be also exposed to ROS users here too, defining an additional argument for the cross-correlation.

[miguelcastillon]

Yes, doing displacement = - pose_k_1 + pose_k gives the correct mean displacement.

However, the covariance is way too large because the cross-correlation is not taken into account (see eq. (24) here). The nice thing is that we can actually know the cross-correlation block given the two poses, it is not an input argument to the function. I will create the PR and we can discuss it there with the code.

Cheers,
Miguel

[jlblancoc]

Closed by #1243