Add simple local ICP algorithm w/ unittest

[EricCousineau-TRI]

As an initial foray into the perception pipeline, this issue will add a simple, local ICP-like algorithm, based on @gizatt's DART-like manipulation_tracking repository (where articulation may come for free due to framework and formulation, but is not the main focus).

The steps are to:

• [x] Add a very simple, positive returns-only unittest on a simple cuboid, only testing the cost for L2 norm of the linearized error.
• [x] Add ICP via SVD for simple, single-body case, and incorporate into unittest.
• [x] Add a unittest using experimental data, either from CORL or another dataset, for a simple convex object. (Before #6124 is resolved, the data will be lightweight.)
• [ ] Leverage the initial point cloud implementation from #6654.
• [ ] Move closed-form code out of dev/

If the priority for real-time tracking becomes higher, then before / alongside pose estimation:

• [ ] Implement free space augmentation with unittest, testing behavior independent of positive returns and with positive returns.
• [ ] Implement non-penetration constraint with unittest, testing behavior in coordination with other costs.

Alternative: Implement something akin to Bayesian Object Tracking, possibly the Gaussian version.

[hongkai-dai]

@EricCousineau-TRI could you update the status of this issue?

[EricCousineau-TRI]

No longer relevant, as there was no concrete use case for this. There is some related work going on in Anzu; if/when it's appropriate to upstream, a separate issue should be used to track that work.

\cc @naveenoid @thduynguyen