Improved speed of SFM::fillWorld3D

[pattacini]

Done some refactoring for get3DPoints* methods as well as fillWorld3D. For the latter, a factor of 10x speed seems to be achieved.

• [ ]  General Comment
• <img border=0 src='https://avatars.githubusercontent.com/u/3738070?v=3' height=16 width=16'> Initial tests confirm that new modifications to SFM are working correctly. However, I've just switched the branch on our cuda machine in order to run a test campaign in the next days. Please, report any possible misbehavior. cc @vtikha @tanismar @Tobias-Fischer

  

[pattacini]

Initial tests confirm that new modifications to SFM are working correctly. However, I've just switched the branch on our cuda machine in order to run a test campaign in the next days.

Please, report any possible misbehavior.

cc @vtikha @tanismar @Tobias-Fischer