Add CPD, LZD and make ICP Python-based

[rhugonnet]

This PR adds Coherent Point Drift (CPD) and Least Z-difference (LZD) coregistrations and modifies Iterative Closest Point (ICP) coregistration to be only NumPy/SciPy-based. This last addition allows to remove the (heavy) OpenCV optional dependency previously used for ICP, and to call consistent fit optimizer and iterative parameters in ICP, comparable to that of other methods.

All three methods are added in the same PR as they use several common subroutines related to rigid point registration.

This PR takes over #581, which is closed. Thanks to the homogenization in #530, we can now use the same core functions as in other coregs at all steps.

Finally, to ensure our ICP/CPD implementations perform well, they are compared to that of other packages (Open3D, OpenCV, pycpd) in a separate module (that could be moved into its own small repository).

Resolves #100
Resolves #483
Resolves #587
Resolves #705
Resolves #706

Background on the methods

The three methods are rigid transformation methods: translations + rotations (CPD can support more complex transforms, but those are not relevant for DEM applications).

ICP and CPD are the two most successful methods of point-point registration, widely used in computer vision and other fields.
LZD is the most successful method of grid-point registration, a much smaller field, mostly used in geoscience. After some investigation, it appears that the Nuth and Kaab (2011) method is actually a special case of LZD for translations only (exact same equation, NK expressed in polar coordinates while LZD is expressed in cartesian coordinates).

Why our own implementations?

I think this is an important question to address first for ICP/CPD where other implementations exist.

The short answer is that, because we need very fine sub-pixel alignment for DEMs (and that ICP/CPD are very sensitive to this), having a modular implementation is key to reduce errors (while it is less important in e.g. computer vision applications).

Existing implementations of ICP in Python (e.g., https://github.com/pglira/simpleICP) don't allow for the modularity we need in xDEM, such as passing any fit optimizer, changing subsampling, or running the various existing optimizations of ICP (that exist in the OpenCV implementation we previously relied on). After inspecting the code in simpleICP, it would be difficult to add the modularity we need there.

Consequently, our coreg/affine methods are becoming a bit big for xDEM. They are applicable to any grid-point or point-point rigid registration problem, not just DEMs. And, to my knowledge, there is no such package doing this in Python.
Therefore, we might think about taking these methods out of xDEM in the long term, for better maintenance and to provide these modular implementations that can be benchmarked by simulation (explained later) for the larger community.

Changes in this PR

ICP

This PR adds a NumPy-SciPy ICP (nearest point done in SciPy, and optimizer defaults to SciPy but can be anything user-defined, like for other methods), and also adds many improvements of ICP to ensure an accuracy equal to that of OpenCV or Open3D's implementation.

Those are:

• The basic "point-to-point" implementation (cost function computed is simply the 3D distance) introduced in Besl and McKay (1992), https://doi.org/10.1117/12.57955/.
• The "point-to-plane" implementation (cost function is 3D distance projected on the plane normals), described in Chen and Medioni (1992), https://doi.org/10.1016/0262-8856(92)90066-C, which performs better in some use cases, especially when the 3D point cloud represent a surface (like DEMs).
• For small rotations (<30°) and the "point-to-plane" method, we include a linearized approximation to the ICP least-squares problem that allows to speed-up the computation of the rigid transformation at each iteration, based on Low (2004), https://www.cs.unc.edu/techreports/04-004.pdf. As for all coregistration in xDEM, the minimization step can also be passed any optimizer (like scipy.optimize.least_squares),
• For outlier-prone data, or data sets that simply have different extent, the selection of closest neighbours is sometimes redundant and reduces performance. To remedy this, we add two "Picky ICP" options described in Zinsser et al. (2003), https://doi.org/10.1109/ICIP.2003.1246775. The first option removes target points that are matched with a same reference point, keeping only the pair with the minimum distance to each reference point. The second option performs an outlier rejection based on the distances between all pairs.
• For speed-up of all methods, we add a multi-resolution scheme which loops over all iteration, each time with a different subset of the full subsample, based on Jost and Hugli (2002), https://ieeexplore.ieee.org/document/1024114.

CPD

This PR adds a NumPy-based implementation of CPD based on the algorithm described in the paper, and inspired by pycpd's rigid implementation: https://github.com/siavashk/pycpd/blob/master/pycpd/rigid_registration.py.

CPD can have trouble with numerics, so we add a standardize option for point-point methods to properly use it, see below.

LZD

This PR adds the Least Z-difference coregistration method of Rosenholm and Torlegård (1988): https://www.asprs.org/wp-content/uploads/pers/1988journal/oct/1988_oct_1385-1389.pdf

Other changes

This PR implements several other changes:

• Adds consistent matrix_from_translations_rotations and translations_rotations_from_matrix conversion functions, used everywhere in other functions and added to the public API,
• Adds a only_translation option to ICP/CPD/LZD, which allows to solve the coregistration optimization considering only the translation,
• Fixes the _get_subsample_mask_pts_rst function to never return non-finite values,
• Adds a standardize option to ICP/CPD which allows to standardize input data before running the algorithm for improved numerics (CPD fails without it).
• Removes internal OpenCV filters in filters.py (that were not listed in the API), and replaces a couple OpenCV functions used in other modules too (usually to generate synthetic data) using only SciPy functions instead,
• Removes the optional OpenCV dependency altogether.

Benchmarking our ICP algo, and more?

In order to benchmark that our ICP performs as well as other algorithms out there, both in terms of accuracy and speed, I've separately written a Python module that runs ICP from OpenCV, Open3D, and other packages. This is currently on a separate repository (as we don't want these dependencies in xDEM).
Should we add that new module into a xdem-benchmark or similar, that can punctually install/run other packages and xDEM, to compare the performance of all these algorithms?

We could consider adding other algorithms to this repo, for instance the ICP of py4dgeo (#504), or of libpointmatcher (potentially via ASP too): #11.
But also other algorithms, in time, if we want (check CPD implementation, etc).

TO-DO to finalize

• Add LZD method,
• Use functions matrix_to_translation_rotations and translation_rotation_to_matrix in tests,
• Finalize Picky ICP options,
• Fix preprocessing sampling to never return NaNs,
• Add "only_translation" option to all methods (to allow the use of "hv" and "xyz" scaling).
• Add "standardize" option to point-point methods for convergence.