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")