-
Notifications
You must be signed in to change notification settings - Fork 209
/
optimize_crappy.py
136 lines (113 loc) · 3.35 KB
/
optimize_crappy.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
from helpers import add_ones
#import autograd.numpy as np
from scipy.optimize import least_squares, leastsq
import cv2
import numpy as np
import sympy as sp
from sympy.utilities.autowrap import autowrap, ufuncify, binary_function
from sympy.printing.ccode import ccode
EPS = 1e-10
def rotation_from_matrix(R):
return cv2.Rodrigues(R)[0].flatten()
def rotation_to_matrix(w):
wx,wy,wz = w
theta = sp.sqrt(wx**2 + wy**2 + wz**2 + wy**2 + wz**2) + EPS
omega = sp.Matrix([[0,-wz,wy],
[wz,0,-wx],
[-wy,wx,0]])
R = sp.eye(3) +\
omega*(sp.sin(theta)/theta) +\
(omega*omega)*((1-sp.cos(theta))/(theta*theta))
return R
"""
# test these
assert(np.allclose(np.eye(3), rotation_to_matrix(np.array([0,0,0]))))
for i in range(20):
w = np.random.randn(3)
what = rotation_from_matrix(rotation_to_matrix(w))
assert(np.allclose(w, what))
"""
def optimize(frames, points, *args):
# get point location guesses + camera poses (initial parameter vector)
x0 = []
for p in points:
x0.append(p.pt)
for f in frames:
t = f.pose[:3, 3]
R = f.pose[:3, :3]
w = rotation_from_matrix(R)
x0.append(t)
x0.append(w)
x0 = np.array(x0).flatten()
# get target residuals (measurement vector)
uvs = []
for p in points:
for f, idx in zip(p.frames, p.idxs):
uv = f.kps[idx]
uvs.append(uv)
b = np.array(uvs).flatten()
# f(ptw(9)) -> uv(2)
def proj(p, t, w):
R = rotation_to_matrix(w)
proj = (R * p)+t
return (proj[0] / proj[2], proj[1] / proj[2])
def get_symbolic_jacobians():
p = sp.Matrix(sp.symbols("px py pz"))
t = sp.Matrix(sp.symbols("tx ty tz"))
w = sp.Matrix(sp.symbols("wx wy wz"))
uv = sp.Matrix(proj(p, t, w))
fuv = autowrap(uv)
fjp = autowrap(uv.jacobian(p))
fjt = autowrap(uv.jacobian(t))
fjw = autowrap(uv.jacobian(w))
return fuv,fjp,fjt,fjw
fuv,fjp,fjt,fjw = get_symbolic_jacobians()
# compute residuals f(x) = b'
def res(x):
J = np.zeros((b.shape[0], x0.shape[0]))
ret = []
j = 0
for i, p in enumerate(points):
for f, idx in zip(p.frames, p.idxs):
pt = x[i*3:(i+1)*3]
fidx = len(points)*3 + f.id*6
tw = x[fidx:fidx+6]
ptw = np.concatenate([pt, tw], axis=0).tolist()
uv = fuv(*ptw)
J[j*2:(j+1)*2, i*3:(i+1)*3] = fjp(*ptw)
J[j*2:(j+1)*2, fidx:fidx+3] = fjt(*ptw)
J[j*2:(j+1)*2, fidx+3:fidx+6] = fjw(*ptw)
j += 1
ret.append(uv)
return np.array(ret).flatten(), J
bhat, J = res(x0)
print(J)
print(J.shape)
print(np.sum((bhat-b)**2))
exit(0)
# TODO: actually do this
# http://www.cs.technion.ac.il/users/wwwb/cgi-bin/tr-get.cgi/2014/MSC/MSC-2014-16.pdf page 17
# http://www.telesens.co/2016/10/13/bundle-adjustment-part-1-jacobians/ defines 2x3 jacobian
# define function fun(parameter) = measurement
def fun(x):
return np.sum((res(x)-b)**2)
# stack poses
grad_fun = grad(fun)
print("computing at x0")
# gradient descent
for i in range(20):
loss = fun(x0)
d = grad_fun(x0)
print(loss, d)
x0 -= d
"""
poses = []
for i, p in enumerate(self.points):
for f, idx in zip(p.frames, p.idxs):
poses.append(f.pose[:3])
poses = np.concatenate(poses, axis=1)
print(poses.shape)
loss = np.dot(poses, x0)
print(loss)
"""
print("running least squares with %d params" % len(x0))