57d4c86b2b
Also, removed the one from modules/python/src2/cv.py and cleared its executable bit, since it's not a script.
361 lines
10 KiB
Python
Executable File
361 lines
10 KiB
Python
Executable File
#!/usr/bin/env python
|
|
|
|
import sys
|
|
import math
|
|
import time
|
|
import random
|
|
|
|
import numpy
|
|
import transformations
|
|
import cv2.cv as cv
|
|
|
|
def clamp(a, x, b):
|
|
return numpy.maximum(a, numpy.minimum(x, b))
|
|
|
|
def norm(v):
|
|
mag = numpy.sqrt(sum([e * e for e in v]))
|
|
return v / mag
|
|
|
|
class Vec3:
|
|
def __init__(self, x, y, z):
|
|
self.v = (x, y, z)
|
|
def x(self):
|
|
return self.v[0]
|
|
def y(self):
|
|
return self.v[1]
|
|
def z(self):
|
|
return self.v[2]
|
|
def __repr__(self):
|
|
return "<Vec3 (%s,%s,%s)>" % tuple([repr(c) for c in self.v])
|
|
def __add__(self, other):
|
|
return Vec3(*[self.v[i] + other.v[i] for i in range(3)])
|
|
def __sub__(self, other):
|
|
return Vec3(*[self.v[i] - other.v[i] for i in range(3)])
|
|
def __mul__(self, other):
|
|
if isinstance(other, Vec3):
|
|
return Vec3(*[self.v[i] * other.v[i] for i in range(3)])
|
|
else:
|
|
return Vec3(*[self.v[i] * other for i in range(3)])
|
|
def mag2(self):
|
|
return sum([e * e for e in self.v])
|
|
def __abs__(self):
|
|
return numpy.sqrt(sum([e * e for e in self.v]))
|
|
def norm(self):
|
|
return self * (1.0 / abs(self))
|
|
def dot(self, other):
|
|
return sum([self.v[i] * other.v[i] for i in range(3)])
|
|
def cross(self, other):
|
|
(ax, ay, az) = self.v
|
|
(bx, by, bz) = other.v
|
|
return Vec3(ay * bz - by * az, az * bx - bz * ax, ax * by - bx * ay)
|
|
|
|
|
|
class Ray:
|
|
|
|
def __init__(self, o, d):
|
|
self.o = o
|
|
self.d = d
|
|
|
|
def project(self, d):
|
|
return self.o + self.d * d
|
|
|
|
class Camera:
|
|
|
|
def __init__(self, F):
|
|
R = Vec3(1., 0., 0.)
|
|
U = Vec3(0, 1., 0)
|
|
self.center = Vec3(0, 0, 0)
|
|
self.pcenter = Vec3(0, 0, F)
|
|
self.up = U
|
|
self.right = R
|
|
|
|
def genray(self, x, y):
|
|
""" -1 <= y <= 1 """
|
|
r = numpy.sqrt(x * x + y * y)
|
|
if 0:
|
|
rprime = r + (0.17 * r**2)
|
|
else:
|
|
rprime = (10 * numpy.sqrt(17 * r + 25) - 50) / 17
|
|
print "scale", rprime / r
|
|
x *= rprime / r
|
|
y *= rprime / r
|
|
o = self.center
|
|
r = (self.pcenter + (self.right * x) + (self.up * y)) - o
|
|
return Ray(o, r.norm())
|
|
|
|
class Sphere:
|
|
|
|
def __init__(self, center, radius):
|
|
self.center = center
|
|
self.radius = radius
|
|
|
|
def hit(self, r):
|
|
# a = mag2(r.d)
|
|
a = 1.
|
|
v = r.o - self.center
|
|
b = 2 * r.d.dot(v)
|
|
c = self.center.mag2() + r.o.mag2() + -2 * self.center.dot(r.o) - (self.radius ** 2)
|
|
det = (b * b) - (4 * c)
|
|
pred = 0 < det
|
|
|
|
sq = numpy.sqrt(abs(det))
|
|
h0 = (-b - sq) / (2)
|
|
h1 = (-b + sq) / (2)
|
|
|
|
h = numpy.minimum(h0, h1)
|
|
|
|
pred = pred & (h > 0)
|
|
normal = (r.project(h) - self.center) * (1.0 / self.radius)
|
|
return (pred, numpy.where(pred, h, 999999.), normal)
|
|
|
|
def pt2plane(p, plane):
|
|
return p.dot(plane) * (1. / abs(plane))
|
|
|
|
class Plane:
|
|
|
|
def __init__(self, p, n, right):
|
|
self.D = -pt2plane(p, n)
|
|
self.Pn = n
|
|
self.right = right
|
|
self.rightD = -pt2plane(p, right)
|
|
self.up = n.cross(right)
|
|
self.upD = -pt2plane(p, self.up)
|
|
|
|
def hit(self, r):
|
|
Vd = self.Pn.dot(r.d)
|
|
V0 = -(self.Pn.dot(r.o) + self.D)
|
|
h = V0 / Vd
|
|
pred = (0 <= h)
|
|
|
|
return (pred, numpy.where(pred, h, 999999.), self.Pn)
|
|
|
|
def localxy(self, loc):
|
|
x = (loc.dot(self.right) + self.rightD)
|
|
y = (loc.dot(self.up) + self.upD)
|
|
return (x, y)
|
|
|
|
# lena = numpy.fromstring(cv.LoadImage("../samples/c/lena.jpg", 0).tostring(), numpy.uint8) / 255.0
|
|
|
|
def texture(xy):
|
|
x,y = xy
|
|
xa = numpy.floor(x * 512)
|
|
ya = numpy.floor(y * 512)
|
|
a = (512 * ya) + xa
|
|
safe = (0 <= x) & (0 <= y) & (x < 1) & (y < 1)
|
|
if 0:
|
|
a = numpy.where(safe, a, 0).astype(numpy.int)
|
|
return numpy.where(safe, numpy.take(lena, a), 0.0)
|
|
else:
|
|
xi = numpy.floor(x * 11).astype(numpy.int)
|
|
yi = numpy.floor(y * 11).astype(numpy.int)
|
|
inside = (1 <= xi) & (xi < 10) & (2 <= yi) & (yi < 9)
|
|
checker = (xi & 1) ^ (yi & 1)
|
|
final = numpy.where(inside, checker, 1.0)
|
|
return numpy.where(safe, final, 0.5)
|
|
|
|
def under(vv, m):
|
|
return Vec3(*(numpy.dot(m, vv.v + (1,))[:3]))
|
|
|
|
class Renderer:
|
|
|
|
def __init__(self, w, h, oversample):
|
|
self.w = w
|
|
self.h = h
|
|
|
|
random.seed(1)
|
|
x = numpy.arange(self.w*self.h) % self.w
|
|
y = numpy.floor(numpy.arange(self.w*self.h) / self.w)
|
|
h2 = h / 2.0
|
|
w2 = w / 2.0
|
|
self.r = [ None ] * oversample
|
|
for o in range(oversample):
|
|
stoch_x = numpy.random.rand(self.w * self.h)
|
|
stoch_y = numpy.random.rand(self.w * self.h)
|
|
nx = (x + stoch_x - 0.5 - w2) / h2
|
|
ny = (y + stoch_y - 0.5 - h2) / h2
|
|
self.r[o] = cam.genray(nx, ny)
|
|
|
|
self.rnds = [random.random() for i in range(10)]
|
|
|
|
def frame(self, i):
|
|
|
|
rnds = self.rnds
|
|
roll = math.sin(i * .01 * rnds[0] + rnds[1])
|
|
pitch = math.sin(i * .01 * rnds[2] + rnds[3])
|
|
yaw = math.pi * math.sin(i * .01 * rnds[4] + rnds[5])
|
|
x = math.sin(i * 0.01 * rnds[6])
|
|
y = math.sin(i * 0.01 * rnds[7])
|
|
|
|
x,y,z = -0.5,0.5,1
|
|
roll,pitch,yaw = (0,0,0)
|
|
|
|
z = 4 + 3 * math.sin(i * 0.1 * rnds[8])
|
|
print z
|
|
|
|
rz = transformations.euler_matrix(roll, pitch, yaw)
|
|
p = Plane(Vec3(x, y, z), under(Vec3(0,0,-1), rz), under(Vec3(1, 0, 0), rz))
|
|
|
|
acc = 0
|
|
for r in self.r:
|
|
(pred, h, norm) = p.hit(r)
|
|
l = numpy.where(pred, texture(p.localxy(r.project(h))), 0.0)
|
|
acc += l
|
|
acc *= (1.0 / len(self.r))
|
|
|
|
# print "took", time.time() - st
|
|
|
|
img = cv.CreateMat(self.h, self.w, cv.CV_8UC1)
|
|
cv.SetData(img, (clamp(0, acc, 1) * 255).astype(numpy.uint8).tostring(), self.w)
|
|
return img
|
|
|
|
#########################################################################
|
|
|
|
num_x_ints = 8
|
|
num_y_ints = 6
|
|
num_pts = num_x_ints * num_y_ints
|
|
|
|
def get_corners(mono, refine = False):
|
|
(ok, corners) = cv.FindChessboardCorners(mono, (num_x_ints, num_y_ints), cv.CV_CALIB_CB_ADAPTIVE_THRESH | cv.CV_CALIB_CB_NORMALIZE_IMAGE)
|
|
if refine and ok:
|
|
corners = cv.FindCornerSubPix(mono, corners, (5,5), (-1,-1), ( cv.CV_TERMCRIT_EPS+cv.CV_TERMCRIT_ITER, 30, 0.1 ))
|
|
return (ok, corners)
|
|
|
|
def mk_object_points(nimages, squaresize = 1):
|
|
opts = cv.CreateMat(nimages * num_pts, 3, cv.CV_32FC1)
|
|
for i in range(nimages):
|
|
for j in range(num_pts):
|
|
opts[i * num_pts + j, 0] = (j / num_x_ints) * squaresize
|
|
opts[i * num_pts + j, 1] = (j % num_x_ints) * squaresize
|
|
opts[i * num_pts + j, 2] = 0
|
|
return opts
|
|
|
|
def mk_image_points(goodcorners):
|
|
ipts = cv.CreateMat(len(goodcorners) * num_pts, 2, cv.CV_32FC1)
|
|
for (i, co) in enumerate(goodcorners):
|
|
for j in range(num_pts):
|
|
ipts[i * num_pts + j, 0] = co[j][0]
|
|
ipts[i * num_pts + j, 1] = co[j][1]
|
|
return ipts
|
|
|
|
def mk_point_counts(nimages):
|
|
npts = cv.CreateMat(nimages, 1, cv.CV_32SC1)
|
|
for i in range(nimages):
|
|
npts[i, 0] = num_pts
|
|
return npts
|
|
|
|
def cvmat_iterator(cvmat):
|
|
for i in range(cvmat.rows):
|
|
for j in range(cvmat.cols):
|
|
yield cvmat[i,j]
|
|
|
|
cam = Camera(3.0)
|
|
rend = Renderer(640, 480, 2)
|
|
cv.NamedWindow("snap")
|
|
|
|
#images = [rend.frame(i) for i in range(0, 2000, 400)]
|
|
images = [rend.frame(i) for i in [1200]]
|
|
|
|
if 0:
|
|
for i,img in enumerate(images):
|
|
cv.SaveImage("final/%06d.png" % i, img)
|
|
|
|
size = cv.GetSize(images[0])
|
|
corners = [get_corners(i) for i in images]
|
|
|
|
goodcorners = [co for (im, (ok, co)) in zip(images, corners) if ok]
|
|
|
|
def checkerboard_error(xformed):
|
|
def pt2line(a, b, c):
|
|
x0,y0 = a
|
|
x1,y1 = b
|
|
x2,y2 = c
|
|
return abs((x2 - x1) * (y1 - y0) - (x1 - x0) * (y2 - y1)) / math.sqrt((x2 - x1) ** 2 + (y2 - y1) ** 2)
|
|
errorsum = 0.
|
|
for im in xformed:
|
|
for row in range(6):
|
|
l0 = im[8 * row]
|
|
l1 = im[8 * row + 7]
|
|
for col in range(1, 7):
|
|
e = pt2line(im[8 * row + col], l0, l1)
|
|
#print "row", row, "e", e
|
|
errorsum += e
|
|
|
|
return errorsum
|
|
|
|
if True:
|
|
from scipy.optimize import fmin
|
|
|
|
def xf(pt, poly):
|
|
x, y = pt
|
|
r = math.sqrt((x - 320) ** 2 + (y - 240) ** 2)
|
|
fr = poly(r) / r
|
|
return (320 + (x - 320) * fr, 240 + (y - 240) * fr)
|
|
def silly(p, goodcorners):
|
|
# print "eval", p
|
|
|
|
d = 1.0 # - sum(p)
|
|
poly = numpy.poly1d(list(p) + [d, 0.])
|
|
|
|
xformed = [[xf(pt, poly) for pt in co] for co in goodcorners]
|
|
|
|
return checkerboard_error(xformed)
|
|
|
|
x0 = [ 0. ]
|
|
#print silly(x0, goodcorners)
|
|
print "initial error", silly(x0, goodcorners)
|
|
xopt = fmin(silly, x0, args=(goodcorners,))
|
|
print "xopt", xopt
|
|
print "final error", silly(xopt, goodcorners)
|
|
|
|
d = 1.0 # - sum(xopt)
|
|
poly = numpy.poly1d(list(xopt) + [d, 0.])
|
|
print "final polynomial"
|
|
print poly
|
|
|
|
for co in goodcorners:
|
|
scrib = cv.CreateMat(480, 640, cv.CV_8UC3)
|
|
cv.SetZero(scrib)
|
|
cv.DrawChessboardCorners(scrib, (num_x_ints, num_y_ints), [xf(pt, poly) for pt in co], True)
|
|
cv.ShowImage("snap", scrib)
|
|
cv.WaitKey()
|
|
|
|
sys.exit(0)
|
|
|
|
for (i, (img, (ok, co))) in enumerate(zip(images, corners)):
|
|
scrib = cv.CreateMat(img.rows, img.cols, cv.CV_8UC3)
|
|
cv.CvtColor(img, scrib, cv.CV_GRAY2BGR)
|
|
if ok:
|
|
cv.DrawChessboardCorners(scrib, (num_x_ints, num_y_ints), co, True)
|
|
cv.ShowImage("snap", scrib)
|
|
cv.WaitKey()
|
|
|
|
print len(goodcorners)
|
|
ipts = mk_image_points(goodcorners)
|
|
opts = mk_object_points(len(goodcorners), .1)
|
|
npts = mk_point_counts(len(goodcorners))
|
|
|
|
intrinsics = cv.CreateMat(3, 3, cv.CV_64FC1)
|
|
distortion = cv.CreateMat(4, 1, cv.CV_64FC1)
|
|
cv.SetZero(intrinsics)
|
|
cv.SetZero(distortion)
|
|
# focal lengths have 1/1 ratio
|
|
intrinsics[0,0] = 1.0
|
|
intrinsics[1,1] = 1.0
|
|
cv.CalibrateCamera2(opts, ipts, npts,
|
|
cv.GetSize(images[0]),
|
|
intrinsics,
|
|
distortion,
|
|
cv.CreateMat(len(goodcorners), 3, cv.CV_32FC1),
|
|
cv.CreateMat(len(goodcorners), 3, cv.CV_32FC1),
|
|
flags = 0) # cv.CV_CALIB_ZERO_TANGENT_DIST)
|
|
print "D =", list(cvmat_iterator(distortion))
|
|
print "K =", list(cvmat_iterator(intrinsics))
|
|
mapx = cv.CreateImage((640, 480), cv.IPL_DEPTH_32F, 1)
|
|
mapy = cv.CreateImage((640, 480), cv.IPL_DEPTH_32F, 1)
|
|
cv.InitUndistortMap(intrinsics, distortion, mapx, mapy)
|
|
for img in images:
|
|
r = cv.CloneMat(img)
|
|
cv.Remap(img, r, mapx, mapy)
|
|
cv.ShowImage("snap", r)
|
|
cv.WaitKey()
|