86 lines
3.1 KiB
Python
86 lines
3.1 KiB
Python
import open3d as o3d
|
|
import numpy as np
|
|
import matplotlib.pyplot as plt
|
|
import sys
|
|
import statistics
|
|
import math
|
|
import copy
|
|
import os,time
|
|
from scipy.stats import mode
|
|
from scipy.spatial import KDTree
|
|
from scipy.spatial import cKDTree
|
|
import matplotlib.font_manager as fm
|
|
|
|
#处理点云文件保存脚踝一下的部分点云
|
|
def dealPointData(pidPath,pidNewPath):
|
|
# 读取 PLY 文件
|
|
point_cloud = o3d.io.read_point_cloud(pidPath)
|
|
# 移除离散点
|
|
cl, ind = point_cloud.remove_statistical_outlier(nb_neighbors=20, std_ratio=0.06)
|
|
point_cloud = point_cloud.select_by_index(ind)
|
|
# 保存处理后的点云
|
|
o3d.io.write_point_cloud(pidNewPath, point_cloud)
|
|
data = np.loadtxt(pidNewPath)
|
|
# 提取xyz坐标
|
|
x = data[:, 0]
|
|
y = data[:, 1]
|
|
z = data[:, 2]
|
|
# 创建一个布尔索引，筛选出在x-y平面内，半径在0.5范围内的点 (z >= 0.00) &
|
|
mask = (z >=0) & (z <= 0.1) & (x**2 + y**2 <= 0.7**2)
|
|
# 根据索引，保留符合条件的点
|
|
filtered_data = data[mask]
|
|
# 将保留的点云数据保存到新的文件
|
|
np.savetxt(pidNewPath, filtered_data, delimiter=' ')
|
|
|
|
|
|
#计算点云的数据中，脚底和地板的切线位置
|
|
def boxCenter(pid):
|
|
pidPath = "D:\\xyz\\" + str(pid) + "_point.xyz"
|
|
pidNewPath = "D:\\xyz\\" + str(pid) + "_new.xyz"
|
|
dealPointData(pidPath,pidNewPath)
|
|
|
|
pcd = o3d.io.read_point_cloud(pidNewPath)
|
|
allPoints = np.asarray(pcd.points)
|
|
if len(allPoints) == 0:
|
|
print("点云为空，无法计算均值。")
|
|
return 0
|
|
|
|
# 使用RANSAC算法提取平面
|
|
plane_model, inliers = pcd.segment_plane(distance_threshold=0.00670, ransac_n=3, num_iterations=1000)
|
|
inlier_cloud = pcd.select_by_index(inliers)
|
|
# 获取被染成红色的点的坐标数组
|
|
red_points = inlier_cloud.points
|
|
red_points_np = np.asarray(red_points)
|
|
# 获取 red_points_np 的 z 值数组
|
|
z_red_values = red_points_np[:, 2]
|
|
# 计算中位数
|
|
medianRed = np.median(z_red_values)
|
|
# 计算中位数向上的平均数
|
|
meanRedUp = np.mean(z_red_values[z_red_values > medianRed])
|
|
#计算中位数向下的平均数
|
|
#meanRedDown = np.mean(z_red_values[z_red_values < medianRed])
|
|
|
|
# Exclude ground points from allPoints
|
|
foot_points_np = np.asarray([point for point in allPoints if point not in red_points_np])
|
|
if len(foot_points_np) == 0 :
|
|
print("脚部点云为空。")
|
|
return 0
|
|
#过滤掉地板，计算出脚底的点云数据
|
|
points_np_foot = foot_points_np[(foot_points_np[:, 2] < 0.046) & (foot_points_np[:, 2] > meanRedUp)]
|
|
# 计算平均值
|
|
mean = np.mean(points_np_foot[:, 2])
|
|
|
|
# 按照 Z 值进行排序
|
|
sorted_points = points_np_foot[np.argsort(points_np_foot[:, 2])]
|
|
# 获取 Z 值最低的前十个点
|
|
if len(sorted_points) == 0:
|
|
print("脚底板最低的点云为空")
|
|
return 0
|
|
try:
|
|
lowest_points = sorted_points[10:20]
|
|
except Exception as e:
|
|
print("获取脚底板最低的倒数第十到20的点出现异常错误")
|
|
return 0
|
|
# 计算平均值
|
|
meanLowEst = np.mean(lowest_points[:, 2]) + 1.05
|
|
return meanLowEst |