Added code to correct sensor mapping. Does not work/has no pratical effect, so I turned it off. Probably we will have to think about a better way to process the resulting map. Changing the sensors' infinity threshold to a lower value improves the results, but we lose chunks of the map as a consequence.