Lot of refactoring on c_vision

- rewied architecture
- now is easier to display results
- still work in progres displaying things...
Merge branch 'master' of github.com:AIRLab-POLIMI/C-SLAM