Add functions to evaluate covariance of optimization results

[schornakj]

• Add function to calculate covariance matrices between a variety of types. These are based on the fuctions @drchrislewis used in WIP Add covariance #36.
• Add some types that contain covariance calculation results.
• Add covariance to the optimization result object for the ExtrinsicCameraOnWrist problem.
• Print the covariance results in the ExtrinsicCameraOnWrist unit tests.
• Add a 2D circle fit optimization. Given a set of observed 2D points, this optimization tries to find center coordinates and a radius that define a circle which minimizes the distance between all the observed points and the edge of the circle. This problem has 3 parameters in 2D space, so it's much simpler than the 6D+ problems described in the other optimizations and therefore useful for testing and debugging things like covariance.

TODOs:

• Add test cases for the CircleFit optimization that demonstrate difference covariance behavior wit h different distributions of points (e.g. nonuniform distributions).
• Add checks to covariance in the ExtrinsicCameraOnWrist tests to make sure it's behaving as expected. One weird thing currently is that the covariance results for the camera and target transforms are the same between each (randomized!) hemisphere test case, but different between the hemisphere cases and the conical cases.

[schornakj]

I wanted to provide some examples of output covariance matrices to make sure these calculations make sense:

In all cases the Ceres covariance options are set to:

• algorithm_type = DENSE_SVD
• min_reciprocal_condition_number = 1e-14 (Ceres default)
• null_space_rank = 0 ( Ceres default)

Circle-Fit Optimizations

In all cases the optimization is solving for a circle defined by the [x, y] coordinate of its center point and the radius r. The "real" circle is always centered at [0, 0] and always has radius 1.0.

Observations are noise-free and evenly-spaced:

	x	y	r
x	0.1	0	-1.37668e-15
y	0	0.1	0
r	-1.37668e-15	0	0.0353553

Observations are noise-free but are tightly clustered within +/- 0.063 radians of two points on opposite sides of the circle at [-1, 0] and [1, 0]. The standard deviation of y is very high because of low diversity of observations along that dimension.

	x	y	r
x	0.15813	0	0
y	0	11.181	0
r	0	0	0.0790569

Observations consist of just two points, at [-1, 0] and [1, 0]. There is a linear correlation between y and r because the problem is underconstrained and changing the estimated circle center point directly changes the radius.

	x	y	r
x	0.353553	9.45565e-16	-3.45504e-16
y	9.45565e-16	0.0317915	-1
r	-3.45504e-16	-1	0.120425

Observations consist of just two points, at [0, -1] and [0, 1]. There is a linear correlation between x and r here.

	x	y	r
x	0.0197708	-1.42085e-09	1
y	-1.42085e-09	0.353553	-1.42086e-09
r	1	-1.42086e-09	0.166688

Just one point is observed at [0, 0]. All parameters are linearly correlated because the observations (or lack thereof) do a totally trash job of constraining the problem.

	x	y	r
x	0.0293246	-1	1
y	-1	0.0324568	-1
r	1	-1	0.10962

Intrinsic Camera Optimizations

This is from the problem set up and solved by experimental_extrinsic_example.launch.

Optimization result for reference:
Intrinsics:

fx = 542.476
fy = 543.626
cx = 320.506
cy = 226.21

Distortions:

k1 = 0.0810764
k2 = -0.222309
p1 = 0.00933949
p2 = -0.00186427
k3 = 0.48253

	fx	fy	cx	cy	k1	k2	p1	p2	k3
fx	7.87711	0.990739	0.168015	0.125206	0.421103	-0.224659	0.053228	-0.00241834	0.31688
fy	0.990739	7.91457	0.19608	0.138212	0.406506	-0.212608	0.0563467	0.00358436	0.303106
cx	0.168015	0.19608	4.42257	-0.074076	-0.130507	0.254892	-0.105898	0.794291	-0.274564
cy	0.125206	0.138212	-0.074076	4.65973	0.168877	-0.194036	0.819674	-0.122959	0.248919
k1	0.421103	0.406506	-0.130507	0.168877	0.0290932	-0.920638	0.172487	-0.170555	0.900519
k2	-0.224659	-0.212608	0.254892	-0.194036	-0.920638	0.132333	-0.235029	0.289255	-0.97708
p1	0.053228	0.0563467	-0.105898	0.819674	0.172487	-0.235029	0.00304791	-0.136461	0.279261
p2	-0.00241834	0.00358436	0.794291	-0.122959	-0.170555	0.289255	-0.136461	0.00286411	-0.327018
k3	0.31688	0.303106	-0.274564	0.248919	0.900519	-0.97708	0.279261	-0.327018	0.197253

Extrinsic Camera Optimizations

These are from the perfect_start_hemisphere case in the extrinsic camera on wrist unit tests.

Covariance of camera-to-wrist transform:

	x	y	z	rx	ry	rz
x	0.00831017	0.97596	0.985605	-0.76954	0.389165	0.023769
y	0.97596	0.00519508	0.982365	-0.80411	0.211492	0.015528
z	0.985605	0.982365	0.00783368	-0.708467	0.282623	0.0159227
rx	-0.76954	-0.80411	-0.708467	0.00311429	-0.214649	-0.0213083
ry	0.389165	0.211492	0.282623	-0.214649	0.00452487	0.0697084
rz	0.023769	0.015528	0.0159227	-0.0213083	0.0697084	0.00239715

Covariance of base-to-target transform:

	x	y	z	rx	ry	rz
x	0.00520212	9.24059e-05	-0.00800198	-0.00427585	0.012232	-0.615188
y	9.24059e-05	0.00370058	-0.0221231	-0.0786829	0.0115902	0.650357
z	-0.00800198	-0.0221231	0.0109208	0.340988	-0.684661	0.000118052
rx	-0.00427585	-0.0786829	0.340988	0.00299453	-0.210727	0.000399889
ry	0.012232	0.0115902	-0.684661	-0.210727	0.00131406	-0.00100679
rz	-0.615188	0.650357	0.000118052	0.000399889	-0.00100679	0.000421135

Covariance between camera-to-wrist and base-to-target transforms:

	x	y	z	rx	ry	rz
x	-0.0100472	-0.024779	0.990707	0.387594	-0.686673	0.000236412
y	-0.00585438	-0.0280255	0.980931	0.256132	-0.696995	-0.000327767
z	-0.0158547	-0.0270426	0.99126	0.310527	-0.665723	8.87206e-05
rx	-0.0169052	0.0192659	-0.745747	-0.22877	0.837655	0.000794097
ry	-0.00630336	-0.045838	0.322901	0.940927	-0.19969	0.00179854
rz	0.0287366	-0.0646129	0.0192759	0.103229	0.0350758	-0.0293351

[schornakj]

Add checks to covariance in the ExtrinsicCameraOnWrist tests to make sure it's behaving as expected. One weird thing currently is that the covariance results for the camera and target transforms are the same between each (randomized!) hemisphere test case, but different between the hemisphere cases and the conical cases.

I think I figured this out: while the initial conditions of these test cases are randomized, the ground truth values for the base-to-target and wrist-to-camera transforms are the same, so the optimization results for each type of test case are the same regardless of the initial conditions.

[schornakj]

@marip8 This is ready for review when you get a moment.

[marip8]

Looks good to me overall. I think it's probably worth thinking about how we print these results such that it's clear which parameter block is across the columns of the matrix and which is across the rows.

[schornakj]

@marip8 I got my CI issues mixed up when we talked earlier today, this one was just failing because of a flaky test. Should be good to go now.