import addPathToModules
import robotics as rb
import visualize as viz
import vision as vsn
import matplotlib.pyplot as plt
import numpy as np
import math
fig = plt.figure(figsize=(6, 6))
ax = fig.add_subplot(111, projection='3d')
viz.defaultAxisSetup(ax)
a = np.array([4.0, 3.0, 1.0])
viz.plotHomgPoint(ax, a)
R = rb.roty(math.pi / 8)
print("R fasit")
print(R)
print("b = R@a")
b = R @ a
viz.plotHomgPoint(ax, b, color='b')
print(f'b: {b}')
R_rec1 = rb.rotation_matrix_from_vectors(a, b)
print("R_rec")
print(R_rec1)
import robotics as rb
import visualize as viz
import vision as vsn
import matplotlib.pyplot as plt
import numpy as np
import math
fig = plt.figure(figsize=(6, 6))
ax = fig.add_subplot(111, projection='3d')
viz.defaultAxisSetup(ax, xyz_lim=[-5, 5], show_axis=False)
E = np.matrix([[.0, -0.3420, .0], [-0.3420, .0, 0.9397], [.0, -0.9397, .0]])
print("Essential matrix:")
print(E)
R1, R2, t1, t2 = vsn.recoverFromEssential(E)
print("R1")
print(R1)
print("R2")
print(R2)
print("t1")
print(t1)
print("t2")
print(t2)
R = rb.roty(math.pi / 2)
T = rb.makeTrans(R1, t1)
print("T")