Added first version of occupancy grid visualization.
Merge branch 'feature/occupancy_grid_visualization' into feature/new_iterators