from scipy.spatial import Delaunay
from mpl_toolkits.mplot3d import Axes3D
import numpy as np
import matplotlib.pyplot as plt
from numpy import *
import operator
from functools import reduce

W = np.loadtxt(\'data.txt\')
# print(W)
# print(W.data)
(m,n) = W.shape
matchedPoints1 = W[0:int(m/2),:].copy()
matchedPoints1 = double(matchedPoints1)  #转为double
matchedPoints2=W[int(m/2 + 1):m,:].copy()#奇数时会丢失一个
matchedPoints2=double(matchedPoints2)
matchedPoints1 = mat(matchedPoints1)
matchedPoints2 = mat(matchedPoints2)

# print(matchedPoints1)
# print(matchedPoints2)
k1=[[3483.7,-3.4942, 614.50],
    [0.0000, 3485.2, 488.13],
    [0.0000, 0.0000, 1.0000]
   ]
k1 = mat(k1)

I=eye(3)
O=[[0],[0],[0]];
O = mat(O)

k2=[[3464.6, -1.1771, 680.15],
    [0.0000, 3462.7, 503.73],
    [0.0000, 0.0000, 1.0000]
    ]
k2 = mat(k2)
R=[ [0.9989, 0.0002, -0.0476],
    [-0.0008, 0.9999, -0.0120],
    [0.0476, 0.0120, 0.9988]
    ]
R = mat(R)

T=[ [279.48],
    [-3.7650],
    [1.9500]
    ]
T = mat(T)

# test = hstack((I,O))
M1=k1*hstack((I,O))
M2=k2* hstack((R,T))
M1 = mat(M1)
M2 = mat(M2)
cameraMatrix1=double(M1).copy()
cameraMatrix2=double(M2).copy()

fl=k1[0,0]
fr=k2[0,0]
Xl=matchedPoints1[:,0].copy()
Yl=matchedPoints1[:,1].copy()
# Xl=mat(Xl)
# Yl=mat(Yl)
Xr=matchedPoints2[:,0].copy()
Yr=matchedPoints2[:,1].copy()

Tx=T[0]; #矩阵
Tz=T[2];
R1=R[0,0]
R2=R[0,1]
R3=R[0,2]
R7=R[2,0]
R8=R[2,1]
R9=R[2,2]
z = []
x = []
y = []
for k in range(0,int(m/2)):
     zz = fl * (fr * Tx - Xr[k] * Tz)/(Xr[k] *(R7 * Xl[k] + R8 * Yl[k]
                + fl * R9) - fr * (R1 * Xl[k] + R2 * Yl[k] + fl * R3))
     z.append(zz[0,0])
     xx = z[k] * Xl[k] / fl;
     x.append(xx[0,0])
     yy = z[k] * Yl[k] / fl;
     y.append(yy[0,0])
     # print(k)
# z = mat(z)
# x = mat(x)
# y = mat(y)
# x=x.transpose()
# y=y.transpose()
# z=z.transpose()
# x = x[0,:].copy()
# y = y[0,:].copy()
# z = z[0,:].copy()
# x = [1.64546964617096,3.29082591875077,4.93606803026144]
# y = [3.29093929234191,3.29082591875077,3.29071202017429]
# z = [5732.32260636576,5732.12512657603,5731.92673234059]
print(x)

#画图
fig = plt.figure()
ax = fig.gca(projection=\'3d\')
ax.plot_trisurf(x, y, z, linewidth=1, antialiased=True)
plt.show()