RTprev = np.eye(4)
origins = []
D4prev = np.array([0, 0, 0]).reshape(3, 1)
im_prev = 0
while True:
for imname in im_list:
im = cv2.imread(dir + imname)
f2 = feate.extract(im)
if f1 != 0:
## 2D points on image
pts2, pts1 = feate.matches_frame(f1, f2)
## R, T between cameras
R, T = feate.getRT(pts1, pts2)
## 3D points
D4 = feate.get3D(R, T, pts1.T, pts2.T).T
D4 = D4[:, :3] / D4[:, 3:4]
mask = D4[:, 2] > 0.05
D4 = D4[mask]
pts1 = pts1[mask]
pts1_predicted = (K @ R @ (D4.T + T)).T
pts1_predicted = pts1_predicted[:, :2] / pts1_predicted[:, 2:3]
draw_cross(im, pts2)
draw_cross(im, pts1_predicted, (0, 0, 255))
