Skip to content

Code cleanup #1

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
wants to merge 3 commits into
base: master
Choose a base branch
from
Open

Code cleanup #1

Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
3eba061
HH: basically working, but E and F confused...
haho12 Sep 30, 2024
1171e02
use essential matrix (working with norm'd camera coordinates)
haho12 Sep 30, 2024
4b77c05
decompose E into R and t
haho12 Sep 30, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
311 changes: 199 additions & 112 deletions src/8point.py
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,131 +1,138 @@
# https://gist.github.com/jensenb/8668000 # decomp E into R, t
# https://github.com/Smelton01/8-point-algorithm.git # original source, images

#%%
import numpy as np
from matplotlib import pyplot as plt
import os

# %%
# low depth variance image same plane
uvMat = [[1855,1352, 1680, 1305], [2100, 2116, 1914, 2084], [2482, 1994, 2310, 1984], [3070, 1336, 2959, 1337], [3450, 1911, 3369, 1966], [1356, 1885, 1173, 1809],\
# low depth variance image same plane -> coplanar = degernate solution
# point-pairs in close1 and close2 : [x_close1, y_close1, x_close2, y_close2]
uvMat0 = [[1855,1352, 1680, 1305], [2100, 2116, 1914, 2084], [2482, 1994, 2310, 1984], [3070, 1336, 2959, 1337], [3450, 1911, 3369, 1966], [1356, 1885, 1173, 1809],\
[3415,2125, 3320, 2190], [3848, 1308, 3846, 1342]]

# low depth variance different plane
# point-pairs in close1 and close2 : [x_close1, y_close1, x_close2, y_close2]
uvMat1 = [[1855,1352, 1680, 1305], [1251,2640, 960, 2518], [525, 2436, 323, 2273], [745, 1840, 611,1734], [1578, 2890, 1174, 2783], [1356, 1885, 1173, 1809],\
[3415,2125, 3320, 2190], [3848, 1308, 3846, 1342]]

# high depth variance image same plane
# high depth variance image same plane -> coplanar = degernate solution
# point-pairs in far1 and far2 : [x_far1, y_far1, x_far2, y_far2]
uvMat2 = [[580, 2362, 492, 1803], [2050, 2097, 1381, 1956], [2558, 2174, 1544, 2115], [1395, 1970, 1166, 1752], [2490, 3003, 466, 2440], [3368, 1622, 3320, 2011],\
[2183, 1500, 2471,1621], [1972,1775, 1674, 1736]]

# high depth variance image different plane
# point-pairs in far1 and far2 : [x_far1, y_far1, x_far2, y_far2]
uvMat3 = [[580, 2362, 492, 1803],[3316,1276, 3242, 1565], [1007,788, 1606,885] , [1900, 1144, 2330, 1250], [984, 1369, 1574, 1335], [3368, 1622, 3320, 2011],\
[2192, 1288, 2469, 1420], [2050, 2097, 1381, 1956]]
uvMat = uvMat2
# %%
def main(uvMat):
close1, close2, far1, far2 = load_imgs()
norm_points, T1, T2 = normalize(uvMat)

f_hat = calc_F(norm_points)

f_mat = restore(f_hat, T1, T2)
plot1(close1, f_mat, uvMat)

plot2(close2, f_mat, uvMat)
main(uvMat)

#%%

def load_imgs():
close1 = plt.imread("../res/same1.jpg")
close2 = plt.imread('../res/same2.jpg')
far1 = plt.imread("../res/extreme1.jpg")
far2 = plt.imread('../res/extreme2.jpg')
return close1, close2, far1, far2
close1, close2, far1, far2 = load_imgs()


# %%
from functools import reduce
def normalize(points):
# T1 acts on x,y to give x_hat
# T2 acts on x'y' to give x'_hat
n = len(points)
img1_pts, img2_pts = [], []
for a,b,c,d in points:
img2_pts.append([a,b])
img1_pts.append([c,d])
sum1 = reduce(lambda x, y: (x[0]+y[0], x[1]+y[1]), img1_pts)
sum2 = reduce(lambda x, y: (x[0]+y[0], x[1]+y[1]), img2_pts)

mean1 = [val/n for val in sum1]
mean2 = [val/n for val in sum2]

s1 = (n*2)**0.5/(sum([((x-mean1[0])**2 + (y-mean1[1])**2)**0.5 for x,y in img1_pts]))
s2 = (2*n)**0.5/(sum([((x-mean2[0])**2 + (y-mean2[1])**2)**0.5 for x,y in img2_pts]))
close1 = plt.imread("../res/same1.jpg")
close2 = plt.imread('../res/same2.jpg')
far1 = plt.imread("../res/extreme1.jpg")
far2 = plt.imread('../res/extreme2.jpg')

T1 = np.array([[s1, 0, -mean1[0]*s1], [0, s1, -mean1[1]*s1], [0, 0, 1]])
T2 = np.array([[s2, 0, -mean2[0]*s2], [0, s2, -mean2[1]*s2], [0, 0, 1]])
# assume camera-matrix is known as:
img_width = close1.shape[1]
img_height = close1.shape[0]
f = 1000. # focal length
pu = img_width / 2
pv = img_height / 2
K = np.array([(f, 0, pu),
(0, f, pv),
(0, 0, 1)])
K_inv = np.linalg.inv(K)

points = [[T1 @ [c, d, 1], T2 @ [a,b,1]] for a,b,c,d in points]
points = [[l[0], l[1], r[0], r[1]] for l,r in points]
return points, T1, T2
# %%
norm_points, T1, T2 = normalize(uvMat)
print(norm_points)
# %%

def calc_F(uvMat):
# compute essential matrix E
def calc_E(uvMat, K):
A = np.zeros((len(uvMat),9))
# img1 x' y' x y im2
K_inv = np.linalg.inv(K)

for i in range(len(uvMat)):
A[i][0] = uvMat[i][0]*uvMat[i][2]
A[i][1] = uvMat[i][1]*uvMat[i][2]
A[i][2] = uvMat[i][2]
A[i][3] = uvMat[i][0]*uvMat[i][3]
A[i][4] = uvMat[i][1]*uvMat[i][3]
A[i][5] = uvMat[i][3]
A[i][6] = uvMat[i][0]
A[i][7] = uvMat[i][1]
A[i][8] = 1.0
uv1 = np.array([uvMat[i][0], uvMat[i][1], 1.0])
x1_norm = K_inv.dot( uv1 )
uv2 = np.array([uvMat[i][2], uvMat[i][3], 1.0])
x2_norm = K_inv.dot( uv2 )
A[i][0] = x1_norm[0]*x2_norm[0] # x1*x2
A[i][1] = x1_norm[1]*x2_norm[0] # y1*x2
A[i][2] = x2_norm[0] # x2
A[i][3] = x1_norm[0]*x2_norm[1] # x1*y2
A[i][4] = x1_norm[1]*x2_norm[1] # y1*y2
A[i][5] = x2_norm[1] # y2
A[i][6] = x1_norm[0] # x1
A[i][7] = x1_norm[1] # y1
A[i][8] = 1.0 # 1.0

_,_,v = np.linalg.svd(A)
# print("v", v)
f_vec = v.transpose()[:,8]
# print("f_vec = ", f_vec)
f_hat = np.reshape(f_vec, (3,3))
# print("Fmat = ", f_hat)

# Enforce rank(F) = 2
s,v,d = np.linalg.svd(f_hat)
f_hat = s @ np.diag([*v[:2], 0]) @ d

return f_hat
_,_,Vt = np.linalg.svd(A) # returns U,S,Vt

E_vec = Vt.transpose()[:,8] # minimal solution: chose eigenvector of smallest eigenvalue
E = E_vec.reshape((3,3))

return E

#%%
# Regular version
f_hat = calc_F(uvMat)
# print(f_hat)
plt.imshow(close1)
for pt_pair in uvMat0:
x = pt_pair[0]
y = pt_pair[1]
plt.plot(x, y, "rx")
for pt_pair in uvMat1:
x = pt_pair[0]
y = pt_pair[1]
plt.plot(x, y, "gx")
plt.title('close1')
plt.show()

# %%
# normalized version
f_hat = calc_F(norm_points)
plt.imshow(close2)
for pt_pair in uvMat0:
x = pt_pair[2]
y = pt_pair[3]
plt.plot(x, y, "rx")
for pt_pair in uvMat1:
x = pt_pair[2]
y = pt_pair[3]
plt.plot(x, y, "gx")
plt.title('close2')
plt.show()

# %%
# Restore to the original coordinates
def restore(f_hat, T1, T2):
f_mat = T2.transpose() @ f_hat @ T1
return f_mat
plt.imshow(far1)
for pt_pair in uvMat2:
x = pt_pair[0]
y = pt_pair[1]
plt.plot(x, y, "rx")
for pt_pair in uvMat3:
x = pt_pair[0]
y = pt_pair[1]
plt.plot(x, y, "gx")
plt.title('far1')
plt.show()

f_mat = restore(f_hat, T1, T2)
# print(f_mat)
plt.imshow(far2)
for pt_pair in uvMat2:
x = pt_pair[2]
y = pt_pair[3]
plt.plot(x, y, "rx")
for pt_pair in uvMat3:
x = pt_pair[2]
y = pt_pair[3]
plt.plot(x, y, "gx")
plt.title('far2')
plt.show()

#%%
cmap = plt.get_cmap("jet_r")

# plot on img1
def plot1(img, f_mat, points):
def plot1(img, E, points):
F = K_inv.T @ E @ K_inv

w = img.shape[1]
num = 1
for x, y, *pt in points:
color = cmap(num/len(points))
a,b,c = np.array([*pt, 1]).transpose() @ f_mat
a,b,c = np.array([*pt, 1]).transpose() @ F
p1 = (0,-c/b)
p2 = (w, -(a*w + c)/b)
plt.plot(*zip(p1,p2), color=color)
Expand All @@ -134,52 +141,132 @@ def plot1(img, f_mat, points):
num+=1
plt.imshow(img)

#%%
plt.show()

# plot on img 2
# epipolar lines based on x'y' and points x,y
def plot2(img, f_mat, points):
def plot2(img, E, points):
F = K_inv.T @ E @ K_inv

w = img.shape[1]
num = 1
for *pt, x,y in points:
color = cmap(num/len(points))
a,b,c = f_mat @ [*pt, 1]
a,b,c = F @ [*pt, 1]
p1 = (0,-c/b)
p2 = (w, -(a*w + c)/b)
plt.plot(*zip(p1,p2), color=color)
plt.plot(x,y, "x", color=color)
plt.text(x,y,f"{num}")
num+=1
plt.imshow(img)

plt.show()

#%%

# Plot the epipolar lines
plot1(close1, f_hat, uvMat)
if False:
#uvMat = uvMat0 # degenerate solution for close image
uvMat = uvMat1 # correct solution for close image
E = calc_E(uvMat, K)

# Plot the epipolar lines
plot1(close1, E, uvMat)
plot2(close2, E, uvMat)
else:
#uvMat = uvMat2 # degenerate solution for far image
uvMat = uvMat3 # correct solution for far image
E = calc_E(uvMat, K)

#%%
plot2(close2, f_hat, uvMat2)
# Plot the epipolar lines
plot1(far1, E, uvMat)
plot2(far2, E, uvMat)

# %%
# Epipoles
def check_epipoles(f_mat):
f_mat = f_hat

# compute epipolar points
def compute_epipoles(f_mat):
f_mat_2 = f_mat.transpose() @ f_mat
w3, v3 = np.linalg.eig(f_mat_2)
smallest = np.argmin(w3)
f_vec = v3[:,smallest]
e1vec = f_vec/f_vec[2]
p1 = uvMat[0]
point1 = p1[2:] #[3641, 2373]#[826, 649]
lp = f_hat @ [*point1,1]
print("e1: ", e1vec)
print(f_hat @ e1vec @ [*point1,1])
e1_vec = f_vec/f_vec[2]
with np.printoptions(precision=1, suppress=True):
print("e1: ", e1_vec)
# p1 = uvMat[0]
# point1 = p1[2:]
# print(f_mat @ e1vec @ [*point1,1])

f_T_f_mat = f_mat @ f_mat.transpose()
w4 , v4 = np.linalg.eig(f_T_f_mat)
smallest = np.argmin(w4)
f_vec = v4[:,smallest]
e2vec = f_vec/f_vec[2]
print("e2: ", e2vec)
point2 = p1[:2]
print(e2vec @ f_hat @ [*point2,1])
check_epipoles(f_hat)
e2_vec = f_vec/f_vec[2]
with np.printoptions(precision=1, suppress=True):
print("e2: ", e2_vec)
# point2 = p1[:2]
# print(e2vec @ f_mat @ [*point2,1])

compute_epipoles(E)

# %%

def in_front_of_both_cameras(first_points, second_points, rot, trans):
# check if the point correspondences are in front of both images
for first, second in zip(first_points, second_points):
first_z = np.dot(rot[0, :] - second[0]*rot[2, :], trans) / np.dot(rot[0, :] - second[0]*rot[2, :], second)
first_3d_point = np.array([first[0] * first_z, second[0] * first_z, first_z])
second_3d_point = np.dot(rot.T, first_3d_point) - np.dot(rot.T, trans)

if first_3d_point[2] < 0 or second_3d_point[2] < 0:
return False

return True

# compute t and R from essential matrix E
def decompose_E(E, uvMat):
# enforce rank(E) = 2
# (gives just minor changes)
# U,s,Vt = np.linalg.svd(E)
# set smallest eigenvalue s[2] to zero:
# s_rank2 = np.diag([s[0], s[1], 0])
# E_rank2 = U @ s_rank2 @ Vt

# convert pixel coordinates to normalized camera coordinates
pts_cam1 = []
pts_cam2 = []
for i in range(len(uvMat)):
uv1 = np.array([uvMat[i][0], uvMat[i][1], 1.0])
x1_norm = K_inv.dot( uv1 )
pts_cam1.append(x1_norm)

uv2 = np.array([uvMat[i][2], uvMat[i][3], 1.0])
x2_norm = K_inv.dot( uv2 )
pts_cam2.append(x2_norm)

# decompose essential matrix into R, t (see Hartley and Zisserman 9.13)
U, s, Vt = np.linalg.svd(E)
W = np.array([0.0, -1.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0]).reshape(3, 3)

# only in one of the four configurations will all the points be in front of both cameras
# First choice: R = U * W * Vt, t = +u_3 (see Hartley Zisserman 9.19)
R = U @ W @ Vt
t = U[:, 2]

if not in_front_of_both_cameras(pts_cam1, pts_cam2, R, t):
# Second choice: R = U * W * Vt, t = -u_3
t = - U[:, 2]

if not in_front_of_both_cameras(pts_cam1, pts_cam2, R, t):
# Third choice: R = U * Wt * Vt, t = u_3
R = U.dot(W.T).dot(Vt)
t = U[:, 2]

if not in_front_of_both_cameras(pts_cam1, pts_cam2, R, t):
# Fourth choice: R = U * Wt * Vt, t = -u_3
t = - U[:, 2]

return R, t

R, t = decompose_E(E, uvMat)