|
import numpy as np |
|
|
|
|
|
def line_to_border(line, size): |
|
|
|
|
|
H, W = size[1], size[0] |
|
a, b, c = line[0], line[1], line[2] |
|
epsa = 1e-8 if a >= 0 else -1e-8 |
|
epsb = 1e-8 if b >= 0 else -1e-8 |
|
intersection_list = [] |
|
|
|
y_left = -c / (b + epsb) |
|
y_right = (-c - a * (W - 1)) / (b + epsb) |
|
x_top = -c / (a + epsa) |
|
x_down = (-c - b * (H - 1)) / (a + epsa) |
|
|
|
if y_left >= 0 and y_left <= H - 1: |
|
intersection_list.append([0, y_left]) |
|
if y_right >= 0 and y_right <= H - 1: |
|
intersection_list.append([W - 1, y_right]) |
|
if x_top >= 0 and x_top <= W - 1: |
|
intersection_list.append([x_top, 0]) |
|
if x_down >= 0 and x_down <= W - 1: |
|
intersection_list.append([x_down, H - 1]) |
|
if len(intersection_list) != 2: |
|
return None |
|
intersection_list = np.asarray(intersection_list) |
|
return intersection_list |
|
|
|
|
|
def find_point_in_line(end_point): |
|
x_span, y_span = ( |
|
end_point[1, 0] - end_point[0, 0], |
|
end_point[1, 1] - end_point[0, 1], |
|
) |
|
mv = np.random.uniform() |
|
point = np.asarray([end_point[0, 0] + x_span * mv, end_point[0, 1] + y_span * mv]) |
|
return point |
|
|
|
|
|
def epi_line(point, F): |
|
homo = np.concatenate([point, np.ones([len(point), 1])], axis=-1) |
|
epi = np.matmul(homo, F.T) |
|
return epi |
|
|
|
|
|
def dis_point_to_line(line, point): |
|
homo = np.concatenate([point, np.ones([len(point), 1])], axis=-1) |
|
dis = line * homo |
|
dis = dis.sum(axis=-1) / (np.linalg.norm(line[:, :2], axis=-1) + 1e-8) |
|
return abs(dis) |
|
|
|
|
|
def SGD_oneiter(F1, F2, size1, size2): |
|
H1, W1 = size1[1], size1[0] |
|
factor1 = 1 / np.linalg.norm(size1) |
|
factor2 = 1 / np.linalg.norm(size2) |
|
p0 = np.asarray([(W1 - 1) * np.random.uniform(), (H1 - 1) * np.random.uniform()]) |
|
epi1 = epi_line(p0[np.newaxis], F1)[0] |
|
border_point1 = line_to_border(epi1, size2) |
|
if border_point1 is None: |
|
return -1 |
|
|
|
p1 = find_point_in_line(border_point1) |
|
epi2 = epi_line(p0[np.newaxis], F2) |
|
d1 = dis_point_to_line(epi2, p1[np.newaxis])[0] * factor2 |
|
epi3 = epi_line(p1[np.newaxis], F2.T) |
|
d2 = dis_point_to_line(epi3, p0[np.newaxis])[0] * factor1 |
|
return (d1 + d2) / 2 |
|
|
|
|
|
def compute_SGD(F1, F2, size1, size2): |
|
np.random.seed(1234) |
|
N = 1000 |
|
max_iter = N * 10 |
|
count, sgd = 0, 0 |
|
for i in range(max_iter): |
|
d1 = SGD_oneiter(F1, F2, size1, size2) |
|
if d1 < 0: |
|
continue |
|
d2 = SGD_oneiter(F2, F1, size1, size2) |
|
if d2 < 0: |
|
continue |
|
count += 1 |
|
sgd += (d1 + d2) / 2 |
|
if count == N: |
|
break |
|
if count == 0: |
|
return 1 |
|
else: |
|
return sgd / count |
|
|
|
|
|
def compute_inlier_rate(x1, x2, size1, size2, F_gt, th=0.003): |
|
t1, t2 = np.linalg.norm(size1) * th, np.linalg.norm(size2) * th |
|
epi1, epi2 = epi_line(x1, F_gt), epi_line(x2, F_gt.T) |
|
dis1, dis2 = dis_point_to_line(epi1, x2), dis_point_to_line(epi2, x1) |
|
mask_inlier = np.logical_and(dis1 < t2, dis2 < t1) |
|
return mask_inlier.mean() if len(mask_inlier) != 0 else 0 |
|
|
