基於移動最小二乘法的點雲曲面擬合（python）

1.移動最小二乘法介紹

  為了更好地對數據量大且形狀復雜的離散數據進行擬合，曾清紅等人^[1]開發出一種新的算法——移動最小二乘法。這種新的最小二乘算法為點雲數據的處理提供了新的方法。使用點雲數據擬合曲面時，由於點雲的數據量大、形狀復雜的特點，如果使用傳統的最小二乘法擬合可能會得到病態的曲面方程，從而導致較大的誤差。而使用移動最小二乘法擬合點雲不僅能夠減少誤差，提升局部的准確率，還能避免分塊擬合和平滑化的過程。下圖為子區域的划分示意圖。

                             

                         

  通過某點確定一個子區域，在該區域內，移動最小二乘法是根據區域內的空間點加權擬合方程，並根據擬合方程解算這一點的坐標。使用移動最小二乘法擬合點雲曲面可以看作是一個插值的過程，每一次插值都對應着一次加權最小二乘的方程擬合，所以它可以逼近曲面但不能得到曲面方程。

  關於該算法的原理敘述請參看曾清紅, 盧德唐. 基於移動最小二乘法的曲線曲面擬合。

2.程序設計

　　在某個子區域中可能會出現空間點數量過少或分布復雜不規律的情況，這會導致最小二乘法解算方程系數時出現奇異矩陣。一般做法是迭代調整閾值，直到子區域內的空間點數量分布符合擬合要求，但這種方法復雜度較高且在點雲本身分布失常的情況下調整閾值沒有意義。為了方便，我們在點雲分布不均勻導致出現奇異矩陣的情況下，引入幾何中心（也可以根據情況選擇其他方法）代替最小二乘的方程求解。

# -*- coding: utf-8 -*-
"""
Created on Sat Apr 11 17:39:16 2020

@author: L JL
"""
import numpy as np 
import matplotlib.pyplot as plt 
from mpl_toolkits.mplot3d import Axes3D

f = open('xuanze.txt','r')
point = f.read()
f.close()
data1 = point.replace('\n',' ')
data2 = data1.split(' ')
data2.pop()
n = len(data2)
data2 = list(map(float,data2))

mat1 = np.array(data2[0:n]) 
mat2 = mat1.reshape(int(n/3),3)

mat3 = []

for each in mat2:
    each_line = list(map(lambda x:float(x),each))
    mat3.append(each_line)
mat4=np.array(mat3)
x = [k[0] for k in mat4]
y = [k[1] for k in mat4]
z = [k[2] for k in mat4]

def D_radius(x,y,X,Y,N):
    ind_mat = np.zeros((N,2))
    for i in range(N):
        s = ((x-X[i])**2+(y-Y[i])**2)**0.5
        ind_mat[i][0] = s
        ind_mat[i][1] = i
    ind_mat = ind_mat[np.lexsort(ind_mat[:,::-1].T)]
    return ind_mat
    
def W_mat(d,x0,y0,x,y): 
    s=(((x-x0)**2+(y-y0)**2)**0.5)/d
    #print((x-x0)**2+(y-y0)**2)
    #print(s)
    if (s<=0.5):
        return (2/3)-4*s**2+4*s**3 
    elif(s<=1):
        return (4/3)-4*s+4*s**2-(4/3)*s**3 
    else: return 0

def A_mat(W,P,M,N):
    A = []
    for m in range(M):
        a = []
        for n in range(M):
            #pmn = p_mn(W,P,N,m,n)
            pmn = 0
            for i in range(N):
                pmn = pmn + W[i]*P[i,m]*P[i,n]
            a.append(float(pmn))
        A.append(a)
    return A

def B_mat(u,W,P,M,N):
    B = []
    for i in range(M):
        sumB = 0
        for j in range(N):
            sumB = sumB + W[j]*P[j,i]*u[j]
        B.append(float(sumB))
    return B

X = np.array(x)
Y = np.array(y)
Z = np.array(z)
Xmax = int(np.max(X))
Xmin = int(np.min(X))
Ymax = int(np.max(Y))
Ymin = int(np.min(Y))


