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] 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,