遇到的问题:func input vector length N=6 must not exceed func output vector length M=1
完整代码:
import open3d as o3d
import numpy as np
from scipy.optimize import curve_fit
voxel_size = 0.01 #越小密度越大
radius =0.07
pcd = o3d.io.read_point_cloud("test.ply")
pcd = pcd.voxel_down_sample(voxel_size) #下采样
cloud = pcd
points = np.asarray(cloud.points) #点云转换为数组 点云数组形式为[[x1,y1,z1],[x2,y2,z2],...]
kdtree = o3d.geometry.KDTreeFlann(cloud) #建立KDTree
num_points = len(cloud.points) #点云中点的个数
pcd.paint_uniform_color([1, 0, 0]) # 初始化所有颜色为红色
# 定义非线性函数,这里假设是一个二次曲面
def func(x, a, b, c, d, e, f):
r= a * x[0]**2 + b * x[1]**2 + c * x[0] * x[1] + d * x[0] + e * x[1] + f
return r.ravel()
def f(x, y):
return popt[0]*x**2 +popt[1]*y**2 +popt[2]* x*y +popt[3]*x + popt[4]*y +popt[5]
# 定义曲面的梯度函数,即一阶偏导数
def gradient(f, x, y):
# 使用中心差分法近似求导 #参考https://cloud.tencent.com/developer/article/1685164
h = 1e-6 # 差分步长,可以根据精度要求调整
df_dx = (f(x + h, y) - f(x - h, y)) / (2 * h) # 对x求偏导
df_dy = (f(x, y + h) - f(x, y - h)) / (2 * h) # 对y求偏导
return df_dx, df_dy
# 定义曲面的曲率函数,即二阶偏导数
def curvature(f, x, y):
# 使用中心差分法近似求导
h = 1e-6 # 差分步长,可以根据精度要求调整
d2f_dx2 = (f(x + h, y) - 2 * f(x, y) + f(x - h, y)) / (h ** 2) # 对x求二阶偏导
d2f_dy2 = (f(x, y + h) - 2 * f(x, y) + f(x, y - h)) / (h ** 2) # 对y求二阶偏导
d2f_dxdy = (f(x + h, y + h) - f(x + h, y - h) - f(x - h, y + h) + f(x - h, y - h)) / (4 * h ** 2) # 对xy求混合偏导
# 根据公式计算高斯曲率K和平均曲率H
df_dx, df_dy = gradient(f, x, y) # 调用梯度函数求一阶偏导
E = 1 + df_dx ** 2
F = df_dx * df_dy
G = 1 + df_dy ** 2
L = d2f_dx2 / np.sqrt(1 + df_dx ** 2 + df_dy ** 2) #np.sqrt()表示开方
M = d2f_dxdy / np.sqrt(1 + df_dx ** 2 + df_dy ** 2)
N = d2f_dy2 / np.sqrt(1 + df_dx ** 2 + df_dy ** 2)
K = (L * N - M ** 2) / (E * G - F ** 2) # 高斯曲率
H = (E * N + G * L - 2 * F * M) / (2 * (E * G - F ** 2)) # 平均曲率
return K, H
curvatures = []
for i in range(num_points):
k, idx, _ = kdtree.search_radius_vector_3d(cloud.points[i], radius) #返回邻域点的个数和索引
neighbors = points[idx] #数组形式为[[x1,y1,z1],[x2,y2,z2],...]
#print(k)
Y = neighbors[:, 2] # 因变量
X = neighbors[:, [0,1]] # 自变量 [[2,3],[1,1],[8,9],[11,12],[4,5],[8,9]] 6*2
popt, pcov = curve_fit(func, xdata=X.T,ydata= Y)
x = cloud.points[i][0] # 某一点的x坐标
y = cloud.points[i][1] # 某一点的y坐标
K, H = curvature(f, x, y) # 计算该点的曲率
curvatures.append([K,H])
print(curvatures)
for i in range(len(curvatures)):
if -0.05<curvatures[i][0] < 0.05 and -0.05<curvatures[i][1] <0.05: #平坦
np.asarray(pcd.colors)[i] = [0, 0, 0]#黑
elif -0.05<curvatures[i][0] < 0.05 and curvatures[i][1] >0.05: #凸
np.asarray(pcd.colors)[i] = [1, 0, 0]#红
elif -0.05<curvatures[i][0] < 0.05 and -0.05<curvatures[i][1] <0.05: #凹
np.asarray(pcd.colors)[i] = [0, 1, 0]#绿
elif curvatures[i][0] < -0.05 and curvatures[i][1] >0.05: #鞍形脊 大部分凸,少部分凹
np.asarray(pcd.colors)[i] = [0, 0, 1]#蓝
elif curvatures[i][0] < -0.05 and curvatures[i][1] <-0.05: #鞍形谷 大部分凹,少部分凸
np.asarray(pcd.colors)[i] = [0, 1, 1]#青
elif curvatures[i][0] > 0.05 and curvatures[i][1] >0.05: #峰
np.asarray(pcd.colors)[i] = [1, 0, 1]#紫
elif curvatures[i][0] > 0.05 and curvatures[i][1] <-0.05: #阱
np.asarray(pcd.colors)[i] = [1, 1, 0]#黄
#显示点云
o3d.visualization.draw_geometries([pcd], window_name="想法3",
width=800, height=800, left=50, top=50,
mesh_show_back_face=False)