import pandas as pd import numpy as np import matplotlib.pyplot as plt import scipy.linalg as linalg import cv2 # 计算旋转矩阵 def rotate_mat(axis, radian): return linalg.expm(np.cross(np.eye(3), axis / linalg.norm(axis) * radian)) # 拟合平面 def fit_plane(dataArray): a00 = np.sum(dataArray[:,0] ** 2) a01 = np.sum(dataArray[:,0] * dataArray[:,1]) a02 = np.sum(dataArray[:,0]) a10 = np.sum(dataArray[:,0] * dataArray[:,1]) a11 = np.sum(dataArray[:,1] ** 2) a12 = np.sum(dataArray[:,1]) a20 = np.sum(dataArray[:,0]) a21 = np.sum(dataArray[:,1]) a22 = dataArray.shape[0] b0 = np.sum(dataArray[:,0] * dataArray[:,2]) b1 = np.sum(dataArray[:,1] * dataArray[:,2]) b2 = np.sum(dataArray[:,2]) matrix = np.matrix([[a00,a01,a02],[a10,a11,a12],[a20,a21,a22]]) matrixI = np.linalg.inv(matrix) x = matrixI.dot(np.array([[b0],[b1],[b2]])) return x # 计算距离矩阵 def euclidean_distances(A, B): BT = B.transpose() vecProd = A * BT SqA = A.getA()**2 sumSqA = np.matrix(np.sum(SqA, axis=1)) sumSqAEx = np.tile(sumSqA.transpose(), (1, vecProd.shape[1])) SqB = B.getA()**2 sumSqB = np.sum(SqB, axis=1) sumSqBEx = np.tile(sumSqB, (vecProd.shape[0], 1)) SqED = sumSqBEx + sumSqAEx - 2*vecProd ED = (SqED.getA())**0.5 return np.matrix(ED) plt.figure() data = pd.read_csv('Level1Plane.csv') data = data[(data['0'] < 15000) | (data['0'] > 55000) | (data['2'] < 800)] data = data[data['2'] < 4000] # 高出4000的部分不认为是产品 data = data[(data['1'] > 40000) & (data['1'] < 150000) ] # y两端不认为是产品 data = data / 1000 plt.subplot(3,2,1) plt.scatter(data['0'],data['2']) plt.subplot(3,2,2) plt.scatter(data['1'],data['2']) dataPlane = data[(data['0'] > 20) & (data['0'] < 50) & (data['1'] > 20) & (data['1'] < 120)] dataPlaneArray = np.array(dataPlane); #plt.subplot(2,2,2) #plt.scatter(dataPlaneArray[:,0],dataPlaneArray[:,2]) x = fit_plane(dataPlaneArray) #print(x) phi0 = np.arctan(x[0]) phi1 = np.arctan(x[1]) rotate_mat0 = rotate_mat([0,1,0],1*phi0[0,0]) rotate_mat1 = rotate_mat([1,0,0],-1*phi1[0,0]) retate_mat = rotate_mat1.dot(rotate_mat0) dataPlaneArrayRotate = retate_mat.dot(dataPlaneArray[:,0:3:1].transpose()).transpose() mean_x = np.mean(dataPlaneArrayRotate[:,0]) mean_y = (np.max(dataPlaneArrayRotate[:,1]) + np.min(dataPlaneArrayRotate[:,1]))/2 mean_z = np.mean(dataPlaneArrayRotate[:,2]) #print(mean_x) #print(mean_y) #print(mean_z) x = fit_plane(dataPlaneArrayRotate) #print(x) dataArray = np.array(data) dataArrayRotate = retate_mat.dot(dataArray[:,0:3:1].transpose()).transpose() #plt.subplot(3,2,3) #plt.scatter(dataArrayRotate[:,0],dataArrayRotate[:,2]) #plt.subplot(3,2,4) #plt.scatter(dataArrayRotate[:,1],dataArrayRotate[:,2]) translate_mat = np.matrix([[1,0,0,-mean_x],[0,1,0,-mean_y],[0,0,1,-mean_z],[0,0,0,1]]) dataArrayTranslate = translate_mat.dot(np.c_[dataArrayRotate,np.ones(dataArrayRotate.shape[0])].transpose()).transpose().getA() #plt.subplot(3,2,5) #plt.scatter(dataArrayTranslate[:,0],dataArrayTranslate[:,2]) #plt.subplot(3,2,6) #plt.scatter(dataArrayTranslate[:,1],dataArrayTranslate[:,2]) standard = pd.read_csv('StandardPlane.csv') #plt.subplot(3,2,3) #plt.scatter(standard['0'],standard['2']) #plt.subplot(3,2,4) #plt.scatter(standard['1'],standard['2']) standardPlane = standard[(standard['0'] > 260) & (standard['0'] < 290)] standardPlaneArray = np.array(standardPlane) standardArray = np.array(standard) standard_x = np.mean(standardPlaneArray[:,0]) standard_y = (np.max(standardPlaneArray[:,1]) + np.min(standardPlaneArray[:,1]))/2 standard_z = np.mean(standardPlaneArray[:,2]) #print(standard_x) #print(standard_y) #print(standard_z) translate_standard_mat = np.matrix([[1,0,0,-standard_x],[0,1,0,-standard_y],[0,0,1,-standard_z],[0,0,0,1]]) standardArrayTranslate = translate_standard_mat.dot(np.c_[standardArray,np.ones(standardArray.shape[0])].transpose()).transpose().getA() #plt.subplot(3,2,3) #plt.scatter(standardArrayTranslate[:,0],standardArrayTranslate[:,2]) #plt.subplot(3,2,4) #plt.scatter(standardArrayTranslate[:,1],standardArrayTranslate[:,2]) sampleDataFrame = pd.DataFrame(dataArrayTranslate).sample(n = 10000) sample = np.array(sampleDataFrame) dataMartix = np.matrix(sample[:,0:3:1]) * np.matrix([[1,0,0],[0,0,0],[0,0,1]]) standardMartix = np.matrix(standardArrayTranslate[0:12000:1,0:3:1]) * np.matrix([[1,0,0],[0,0,0],[0,0,1]]) distanceMatrix = euclidean_distances(dataMartix,standardMartix) seedStartP = 0.2 seedStartN = -0.2 dataMartixP = dataMartix dataMartixN = dataMartix seedP = seedStartP seedN = seedStartN for index in range(0,8,1): dataMartixP = np.c_[dataMartix[:,0:2:1], dataMartix[:,2] + seedP] distanceMatrix = euclidean_distances(dataMartixP,standardMartix) sumP = distanceMatrix.min(1).getA().sum() dataMartixN = np.c_[dataMartix[:,0:2:1], dataMartix[:,2] + seedN] distanceMatrix = euclidean_distances(dataMartixN,standardMartix) sumN = distanceMatrix.min(1).getA().sum() print("sumP:") print(sumP) print("sumN:") print(sumN) print("========================================================") if(sumP < sumN): seedN = (seedP + seedN) / 2 else: seedP = (seedP + seedN) / 2 transZ = (seedP + seedN) / 2 print("+++++++++++++++++++++++++++++++++++++++++++++++++++++") seedP = -2 seedN = 2 for index in range(0,8,1): dataMartixP = np.c_[dataMartix[:,0] + seedP,dataMartix[:,1:3:1]] distanceMatrix = euclidean_distances(dataMartixP,standardMartix) sumP = distanceMatrix.min(1).getA().sum() dataMartixN = np.c_[dataMartix[:,0] + seedN,dataMartix[:,1:3:1]] distanceMatrix = euclidean_distances(dataMartixN,standardMartix) sumN = distanceMatrix.min(1).getA().sum() print("sumP:") print(sumP) print("sumN:") print(sumN) print("========================================================") if(sumP < sumN): seedN = (seedP + seedN) / 2 else: seedP = (seedP + seedN) / 2 transX = (seedP + seedN) / 2 dataArrayTranslate1 = np.c_[dataArrayTranslate[:,0] + transX,dataArrayTranslate[:,1:3:1]] dataArrayTranslate2 = np.c_[dataArrayTranslate1[:,0:2:1], dataArrayTranslate1[:,2] + transZ] distance = np.zeros([15,10000]) for index in range(0,15,1): start = index * 10000 end = index * 10000 + 10000 distanceMatrix = euclidean_distances(np.matrix(dataArrayTranslate2[start:end:1,:]),standardMartix) distanceTemp = distanceMatrix.min(1).getA() ** 0.5 distance[index] = distanceTemp[:,0] result = np.c_[dataArrayTranslate2[0:150000:1,:],distance.reshape(-1,1)] pd.DataFrame(result).to_csv('PlaneResult.csv',index = False) #plt.show()