Go to the documentation of this file.00001
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:
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
00031
00032 dx = bounds[0] * resolution
00033 dy = (map_image.size[1] - bounds[3]) * resolution
00034
00035
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
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