73 lines
1.6 KiB
Python
73 lines
1.6 KiB
Python
import numpy as np
|
|
import cv2
|
|
|
|
def to_list(a):
|
|
return [tuple(p) for p in a]
|
|
|
|
def anorm2(a):
|
|
return (a*a).sum(-1)
|
|
def anorm(a):
|
|
return np.sqrt( anorm2(a) )
|
|
|
|
def homotrans(H, x, y):
|
|
xs = H[0, 0]*x + H[0, 1]*y + H[0, 2]
|
|
ys = H[1, 0]*x + H[1, 1]*y + H[1, 2]
|
|
s = H[2, 0]*x + H[2, 1]*y + H[2, 2]
|
|
return xs/s, ys/s
|
|
|
|
def to_rect(a):
|
|
a = np.ravel(a)
|
|
if len(a) == 2:
|
|
a = (0, 0, a[0], a[1])
|
|
return np.array(a, np.float64).reshape(2, 2)
|
|
|
|
def rect2rect_mtx(src, dst):
|
|
src, dst = to_rect(src), to_rect(dst)
|
|
cx, cy = (dst[1] - dst[0]) / (src[1] - src[0])
|
|
tx, ty = dst[0] - src[0] * (cx, cy)
|
|
M = np.float64([[ cx, 0, tx],
|
|
[ 0, cy, ty],
|
|
[ 0, 0, 1]])
|
|
return M
|
|
|
|
|
|
def lookat(eye, target, up = (0, 0, 1)):
|
|
fwd = np.asarray(target, np.float64) - eye
|
|
fwd /= anorm(fwd)
|
|
right = np.cross(fwd, up)
|
|
right /= anorm(right)
|
|
down = np.cross(fwd, right)
|
|
Rt = np.zeros((3, 4))
|
|
Rt[:,:3] = [right, down, fwd]
|
|
Rt[:,3] = -np.dot(Rt[:,:3], eye)
|
|
return Rt
|
|
|
|
def mtx2rvec(R):
|
|
pass
|
|
|
|
|
|
if __name__ == '__main__':
|
|
import cv2
|
|
from time import clock
|
|
|
|
'''
|
|
w, h = 640, 480
|
|
while True:
|
|
img = np.zeros((h, w, 3), np.uint8)
|
|
t = clock()
|
|
eye = [5*cos(t), 5*sin(t), 3]
|
|
Rt = lookat(eye, [0, 0, 0])
|
|
'''
|
|
|
|
|
|
|
|
eye = [1, -4, 3]
|
|
target = [0, 0, 0]
|
|
Rt = lookat(eye, [0, 0, 0])
|
|
print Rt
|
|
p = [0, 0, 0]
|
|
print cv2.transform(np.float64([[p]]), Rt)
|
|
|
|
print cv2.SVDecomp(Rt[:,:3])
|
|
|