def q5_2():
# test rodrigues
import random
rx = np.radians(random.randint(0, 180))
ry = np.radians(random.randint(0, 180))
rz = np.radians(random.randint(0, 180))
r = np.array([rx, ry, rz]).T
R = rodrigues(r)
r_ = invRodrigues(R)
assert np.allclose(r, r_), f"r_out: {r_} != r_in: {r}"
print(f"r: {r}\nR: {R}\nr_: {r_}")
x2, y2 = sub.epipolarCorrespondence(im1, im2, F8, data['pts1'][0, 0],
data['pts1'][0, 1])
assert np.isscalar(x2) & np.isscalar(
y2), 'epipolarCoorespondence returns x & y coordinates'
# 5.1
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'
import numpy as np
import submission as sub
if __name__ == '__main__':
r = np.vstack([2, 2, 3])
# print('original r',r)
R = sub.rodrigues(r)
# print('R=',R)
# R=np.eye(3)
print('R=', R)
r_new = sub.invRodrigues(R)
print('inverse r', r_new)