F = sub.ransacF(data['pts1'], data['pts2'], M)
assert F.shape == (3, 3), 'ransacF returns 3x3 matrix'
# 5.2
r = np.ones([3, 1])
R = sub.rodrigues(r)
assert R.shape == (3, 3), 'rodrigues returns 3x3 matrix'
R = np.eye(3)
r = sub.invRodrigues(R)
assert (r.shape == (3, )) | (r.shape
== (3, 1)), 'invRodrigues returns 3x1 vector'
# 5.3
K1 = np.random.rand(3, 3)
K2 = np.random.rand(3, 3)
M1 = np.concatenate([np.random.rand(3, 3), np.ones([3, 1])], axis=1)
M2 = np.concatenate([np.random.rand(3, 3), np.ones([3, 1])], axis=1)
r2 = np.ones(3)
t2 = np.ones(3)
x = np.concatenate([P.reshape([-1]), r2, t2])
residuals = sub.rodriguesResidual(K1, M1, data['pts1'], K2, data['pts1'], x)
assert residuals.shape == (4 * N,
1), 'rodriguesResidual returns vector of size 4Nx1'
M2, P = sub.bundleAdjustment(K1, M1, data['pts1'], K2, M2, data['pts1'], P)
assert M2.shape == (3, 4), 'bundleAdjustment returns 3x4 matrix M'
assert P.shape == (N, 3), 'bundleAdjustment returns Nx3 matrix P'
print('Format check passed.')
sol = M2[:, :, i]
C2 = np.matmul(K2, M2[:, :, i])
break
M2 = sol
C2 = np.matmul(K2, M2)
P, err = triangulate(C1, pts1_inliers, C2, pts2_inliers)
# print(P)
R2 = M2[:, 0:3]
t2 = M2[:, 3]
print('The Initial value of M2')
#change the below latter
r2 = invRodrigues(R2)
r2 = np.reshape(r2, [1, 3])
# print(M2)
x = np.concatenate([P.reshape([-1]), r2[-1], t2])
residuals = sub.rodriguesResidual(K1, M1, pts1_inliers, K2, pts2_inliers,
x)
M2, P2 = sub.bundleAdjustment(K1, M1, pts1_inliers, K2, M2, pts2_inliers,
P)
# print('The Optimized value of M2')
# print(P2)
#print(P2)
fig = plt.figure(1)
#ax = Axes3D(fig)
ax = fig.add_subplot(111, projection='3d')
x = P2[:, 0]
y = P2[:, 1]
z = P2[:, 2]
plt.gca().set_aspect('equal', adjustable='box')
# ax.scatter(x,y,z, color='blue')