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