ransac_grid.py

   1 import pickle
   2 import matplotlib.pyplot as pyplot
   3 from math import sqrt
   4 import random
   5 import sys
   6 import time
   7
   8 import src.linef as linef
   9 import src.gridf as gridf
  10 from src.manual import lines as g_grid
  11
  12 import new_geometry as gm
  13
  14 random.seed(12345)
  15
  16 def plot_line(line, c):
  17     points = linef.line_from_angl_dist(line, (520, 390))
  18     pyplot.plot(*zip(*points), color=c)
  19
  20 def dst((x, y), (a, b, c)):
  21     return abs(a * x + b * y + c) / sqrt(a*a+b*b)
  22
  23 def points_to_line((x1, y1), (x2, y2)):
  24     return (y2 - y1, x1 - x2, x2 * y1 - x1 * y2)
  25
  26 def to_general(line):
  27     points = linef.line_from_angl_dist(line, (520, 390))
  28     return points_to_line(*points)
  29
  30 def nearest(lines, point):
  31     return min(map(lambda l: dst(point, l), lines))
  32
  33 def nearest2(lines, point):
  34     return min(map(lambda l: dst(point, points_to_line(*l)), lines))
  35
  36 size = (520, 390)
  37
  38 def generate_models(sgrid, lh):
  39     for f in [0, 1, 2, 3, 5, 7, 8, 11, 15, 17]:
  40         grid = gm.fill(sgrid[0], sgrid[1], lh , f)
  41         grid = [sgrid[0]] + grid + [sgrid[1]]
  42         for s in xrange(17 - f):
  43             grid = [gm.expand_left(grid, lh)] + grid
  44         yield grid
  45         for i in xrange(17 - f):
  46             grid = grid[1:]
  47             grid.append(gm.expand_right(grid, lh))
  48             yield grid
  49
  50 def score(grid, points, limit):
  51     d = max(map(lambda l: dst((0, 0), points_to_line(*l)), grid + grid))
  52     if d > limit:
  53         return 0
  54     return len([p for p in points if nearest2(grid, p) <= 2])
  55
  56 points = pickle.load(open('edges.pickle'))
  57
  58 lines = pickle.load(open('lines.pickle'))
  59
  60 r_lines = pickle.load(open('r_lines.pickle'))
  61
  62 #pyplot.scatter(*zip(*sum(r_lines, [])))
  63 #pyplot.show()
  64
  65 l1, l2 = lines
  66
  67 lines_general = map(to_general, sum(lines, []))
  68 near_points = [p for p in points if nearest(lines_general, p) <= 2]
  69
  70 while True:
  71     t0 = time.time()
  72     #l1s = random.sample(l1, 2)
  73     l1s = [l1[0], l1[-1]]
  74     l1s.sort(key=lambda l: l[1])
  75     sgrid = map(lambda l:linef.line_from_angl_dist(l, size), l1s)
  76     middle = lambda m: ((m, 0),(m, 390))
  77     middle = middle(gm.intersection((sgrid[0][0], sgrid[1][1]),
  78                                     (sgrid[0][1], sgrid[1][0]))[0])
  79     lh = (gm.intersection(sgrid[0], middle), gm.intersection(sgrid[1], middle))
  80     sc, grid = max(map(lambda g: (score(g, points, 400), g), generate_models(sgrid, lh)))
  81     map(lambda l: pyplot.plot(*zip(*l), color='b'), grid)
  82     #l2s = random.sample(l2, 2)
  83     l2s = [l2[0], l2[-1]]
  84     l2s.sort(key=lambda l: l[1])
  85     sgrid = map(lambda l:linef.line_from_angl_dist(l, size), l2s)
  86     middle = lambda m: ((0, m),(520, m))
  87     middle = middle(gm.intersection((sgrid[0][0], sgrid[1][1]),
  88                                     (sgrid[0][1], sgrid[1][0]))[1])
  89     lh = (gm.intersection(sgrid[0], middle), gm.intersection(sgrid[1], middle))
  90     sc, grid = max(map(lambda g: (score(g, points, 530), g), generate_models(sgrid, lh)))
  91     print time.time() - t0
  92
  93     pyplot.scatter(*zip(*near_points))
  94     map(lambda l: pyplot.plot(*zip(*l), color='b'), grid)
  95     plot_line(l2s[0], 'r')
  96     plot_line(l2s[1], 'r')
  97     plot_line(l1s[0], 'r')
  98     plot_line(l1s[1], 'r')
  99     pyplot.xlim(0, 520)
 100     pyplot.ylim(0, 390)
 101     pyplot.show()
 102
 103 sys.exit()
 104
 105
 106
 107
 108 for l in lines[0]:
 109     plot_line(l, 'g')
 110
 111 for l in lines[1]:
 112     plot_line(l, 'g')
 113
 114 pyplot.xlim(0, 520)
 115 pyplot.ylim(0, 390)
 116 pyplot.show()