2d occupancy grid mapping c++