[ImuFactor] Unit test the on manifold ImuFactor

[bradley-solliday-skydio]

Adds three tests meant to jointly test the
ImuManifoldPreintegrationUpdate function.

The calculated covariance is tested by sampling the values it is a
covariance of and comparing it to the numircally calculated covariance
from the samples. Warning, there is a tradeoff between time (number of
samples) and accuracy for the calculated covariance. To speed things up,
the sampling process is multi-threaded. Note, with my current parameters,
it takes about 2^23 samples to be pretty robust to changes in the random
seed. But that takes a minute to run on my desktop with 16 cores. So in
the interest of testing time, I've cut the sample size by a bit. Of
course, this means if you change the seed, the test might end up
failing. To know if it's really a problem, bump up the sample size and
see if it persists.

The derivatives wrt to bias are calculated by perturbing the input in
each direction to numerically calculate the columns of the derivative.

The actual change in state (DR, Dv, Dp) isn't tested in a very
intelligent manner. They didn't have any simple and obvious properties
that could be easily tested. Still, their implementations are pretty
simple, so I figured comparing to a C++ implementation would at the very
least protect against someone accidentally deleting a line in the
symbolic code or whatever.

Topic: on_manifold_imu_factor_test
Relative: on_manifold_imu_factor

[bradley-solliday-skydio]

Reviews in this chain:
└#279 [ImuFactor] Add an on manifold Imu Factor
　└#280 [ImuFactor] Unit test the on manifold ImuFactor
　　└#281 [ImuFactor] tangent space Imu Factor

[bradley-solliday-skydio]

#	head	base	diff	date	summary
0	6b961703	b58932b1	diff	Dec 19 16:48 PM	2 files changed, 332 insertions(+)
1	cbff03c3	cb930c9d	rebase	Dec 20 1:09 AM	0 files changed
2	74362513	c75ca927	rebase	Dec 20 19:54 PM	0 files changed
3	c66a1560	ac03a31b	diff	Dec 21 0:49 AM	1 file changed, 10 insertions(+), 10 deletions(-)
4	e0dc1941	6a10ac05	rebase	Dec 21 2:12 AM	0 files changed
5	8b12cfb4	6a10ac05	diff	Dec 22 1:47 AM	1 file changed, 64 insertions(+), 101 deletions(-)
6	4e8c4700	d1c3e651	rebase	Jan 3 13:30 PM	0 files changed
7	acaa223d	49a9c23d	diff	Jan 6 9:04 AM	1 file changed, 30 insertions(+), 21 deletions(-)
8	eaac1763	cd66203d	rebase	Jan 6 9:51 AM	0 files changed
9	b99759f7	164c5e3e	rebase	Jan 6 12:23 PM	0 files changed
10	fa0d4f8d	d7d5a05f	rebase	Jan 10 16:25 PM	0 files changed
11	588dc03b	6e00ec4f	rebase	Jan 11 10:57 AM	0 files changed
12	ac8a8631	a74eae62	rebase	Jan 11 15:27 PM	0 files changed
13	abc44f74	91255e99	rebase	Jan 11 15:33 PM	0 files changed
14	bce1cc61	0fe0e7e5	rebase	Jan 11 18:31 PM	0 files changed
15	0bcdc34b	e311bc4e	diff	Jan 12 11:45 AM	1 file changed, 3 insertions(+), 3 deletions(-)
16	6dc39abf	7970c6f0	diff	Jan 12 12:42 PM	0 files changed
17	43b67655	63a4d237	rebase	Jan 12 16:23 PM	0 files changed
18	6ed55ca6	9630e6b6	rebase	Jan 19 13:57 PM	0 files changed
19	fa5c8aef	fa8474dd	rebase	Jan 19 18:41 PM	0 files changed
20	dd8fe3ec	9c6f3d96	diff	Jan 20 10:32 AM	1 file changed, 6 insertions(+), 8 deletions(-)
21	a741d8a6	b75e14bf	diff	Jan 25 10:12 AM	1 file changed, 3 insertions(+), 3 deletions(-)
22	90e5eeff	b75e14bf	diff	Jan 25 18:01 PM	1 file changed, 28 insertions(+), 17 deletions(-)
23	628c1365	b75e14bf	diff	Jan 25 18:54 PM	1 file changed, 21 insertions(+), 19 deletions(-)
24	9e3c1fd8	b75e14bf	diff	Jan 25 19:26 PM	1 file changed, 6 insertions(+), 6 deletions(-)
25	25e592a1	50a2f1da	diff	Jan 25 21:09 PM	0 files changed
26	2e9523a2	a83f3772	diff	Jan 31 17:41 PM	3 files changed, 144 insertions(+), 72 deletions(-)
27	11ea0008	111652d9	diff	Jan 31 17:53 PM	1 file changed, 5 insertions(+), 1 deletion(-)
28	82821709	615a9a36	diff	Jan 31 17:57 PM	0 files changed
29	df7fd0c5	615a9a36	diff	Jan 31 18:02 PM	1 file changed, 2 insertions(+), 2 deletions(-)
30	3be09c4d	deda3fc9	diff	Feb 1 14:33 PM	1 file changed, 7 insertions(+), 7 deletions(-)

[aaron-skydio]

This should not be 16 by default, it should really be 1

[bradley-solliday-skydio]

I'll do this for now, but is there really that much of an overhead of spawning threads when only a single core is available? Especially if we're only spawning a fixed number and the process is kind of long.

[aaron-skydio]

For this test probably not really, since CPU is the only resource that scales with number of threads here, and not memory. In general though the test runner (ctest, bazel, etc) should have control over test parallelism and will run tests with whatever parallelism is requested there, and individual tests trying to parallelize themselves makes that less effective

[aaron-skydio]

Do we really need 8 million samples for this? How long does this one take? Could we just use a smaller matrix?

[bradley-solliday-skydio]

It takes 4 seconds. I wanted to use a 9x9 matrix because that's the size of the covariance that we use in the next unit test (and this unit test more or less checks that the test infra-structure is right).

[aaron-skydio]

This function is the non-auto one :)

Explains why they're so close, I was a bit surprised

[bradley-solliday-skydio]

Thanks for catching this. We discussed this in person, but it turns out the test would have been failing (severely) had I called the other function correctly. Specifically, it turned out that the hand-written symbolic implementation of right_jacobian added a term it should have been subtracting, causing the problems. With that fixed, the difference between the two (using the frobenius norm) was on the order of 10-8.