# 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)
