Revert "This doesnt work yet! But we are almost there"

This reverts commit d96fc2e53ef9bfd0972ecefac703e917fc3400c2.
Revert "Revert "This doesnt work yet! But we are almost there""

This reverts commit 383255babc10d9d25bfc2224009e97f4aeab20e7.
added filters, now the code will run pass through filters in x,y,z and then a ransac algorithm over the rest to find the floor.