More options for covariances of subsets of keys

[asa]

Perhaps add these to the optimization example along with how to compute metric scale for them.

[joansola]

Hi I adhere to this.

Since SLAM is relative but all is expressed in absolute reference, it is interesting to recover the relative variance between variables. Say we are interested in the variance or relative error of var2 wrt. var1, that is, in the covariance of var2-var1. To do so, we need the cross-variance between var1 and var2.

In particular, I would like to have either of these in symforce:

# 1. compute the cross-variance explicitly
cross_1_2 = compute_cross_variance(key1, key2) 
# 2. compute the covariance of a subset of keys
cov_some = compute_co_cross_variances([key1, key2, ..., keyN])
# and then
cross_1_2 = cov_some[key_i][key_j] # to get the cross-var between var_i and var_j
cov_1_2 = cov_some[[key_i, key_j],[key_i, key_j]] # or similar, to get the block of var_i and var_j
# 3. compute all the covariance
cov_all = compute_full_covariance(...)
# then:
cross_1_2 = cov_all[key1][key2]

where I guess option 2 is the most interesting

[aaron-skydio]

Thanks that's a good callout - (2) should work nicely I think. Actually we should probably just expose (3) also, since what we already have is computing the full covariance anyway and then we're just pulling out and returning the diagonal blocks

[joansola]

Oh, then if the computations are done, to implement 3. we only need the correct accessors!

This would be very interesting for my current project since I have to provide the relative accuracy between two estimated entities

[aaron-skydio]

Yep exactly, (3) should be a quick change

[aaron-skydio]

Did (3) in 1d870c1e9c756be5dbea483208918cb9e282b510, which is in main now

[joansola]

Will check it out asap! Thanks!

[joansola]

I see the full covariance is computed and returned as a single large Matrix.

What is the method of accessing a particular off-diagonal block, using the keys {key1, key2} of the block?

[aaron-skydio]

The block you want should be:

index = optimizer.linearization_index()
cov = optimizer.compute_full_covariance(values)

cov[index[key1].offset : index[key1].offset + index[key1].tangent_dim,
    index[key2].offset : index[key2].offset + index[key2].tangent_dim]