Added logging and moved map fusion into map class