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)