Fix blobs constraints usage in optimization
This fixes/improves the use of point-to-blob residuals in optimization. These residuals only bring poor and approximate constraints, so they should only used when the environment is really not well structured.
- Fix point-to-blob constraints
- Add blob parameters setters/getters
- Replace
FastSlam
option by more straightforwardUseBlobs
parameter.
Blob neighborhoods need to approximately represent an ellipsoid, which is neither flat (plane) not straight (line). This criterion can be implemented by checking that the smallest and largest PCA eigenvalues ratio is not too small.
The following image shows the blob map, colored with mean optimization weight. The brighter, the more the derived constraint is used in optimization.