python三维数据点拟合平面_3D平面拟合方法（C ++与Python）

importnumpyasnpimportscipy.optimizefrommpl_toolkits.mplot3dimportAxes3Dimportmatplotlib.pyplotaspltimportpandas

fig=plt.figure()ax=fig.gca(projection='3d')deffitPlaneLTSQ(XYZ):(rows,cols)=XYZ.shapeprint(rows,cols)G=np.ones((rows,3))print(XYZ)print(XYZ[0])G[:,0]=XYZ[0]# XG[:,1]=XYZ[1]# Yprint(G)Z=XYZ[2](a,b,c),resid,rank,s=np.linalg.lstsq(G,Z)print("a : ",a)print("b : ",b)print("c : ",c)print("Residual : ",resid)print("Rank : ",rank)print("Singular Value",s)normal=(a,b,-1)# I Don't Know WHY ?print("normal : ",normal)'''

normala = abs(pow(a,2)+pow(b,2)+pow(-1,2))

np.sqrt(normala)

abb=normal/normala

print("Normala : ",normala)

print("Normala : ", abb)

'''nn=np.linalg.norm(normal)# I Don't Know WHY ?print("nn : ",nn)normal=normal/nn# I Don't Know WHY ?print("Normal : ",normal)return(c,normal)#Import Data from CSVresult=pandas.read_csv("C:/Users/Logs/points_L.csv",header=None)# , names=['X', 'Y','Z']#result =result.head(5)print(result)normal1=pandas.read_csv("C:/Users/Logs/pose_left.csv",header=None)# , names=['X', 'Y','Z']print(normal1)abc=pandas.read_csv("C:/Users/Logs/abc.csv",header=None)# , names=['X', 'Y','Z']print(abc)#standard normal distribution / Bell.#np.random.seed(seed=1)data=result#print(data)print("NEW : ")print(data)c,normal=fitPlaneLTSQ(data)print(c,normal)# plot fitted planemaxx=np.max(data[0])maxy=np.max(data[1])minx=np.min(data[0])miny=np.min(data[1])print(maxx,maxy,minx,miny)point=np.array([0.0,0.0,c])# I Don't Know WHY ?print("Point : ",point)d=-point.dot(normal)# I Don't Know WHY ?print("D : ",d)# plot original pointsax.scatter(data[0],data[1],data[2])ax.quiver(data[0],data[1],data[2],normal1[0],normal1[1],normal1[2],length=0.2)# compute needed points for plane plottingxx,yy=np.meshgrid([minx,maxx],[miny,maxy])print(xx)print(yy)print("minx : ",minx)print("maxx : ",maxx)print("miny : ",miny)print("maxy : ",maxy)print("xx : ",xx)print("yy : ",yy)z=(-normal[0]*xx-normal[1]*yy-d)*1./normal[2]# I Don't Know WHY ?unit1=np.sqrt(pow(normal[0],2)+pow(normal[1],2)+pow(normal[2],2))print("Unit 1 : ",unit1)Error=abs(normal[0]*data[0]+normal[1]*data[1]+normal[2]*data[2]+d)/unit1print("Error",Error)Error_F=pandas.DataFrame(Error)print("Print : ",Error_F)Error_F.to_csv("C:/Users/Logs/Py_Error.csv")print("Z : ",z)# plot planeax.plot_surface(xx,yy,z,alpha=0.2)ax.set_xlim(-1,1)ax.set_ylim(-1,1)ax.set_zlim(1,2)ax.set_xlabel('x')ax.set_ylabel('y')ax.set_zlabel('z')plt.show()