Fast Map Merging Implemented in Matlab working.