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