T_WC[:3, :3] = R_WC
T_WC[:3, 3:4] = t_W
T_WC[3, 3] = 1.
return T_WC, T_CW
if __name__ == "__main__":
p = Plot()
geo = Geometry()
d = 10
pt0 = np.array([[0], [0], [0]])
p.plotPoint(pt0)
for i in range(10):
angle = np.deg2rad(10 * i)
pt = np.array([[d * np.cos(angle)],
[-d * np.sin(angle)],
[i]])
if i == 0:
p.plotPoint(pt, 'g.')
else:
p.plotPoint(pt)
r = Ray(pt0, pt)
T_WC, T_CW = r.toT(pt)
p.plotCam(T_WC)
p.plotRay(r.val, scale=10)
r = Ray(pt, pt0, pt)
p.plotRay(r.val, 'g', scale=10)
p.show(65, 40)
