# projPointsLeft = np.asarray(projPointsLeft, dtype=np.float32).T # projPointsRight = np.asarray(projPointsRight, dtype=np.float32).T # parallaxDis = mycv.parallaxDis(projPointsLeft,projPointsRight) # print "每幅图平均视差",parallaxDis # print "三角化法" # triangulatePoints4D = cv2.triangulatePoints(P1, P2, projPointsLeft, projPointsRight) # triangulatePoints4D = triangulatePoints4D / triangulatePoints4D[3] # Dis_t = np.linalg.norm(triangulatePoints4D[:, 0] - triangulatePoints4D[:, 1]) # print "triangulate算法 空间坐标", triangulatePoints4D # print "triangulate算法 空间距离", Dis_t # print"triangulate算法 实际距离", Dis_t * Dis_P1P2 # # parallax # print "视差法" # parallaxPoints4D = mycv.parallaxPoints(P2, projPointsLeft, projPointsRight, imgLeft.shape[0]) # Dis_p = np.linalg.norm(parallaxPoints4D[:, 0] - parallaxPoints4D[:, 1]) # print "视差法 空间坐标", parallaxPoints4D # print "parallaxPoints算法 空间距离", Dis_p # print "parallaxPoints算法 实际距离", Dis_p * Dis_P1P2 Dis_t, Dis_P, realDis_t, realDis_p = mycv.computeDistance(P1, P2, imgLeft, projPointsLeft, projPointsRight) print "Dis_t:", Dis_t print "realDis_t:", realDis_t print "Dis_p:", Dis_P print "realDis_p:", realDis_p cv2.destroyAllWindows()
# 选出的两个点的匹配点 rightPoints = [] # 保存对应点的ssd ssds = [] # 15个候选点的索引 indexs = [] for p in leftPoints: tempSSD,tempRP ,tempIndex= mytest.getIndex(p[0], p[1], left, right, r) ssds.append(tempSSD) rightPoints.append(tempRP) indexs.append(tempIndex) # print rightPoints cright = mytest.drawLineInRight(right, rightPoints) print "测试:" Dis_t, Dis_P, realDis_t, realDis_p = mycv.computeDistance(P1, P2, left, leftPoints, rightPoints) print "三角化法:" print "空间距离:",Dis_t print "实际距离:",realDis_t print "视差法:" print "空间距离:",Dis_P print "实际距离:",realDis_p # 绘制左右视图 plt.figure(1) ax1 = plt.subplot(211) ax2 = plt.subplot(212) plt.sca(ax1) plt.title("left") plt.imshow(cleft, "gray") plt.sca(ax2)