T_CW[:3, 3:4] = t_C
T_CW[3, 3] = 1.
T_WC = np.zeros((4, 4))
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)
PC_rotated = m.dot(PC)
# Define radius vector
vecR = geo.twoPts2Vec(PO, PC_rotated) * R
vecR_size = geo.normVec(vecR)
# Define viewing vectors
vec = geo.twoPts2Vec(PC_rotated, Ppx_rotated)
# Call function
vecView = geo.projectVecs2Depth(T, vecR, vec)
vecView_size = geo.normVec(vecView)
# Update viewing vectors
vec = vecView - vecR.reshape((1, 1, 3))
vec_size = geo.normVec(vec)
# Plot
p.plotVec(vecR, PO, 'r', vecR_size)
p.plotPoint(PC_rotated, '.g')
for i in range(h):
for j in range(w):
p.plotVec(vecView[i, j, :].reshape((3, 1)),
PO, 'm',
vecView_size[i, j])
p.plotVec(vec[i, j, :].reshape((3, 1)),
PC_rotated, 'g',
vec_size[i, j])
p.plotAxis()
p.show(90, 180)
