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