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