Add trimming option in ICP

[ListIndexOutOfRange]

The ICP algorithm is notably sensitive to outliers, as their corresponding distances s[i] X[i] R[i] + T[i] = Y[NN[i]] cost a lot. A simple strategy to alleviate this effect is to crop out points whose distance is above a threshold, computed as the r-th quantile of the point-to-point distances, where r in [0, 1] is the ratio of outliers.

This is usually called trimming, see e.g. D. Chetverikov et al., The Trimmed Iterative Closest Point Algorithm, 2002.

I propose to add the possibility to trim sources point clouds in the iterative_closest_point function, through a new parameter trim_fraction.

This is basically done by computing a mask for the distances, taking into account the already existing mask for padding.