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_}")
assert P.shape == (N, 3), 'triangulate returns Nx3 matrix P'
assert np.isscalar(err), 'triangulate returns scalar err'
# 4.1
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])
