Merge martin-velas contribution

[laboshinl]

[vkee]

This seems to have many new dependency requirements, have you been able to get it to build and run? If so, what improvements are added?

[facontidavide]

I took a look to the change and I do definitively endorse to split the code in smaller functions with a readable name which clearly express the intent of the code.

I suggest @laboshinl to cherry pick here and there some changes to clean up his code.

By the way, Eigen is NOT a new dependency, since PCL already requires it.

[facontidavide]

@laboshinl if we split this big PR in smaller one that you can review more easily, will you accept them? Do you have time to maintain the repository?

@martin-velas @StefanGlaser