Refactor

Nice try with PCL :-(