Added correct displacement covariance calculation between two poses with cross-correlation

[miguelcastillon]

Changed apps/libraries

• modified libs/poses/include/mrpt/poses/CPose3DQuatPDFGaussian.h
• modified libs/poses/src/CPose3DQuatPDFGaussian.cpp

PR Description

• This PR Closes issue #1242

When we want to compute the displacement $\Delta xk$ between two poses $x{k-1}$ and $x_k$, we have to take into account the cross-correlation between both poses in order to get a correct value for the covariance matrix of the displacement. Mathematically:

where C is the cross-correlation term. It can be computed as

where the cross-correlation covariance matrix is

Special care has to be taken to ensure that all the jacobians are evaluated at the proper points:

I have created the function in the library but I'm unsure about a few things:

• I only added the relativeDisplacement function for CPose3DQuatPDFGaussian, but I guess it should also be available for CPose3DPDFGaussian.
• I created the relativeDisplacement function because I don't see how it could be done via -=, as you suggested in issue #1242.
• I don't know where to write these equations to document the function (pasting the latex source code in the library is not very readable).
• If you consider that a more extensive explanation of the equations is needed, just let me know.
• I'm unsure whether the function needs some asserts.
• I don't know how to write the unit test, any tips?

Cheers, Miguel

---

I acknowledge to have:

• Read the CONTRIBUTING page
• Updated doc/doxygen-pages/changeLog_doc.h to describe these changes (if applicable)
• Updated AUTHORS (if applicable)

(Notify: @MRPT/owners )

[codecov[bot]]

Codecov Report

Merging #1243 (0cf05ef) into develop (7ec61a3) will increase coverage by 0.02%. The diff coverage is 90.62%.

@@             Coverage Diff             @@
##           develop    #1243      +/-   ##
===========================================
+ Coverage    39.24%   39.27%   +0.02%     
===========================================
  Files         1292     1292              
  Lines       109853   109913      +60     
===========================================
+ Hits         43116    43170      +54     
- Misses       66737    66743       +6     

Impacted Files	Coverage Δ
.../poses/include/mrpt/poses/CPose3DQuatPDFGaussian.h	71.42% <ø> (ø)
libs/poses/src/CPose3DQuatPDFGaussian_unittest.cpp	80.95% <75.00%> (-0.58%)	:arrow_down:
libs/poses/src/CPose3DQuatPDFGaussian.cpp	50.25% <100.00%> (+10.98%)	:arrow_up:

---

Continue to review full report at Codecov.

Legend - Click here to learn more Δ = absolute <relative> (impact), ø = not affected, ? = missing data Powered by Codecov. Last update 7ec61a3...0cf05ef. Read the comment docs.

[jlblancoc]

Awesome!

I will have to delay studying this in depth for a few days, since other urgent fixes are needed on the ROS wrappers which are blocking a release there.

In the meanwhile, a great way to do a proper unit test (and/or a proof-of-concept demo) would be in this case to just use MonteCarlo:

• Draw N (large) samples from two SE(3) (CPose3DPDFGaussian) with different covariance matrices each; the first N will be "A", the second set, "B".
• For each pair, compute "diff = B \oplus A".
• Evaluate the Gaussian fit for "diff" samples (it could be done in C++ converting a CPose3DPDFParticles into CPose3DPDFGaussian),
• Compare with the new formula!

(Alternatively, draw the samples from "A" and the "diff", then compute "B" from them??)

If it seems too complex for the C++, try it first in matlab?

Now I see your point after reading the PR description. Perhaps it could be stated more directly starting with a generative model, like "pk = p{k-1} \oplus d", with arbitrary covariances for "p_{k-1}" and "d". Then, computing the cov. of "p_k" and stacking "pk" and "p{k-1}" together leads to the 2x2 block matrix whose diagonal block is your cross-correlation... that's the idea, right?

It makes me wonder whether it then might be useful to keep the older version of the "-=" operation. I need to revisit that area, since many numerical tests and unit tests have been done over it and nothing suspicious was found...

Cheers,

[miguelcastillon]

Hi!

• I have created and pushed a unit test for relativeDisplacement but I can't seem to find how to build it and run it. I have tried following the documentation but I have not succeeded (No tests were found!!!). I compiled the library using cmake -DBUILD_TESTING=ON ...
  • I have implemented the test that I used to check that the function worked when I wrote it. It is not exactly what you proposed above, I can change it if you think that would be better.
• Now I see how you encode the operation - pose_1 + pose_2 into pose2 - pose_1, I was not used to it. Then the function relativeDisplacement should probably be operator- or operator-=.
  • I think that the old unit test for that operation works because the cross-correlation is found numerically via transform_gaussian_linear, but that isn't done by default when calling -= (if I have understood it correctly).
  • With this PR that cross-correlation term is computed analytically and is always done by default.
  • I can't find any situation in which you could do -= between two poses without taking the cross-correlation into account, mainly because the displacement that you want to compute is precisely what relates both poses.

Cheers, Miguel

[jlblancoc]

Great! I could run your new test with just make run_tests_mrpt_poses (make test also works but the console output is much terser by default), good!

Yes, the "relativeDisplacement" is actually virtually equivalent to our operator- (and/or operator -=).

