import pandas as pd import numpy as np import matplotlib.pyplot as plt import sklearn.neighbors as ne import math import cv2 import socket import sys # 计算距离矩阵 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) # 拼接 # x,y,z,1 def joint(path): matrix12 = np.matrix([[ 0.999994, -2.30179e-005, -0.00338715, 22.9331], [2.30258e-005, 1.0, 2.28675e-006, 47.9336], [ 0.00338715, -2.36473e-006, 0.999994, -30.3661], [ 0.0, 0.0, 0.0, 1.0]]) matrix13 = np.matrix([[ 0.999988, -1.16345e-005, -0.00490587, 48.7739], [1.17334e-005, 1.0, 2.01129e-005, 91.5852], [ 0.00490587, -2.01702e-005, 0.999988, -71.2706], [ 0.0, 0.0, 0.0, 1.0]]) matrix14 = np.matrix([[ 0.999985, -2.39867e-005, -0.00554939, 74.1329], [2.41246e-005, 1.0, 2.47831e-005, 142.206], [ 0.00554939, -2.49166e-005, 0.999985, -113.294], [ 0.0, 0.0, 0.0, 1.0]]) matrix15 = np.matrix([[ 0.999981, -4.08954e-005, -0.00621533, 98.6007], [4.11151e-005, 1.0, 3.52216e-005, 192.034], [ 0.00621533, -3.54764e-005, 0.999981, -163.256], [ 0.0, 0.0, 0.0, 1.0]]) matrix16 = np.matrix([[ 0.999975, -6.00843e-005, -0.00709497, 129.069], [6.03902e-005, 1.0, 4.28944e-005, 236.579], [ 0.00709497, -4.33218e-005, 0.999975, -226.242], [ 0.0, 0.0, 0.0, 1.0]]) matrix17 = np.matrix([[ 0.999971, -7.82354e-005, -0.00755777, 154.855], [7.86353e-005, 1.0, 5.26223e-005, 282.774], [ 0.00755777, -5.32151e-005, 0.999971, -279.706], [ 0.0, 0.0, 0.0, 1.0]]) matrix18 = np.matrix([[ 0.999965, -0.000100226, -0.00832648, 184.996], [ 0.000100805, 1.0, 6.90642e-005, 323.342], [ 0.00832648, -6.99012e-005, 0.999965, -354.304], [ 0.0, 0.0, 0.0, 1.0]]) matrix22 = np.matrix([[ 0.872123, 0.000773, 0.489285, 4942.74], [-0.000745115, 1.0, -0.000251729, -2.8314], [ -0.489285, -0.000145035, 0.872124, -598.535], [ 0.0, 0.0, 0.0, 1.0]]) matrix32 = np.matrix([[ -0.869019, -0.00114372, -0.494777, 64430.3], [-0.000670692, -0.999994, 0.00348957, 186874.4], [ -0.494778, 0.00336434, 0.869013, -325.194], [ 0.0, 0.0, 0.0, 1.0]]) data12 = pd.read_csv(path + "12.csv") print("data12 load completed!") data13 = pd.read_csv(path + "13.csv") print("data13 load completed!") data14 = pd.read_csv(path + "14.csv") print("data14 load completed!") data15 = pd.read_csv(path + "15.csv") print("data15 load completed!") data16 = pd.read_csv(path + "16.csv") print("data16 load completed!") data17 = pd.read_csv(path + "17.csv") print("data17 load completed!") data18 = pd.read_csv(path + "18.csv") print("data18 load completed!") data22 = pd.read_csv(path + "22.csv") print("data22 load completed!") data32 = pd.read_csv(path + "32.csv") print("data32 load completed!") dataArray12 = np.c_[np.array(data12),np.ones(np.array(data12).shape[0])] dataArray13 = np.c_[np.array(data13),np.ones(np.array(data13).shape[0])] dataArray14 = np.c_[np.array(data14),np.ones(np.array(data14).shape[0])] dataArray15 = np.c_[np.array(data15),np.ones(np.array(data15).shape[0])] dataArray16 = np.c_[np.array(data16),np.ones(np.array(data16).shape[0])] dataArray17 = np.c_[np.array(data17),np.ones(np.array(data17).shape[0])] dataArray18 = np.c_[np.array(data18),np.ones(np.array(data18).shape[0])] dataArray22 = np.c_[np.array(data22),np.ones(np.array(data22).shape[0])] dataArray32 = np.c_[np.array(data32),np.ones(np.array(data32).shape[0])] dataTrans12 = matrix12.dot(dataArray12.transpose()) dataTrans13 = matrix13.dot(dataArray13.transpose()) dataTrans14 = matrix14.dot(dataArray14.transpose()) dataTrans15 = matrix15.dot(dataArray15.transpose()) dataTrans16 = matrix16.dot(dataArray16.transpose()) dataTrans17 = matrix17.dot(dataArray17.transpose()) dataTrans18 = matrix18.dot(dataArray18.transpose()) dataTrans22 = matrix22.dot(dataArray22.transpose()) dataTrans32 = matrix32.dot(dataArray32.transpose()) dataTrans = np.r_[dataTrans12.transpose(), dataTrans13.transpose(), dataTrans14.transpose(), dataTrans15.transpose(), dataTrans16.transpose(), dataTrans17.transpose(), dataTrans18.transpose(), dataTrans22.transpose(), dataTrans32.transpose()] return dataTrans # 读取一行 # x:筛选线中心x # y:筛选线中心y # a:筛选线角度，为弧度制 且在（-pi/2,pi/2）之间 # r:筛选线宽度 def read_row1(dataFrame,x0,y0,a,r): t1 = (dataFrame[1] > math.tan(a) * dataFrame[0] + math.tan(a) * x0 + y0 - math.cos(a) * r) t2 = (dataFrame[1] < math.tan(a) * dataFrame[0] + math.tan(a) * x0 + y0 + math.cos(a) * r) dataRow = dataFrame[t1 & t2] return dataRow # knn数据分层，目前有问题 def level(dataFrame,trainFile): knn = ne.KNeighborsClassifier(n_neighbors = 5) dataSort = dataFrame.sort_values(by=0) testArray = np.array(dataSort) dataTrain = pd.read_csv(trainFile) trainArray = np.array(dataTrain) knn.fit(trainArray[:,1:3],trainArray[:,3]) predict = knn.predict(testArray[:,0:3:2]) result = np.c_[testArray,predict] resultDataFrame = pd.DataFrame(result) dataReAnalyze = resultDataFrame[((resultDataFrame[3]==2) | (resultDataFrame[3]==3)) & (((resultDataFrame[0] > 6500) & (resultDataFrame[0] < 8500 )) | ((resultDataFrame[0] > 58500) & (resultDataFrame[0] < 60500 ))) ] for index in range(0,dataReAnalyze.shape[0]-2,1): deltaX = dataReAnalyze.iloc[index+1][0]-dataReAnalyze.iloc[index][0] deltaY = np.abs(dataReAnalyze.iloc[index+1][2]-dataReAnalyze.iloc[index][2]) if(deltaX < 1 and deltaY > 10): if(dataReAnalyze.iloc[index + 1][2] > dataReAnalyze.iloc[index][2]): resultDataFrame.loc[dataReAnalyze.index.tolist()[index],3] = 3 resultDataFrame.loc[dataReAnalyze.index.tolist()[index + 1],3] = 2 else: resultDataFrame.loc[dataReAnalyze.index.tolist()[index],3] = 2 resultDataFrame.loc[dataReAnalyze.index.tolist()[index + 1],3] = 3 level1 = resultDataFrame[resultDataFrame[4]==1] level2 = resultDataFrame[resultDataFrame[4]==2] level3 = resultDataFrame[resultDataFrame[4]==3] # 展示分层 plt.scatter(level1[0],level1[2],c='r') plt.scatter(level2[0],level2[2],c='b') plt.scatter(level3[0],level3[2],c='g') plt.grid() plt.show() return level1 # 方法2数据分层 # x,y,z,1 # dataFrame:输入数据帧 # levelRange:x向范围 # tempLen:小片段长度 # level:分层界限 def level_1(dataFrame,levelRange,tempLen,levelSpec): level1 = np.array([[0,0,0,0]]) level2 = np.array([[0,0,0,0]]) level3 = np.array([[0,0,0,0]]) for index in range(levelRange[0],levelRange[1],tempLen): datatemp = dataFrame[(dataFrame[0] > index) & (dataFrame[0] < index + tempLen-1)]# 数据分成250一段 if(datatemp.shape[0]==0): continue dataSortedZ = datatemp.sort_values(by = 2 ,ascending = False)# Z向排序 dataArray = np.array(dataSortedZ) array1 = dataArray[:,2] #z列数据 array2 = np.r_[dataArray[:,2][0],dataArray[:,2]] #z列数据后移一位 array2 = np.delete(array2,array2.shape[0] -1 ) #z列数据删除第一位 arrayMinus = array2 - array1 #错位相减 m = np.argwhere(arrayMinus > levelSpec) if(m.shape[0] == 0): level1 = np.r_[level1, dataArray] else: if(m.shape[0] == 1): level1 = np.r_[level1, dataArray[0:m[0][0]:1,:]] #（0，m[0][0]） level2 = np.r_[level2, dataArray[m[0][0]:dataArray.shape[0]:1,:]] #（m[0][0],m.len） else: level1 = np.r_[level1, dataArray[0:m[0][0]:1,:]] level2 = np.r_[level2, dataArray[m[0][0]:m[1][0]:1,:]] level3 = np.r_[level3, dataArray[m[1][0]:dataArray.shape[0]:1,:]] level1 = np.delete(level1,0,axis = 0) level2 = np.delete(level2,0,axis = 0) level3 = np.delete(level3,0,axis = 0) level1DataFrame = pd.DataFrame(level1) level2DataFrame = pd.DataFrame(level2) level3DataFrame = pd.DataFrame(level3) # 展示分层 """ plt.scatter(level1[:,0],level1[:,2],c='r') plt.scatter(level2[:,0],level2[:,2],c='b') plt.scatter(level3[:,0],level3[:,2],c='g') plt.grid() plt.show() """ return level1DataFrame # 线轮廓度初步计算 # x,y,z,1 def profile1(dataFrame): standard = pd.read_csv("D:\\standard.csv") standardArray = np.array(standard).transpose() dataFrame = dataFrame.sort_values(by = 0) dataFrame = dataFrame.reset_index(drop = True) # **************************去掉重叠部分******************************* A = np.matrix(np.array(dataFrame[(dataFrame[0].index % 2 == 0) & (dataFrame[0] > 4000) & (dataFrame[0] <61000)])[:,0:3:2]) B = np.matrix(np.array(dataFrame[(dataFrame[0].index % 2 == 1) & (dataFrame[0] > 4000) & (dataFrame[0] <61000)])[:,0:3:2]) A1 = np.matrix(np.array(dataFrame[(dataFrame[0].index % 2 == 0) & (dataFrame[0] > 2600) & (dataFrame[0] <4000)])[:,0:3:2]) B1 = np.matrix(np.array(dataFrame[(dataFrame[0].index % 2 == 1) & (dataFrame[0] > 2600) & (dataFrame[0] <4000)])[:,0:3:2]) A2 = np.matrix(np.array(dataFrame[(dataFrame[0].index % 2 == 0) & (dataFrame[0] > 61000) & (dataFrame[0] <66000)])[:,0:3:2]) B2 = np.matrix(np.array(dataFrame[(dataFrame[0].index % 2 == 1) & (dataFrame[0] > 61000) & (dataFrame[0] <66000)])[:,0:3:2]) C = np.matrix(np.array(dataFrame[(dataFrame[0] <= 2600) | (dataFrame[0] >=66000)])[:,0:3:2]) # 根据距离计算 #distanceMatrix = EuclideanDistances(A , B) #print(distanceMatrix) #print(distanceMatrix.shape) #overlapIndex = np.argwhere(distanceMatrix.diagonal()<5) # 根据Y方向偏差计算 #==============Part1====================== if(A.shape[0] > B.shape[0]): A = np.delete(A,A.shape[0]-1,0) else: if(A.shape[0] < B.shape[0]): B = np.delete(B,B.shape[0]-1,0) yoffset = (A - B)[:,1] overlapIndexA =np.argwhere(yoffset>2.5)[:,0] A = np.delete(A,overlapIndexA,0) overlapIndexB =np.argwhere(yoffset<-2.5)[:,0] B = np.delete(B,overlapIndexB,0) #==============Part2======================= if(A1.shape[0] > B1.shape[0]): A1 = np.delete(A1,A1.shape[0]-1,0) else: if(A1.shape[0] < B1.shape[0]): B1 = np.delete(B1,B1.shape[0]-1,0) yoffset = (A1 - B1)[:,1] overlapIndexA =np.argwhere(yoffset>5)[:,0] A1 = np.delete(A1,overlapIndexA,0) overlapIndexB =np.argwhere(yoffset<-5)[:,0] B1 = np.delete(B1,overlapIndexB,0) #=============Part3======================== if(A2.shape[0] > B2.shape[0]): A2 = np.delete(A2,A2.shape[0]-1,0) else: if(A2.shape[0] < B2.shape[0]): B2 = np.delete(B2,B2.shape[0]-1,0) yoffset = (A2 - B2)[:,1] overlapIndexA =np.argwhere(yoffset>5)[:,0] A2 = np.delete(A2,overlapIndexA,0) overlapIndexB =np.argwhere(yoffset<-5)[:,0] B2 = np.delete(B2,overlapIndexB,0) D = np.r_[A,A1,A2,B,B1,B2,C] #plt.scatter(D[:,0].getA(),D[:,1].getA(),color='r') #plt.grid() #plt.show() # ********************************************************************* #dataArray = np.array(data).transpose()[1:4:2,:]/1000 D = np.array(pd.DataFrame(D).sort_values(by = 0)) dataArray = np.array(D).transpose()/1000 mirroring = np.array([[1,0],[0,-1]]) dataMirroring = mirroring.dot(dataArray) # 只取第一个数据x方向 mean_x = dataMirroring.mean(axis = 1)[0] mean_x_s = standardArray.mean(axis = 1)[0] dataFrameMirroring = pd.DataFrame(dataMirroring.transpose()) dataLine = np.array(dataFrameMirroring[(dataFrameMirroring[0] > 20) & (dataFrameMirroring[0] < 50)]).transpose() # 直线部分 dataLine_s = np.array(standard[(standard['0'] > 260) & (standard['0'] < 290)]).transpose() # 标准直线部分 # 只取y方向 mean_y = dataLine.mean(axis =1)[1] mean_y_s = dataLine_s.mean(axis = 1)[1] p = np.poly1d(np.polyfit(dataLine[0,:],dataLine[1,:],1)) yval = p(dataLine[0,:]) p_s = np.poly1d(np.polyfit(dataLine_s[0,:],dataLine_s[1,:],1)) phi = math.atan(p[1])*180/math.pi rotateMatrix = cv2.getRotationMatrix2D((mean_x,mean_y),phi,1) dataTrans1 = rotateMatrix.dot(np.r_[dataMirroring,np.ones(dataMirroring.shape[1]).reshape(1,dataMirroring.shape[1])]) transMatrix = np.array([[1,0,mean_x_s - mean_x],[0,1,mean_y_s - mean_y]]) dataTrans2 = transMatrix.dot(np.r_[dataTrans1,np.ones(dataTrans1.shape[1]).reshape(1,dataTrans1.shape[1])]) dataFrameTrans2 = pd.DataFrame(dataTrans2.transpose()) #dataFrameTrans2[dataFrameTrans2[1] > 443.71].to_csv("D:\\Level1Trans.csv") plt.figure() plt.scatter(dataTrans2[0,:] , dataTrans2[1,:]) plt.plot(standardArray[0,:] , standardArray[1,:],color='r') plt.show() return dataFrameTrans2[dataFrameTrans2[1] > 443.71] # 先轮廓度二分法精确计算 def profile2(dataFrame): standard = pd.read_csv("D:\\standard.csv") standard = standard[standard['0'].index % 2 == 0] # 标准抽样 standardMatrix = np.matrix(standard) dataFrameSample = dataFrame[dataFrame[0].index % 5 == 0] dataMatrix = np.matrix(dataFrame) dataMatrixSample = np.matrix(dataFrameSample) seedStartP = 2.0 seedStartN = -2.0 seedP = seedStartP seedN = seedStartN for index in range(0,11,1): dataMatrixP = np.c_[dataMatrixSample[:,0], dataMatrixSample[:,1] + seedP] distanceMatrix = euclidean_distances(dataMatrixP,standardMatrix) sumP = (distanceMatrix.min(1).getA()**2).sum() dataMatrixN = np.c_[dataMatrixSample[:,0], dataMatrixSample[:,1] + seedN] distanceMatrix = euclidean_distances(dataMatrixN,standardMatrix) sumN = (distanceMatrix.min(1).getA()**2).sum() print("sumP:") print(sumP) print("sumN:") print(sumN) print("========================================================") if(sumP < sumN): seedN = (seedP + seedN) / 2.0 else: seedP = (seedP + seedN) / 2.0 transZ = (seedP + seedN) / 2 seedP = seedStartP seedN = seedStartN for index in range(0,11,1): dataMatrixP = np.c_[dataMatrixSample[:,0]+ seedP, dataMatrixSample[:,1]] distanceMatrix = euclidean_distances(dataMatrixP,standardMatrix) sumP = (distanceMatrix.min(1).getA()**2).sum() dataMatrixN = np.c_[dataMatrixSample[:,0]+ seedN, dataMatrixSample[:,1]] distanceMatrix = euclidean_distances(dataMatrixN,standardMatrix) sumN = (distanceMatrix.min(1).getA()**2).sum() print("sumP:") print(sumP) print("sumN:") print(sumN) print("========================================================") if(sumP < sumN): seedN = (seedP + seedN) / 2.0 else: seedP = (seedP + seedN) / 2.0 transX = (seedP + seedN) / 2 dataMatrixCorrected = np.c_[dataMatrix[:,0] + transX, dataMatrix[:,1] + transZ] distance = np.zeros([15,1000]) for index in range(0,15,1): start = index * 1000 end = index * 1000 + 1000 if(start > dataMatrixCorrected.shape[0]): break; if(end > dataMatrixCorrected.shape[0]): dataMatrixTemp = dataMatrixCorrected[start:dataMatrixCorrected.shape[0]:1,:] else: dataMatrixTemp = dataMatrixCorrected[start:end:1,:] distanceMatrix = euclidean_distances(dataMatrixTemp,standardMatrix) distanceTemp = distanceMatrix.min(1).getA() if(distanceTemp.shape[0]==1000): distance[index] = distanceTemp[:,0] else: distance[index] = np.append(distanceTemp[:,0],np.zeros(1000 - distanceTemp[:,0].shape[0])) distance = distance.reshape(-1,1) result = np.c_[dataMatrix[:,0] + transX, dataMatrix[:,1] + transZ,distance[0:dataMatrix.shape[0]:1,:]] return pd.DataFrame(result) # 计算极性 def polarity(dataFrame): standard = pd.read_csv("D:\\standard.csv") dataArray = np.array(dataFrame)[:,0:2:1]*1000 standardArray = np.array(standard)*1000 polarity = np.array([0]) for index in range(0,dataArray.shape[0],1): temp = cv2.pointPolygonTest(standardArray.astype(int),tuple(dataArray[index,:]),False) polarity =np.r_[polarity,temp] polarity = np.delete(polarity.reshape(1,-1).transpose(),0,0) polarityDistance = np.array(dataFrame)[:,2] * polarity.reshape(1,-1) newData = np.array(dataFrame)[:,0:2:1] newData = np.c_[newData,polarityDistance.transpose()] polarityData = pd.DataFrame(newData).sort_values(by=0) return polarityData # 平面抽样 def planeSample(dataFrame): data_sortX = data.sort_values(by = 0) data_sampleX = data_sortX.iloc[0:data_sortX.shape[0]:10,:] data_sortY = data_sampleX.sort_values(by = 1) data_sample = data_sortY.iloc[0:data_sortY.shape[0]:10,:] return data_sample #data = joint(sys.argv[1]) data = joint("E:\\Test\\1#190110_150215\\") dataFrame = pd.DataFrame(data) dataSample = dataFrame.sample(frac = 0.01,axis = 0) dataSample.to_csv("D:\\test\\Sample.csv",index = False,header = False) """ dataRowFrame9 = read_row1(dataFrame,-1000,90000,0,50) dataRowFrame9.to_csv("D:\\test\\dataRowFrame9.csv",index = False) level1 = level_1(dataRowFrame9,[-1000,70000],250,80) level1.to_csv("D:\\test\\level1.csv",index = False) dataTrans = profile1(level1) dataTrans.to_csv("D:\\test\\trans9.csv",index = False) # dataTrans = pd.read_csv("D:\\test\\trans9.csv") result = profile2(dataTrans) result.to_csv("D:\\test\\result.csv",index = False) polarityResult = polarity(result) polarityResult.to_csv("D:\\test\\polarityResult.csv",index = False) """