Scale covariance with geometric meaningful weight

Recall:

• The final covariance is C = (J.t S^(-1) J)^(−1) where J is the Jacobian of the cost function and S is the covariance of all residuals (see http://ceres-solver.org/nnls_covariance.html for more info). So, the covariance reflects the models (planes/lines) uncertainties and the final fitting error of the ICP like algorithm.

Changes :

• For now, the residuals are weighted with a custom weight fitQualityWeight = 1 - RMSE/UserParam
• This leads to a biased covariance scale
• To make the covariance output more reliable in this MR, the residuals are weighted with the target models' variances information extracted from neighborhoods
• The MaxLineDistance and MaxPlaneDistance parameters are changed (from 0.2 to 0.1) to obtain as good results as before

Results :

• The resulting covariance is much smaller.
  • Explication on covariances : the covariances of the models and the final result covariance are directly linked (C = (J.t S^(-1) J)^(−1) : if the first one decrease, the final one decreases).
  • Explication on weights : the weight induces by the covariance on one residual is bigger than the actual weight of current master custom version. Bigger weight -> bigger Jacobian -> bigger J^T*J -> smaller cov. This behavior makes sense if the convergence took place : the less gradient there is, the less localized the system seems to be. However this covariance does not reflect the final quality of the residuals relatively to their model : if the models are very accurate (small covariance) and the points are very far from them at convergence, the covariance does not capture it.
• The new weights lead to more heterogeneity in residuals relatively to model accuracy.
• It improves some parts and worsens some others.
• The processing time and the keypoint matches number are kept approximately the same as master's.