• Home
  • Line#
  • Scopes#
  • Navigate#
  • Raw
  • Download
1#!/usr/bin/env python
2
3import sys
4import math
5import time
6import random
7
8import numpy
9import transformations
10import cv2.cv as cv
11
12def clamp(a, x, b):
13    return numpy.maximum(a, numpy.minimum(x, b))
14
15def norm(v):
16    mag = numpy.sqrt(sum([e * e for e in v]))
17    return v / mag
18
19class Vec3:
20    def __init__(self, x, y, z):
21        self.v = (x, y, z)
22    def x(self):
23        return self.v[0]
24    def y(self):
25        return self.v[1]
26    def z(self):
27        return self.v[2]
28    def __repr__(self):
29        return "<Vec3 (%s,%s,%s)>" % tuple([repr(c) for c in self.v])
30    def __add__(self, other):
31        return Vec3(*[self.v[i] + other.v[i] for i in range(3)])
32    def __sub__(self, other):
33        return Vec3(*[self.v[i] - other.v[i] for i in range(3)])
34    def __mul__(self, other):
35        if isinstance(other, Vec3):
36            return Vec3(*[self.v[i] * other.v[i] for i in range(3)])
37        else:
38            return Vec3(*[self.v[i] * other for i in range(3)])
39    def mag2(self):
40        return sum([e * e for e in self.v])
41    def __abs__(self):
42        return numpy.sqrt(sum([e * e for e in self.v]))
43    def norm(self):
44        return self * (1.0 / abs(self))
45    def dot(self, other):
46        return sum([self.v[i] * other.v[i] for i in range(3)])
47    def cross(self, other):
48        (ax, ay, az) = self.v
49        (bx, by, bz) = other.v
50        return Vec3(ay * bz - by * az, az * bx - bz * ax, ax * by - bx * ay)
51
52
53class Ray:
54
55    def __init__(self, o, d):
56        self.o = o
57        self.d = d
58
59    def project(self, d):
60        return self.o + self.d * d
61
62class Camera:
63
64    def __init__(self, F):
65        R = Vec3(1., 0., 0.)
66        U = Vec3(0, 1., 0)
67        self.center = Vec3(0, 0, 0)
68        self.pcenter = Vec3(0, 0, F)
69        self.up = U
70        self.right = R
71
72    def genray(self, x, y):
73        """ -1 <= y <= 1 """
74        r = numpy.sqrt(x * x + y * y)
75        if 0:
76            rprime = r + (0.17 * r**2)
77        else:
78            rprime = (10 * numpy.sqrt(17 * r + 25) - 50) / 17
79        print "scale", rprime / r
80        x *= rprime / r
81        y *= rprime / r
82        o = self.center
83        r = (self.pcenter + (self.right * x) + (self.up * y)) - o
84        return Ray(o, r.norm())
85
86class Sphere:
87
88    def __init__(self, center, radius):
89        self.center = center
90        self.radius = radius
91
92    def hit(self, r):
93        # a = mag2(r.d)
94        a = 1.
95        v = r.o - self.center
96        b = 2 * r.d.dot(v)
97        c = self.center.mag2() + r.o.mag2() + -2 * self.center.dot(r.o) - (self.radius ** 2)
98        det = (b * b) - (4 * c)
99        pred = 0 < det
100
101        sq = numpy.sqrt(abs(det))
102        h0 = (-b - sq) / (2)
103        h1 = (-b + sq) / (2)
104
105        h = numpy.minimum(h0, h1)
106
107        pred = pred & (h > 0)
108        normal = (r.project(h) - self.center) * (1.0 / self.radius)
109        return (pred, numpy.where(pred, h, 999999.), normal)
110
111def pt2plane(p, plane):
112    return p.dot(plane) * (1. / abs(plane))
113
114class Plane:
115
116    def __init__(self, p, n, right):
117        self.D = -pt2plane(p, n)
118        self.Pn = n
119        self.right = right
120        self.rightD = -pt2plane(p, right)
121        self.up = n.cross(right)
122        self.upD = -pt2plane(p, self.up)
123
124    def hit(self, r):
125        Vd = self.Pn.dot(r.d)
126        V0 = -(self.Pn.dot(r.o) + self.D)
127        h = V0 / Vd
128        pred = (0 <= h)
129
130        return (pred, numpy.where(pred, h, 999999.), self.Pn)
131
132    def localxy(self, loc):
133        x = (loc.dot(self.right) + self.rightD)
134        y = (loc.dot(self.up) + self.upD)
135        return (x, y)
136
137# lena = numpy.fromstring(cv.LoadImage("../samples/c/lena.jpg", 0).tostring(), numpy.uint8) / 255.0
138
139def texture(xy):
140    x,y = xy
141    xa = numpy.floor(x * 512)
142    ya = numpy.floor(y * 512)
143    a = (512 * ya) + xa
144    safe = (0 <= x) & (0 <= y) & (x < 1) & (y < 1)
145    if 0:
146        a = numpy.where(safe, a, 0).astype(numpy.int)
147        return numpy.where(safe, numpy.take(lena, a), 0.0)
148    else:
149        xi = numpy.floor(x * 11).astype(numpy.int)
150        yi = numpy.floor(y * 11).astype(numpy.int)
151        inside = (1 <= xi) & (xi < 10) & (2 <= yi) & (yi < 9)
152        checker = (xi & 1) ^ (yi & 1)
153        final = numpy.where(inside, checker, 1.0)
154        return numpy.where(safe, final, 0.5)
155
156def under(vv, m):
157    return Vec3(*(numpy.dot(m, vv.v + (1,))[:3]))
158
159class Renderer:
160
161    def __init__(self, w, h, oversample):
162        self.w = w
163        self.h = h
164
165        random.seed(1)
166        x = numpy.arange(self.w*self.h) % self.w
167        y = numpy.floor(numpy.arange(self.w*self.h) / self.w)
168        h2 = h / 2.0
169        w2 = w / 2.0
170        self.r = [ None ] * oversample
171        for o in range(oversample):
172            stoch_x = numpy.random.rand(self.w * self.h)
173            stoch_y = numpy.random.rand(self.w * self.h)
174            nx = (x + stoch_x - 0.5 - w2) / h2
175            ny = (y + stoch_y - 0.5 - h2) / h2
176            self.r[o] = cam.genray(nx, ny)
177
178        self.rnds = [random.random() for i in range(10)]
179
180    def frame(self, i):
181
182        rnds = self.rnds
183        roll = math.sin(i * .01 * rnds[0] + rnds[1])
184        pitch = math.sin(i * .01 * rnds[2] + rnds[3])
185        yaw = math.pi * math.sin(i * .01 * rnds[4] + rnds[5])
186        x = math.sin(i * 0.01 * rnds[6])
187        y = math.sin(i * 0.01 * rnds[7])
188
189        x,y,z = -0.5,0.5,1
190        roll,pitch,yaw = (0,0,0)
191
192        z = 4 + 3 * math.sin(i * 0.1 * rnds[8])
193        print z
194
195        rz = transformations.euler_matrix(roll, pitch, yaw)
196        p = Plane(Vec3(x, y, z), under(Vec3(0,0,-1), rz), under(Vec3(1, 0, 0), rz))
197
198        acc = 0
199        for r in self.r:
200            (pred, h, norm) = p.hit(r)
201            l = numpy.where(pred, texture(p.localxy(r.project(h))), 0.0)
202            acc += l
203        acc *= (1.0 / len(self.r))
204
205        # print "took", time.time() - st
206
207        img = cv.CreateMat(self.h, self.w, cv.CV_8UC1)
208        cv.SetData(img, (clamp(0, acc, 1) * 255).astype(numpy.uint8).tostring(), self.w)
209        return img
210
211#########################################################################
212
213num_x_ints = 8
214num_y_ints = 6
215num_pts = num_x_ints * num_y_ints
216
217def get_corners(mono, refine = False):
218    (ok, corners) = cv.FindChessboardCorners(mono, (num_x_ints, num_y_ints), cv.CV_CALIB_CB_ADAPTIVE_THRESH | cv.CV_CALIB_CB_NORMALIZE_IMAGE)
219    if refine and ok:
220        corners = cv.FindCornerSubPix(mono, corners, (5,5), (-1,-1), ( cv.CV_TERMCRIT_EPS+cv.CV_TERMCRIT_ITER, 30, 0.1 ))
221    return (ok, corners)
222
223def mk_object_points(nimages, squaresize = 1):
224    opts = cv.CreateMat(nimages * num_pts, 3, cv.CV_32FC1)
225    for i in range(nimages):
226        for j in range(num_pts):
227            opts[i * num_pts + j, 0] = (j / num_x_ints) * squaresize
228            opts[i * num_pts + j, 1] = (j % num_x_ints) * squaresize
229            opts[i * num_pts + j, 2] = 0
230    return opts
231
232def mk_image_points(goodcorners):
233    ipts = cv.CreateMat(len(goodcorners) * num_pts, 2, cv.CV_32FC1)
234    for (i, co) in enumerate(goodcorners):
235        for j in range(num_pts):
236            ipts[i * num_pts + j, 0] = co[j][0]
237            ipts[i * num_pts + j, 1] = co[j][1]
238    return ipts
239
240def mk_point_counts(nimages):
241    npts = cv.CreateMat(nimages, 1, cv.CV_32SC1)
242    for i in range(nimages):
243        npts[i, 0] = num_pts
244    return npts
245
246def cvmat_iterator(cvmat):
247    for i in range(cvmat.rows):
248        for j in range(cvmat.cols):
249            yield cvmat[i,j]
250
251cam = Camera(3.0)
252rend = Renderer(640, 480, 2)
253cv.NamedWindow("snap")
254
255#images = [rend.frame(i) for i in range(0, 2000, 400)]
256images = [rend.frame(i) for i in [1200]]
257
258if 0:
259    for i,img in enumerate(images):
260        cv.SaveImage("final/%06d.png" % i, img)
261
262size = cv.GetSize(images[0])
263corners = [get_corners(i) for i in images]
264
265goodcorners = [co for (im, (ok, co)) in zip(images, corners) if ok]
266
267def checkerboard_error(xformed):
268    def pt2line(a, b, c):
269        x0,y0 = a
270        x1,y1 = b
271        x2,y2 = c
272        return abs((x2 - x1) * (y1 - y0) - (x1 - x0) * (y2 - y1)) / math.sqrt((x2 - x1) ** 2 + (y2 - y1) ** 2)
273    errorsum = 0.
274    for im in xformed:
275        for row in range(6):
276            l0 = im[8 * row]
277            l1 = im[8 * row + 7]
278            for col in range(1, 7):
279                e = pt2line(im[8 * row + col], l0, l1)
280                #print "row", row, "e", e
281                errorsum += e
282
283    return errorsum
284
285if True:
286    from scipy.optimize import fmin
287
288    def xf(pt, poly):
289        x, y = pt
290        r = math.sqrt((x - 320) ** 2 + (y - 240) ** 2)
291        fr = poly(r) / r
292        return (320 + (x - 320) * fr, 240 + (y - 240) * fr)
293    def silly(p, goodcorners):
294    #    print "eval", p
295
296        d = 1.0 # - sum(p)
297        poly = numpy.poly1d(list(p) + [d, 0.])
298
299        xformed = [[xf(pt, poly) for pt in co] for co in goodcorners]
300
301        return checkerboard_error(xformed)
302
303    x0 = [ 0. ]
304    #print silly(x0, goodcorners)
305    print "initial error", silly(x0, goodcorners)
306    xopt = fmin(silly, x0, args=(goodcorners,))
307    print "xopt", xopt
308    print "final error", silly(xopt, goodcorners)
309
310    d = 1.0 # - sum(xopt)
311    poly = numpy.poly1d(list(xopt) + [d, 0.])
312    print "final polynomial"
313    print poly
314
315    for co in goodcorners:
316        scrib = cv.CreateMat(480, 640, cv.CV_8UC3)
317        cv.SetZero(scrib)
318        cv.DrawChessboardCorners(scrib, (num_x_ints, num_y_ints), [xf(pt, poly) for pt in co], True)
319        cv.ShowImage("snap", scrib)
320        cv.WaitKey()
321
322    sys.exit(0)
323
324for (i, (img, (ok, co))) in enumerate(zip(images, corners)):
325    scrib = cv.CreateMat(img.rows, img.cols, cv.CV_8UC3)
326    cv.CvtColor(img, scrib, cv.CV_GRAY2BGR)
327    if ok:
328        cv.DrawChessboardCorners(scrib, (num_x_ints, num_y_ints), co, True)
329    cv.ShowImage("snap", scrib)
330    cv.WaitKey()
331
332print len(goodcorners)
333ipts = mk_image_points(goodcorners)
334opts = mk_object_points(len(goodcorners), .1)
335npts = mk_point_counts(len(goodcorners))
336
337intrinsics = cv.CreateMat(3, 3, cv.CV_64FC1)
338distortion = cv.CreateMat(4, 1, cv.CV_64FC1)
339cv.SetZero(intrinsics)
340cv.SetZero(distortion)
341# focal lengths have 1/1 ratio
342intrinsics[0,0] = 1.0
343intrinsics[1,1] = 1.0
344cv.CalibrateCamera2(opts, ipts, npts,
345           cv.GetSize(images[0]),
346           intrinsics,
347           distortion,
348           cv.CreateMat(len(goodcorners), 3, cv.CV_32FC1),
349           cv.CreateMat(len(goodcorners), 3, cv.CV_32FC1),
350           flags = 0) # cv.CV_CALIB_ZERO_TANGENT_DIST)
351print "D =", list(cvmat_iterator(distortion))
352print "K =", list(cvmat_iterator(intrinsics))
353mapx = cv.CreateImage((640, 480), cv.IPL_DEPTH_32F, 1)
354mapy = cv.CreateImage((640, 480), cv.IPL_DEPTH_32F, 1)
355cv.InitUndistortMap(intrinsics, distortion, mapx, mapy)
356for img in images:
357    r = cv.CloneMat(img)
358    cv.Remap(img, r, mapx, mapy)
359    cv.ShowImage("snap", r)
360    cv.WaitKey()
361