def visualizeDense(IM1_PATH, IM2_PATH, TEMPLE_CORRS, F, K1, K2):
fig = plt.figure()
ax = Axes3D(fig)
# ax.set_xlim/set_ylim/set_zlim/
# ax.set_aspect('equal')
# may be useful
# you'll want a roughly cubic meter
# around the center of mass
# Define pts and ims
'''
SAME SHIT EXCEPT at this location (C)
'''
from scipy import io as Mat
corrs = Mat.loadmat(TEMPLE_CORRS)
pts = np.concatenate([corrs['x1'], corrs['y1']], axis=1)
from skimage import io as Im
im1, im2 = Im.imread(IM1_PATH), Im.imread(IM2_PATH)
# Use the epipolarCorrespondence function from before to compute the new points
#new_pts = np.zeros([len(pts)], dtype=np.ndarray)
new__pts = np.zeros([len(pts)], dtype=np.ndarray)
new_pts = []
for i in range(0, len(pts), 1):
x1, y1 = int(corrs['x1'][i]), int(corrs['y1'][i])
print(epipolarCorrespondence(im1, im2, F, x1, y1))
x, y = epipolarCorrespondence(im1, im2, F, x1, y1)
new_pts.append([x, y])
new__pts[i] = [epipolarCorrespondence(im1, im2, F, x1, y1)]
# From run_q3, triangulate the essential matrix (verbatim code from run_q3.py)
E = essentialMatrix(F, K1, K2)
E = E / E[2, 2]
M2s = camera2(E)
#print (new_pts)
# Q3.2 / 3.3
C1 = np.hstack([np.eye(3), np.zeros((3, 1))])
new_pts = np.array(new_pts)
for C2 in M2s:
P, err = triangulate(K1.dot(C1), pts, K2.dot(C2), new_pts)
if (P.min(0)[2] > 0):
break
kc1, kc2 = np.dot(K1, C1), np.dot(K2, C2)
scipy.io.savemat('q4_2.mat', {
'F': F,
'M1': C1,
'M2': C2,
'C1': kc1,
'C2': kc2
})
ax.scatter(P[:, 0], P[:, 1], P[:, 2])
plt.show()
print('M2: ' + str(C2))
print('C2: ' + str(np.dot(K2, C2)))
return
def visualizeDense(IM1_PATH, IM2_PATH, TEMPLE_CORRS, F, K1, K2):
fig = plt.figure()
ax = Axes3D(fig)
# load image and pts pairs
im1 = skimage.io.imread(IM1_PATH)
im2 = skimage.io.imread(IM2_PATH)
corrs = scipy.io.loadmat(TEMPLE_CORRS)
# make pts1
x1 = []
y1 = []
for x in range(im1.shape[1]):
for y in range(im2.shape[0]):
if np.mean(im1[y, x, :] > 100):
x1.append(x)
y1.append(y)
pts1 = np.stack([x1, y1], axis=1) # (num, 2)
# compute F and x2,y2
pts2 = []
for idx in range(len(x1)):
x2, y2 = epipolarCorrespondence(im1, im2, F, int(x1[idx]),
int(y1[idx]))
pts2.append([x2, y2])
pts2 = np.array(pts2)
# pdb.set_trace()
# compute E and triangulate
E = essentialMatrix(F, K1, K2)
E = E / E[2, 2]
M2s = camera2(E)
C1 = np.hstack([np.eye(3), np.zeros((3, 1))])
# print(M2s)
for C2 in M2s:
P, err = triangulate(K1.dot(C1), pts1, K2.dot(C2), pts2)
if (P.min(0)[2] > 0):
break
scipy.io.savemat('q4_3.mat', {
'F': F,
'M1': C1,
'M2': C2,
'C1': np.dot(K1, C1),
'C2': np.dot(K2, C2)
})
ax.scatter(P[:, 0], P[:, 1], P[:, 2])
plt.show()
print('M2')
print(C2)
print('C2')
print(np.dot(K2, C2))
return
def visualize(IM1_PATH, IM2_PATH, TEMPLE_CORRS, F, K1, K2):
fig = plt.figure()
ax = Axes3D(fig)
# ax.set_xlim/set_ylim/set_zlim/
# ax.set_aspect('equal')
# may be useful
# you'll want a roughly cubic meter
# around the center of mass
# load image and pts pairs
im1 = skimage.io.imread(IM1_PATH)
im2 = skimage.io.imread(IM2_PATH)
corrs = scipy.io.loadmat(TEMPLE_CORRS)
x1 = corrs['x1']
y1 = corrs['y1']
pts1 = np.concatenate([x1, y1], axis=1) # (num, 2)
# compute F and x2,y2
pts2 = []
for idx in range(len(x1)):
x2, y2 = epipolarCorrespondence(im1, im2, F, int(x1[idx]),
int(y1[idx]))
pts2.append([x2, y2])
pts2 = np.array(pts2)
# pdb.set_trace()
# compute E and triangulate
E = essentialMatrix(F, K1, K2)
E = E / E[2, 2]
pdb.set_trace()
M2s = camera2(E)
C1 = np.hstack([np.eye(3), np.zeros((3, 1))])
# print(M2s)
for C2 in M2s:
P, err = triangulate(K1.dot(C1), pts1, K2.dot(C2), pts2)
if (P.min(0)[2] > 0):
break
scipy.io.savemat('q4_2.mat', {
'F': F,
'M1': C1,
'M2': C2,
'C1': np.dot(K1, C1),
'C2': np.dot(K2, C2)
})
ax.scatter(P[:, 0], P[:, 1], P[:, 2])
plt.show()
print('M2')
print(C2)
print('C2')
print(np.dot(K2, C2))
F = eightpoint(pts1, pts2, max(im1.shape))
F = F / F[2, 2]
print('F')
print(F)
E = essentialMatrix(F, K1, K2)
E = E / E[2, 2]
print('E')
print(E)
M2s = camera2(E)
# Q3.2 / 3.3
C1 = np.hstack([np.eye(3), np.zeros((3, 1))])
print(M2s)
for C2 in M2s:
P, err = triangulate(K1.dot(C1), pts1, K2.dot(C2), pts2)
if (P.min(0)[2] > 0):
# we're the right one!
scipy.io.savemat('q3_3.mat', {'M2':C2, 'C2':np.dot(K2,C2), \
'p1':pts1, 'p2':pts2, 'P':P})
break
print('M2')
print(C2)
print('C2')
print(np.dot(K2, C2))
# print('P')
# print(P)
print('error')
print(err)
r = invRodrigues(R)
print('should be r')
print(r)
# Q5.3
# pdb.set_trace()
E = essentialMatrix(F, K1, K2)
E = E / E[2, 2]
M2s = camera2(E)
goodP1 = pts1[inliers]
goodP2 = pts2[inliers]
C1 = np.hstack([np.eye(3), np.zeros((3, 1))])
for C2 in M2s:
P, err = triangulate(K1.dot(C1), goodP1, K2.dot(C2), goodP2)
if (P.min(0)[2] > 0):
# we're the right one!
break
print('original error is ', err)
# you should create this from the above (using P,C2)
R2 = C2[:, :3]
r2 = invRodrigues(R2)
T2 = C2[:, 3]
t2 = -np.dot(np.linalg.inv(R2), T2)
initialx = [P.flatten(), r2, t2]
err = rodriguesResidual(K1, C1, goodP1, K2, goodP2, initialx)
print('initial error is ', err)
C2n, Pn = bundleAdjustment(K1, C1, goodP1, K2, C2, goodP2, P)
def visualize(IM1_PATH,IM2_PATH,TEMPLE_CORRS,F,K1,K2):
fig = plt.figure()
# ax = Axes3D(fig)
ax = fig.add_subplot(111, projection='3d')
# ax.set_xlim/set_ylim/set_zlim/
# ax.set_aspect('equal')
# may be useful
# you'll want a roughly cubic meter
# around the center of mass
im1 = skimage.io.imread(IM1_PATH)
im2 = skimage.io.imread(IM2_PATH)
im1 = rgb2gray(im1)
im2 = rgb2gray(im2)
coor = scipy.io.loadmat(TEMPLE_CORRS)
x1 = coor['x1']
y1 = coor['y1']
pts1 = np.hstack((x1,y1))
pts2 = []
x2 = np.zeros((np.shape(x1)[0]))
y2 = np.zeros((np.shape(y1)[0]))
for i in range(np.shape(x1)[0]):
p2e = epipolarCorrespondence(im1, im2, F, pts1[i,0], pts1[i,1])
pts2.append(p2e)
pts2 = np.array(pts2)
# M1 = [I|0];
M1 = np.zeros((3,4))
M1[0,0] = 1
M1[1,1] = 1
M1[2,2] = 1
C1 = K1 @ M1
# M2 = [R|t];
#Go through 4 M2 and store the outputs coor
E = essentialMatrix(F,K1,K2)
M2e = camera2(E)
P = np.zeros((np.size(x1),3))
err = np.zeros((4,1))
z_position = np.zeros((4,1))
#find thr posotive Z coor
for i in range (4):
C2 = K2 @ M2e[i,:,:]
p, err = triangulate( C1, pts1, C2, pts2 )
if (p[:,2]>0).all():
P = p
M2 = M2e[i,:,:]
# P_total[:,:,i] = P
# idx = np.where(P[:,2]>0)
# z_position[i,:] = length(idx)
# correct = np.where(z_position == np.size(P,0))
PX = P[:,0]
PY = P[:,1]
PZ = P[:,2]
ax.scatter(PX, PY, PZ, c='blue', marker = 'o',s=3)
# ax.set_aspect('equal')
ax.set_xlabel('X Label')
ax.set_ylabel('Y Label')
ax.set_zlabel('Z Label')
plt.show()
scipy.io.savemat('q4_2.mat', {'M1':M1,'M2':M2,'C1':C1,'C2':C2,})
def visualizeDense(IM1_PATH,IM2_PATH,TEMPLE_CORRS,F,K1,K2):
fig = plt.figure()
# ax = Axes3D(fig)
ax = fig.add_subplot(111, projection='3d')
# ax.set_xlim/set_ylim/set_zlim/
# ax.set_aspect('equal')
# may be useful
# you'll want a roughly cubic meter
# around the center of mass
im1 = skimage.io.imread(IM1_PATH)
im2 = skimage.io.imread(IM2_PATH)
im1 = rgb2gray(im1)
im2 = rgb2gray(im2)
pts1 = []
pts2 = []
# for i in range(np.shape(im1)[0]):
# for j in range(np.shape(im1)[1]):
# a = [i,j]
# pts1.append(a)
npoint = 150000
randomIdx_x = np.random.randint(5,635,npoint)
randomIdx_y = np.random.randint(5,475,npoint)
pts1 = []
for i in range(npoint):
a = np.array([randomIdx_x[i],randomIdx_y[i]])
pts1.append(a)
pts1 = np.array(pts1)
for i in range(np.shape(pts1)[0]):
p2e = epipolarCorrespondence(im1, im2, F, pts1[i,0], pts1[i,1])
pts2.append(p2e)
pts2 = np.array(pts2)
# M1 = [I|0];
M1 = np.zeros((3,4))
M1[0,0] = 1
M1[1,1] = 1
M1[2,2] = 1
C1 = K1 @ M1
# M2 = [R|t];
#Go through 4 M2 and store the outputs coor
E = essentialMatrix(F,K1,K2)
M2e = camera2(E)
P = np.zeros((np.shape(pts1)[0],3))
err = np.zeros((4,1))
z_position = np.zeros((4,1))
#find thr posotive Z coor
for i in range (4):
C2 = K2 @ M2e[i,:,:]
p, err = triangulate( C1, pts1, C2, pts2 )
if (p[:,2]>0).all():
P = p
M2 = M2e[i,:,:]
# P_total[:,:,i] = P
# idx = np.where(P[:,2]>0)
# z_position[i,:] = length(idx)
# correct = np.where(z_position == np.size(P,0))
PX = P[:,0]
PY = P[:,1]
PZ = P[:,2]
ax.scatter(PX, PY, PZ, c='blue', marker = 'o', s = 0.01)
# ax.set_aspect('equal')
ax.set_xlabel('X Label')
ax.set_ylabel('Y Label')
ax.set_zlabel('Z Label')
ax.set_zlim(3.70,4.00)
plt.show()
