Spaces:
Build error
Build error
Commit
·
858279b
1
Parent(s):
981c013
Upload calib_utils.py
Browse files- calib_utils.py +212 -0
calib_utils.py
ADDED
@@ -0,0 +1,212 @@
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
1 |
+
import cv2 as cv
|
2 |
+
import numpy as np
|
3 |
+
|
4 |
+
pyramid_source = []
|
5 |
+
pyramid_target = []
|
6 |
+
patch_source_pyramid = []
|
7 |
+
JT_source_pyramid = []
|
8 |
+
Hinv_source_pyramid = []
|
9 |
+
windows = []
|
10 |
+
|
11 |
+
|
12 |
+
class Window:
|
13 |
+
def __init__(self, center_x, center_y, window_size):
|
14 |
+
self.center_x = center_x
|
15 |
+
self.center_y = center_y
|
16 |
+
self.window_size = window_size
|
17 |
+
# The displacement vectors.
|
18 |
+
# Important for simulating the internal calculation vector: g and d. Also they record the final results.
|
19 |
+
self.Dx = 0
|
20 |
+
self.Dy = 0
|
21 |
+
|
22 |
+
self.map_x = None
|
23 |
+
self.map_y = None
|
24 |
+
self.generate_map()
|
25 |
+
|
26 |
+
def generate_map(self):
|
27 |
+
epsilon = 0.001
|
28 |
+
start_x = self.center_x - self.window_size//2
|
29 |
+
start_y = self.center_y - self.window_size//2
|
30 |
+
# print(start_x, start_y)
|
31 |
+
# When window_size is odd, we must use this form to enforce the size of map to be window_size.
|
32 |
+
crop_x = np.arange(start_x, start_x+self.window_size - epsilon, 1.0).astype(np.float32).reshape(1, self.window_size)
|
33 |
+
self.map_x = np.repeat(crop_x, self.window_size, axis=0)
|
34 |
+
crop_y = np.arange(start_y, start_y + self.window_size - epsilon, 1.0).astype(np.float32).reshape(self.window_size, 1)
|
35 |
+
self.map_y = np.repeat(crop_y, self.window_size, axis=1)
|
36 |
+
|
37 |
+
def pyrDown(self):
|
38 |
+
"""
|
39 |
+
When down-sample the original patch, the corresponding point position should be /2.
|
40 |
+
However the maps' coordinate should not be dimply /2, therefore the maps need regenerate.
|
41 |
+
"""
|
42 |
+
self.center_x = self.center_x / 2
|
43 |
+
self.center_y = self.center_y / 2
|
44 |
+
self.Dx = self.Dx / 2
|
45 |
+
self.Dy = self.Dy / 2
|
46 |
+
self.generate_map()
|
47 |
+
|
48 |
+
def pyrUp(self):
|
49 |
+
"""
|
50 |
+
When calculating the pyramidal LK and moving to the next (bigger) pyramid, the patch size will be doubled.
|
51 |
+
Thus the corresponding point position should be *2.
|
52 |
+
|
53 |
+
Here we should consider the displacement vector (Dx, Dy), to simulate the equation: g_(L-1) = 2*(g_L + d_L)
|
54 |
+
(d_L calculated in this level iteration and g_L is inherited from the former level iteration, both stored in
|
55 |
+
displacement vector)
|
56 |
+
"""
|
57 |
+
self.center_x = self.center_x * 2
|
58 |
+
self.center_y = self.center_y * 2
|
59 |
+
self.Dx = self.Dx * 2
|
60 |
+
self.Dy = self.Dy * 2
|
61 |
+
self.generate_map()
|
62 |
+
|
63 |
+
def move(self, delta_x, delta_y):
|
64 |
+
self.Dx += delta_x
|
65 |
+
self.Dy += delta_y
|
66 |
+
|
67 |
+
def crop(self, img):
|
68 |
+
# Notice!!: map_column calculated from x, while map_row calculated from y.
|
69 |
+
# Which contradict to the matrix index.
|
70 |
+
patch = cv.remap(img, self.map_x + self.Dx,
|
71 |
+
self.map_y + self.Dy, cv.INTER_LINEAR)
|
72 |
+
return patch
|
73 |
+
|
74 |
+
|
75 |
+
def generate_weight(patch_size):
|
76 |
+
"""
|
77 |
+
Generate the weight matrix
|
78 |
+
:param patch_size: (Int) The patch_size
|
79 |
+
:return: The weight map (patch_size * patch_size * 1).
|
80 |
+
"""
|
81 |
+
center = [patch_size // 2, patch_size // 2]
|
82 |
+
sigma_x = sigma_y = patch_size // 2
|
83 |
+
maps = np.fromfunction(lambda x, y: ((x - center[0])/sigma_x) ** 2 +
|
84 |
+
((y - center[1])/sigma_y) ** 2,
|
85 |
+
(patch_size, patch_size),
|
86 |
+
dtype=int)
|
87 |
+
return np.expand_dims(np.exp(maps/-2.0), -1)
|
88 |
+
|
89 |
+
|
90 |
+
def craft_pyramid(image, level, pyramid_container):
|
91 |
+
pyramid_container.clear()
|
92 |
+
pyramid_container.append(image)
|
93 |
+
for i in range(level - 1):
|
94 |
+
image = cv.pyrDown(image)
|
95 |
+
pyramid_container.append(image)
|
96 |
+
|
97 |
+
|
98 |
+
def lk_track(face_source, face_target, landmarks_source, window_size, pyramid_level):
|
99 |
+
# Create the image pyramid for both source and target.
|
100 |
+
craft_pyramid(face_source, pyramid_level, pyramid_source)
|
101 |
+
craft_pyramid(face_target, pyramid_level, pyramid_target)
|
102 |
+
|
103 |
+
# Generate the weight map
|
104 |
+
weight_map = generate_weight(window_size)
|
105 |
+
|
106 |
+
# Create windows for cropping patches.
|
107 |
+
windows.clear()
|
108 |
+
for landmark in landmarks_source:
|
109 |
+
x, y = landmark
|
110 |
+
# windows.append(Window(x, y, patch_size, face_source.shape[0], face_source.shape[0]))
|
111 |
+
windows.append(Window(x, y, window_size))
|
112 |
+
|
113 |
+
# Initialize the patches of both the source.
|
114 |
+
# Notice that here both using the same window, i.e., d = 0.
|
115 |
+
# Afterwards, patch_target will be changed while patch_source will fixed.
|
116 |
+
patch_source_pyramid.clear()
|
117 |
+
JT_source_pyramid.clear()
|
118 |
+
Hinv_source_pyramid.clear()
|
119 |
+
|
120 |
+
for level in range(pyramid_level):
|
121 |
+
patch_source = []
|
122 |
+
for window in windows:
|
123 |
+
patch_source.append(window.crop(pyramid_source[level]))
|
124 |
+
if level < pyramid_level - 1:
|
125 |
+
window.pyrDown()
|
126 |
+
|
127 |
+
# Calculate the Jacobian and Hessen matrix of patch_source
|
128 |
+
JT_source = []
|
129 |
+
Hinv_source = []
|
130 |
+
for patch in patch_source:
|
131 |
+
"""
|
132 |
+
# cv.Sobel(_, _, x, y, ...), x indicating the horizontal,
|
133 |
+
# while it's in fact the y axis, for the y is the column.
|
134 |
+
# horizontal means increase at column.
|
135 |
+
"""
|
136 |
+
gradient_x = cv.Sobel(patch, cv.CV_64F, 1, 0, ksize=3)
|
137 |
+
gradient_y = cv.Sobel(patch, cv.CV_64F, 0, 1, ksize=3)
|
138 |
+
gradient_x_w = gradient_x * weight_map
|
139 |
+
gradient_y_w = gradient_y * weight_map
|
140 |
+
|
141 |
+
J_x = np.reshape(gradient_x, (-1, 1))
|
142 |
+
J_y = np.reshape(gradient_y, (-1, 1))
|
143 |
+
J_x_w = np.reshape(gradient_x_w, (-1, 1))
|
144 |
+
J_y_w = np.reshape(gradient_y_w, (-1, 1))
|
145 |
+
|
146 |
+
J = np.concatenate((J_x, J_y), axis=1)
|
147 |
+
J_w = np.concatenate((J_x_w, J_y_w), axis=1)
|
148 |
+
JT_w = np.transpose(J_w)
|
149 |
+
H = np.matmul(JT_w, J)
|
150 |
+
Hinv = np.linalg.inv(H)
|
151 |
+
# Noticed that we only collect the weighted JT here.
|
152 |
+
JT_source.append(JT_w)
|
153 |
+
Hinv_source.append(Hinv)
|
154 |
+
|
155 |
+
# Collect all the pre-processed data in each level.
|
156 |
+
patch_source_pyramid.append(patch_source)
|
157 |
+
JT_source_pyramid.append(JT_source)
|
158 |
+
Hinv_source_pyramid.append(Hinv_source)
|
159 |
+
#
|
160 |
+
# """
|
161 |
+
# Sequential Execution
|
162 |
+
# """
|
163 |
+
max_iter_step = 15
|
164 |
+
for level in range(pyramid_level-1, -1, -1):
|
165 |
+
epsilon_der1 = 1.0 + level
|
166 |
+
for patch_s, window, JT, Hinv in zip(patch_source_pyramid[level], windows, JT_source_pyramid[level], Hinv_source_pyramid[level]):
|
167 |
+
count = 1
|
168 |
+
while True:
|
169 |
+
# Patch of target. which will move in each iteration.
|
170 |
+
patch_t = window.crop(pyramid_target[level])
|
171 |
+
# Calculate the residual
|
172 |
+
r = patch_t - patch_s
|
173 |
+
r = np.reshape(r, (-1, 1))
|
174 |
+
der1 = np.matmul(JT, r)
|
175 |
+
der1_norm = np.linalg.norm(der1)
|
176 |
+
delta = - np.matmul(Hinv, der1)
|
177 |
+
if der1_norm < epsilon_der1 or count > max_iter_step:
|
178 |
+
if level != 0:
|
179 |
+
# When reach the final level, stop the up-sample.
|
180 |
+
window.pyrUp()
|
181 |
+
break
|
182 |
+
else:
|
183 |
+
window.move(delta[0][0], delta[1][0])
|
184 |
+
count += 1
|
185 |
+
predictions = []
|
186 |
+
for window in windows: # type: Window
|
187 |
+
predictions.append([window.center_x + window.Dx, window.center_y + window.Dy])
|
188 |
+
return np.array(predictions)
|
189 |
+
|
190 |
+
|
191 |
+
def track_bidirectional(faces, locations):
|
192 |
+
patch_size = 15
|
193 |
+
frames_num = len(faces)
|
194 |
+
pyramid_level = 4
|
195 |
+
|
196 |
+
forward_pts = [locations[0].copy()]
|
197 |
+
for i in range(1, frames_num):
|
198 |
+
feature_old = faces[i-1] / 255.0
|
199 |
+
feature_new = faces[i] / 255.0
|
200 |
+
location_old = forward_pts[i - 1]
|
201 |
+
forward_pt = lk_track(feature_old, feature_new, location_old, patch_size, pyramid_level)
|
202 |
+
forward_pts.append(forward_pt)
|
203 |
+
|
204 |
+
feedback_pts = [None] * (frames_num - 1) + [forward_pts[-1].copy()]
|
205 |
+
for i in range(frames_num - 2, -1, -1):
|
206 |
+
feature_old = faces[i+1] / 255.0
|
207 |
+
feature_new = faces[i] / 255.0
|
208 |
+
location_old = feedback_pts[i - 1]
|
209 |
+
feedback_pt = lk_track(feature_old, feature_new, location_old, patch_size, pyramid_level)
|
210 |
+
feedback_pts[i] = feedback_pt
|
211 |
+
|
212 |
+
return forward_pts, feedback_pts
|