Map Merge Developments

[cj-mclaughlin]

We have two scenarios which we have to address before full use in simulation:

S1. Merging Different-Resolution Maps
--> A1. Pad maps (with unknown cell) to the resolution of the largest map. This is very simple, but we need to test in integration that we are padding the correct side of the smaller map.

S2. Error Handling
--> A2. Assert that information was not lost. From each map, we can count the number of known pixels N_k = (# occupied + # unoccupied cells). The merged map should have at least as much information as that of the larger map: min(N_k(m1), N_k(m2)). Any less information and we have likely performed an invalid merge.
--> A3. As our merging algorithm has some randomness, we can perform it multiple times to identify outlier transformations, potentially returning the median transformation (in terms of the angle of the transformation). Additionally, we can perform multiple merges after applying transformation-preserving augmentations to the maps (i.e. upsample each map by 2x), and check that the predicted transformation is similar (within ~2 degrees).

When we detect an error, we should throw an exception to be caught by the state machine, which will indicate that the merge could not be performed.