print "good_kp2", len(match.good_kp2)
match.draw_matches(roi, match.good_matches)
cv2.namedWindow('roi', cv2.WINDOW_NORMAL)
cv2.imshow('roi', roi)
cv2.waitKey(0)
cv2.destroyAllWindows()
# Compute F
vo = VisualOdometry()
[match.good_kp1, match.good_kp2] = vo.EstimateF_multiprocessing(match.good_kp1,
match.good_kp2)
print vo.F
# Obtener matrices de cmara
vo.P_from_F(vo.F)
vo.create_P1()
print "P1", vo.cam1.P
print "P2", vo.cam2.P
# Triangulate points
vo.optimal_triangulation(match.good_kp2, match.good_kp1)
print "origianl points1", match.good_kp1[0:5, :]
print "corrected points1", vo.correctedkpts1[:, 0:5]
print "origianl points2", match.good_kp2[0:5, :]
print "corrected points2", vo.correctedkpts2[:, 0:5]
# print "structure", np.shape(vo.structure)
print "structure", vo.structure[:, 1]
image = vo.cam2.project(vo.structure)
print "reprojected", image[:, 0:5]
image2 = vo.cam1.project(vo.structure)
print "reprojected", image2[:, 0:5]
