cleanup occupancy grid and add const correctness