New fusion with planar-based error estimation

[rohbotics]

This basically combines #163 and #160

[jim-v]

There still seems to be a problem with the robot pose fusion:

Pose 109 1.375130 1.712225 -0.098764 0.168028 -0.039671 -2.849728 0.095198
Pose 309 1.067837 1.589005 -0.236499 0.175031 -0.147566 -2.895403 0.074913
Pose 303 0.702363 1.806369 -0.182504 0.060763 -0.286554 -2.913001 -0.020498
Pose 105 1.137734 1.782056 -0.144299 0.114894 -0.115048 -2.913330 0.020852
Pose 108 1.172818 1.605811 -0.156398 0.190830 -0.116696 -2.892508 0.080129
Bogus pose -24.505989 3.214112 -2.394573 2.377064 1.100882 -0.964244 0.000001

[rohbotics]

Hey Jim,

How are you testing the fusion? I want to build a unit test that can take a bunch of poses and combine them.

Also in both pathological examples we have, the variance of one of the elements is negative which should never happen. That could be what is going on.

[jim-v]

I've been testing by playing a playing the bag test_bag_3_robot.bag and looking at the logs with the Python matplotlib scripts. The variance being negative was caused by the s2*s3 bug you found last night. Once I updated that it didn't happen again.

[rohbotics]

Did the widely off pose fusion happen again after the s3*s3 was fixed?

[jim-v]

Did the widely off pose fusion happen again after the s3*s3 was fixed?

No it didn't. The worst robot pose estimate had a z of 10cm

[rohbotics]

Ya, it looks like the fusion code is working correctly. Pose 303 is widely off with a large variance and it doesn't change the output pose much.

Pose 303 -0.917836 -0.204958 1.435643 -0.368849 1.004501 -0.810807 5.439666
Pose 100 0.689180 -0.619754 -0.108538 0.011713 0.111277 -0.773471 0.071655
Pose 101 1.030231 -0.860033 0.070909 0.067195 -0.045424 -0.800591 0.041309
Bogus pose 0.896774 -0.769448 0.012146 0.044673 0.016408 -0.788782 0.026077

[rohbotics]

Another couple examples of seemingly correct fusion.

Pose 112 0.776707 -1.897073 0.077582 0.010025 -0.008538 2.771833 0.016713
Pose 306 0.260884 -1.849020 0.161641 0.082878 -0.218599 2.739358 0.251656
Pose 305 0.607025 -1.347225 0.309565 -0.197715 -0.154342 2.804966 0.354700
Pose 148 0.735699 -1.976752 0.061941 0.047347 -0.012971 2.773134 0.023470
Pose 105 0.699555 -1.899469 0.066479 0.024743 -0.039207 2.767800 0.023014
Pose 110 0.720370 -1.856583 0.107594 0.002665 -0.035130 2.771646 0.026539
Pose 106 0.768049 -1.904689 0.046384 0.012746 -0.013376 2.770470 0.013517
Pose ALL 0.737399 -1.900083 0.072520 0.017025 -0.024196 2.770774 0.003783

Pose 114 0.533678 -1.874116 0.081906 0.052626 -0.016612 2.722129 0.028880
Pose 306 0.251565 -1.936364 0.193661 0.138075 -0.119512 2.706324 0.180140
Pose 305 0.601235 -1.443917 0.163590 -0.124088 -0.060938 2.733261 0.112873
Pose 105 0.565207 -1.801968 0.056826 0.019691 -0.019184 2.729646 0.016252
Pose 106 0.595939 -1.814743 0.065900 0.019714 -0.004653 2.729284 0.015984
Pose 148 0.633553 -1.853996 0.038905 0.027722 0.016745 2.727438 0.015708
Pose 112 0.606278 -1.782536 0.083324 0.004235 -0.007214 2.723899 0.017223
Pose 110 0.563788 -1.795994 0.081810 0.016481 -0.019159 2.726702 0.019247
Pose ALL 0.582712 -1.809883 0.070774 0.019595 -0.010407 2.726744 0.002883

