Slam merge requestshttps://gitlab.kitware.com/keu-computervision/slam/-/merge_requests2021-10-27T11:25:14-04:00https://gitlab.kitware.com/keu-computervision/slam/-/merge_requests/137Scale covariance with geometric meaningful weight2021-10-27T11:25:14-04:00Julia SanchezScale 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 ref...**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.