WIP: Covariance with local parameterization

[marip8]

This PR adds the ability to calculate covariance for parameters that were locally parameterized in the optimization. My test case for development was the Eigen-based PnP optimization in #54.

PnP Covariance Results

In these tests the simulated camera was positioned over the center of the target (rather than the origin, which is in the corner) at a 1m standoff.

Because the quaternion is locally parameterized by Ceres, I'm not entirely sure what the three output values physically represent. This will require a little more research into Ceres

5x7, 0.025m target, perfect initial condition

	x	y	z	rx?	ry?	rz?
x	0.1035	0.001181	0.5383	-0.0005392	1	-0.3187
y	0.001181	0.1342	-0.5589	-1	0.0006415	-0.207
z	0.5383	-0.5589	0.007979	0.5598	0.5394	-0.05595
rx?	-0.0005392	-1	0.5598	0.067	1.457e-14	0.2059
ry?	1	0.0006415	0.5394	1.457e-14	0.05164	-0.3175
rz?	-0.3187	-0.207	-0.05595	0.2059	-0.3175	0.002711

10x14, 0.025m target, perfect initial condition

	x	y	z	rx?	ry?	rz?
x	0.01265	0.004706	0.5299	-0.002156	0.9999	-0.3153
y	0.004706	0.01627	-0.5515	-0.9999	0.002551	-0.2081
z	0.5299	-0.5515	0.001948	0.5553	0.5342	-0.05263
rx?	-0.002156	-0.9999	0.5553	0.008095	-1.862e-15	0.2038
ry?	0.9999	0.002551	0.5342	-1.862e-15	0.006258	-0.3104
rz?	-0.3153	-0.2081	-0.05263	0.2038	-0.3104	0.0006687

10x14, 0.05m target, perfect initial condition

	x	y	z	rx?	ry?	rz?
x	0.003261	0.01837	0.5167	-0.008358	0.9985	-0.3293
y	0.01837	0.004135	-0.5399	-0.999	0.01004	-0.2207
z	0.5167	-0.5399	0.0009742	0.5553	0.5342	-0.05263
rx?	-0.008358	-0.999	0.5553	0.002024	-1.045e-16	0.2038
ry?	0.9985	0.01004	0.5342	-1.045e-16	0.001565	-0.3104
rz?	-0.3293	-0.2207	-0.05263	0.2038	-0.3104	0.0003344

Interesting to note that the variance of the parameters goes down as the target got physically larger and as the number of points increased. The off-diagonal terms, however, seemed unaffected

[marip8]

Judging by the algorithm on the Ceres website, it seems like the local parameterization of the quaternion is the non-normalized angle-axis representation, just as in the Pose6d class

[marip8]

There were a few mistakes in #54 that are addressed by #63 which caused some confusion with my previous comments. Using the corrected version of the cost function and unit tests, I was able to create the covariance matrices for various parameterizations of this problem

Covariance - Pose6d

I slightly modified the original PnP cost function (from 0.1.0) to take two pointers (angle axis and position) instead of one.

	x	y	z	rx	ry	rz
x	0.0002255	0.4271	-0.06	0.07556	-0.5086	0.2647
y	0.4271	0.0002983	0.4996	0.3826	-0.7978	-0.2957
z	-0.06	0.4996	0.003153	0.4913	-0.3412	-0.7769
rx	0.07556	0.3826	0.4913	0.01619	-0.2038	-5.375e-16
ry	-0.5086	-0.7978	-0.3412	-0.2038	0.002101	0.3104
rz	0.2647	-0.2957	-0.7769	-5.429e-16	0.3104	0.01966

Covariance - Eigen AngleAxis (no local parameterization)

I then slightly modified the new cost function to cast the input orientation pointer to an Eigen::AxisAngle rather than Eigen::Quaternion to replicate the behavior of the Pose6d class.

	x	y	z	rx	ry	rz
x	0.0002255	0.4271	-0.06	0.07556	-0.5086	0.2647
y	0.4271	0.0002983	0.4996	0.3826	-0.7978	-0.2957
z	-0.06	0.4996	0.003153	0.4913	-0.3412	-0.7769
rx	0.07556	0.3826	0.4913	0.01619	-0.2038	-5.375e-16
ry	-0.5086	-0.7978	-0.3412	-0.2038	0.002101	0.3104
rz	0.2647	-0.2957	-0.7769	-5.429e-16	0.3104	0.01966

Covariance - Eigen Quaternion with Local Parameterization

I generated the covariance matrix using the existing cost function that leverages quaternions and local parameterization

	x	y	z	r1	r2	r3
x	0.0002255	0.4271	-0.06	0.07556	-0.2647	-0.5086
y	0.4271	0.0002983	0.4996	0.3826	0.2957	-0.7978
z	-0.06	0.4996	0.003153	0.4913	0.7769	-0.3412
r1	0.07556	0.3826	0.4913	0.008095	-3.344e-16	-0.2038
r2	-0.2647	0.2957	0.7769	-3.678e-16	0.006258	-0.3104
r3	-0.5086	-0.7978	-0.3412	-0.2038	-0.3104	0.0006687

Conclusions

• Local parameterization covariance returns correct position covariance
• The locally parameterized orientations look like [rx, ry, rz], but:
  • Orientation has same covariance blocks with position variables, but in a different order
  • Orientation has a different "self" covariance block
• If we want to use Eigen objects in cost functions, we should use a Vector3 for position and a Vector3 for un-normalized angle axis orientation without local parameterization in order for the covariance matrices to correspond to physical quantities that we understand (i.e. [rx, ry, rz] rather than [r1, r2, r3])