N = len(X)
M = 3
P = np.mat(np.zeros((N,M)))
W = np.mat(np.zeros((N,1)))
A = np.mat(np.zeros((M,M)))
B = np.mat(np.zeros((M,N)))
u = np.mat(Z).T
a = np.mat(np.zeros((M,1)))
d_mat = np.mat(np.zeros((N,1)))
#dataZ = []

dataX = np.arange(Xmin,Xmax,0.5)
dataY = np.arange(Ymin,Ymax,0.5)

print(Xmin,Xmax)
print(Ymin,Ymax)

f2 = open("dataZ.txt","w")
for i in dataX:
    for j in dataY:
        #d = D_radius(i,j,X,Y,N)
        d = 2
        ind_mat = D_radius(i,j,X,Y,N)
        #print(ind_mat)
        if ind_mat[3,0] <= d:
            try:
                W = np.mat(np.zeros((N,1)))
                for n in range(0,N):
                    P[n,0] = 1 
                    P[n,1] = X[n]
                    P[n,2] = Y[n] 
                    W[n] = W_mat(d,X[n],Y[n],i,j)
                #print(W)
                A = A_mat(W,P,M,N)
                B = B_mat(u,W,P,M,N)
                c = np.linalg.solve(A,B) 
                #dataZ.append(c[0]+c[1]*i+c[2]*j)
                dataZ = c[0]+c[1]*i+c[2]*j
                print('A')
                #print(dataZ)
            except:
                ind_Zsum = 0
                for ind in range(0,4):
                    ind_Zsum += Z[int(ind_mat[ind,1])]
                #dataZ.append(ind_Zsum/4)
                dataZ = ind_Zsum/4
                print('B')
                #print(dataZ)
        else:
            ind_Zsum = 0
            for ind in range(0,4):
                ind_Zsum += Z[int(ind_mat[ind,1])]
            #dataZ.append(ind_Zsum/4)
            dataZ = ind_Zsum/4
            print('C')
            #print(dataZ)
        f2.write(str(i) + ',' + str(j) + ',' + str(dataZ) + '\n')          
        print(i,j)

f2.close()        

# 3D繪圖示意
import numpy as np 
import matplotlib.pyplot as plt 
from mpl_toolkits.mplot3d import Axes3D

fig = plt.figure()
ax = Axes3D(fig)
Xmin = -82
Xmax = 23
Ymin = 0
Ymax = 42

x = np.arange(Xmin, Xmax, 0.5)
y = np.arange(Ymin, Ymax, 0.5)

y,x  = np.meshgrid(y, x)

f = open('dataZ.txt','r')
point = f.read()
f.close()
data1 = point.replace('\n',',')
data2 = data1.split(',')
data2.pop()
n = len(data2)
data2 = list(map(float,data2))

mat1 = np.array(data2[0:n])
mat2 = mat1.reshape(int(n/3),3)

z = mat2[:,2].reshape(x.shape)
'''
print("網格化后的X=",x)
print("X維度信息",x.shape)
print("網格化后的Y=",y)
print("Y維度信息", y.shape)
print("網格化后的Z=",z)
'''

ax.plot_surface(x, y, z, cmap=plt.cm.hot)      # 漸變顏色
#ax.contourf(x, y, z,cmap=plt.cm.hot)
ax.set_xlabel('X Label (m)')
ax.set_ylabel('Y Label (m)')
ax.set_zlabel('Z Label (m)')
ax.set_title('Point Cloud Surface Fitting')
plt.show()

 

3.擬合情況及存在的問題

  其他點雲的擬合情況，如下圖所示。

 

  （1）部分區域出現擬合異常；

  （2）程序計算量大，復雜度太高，有待優化；

  （3）對高分辨率點雲，通過調整步長，可以調整插值步數，提高精度。

 

參考文獻：

  [1] 曾清紅, 盧德唐. 基於移動最小二乘法的曲線曲面擬合 [J]. 工程圖學學報, 2004