[jim-v]

@rohbotics There is more cleanup and enhancements we could add, but do you want to merge this and do the others as separate PRs?

[rohbotics]

There are a couple more things that I want to wrap my head around in this code before merging.

1) Multi fiducial estimation, are we sure that it is completely disabled?

2) Split fusion stuff into seperate file. I think splitting out the TransformWithVariance stuff is a good idea for structure/clarity, and it makes it easier to create a unit tests for it.

[jim-v]

1. Multi fiducial estimation, are we sure that it is completely disabled?

Yes, I am sure it's disabled because there are no fid0 transforms. If you think it is better to remove the code, and add it back later when we have the time to validate it, that's OK with me.

2. Split fusion stuff into seperate file. I think splitting out the TransformWithVariance stuff is a good idea for structure/clarity, and it makes it easier to create a unit tests for it.

I like that idea

[rohbotics]

Turns out we never made average transforms use the new fusion method, so I fixed that while splitting out the code. This needs more testing.

[jim-v]

Turns out we never made average transforms use the new fusion method, so I fixed that while splitting out the code. This needs more testing.

I will do some more testing.

[jim-v]

[jim-v]

With limited testing, the pose deviates less than before (on the robot 3 dataset).

[rohbotics]

More fusion data (actually looking at david method this time):

Pose 306 1.461374 0.888577 -0.081352 0.007281 0.022536 0.607242 0.018861
Pose 114 1.462517 0.891032 -0.070745 0.008897 0.022524 0.603368 0.017350
Pose 105 1.459651 1.053491 -0.042113 0.066016 -0.012091 0.595777 0.029765
Pose 107 1.521834 0.952463 -0.045099 0.015106 -0.012347 0.596071 0.013556
Pose 303 0.709667 0.988054 -0.077593 0.219989 0.256942 0.617959 0.464816
Pose 148 1.505840 0.961031 -0.068196 0.022568 -0.006722 0.599645 0.016868
Pose 101 1.393912 0.859971 -0.129244 0.013708 0.050695 0.601361 0.037727
Pose 108 1.645389 0.980057 -0.041672 -0.005444 -0.053872 0.608220 0.023452
Pose ALL 1.501016 0.942316 -0.064516 0.016770 -0.001383 0.601407 0.004878

Pose 113 0.035477 0.001673 -0.059562 0.032328 0.023723 1.691326 0.019977
Pose 149 0.238479 0.144272 0.059549 -0.057493 -0.023743 1.681912 0.029008
Pose 100 0.121918 0.060244 -0.032036 -0.002811 0.007245 1.715180 0.011268
Pose 107 0.176362 -0.111700 0.022032 -0.005562 0.081160 1.737469 0.036899
Pose ALL 0.130457 0.030187 -0.017176 -0.003864 0.019400 1.711777 0.007912

[rohbotics]

I added some really simple test cases, is there anything specific that we want to test?

[jim-v]

I added some really simple test cases, is there anything specific that we want to test?

That's great. The test cases you had earlier would be useful: an outlier with a high variance that has little effect, and an outlier with a low variance that has a small effect. We should also test some rotations.

[rohbotics]

I added the outlier cases, but I'm not sure how to deal with rotation

[jim-v]

For rotation, we can do a similar thing, but pick some arbitrary rotations, eg. Euler (30, 0, 0) = [ 0.6502878, 0, 0, -0.7596879 ] Euler (45, 0, 0) = [ -0.4871745, 0, 0, -0.8733046 ]

We can assume that the tf2::Quaternion class is correct and don't need test cases for that but we need to test what we do to fuse them is sensible

[rohbotics]

Added simple rotation stuff, and cleaned up some TODOs

[rohbotics]

@jim-v Confirming that we are good to merge correct?

[jim-v]

@jim-v Confirming that we are good to merge correct?

Yes, please do.