You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
86 lines
3.2 KiB
86 lines
3.2 KiB
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 |