lipan
29f1d45d66
算法更新: delivery v1.2.1、平整度v1.5、测距v2.2、整车配准v1.3 适配器: vehicle-registration 新增、各算法 manifest 和 adapter 更新 后端: taskCadFiles/source 字段迁移、derivedPointClouds 注册、取消任务联动 前端: ElapsedTime 时长显示、两步算法流程、canRun 逻辑修复 文档: 新增5份算法性能分析报告
63 lines
2.0 KiB
Python
63 lines
2.0 KiB
Python
from __future__ import annotations
|
|
|
|
from dataclasses import dataclass
|
|
|
|
import numpy as np
|
|
import open3d as o3d
|
|
|
|
|
|
@dataclass
|
|
class PlaneResult:
|
|
model: tuple[float, float, float, float]
|
|
inliers: list[int]
|
|
inlier_ratio: float
|
|
normal_angle_deg: float
|
|
rmse: float
|
|
pointcloud: o3d.geometry.PointCloud
|
|
|
|
|
|
class PlaneEstimator:
|
|
"""Generate noisy planes and estimate planes using Open3D RANSAC."""
|
|
|
|
def create_square_plane_with_noise(
|
|
self,
|
|
size_mm: float = 1000.0,
|
|
noise_std_mm: float = 0.5,
|
|
num_points: int = 10000,
|
|
) -> o3d.geometry.PointCloud:
|
|
half_size_m = size_mm / 2000.0
|
|
noise_std_m = noise_std_mm / 1000.0
|
|
xy = np.random.uniform(-half_size_m, half_size_m, size=(num_points, 2))
|
|
z = np.random.normal(0, noise_std_m, size=(num_points, 1))
|
|
points = np.hstack([xy, z])
|
|
pcd = o3d.geometry.PointCloud()
|
|
pcd.points = o3d.utility.Vector3dVector(points)
|
|
return pcd
|
|
|
|
def estimate(
|
|
self,
|
|
pointcloud: o3d.geometry.PointCloud,
|
|
threshold_mm: float = 0.5,
|
|
max_iterations: int = 2000,
|
|
) -> PlaneResult:
|
|
threshold_m = threshold_mm / 1000.0
|
|
plane_model, inliers = pointcloud.segment_plane(
|
|
distance_threshold=threshold_m,
|
|
ransac_n=3,
|
|
num_iterations=max_iterations,
|
|
)
|
|
|
|
a, b, c, d = plane_model
|
|
normal = np.array([a, b, c], dtype=np.float64)
|
|
normal = normal / (np.linalg.norm(normal) + 1e-12)
|
|
ideal = np.array([0.0, 0.0, 1.0])
|
|
angle = float(np.degrees(np.arccos(np.clip(abs(np.dot(normal, ideal)), -1.0, 1.0))))
|
|
|
|
points = np.asarray(pointcloud.points)
|
|
inlier_points = points[inliers]
|
|
distances = np.abs(np.dot(inlier_points, np.array([a, b, c])) + d)
|
|
rmse = float(np.sqrt(np.mean(distances ** 2))) if len(distances) else 0.0
|
|
ratio = len(inliers) / len(points) * 100 if len(points) else 0.0
|
|
|
|
return PlaneResult(tuple(float(x) for x in plane_model), list(inliers), ratio, angle, rmse, pointcloud)
|