Adding certifiable estimation applications

[zhexin1904]

Summary

Adds certifiable pose-graph optimization, landmark-SLAM and range-aided SLAM. Also a few fixes in using ALM with Riemannian staircase optimizer.

What changed

Results

Pose-graph optimization:

dataset	init	level	objective	solve	verify	round	other
MIT	fast-sync	2	30.58	0.222	0.0013	0.0007	0.025
MIT	random	2	30.58	0.293	0.0020	0.0005	0.027
intel	fast-sync	2	26.17	0.497	0.0048	0.0015	0.114
intel	random	4	26.17	5.53	0.537	0.0011	0.161
CSAIL	fast-sync	2	15.85	0.198	0.0019	0.0009	0.043
CSAIL	random	2	15.85	0.293	0.0019	0.0006	0.039
sphere2500	fast-sync	3	843.5	3.46	0.031	0.0027	0.471
sphere2500	random	3	843.5	15.6	0.031	0.0036	0.481

Rotation averaging, unchanged by this PR and included as a reference:

dataset	init	level	objective	solve	verify	round	other
MIT	fast-sync	2	19.41	0.097	0.00083	0.00027	0.010
MIT	random	4	19.41	1.25	0.058	0.00031	0.015
intel	fast-sync	2	1.820	0.259	0.0019	0.00089	0.054
intel	random	4	1.820	2.49	0.14	0.00071	0.073
CSAIL	fast-sync	2	11.16	0.109	0.00083	0.00035	0.017
CSAIL	random	2	11.16	0.143	0.00083	0.00034	0.017
sphere2500	fast-sync	3	442.7	1.68	0.019	0.0021	0.267
sphere2500	random	3	442.7	3.38	0.019	0.0025	0.264

Landmark SLAM:

dataset	init	level	objective	solve	verify	round	QCQP build	total
cityTrees_1k	fast-sync	2	23.7559	0.255	0.0021	0.0006	0.0036	0.302
cityTrees_1k	random	4	23.7559	13.93	4.709	0.0006	0.0091	18.70
victoriaPark_1k	fast-sync	2	46.8794	0.227	0.0023	0.0006	0.0038	0.272
victoriaPark_1k	random	3	46.9118	4.240	0.0159	0.0006	0.0064	4.313

Range-aided SLAM (objective reported as the local refined on):

dataset	init	level	objective	solve	verify	round	QCQP build	total
goats_15	fast-sync	3	18204.6	6.431	0.186	0.0006	0.0040	6.646
goats_15	random	3	18204.6	9.157	0.192	0.0006	0.0041	9.378
goats_16	fast-sync	4	3894.12	11.91	3.342	0.0006	0.0035	15.27
goats_16	random	3	3894.12	1.524	1.681	0.0007	0.0024	3.215

Known issue

To do

• Performance enhancement, as in the issue Compile a rank-independent QCQP representation for BM staircases #2683 above.
• Better inner local solver.

[dellaert]

NIce! Would be great if there was also an example notebook (and python wrapper). I already showed the way, I think it’s just mechanical.

[dellaert]

And please check CI failures :-)

[zhexin1904]

Hi Frank @dellaert , I have the landmark SLAM and range-aided SLAM application almost ready now. I plan to convert this PR back to a draft, add those commits, and then mark it as ready for review again.

[dellaert]

FYI @avinashresearch1 - BM work in progress.

[zhexin1904]

Rot2 is now used, and Unit2 has been removed, but I still have a few optimizer changes to commit before completing this PR.