crop_map.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 
00003 import sys
00004 import yaml
00005 from PIL import Image
00006 import math
00007 
00008 def find_bounds(map_image):
00009     x_min = map_image.size[0]
00010     x_end = 0
00011     y_min = map_image.size[1]
00012     y_end = 0
00013     pix = map_image.load()
00014     for x in range(map_image.size[0]):
00015         for y in range(map_image.size[1]):
00016             val = pix[x, y]
00017             if val != 205:  # not unknown
00018                 x_min = min(x, x_min)
00019                 x_end = max(x, x_end)
00020                 y_min = min(y, y_min)
00021                 y_end = max(y, y_end)
00022     return x_min, x_end, y_min, y_end
00023 
00024 def computed_cropped_origin(map_image, bounds, resolution, origin):
00025     """ Compute the image for the cropped map when map_image is cropped by bounds and had origin before. """
00026     ox = origin[0]
00027     oy = origin[1]
00028     oth = origin[2]
00029 
00030     # First figure out the delta we have to translate from the lower left corner (which is the origin)
00031     # in the image system
00032     dx = bounds[0] * resolution
00033     dy = (map_image.size[1] - bounds[3]) * resolution
00034 
00035     # Next rotate this by the theta and add to the old origin
00036 
00037     new_ox = ox + dx * math.cos(oth) - dy * math.sin(oth)
00038     new_oy = oy + dx * math.sin(oth) + dy * math.cos(oth)
00039 
00040     return [new_ox, new_oy, oth]
00041 
00042 if __name__ == "__main__":
00043     if len(sys.argv) < 2:
00044         print >> sys.stderr, "Usage: %s map.yaml [cropped.yaml]" % sys.argv[0]
00045         sys.exit(1)
00046 
00047     with open(sys.argv[1]) as f:
00048         map_data = yaml.safe_load(f)
00049 
00050     if len(sys.argv) > 2:
00051         crop_name = sys.argv[2]
00052         if crop_name.endswith(".yaml"):
00053             crop_name = crop_name[:-5]
00054         crop_yaml = crop_name + ".yaml"
00055         crop_image = crop_name + ".pgm"
00056     else:
00057         crop_yaml = "cropped.yaml"
00058         crop_image = "cropped.pgm"
00059 
00060     map_image_file = map_data["image"]
00061     resolution = map_data["resolution"]
00062     origin = map_data["origin"]
00063 
00064     map_image = Image.open(map_image_file)
00065 
00066     bounds = find_bounds(map_image)
00067 
00068     # left, upper, right, lower
00069     cropped_image = map_image.crop((bounds[0], bounds[2], bounds[1] + 1, bounds[3] + 1))
00070 
00071     cropped_image.save(crop_image)
00072     map_data["image"] = crop_image
00073     map_data["origin"] = computed_cropped_origin(map_image, bounds, resolution, origin)
00074     with open(crop_yaml, "w") as f:
00075         yaml.dump(map_data, f)
00076 


map_server
Author(s): Brian Gerkey, Tony Pratkanis, contradict@gmail.com
autogenerated on Thu Aug 27 2015 14:06:52