• I think that the old unit test for that operation works because the cross-correlation is found numerically via transform_gaussian_linear, but that isn't done by default when calling -= (if I have understood it correctly).

I wrote that ages ago... :-) But looking a few lines above, it seems my assumption there was uncorrelated poses, since the 14x14 covariance is only filled in in the two diagonal blocks.

I'm thinking that perhaps it might exists the case where the current version (no cross-correlation) makes sense: in those where the two absolute poses come from very different sources, or are very distant in a SLAM trajectory, etc. Then, their "noise" / "uncertainty" could be interpreted to be independent from each other. I think your case arises because you are thinking of a kind of incremental odometry pose accumulation, right? When pose "b" naturally comes from adding an increment (unknown) to the old pose "a".

So, perhaps we could keep both versions, don't you agree? Please, feel free of adding some notes to the current "-" operator specifying that it assumes statistical independence between the two variables, and that in case "b=a \oplus incr" is the direct generative model , your new function should be used instead, with a Doxygen entry like \sa YOUR_NEW_FUNCTION.

Now that I read your unit test, yes, it totally seems we started from different assumptions, but both make sense, IMO

Feel free of changing the name of the method if you think of anything better, adding some comments, etc. and let me know when you are ok with merging this one.

[miguelcastillon]

OK, I changed the function name and added some notes.

I feel like I did this PR a bit chaotic, I'll make a summary here:

• The function (inverseCompositionCrossCorrelation) adds the cross-correlation term (written at the beginning of this PR) to the computation of the displacement covariance.
  • In the code I added a link to this PR because writing the equations there did not seem readable. However, I can change it to another reference.
  • This function only belongs to class CPose3DQuatPDFGaussian. Should it be callable from other similar classes?
  • I added an assert because a very particular case can in theory happen. If you call the function inverseCompositionCrossCorrelation to compute the displacement between pose_1 and pose_2, the resulting covariance matrix might not be positive semi-definite (which is a condition of covariance matrices).
  • This happens (I think) when the covariance of pose_2 is smaller than the covariance of pose_1 along any axis.
  • The resulting matrix is mathematically sound but is not a covariance matrix.
  • I added an assert to ensure that the diagonal terms of the resulting covariance matrix are non-negative. This is not sufficient for a matrix to be PSD, but it might be a reliable indicator in many cases. What do you think?
• We're keeping operator- for the case of independent poses.
  • I agree that it is better to keep it for completeness.
  • However, I still can't seem to find an example in which it is necessary to compute the displacement (and the displacement covariance) between two independent poses. You can compute the displacement covariance assuming independence between pose_1 and pose_2, but then you can't use the result to do pose_2 = pose_1 + displacement to compute the covariance of pose_2, because the result will be wrong. So I wonder in which case you would use the displacement covariance between two independent poses? I'm aware that I might be overlooking some features of the back-end in which it could be needed.

I'm OK with merging this PR when you consider it fit but I'm not in a rush to do it, so feel free to add comments/suggestions :)

Cheers, Miguel

[jolting]

Would it make sense to have a special displacement type, instead of making that just use poses for everything?

Like in chrono, time_points vs durations.

[jlblancoc]

Great, point, Hunter. The comparison with std::chrono is probably a good one, although no one in the literature makes any distinction about SE(3) poses depending on their "source"... It would be probably confusing for users and complicate the possibilities (what about composing a relativeDisplacement \oplus pose, or the other way around?). A cosmetic solution would be a syntactic sugar like:

using RelativePose = CPose3D;
using AbsolutePose = CPose3D;

just to make the code API more explicit... (??).

@miguelcastillon This "what's behind the scenes" discussion makes me think... e.g. say we have a external positioning system (cameras, etc.) giving the SE(3) pose of a robot A and another source of pose as its wheels odometry B. Their errors are uncorrelated, so there's no cross-correlation. If we then use the "old" $\ominus$ operator to find out $D=B \ominus A$, I think it would be correct. If we then a posteriori compute $B' = A \oplus D$, I think that this new B' should be indeed correlated with $A$, but that doesn't change the fact that the original random variable $B$ was not.

It's rather philosophical probably given this point XD

I'll merge as is now, and fix the tiny format errors CI is complaining about later.

A big thanks for the PR!

[jolting]

I could see building a wrapper for this sort of thing. IDK, maybe something like this.

template <typename POSE_TYPE>
class RelativePoseImpl
{
private:
  POSE_TYPE m_pose;
};

template <typename POSE_SOURCE, typename POSE_TYPE>
class AbsolutePoseImpl
{
public:
  RelativePoseImpl<POSE_TYPE> operator-(const AbsolutePoseImpl<POSE_SOURCE, POSE_TYPE>&);
  AbsolutePoseImpl<POSE_SOURCE, POSE_TYPE> operator+(const RelativePoseImpl<POSE_TYPE>&);

private:
  POSE_TYPE m_pose;
};

class PoseSource
{
public:
  using PoseType = CPose3D;
  using AbsolutePose = AbsolutePoseImpl<PoseSource, PoseType>;
  using RelativePose = RelativePoseImpl<PoseType>;
};