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]
print type(match.good_kp1)
# pointsho = vo.make_homog(np.transpose(match.good_kp1))
# pointsho2 = vo.make_homog(np.transpose(match.good_kp2))
print np.shape(vo.correctedkpts1[0, :, :])
pointsho = vo.make_homog(np.transpose(vo.correctedkpts1[0, :, :]))
pointsho2 = vo.make_homog(np.transpose(vo.correctedkpts2[0, :, :]))
print "pointsho", pointsho[0:3, 1]
print "pointsho", pointsho2[0:3, 1]
points3d = vo.triangulate_point(pointsho[0:3, 1], pointsho2[0:3, 1],
vo.cam1.P, vo.cam2.P)
print "X", points3d
point2d = vo.cam1.project(points3d)
print "Project to camera 1:", point2d
point2d = vo.cam2.project(points3d)
print "Project to camera 2:", point2d
print "shape of pointsho", np.shape(pointsho)
scene = vo.opt_triangulation(match.good_kp1, match.good_kp2,
vo.cam1.P, vo.cam2.P)
point2d = vo.cam1.project(scene)
point2d_prime = vo.cam2.project(scene)
print scene[:, :3]
print point2d[:, 0]
print np.shape(vo.correctedkpts1[0])
