feat: 算法升级、整车配准集成与性能分析文档
算法更新: delivery v1.2.1、平整度v1.5、测距v2.2、整车配准v1.3 适配器: vehicle-registration 新增、各算法 manifest 和 adapter 更新 后端: taskCadFiles/source 字段迁移、derivedPointClouds 注册、取消任务联动 前端: ElapsedTime 时长显示、两步算法流程、canRun 逻辑修复 文档: 新增5份算法性能分析报告
This commit is contained in:
68
alg/delivery_v1.2.1/coupler_panel_release/README.md
Normal file
68
alg/delivery_v1.2.1/coupler_panel_release/README.md
Normal file
@@ -0,0 +1,68 @@
|
||||
# coupler-panel-validation
|
||||
|
||||
高铁车钩面板**偏转角测量**算法。输入待测点云 + ROI + 理论 CAD，输出面板相对车体纵轴的两个方向偏转角及最大偏差，并生成 Excel 报表。
|
||||
|
||||
## 这个项目做什么
|
||||
|
||||
给定一块车钩安装面板的扫描点云，算法测量它相对理论安装姿态的**偏转**：
|
||||
|
||||
- `θ_right` —— 面板法向在一个方向上的偏转角（度）
|
||||
- `θ_ref` —— 面板法向在另一个方向上的偏转角（度）
|
||||
- `dx_y_max` / `dx_z_max` —— 由偏转引起的最大 x 向偏差（mm）
|
||||
|
||||
支持两种输入规模：
|
||||
- **单零件点云**（对零件 STP 采样得到的稠密点云）
|
||||
- **整车扫描点云**（GB 级 PLY，用 ROI 框出车钩区域后内部流式裁剪）
|
||||
|
||||
## 三件套输入（集成主路径）
|
||||
|
||||
| 输入 | 说明 |
|
||||
|---|---|
|
||||
| **ROI JSON** | 6 个数的轴对齐包围盒，框出待测面板区域（即裁剪超参数） |
|
||||
| **点云 PLY** | 待测点云，可为整车（多 GB），算法内部按 ROI 流式裁剪 |
|
||||
| **CAD STP** | 理论模型，作辅助 / 审计 |
|
||||
|
||||
输出：`eval_report.xlsx`（含两个偏转角的 GT / 预测 / 误差 + 中间量 + 运行时间）。
|
||||
|
||||
## 快速开始
|
||||
|
||||
```bash
|
||||
# 安装（锁定版本见 requirements.txt）
|
||||
python -m pip install -r requirements.txt
|
||||
pip install -e ".[all]"
|
||||
|
||||
# 三件套集成入口
|
||||
python examples/run_integration.py \
|
||||
--roi examples/roi_train2_coupler.json \
|
||||
--cloud /path/to/train_2_1T.ply \
|
||||
--stp /path/to/asm.stp \
|
||||
--out runs/train2 \
|
||||
--profile train2_coupler
|
||||
```
|
||||
|
||||
库调用：
|
||||
|
||||
```python
|
||||
from coupler_panel.pipeline import run_coupler_measurement_pipeline
|
||||
from coupler_panel.roi_select import load_roi_json
|
||||
|
||||
result = run_coupler_measurement_pipeline(
|
||||
input_cloud = "train_2_1T.ply",
|
||||
out_dir = "runs/train2",
|
||||
profile = "train2_coupler",
|
||||
roi_spec = load_roi_json("roi_train2_coupler.json"),
|
||||
)
|
||||
print(result.theta_right_deg, result.theta_ref_deg)
|
||||
```
|
||||
|
||||
## 文档
|
||||
|
||||
| 文档 | 内容 |
|
||||
|---|---|
|
||||
| [USAGE.md](USAGE.md) | **集成方使用指南** —— 三件套输入、API、ROI JSON、profile、输出、坐标系 |
|
||||
| [CHANGES.md](CHANGES.md) | **修改记录** —— 每轮改了什么、为什么、遇到的问题与定位解决 |
|
||||
|
||||
## 坐标系约定
|
||||
|
||||
- 算法以面板法向 `n=(n_x,n_y,n_z)` 相对 x 轴的夹角定义偏转，强制 `n_x>0`。
|
||||
- **坐标轴含义由数据集决定**：项目标称约定为 x=纵向、y=宽度、z=高度；但某些整车数据集为 x=车长、y=车高、z=车宽（y/z 对调）。偏转角公式对称、理论 0 不受影响，但 `D_y_max`/`D_z_max` 对应的物理轴随之对调。ROI JSON 的六个边界必须与**点云自身坐标系**一致。详见 USAGE.md。
|
||||
256
alg/delivery_v1.2.1/coupler_panel_release/USAGE.md
Normal file
256
alg/delivery_v1.2.1/coupler_panel_release/USAGE.md
Normal file
@@ -0,0 +1,256 @@
|
||||
# coupler_panel — 集成开发者使用指南
|
||||
|
||||
> 高铁车钩面板偏转角测量算法
|
||||
>
|
||||
> **主输入：ROI JSON + 点云 PLY + CAD STP（三件套）**
|
||||
> **主输出：含两个方向偏转角的 Excel 报表**
|
||||
|
||||
---
|
||||
|
||||
## 1. 一句话集成
|
||||
|
||||
```python
|
||||
from coupler_panel.pipeline import run_coupler_measurement_pipeline
|
||||
from coupler_panel.roi_select import load_roi_json
|
||||
|
||||
result = run_coupler_measurement_pipeline(
|
||||
input_cloud = "train_2_1T.ply", # 待测点云（可为整车，内部按 ROI 流式裁剪）
|
||||
out_dir = "runs/today", # 中间产物 + report.json 落盘
|
||||
profile = "train2_coupler", # 该零件 profile
|
||||
roi_spec = load_roi_json("roi.json"), # ROI = 裁剪超参数 ./examples/roi_train2_coupler.json
|
||||
)
|
||||
print(result.theta_right_deg, result.theta_ref_deg) # 两个偏转角
|
||||
print(result.dx_y_max_mm, result.dx_z_max_mm) # 最大 x 偏差
|
||||
```
|
||||
|
||||
要直接产出 Excel，用集成入口脚本（§3）。
|
||||
|
||||
---
|
||||
|
||||
## 2. 安装
|
||||
|
||||
```bash
|
||||
python -m pip install -r requirements.txt # 锁定版本（Python 3.10 验证）
|
||||
pip install -e ".[all]" # core + cloud + cad + viz + report
|
||||
```
|
||||
|
||||
按需子集见 `pyproject.toml` 的 extras：`[cloud]` `[cad]` `[viz]` `[report]`。
|
||||
|
||||
---
|
||||
|
||||
## 3. 集成入口（三文件 → Excel）
|
||||
|
||||
`examples/run_integration.py` 是给集成方的标准入口：
|
||||
|
||||
```bash
|
||||
python examples/run_integration.py \
|
||||
--roi examples/roi_train2_coupler.json \ # ROI JSON
|
||||
--cloud /path/to/train_2_1T.ply \ # 整车点云（可多 GB）
|
||||
--stp /path/to/asm.stp \ # CAD STP（审计）
|
||||
--out runs/train2 \ # 输出目录
|
||||
--profile train2_coupler
|
||||
```
|
||||
|
||||
产出 `runs/train2/eval_report.xlsx`（30 列，见 §7）+ 全部中间产物。
|
||||
|
||||
可选参数：`--gt-theta-right` / `--gt-theta-ref`（已知理论偏转，用于误差列，默认 0）、`--sample-id`、`--config`。
|
||||
|
||||
---
|
||||
|
||||
## 4. 三件输入说明
|
||||
|
||||
### 4.1 ROI JSON（裁剪超参数）
|
||||
|
||||
6-key 轴对齐包围盒，**坐标系必须与点云一致**，单位 mm：
|
||||
|
||||
```json
|
||||
{
|
||||
"x_min": 11526.0, "x_max": 11733.0,
|
||||
"y_min": 588.0, "y_max": 1192.0,
|
||||
"z_min": -950.0, "z_max": 950.0
|
||||
}
|
||||
```
|
||||
|
||||
- 点 `(x,y,z)` 保留条件：`x_min<=x<=x_max ∧ y_min<=y<=y_max ∧ z_min<=z<=z_max`
|
||||
- 必须 `min<max`，否则报 `ValueError`；多余 key（如 `comment`）忽略
|
||||
- **算法直接拿这六个数裁剪**。当输入是二进制 PLY 且 ROI 是有限 bbox 时，走**流式裁剪**（memmap 分块，不整体加载），可处理 GB 级整车点云
|
||||
- 想全保留：传 `roi_json=None` 或 `roi_spec=default_roi_spec()`（±∞）
|
||||
- 裁剪诊断写到 `out_dir/roi.json`（`mode`、`n_in`、`n_out`、`bbox`）
|
||||
|
||||
> **坐标系坑（必读）**：ROI 的六个边界写在**点云自己的坐标系**里。若点云是 x=车长 / y=车高 / z=车宽（部分整车数据集如此），ROI 也按此填。ROI 裁出 0 点时算法抛 `InsufficientPointsError` 并提示检查坐标系。
|
||||
|
||||
### 4.2 点云 PLY
|
||||
|
||||
- 支持 `.ply`（二进制/ASCII）/ `.pcd` / `.xyz` / `.txt` / `.csv`
|
||||
- 单位 **mm**；算法只读 x/y/z 前三列（颜色/法向等额外列容忍）
|
||||
- **可为整车扫描**（多 GB）——ROI 流式裁剪后才进入算法
|
||||
- **算法不做配准**：假设上游扫描端外参已标定、点云已在 CAD/车体帧（原因见 §8）
|
||||
|
||||
### 4.3 CAD STP
|
||||
|
||||
当前 STP 的角色取决于规模：
|
||||
|
||||
- **单零件 STP**：可被 `parse_cad` 解析，用于提供面板法向 seed + 自动整定检测参数（见 §6）。传 `cad_stp_path=...`。
|
||||
- **整车装配 STP**（数百 MB、数千实例）：项目的 `parse_cad`（单零件设计）无法处理，此时 STP 仅作**审计/可追溯**输入（路径记入报表），不参与计算——检测参数由 profile 提供、法向用 PCA。
|
||||
|
||||
> 若将来需要从整车 STP 提取目标几何，需另写装配级 XCAF 解析（不能复用单零件 `parse_cad`）。
|
||||
|
||||
---
|
||||
|
||||
## 5. 主入口 API
|
||||
|
||||
```python
|
||||
def run_coupler_measurement_pipeline(
|
||||
input_cloud : str | Path, # 必填：点云路径（可整车）
|
||||
out_dir : str | Path, # 必填：输出目录
|
||||
*,
|
||||
config_path : str | Path | None = None, # 可选：自定义 YAML / dict
|
||||
profile : str = "auto", # 可选：preset profile
|
||||
cad_stp_path: str | Path | None = None, # 可选：单零件 STP（法向 + autotune）
|
||||
roi_spec : ROISpec | None = None, # 二选一：内存 ROI 对象
|
||||
roi_json : str | Path | None = None, # 二选一：ROI JSON 路径
|
||||
progress : Callable[[str, float], None] | None = None,
|
||||
) -> CouplerMeasurement
|
||||
```
|
||||
|
||||
**算法链路**：
|
||||
|
||||
```
|
||||
crop_ply_bbox(流式 ROI 裁剪)
|
||||
→ [layer_select] 沿法向隔离前层（profile 开关，去多层重叠）
|
||||
→ [autotune] 从 CAD 孔径 + 点距自整定（仅默认 profile + 单零件 CAD 时）
|
||||
→ denoise_cloud
|
||||
→ compute_coupler_measurement
|
||||
validate → detect 4 holes → build frame
|
||||
→ extract ROI → fit plane(RANSAC+SVD) → metrics
|
||||
```
|
||||
|
||||
**profile 选择**：
|
||||
- 有匹配 profile（如 `train2_coupler` / `lu1_af4`）→ 直接指定，最稳。
|
||||
- 无 profile 但传了单零件 `cad_stp_path` → `profile="auto"`，走自动整定（§6）。
|
||||
|
||||
**配置覆盖**：`config_path` 可传 dict 覆盖任意 `configs/default.yaml` 字段。
|
||||
|
||||
### 5.1 返回值 `CouplerMeasurement`
|
||||
|
||||
```python
|
||||
theta_right_deg, theta_ref_deg # 两个偏转角（度）
|
||||
D_y_max_mm, D_z_max_mm # 两方向最大距中心偏差
|
||||
dx_y_max_mm, dx_z_max_mm # 最大 x 向偏差
|
||||
center_C, normal # 面板中心、拟合法向（n_x>0）
|
||||
holes, plane, frame # 4 孔 / 平面 / 参考系子结构
|
||||
quality_flags, timings, report # 诊断；report == out_dir/report.json
|
||||
result.to_dict() # 完全 JSON 化
|
||||
```
|
||||
|
||||
### 5.2 异常
|
||||
|
||||
```
|
||||
CouplerError
|
||||
├── InvalidPointCloudError / UnsupportedFormatError 输入非法 / 格式不支持
|
||||
├── InsufficientPointsError 点数过少 / ROI 裁出 0 点
|
||||
├── CADParseError STP 解析失败
|
||||
├── HoleDetectionError 4 孔检测失败
|
||||
└── PlaneFitError 平面拟合失败
|
||||
```
|
||||
|
||||
非致命问题在 `result.quality_flags`（如 `plane_inlier_ratio_low`）。
|
||||
|
||||
---
|
||||
|
||||
## 6. 检测参数：profile vs 自动整定
|
||||
|
||||
不同零件的定位孔半径、是否有中心孔、点云密度都不同，检测参数必须匹配，否则报 `HoleDetectionError: only N hole-shaped cavities found`。两种适配方式：
|
||||
|
||||
1. **per-part profile（推荐用于已知零件）**：`configs/profiles/<name>.yaml` 写死该零件的孔径带、面积窗、是否前层隔离等。已有 `train2_coupler`（整车车钩面板）、`lu1_af4`（单零件车钩面板）。
|
||||
2. **autotune（单零件 + 默认 profile 时）**：传单零件 `cad_stp_path` 且 `profile=auto`、无 `config`，算法从 CAD 定位孔半径 + 去噪后点距自动推导检测参数（`coupler_panel/detection/autotune.py`）。指定具体 profile 或 config 时**完全跳过**。
|
||||
|
||||
> 整车场景用 profile（`train2_coupler`），因为整车 STP 不能 parse、autotune 不可用。
|
||||
|
||||
---
|
||||
|
||||
## 7. 输出
|
||||
|
||||
### 7.1 Excel 报表（`out_dir/eval_report.xlsx`，30 列）
|
||||
|
||||
样本标识、输入 PLY、ROI kind、**θ_right / θ_ref 的 GT/预测/误差**、D_y/D_z_max、dx_y/dx_z_max、中心 C(x/y/z)、法向 n(x/y/z)、平面内点率/残差、配准 RMSE/最大残差（主路径恒空）、ROI 前后点数、quality_flags、**算法运行时间 `Algorithm runtime (s)`**（输入→返回整段 wall-clock）、内核阶段累计、算法/schema 版本。
|
||||
|
||||
多组结果用 `coupler_panel.reporting.write_excel_report(rows, path)` 合并成一张表。
|
||||
|
||||
### 7.2 落盘中间产物（`out_dir/`）
|
||||
|
||||
| 文件 | 内容 |
|
||||
|---|---|
|
||||
| `config.resolved.yaml` | 实际生效的合并配置 |
|
||||
| `roi.json` | ROI spec + 裁剪诊断 |
|
||||
| `01_gt.json` | （单零件 CAD parse 时）审计 GT |
|
||||
| `02_sampled.ply` | ROI 裁剪后的点云 |
|
||||
| `transformed.ply` | （前层隔离开启时）隔离后的层 |
|
||||
| `05_denoised.ply` | 去噪后点云（喂给测量算法的最终点云） |
|
||||
| `report.json` | 完整结果，等于 `result.to_dict()` |
|
||||
|
||||
---
|
||||
|
||||
## 8. 为什么不做配准
|
||||
|
||||
车钩面板的 4 个定位孔与面板一起刚体旋转。用「扫到的 4 孔 → CAD 原始 4 孔」做 Kabsch，会精确解出「扫描器位姿 · 面板偏转」的复合刚体变换并反向作用，把偏转随位姿误差**一起消掉**，下游再读不到偏转——这是几何性质，不是 bug。
|
||||
|
||||
故主算法假设上游扫描端外参标定、点云已在车体帧。研究路径 `run_from_cad_pipeline`（含 Kabsch + 注入 oracle）完整保留作上限对照。
|
||||
|
||||
---
|
||||
|
||||
## 9. 集成模板
|
||||
|
||||
```python
|
||||
import logging, time, json
|
||||
from pathlib import Path
|
||||
from coupler_panel.pipeline import run_coupler_measurement_pipeline
|
||||
from coupler_panel.roi_select import load_roi_json
|
||||
from coupler_panel.reporting import build_row_from_summary, write_excel_report
|
||||
from coupler_panel.exceptions import CouplerError
|
||||
|
||||
log = logging.getLogger(__name__)
|
||||
|
||||
def measure(cloud, roi_json, stp, out_dir, profile="train2_coupler"):
|
||||
t0 = time.perf_counter()
|
||||
try:
|
||||
r = run_coupler_measurement_pipeline(
|
||||
input_cloud=cloud, out_dir=out_dir,
|
||||
profile=profile, roi_spec=load_roi_json(roi_json))
|
||||
except CouplerError as e:
|
||||
log.error("measurement failed: %s", e)
|
||||
return None
|
||||
dt = time.perf_counter() - t0
|
||||
|
||||
diag = json.loads((Path(out_dir)/"roi.json").read_text(encoding="utf-8"))["diagnostics"]
|
||||
row = build_row_from_summary(
|
||||
sample_id=Path(cloud).stem, ply_path=str(cloud), roi_kind="aabb",
|
||||
measured=r.to_dict(), gt_angles=(0.0, 0.0), registration=None,
|
||||
n_points_in=diag.get("n_in", 0), n_points_after_roi=diag.get("n_out", 0),
|
||||
pipeline_seconds=dt)
|
||||
write_excel_report([row], Path(out_dir)/"eval_report.xlsx")
|
||||
return r
|
||||
```
|
||||
|
||||
---
|
||||
|
||||
## 10. 坐标 & 单位速查
|
||||
|
||||
| 量 | 单位 | 说明 |
|
||||
|---|---|---|
|
||||
| 点云坐标 | mm | 轴含义由数据集决定（见 README / §4.1） |
|
||||
| 偏转角 | 度 | `θ_right=atan2(n_y,n_x)`、`θ_ref=atan2(n_z,n_x)` |
|
||||
| 法向 | 单位向量 | 强制 `n_x>0` |
|
||||
| `right_axis_sign` | ±1 | 右方向符号（profile `metrics` 配置） |
|
||||
|
||||
---
|
||||
|
||||
## 11. 版本兼容
|
||||
|
||||
| 标识 | 含义 |
|
||||
|---|---|
|
||||
| `coupler_panel.__version__` | 包版本（语义化） |
|
||||
| `coupler_panel.ALGORITHM_VERSION` | 算法版本，升级时**数值可能变化** |
|
||||
| `coupler_panel.SCHEMA_VERSION` | 结果结构版本，仅破坏性变更递增 |
|
||||
|
||||
建议：落库时记录 `algorithm_version`；反序列化 `report.json` 时校验 `schema_version` 主版本。
|
||||
145
alg/delivery_v1.2.1/coupler_panel_release/configs/default.yaml
Normal file
145
alg/delivery_v1.2.1/coupler_panel_release/configs/default.yaml
Normal file
@@ -0,0 +1,145 @@
|
||||
# Default configuration for coupler panel measurement pipeline.
|
||||
# All numeric units in mm / degree unless otherwise stated.
|
||||
# Override fields by passing a dict or a YAML path with the same nested shape.
|
||||
|
||||
schema_version: "1.0"
|
||||
|
||||
# --- CAD parsing & GT extraction --------------------------------------------
|
||||
cad:
|
||||
# OpenCASCADE tessellation quality.
|
||||
linear_deflection_mm: 0.5
|
||||
angular_deflection_rad: 0.1
|
||||
# Hole topology classification.
|
||||
# The four locating holes are expected to be small and equal-radius;
|
||||
# the central pass-through hole is expected to be much larger.
|
||||
locating_hole_radius_mm: [4.0, 12.0] # [min, max] of expected locating hole radius
|
||||
central_hole_radius_min_mm: 20.0 # any circle larger than this is treated as the centre hole
|
||||
rectangle_aspect_tol: 0.15 # tolerance for "the 4 hole centres form a rectangle"
|
||||
|
||||
# --- Sampling ---------------------------------------------------------------
|
||||
sampling:
|
||||
method: poisson_disk # uniform_area | poisson_disk | adaptive | grid_spacing
|
||||
# Use either spacing_mm OR num_points; if both set, spacing_mm wins.
|
||||
spacing_mm: 1.0
|
||||
num_points: 100000
|
||||
seed: 42
|
||||
adaptive:
|
||||
curvature_weight: 1.0
|
||||
min_spacing_factor: 0.25 # smallest spacing = spacing_mm * factor
|
||||
max_spacing_factor: 4.0
|
||||
|
||||
# --- Simulated scan (optional) ----------------------------------------------
|
||||
scan_simulator:
|
||||
enabled: false
|
||||
view_origin: [10000.0, 0.0, 0.0] # camera position in CAD frame, mm
|
||||
hpr_radius_mm: 50000.0 # hidden-point-removal radius
|
||||
|
||||
# --- Panel deflection injection (the SIGNAL the algorithm should measure) --
|
||||
# This rotates the panel (and its 4 locating holes) by a known small angle.
|
||||
# After running the pipeline, theta_right_deg / theta_ref_deg in the result
|
||||
# should match these values within the algorithm's recovery accuracy.
|
||||
panel_deflection:
|
||||
enabled: false
|
||||
theta_right_deg: 0.0 # atan2(n_y, n_x) target (rotation around z)
|
||||
theta_ref_deg: 0.0 # atan2(n_z, n_x) target (rotation around -y)
|
||||
|
||||
# --- Synthetic noise --------------------------------------------------------
|
||||
# Realistic LIDAR / structured-light noise on a ~2 m × 2 m panel is ≈ 50-80
|
||||
# µm = 0.05-0.08 mm. Larger sigma is mostly to test robustness, not to
|
||||
# match a real sensor.
|
||||
noise:
|
||||
enabled: true
|
||||
sigma_mm: 0.05
|
||||
mode: isotropic # isotropic | along_normal
|
||||
seed: 43
|
||||
|
||||
# --- Synthetic scan-pose perturbation ---------------------------------------
|
||||
# Mounting / scanner-stage jitter. 4-hole Kabsch should cancel this; if it
|
||||
# leaks into the final angles that's a registration error, not a measurement
|
||||
# of panel deflection. Set rotation_deg=0, translation_mm=0 for "ideal pose"
|
||||
# experiments that isolate the deflection-recovery accuracy.
|
||||
transform:
|
||||
enabled: true
|
||||
rotation_deg: 0.5 # magnitude; axis is randomised
|
||||
rotation_axis: random # random | x | y | z | [ax, ay, az]
|
||||
translation_mm: 2.0 # magnitude; direction is randomised
|
||||
translation_dir: random
|
||||
seed: 44
|
||||
|
||||
# --- Denoise pipeline -------------------------------------------------------
|
||||
# Steps are executed in the listed order. Set steps to [] to disable.
|
||||
denoise:
|
||||
steps:
|
||||
- type: passthrough
|
||||
bbox_padding_mm: 50.0
|
||||
- type: voxel
|
||||
voxel_mm: 0.5
|
||||
- type: statistical
|
||||
nb_neighbors: 30
|
||||
std_ratio: 2.0
|
||||
- type: radius
|
||||
nb_points: 8
|
||||
radius_mm: 2.0
|
||||
# Optional MLS smoothing (requires open3d ≥ 0.18 with ml extras).
|
||||
smoothing:
|
||||
enabled: false
|
||||
search_radius_mm: 2.0
|
||||
polynomial_order: 2
|
||||
|
||||
# --- Registration -----------------------------------------------------------
|
||||
registration:
|
||||
method: kabsch_four_holes # only this method is implemented; documented & enforced
|
||||
# The four GT hole centres come from CAD; the four scan hole centres come
|
||||
# from the detection module. Pairing is by greedy nearest-neighbour after
|
||||
# a coarse PCA-based pre-alignment of the centroid.
|
||||
pre_align: pca_centroid
|
||||
refuse_if_residual_above_mm: 5.0 # quality flag if RMS pairing residual exceeds this
|
||||
|
||||
# --- 4-hole detection in point cloud ----------------------------------------
|
||||
detection:
|
||||
# Local panel-plane projection.
|
||||
panel_normal_seed: from_cad # from_cad | pca | given
|
||||
panel_normal_pca_kth: 1 # use k-th smallest PCA eigenvector as panel normal
|
||||
# 2D occupancy grid.
|
||||
grid_pixel_mm: 0.5
|
||||
panel_min_area_ratio: 0.4 # largest connected component must be ≥ this fraction of bbox
|
||||
hole_min_area_mm2: 30.0
|
||||
hole_max_area_mm2: 800.0
|
||||
# Circle fitting on hole boundary.
|
||||
circle_fit:
|
||||
method: algebraic_then_lsq # algebraic_then_lsq | hyper | taubin
|
||||
inlier_residual_mm: 0.5
|
||||
# Selection.
|
||||
select_top_k_by_radius_match: 4
|
||||
rectangle_aspect_tol: 0.20
|
||||
|
||||
# --- Panel ROI extraction ---------------------------------------------------
|
||||
roi:
|
||||
hole_dilate_mm: 3.0 # exclude points within (hole_radius + dilate)
|
||||
central_hole_dilate_mm: 5.0
|
||||
edge_buffer_mm: 5.0 # shrink ROI rectangle inward by this
|
||||
chamfer_normal_dot_min: 0.85 # exclude points whose normal deviates more than acos(0.85) from panel normal
|
||||
outlier_z_score: 3.0
|
||||
|
||||
# --- Plane fit --------------------------------------------------------------
|
||||
plane_fit:
|
||||
method: ransac_svd
|
||||
ransac_iterations: 200
|
||||
ransac_inlier_thresh_mm: auto # auto = 3*sigma_mm
|
||||
min_inlier_ratio: 0.6
|
||||
enforce_nx_positive: true
|
||||
|
||||
# --- Metrics ----------------------------------------------------------------
|
||||
metrics:
|
||||
# If your right axis is +y, leave this 1. If right is -y, set to -1.
|
||||
right_axis_sign: 1
|
||||
# Angles smaller than this are clamped to 0 (purely cosmetic; raw value is
|
||||
# always in the report).
|
||||
angle_clamp_deg: 0.0
|
||||
|
||||
# --- Output / artifacts -----------------------------------------------------
|
||||
artifacts:
|
||||
save_intermediate: true
|
||||
ply_format: binary # binary | ascii
|
||||
image_dpi: 150
|
||||
heatmap_colormap: viridis
|
||||
@@ -0,0 +1,17 @@
|
||||
# Profile: high-density scan (≈100w points, sub-mm spacing).
|
||||
sampling:
|
||||
spacing_mm: 0.25
|
||||
num_points: 1000000
|
||||
denoise:
|
||||
steps:
|
||||
- type: passthrough
|
||||
bbox_padding_mm: 50.0
|
||||
- type: voxel
|
||||
voxel_mm: 0.25
|
||||
- type: statistical
|
||||
nb_neighbors: 50
|
||||
std_ratio: 2.0
|
||||
detection:
|
||||
grid_pixel_mm: 0.25
|
||||
plane_fit:
|
||||
ransac_iterations: 500
|
||||
@@ -0,0 +1,18 @@
|
||||
# Profile: low-density scan (≈1w points, ≈2 mm spacing).
|
||||
sampling:
|
||||
spacing_mm: 2.0
|
||||
num_points: 10000
|
||||
denoise:
|
||||
steps:
|
||||
- type: passthrough
|
||||
bbox_padding_mm: 50.0
|
||||
- type: voxel
|
||||
voxel_mm: 1.0
|
||||
- type: statistical
|
||||
nb_neighbors: 15
|
||||
std_ratio: 2.5
|
||||
detection:
|
||||
grid_pixel_mm: 1.0
|
||||
hole_min_area_mm2: 20.0
|
||||
plane_fit:
|
||||
ransac_iterations: 100
|
||||
@@ -0,0 +1,106 @@
|
||||
# Profile tuned for the lu1-M000004835653_AF4_ASMP072708187_N_AF4.stp panel.
|
||||
#
|
||||
# Topology summary (from cad/iter_circular_edges):
|
||||
# - locating holes: r = 16 mm × 4 corners at (y=±100, z=±180)
|
||||
# - central hole: r = 100 mm at (y≈1, z=0)
|
||||
# - panel size: ~200 × 360 mm in y-z, ~76 mm thick along x
|
||||
#
|
||||
# Use:
|
||||
# coupler-measure --stp <path> --out-dir <dir> --profile lu1_af4
|
||||
|
||||
cad:
|
||||
linear_deflection_mm: 0.5
|
||||
angular_deflection_rad: 0.1
|
||||
# CAD-side: strict band that selects only the r=16 mm corner holes.
|
||||
locating_hole_radius_mm: [14.0, 18.0]
|
||||
central_hole_radius_min_mm: 80.0
|
||||
rectangle_aspect_tol: 0.30
|
||||
|
||||
sampling:
|
||||
method: poisson_disk
|
||||
spacing_mm: null
|
||||
num_points: 500000
|
||||
seed: 42
|
||||
|
||||
# Critical: simulate a single-view "+x looking at -x" scan. Without this,
|
||||
# back-face points project into the hole regions and detection fails.
|
||||
scan_simulator:
|
||||
enabled: true
|
||||
view_origin: [10000.0, 0.0, 0.0]
|
||||
hpr_radius_mm: 200000.0
|
||||
|
||||
# --- Panel deflection injection -------------------------------------------
|
||||
# This is the *signal* we want the algorithm to recover. By default it is
|
||||
# off — sweeps override these.
|
||||
panel_deflection:
|
||||
enabled: true
|
||||
theta_right_deg: 0.5 # rotation around z so n_y > 0
|
||||
theta_ref_deg: 0.3 # rotation around -y so n_z > 0
|
||||
|
||||
# --- Realistic LiDAR / structured-light noise -----------------------------
|
||||
noise:
|
||||
enabled: true
|
||||
sigma_mm: 0.05 # ≈ 50 µm; matches typical industrial scanners
|
||||
mode: isotropic
|
||||
seed: 43
|
||||
|
||||
# --- Scanner-pose jitter --------------------------------------------------
|
||||
# Modest values: 4-hole Kabsch should remove this completely.
|
||||
transform:
|
||||
enabled: true
|
||||
rotation_deg: 0.5
|
||||
translation_mm: 2.0
|
||||
rotation_axis: random
|
||||
translation_dir: random
|
||||
seed: 44
|
||||
|
||||
denoise:
|
||||
steps:
|
||||
- type: passthrough
|
||||
bbox_padding_mm: 50.0
|
||||
- type: voxel
|
||||
voxel_mm: 0.5 # finer voxel for low-noise data
|
||||
- type: statistical
|
||||
nb_neighbors: 20
|
||||
std_ratio: 2.5
|
||||
|
||||
detection:
|
||||
panel_normal_seed: pca # critical: use cloud's own PCA, not CAD seed
|
||||
grid_pixel_mm: 2.0
|
||||
closing_iters: 0
|
||||
fill_bg_below_mm2: 100.0
|
||||
panel_min_area_ratio: 0.15
|
||||
hole_min_area_mm2: 600.0
|
||||
hole_max_area_mm2: 5000.0
|
||||
locating_hole_radius_mm: [14.0, 22.0]
|
||||
circle_fit:
|
||||
method: algebraic_then_lsq
|
||||
inlier_residual_mm: 1.5
|
||||
rectangle_aspect_tol: 0.40
|
||||
|
||||
roi:
|
||||
hole_dilate_mm: 5.0
|
||||
central_hole_dilate_mm: 10.0
|
||||
edge_buffer_mm: 8.0
|
||||
chamfer_normal_dot_min: 0.80
|
||||
outlier_z_score: 3.0
|
||||
|
||||
plane_fit:
|
||||
ransac_iterations: 300
|
||||
ransac_inlier_thresh_mm: auto
|
||||
min_inlier_ratio: 0.5
|
||||
enforce_nx_positive: true
|
||||
|
||||
metrics:
|
||||
right_axis_sign: 1
|
||||
angle_clamp_deg: 0.0
|
||||
|
||||
artifacts:
|
||||
save_intermediate: true
|
||||
ply_format: binary
|
||||
image_dpi: 150
|
||||
heatmap_colormap: viridis
|
||||
|
||||
registration:
|
||||
method: kabsch_four_holes
|
||||
refuse_if_residual_above_mm: 5.0
|
||||
@@ -0,0 +1,94 @@
|
||||
# Profile for the train_2 coupler mounting panel measured from a full-vehicle
|
||||
# scan (train_2_1T.ply, cropped to the coupler ROI).
|
||||
#
|
||||
# Part topology (measured from the 1T cloud, dataset frame x=length,
|
||||
# y=height, z=width):
|
||||
# - a wide base plate with a RAISED central square mounting plate
|
||||
# - the central plate has 4 corner bolt holes, r≈15.6 mm,
|
||||
# rectangle 360 mm (z) × 200 mm (y), at z=±180, y=835/1035
|
||||
# - NO central pass-through hole
|
||||
# - the ROI box also slices a roof truss + the base plate behind the
|
||||
# raised plate, so a normal-direction layer isolation is required
|
||||
#
|
||||
# Use:
|
||||
# run_coupler_measurement_pipeline(input_cloud=roi.ply,
|
||||
# profile="train2_coupler", roi_spec=<bbox>) # no CAD, no Kabsch
|
||||
|
||||
# --- CAD block (not used for STP parse here; kept for radius thresholds) ---
|
||||
cad:
|
||||
linear_deflection_mm: 0.5
|
||||
angular_deflection_rad: 0.1
|
||||
locating_hole_radius_mm: [11.0, 19.0] # r≈15.6 corner bolt holes
|
||||
central_hole_radius_min_mm: 999.0 # this part has NO central hole
|
||||
rectangle_aspect_tol: 0.40
|
||||
|
||||
# --- Normal-direction layer isolation (the key pre-processing step) -------
|
||||
# Isolate the frontmost dominant layer = the raised central plate, so the
|
||||
# back plate + truss don't fill the holes / pollute the plane fit.
|
||||
layer_select:
|
||||
enabled: true
|
||||
normal_seed: pca # pca | from_cad
|
||||
half_width_mm: 8.0 # slab half-thickness around the chosen layer
|
||||
pick: frontmost # frontmost (toward scanner) | densest
|
||||
bins: 240
|
||||
|
||||
# --- Denoise: drop the density-fragile radius filter (sparse-ish scan) ----
|
||||
denoise:
|
||||
steps:
|
||||
- type: passthrough
|
||||
bbox_padding_mm: 50.0
|
||||
- type: voxel
|
||||
voxel_mm: 0.5
|
||||
- type: statistical
|
||||
nb_neighbors: 20
|
||||
std_ratio: 2.5
|
||||
|
||||
# --- 4-hole detection tuned for r≈15.6 mm bolt holes ----------------------
|
||||
detection:
|
||||
panel_normal_seed: pca
|
||||
grid_pixel_mm: 2.0
|
||||
closing_iters: 1
|
||||
fill_bg_below_mm2: 60.0
|
||||
panel_min_area_ratio: 0.15
|
||||
hole_min_area_mm2: 300.0 # r=15.6 -> ~765 mm²; floor well below
|
||||
hole_max_area_mm2: 1400.0 # ceil above; excludes larger back-layer holes
|
||||
locating_hole_radius_mm: [11.0, 19.0]
|
||||
rectangle_aspect_tol: 0.40
|
||||
circle_fit:
|
||||
method: algebraic_then_lsq
|
||||
inlier_residual_mm: 1.5
|
||||
|
||||
# --- Panel ROI extraction -------------------------------------------------
|
||||
roi:
|
||||
hole_dilate_mm: 5.0
|
||||
central_hole_dilate_mm: 10.0
|
||||
edge_buffer_mm: 8.0
|
||||
chamfer_normal_dot_min: 0.80
|
||||
outlier_z_score: 3.0
|
||||
|
||||
# --- Plane fit ------------------------------------------------------------
|
||||
plane_fit:
|
||||
ransac_iterations: 400
|
||||
ransac_inlier_thresh_mm: 1.0
|
||||
min_inlier_ratio: 0.3
|
||||
enforce_nx_positive: true
|
||||
|
||||
# --- Metrics --------------------------------------------------------------
|
||||
# NOTE: this dataset's frame is x=length, y=height, z=width — i.e. y and z
|
||||
# are swapped relative to the package's nominal (y=width, z=height). The
|
||||
# angle formulas are symmetric in form; here theta_right = atan2(n_y,n_x)
|
||||
# is the tilt toward car-height and theta_ref = atan2(n_z,n_x) is the tilt
|
||||
# toward car-width. Both are ~0 for an undeflected panel.
|
||||
metrics:
|
||||
right_axis_sign: 1
|
||||
angle_clamp_deg: 0.0
|
||||
|
||||
registration:
|
||||
method: kabsch_four_holes
|
||||
refuse_if_residual_above_mm: 5.0
|
||||
|
||||
artifacts:
|
||||
save_intermediate: true
|
||||
ply_format: binary
|
||||
image_dpi: 150
|
||||
heatmap_colormap: viridis
|
||||
@@ -0,0 +1,116 @@
|
||||
"""Coupler-panel CAD-STP to point-cloud measurement validation system.
|
||||
|
||||
The package is split into two distinct layers:
|
||||
|
||||
**1. Public measurement algorithm (integration API).**
|
||||
The single function software developers should call from their
|
||||
product code. Takes a clean point cloud, returns a
|
||||
:class:`CouplerMeasurement`. Stable schema across patch releases —
|
||||
see ``docs/USAGE.md``.
|
||||
|
||||
Entry points:
|
||||
|
||||
- :func:`compute_coupler_measurement` — pure, in-memory.
|
||||
- :class:`CouplerMeasurement`, :class:`HoleSet`, :class:`PanelPlane`,
|
||||
:class:`ReferenceFrame`, :class:`Hole`.
|
||||
- :mod:`coupler_panel.exceptions`.
|
||||
- :func:`load_point_cloud`, :func:`write_point_cloud` for IO.
|
||||
|
||||
**2. Validation / data-preparation harness (research only).**
|
||||
Used to generate synthetic point clouds from CAD, denoise them, and
|
||||
register them into the CAD frame. Not part of the integration
|
||||
surface; layout and signatures may change.
|
||||
|
||||
Entry points:
|
||||
|
||||
- :mod:`coupler_panel.data_prep` — STP → mesh → sample → noise →
|
||||
transform → denoise → register.
|
||||
- :mod:`coupler_panel.pipeline` — convenience wrappers that save
|
||||
every artifact to disk.
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
# --- Stable, integration-safe public surface --------------------------------
|
||||
from ._version import ALGORITHM_VERSION, SCHEMA_VERSION, __version__
|
||||
from .exceptions import (
|
||||
CADParseError,
|
||||
CouplerError,
|
||||
HoleDetectionError,
|
||||
InsufficientPointsError,
|
||||
InvalidPointCloudError,
|
||||
PlaneFitError,
|
||||
RegistrationError,
|
||||
UnsupportedFormatError,
|
||||
)
|
||||
from .io_utils import load_point_cloud, write_point_cloud
|
||||
from .structures import (
|
||||
CouplerMeasurement,
|
||||
Hole,
|
||||
HoleSet,
|
||||
PanelPlane,
|
||||
ReferenceFrame,
|
||||
StageTiming,
|
||||
)
|
||||
|
||||
|
||||
def __getattr__(name: str):
|
||||
"""Lazy export of heavy / optional symbols.
|
||||
|
||||
Avoids importing matplotlib / open3d / OCP at ``import coupler_panel``
|
||||
time so a thin integration deployment keeps a fast, dependency-light
|
||||
startup.
|
||||
"""
|
||||
if name in ("compute_coupler_measurement", "ProgressCallback"):
|
||||
from .api import compute_coupler_measurement, ProgressCallback
|
||||
|
||||
return {"compute_coupler_measurement": compute_coupler_measurement, "ProgressCallback": ProgressCallback}[name]
|
||||
if name == "run_coupler_measurement_pipeline":
|
||||
from .pipeline import run_coupler_measurement_pipeline
|
||||
|
||||
return run_coupler_measurement_pipeline
|
||||
if name == "run_from_cad_pipeline":
|
||||
from .pipeline import run_from_cad_pipeline
|
||||
|
||||
return run_from_cad_pipeline
|
||||
if name == "prepare_cloud_from_cad":
|
||||
from .data_prep import prepare_cloud_from_cad
|
||||
|
||||
return prepare_cloud_from_cad
|
||||
if name == "prepare_cloud_from_scan":
|
||||
from .data_prep import prepare_cloud_from_scan
|
||||
|
||||
return prepare_cloud_from_scan
|
||||
raise AttributeError(name)
|
||||
|
||||
|
||||
__all__ = [
|
||||
"__version__",
|
||||
"ALGORITHM_VERSION",
|
||||
"SCHEMA_VERSION",
|
||||
# Stable measurement API
|
||||
"compute_coupler_measurement",
|
||||
"ProgressCallback",
|
||||
"CouplerMeasurement",
|
||||
"Hole",
|
||||
"HoleSet",
|
||||
"PanelPlane",
|
||||
"ReferenceFrame",
|
||||
"StageTiming",
|
||||
"load_point_cloud",
|
||||
"write_point_cloud",
|
||||
# Exceptions
|
||||
"CouplerError",
|
||||
"InvalidPointCloudError",
|
||||
"UnsupportedFormatError",
|
||||
"InsufficientPointsError",
|
||||
"CADParseError",
|
||||
"HoleDetectionError",
|
||||
"RegistrationError",
|
||||
"PlaneFitError",
|
||||
# Research-only convenience (data prep + persisting orchestration)
|
||||
"run_coupler_measurement_pipeline",
|
||||
"run_from_cad_pipeline",
|
||||
"prepare_cloud_from_cad",
|
||||
"prepare_cloud_from_scan",
|
||||
]
|
||||
@@ -0,0 +1,19 @@
|
||||
"""Version markers for coupler_panel.
|
||||
|
||||
- ``__version__`` — package version (semver).
|
||||
- ``ALGORITHM_VERSION`` — algorithm/numerics version. Numerical results may
|
||||
change when this increments. Increment whenever any computation in
|
||||
``compute_coupler_measurement`` changes.
|
||||
- ``SCHEMA_VERSION`` — result-schema version. Only increments on a
|
||||
*breaking* change to the ``CouplerMeasurement`` dataclass or to
|
||||
``report.json`` field semantics. New optional fields do **not** bump
|
||||
this.
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
__version__ = "0.1.0"
|
||||
ALGORITHM_VERSION = "0.1.0"
|
||||
SCHEMA_VERSION = "1.0"
|
||||
|
||||
__all__ = ["__version__", "ALGORITHM_VERSION", "SCHEMA_VERSION"]
|
||||
251
alg/delivery_v1.2.1/coupler_panel_release/coupler_panel/api.py
Normal file
251
alg/delivery_v1.2.1/coupler_panel_release/coupler_panel/api.py
Normal file
@@ -0,0 +1,251 @@
|
||||
"""Public Python API — **measurement algorithm only**.
|
||||
|
||||
This module is what software developers integrate against. It takes a
|
||||
*clean, already-registered* point cloud (in the CAD / world frame) of
|
||||
the coupler panel and returns a :class:`CouplerMeasurement`.
|
||||
|
||||
Responsibilities of this layer:
|
||||
|
||||
validate → detect 4 holes → build reference frame
|
||||
→ extract panel ROI → fit panel plane → compute metrics
|
||||
|
||||
Things that are **not** in this layer (they live in
|
||||
:mod:`coupler_panel.data_prep`):
|
||||
|
||||
- Reading STEP files / generating synthetic point clouds
|
||||
- Adding noise / random rigid transforms
|
||||
- Denoising (passthrough / SOR / ROR / voxel / smoothing)
|
||||
- 4-hole Kabsch registration into the CAD frame
|
||||
|
||||
The reason: those steps are *engineering* steps that vary per sensor
|
||||
setup; the production caller usually has their own scan + cleanup
|
||||
pipeline and just wants the deflection numbers out the other side.
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import time
|
||||
import tracemalloc
|
||||
from typing import Any, Callable
|
||||
|
||||
import numpy as np
|
||||
|
||||
from ._version import ALGORITHM_VERSION, SCHEMA_VERSION
|
||||
from .config import load_config, resolve_auto
|
||||
from .detection import detect_four_holes
|
||||
from .exceptions import (
|
||||
InsufficientPointsError,
|
||||
InvalidPointCloudError,
|
||||
PlaneFitError,
|
||||
)
|
||||
from .geometry import build_reference_frame, extract_panel_roi, fit_panel_plane
|
||||
from .metrics import compute_deflection_metrics
|
||||
from .structures import (
|
||||
CouplerMeasurement,
|
||||
HoleSet,
|
||||
StageTiming,
|
||||
)
|
||||
|
||||
|
||||
ProgressCallback = Callable[[str, float], None]
|
||||
|
||||
|
||||
_STAGES = (
|
||||
"validate",
|
||||
"detect_holes",
|
||||
"build_frame",
|
||||
"extract_roi",
|
||||
"fit_plane",
|
||||
"metrics",
|
||||
)
|
||||
|
||||
|
||||
def _ping(progress: ProgressCallback | None, stage: str, frac: float) -> None:
|
||||
if progress is not None:
|
||||
try:
|
||||
progress(stage, float(frac))
|
||||
except Exception:
|
||||
pass
|
||||
|
||||
|
||||
def _validate_points(points: Any) -> np.ndarray:
|
||||
try:
|
||||
pts = np.asarray(points, dtype=np.float64)
|
||||
except Exception as exc:
|
||||
raise InvalidPointCloudError(f"could not coerce points to ndarray: {exc}") from exc
|
||||
if pts.ndim != 2 or pts.shape[1] != 3:
|
||||
raise InvalidPointCloudError(f"expected (N, 3) points; got shape {pts.shape}")
|
||||
if pts.shape[0] < 200:
|
||||
raise InsufficientPointsError(f"too few points: {pts.shape[0]} (need ≥ 200)")
|
||||
if not np.isfinite(pts).all():
|
||||
raise InvalidPointCloudError("input contains NaN/Inf")
|
||||
return pts
|
||||
|
||||
|
||||
def compute_coupler_measurement(
|
||||
points: Any,
|
||||
config: str | dict[str, Any] | None = None,
|
||||
profile: str = "auto",
|
||||
*,
|
||||
panel_normal_seed: np.ndarray | None = None,
|
||||
progress: ProgressCallback | None = None,
|
||||
) -> CouplerMeasurement:
|
||||
"""Run the **measurement algorithm** on an already-prepared point cloud.
|
||||
|
||||
Parameters
|
||||
----------
|
||||
points : array-like
|
||||
``(N, 3)`` cloud in the CAD / world frame, units mm.
|
||||
The cloud is expected to be denoised and rigidly aligned to the
|
||||
CAD frame before this call. See :mod:`coupler_panel.data_prep`
|
||||
for a reference implementation of that preparation step.
|
||||
config : path / dict / None
|
||||
Optional override of the default YAML config.
|
||||
profile : str
|
||||
``"auto"`` / ``"default"`` skip profile loading. Other names are
|
||||
resolved against ``configs/profiles/<name>.yaml``.
|
||||
panel_normal_seed : (3,) or None
|
||||
Optional prior on the panel normal (e.g. the GT normal). Speeds
|
||||
up and stabilises the in-plane projection used by hole detection.
|
||||
``None`` falls back to PCA on the cloud.
|
||||
progress : callable or None
|
||||
``progress(stage_name, fraction)`` called once per stage.
|
||||
|
||||
Returns
|
||||
-------
|
||||
:class:`CouplerMeasurement`
|
||||
|
||||
Raises
|
||||
------
|
||||
InvalidPointCloudError, InsufficientPointsError, HoleDetectionError,
|
||||
PlaneFitError, CouplerError
|
||||
"""
|
||||
cfg = load_config(config, profile=profile)
|
||||
cfg = resolve_auto(cfg)
|
||||
|
||||
quality_flags: list[str] = []
|
||||
timings: list[StageTiming] = []
|
||||
|
||||
# ---- validate ----------------------------------------------------------
|
||||
pts = _validate_points(points)
|
||||
_ping(progress, "validate", 1.0 / len(_STAGES))
|
||||
|
||||
# ---- detect 4 holes ----------------------------------------------------
|
||||
t0 = time.perf_counter()
|
||||
tracemalloc.start()
|
||||
detection = detect_four_holes(
|
||||
pts,
|
||||
detection_cfg=cfg["detection"],
|
||||
cad_cfg=cfg["cad"],
|
||||
panel_normal_seed=panel_normal_seed,
|
||||
)
|
||||
_, peak = tracemalloc.get_traced_memory()
|
||||
tracemalloc.stop()
|
||||
timings.append(StageTiming("detect_holes", time.perf_counter() - t0, peak / 1024))
|
||||
_ping(progress, "detect_holes", 2.0 / len(_STAGES))
|
||||
|
||||
detected_holes = detection.holes_3d
|
||||
|
||||
# ---- reference frame ---------------------------------------------------
|
||||
t0 = time.perf_counter()
|
||||
frame = build_reference_frame(detected_holes)
|
||||
timings.append(StageTiming("build_frame", time.perf_counter() - t0, 0.0))
|
||||
_ping(progress, "build_frame", 3.0 / len(_STAGES))
|
||||
|
||||
# ---- extract panel ROI -------------------------------------------------
|
||||
t0 = time.perf_counter()
|
||||
tracemalloc.start()
|
||||
if panel_normal_seed is not None:
|
||||
seed3 = np.asarray(panel_normal_seed, dtype=np.float64)
|
||||
else:
|
||||
seed3 = np.array([1.0, 0.0, 0.0])
|
||||
roi_pts, roi_diag = extract_panel_roi(
|
||||
pts,
|
||||
holes=detected_holes,
|
||||
panel_normal_seed=seed3,
|
||||
roi_cfg=cfg["roi"],
|
||||
)
|
||||
if roi_pts.shape[0] < 100:
|
||||
raise InsufficientPointsError(
|
||||
f"only {roi_pts.shape[0]} points survived ROI extraction"
|
||||
)
|
||||
_, peak = tracemalloc.get_traced_memory()
|
||||
tracemalloc.stop()
|
||||
timings.append(StageTiming("extract_roi", time.perf_counter() - t0, peak / 1024))
|
||||
_ping(progress, "extract_roi", 4.0 / len(_STAGES))
|
||||
|
||||
# ---- fit panel plane ---------------------------------------------------
|
||||
t0 = time.perf_counter()
|
||||
tracemalloc.start()
|
||||
plane = fit_panel_plane(roi_pts, plane_fit_cfg=cfg["plane_fit"])
|
||||
if plane.inlier_ratio < float(cfg["plane_fit"].get("min_inlier_ratio", 0.6)):
|
||||
quality_flags.append("plane_inlier_ratio_low")
|
||||
_, peak = tracemalloc.get_traced_memory()
|
||||
tracemalloc.stop()
|
||||
timings.append(StageTiming("fit_plane", time.perf_counter() - t0, peak / 1024))
|
||||
_ping(progress, "fit_plane", 5.0 / len(_STAGES))
|
||||
|
||||
# ---- metrics -----------------------------------------------------------
|
||||
t0 = time.perf_counter()
|
||||
metrics_cfg = cfg.get("metrics", {})
|
||||
metrics = compute_deflection_metrics(
|
||||
normal=plane.normal,
|
||||
center_C=frame.center_C,
|
||||
panel_points=roi_pts,
|
||||
right_axis_sign=int(metrics_cfg.get("right_axis_sign", 1)),
|
||||
angle_clamp_deg=float(metrics_cfg.get("angle_clamp_deg", 0.0)),
|
||||
)
|
||||
timings.append(StageTiming("metrics", time.perf_counter() - t0, 0.0))
|
||||
_ping(progress, "metrics", 1.0)
|
||||
|
||||
report = {
|
||||
"schema_version": SCHEMA_VERSION,
|
||||
"algorithm_version": ALGORITHM_VERSION,
|
||||
"config": cfg,
|
||||
"n_points_in": int(pts.shape[0]),
|
||||
"detection": detection.diagnostics,
|
||||
"roi": roi_diag,
|
||||
"plane": {
|
||||
"normal": list(plane.normal),
|
||||
"point_on_plane": list(plane.point_on_plane),
|
||||
"d_offset": plane.d_offset,
|
||||
"inlier_ratio": plane.inlier_ratio,
|
||||
"inlier_residual_rms_mm": plane.inlier_residual_rms_mm,
|
||||
},
|
||||
"frame": {
|
||||
"center_C": list(frame.center_C),
|
||||
"reference_plane_z": frame.reference_plane_z,
|
||||
"longitudinal_mid_y": frame.longitudinal_mid_y,
|
||||
},
|
||||
"metrics": metrics.to_dict(),
|
||||
"quality_flags": quality_flags,
|
||||
"timings": [
|
||||
{"stage": t.stage, "seconds": t.seconds, "peak_kib": t.peak_kib}
|
||||
for t in timings
|
||||
],
|
||||
}
|
||||
|
||||
return CouplerMeasurement(
|
||||
schema_version=SCHEMA_VERSION,
|
||||
algorithm_version=ALGORITHM_VERSION,
|
||||
theta_right_deg=metrics.theta_right_deg,
|
||||
theta_ref_deg=metrics.theta_ref_deg,
|
||||
D_y_max_mm=metrics.D_y_max_mm,
|
||||
D_z_max_mm=metrics.D_z_max_mm,
|
||||
dx_y_max_mm=metrics.dx_y_max_mm,
|
||||
dx_z_max_mm=metrics.dx_z_max_mm,
|
||||
center_C=frame.center_C,
|
||||
normal=plane.normal,
|
||||
holes=detected_holes,
|
||||
plane=plane,
|
||||
frame=frame,
|
||||
quality_flags=quality_flags,
|
||||
timings=timings,
|
||||
report=report,
|
||||
)
|
||||
|
||||
|
||||
__all__ = [
|
||||
"compute_coupler_measurement",
|
||||
"ProgressCallback",
|
||||
]
|
||||
@@ -0,0 +1,21 @@
|
||||
"""CAD STP parsing + GT extraction.
|
||||
|
||||
This package depends on ``cadquery-ocp`` (OCP) for OpenCascade access. If
|
||||
OCP is not installed the modules will raise :class:`ImportError` at import
|
||||
time only when actually used; pure point-cloud workflows do not need it.
|
||||
"""
|
||||
|
||||
from .stp_parser import StepShape, load_step
|
||||
from .mesh import tessellate_to_trimesh
|
||||
from .gt_extractor import (
|
||||
GroundTruth,
|
||||
extract_ground_truth,
|
||||
)
|
||||
|
||||
__all__ = [
|
||||
"StepShape",
|
||||
"load_step",
|
||||
"tessellate_to_trimesh",
|
||||
"GroundTruth",
|
||||
"extract_ground_truth",
|
||||
]
|
||||
@@ -0,0 +1,310 @@
|
||||
"""Extract Ground Truth geometry from a parsed STEP shape.
|
||||
|
||||
Strategy
|
||||
--------
|
||||
|
||||
1. Walk every circular edge in the shape (``iter_circular_edges``).
|
||||
2. Cluster by radius:
|
||||
- radii inside ``locating_hole_radius_mm`` → candidate locating holes
|
||||
- radii ≥ ``central_hole_radius_min_mm`` → candidate central hole
|
||||
3. Discard duplicates (front + back face of the same hole share centre and
|
||||
axis up to a sign).
|
||||
4. Find a set of four locating holes whose centres form a near-rectangle:
|
||||
pairwise sort by axis-distance, then verify diagonals roughly equal.
|
||||
5. Order the four locating holes consistently (anti-clockwise when viewed
|
||||
from +x looking down −x). This ordering is the canonical pairing for
|
||||
Kabsch later.
|
||||
6. Define:
|
||||
- ``C`` = mean of the four locating-hole centres (== diagonal
|
||||
intersection for a rectangle)
|
||||
- ``n`` = panel normal = unit axis of the locating circles, signed
|
||||
so that ``n_x > 0``
|
||||
- reference line, plane, longitudinal mid-plane in :class:`ReferenceFrame`
|
||||
|
||||
This GT can be used as
|
||||
- the gold standard the measurement is compared against, AND
|
||||
- a prior law that bootstraps point-cloud-side hole detection (we know the
|
||||
panel normal a priori).
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
from dataclasses import dataclass, field
|
||||
from typing import Any
|
||||
|
||||
import numpy as np
|
||||
|
||||
from ..exceptions import CADParseError
|
||||
from ..structures import Hole, HoleSet, PanelPlane, ReferenceFrame
|
||||
from .stp_parser import StepShape, iter_circular_edges
|
||||
|
||||
|
||||
@dataclass
|
||||
class GroundTruth:
|
||||
"""All CAD-derived reference values used as the GT for validation."""
|
||||
|
||||
holes: HoleSet
|
||||
plane: PanelPlane
|
||||
frame: ReferenceFrame
|
||||
raw_circles: list[dict[str, Any]] = field(default_factory=list)
|
||||
|
||||
def to_dict(self) -> dict[str, Any]:
|
||||
return {
|
||||
"holes": {
|
||||
"locating": [
|
||||
{
|
||||
"center": list(h.center),
|
||||
"radius_mm": h.radius_mm,
|
||||
"fit_residual_rms_mm": h.fit_residual_rms_mm,
|
||||
"role": h.role,
|
||||
}
|
||||
for h in self.holes.locating
|
||||
],
|
||||
"central": (
|
||||
None
|
||||
if self.holes.central is None
|
||||
else {
|
||||
"center": list(self.holes.central.center),
|
||||
"radius_mm": self.holes.central.radius_mm,
|
||||
"fit_residual_rms_mm": self.holes.central.fit_residual_rms_mm,
|
||||
"role": self.holes.central.role,
|
||||
}
|
||||
),
|
||||
},
|
||||
"plane": {
|
||||
"normal": list(self.plane.normal),
|
||||
"point_on_plane": list(self.plane.point_on_plane),
|
||||
"d_offset": self.plane.d_offset,
|
||||
"inlier_ratio": self.plane.inlier_ratio,
|
||||
"inlier_residual_rms_mm": self.plane.inlier_residual_rms_mm,
|
||||
},
|
||||
"frame": {
|
||||
"center_C": list(self.frame.center_C),
|
||||
"line_direction": list(self.frame.line_direction),
|
||||
"reference_plane_z": self.frame.reference_plane_z,
|
||||
"longitudinal_mid_y": self.frame.longitudinal_mid_y,
|
||||
},
|
||||
"raw_circles": self.raw_circles,
|
||||
}
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# Public entry point
|
||||
# ---------------------------------------------------------------------------
|
||||
|
||||
|
||||
def extract_ground_truth(
|
||||
step: StepShape,
|
||||
cad_cfg: dict[str, Any],
|
||||
) -> GroundTruth:
|
||||
"""Extract GT from a parsed STEP file using the ``cad`` config block."""
|
||||
r_lo, r_hi = cad_cfg["locating_hole_radius_mm"]
|
||||
r_central_min = float(cad_cfg["central_hole_radius_min_mm"])
|
||||
rect_aspect_tol = float(cad_cfg.get("rectangle_aspect_tol", 0.15))
|
||||
|
||||
raw: list[dict[str, Any]] = []
|
||||
locating: list[dict[str, Any]] = []
|
||||
central: list[dict[str, Any]] = []
|
||||
|
||||
for center, axis, radius, _edge in iter_circular_edges(step.shape):
|
||||
item = {
|
||||
"center": center,
|
||||
"axis": axis,
|
||||
"radius": radius,
|
||||
}
|
||||
raw.append(item)
|
||||
if r_lo <= radius <= r_hi:
|
||||
locating.append(item)
|
||||
elif radius >= r_central_min:
|
||||
central.append(item)
|
||||
|
||||
if not locating:
|
||||
raise CADParseError(
|
||||
f"no circular edge with radius in [{r_lo}, {r_hi}] mm; "
|
||||
"adjust cad.locating_hole_radius_mm to match your panel"
|
||||
)
|
||||
|
||||
locating = _dedupe_circles(locating, r_tol=0.05, c_tol=0.5)
|
||||
central = _dedupe_circles(central, r_tol=0.5, c_tol=2.0)
|
||||
|
||||
four = _select_rectangle(locating, rect_aspect_tol=rect_aspect_tol)
|
||||
four_ordered = _order_rectangle(four)
|
||||
|
||||
panel_normal = _consensus_axis(four_ordered)
|
||||
panel_normal = _enforce_nx_positive(panel_normal)
|
||||
|
||||
centers = np.asarray([c["center"] for c in four_ordered], dtype=np.float64)
|
||||
C = centers.mean(axis=0)
|
||||
|
||||
holes = HoleSet(
|
||||
locating=tuple( # type: ignore[arg-type]
|
||||
Hole(
|
||||
center=tuple(c["center"]),
|
||||
radius_mm=float(c["radius"]),
|
||||
fit_residual_rms_mm=0.0,
|
||||
role="locating",
|
||||
)
|
||||
for c in four_ordered
|
||||
),
|
||||
central=(
|
||||
Hole(
|
||||
center=tuple(central[0]["center"]),
|
||||
radius_mm=float(central[0]["radius"]),
|
||||
fit_residual_rms_mm=0.0,
|
||||
role="central",
|
||||
)
|
||||
if central
|
||||
else None
|
||||
),
|
||||
)
|
||||
|
||||
plane = PanelPlane(
|
||||
normal=tuple(panel_normal),
|
||||
point_on_plane=tuple(C),
|
||||
d_offset=float(-np.dot(panel_normal, C)),
|
||||
inlier_ratio=1.0,
|
||||
inlier_residual_rms_mm=0.0,
|
||||
)
|
||||
|
||||
frame = ReferenceFrame(
|
||||
center_C=tuple(C),
|
||||
line_direction=(1.0, 0.0, 0.0),
|
||||
reference_plane_z=float(C[2]),
|
||||
longitudinal_mid_y=float(C[1]),
|
||||
)
|
||||
|
||||
return GroundTruth(holes=holes, plane=plane, frame=frame, raw_circles=raw)
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# Helpers
|
||||
# ---------------------------------------------------------------------------
|
||||
|
||||
|
||||
def _dedupe_circles(
|
||||
items: list[dict[str, Any]],
|
||||
*,
|
||||
r_tol: float,
|
||||
c_tol: float,
|
||||
) -> list[dict[str, Any]]:
|
||||
"""Drop circles that share centre and radius (front/back face copies)."""
|
||||
out: list[dict[str, Any]] = []
|
||||
for it in items:
|
||||
c = np.asarray(it["center"])
|
||||
r = it["radius"]
|
||||
is_dup = False
|
||||
for kept in out:
|
||||
if abs(kept["radius"] - r) <= r_tol:
|
||||
if np.linalg.norm(np.asarray(kept["center"]) - c) <= c_tol:
|
||||
is_dup = True
|
||||
break
|
||||
if not is_dup:
|
||||
out.append(it)
|
||||
return out
|
||||
|
||||
|
||||
def _select_rectangle(
|
||||
items: list[dict[str, Any]],
|
||||
*,
|
||||
rect_aspect_tol: float,
|
||||
) -> list[dict[str, Any]]:
|
||||
"""Pick four items whose centres look most like a rectangle.
|
||||
|
||||
For tractability we (a) require ≥ 4 candidates, (b) if exactly 4, return
|
||||
them, (c) otherwise enumerate all 4-subsets and pick the one with the
|
||||
smallest "rectangleness" residual.
|
||||
"""
|
||||
if len(items) < 4:
|
||||
raise CADParseError(
|
||||
f"only {len(items)} locating-hole candidates found; need ≥ 4"
|
||||
)
|
||||
if len(items) == 4:
|
||||
return items
|
||||
|
||||
from itertools import combinations
|
||||
|
||||
best_score = float("inf")
|
||||
best: list[dict[str, Any]] | None = None
|
||||
for combo in combinations(items, 4):
|
||||
s = _rectangle_score(combo)
|
||||
if s < best_score:
|
||||
best_score = s
|
||||
best = list(combo)
|
||||
assert best is not None
|
||||
if best_score > rect_aspect_tol:
|
||||
# Still accept the best — the tolerance is informational; warn via
|
||||
# raw_circles in the GT report.
|
||||
pass
|
||||
return best
|
||||
|
||||
|
||||
def _rectangle_score(items) -> float:
|
||||
"""0 = perfect rectangle. Larger = worse."""
|
||||
c = np.asarray([i["center"] for i in items], dtype=np.float64)
|
||||
# Use distances between the 4 centres. For a rectangle, the 6 pairwise
|
||||
# distances are: two "long" sides equal, two "short" sides equal, two
|
||||
# diagonals equal.
|
||||
from itertools import combinations
|
||||
|
||||
d = sorted(np.linalg.norm(c[i] - c[j]) for i, j in combinations(range(4), 2))
|
||||
if d[0] < 1e-6:
|
||||
return 1e6
|
||||
s1, s2 = d[0], d[1] # short sides
|
||||
l1, l2 = d[2], d[3] # long sides
|
||||
g1, g2 = d[4], d[5] # diagonals
|
||||
score = (
|
||||
abs(s1 - s2) / max(s1, s2)
|
||||
+ abs(l1 - l2) / max(l1, l2)
|
||||
+ abs(g1 - g2) / max(g1, g2)
|
||||
)
|
||||
return score
|
||||
|
||||
|
||||
def _order_rectangle(items: list[dict[str, Any]]) -> list[dict[str, Any]]:
|
||||
"""Return four hole dicts in a canonical (-y/-z, +y/-z, +y/+z, -y/+z) order.
|
||||
|
||||
The ordering is taken in the y-z plane (panel normal is along ±x). This
|
||||
matches how a panel is normally described: bottom-left, bottom-right,
|
||||
top-right, top-left when viewed from +x looking down −x.
|
||||
"""
|
||||
c = np.asarray([i["center"] for i in items], dtype=np.float64)
|
||||
cy = c[:, 1].mean()
|
||||
cz = c[:, 2].mean()
|
||||
quadrant: dict[tuple[bool, bool], int] = {}
|
||||
for idx, p in enumerate(c):
|
||||
key = (p[1] >= cy, p[2] >= cz)
|
||||
quadrant[key] = idx
|
||||
order = [
|
||||
(False, False),
|
||||
(True, False),
|
||||
(True, True),
|
||||
(False, True),
|
||||
]
|
||||
if not all(q in quadrant for q in order):
|
||||
# Geometry is degenerate; fall back to angular sort.
|
||||
ang = np.arctan2(c[:, 2] - cz, c[:, 1] - cy)
|
||||
idxs = np.argsort(ang)
|
||||
return [items[i] for i in idxs]
|
||||
return [items[quadrant[q]] for q in order]
|
||||
|
||||
|
||||
def _consensus_axis(items: list[dict[str, Any]]) -> np.ndarray:
|
||||
"""Average and re-normalise the circle axis directions."""
|
||||
axes = np.asarray([i["axis"] for i in items], dtype=np.float64)
|
||||
# Flip any axes that disagree with the first.
|
||||
ref = axes[0]
|
||||
for k in range(1, len(axes)):
|
||||
if np.dot(axes[k], ref) < 0:
|
||||
axes[k] = -axes[k]
|
||||
n = axes.mean(axis=0)
|
||||
n /= max(np.linalg.norm(n), 1e-12)
|
||||
return n
|
||||
|
||||
|
||||
def _enforce_nx_positive(n: np.ndarray) -> np.ndarray:
|
||||
if n[0] < 0:
|
||||
return -n
|
||||
return n
|
||||
|
||||
|
||||
__all__ = ["GroundTruth", "extract_ground_truth"]
|
||||
@@ -0,0 +1,143 @@
|
||||
"""Tessellation of a TopoDS_Shape into a (V, F) triangle mesh.
|
||||
|
||||
The output is a small dataclass — we do not import ``trimesh`` until the
|
||||
caller actually asks for it, to keep the core dependency surface minimal.
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
from dataclasses import dataclass
|
||||
from typing import Any
|
||||
|
||||
import numpy as np
|
||||
|
||||
from .stp_parser import StepShape, _import_ocp, iter_faces
|
||||
|
||||
|
||||
@dataclass
|
||||
class TriangleMesh:
|
||||
"""Vertex / face arrays.
|
||||
|
||||
Attributes
|
||||
----------
|
||||
vertices : ``(V, 3) float64``
|
||||
faces : ``(F, 3) int64`` (vertex indices)
|
||||
face_normals : ``(F, 3) float64``
|
||||
"""
|
||||
|
||||
vertices: np.ndarray
|
||||
faces: np.ndarray
|
||||
face_normals: np.ndarray
|
||||
|
||||
@property
|
||||
def n_vertices(self) -> int:
|
||||
return int(self.vertices.shape[0])
|
||||
|
||||
@property
|
||||
def n_faces(self) -> int:
|
||||
return int(self.faces.shape[0])
|
||||
|
||||
def to_trimesh(self):
|
||||
"""Return a ``trimesh.Trimesh`` (lazy import)."""
|
||||
try:
|
||||
import trimesh
|
||||
except ImportError as exc:
|
||||
raise ImportError(
|
||||
"trimesh is required for to_trimesh(); "
|
||||
"install with: pip install 'coupler-panel-validation[cad]'"
|
||||
) from exc
|
||||
return trimesh.Trimesh(
|
||||
vertices=self.vertices,
|
||||
faces=self.faces,
|
||||
face_normals=self.face_normals,
|
||||
process=False,
|
||||
)
|
||||
|
||||
|
||||
def tessellate_to_trimesh(
|
||||
step: StepShape,
|
||||
*,
|
||||
linear_deflection_mm: float = 0.5,
|
||||
angular_deflection_rad: float = 0.1,
|
||||
) -> TriangleMesh:
|
||||
"""Tessellate a :class:`StepShape` into a single combined triangle mesh.
|
||||
|
||||
Each face is tessellated independently (with the location of the edge
|
||||
transferred onto the vertices), then concatenated. Triangle winding is
|
||||
flipped where the OCP face is reversed, so :attr:`face_normals` is the
|
||||
*outward* normal in the assembly frame.
|
||||
"""
|
||||
occ = _import_ocp()
|
||||
BRep_Tool = occ["BRep_Tool"]
|
||||
TopLoc_Location = occ["TopLoc_Location"]
|
||||
BRepMesh_IncrementalMesh = occ["BRepMesh_IncrementalMesh"]
|
||||
|
||||
BRepMesh_IncrementalMesh(
|
||||
step.shape,
|
||||
float(linear_deflection_mm),
|
||||
False,
|
||||
float(angular_deflection_rad),
|
||||
True,
|
||||
)
|
||||
|
||||
all_v: list[np.ndarray] = []
|
||||
all_f: list[np.ndarray] = []
|
||||
all_n: list[np.ndarray] = []
|
||||
v_offset = 0
|
||||
|
||||
for face in iter_faces(step.shape):
|
||||
loc = TopLoc_Location()
|
||||
triangulation = BRep_Tool.Triangulation_s(face, loc)
|
||||
if triangulation is None:
|
||||
continue
|
||||
|
||||
trsf = loc.Transformation()
|
||||
n_nodes = triangulation.NbNodes()
|
||||
n_tri = triangulation.NbTriangles()
|
||||
if n_nodes == 0 or n_tri == 0:
|
||||
continue
|
||||
|
||||
v = np.empty((n_nodes, 3), dtype=np.float64)
|
||||
for i in range(n_nodes):
|
||||
p = triangulation.Node(i + 1).Transformed(trsf)
|
||||
v[i, 0] = p.X()
|
||||
v[i, 1] = p.Y()
|
||||
v[i, 2] = p.Z()
|
||||
|
||||
f = np.empty((n_tri, 3), dtype=np.int64)
|
||||
is_reversed = face.Orientation() == 1 # TopAbs_REVERSED
|
||||
for j in range(n_tri):
|
||||
tri = triangulation.Triangle(j + 1)
|
||||
i1, i2, i3 = tri.Get()
|
||||
if is_reversed:
|
||||
f[j] = (i3 - 1, i2 - 1, i1 - 1)
|
||||
else:
|
||||
f[j] = (i1 - 1, i2 - 1, i3 - 1)
|
||||
|
||||
# Outward face normal per triangle.
|
||||
a = v[f[:, 0]]
|
||||
b = v[f[:, 1]]
|
||||
c = v[f[:, 2]]
|
||||
normals = np.cross(b - a, c - a)
|
||||
norms = np.linalg.norm(normals, axis=1, keepdims=True)
|
||||
norms[norms < 1e-12] = 1.0
|
||||
normals = normals / norms
|
||||
|
||||
all_v.append(v)
|
||||
all_f.append(f + v_offset)
|
||||
all_n.append(normals)
|
||||
v_offset += n_nodes
|
||||
|
||||
if not all_v:
|
||||
raise RuntimeError(
|
||||
"tessellation produced no triangles; "
|
||||
"try a finer linear_deflection_mm"
|
||||
)
|
||||
|
||||
vertices = np.concatenate(all_v, axis=0)
|
||||
faces = np.concatenate(all_f, axis=0)
|
||||
face_normals = np.concatenate(all_n, axis=0)
|
||||
return TriangleMesh(vertices=vertices, faces=faces, face_normals=face_normals)
|
||||
|
||||
|
||||
__all__ = ["TriangleMesh", "tessellate_to_trimesh"]
|
||||
@@ -0,0 +1,219 @@
|
||||
"""STP file loading via OCP (OpenCascade Python bindings).
|
||||
|
||||
The OCP import is deferred so the rest of the package does not require
|
||||
OpenCascade to be installed.
|
||||
|
||||
The parser exposes a thin :class:`StepShape` wrapper that gives the rest
|
||||
of the package what it needs: the underlying ``TopoDS_Shape`` for
|
||||
tessellation and topology traversal.
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
from dataclasses import dataclass
|
||||
from pathlib import Path
|
||||
from typing import Any
|
||||
|
||||
from ..exceptions import CADParseError
|
||||
|
||||
|
||||
@dataclass
|
||||
class StepShape:
|
||||
"""Result of loading a STEP file.
|
||||
|
||||
Attributes
|
||||
----------
|
||||
shape : ``OCP.TopoDS.TopoDS_Shape``
|
||||
The root shape (assembly compound or single solid).
|
||||
source : ``Path``
|
||||
Source file path, for debugging.
|
||||
"""
|
||||
|
||||
shape: Any
|
||||
source: Path
|
||||
|
||||
|
||||
def _import_ocp() -> dict[str, Any]:
|
||||
"""Import the OCP modules we need. Raises a clean error if missing."""
|
||||
try:
|
||||
from OCP.STEPControl import STEPControl_Reader # type: ignore
|
||||
from OCP.IFSelect import IFSelect_RetDone # type: ignore
|
||||
from OCP.TopoDS import TopoDS, TopoDS_Shape # type: ignore
|
||||
from OCP.TopAbs import ( # type: ignore
|
||||
TopAbs_FACE,
|
||||
TopAbs_EDGE,
|
||||
TopAbs_VERTEX,
|
||||
)
|
||||
from OCP.TopExp import TopExp_Explorer # type: ignore
|
||||
from OCP.TopLoc import TopLoc_Location # type: ignore
|
||||
from OCP.BRep import BRep_Tool # type: ignore
|
||||
from OCP.BRepMesh import BRepMesh_IncrementalMesh # type: ignore
|
||||
from OCP.BRepGProp import BRepGProp # type: ignore
|
||||
from OCP.GProp import GProp_GProps # type: ignore
|
||||
from OCP.GeomAbs import GeomAbs_Circle # type: ignore
|
||||
from OCP.BRepAdaptor import BRepAdaptor_Curve, BRepAdaptor_Surface # type: ignore
|
||||
from OCP.gp import gp_Pnt, gp_Dir, gp_Vec # type: ignore
|
||||
except ImportError as exc:
|
||||
raise ImportError(
|
||||
"OCP (cadquery-ocp) is required to parse STEP files. "
|
||||
"Install with: pip install 'coupler-panel-validation[cad]'"
|
||||
) from exc
|
||||
|
||||
return {
|
||||
"STEPControl_Reader": STEPControl_Reader,
|
||||
"IFSelect_RetDone": IFSelect_RetDone,
|
||||
"TopoDS": TopoDS,
|
||||
"TopoDS_Shape": TopoDS_Shape,
|
||||
"TopAbs_FACE": TopAbs_FACE,
|
||||
"TopAbs_EDGE": TopAbs_EDGE,
|
||||
"TopAbs_VERTEX": TopAbs_VERTEX,
|
||||
"TopExp_Explorer": TopExp_Explorer,
|
||||
"TopLoc_Location": TopLoc_Location,
|
||||
"BRep_Tool": BRep_Tool,
|
||||
"BRepMesh_IncrementalMesh": BRepMesh_IncrementalMesh,
|
||||
"BRepGProp": BRepGProp,
|
||||
"GProp_GProps": GProp_GProps,
|
||||
"GeomAbs_Circle": GeomAbs_Circle,
|
||||
"BRepAdaptor_Curve": BRepAdaptor_Curve,
|
||||
"BRepAdaptor_Surface": BRepAdaptor_Surface,
|
||||
"gp_Pnt": gp_Pnt,
|
||||
"gp_Dir": gp_Dir,
|
||||
"gp_Vec": gp_Vec,
|
||||
}
|
||||
|
||||
|
||||
def load_step(path: str | Path) -> StepShape:
|
||||
"""Read a STEP file and return its root :class:`TopoDS_Shape`.
|
||||
|
||||
Raises :class:`CADParseError` on read failure.
|
||||
"""
|
||||
p = Path(path)
|
||||
if not p.exists():
|
||||
raise CADParseError(f"STEP file not found: {p}")
|
||||
|
||||
occ = _import_ocp()
|
||||
reader = occ["STEPControl_Reader"]()
|
||||
status = reader.ReadFile(str(p))
|
||||
if status != occ["IFSelect_RetDone"]:
|
||||
raise CADParseError(f"OCP failed to read {p} (status={status})")
|
||||
n_roots = reader.TransferRoots()
|
||||
if n_roots <= 0:
|
||||
raise CADParseError(f"no roots transferred from {p}")
|
||||
|
||||
shape = reader.OneShape()
|
||||
return StepShape(shape=shape, source=p)
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# Topology iteration helpers (used by gt_extractor)
|
||||
# ---------------------------------------------------------------------------
|
||||
|
||||
|
||||
def _shape_key(shape) -> int:
|
||||
"""Stable identity key for a TopoDS_Shape across OCP versions.
|
||||
|
||||
OCP 7.8+ removed ``HashCode``. We fall back to the underlying TShape's
|
||||
Python id, which is stable per OpenCASCADE topology object.
|
||||
"""
|
||||
try:
|
||||
return shape.HashCode(2**31 - 1)
|
||||
except AttributeError:
|
||||
try:
|
||||
return id(shape.TShape())
|
||||
except Exception:
|
||||
return id(shape)
|
||||
|
||||
|
||||
def iter_faces(shape: Any):
|
||||
"""Yield every TopoDS_Face inside ``shape``.
|
||||
|
||||
Two visits of the *same underlying surface with opposite orientations*
|
||||
are treated as distinct because the orientation governs the outward
|
||||
normal of the produced triangulation.
|
||||
"""
|
||||
occ = _import_ocp()
|
||||
TopoDS = occ["TopoDS"]
|
||||
explorer = occ["TopExp_Explorer"](shape, occ["TopAbs_FACE"])
|
||||
while explorer.More():
|
||||
raw = explorer.Current()
|
||||
try:
|
||||
yield TopoDS.Face_s(raw)
|
||||
except Exception:
|
||||
pass
|
||||
explorer.Next()
|
||||
|
||||
|
||||
def iter_circular_edges(shape: Any):
|
||||
"""Yield ``(center_xyz, axis_xyz, radius_mm, edge)`` for every circular
|
||||
edge in ``shape``. Duplicates (same circle on two adjacent faces) are
|
||||
NOT removed here — downstream ``_dedupe_circles`` does that by
|
||||
geometry (centre + radius).
|
||||
"""
|
||||
occ = _import_ocp()
|
||||
explorer = occ["TopExp_Explorer"](shape, occ["TopAbs_EDGE"])
|
||||
TopoDS = occ["TopoDS"]
|
||||
BRepAdaptor_Curve = occ["BRepAdaptor_Curve"]
|
||||
GeomAbs_Circle = occ["GeomAbs_Circle"]
|
||||
while explorer.More():
|
||||
raw = explorer.Current()
|
||||
try:
|
||||
edge = TopoDS.Edge_s(raw)
|
||||
curve = BRepAdaptor_Curve(edge)
|
||||
if curve.GetType() == GeomAbs_Circle:
|
||||
circ = curve.Circle()
|
||||
loc = circ.Location()
|
||||
ax = circ.Axis().Direction()
|
||||
radius = float(circ.Radius())
|
||||
center = (float(loc.X()), float(loc.Y()), float(loc.Z()))
|
||||
axis = (float(ax.X()), float(ax.Y()), float(ax.Z()))
|
||||
yield center, axis, radius, edge
|
||||
except Exception:
|
||||
pass
|
||||
explorer.Next()
|
||||
|
||||
|
||||
def iter_cylindrical_faces(shape: Any):
|
||||
"""Yield ``(center, axis, radius, face)`` for every cylinder face.
|
||||
|
||||
Cylinder faces are typically the *walls* of through-holes; their
|
||||
radii match the circular edges that bound them.
|
||||
"""
|
||||
occ = _import_ocp()
|
||||
TopoDS = occ["TopoDS"]
|
||||
BRepAdaptor_Surface = occ["BRepAdaptor_Surface"]
|
||||
explorer = occ["TopExp_Explorer"](shape, occ["TopAbs_FACE"])
|
||||
while explorer.More():
|
||||
raw = explorer.Current()
|
||||
try:
|
||||
face = TopoDS.Face_s(raw)
|
||||
surf = BRepAdaptor_Surface(face)
|
||||
if int(surf.GetType()) == 1:
|
||||
cyl = surf.Cylinder()
|
||||
loc = cyl.Location()
|
||||
ax = cyl.Axis().Direction()
|
||||
radius = float(cyl.Radius())
|
||||
yield (
|
||||
(float(loc.X()), float(loc.Y()), float(loc.Z())),
|
||||
(float(ax.X()), float(ax.Y()), float(ax.Z())),
|
||||
radius,
|
||||
face,
|
||||
)
|
||||
except Exception:
|
||||
pass
|
||||
explorer.Next()
|
||||
|
||||
|
||||
def face_area(face: Any) -> float:
|
||||
occ = _import_ocp()
|
||||
props = occ["GProp_GProps"]()
|
||||
occ["BRepGProp"].SurfaceProperties_s(face, props)
|
||||
return float(props.Mass())
|
||||
|
||||
|
||||
__all__ = [
|
||||
"StepShape",
|
||||
"load_step",
|
||||
"iter_faces",
|
||||
"iter_circular_edges",
|
||||
"face_area",
|
||||
]
|
||||
161
alg/delivery_v1.2.1/coupler_panel_release/coupler_panel/cli.py
Normal file
161
alg/delivery_v1.2.1/coupler_panel_release/coupler_panel/cli.py
Normal file
@@ -0,0 +1,161 @@
|
||||
"""``coupler-measure`` command-line entry point.
|
||||
|
||||
Three modes::
|
||||
|
||||
# measure an existing point cloud (production-like flow)
|
||||
coupler-measure --input-cloud scan.ply --out-dir runs/demo
|
||||
|
||||
# research: synthesise a cloud from a STEP file and measure it
|
||||
coupler-measure --stp panel.stp --out-dir runs/from_cad
|
||||
|
||||
# research: parameter sweep
|
||||
coupler-measure --sweep matrix.yaml --out-dir runs/sweep_xx
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import argparse
|
||||
import json
|
||||
import logging
|
||||
import sys
|
||||
from pathlib import Path
|
||||
|
||||
|
||||
log = logging.getLogger("coupler_panel.cli")
|
||||
|
||||
|
||||
def _build_parser() -> argparse.ArgumentParser:
|
||||
p = argparse.ArgumentParser(
|
||||
prog="coupler-measure",
|
||||
description="Coupler-panel CAD-STP to point-cloud measurement & validation.",
|
||||
)
|
||||
src = p.add_mutually_exclusive_group(required=True)
|
||||
src.add_argument("--input-cloud", type=Path, help="Path to a PLY/XYZ/CSV/PCD point cloud.")
|
||||
src.add_argument("--stp", type=Path, help="Path to a STEP file (research mode).")
|
||||
src.add_argument("--sweep", type=Path, help="Path to a sweep matrix YAML.")
|
||||
|
||||
p.add_argument("--out-dir", type=Path, required=True, help="Output directory.")
|
||||
p.add_argument("--config", type=Path, default=None, help="YAML config overrides.")
|
||||
p.add_argument(
|
||||
"--profile",
|
||||
type=str,
|
||||
default="auto",
|
||||
help="auto | default | high_density | low_density | custom",
|
||||
)
|
||||
p.add_argument(
|
||||
"--target-holes",
|
||||
type=Path,
|
||||
default=None,
|
||||
help="DEPRECATED — the main pipeline no longer performs 4-hole Kabsch "
|
||||
"registration (it cancels the panel deflection signal). The flag "
|
||||
"is accepted for backward compatibility but is ignored.",
|
||||
)
|
||||
p.add_argument(
|
||||
"--render",
|
||||
action="store_true",
|
||||
help="After measurement, render PNG diagnostics into <out-dir>/images/.",
|
||||
)
|
||||
p.add_argument("-v", "--verbose", action="count", default=0, help="-v: INFO, -vv: DEBUG")
|
||||
return p
|
||||
|
||||
|
||||
def _setup_logging(verbose: int) -> None:
|
||||
level = logging.WARNING
|
||||
if verbose == 1:
|
||||
level = logging.INFO
|
||||
elif verbose >= 2:
|
||||
level = logging.DEBUG
|
||||
logging.basicConfig(
|
||||
level=level,
|
||||
format="%(asctime)s [%(levelname)s] %(name)s: %(message)s",
|
||||
)
|
||||
|
||||
|
||||
def _on_progress(stage: str, fraction: float) -> None:
|
||||
log.info("[%5.1f%%] %s", fraction * 100, stage)
|
||||
|
||||
|
||||
def _maybe_render(out_dir: Path) -> None:
|
||||
try:
|
||||
from .viz import render_all
|
||||
|
||||
render_all(out_dir)
|
||||
except ImportError:
|
||||
log.warning("matplotlib not installed; skip --render")
|
||||
except Exception as exc:
|
||||
log.warning("render_all failed: %s", exc)
|
||||
|
||||
|
||||
def main(argv: list[str] | None = None) -> int:
|
||||
args = _build_parser().parse_args(argv)
|
||||
_setup_logging(args.verbose)
|
||||
|
||||
try:
|
||||
if args.input_cloud is not None:
|
||||
from .pipeline import run_coupler_measurement_pipeline
|
||||
|
||||
if args.target_holes is not None:
|
||||
log.warning(
|
||||
"--target-holes is deprecated and ignored; the main pipeline "
|
||||
"no longer performs Kabsch registration."
|
||||
)
|
||||
result = run_coupler_measurement_pipeline(
|
||||
input_cloud=args.input_cloud,
|
||||
out_dir=args.out_dir,
|
||||
config_path=args.config,
|
||||
profile=args.profile,
|
||||
progress=_on_progress,
|
||||
)
|
||||
print(
|
||||
json.dumps(
|
||||
{
|
||||
"theta_right_deg": result.theta_right_deg,
|
||||
"theta_ref_deg": result.theta_ref_deg,
|
||||
"dx_y_max_mm": result.dx_y_max_mm,
|
||||
"dx_z_max_mm": result.dx_z_max_mm,
|
||||
"quality_flags": result.quality_flags,
|
||||
},
|
||||
indent=2,
|
||||
ensure_ascii=False,
|
||||
)
|
||||
)
|
||||
|
||||
elif args.stp is not None:
|
||||
from .pipeline import run_from_cad_pipeline
|
||||
|
||||
summary = run_from_cad_pipeline(
|
||||
stp_path=args.stp,
|
||||
out_dir=args.out_dir,
|
||||
config_path=args.config,
|
||||
profile=args.profile,
|
||||
progress=_on_progress,
|
||||
)
|
||||
print(
|
||||
json.dumps(
|
||||
{
|
||||
"diff": summary["diff"],
|
||||
"quality_flags": summary["measured"]["quality_flags"],
|
||||
},
|
||||
indent=2,
|
||||
ensure_ascii=False,
|
||||
)
|
||||
)
|
||||
|
||||
elif args.sweep is not None:
|
||||
from importlib import import_module
|
||||
|
||||
mod = import_module("experiments.sweep_runner")
|
||||
mod.main(matrix_path=args.sweep, out_dir=args.out_dir, config_path=args.config)
|
||||
|
||||
if args.render:
|
||||
_maybe_render(args.out_dir)
|
||||
|
||||
except Exception as exc:
|
||||
log.exception("pipeline failed: %s", exc)
|
||||
return 2
|
||||
|
||||
return 0
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
sys.exit(main())
|
||||
@@ -0,0 +1,209 @@
|
||||
"""Configuration loading and merging.
|
||||
|
||||
Configs are nested dicts. The single source of truth for defaults is
|
||||
``configs/default.yaml`` (shipped alongside the package); ``DEFAULT_CONFIG``
|
||||
mirrors its structure for in-memory programmatic overrides.
|
||||
|
||||
Public functions:
|
||||
|
||||
- :func:`load_config` — load a YAML file or accept an already-built dict.
|
||||
- :func:`load_profile` — load ``configs/profiles/<name>.yaml`` and merge
|
||||
on top of the defaults.
|
||||
- :func:`deep_merge` — recursive dict merge (override wins).
|
||||
- :func:`apply_dotted` — set a value via ``"sampling.spacing_mm"``.
|
||||
- :func:`resolve_auto` — fill in fields whose value is the literal
|
||||
string ``"auto"`` (currently: ``plane_fit.ransac_inlier_thresh_mm``).
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import copy
|
||||
from pathlib import Path
|
||||
from typing import Any
|
||||
|
||||
import yaml
|
||||
|
||||
|
||||
_PKG_ROOT = Path(__file__).resolve().parent
|
||||
_DEFAULT_YAML = _PKG_ROOT.parent / "configs" / "default.yaml"
|
||||
|
||||
|
||||
def _load_yaml(path: Path) -> dict[str, Any]:
|
||||
with path.open("r", encoding="utf-8") as fh:
|
||||
data = yaml.safe_load(fh)
|
||||
if not isinstance(data, dict):
|
||||
raise ValueError(f"{path}: expected a mapping at the top level")
|
||||
return data
|
||||
|
||||
|
||||
def _load_default() -> dict[str, Any]:
|
||||
if _DEFAULT_YAML.exists():
|
||||
return _load_yaml(_DEFAULT_YAML)
|
||||
# Package may have been installed without configs/. Fall back to a minimal
|
||||
# in-code default so library use without the YAML still works.
|
||||
return _IN_CODE_FALLBACK
|
||||
|
||||
|
||||
# Loaded lazily; recomputed on each call to keep tests deterministic.
|
||||
def default_config() -> dict[str, Any]:
|
||||
"""Return a fresh copy of the default configuration."""
|
||||
return copy.deepcopy(_load_default())
|
||||
|
||||
|
||||
DEFAULT_CONFIG: dict[str, Any] # populated below for convenience
|
||||
|
||||
|
||||
def load_config(
|
||||
source: str | Path | dict[str, Any] | None,
|
||||
*,
|
||||
profile: str = "auto",
|
||||
) -> dict[str, Any]:
|
||||
"""Resolve the final configuration to use.
|
||||
|
||||
The merge order is::
|
||||
|
||||
default.yaml < profile (if any) < source (path or dict)
|
||||
|
||||
Parameters
|
||||
----------
|
||||
source : path, dict, or None
|
||||
If ``None``, only defaults (and the profile) are used.
|
||||
profile : str
|
||||
``"auto"`` / ``"default"`` skip the profile step. Any other name
|
||||
is looked up at ``configs/profiles/<name>.yaml``.
|
||||
"""
|
||||
cfg = default_config()
|
||||
|
||||
if profile not in (None, "auto", "default", "custom"):
|
||||
prof_path = _PKG_ROOT.parent / "configs" / "profiles" / f"{profile}.yaml"
|
||||
if not prof_path.exists():
|
||||
raise FileNotFoundError(f"profile '{profile}' not found at {prof_path}")
|
||||
cfg = deep_merge(cfg, _load_yaml(prof_path))
|
||||
|
||||
if source is None:
|
||||
pass
|
||||
elif isinstance(source, dict):
|
||||
cfg = deep_merge(cfg, source)
|
||||
else:
|
||||
cfg = deep_merge(cfg, _load_yaml(Path(source)))
|
||||
|
||||
return cfg
|
||||
|
||||
|
||||
def load_profile(name: str) -> dict[str, Any]:
|
||||
"""Load a profile YAML directly; useful for tests."""
|
||||
path = _PKG_ROOT.parent / "configs" / "profiles" / f"{name}.yaml"
|
||||
return _load_yaml(path)
|
||||
|
||||
|
||||
def deep_merge(base: dict[str, Any], override: dict[str, Any]) -> dict[str, Any]:
|
||||
"""Recursive dict merge. ``override`` wins on leaf collisions."""
|
||||
out = copy.deepcopy(base)
|
||||
for key, val in override.items():
|
||||
if (
|
||||
key in out
|
||||
and isinstance(out[key], dict)
|
||||
and isinstance(val, dict)
|
||||
):
|
||||
out[key] = deep_merge(out[key], val)
|
||||
else:
|
||||
out[key] = copy.deepcopy(val)
|
||||
return out
|
||||
|
||||
|
||||
def apply_dotted(cfg: dict[str, Any], dotted_key: str, value: Any) -> dict[str, Any]:
|
||||
"""Set ``cfg["a"]["b"]["c"] = value`` from ``"a.b.c"``. Returns ``cfg``."""
|
||||
parts = dotted_key.split(".")
|
||||
node = cfg
|
||||
for p in parts[:-1]:
|
||||
node = node.setdefault(p, {})
|
||||
if not isinstance(node, dict):
|
||||
raise TypeError(f"cannot apply '{dotted_key}': '{p}' is not a dict")
|
||||
node[parts[-1]] = value
|
||||
return cfg
|
||||
|
||||
|
||||
def resolve_auto(cfg: dict[str, Any]) -> dict[str, Any]:
|
||||
"""Replace literal ``"auto"`` markers with concrete values.
|
||||
|
||||
Currently:
|
||||
|
||||
- ``plane_fit.ransac_inlier_thresh_mm == "auto"`` → ``3 * noise.sigma_mm``
|
||||
(or 0.5 mm if ``noise.sigma_mm == 0``).
|
||||
"""
|
||||
out = copy.deepcopy(cfg)
|
||||
pf = out.get("plane_fit", {})
|
||||
if pf.get("ransac_inlier_thresh_mm", None) == "auto":
|
||||
sigma = float(out.get("noise", {}).get("sigma_mm", 0.0) or 0.0)
|
||||
pf["ransac_inlier_thresh_mm"] = max(3.0 * sigma, 0.5)
|
||||
out["plane_fit"] = pf
|
||||
return out
|
||||
|
||||
|
||||
# Minimal in-code fallback used only when the YAML is missing.
|
||||
_IN_CODE_FALLBACK: dict[str, Any] = {
|
||||
"schema_version": "1.0",
|
||||
"cad": {
|
||||
"linear_deflection_mm": 0.5,
|
||||
"angular_deflection_rad": 0.1,
|
||||
"locating_hole_radius_mm": [4.0, 12.0],
|
||||
"central_hole_radius_min_mm": 20.0,
|
||||
"rectangle_aspect_tol": 0.15,
|
||||
},
|
||||
"sampling": {"method": "poisson_disk", "spacing_mm": 1.0, "num_points": 100000, "seed": 42},
|
||||
"noise": {"enabled": True, "sigma_mm": 0.5, "mode": "isotropic", "seed": 43},
|
||||
"transform": {
|
||||
"enabled": True,
|
||||
"rotation_deg": 3.0,
|
||||
"rotation_axis": "random",
|
||||
"translation_mm": 5.0,
|
||||
"translation_dir": "random",
|
||||
"seed": 44,
|
||||
},
|
||||
"denoise": {"steps": []},
|
||||
"registration": {"method": "kabsch_four_holes", "pre_align": "pca_centroid"},
|
||||
"detection": {
|
||||
"panel_normal_seed": "from_cad",
|
||||
"grid_pixel_mm": 0.5,
|
||||
"panel_min_area_ratio": 0.4,
|
||||
"hole_min_area_mm2": 30.0,
|
||||
"hole_max_area_mm2": 800.0,
|
||||
"circle_fit": {"method": "algebraic_then_lsq", "inlier_residual_mm": 0.5},
|
||||
"select_top_k_by_radius_match": 4,
|
||||
"rectangle_aspect_tol": 0.20,
|
||||
},
|
||||
"roi": {
|
||||
"hole_dilate_mm": 3.0,
|
||||
"central_hole_dilate_mm": 5.0,
|
||||
"edge_buffer_mm": 5.0,
|
||||
"chamfer_normal_dot_min": 0.85,
|
||||
"outlier_z_score": 3.0,
|
||||
},
|
||||
"plane_fit": {
|
||||
"method": "ransac_svd",
|
||||
"ransac_iterations": 200,
|
||||
"ransac_inlier_thresh_mm": "auto",
|
||||
"min_inlier_ratio": 0.6,
|
||||
"enforce_nx_positive": True,
|
||||
},
|
||||
"metrics": {"right_axis_sign": 1, "angle_clamp_deg": 0.0},
|
||||
"artifacts": {
|
||||
"save_intermediate": True,
|
||||
"ply_format": "binary",
|
||||
"image_dpi": 150,
|
||||
"heatmap_colormap": "viridis",
|
||||
},
|
||||
}
|
||||
|
||||
DEFAULT_CONFIG = copy.deepcopy(_IN_CODE_FALLBACK)
|
||||
|
||||
|
||||
__all__ = [
|
||||
"DEFAULT_CONFIG",
|
||||
"default_config",
|
||||
"load_config",
|
||||
"load_profile",
|
||||
"deep_merge",
|
||||
"apply_dotted",
|
||||
"resolve_auto",
|
||||
]
|
||||
@@ -0,0 +1,528 @@
|
||||
"""Data-preparation pipeline (research / validation use only).
|
||||
|
||||
This module is **not** part of the integration API. It exists so that
|
||||
researchers can take a CAD STP file, synthesise scan-like point clouds,
|
||||
denoise them, and align them to the CAD frame — i.e. it produces an
|
||||
input that :func:`coupler_panel.api.compute_coupler_measurement` can
|
||||
then run on.
|
||||
|
||||
Stages performed (each one is independently re-runnable so the experiment
|
||||
sweep can iterate over individual knobs)::
|
||||
|
||||
parse_cad STP → mesh + GT
|
||||
synthesize_cloud mesh → sampled (multi-strategy) → noisy → transformed
|
||||
denoise_cloud cloud → cloud (configurable pipeline)
|
||||
register_cloud cloud → cloud (4-hole Kabsch into CAD frame)
|
||||
|
||||
The end-to-end convenience wrapper :func:`prepare_cloud_from_cad` glues
|
||||
the four stages together and returns a :class:`PreparedCloud` ready to
|
||||
hand to the measurement API.
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
from dataclasses import dataclass, field
|
||||
from pathlib import Path
|
||||
from typing import Any
|
||||
|
||||
import numpy as np
|
||||
|
||||
from .config import load_config, resolve_auto
|
||||
from .denoise import run_pipeline as run_denoise
|
||||
from .detection import detect_four_holes
|
||||
from .exceptions import RegistrationError
|
||||
from .registration import register_four_holes
|
||||
from .structures import Hole, HoleSet
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# Result types
|
||||
# ---------------------------------------------------------------------------
|
||||
|
||||
|
||||
@dataclass
|
||||
class CADParsed:
|
||||
"""Output of :func:`parse_cad`."""
|
||||
|
||||
mesh: Any # coupler_panel.cad.mesh.TriangleMesh
|
||||
gt: Any # coupler_panel.cad.gt_extractor.GroundTruth
|
||||
|
||||
|
||||
@dataclass
|
||||
class SynthesizedCloud:
|
||||
"""Output of :func:`synthesize_cloud`."""
|
||||
|
||||
sampled: np.ndarray # (Ns, 3) fresh from the mesh, optional HPR applied
|
||||
deflected: np.ndarray # (Ns, 3) + panel-deflection injection (known GT)
|
||||
noisy: np.ndarray # (Ns, 3) + Gaussian noise
|
||||
transformed: np.ndarray # (Ns, 3) + scanner-pose rigid transform
|
||||
gt_transform_4x4: np.ndarray # (4, 4) scanner-pose transform applied at end
|
||||
gt_holes_in_scan_frame: HoleSet # deflected GT holes after T_jitter — Kabsch target
|
||||
# ---- Panel-deflection ground truth ----------------------------------
|
||||
panel_deflection_R: np.ndarray # (3, 3) rotation that tilts the panel
|
||||
panel_deflection_theta_right_deg: float
|
||||
panel_deflection_theta_ref_deg: float
|
||||
gt_holes_after_deflection: HoleSet # in CAD frame, post-deflection
|
||||
gt_normal_after_deflection: np.ndarray # (3,) deflected panel normal in CAD frame
|
||||
info: dict[str, Any] = field(default_factory=dict)
|
||||
|
||||
|
||||
@dataclass
|
||||
class DenoisedCloud:
|
||||
"""Output of :func:`denoise_cloud`."""
|
||||
|
||||
points: np.ndarray
|
||||
log: list[dict[str, Any]]
|
||||
|
||||
|
||||
@dataclass
|
||||
class RegisteredCloud:
|
||||
"""Output of :func:`register_cloud`."""
|
||||
|
||||
points: np.ndarray # cloud aligned into the CAD frame
|
||||
transform_4x4: np.ndarray # the rigid alignment (scan → CAD)
|
||||
rmse_mm: float
|
||||
max_residual_mm: float
|
||||
detected_holes_in_cad_frame: HoleSet
|
||||
|
||||
|
||||
@dataclass
|
||||
class PreparedCloud:
|
||||
"""End-to-end output, ready to feed into the public measurement API.
|
||||
|
||||
``points`` is in the CAD frame, denoised, with all the meta-data the
|
||||
experiment / validation harness needs to score the measurement.
|
||||
"""
|
||||
|
||||
points: np.ndarray
|
||||
gt: Any # GroundTruth
|
||||
gt_transform_4x4: np.ndarray | None
|
||||
registration_transform_4x4: np.ndarray | None
|
||||
registration_rmse_mm: float | None
|
||||
detected_holes_in_cad_frame: HoleSet | None
|
||||
intermediates: dict[str, Any] = field(default_factory=dict)
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# Stage 1 — STP → mesh + GT
|
||||
# ---------------------------------------------------------------------------
|
||||
|
||||
|
||||
def parse_cad(stp_path: str | Path, cad_cfg: dict[str, Any]) -> CADParsed:
|
||||
from .cad import extract_ground_truth, load_step, tessellate_to_trimesh
|
||||
|
||||
step = load_step(stp_path)
|
||||
mesh = tessellate_to_trimesh(
|
||||
step,
|
||||
linear_deflection_mm=float(cad_cfg["linear_deflection_mm"]),
|
||||
angular_deflection_rad=float(cad_cfg["angular_deflection_rad"]),
|
||||
)
|
||||
gt = extract_ground_truth(step, cad_cfg)
|
||||
return CADParsed(mesh=mesh, gt=gt)
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# Stage 2 — mesh → sampled → noisy → transformed
|
||||
# ---------------------------------------------------------------------------
|
||||
|
||||
|
||||
def _build_panel_deflection_R(theta_right_deg: float, theta_ref_deg: float) -> np.ndarray:
|
||||
"""Build the 3×3 rotation that tilts a panel whose normal is (1,0,0).
|
||||
|
||||
The post-rotation normal is::
|
||||
|
||||
n = (cos(th_ref)·cos(th_right), cos(th_ref)·sin(th_right), sin(th_ref))
|
||||
|
||||
so that ``atan2(n_y, n_x) = th_right`` and ``atan2(n_z, n_x) = th_ref``
|
||||
exactly.
|
||||
|
||||
R = R_z(th_right) · R_y(-th_ref)
|
||||
"""
|
||||
a = np.deg2rad(float(theta_right_deg))
|
||||
b = np.deg2rad(float(theta_ref_deg))
|
||||
Rz = np.array(
|
||||
[
|
||||
[np.cos(a), -np.sin(a), 0.0],
|
||||
[np.sin(a), np.cos(a), 0.0],
|
||||
[0.0, 0.0, 1.0],
|
||||
]
|
||||
)
|
||||
Ry_neg = np.array(
|
||||
[
|
||||
[np.cos(b), 0.0, -np.sin(b)],
|
||||
[0.0, 1.0, 0.0],
|
||||
[np.sin(b), 0.0, np.cos(b)],
|
||||
]
|
||||
)
|
||||
return Rz @ Ry_neg
|
||||
|
||||
|
||||
def _rotate_holeset(holes: HoleSet, R: np.ndarray) -> HoleSet:
|
||||
locating = tuple(
|
||||
Hole(
|
||||
center=tuple((R @ np.asarray(h.center)).tolist()),
|
||||
radius_mm=h.radius_mm,
|
||||
fit_residual_rms_mm=h.fit_residual_rms_mm,
|
||||
role=h.role,
|
||||
)
|
||||
for h in holes.locating
|
||||
)
|
||||
central = (
|
||||
None
|
||||
if holes.central is None
|
||||
else Hole(
|
||||
center=tuple((R @ np.asarray(holes.central.center)).tolist()),
|
||||
radius_mm=holes.central.radius_mm,
|
||||
fit_residual_rms_mm=holes.central.fit_residual_rms_mm,
|
||||
role="central",
|
||||
)
|
||||
)
|
||||
return HoleSet(locating=locating, central=central)
|
||||
|
||||
|
||||
def synthesize_cloud(
|
||||
parsed: CADParsed,
|
||||
*,
|
||||
sampling_cfg: dict[str, Any],
|
||||
noise_cfg: dict[str, Any],
|
||||
transform_cfg: dict[str, Any],
|
||||
scan_cfg: dict[str, Any] | None = None,
|
||||
deflection_cfg: dict[str, Any] | None = None,
|
||||
) -> SynthesizedCloud:
|
||||
from .sampling import resolve_num_points, sample_mesh
|
||||
from .simulation import (
|
||||
add_gaussian_noise,
|
||||
apply_rigid,
|
||||
sample_random_rigid,
|
||||
simulate_scan_view,
|
||||
)
|
||||
|
||||
n = resolve_num_points(
|
||||
parsed.mesh,
|
||||
num_points=sampling_cfg.get("num_points"),
|
||||
spacing_mm=sampling_cfg.get("spacing_mm"),
|
||||
)
|
||||
sampled = sample_mesh(
|
||||
parsed.mesh,
|
||||
method=sampling_cfg["method"],
|
||||
num_points=n,
|
||||
spacing_mm=sampling_cfg.get("spacing_mm"),
|
||||
seed=int(sampling_cfg.get("seed", 42)),
|
||||
curvature_weight=float(sampling_cfg.get("adaptive", {}).get("curvature_weight", 1.0)),
|
||||
)
|
||||
|
||||
# Optional: simulate a single-view scan via hidden-point removal.
|
||||
# Without this, the sampler returns points from BOTH sides of the panel
|
||||
# (front face, back face, cylinder hole walls, …); the projected
|
||||
# occupancy grid then has no cavities because the back-face points fill
|
||||
# the hole region. Enable this for any "facing-scan" simulation.
|
||||
if scan_cfg and bool(scan_cfg.get("enabled", False)):
|
||||
view = scan_cfg.get("view_origin", [10000.0, 0.0, 0.0])
|
||||
sampled = simulate_scan_view(
|
||||
sampled,
|
||||
view_origin=view,
|
||||
hpr_radius_mm=float(scan_cfg.get("hpr_radius_mm", 50000.0)),
|
||||
)
|
||||
|
||||
# ---- Panel deflection injection ---------------------------------------
|
||||
# This is the *signal* the algorithm should later recover. We rotate
|
||||
# the entire panel (and its 4 locating holes + central hole) by a known
|
||||
# small angle so that the GT panel normal becomes
|
||||
# n_new = R · (1, 0, 0)
|
||||
# with atan2(n_new.y, n_new.x) = theta_right_deg (rotation around z)
|
||||
# and atan2(n_new.z, n_new.x) = theta_ref_deg (rotation around -y).
|
||||
# The 4 hole centres rotate with the panel, so 4-hole Kabsch (which
|
||||
# targets the *deflected* GT holes) does NOT undo the deflection — the
|
||||
# panel orientation in the registered cloud still carries the signal.
|
||||
th_right = 0.0
|
||||
th_ref = 0.0
|
||||
if deflection_cfg and bool(deflection_cfg.get("enabled", False)):
|
||||
th_right = float(deflection_cfg.get("theta_right_deg", 0.0))
|
||||
th_ref = float(deflection_cfg.get("theta_ref_deg", 0.0))
|
||||
R_panel = _build_panel_deflection_R(th_right, th_ref)
|
||||
deflected = sampled @ R_panel.T
|
||||
gt_holes_after = _rotate_holeset(parsed.gt.holes, R_panel)
|
||||
gt_normal_after = R_panel @ np.asarray(parsed.gt.plane.normal, dtype=np.float64)
|
||||
sampled = deflected # downstream uses the deflected cloud as the new ground
|
||||
|
||||
if bool(noise_cfg.get("enabled", True)) and float(noise_cfg.get("sigma_mm", 0.0)) > 0:
|
||||
noisy = add_gaussian_noise(
|
||||
sampled,
|
||||
sigma_mm=float(noise_cfg["sigma_mm"]),
|
||||
mode=noise_cfg.get("mode", "isotropic"),
|
||||
seed=int(noise_cfg.get("seed", 43)),
|
||||
)
|
||||
else:
|
||||
noisy = sampled.copy()
|
||||
|
||||
# The "GT-side" scan-pose perturbation: shifts and rotates the cloud as
|
||||
# a whole to mimic mounting / scanner-stage repeatability error. It is
|
||||
# NOT the deflection we want to measure; 4-hole Kabsch should cancel it.
|
||||
if bool(transform_cfg.get("enabled", True)):
|
||||
T = sample_random_rigid(
|
||||
rotation_deg=float(transform_cfg.get("rotation_deg", 0.0)),
|
||||
translation_mm=float(transform_cfg.get("translation_mm", 0.0)),
|
||||
rotation_axis=transform_cfg.get("rotation_axis", "random"),
|
||||
translation_dir=transform_cfg.get("translation_dir", "random"),
|
||||
seed=int(transform_cfg.get("seed", 44)),
|
||||
)
|
||||
transformed = apply_rigid(noisy, T.matrix)
|
||||
Mat = T.matrix
|
||||
R = Mat[:3, :3]
|
||||
t = Mat[:3, 3]
|
||||
# Deflected GT holes, then jitter — same composition the cloud went through.
|
||||
gt_holes_scan = HoleSet(
|
||||
locating=tuple( # type: ignore[arg-type]
|
||||
Hole(
|
||||
center=tuple((R @ np.asarray(h.center) + t).tolist()),
|
||||
radius_mm=h.radius_mm,
|
||||
fit_residual_rms_mm=0.0,
|
||||
role=h.role,
|
||||
)
|
||||
for h in gt_holes_after.locating
|
||||
),
|
||||
central=(
|
||||
None
|
||||
if gt_holes_after.central is None
|
||||
else Hole(
|
||||
center=tuple(
|
||||
(R @ np.asarray(gt_holes_after.central.center) + t).tolist()
|
||||
),
|
||||
radius_mm=gt_holes_after.central.radius_mm,
|
||||
fit_residual_rms_mm=0.0,
|
||||
role="central",
|
||||
)
|
||||
),
|
||||
)
|
||||
info = {"transform": T.to_dict()}
|
||||
gt_T = T.matrix
|
||||
else:
|
||||
transformed = noisy.copy()
|
||||
gt_holes_scan = gt_holes_after
|
||||
info = {"transform": {"enabled": False}}
|
||||
gt_T = np.eye(4)
|
||||
|
||||
return SynthesizedCloud(
|
||||
sampled=sampled,
|
||||
deflected=deflected,
|
||||
noisy=noisy,
|
||||
transformed=transformed,
|
||||
gt_transform_4x4=gt_T,
|
||||
gt_holes_in_scan_frame=gt_holes_scan,
|
||||
panel_deflection_R=R_panel,
|
||||
panel_deflection_theta_right_deg=th_right,
|
||||
panel_deflection_theta_ref_deg=th_ref,
|
||||
gt_holes_after_deflection=gt_holes_after,
|
||||
gt_normal_after_deflection=gt_normal_after,
|
||||
info=info,
|
||||
)
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# Stage 3 — denoise
|
||||
# ---------------------------------------------------------------------------
|
||||
|
||||
|
||||
def denoise_cloud(
|
||||
points: np.ndarray,
|
||||
denoise_cfg: dict[str, Any],
|
||||
) -> DenoisedCloud:
|
||||
pts, log = run_denoise(points, denoise_cfg.get("steps", []))
|
||||
return DenoisedCloud(points=pts, log=log)
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# Stage 4 — register (4-hole Kabsch)
|
||||
# ---------------------------------------------------------------------------
|
||||
|
||||
|
||||
def register_cloud(
|
||||
points: np.ndarray,
|
||||
*,
|
||||
target_holes: HoleSet,
|
||||
detection_cfg: dict[str, Any],
|
||||
cad_cfg: dict[str, Any],
|
||||
registration_cfg: dict[str, Any],
|
||||
panel_normal_seed: np.ndarray | None = None,
|
||||
) -> RegisteredCloud:
|
||||
"""Detect 4 holes in ``points`` and Kabsch-align them to ``target_holes``.
|
||||
|
||||
``target_holes`` is the GT (CAD-frame) hole set. The output cloud is
|
||||
in the CAD frame. The 4-hole pairing is canonical-quadrant-ordered
|
||||
on both sides, so no greedy NN matching is needed.
|
||||
"""
|
||||
detection = detect_four_holes(
|
||||
points,
|
||||
detection_cfg=detection_cfg,
|
||||
cad_cfg=cad_cfg,
|
||||
panel_normal_seed=panel_normal_seed,
|
||||
)
|
||||
|
||||
src = detection.holes_3d.locating_centers()
|
||||
tgt = target_holes.locating_centers()
|
||||
kabsch = register_four_holes(
|
||||
src,
|
||||
tgt,
|
||||
refuse_above_mm=registration_cfg.get("refuse_if_residual_above_mm"),
|
||||
)
|
||||
|
||||
R = kabsch.matrix[:3, :3]
|
||||
t = kabsch.matrix[:3, 3]
|
||||
aligned = points @ R.T + t
|
||||
|
||||
detected_in_cad = HoleSet(
|
||||
locating=tuple( # type: ignore[arg-type]
|
||||
Hole(
|
||||
center=tuple((R @ np.asarray(h.center) + t).tolist()),
|
||||
radius_mm=h.radius_mm,
|
||||
fit_residual_rms_mm=h.fit_residual_rms_mm,
|
||||
role=h.role,
|
||||
)
|
||||
for h in detection.holes_3d.locating
|
||||
),
|
||||
central=(
|
||||
None
|
||||
if detection.holes_3d.central is None
|
||||
else Hole(
|
||||
center=tuple(
|
||||
(R @ np.asarray(detection.holes_3d.central.center) + t).tolist()
|
||||
),
|
||||
radius_mm=detection.holes_3d.central.radius_mm,
|
||||
fit_residual_rms_mm=detection.holes_3d.central.fit_residual_rms_mm,
|
||||
role="central",
|
||||
)
|
||||
),
|
||||
)
|
||||
|
||||
return RegisteredCloud(
|
||||
points=aligned,
|
||||
transform_4x4=kabsch.matrix,
|
||||
rmse_mm=kabsch.rmse_mm,
|
||||
max_residual_mm=kabsch.max_residual_mm,
|
||||
detected_holes_in_cad_frame=detected_in_cad,
|
||||
)
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# End-to-end convenience: STP → PreparedCloud
|
||||
# ---------------------------------------------------------------------------
|
||||
|
||||
|
||||
def prepare_cloud_from_cad(
|
||||
stp_path: str | Path,
|
||||
config: str | Path | dict[str, Any] | None = None,
|
||||
profile: str = "auto",
|
||||
) -> PreparedCloud:
|
||||
"""Run all four stages and return a :class:`PreparedCloud`.
|
||||
|
||||
The returned cloud is denoised and aligned into the CAD frame; pass
|
||||
``prepared.points`` to
|
||||
:func:`coupler_panel.api.compute_coupler_measurement`.
|
||||
"""
|
||||
cfg = load_config(config, profile=profile)
|
||||
cfg = resolve_auto(cfg)
|
||||
|
||||
parsed = parse_cad(stp_path, cfg["cad"])
|
||||
synth = synthesize_cloud(
|
||||
parsed,
|
||||
sampling_cfg=cfg["sampling"],
|
||||
noise_cfg=cfg.get("noise", {}),
|
||||
transform_cfg=cfg.get("transform", {}),
|
||||
scan_cfg=cfg.get("scan_simulator", {}),
|
||||
deflection_cfg=cfg.get("panel_deflection", {}),
|
||||
)
|
||||
denoised = denoise_cloud(synth.transformed, cfg.get("denoise", {}))
|
||||
# Important: register against the *deflected* GT — the holes have moved
|
||||
# with the panel, so Kabsch only undoes the scan-pose jitter, NOT the
|
||||
# injected deflection. The plane fit downstream then sees the tilted
|
||||
# panel and can recover theta_right / theta_ref.
|
||||
registered = register_cloud(
|
||||
denoised.points,
|
||||
target_holes=synth.gt_holes_after_deflection,
|
||||
detection_cfg=cfg["detection"],
|
||||
cad_cfg=cfg["cad"],
|
||||
registration_cfg=cfg["registration"],
|
||||
)
|
||||
|
||||
return PreparedCloud(
|
||||
points=registered.points,
|
||||
gt=parsed.gt,
|
||||
gt_transform_4x4=synth.gt_transform_4x4,
|
||||
registration_transform_4x4=registered.transform_4x4,
|
||||
registration_rmse_mm=registered.rmse_mm,
|
||||
detected_holes_in_cad_frame=registered.detected_holes_in_cad_frame,
|
||||
intermediates={
|
||||
"config": cfg,
|
||||
"sampled": synth.sampled,
|
||||
"noisy": synth.noisy,
|
||||
"transformed": synth.transformed,
|
||||
"denoised": denoised.points,
|
||||
"denoise_log": denoised.log,
|
||||
},
|
||||
)
|
||||
|
||||
|
||||
def prepare_cloud_from_scan(
|
||||
points: np.ndarray,
|
||||
*,
|
||||
config: str | Path | dict[str, Any] | None = None,
|
||||
profile: str = "auto",
|
||||
target_holes: HoleSet | None = None,
|
||||
) -> PreparedCloud:
|
||||
"""Same as :func:`prepare_cloud_from_cad` but starting from a real scan.
|
||||
|
||||
Skips parsing / synthesis. If ``target_holes`` is given, runs the
|
||||
4-hole Kabsch registration; otherwise assumes the cloud is already
|
||||
in the CAD frame.
|
||||
"""
|
||||
cfg = load_config(config, profile=profile)
|
||||
cfg = resolve_auto(cfg)
|
||||
|
||||
denoised = denoise_cloud(np.asarray(points, dtype=np.float64), cfg.get("denoise", {}))
|
||||
|
||||
if target_holes is None:
|
||||
return PreparedCloud(
|
||||
points=denoised.points,
|
||||
gt=None,
|
||||
gt_transform_4x4=None,
|
||||
registration_transform_4x4=None,
|
||||
registration_rmse_mm=None,
|
||||
detected_holes_in_cad_frame=None,
|
||||
intermediates={"config": cfg, "denoise_log": denoised.log},
|
||||
)
|
||||
|
||||
registered = register_cloud(
|
||||
denoised.points,
|
||||
target_holes=target_holes,
|
||||
detection_cfg=cfg["detection"],
|
||||
cad_cfg=cfg["cad"],
|
||||
registration_cfg=cfg["registration"],
|
||||
)
|
||||
return PreparedCloud(
|
||||
points=registered.points,
|
||||
gt=None,
|
||||
gt_transform_4x4=None,
|
||||
registration_transform_4x4=registered.transform_4x4,
|
||||
registration_rmse_mm=registered.rmse_mm,
|
||||
detected_holes_in_cad_frame=registered.detected_holes_in_cad_frame,
|
||||
intermediates={
|
||||
"config": cfg,
|
||||
"denoised": denoised.points,
|
||||
"denoise_log": denoised.log,
|
||||
},
|
||||
)
|
||||
|
||||
|
||||
__all__ = [
|
||||
"CADParsed",
|
||||
"SynthesizedCloud",
|
||||
"DenoisedCloud",
|
||||
"RegisteredCloud",
|
||||
"PreparedCloud",
|
||||
"parse_cad",
|
||||
"synthesize_cloud",
|
||||
"denoise_cloud",
|
||||
"register_cloud",
|
||||
"prepare_cloud_from_cad",
|
||||
"prepare_cloud_from_scan",
|
||||
]
|
||||
@@ -0,0 +1,29 @@
|
||||
"""Composable point cloud denoising pipeline.
|
||||
|
||||
Each step is a pure function ``points -> points``. Steps are configured
|
||||
via YAML and chained by :func:`run_pipeline`.
|
||||
|
||||
Available steps (matching ``denoise.steps[].type``):
|
||||
|
||||
- ``passthrough`` — keep points inside an axis-aligned bounding box
|
||||
- ``voxel`` — voxel-grid downsample
|
||||
- ``statistical`` — statistical outlier removal (SOR)
|
||||
- ``radius`` — radius outlier removal (ROR)
|
||||
- ``smoothing`` — MLS smoothing (only when explicitly enabled)
|
||||
"""
|
||||
|
||||
from .passthrough import passthrough_filter
|
||||
from .voxel import voxel_downsample
|
||||
from .statistical import statistical_outlier_removal
|
||||
from .radius import radius_outlier_removal
|
||||
from .smoothing import mls_smoothing
|
||||
from .pipeline import run_pipeline
|
||||
|
||||
__all__ = [
|
||||
"passthrough_filter",
|
||||
"voxel_downsample",
|
||||
"statistical_outlier_removal",
|
||||
"radius_outlier_removal",
|
||||
"mls_smoothing",
|
||||
"run_pipeline",
|
||||
]
|
||||
@@ -0,0 +1,41 @@
|
||||
"""Passthrough (axis-aligned bounding-box) filter."""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import numpy as np
|
||||
|
||||
|
||||
def passthrough_filter(
|
||||
points: np.ndarray,
|
||||
*,
|
||||
bbox: tuple[tuple[float, float], tuple[float, float], tuple[float, float]] | None = None,
|
||||
bbox_padding_mm: float | None = None,
|
||||
) -> np.ndarray:
|
||||
"""Keep only points inside ``bbox``.
|
||||
|
||||
``bbox`` is ``((xmin, xmax), (ymin, ymax), (zmin, zmax))``.
|
||||
|
||||
If ``bbox`` is ``None`` and ``bbox_padding_mm`` is given, the bbox is
|
||||
derived from the cloud itself, padded by ``bbox_padding_mm`` on each
|
||||
side — which is a useful no-op default that nonetheless drops far-out
|
||||
NaNs / spurious points.
|
||||
"""
|
||||
pts = np.asarray(points, dtype=np.float64)
|
||||
if bbox is None:
|
||||
if bbox_padding_mm is None:
|
||||
return pts.copy()
|
||||
pad = float(bbox_padding_mm)
|
||||
lo = pts.min(axis=0) - pad
|
||||
hi = pts.max(axis=0) + pad
|
||||
bbox = ((lo[0], hi[0]), (lo[1], hi[1]), (lo[2], hi[2]))
|
||||
|
||||
(xlo, xhi), (ylo, yhi), (zlo, zhi) = bbox
|
||||
mask = (
|
||||
(pts[:, 0] >= xlo) & (pts[:, 0] <= xhi)
|
||||
& (pts[:, 1] >= ylo) & (pts[:, 1] <= yhi)
|
||||
& (pts[:, 2] >= zlo) & (pts[:, 2] <= zhi)
|
||||
)
|
||||
return pts[mask]
|
||||
|
||||
|
||||
__all__ = ["passthrough_filter"]
|
||||
@@ -0,0 +1,71 @@
|
||||
"""Compose denoising steps from a YAML-style step list.
|
||||
|
||||
Example config::
|
||||
|
||||
denoise:
|
||||
steps:
|
||||
- type: passthrough
|
||||
bbox_padding_mm: 50.0
|
||||
- type: voxel
|
||||
voxel_mm: 0.5
|
||||
- type: statistical
|
||||
nb_neighbors: 30
|
||||
std_ratio: 2.0
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
from typing import Any, Callable
|
||||
|
||||
import numpy as np
|
||||
|
||||
from .passthrough import passthrough_filter
|
||||
from .radius import radius_outlier_removal
|
||||
from .smoothing import mls_smoothing
|
||||
from .statistical import statistical_outlier_removal
|
||||
from .voxel import voxel_downsample
|
||||
|
||||
|
||||
_STEP_FNS: dict[str, Callable[..., np.ndarray]] = {
|
||||
"passthrough": passthrough_filter,
|
||||
"voxel": voxel_downsample,
|
||||
"statistical": statistical_outlier_removal,
|
||||
"radius": radius_outlier_removal,
|
||||
"smoothing": mls_smoothing,
|
||||
}
|
||||
|
||||
|
||||
def run_pipeline(
|
||||
points: np.ndarray,
|
||||
steps: list[dict[str, Any]] | None,
|
||||
*,
|
||||
on_step: Callable[[str, np.ndarray], None] | None = None,
|
||||
) -> tuple[np.ndarray, list[dict[str, Any]]]:
|
||||
"""Apply each step in order and return the final cloud + a per-step log.
|
||||
|
||||
Each log entry is::
|
||||
|
||||
{"type": ..., "params": {...}, "n_in": int, "n_out": int}
|
||||
"""
|
||||
pts = np.asarray(points, dtype=np.float64)
|
||||
log: list[dict[str, Any]] = []
|
||||
if not steps:
|
||||
return pts.copy(), log
|
||||
|
||||
for raw in steps:
|
||||
cfg = dict(raw)
|
||||
kind = cfg.pop("type")
|
||||
if kind not in _STEP_FNS:
|
||||
raise ValueError(f"unknown denoise step '{kind}'; valid: {sorted(_STEP_FNS)}")
|
||||
fn = _STEP_FNS[kind]
|
||||
n_in = pts.shape[0]
|
||||
pts = fn(pts, **cfg)
|
||||
log.append(
|
||||
{"type": kind, "params": cfg, "n_in": n_in, "n_out": int(pts.shape[0])}
|
||||
)
|
||||
if on_step is not None:
|
||||
on_step(kind, pts)
|
||||
return pts, log
|
||||
|
||||
|
||||
__all__ = ["run_pipeline"]
|
||||
@@ -0,0 +1,38 @@
|
||||
"""Radius outlier removal (ROR).
|
||||
|
||||
A point is kept iff at least ``nb_points`` other points lie within
|
||||
``radius_mm``. Useful for getting rid of spray noise in scanner data.
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import numpy as np
|
||||
|
||||
|
||||
def radius_outlier_removal(
|
||||
points: np.ndarray,
|
||||
*,
|
||||
nb_points: int = 8,
|
||||
radius_mm: float = 2.0,
|
||||
) -> np.ndarray:
|
||||
pts = np.asarray(points, dtype=np.float64)
|
||||
n = pts.shape[0]
|
||||
if n == 0:
|
||||
return pts.copy()
|
||||
|
||||
try:
|
||||
from scipy.spatial import cKDTree
|
||||
|
||||
tree = cKDTree(pts)
|
||||
counts = tree.query_ball_point(pts, r=float(radius_mm), return_length=True)
|
||||
# `counts` includes the point itself; threshold accordingly.
|
||||
keep = counts >= int(nb_points) + 1
|
||||
except ImportError:
|
||||
diff = pts[:, None, :] - pts[None, :, :]
|
||||
d = np.sqrt(np.sum(diff * diff, axis=2))
|
||||
keep = (d <= float(radius_mm)).sum(axis=1) >= int(nb_points) + 1
|
||||
|
||||
return pts[keep]
|
||||
|
||||
|
||||
__all__ = ["radius_outlier_removal"]
|
||||
@@ -0,0 +1,84 @@
|
||||
"""Optional moving-least-squares (MLS) smoothing.
|
||||
|
||||
For each point, fit a local polynomial in the neighbour set's tangent
|
||||
plane and project the point onto it. Implementation here is a degree-2
|
||||
local fit using only numpy + scipy.cKDTree. For most coupler-panel work
|
||||
this stage is *off* by default — leave it disabled unless you actually
|
||||
need smoothing in addition to SOR/ROR.
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import numpy as np
|
||||
|
||||
|
||||
def mls_smoothing(
|
||||
points: np.ndarray,
|
||||
*,
|
||||
search_radius_mm: float = 2.0,
|
||||
polynomial_order: int = 2,
|
||||
min_neighbors: int = 8,
|
||||
) -> np.ndarray:
|
||||
pts = np.asarray(points, dtype=np.float64)
|
||||
n = pts.shape[0]
|
||||
if n == 0:
|
||||
return pts.copy()
|
||||
|
||||
try:
|
||||
from scipy.spatial import cKDTree
|
||||
except ImportError:
|
||||
# Without a tree this is hopelessly slow; skip safely.
|
||||
return pts.copy()
|
||||
|
||||
tree = cKDTree(pts)
|
||||
out = pts.copy()
|
||||
|
||||
for i in range(n):
|
||||
idx = tree.query_ball_point(pts[i], r=float(search_radius_mm))
|
||||
if len(idx) < min_neighbors:
|
||||
continue
|
||||
|
||||
nbh = pts[idx]
|
||||
center = nbh.mean(axis=0)
|
||||
# Local frame from PCA on the neighbourhood.
|
||||
Q = nbh - center
|
||||
cov = (Q.T @ Q) / max(len(idx) - 1, 1)
|
||||
w, V = np.linalg.eigh(cov)
|
||||
# Smallest eigenvector ≈ surface normal; the other two span the tangent.
|
||||
normal = V[:, 0]
|
||||
tangents = V[:, 1:]
|
||||
|
||||
local_xy = Q @ tangents # (k, 2)
|
||||
local_z = Q @ normal # (k,)
|
||||
|
||||
if polynomial_order == 1:
|
||||
A = np.column_stack([np.ones(len(idx)), local_xy])
|
||||
else:
|
||||
x, y = local_xy[:, 0], local_xy[:, 1]
|
||||
A = np.column_stack([np.ones(len(idx)), x, y, x * x, x * y, y * y])
|
||||
|
||||
try:
|
||||
coef, *_ = np.linalg.lstsq(A, local_z, rcond=None)
|
||||
except np.linalg.LinAlgError:
|
||||
continue
|
||||
|
||||
# Project pts[i] onto the surface in local frame.
|
||||
rel = pts[i] - center
|
||||
xy_i = np.array([rel @ tangents[:, 0], rel @ tangents[:, 1]])
|
||||
if polynomial_order == 1:
|
||||
z_i = coef[0] + coef[1] * xy_i[0] + coef[2] * xy_i[1]
|
||||
else:
|
||||
z_i = (
|
||||
coef[0]
|
||||
+ coef[1] * xy_i[0]
|
||||
+ coef[2] * xy_i[1]
|
||||
+ coef[3] * xy_i[0] ** 2
|
||||
+ coef[4] * xy_i[0] * xy_i[1]
|
||||
+ coef[5] * xy_i[1] ** 2
|
||||
)
|
||||
out[i] = center + tangents[:, 0] * xy_i[0] + tangents[:, 1] * xy_i[1] + normal * z_i
|
||||
|
||||
return out
|
||||
|
||||
|
||||
__all__ = ["mls_smoothing"]
|
||||
@@ -0,0 +1,44 @@
|
||||
"""Statistical outlier removal (SOR).
|
||||
|
||||
For each point, compute the mean distance to its ``k`` nearest neighbours.
|
||||
A point whose mean distance exceeds the global mean by ``std_ratio`` ×
|
||||
the global std is dropped.
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import numpy as np
|
||||
|
||||
|
||||
def statistical_outlier_removal(
|
||||
points: np.ndarray,
|
||||
*,
|
||||
nb_neighbors: int = 30,
|
||||
std_ratio: float = 2.0,
|
||||
) -> np.ndarray:
|
||||
pts = np.asarray(points, dtype=np.float64)
|
||||
n = pts.shape[0]
|
||||
if n <= nb_neighbors + 1:
|
||||
return pts.copy()
|
||||
|
||||
try:
|
||||
from scipy.spatial import cKDTree
|
||||
|
||||
tree = cKDTree(pts)
|
||||
dist, _ = tree.query(pts, k=nb_neighbors + 1)
|
||||
# First column is the point itself (distance 0).
|
||||
mean_d = dist[:, 1:].mean(axis=1)
|
||||
except ImportError:
|
||||
# Plain numpy fallback (O(N²); fine for N up to a few thousand).
|
||||
diff = pts[:, None, :] - pts[None, :, :]
|
||||
d = np.sqrt(np.sum(diff * diff, axis=2))
|
||||
d.sort(axis=1)
|
||||
mean_d = d[:, 1 : nb_neighbors + 1].mean(axis=1)
|
||||
|
||||
mu = mean_d.mean()
|
||||
sigma = mean_d.std()
|
||||
keep = mean_d <= mu + float(std_ratio) * sigma
|
||||
return pts[keep]
|
||||
|
||||
|
||||
__all__ = ["statistical_outlier_removal"]
|
||||
@@ -0,0 +1,37 @@
|
||||
"""Voxel-grid downsampling."""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import numpy as np
|
||||
|
||||
|
||||
def voxel_downsample(points: np.ndarray, *, voxel_mm: float) -> np.ndarray:
|
||||
"""Replace each occupied voxel with the centroid of its points."""
|
||||
if voxel_mm <= 0:
|
||||
raise ValueError("voxel_mm must be > 0")
|
||||
pts = np.asarray(points, dtype=np.float64)
|
||||
if pts.shape[0] == 0:
|
||||
return pts.copy()
|
||||
|
||||
keys = np.floor(pts / float(voxel_mm)).astype(np.int64)
|
||||
# Compose a single 1-D key for fast unique() grouping.
|
||||
kmin = keys.min(axis=0)
|
||||
keys = keys - kmin
|
||||
span = keys.max(axis=0) + 1
|
||||
flat = keys[:, 0] + span[0] * (keys[:, 1] + span[1] * keys[:, 2])
|
||||
|
||||
order = np.argsort(flat, kind="stable")
|
||||
flat_sorted = flat[order]
|
||||
pts_sorted = pts[order]
|
||||
|
||||
# Group by run-length on the sorted flat keys.
|
||||
boundaries = np.concatenate(([0], np.where(np.diff(flat_sorted) != 0)[0] + 1, [pts.shape[0]]))
|
||||
n_groups = len(boundaries) - 1
|
||||
out = np.empty((n_groups, 3), dtype=np.float64)
|
||||
for i in range(n_groups):
|
||||
s, e = boundaries[i], boundaries[i + 1]
|
||||
out[i] = pts_sorted[s:e].mean(axis=0)
|
||||
return out
|
||||
|
||||
|
||||
__all__ = ["voxel_downsample"]
|
||||
@@ -0,0 +1,36 @@
|
||||
"""Four-hole detection on a panel point cloud.
|
||||
|
||||
Pipeline (see ``detect_four_holes``)::
|
||||
|
||||
1. plane_local.project_to_panel(...) # cloud -> 2D coords
|
||||
2. occupancy_grid.build(...) # 2D grid + connected components
|
||||
3. occupancy_grid.find_holes(...) # holes (cavities) inside main panel CC
|
||||
4. circle_fit.fit_circle(...) # boundary point fit
|
||||
5. four_hole_selector.select(...) # keep the 4 best, ordered
|
||||
"""
|
||||
|
||||
from .plane_local import LocalPanelFrame, project_to_panel, lift_back
|
||||
from .circle_fit import fit_circle, fit_circle_lsq
|
||||
from .occupancy_grid import (
|
||||
OccupancyGrid,
|
||||
HoleCandidate,
|
||||
build_grid,
|
||||
find_panel_holes,
|
||||
)
|
||||
from .four_hole_selector import select_four_holes
|
||||
from .detect_holes import detect_four_holes, FourHoleDetection
|
||||
|
||||
__all__ = [
|
||||
"LocalPanelFrame",
|
||||
"project_to_panel",
|
||||
"lift_back",
|
||||
"fit_circle",
|
||||
"fit_circle_lsq",
|
||||
"OccupancyGrid",
|
||||
"HoleCandidate",
|
||||
"build_grid",
|
||||
"find_panel_holes",
|
||||
"select_four_holes",
|
||||
"detect_four_holes",
|
||||
"FourHoleDetection",
|
||||
]
|
||||
@@ -0,0 +1,114 @@
|
||||
"""Derive scale/density-dependent config from CAD geometry + cloud spacing.
|
||||
|
||||
This is a **configuration helper**, not part of the measurement algorithm.
|
||||
The denoise, detection, ROI and plane-fit algorithms are all unchanged;
|
||||
this module only fills in the knobs whose *correct value depends on the
|
||||
panel size and the point spacing*, so that the "CAD STP + cloud + ROI"
|
||||
input contract is self-sufficient and does not need a hand-tuned profile
|
||||
per panel.
|
||||
|
||||
Why it is needed
|
||||
----------------
|
||||
|
||||
The shipped ``default.yaml`` is tuned for a small, densely-sampled panel.
|
||||
Two groups of knobs break on a large, sparsely-scanned panel (e.g. the
|
||||
lu1 coupler panel: r≈16 mm locating holes, ~3-4 mm point spacing over a
|
||||
~1.5 m face):
|
||||
|
||||
1. **Denoise radius filter** ``{type: radius, nb_points: 8, radius_mm: 2}``
|
||||
demands 8 neighbours within 2 mm. At 3.7 mm spacing *every* point fails
|
||||
that test, so the whole cloud is deleted → ``too few points: 0``.
|
||||
|
||||
2. **Detection grid / area band** (``grid_pixel_mm=0.5``,
|
||||
``hole_*_area_mm2=[30,800]``, ``locating_hole_radius_mm=[4,12]``) make
|
||||
the occupancy grid too fine and the cavity-area window too small, so the
|
||||
locating holes never form detectable cavities →
|
||||
``only N hole-shaped cavities found``.
|
||||
|
||||
Two-stage application (around denoise)
|
||||
--------------------------------------
|
||||
|
||||
The two groups must be tuned at *different* moments because the point
|
||||
spacing changes across denoising:
|
||||
|
||||
- :func:`relax_denoise_steps` runs **before** denoising. It drops the
|
||||
density-fragile radius filter (the statistical filter still removes
|
||||
outliers), so a sparse cloud survives.
|
||||
- :func:`detection_overlay` runs **after** denoising, measuring the point
|
||||
spacing on the *denoised* cloud — the same cloud detection will see —
|
||||
and sizes the grid / fill / area band from the CAD locating-hole radius
|
||||
plus that spacing.
|
||||
|
||||
Together they reproduce, to within a few percent, the values previously
|
||||
hand-tuned in ``configs/profiles/lu1_af4.yaml``.
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import copy
|
||||
from typing import Any
|
||||
|
||||
import numpy as np
|
||||
|
||||
|
||||
def median_nn_spacing(points: np.ndarray, *, max_samples: int = 5000) -> float:
|
||||
"""Median nearest-neighbour distance, sub-sampled for speed (deterministic)."""
|
||||
pts = np.asarray(points, dtype=np.float64)
|
||||
n = pts.shape[0]
|
||||
if n < 2:
|
||||
return 1.0
|
||||
try:
|
||||
from scipy.spatial import cKDTree
|
||||
except ImportError:
|
||||
span = float(np.linalg.norm(pts.max(axis=0) - pts.min(axis=0)))
|
||||
return max(span / max(n ** (1.0 / 3.0), 1.0), 1e-6)
|
||||
|
||||
step = max(1, n // max_samples)
|
||||
probe = pts[::step]
|
||||
tree = cKDTree(pts)
|
||||
d, _ = tree.query(probe, k=2)
|
||||
return float(np.median(d[:, 1]))
|
||||
|
||||
|
||||
def relax_denoise_steps(denoise_cfg: dict[str, Any]) -> dict[str, Any]:
|
||||
"""Return a denoise cfg with the density-fragile ``radius`` filter removed.
|
||||
|
||||
The radius outlier filter deletes everything on a cloud sparser than its
|
||||
``radius_mm``; the statistical filter already handles outliers, so for an
|
||||
auto-tuned run we simply drop the radius step (this matches the proven
|
||||
lu1_af4 denoise recipe). Other steps are kept as-is.
|
||||
"""
|
||||
out = copy.deepcopy(denoise_cfg) if denoise_cfg else {}
|
||||
steps = out.get("steps", [])
|
||||
out["steps"] = [s for s in steps if s.get("type") != "radius"]
|
||||
return out
|
||||
|
||||
|
||||
def detection_overlay(locating_hole_radius_mm: float, points: np.ndarray) -> dict[str, Any]:
|
||||
"""Detection sub-config derived from locating-hole radius + DENOISED spacing.
|
||||
|
||||
``points`` must be the cloud detection will actually run on (i.e. the
|
||||
denoised cloud), because the fill-gap threshold is sized from its point
|
||||
spacing.
|
||||
"""
|
||||
r = float(locating_hole_radius_mm)
|
||||
nn = median_nn_spacing(points)
|
||||
# Grid fine enough to resolve a locating hole (~16 px across its
|
||||
# diameter), but never finer than 0.5 mm.
|
||||
pixel = round(max(r / 8.0, 0.5), 2)
|
||||
# Fill background gaps up to a few point-spacings so sub-sampled panel
|
||||
# regions read as solid rather than as spurious cavities.
|
||||
fill_bg = round((2.5 * nn) ** 2, 1)
|
||||
a_loc = np.pi * r * r
|
||||
return {
|
||||
"grid_pixel_mm": pixel,
|
||||
"fill_bg_below_mm2": fill_bg,
|
||||
"panel_min_area_ratio": 0.15,
|
||||
"hole_min_area_mm2": round(0.30 * a_loc, 1),
|
||||
"hole_max_area_mm2": round(2.2 * a_loc, 1),
|
||||
"locating_hole_radius_mm": [round(r * 0.80, 2), round(r * 1.45, 2)],
|
||||
"panel_normal_seed": "pca",
|
||||
}
|
||||
|
||||
|
||||
__all__ = ["median_nn_spacing", "relax_denoise_steps", "detection_overlay"]
|
||||
@@ -0,0 +1,106 @@
|
||||
"""2D circle fitting.
|
||||
|
||||
Two routines:
|
||||
|
||||
- :func:`fit_circle_algebraic` — Pratt-style algebraic fit (closed-form).
|
||||
Fast, biased, but a great initial guess.
|
||||
- :func:`fit_circle_lsq` — Levenberg-Marquardt geometric fit.
|
||||
Unbiased, requires an initial guess.
|
||||
- :func:`fit_circle` — algebraic → geometric, returns the result
|
||||
along with the per-point residuals.
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
from dataclasses import dataclass
|
||||
|
||||
import numpy as np
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class CircleFit:
|
||||
cx: float
|
||||
cy: float
|
||||
radius: float
|
||||
residual_rms_mm: float
|
||||
residual_max_mm: float
|
||||
n_points: int
|
||||
|
||||
|
||||
def fit_circle_algebraic(uv: np.ndarray) -> tuple[float, float, float]:
|
||||
"""Return (cx, cy, radius) by Pratt's algebraic fit."""
|
||||
if uv.shape[0] < 3:
|
||||
raise ValueError("need ≥ 3 points to fit a circle")
|
||||
x = uv[:, 0]
|
||||
y = uv[:, 1]
|
||||
A = np.column_stack([2.0 * x, 2.0 * y, np.ones_like(x)])
|
||||
b = x ** 2 + y ** 2
|
||||
sol, *_ = np.linalg.lstsq(A, b, rcond=None)
|
||||
cx, cy, c = sol
|
||||
r2 = c + cx ** 2 + cy ** 2
|
||||
if r2 <= 0:
|
||||
raise ValueError("algebraic fit produced non-positive squared radius")
|
||||
return float(cx), float(cy), float(np.sqrt(r2))
|
||||
|
||||
|
||||
def fit_circle_lsq(
|
||||
uv: np.ndarray,
|
||||
*,
|
||||
init: tuple[float, float, float] | None = None,
|
||||
max_iter: int = 30,
|
||||
tol: float = 1e-8,
|
||||
) -> tuple[float, float, float]:
|
||||
"""Geometric (orthogonal-residual) least-squares circle fit.
|
||||
|
||||
Uses Gauss–Newton on f_i(p) = sqrt((x_i-cx)² + (y_i-cy)²) - r.
|
||||
"""
|
||||
if init is None:
|
||||
cx, cy, r = fit_circle_algebraic(uv)
|
||||
else:
|
||||
cx, cy, r = init
|
||||
|
||||
x = uv[:, 0]
|
||||
y = uv[:, 1]
|
||||
for _ in range(max_iter):
|
||||
dx = x - cx
|
||||
dy = y - cy
|
||||
d = np.sqrt(dx * dx + dy * dy)
|
||||
d_safe = np.where(d > 1e-12, d, 1e-12)
|
||||
# Residual.
|
||||
f = d - r
|
||||
# Jacobian wrt (cx, cy, r).
|
||||
J = np.column_stack([-dx / d_safe, -dy / d_safe, -np.ones_like(d)])
|
||||
try:
|
||||
delta, *_ = np.linalg.lstsq(J, -f, rcond=None)
|
||||
except np.linalg.LinAlgError:
|
||||
break
|
||||
cx += delta[0]
|
||||
cy += delta[1]
|
||||
r += delta[2]
|
||||
if np.max(np.abs(delta)) < tol:
|
||||
break
|
||||
return float(cx), float(cy), float(r)
|
||||
|
||||
|
||||
def fit_circle(uv: np.ndarray, *, refine: bool = True) -> CircleFit:
|
||||
cx, cy, r = fit_circle_algebraic(uv)
|
||||
if refine:
|
||||
cx, cy, r = fit_circle_lsq(uv, init=(cx, cy, r))
|
||||
d = np.sqrt((uv[:, 0] - cx) ** 2 + (uv[:, 1] - cy) ** 2)
|
||||
res = d - r
|
||||
return CircleFit(
|
||||
cx=cx,
|
||||
cy=cy,
|
||||
radius=r,
|
||||
residual_rms_mm=float(np.sqrt(np.mean(res ** 2))),
|
||||
residual_max_mm=float(np.max(np.abs(res))),
|
||||
n_points=int(uv.shape[0]),
|
||||
)
|
||||
|
||||
|
||||
__all__ = [
|
||||
"CircleFit",
|
||||
"fit_circle",
|
||||
"fit_circle_algebraic",
|
||||
"fit_circle_lsq",
|
||||
]
|
||||
@@ -0,0 +1,154 @@
|
||||
"""High-level orchestration: cloud → 4 ordered hole centres in 3D."""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
from dataclasses import dataclass
|
||||
from typing import Any
|
||||
|
||||
import numpy as np
|
||||
|
||||
from ..exceptions import HoleDetectionError
|
||||
from ..structures import Hole, HoleSet
|
||||
from .circle_fit import fit_circle
|
||||
from .four_hole_selector import CircleCandidate, select_four_holes
|
||||
from .occupancy_grid import build_grid, find_panel_holes
|
||||
from .plane_local import LocalPanelFrame, project_to_panel
|
||||
|
||||
|
||||
@dataclass
|
||||
class FourHoleDetection:
|
||||
holes_3d: HoleSet
|
||||
frame: LocalPanelFrame
|
||||
candidates: list[CircleCandidate]
|
||||
diagnostics: dict[str, Any]
|
||||
|
||||
|
||||
def detect_four_holes(
|
||||
points: np.ndarray,
|
||||
*,
|
||||
detection_cfg: dict[str, Any],
|
||||
cad_cfg: dict[str, Any],
|
||||
panel_normal_seed: np.ndarray | None = None,
|
||||
) -> FourHoleDetection:
|
||||
"""Detect the four locating-hole centres in 3D from a panel point cloud."""
|
||||
pts = np.asarray(points, dtype=np.float64)
|
||||
if pts.shape[0] < 100:
|
||||
raise HoleDetectionError(f"too few points for hole detection: {pts.shape[0]}")
|
||||
|
||||
# 1. Project onto local panel plane.
|
||||
seed_mode = detection_cfg.get("panel_normal_seed", "from_cad")
|
||||
if panel_normal_seed is not None and seed_mode != "pca":
|
||||
seed = np.asarray(panel_normal_seed, dtype=np.float64)
|
||||
else:
|
||||
seed = "pca"
|
||||
frame, uv = project_to_panel(pts, normal_seed=seed)
|
||||
|
||||
# 2. Build 2D occupancy grid.
|
||||
pixel_mm = float(detection_cfg["grid_pixel_mm"])
|
||||
closing_iters = int(detection_cfg.get("closing_iters", 0))
|
||||
fill_bg_below = float(detection_cfg.get("fill_bg_below_mm2", 0.0))
|
||||
grid = build_grid(
|
||||
uv,
|
||||
pixel_mm=pixel_mm,
|
||||
closing_iters=closing_iters,
|
||||
fill_bg_below_mm2=fill_bg_below,
|
||||
)
|
||||
|
||||
# 3. Find panel + hole cavities.
|
||||
panel_mask, holes = find_panel_holes(
|
||||
grid,
|
||||
panel_min_area_ratio=float(detection_cfg.get("panel_min_area_ratio", 0.4)),
|
||||
hole_min_area_mm2=float(detection_cfg.get("hole_min_area_mm2", 30.0)),
|
||||
hole_max_area_mm2=float(detection_cfg.get("hole_max_area_mm2", 800.0)),
|
||||
point_uv=uv,
|
||||
)
|
||||
if len(holes) < 4:
|
||||
raise HoleDetectionError(
|
||||
f"only {len(holes)} hole-shaped cavities found in occupancy grid; "
|
||||
"consider lowering grid_pixel_mm or relaxing hole_*_area_mm2"
|
||||
)
|
||||
|
||||
# 4. Fit a circle to each cavity boundary.
|
||||
candidates: list[CircleCandidate] = []
|
||||
for h in holes:
|
||||
try:
|
||||
cf = fit_circle(h.boundary_uv)
|
||||
except Exception:
|
||||
continue
|
||||
candidates.append(
|
||||
CircleCandidate(
|
||||
fit=cf,
|
||||
centroid_uv=h.centroid_uv,
|
||||
area_mm2=h.area_mm2,
|
||||
n_boundary_points=int(h.boundary_uv.shape[0]),
|
||||
)
|
||||
)
|
||||
if len(candidates) < 4:
|
||||
raise HoleDetectionError(
|
||||
f"only {len(candidates)} circles fitted (need ≥ 4)"
|
||||
)
|
||||
|
||||
# 5. Pick the 4 most rectangle-like, equal-radius circles.
|
||||
# Detection can override the CAD-side radius range (rasterisation
|
||||
# biases the fitted radius outwards).
|
||||
r_lo, r_hi = detection_cfg.get(
|
||||
"locating_hole_radius_mm",
|
||||
cad_cfg["locating_hole_radius_mm"],
|
||||
)
|
||||
ordered, sel_idx, diag = select_four_holes(
|
||||
candidates,
|
||||
target_radius_lo_mm=float(r_lo),
|
||||
target_radius_hi_mm=float(r_hi),
|
||||
rectangle_aspect_tol=float(
|
||||
detection_cfg.get(
|
||||
"rectangle_aspect_tol",
|
||||
cad_cfg.get("rectangle_aspect_tol", 0.20),
|
||||
)
|
||||
),
|
||||
)
|
||||
|
||||
# 6. Lift the four hole centres back to 3D (signed_distance = 0).
|
||||
centers_uv = np.array([(c.fit.cx, c.fit.cy) for c in ordered])
|
||||
centers_3d = frame.from_2d(centers_uv, signed_distance=0.0)
|
||||
|
||||
# 7. Identify a central hole (radius ≥ central_hole_radius_min_mm).
|
||||
central_hole = None
|
||||
central_min = float(cad_cfg.get("central_hole_radius_min_mm", 20.0))
|
||||
for cand in candidates:
|
||||
if cand.fit.radius >= central_min:
|
||||
uv_c = np.array([[cand.fit.cx, cand.fit.cy]])
|
||||
xyz_c = frame.from_2d(uv_c, signed_distance=0.0)[0]
|
||||
central_hole = Hole(
|
||||
center=tuple(xyz_c.tolist()),
|
||||
radius_mm=float(cand.fit.radius),
|
||||
fit_residual_rms_mm=float(cand.fit.residual_rms_mm),
|
||||
role="central",
|
||||
)
|
||||
break
|
||||
|
||||
holes_3d = HoleSet(
|
||||
locating=tuple( # type: ignore[arg-type]
|
||||
Hole(
|
||||
center=tuple(centers_3d[i].tolist()),
|
||||
radius_mm=float(c.fit.radius),
|
||||
fit_residual_rms_mm=float(c.fit.residual_rms_mm),
|
||||
role="locating",
|
||||
)
|
||||
for i, c in enumerate(ordered)
|
||||
),
|
||||
central=central_hole,
|
||||
)
|
||||
|
||||
return FourHoleDetection(
|
||||
holes_3d=holes_3d,
|
||||
frame=frame,
|
||||
candidates=candidates,
|
||||
diagnostics={
|
||||
"n_cavities": len(holes),
|
||||
"n_circle_fits": len(candidates),
|
||||
"selection": diag,
|
||||
},
|
||||
)
|
||||
|
||||
|
||||
__all__ = ["FourHoleDetection", "detect_four_holes"]
|
||||
@@ -0,0 +1,145 @@
|
||||
"""Pick four locating holes out of a list of fitted-circle candidates.
|
||||
|
||||
Given more than four candidates, choose the 4-subset that:
|
||||
|
||||
1. Has the smallest standard deviation of radius (the four locating
|
||||
holes are equal-radius by spec).
|
||||
2. Forms a near-rectangle (small `rectangle_score`, see CAD GT module).
|
||||
3. Each radius is within ``[r_lo, r_hi]``.
|
||||
|
||||
We score the cartesian product of "best radii" and "best rectangle"
|
||||
jointly, weighted.
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
from dataclasses import dataclass
|
||||
from itertools import combinations
|
||||
|
||||
import numpy as np
|
||||
|
||||
from .circle_fit import CircleFit
|
||||
|
||||
|
||||
@dataclass
|
||||
class CircleCandidate:
|
||||
"""A candidate locating hole, in the panel's local 2D frame."""
|
||||
|
||||
fit: CircleFit
|
||||
centroid_uv: tuple[float, float] # from occupancy grid (used as initial guess)
|
||||
area_mm2: float
|
||||
n_boundary_points: int
|
||||
|
||||
|
||||
def _rectangle_score(centers_uv: np.ndarray) -> float:
|
||||
"""0 = perfect rectangle. Larger = worse."""
|
||||
if centers_uv.shape[0] != 4:
|
||||
return float("inf")
|
||||
d = sorted(
|
||||
float(np.linalg.norm(centers_uv[i] - centers_uv[j]))
|
||||
for i, j in combinations(range(4), 2)
|
||||
)
|
||||
if d[0] < 1e-6:
|
||||
return 1e6
|
||||
s1, s2 = d[0], d[1]
|
||||
l1, l2 = d[2], d[3]
|
||||
g1, g2 = d[4], d[5]
|
||||
return (
|
||||
abs(s1 - s2) / max(s1, s2)
|
||||
+ abs(l1 - l2) / max(l1, l2)
|
||||
+ abs(g1 - g2) / max(g1, g2)
|
||||
)
|
||||
|
||||
|
||||
def _radius_score(radii: np.ndarray, target_lo: float, target_hi: float) -> float:
|
||||
if np.any(radii < target_lo) or np.any(radii > target_hi):
|
||||
return float("inf")
|
||||
if radii.mean() <= 0:
|
||||
return float("inf")
|
||||
return float(radii.std() / max(radii.mean(), 1e-6))
|
||||
|
||||
|
||||
def _order_quadrants(centers_uv: np.ndarray, candidates) -> tuple[list, list[int]]:
|
||||
"""Order 4 candidates as (-u/-v, +u/-v, +u/+v, -u/+v) in the local frame."""
|
||||
cu = centers_uv[:, 0].mean()
|
||||
cv = centers_uv[:, 1].mean()
|
||||
quadrant: dict[tuple[bool, bool], int] = {}
|
||||
for idx, p in enumerate(centers_uv):
|
||||
quadrant[(p[0] >= cu, p[1] >= cv)] = idx
|
||||
order = [(False, False), (True, False), (True, True), (False, True)]
|
||||
if not all(q in quadrant for q in order):
|
||||
# Degenerate, fall back to angular sort.
|
||||
ang = np.arctan2(centers_uv[:, 1] - cv, centers_uv[:, 0] - cu)
|
||||
idxs = np.argsort(ang).tolist()
|
||||
return [candidates[i] for i in idxs], idxs
|
||||
idxs = [quadrant[q] for q in order]
|
||||
return [candidates[i] for i in idxs], idxs
|
||||
|
||||
|
||||
def select_four_holes(
|
||||
candidates: list[CircleCandidate],
|
||||
*,
|
||||
target_radius_lo_mm: float,
|
||||
target_radius_hi_mm: float,
|
||||
rectangle_aspect_tol: float = 0.20,
|
||||
radius_weight: float = 1.0,
|
||||
rectangle_weight: float = 1.0,
|
||||
) -> tuple[list[CircleCandidate], list[int], dict]:
|
||||
"""Pick the best 4-subset; return (ordered subset, indices into input, diag).
|
||||
|
||||
Raises ``RuntimeError`` if no valid 4-subset exists.
|
||||
"""
|
||||
n = len(candidates)
|
||||
if n < 4:
|
||||
raise RuntimeError(f"need ≥ 4 candidates, got {n}")
|
||||
|
||||
best_idx: tuple[int, ...] | None = None
|
||||
best_score = float("inf")
|
||||
best_diag: dict = {}
|
||||
|
||||
for combo in combinations(range(n), 4):
|
||||
radii = np.array([candidates[i].fit.radius for i in combo])
|
||||
rs = _radius_score(radii, target_radius_lo_mm, target_radius_hi_mm)
|
||||
if not np.isfinite(rs):
|
||||
continue
|
||||
centers = np.array([candidates[i].fit.cx for i in combo]), np.array(
|
||||
[candidates[i].fit.cy for i in combo]
|
||||
)
|
||||
centers_uv = np.column_stack(centers)
|
||||
rect = _rectangle_score(centers_uv)
|
||||
score = radius_weight * rs + rectangle_weight * rect
|
||||
if score < best_score:
|
||||
best_score = score
|
||||
best_idx = combo
|
||||
best_diag = {
|
||||
"radius_score": rs,
|
||||
"rectangle_score": rect,
|
||||
"score": score,
|
||||
"radii_mean": float(radii.mean()),
|
||||
"radii_std": float(radii.std()),
|
||||
}
|
||||
|
||||
if best_idx is None:
|
||||
raise RuntimeError(
|
||||
"no valid 4-subset (radius constraints excluded all combinations); "
|
||||
f"target_radius=[{target_radius_lo_mm}, {target_radius_hi_mm}], "
|
||||
f"got {sorted(round(c.fit.radius, 3) for c in candidates)}"
|
||||
)
|
||||
|
||||
subset = [candidates[i] for i in best_idx]
|
||||
centers_uv = np.array([(c.fit.cx, c.fit.cy) for c in subset])
|
||||
ordered, perm = _order_quadrants(centers_uv, subset)
|
||||
final_idx = [best_idx[p] for p in perm]
|
||||
best_diag["selected_indices"] = list(final_idx)
|
||||
best_diag["selected_radii"] = [c.fit.radius for c in ordered]
|
||||
|
||||
if best_diag["rectangle_score"] > rectangle_aspect_tol:
|
||||
best_diag.setdefault("warnings", []).append("rectangle_score_above_tol")
|
||||
|
||||
return ordered, final_idx, best_diag
|
||||
|
||||
|
||||
__all__ = [
|
||||
"CircleCandidate",
|
||||
"select_four_holes",
|
||||
]
|
||||
@@ -0,0 +1,129 @@
|
||||
"""Isolate a single panel layer from a multi-layer ROI cloud.
|
||||
|
||||
A ROI box cut out of a real vehicle scan can slice through several
|
||||
structures stacked along the panel normal — e.g. the big base plate, a
|
||||
raised central mounting plate, and a roof truss behind them. The 4-hole
|
||||
detector and the plane fit both assume a *single* thin plate; feeding
|
||||
them the multi-layer cloud makes holes fill in (back-layer points project
|
||||
into the front holes) and pollutes the plane fit.
|
||||
|
||||
This module keeps only the points belonging to one layer along the panel
|
||||
normal, selected by a 1-D histogram of signed distance to a robust plane
|
||||
seed (PCA normal by default, or a supplied CAD normal).
|
||||
|
||||
Pure geometry — it does not touch the detection / plane-fit algorithms.
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
from dataclasses import dataclass
|
||||
from typing import Any
|
||||
|
||||
import numpy as np
|
||||
|
||||
|
||||
@dataclass
|
||||
class LayerResult:
|
||||
points: np.ndarray # kept layer points
|
||||
normal: np.ndarray # (3,) unit normal used, n_x > 0
|
||||
center_signed: float # signed-distance center of the kept layer
|
||||
half_width_mm: float
|
||||
diagnostics: dict[str, Any]
|
||||
|
||||
|
||||
def _pca_normal(points: np.ndarray) -> np.ndarray:
|
||||
c = points - points.mean(axis=0)
|
||||
cov = (c.T @ c) / max(points.shape[0] - 1, 1)
|
||||
_, V = np.linalg.eigh(cov)
|
||||
n = V[:, 0]
|
||||
if n[0] < 0:
|
||||
n = -n
|
||||
return n / max(np.linalg.norm(n), 1e-12)
|
||||
|
||||
|
||||
def select_panel_layer(
|
||||
points: np.ndarray,
|
||||
*,
|
||||
normal_seed: np.ndarray | str = "pca",
|
||||
half_width_mm: float = 6.0,
|
||||
bins: int = 240,
|
||||
pick: str = "frontmost",
|
||||
) -> LayerResult:
|
||||
"""Keep one layer of ``points`` along the panel normal.
|
||||
|
||||
Parameters
|
||||
----------
|
||||
normal_seed : (3,) or "pca"
|
||||
Plane normal used to compute signed distances. ``"pca"`` estimates
|
||||
it from the cloud (smallest-variance axis).
|
||||
half_width_mm : float
|
||||
Half-thickness of the kept slab around the chosen layer center.
|
||||
bins : int
|
||||
Histogram resolution for finding layer peaks.
|
||||
pick : "frontmost" | "densest"
|
||||
``"frontmost"`` keeps the layer with the largest signed distance
|
||||
(the face toward the scanner, +normal side) among the dominant
|
||||
peaks — this is the raised plate the user wants. ``"densest"``
|
||||
keeps the single most-populated layer.
|
||||
"""
|
||||
pts = np.asarray(points, dtype=np.float64)
|
||||
if pts.shape[0] == 0:
|
||||
raise ValueError("empty cloud")
|
||||
|
||||
if isinstance(normal_seed, str):
|
||||
if normal_seed != "pca":
|
||||
raise ValueError(f"unknown normal_seed '{normal_seed}'")
|
||||
normal = _pca_normal(pts)
|
||||
else:
|
||||
normal = np.asarray(normal_seed, dtype=np.float64).reshape(3)
|
||||
normal = normal / max(np.linalg.norm(normal), 1e-12)
|
||||
if normal[0] < 0:
|
||||
normal = -normal
|
||||
|
||||
sd = pts @ normal
|
||||
hist, edges = np.histogram(sd, bins=bins)
|
||||
centers = 0.5 * (edges[:-1] + edges[1:])
|
||||
|
||||
# Candidate layers = histogram peaks holding a meaningful share of points.
|
||||
thresh = 0.25 * hist.max()
|
||||
peak_idx = [i for i in range(len(hist)) if hist[i] >= thresh]
|
||||
# group contiguous bins into peaks; take each group's argmax center
|
||||
groups: list[list[int]] = []
|
||||
for i in peak_idx:
|
||||
if groups and i == groups[-1][-1] + 1:
|
||||
groups[-1].append(i)
|
||||
else:
|
||||
groups.append([i])
|
||||
peak_centers = []
|
||||
for g in groups:
|
||||
gi = max(g, key=lambda i: hist[i])
|
||||
peak_centers.append((centers[gi], int(hist[gi])))
|
||||
|
||||
if not peak_centers:
|
||||
peak_centers = [(centers[int(np.argmax(hist))], int(hist.max()))]
|
||||
|
||||
if pick == "densest":
|
||||
chosen = max(peak_centers, key=lambda c: c[1])[0]
|
||||
else: # frontmost = max signed distance (toward +normal / scanner)
|
||||
chosen = max(peak_centers, key=lambda c: c[0])[0]
|
||||
|
||||
mask = np.abs(sd - chosen) <= half_width_mm
|
||||
kept = pts[mask]
|
||||
|
||||
return LayerResult(
|
||||
points=kept,
|
||||
normal=normal,
|
||||
center_signed=float(chosen),
|
||||
half_width_mm=float(half_width_mm),
|
||||
diagnostics={
|
||||
"n_in": int(pts.shape[0]),
|
||||
"n_out": int(kept.shape[0]),
|
||||
"normal": [float(v) for v in normal],
|
||||
"layer_center_signed": float(chosen),
|
||||
"peak_centers": [[float(c), int(n)] for c, n in peak_centers],
|
||||
"pick": pick,
|
||||
},
|
||||
)
|
||||
|
||||
|
||||
__all__ = ["LayerResult", "select_panel_layer"]
|
||||
@@ -0,0 +1,350 @@
|
||||
"""2D occupancy grid + connected-component analysis on panel-projected points.
|
||||
|
||||
Steps used by :func:`detect_four_holes`:
|
||||
|
||||
1. ``build_grid`` — rasterise (u, v) points to a binary grid.
|
||||
2. ``find_panel_holes`` — find the largest connected component (= panel),
|
||||
then enumerate its internal cavities (= holes). Each cavity gets its
|
||||
boundary points (in the original 3D order) so we can fit a circle.
|
||||
|
||||
Implementation is pure numpy + a small flood-fill — no scikit-image
|
||||
dependency.
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
from dataclasses import dataclass
|
||||
|
||||
import numpy as np
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class OccupancyGrid:
|
||||
occupied: np.ndarray # (H, W) bool
|
||||
pixel_mm: float
|
||||
u_min: float
|
||||
v_min: float
|
||||
point_indices: np.ndarray # (H, W, K_max)? No — see point_pixel_idx instead
|
||||
point_pixel_idx: np.ndarray # (N, 2) int (row, col)
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class HoleCandidate:
|
||||
"""One internal cavity of the panel CC."""
|
||||
|
||||
boundary_uv: np.ndarray # (M, 2) float — boundary pixel centres
|
||||
boundary_point_idx: np.ndarray # (M,) int — indices into the original cloud, may be empty
|
||||
centroid_uv: tuple[float, float]
|
||||
area_mm2: float
|
||||
bbox_uv: tuple[float, float, float, float] # (umin, vmin, umax, vmax)
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
|
||||
|
||||
def _binary_dilate(grid: np.ndarray, iters: int = 1) -> np.ndarray:
|
||||
"""4-connected dilation, ``iters`` times. Pure numpy."""
|
||||
if iters <= 0:
|
||||
return grid.copy()
|
||||
out = grid.copy()
|
||||
for _ in range(iters):
|
||||
nxt = out.copy()
|
||||
nxt[1:, :] |= out[:-1, :]
|
||||
nxt[:-1, :] |= out[1:, :]
|
||||
nxt[:, 1:] |= out[:, :-1]
|
||||
nxt[:, :-1] |= out[:, 1:]
|
||||
out = nxt
|
||||
return out
|
||||
|
||||
|
||||
def _binary_erode(grid: np.ndarray, iters: int = 1) -> np.ndarray:
|
||||
if iters <= 0:
|
||||
return grid.copy()
|
||||
inv = ~grid
|
||||
inv = _binary_dilate(inv, iters)
|
||||
return ~inv
|
||||
|
||||
|
||||
def _binary_close(grid: np.ndarray, iters: int = 1) -> np.ndarray:
|
||||
"""Close = dilate then erode. Fills small gaps without growing the mask."""
|
||||
return _binary_erode(_binary_dilate(grid, iters), iters)
|
||||
|
||||
|
||||
def _fill_small_bg_components(grid: np.ndarray, min_keep_area_px: int) -> np.ndarray:
|
||||
"""Set every 0-component smaller than ``min_keep_area_px`` to 1.
|
||||
|
||||
Equivalent to "area opening of the background" — fills sparse-coverage
|
||||
gaps without shrinking the real cavities the caller wants to detect.
|
||||
Border-touching background components are always kept (they're outside
|
||||
the panel, not real holes).
|
||||
"""
|
||||
H, W = grid.shape
|
||||
bg_labels = _flood_fill_label(~grid)
|
||||
if bg_labels.max() == 0:
|
||||
return grid.copy()
|
||||
|
||||
# Identify border components.
|
||||
border = set()
|
||||
for j in range(W):
|
||||
if bg_labels[0, j] != 0:
|
||||
border.add(int(bg_labels[0, j]))
|
||||
if bg_labels[H - 1, j] != 0:
|
||||
border.add(int(bg_labels[H - 1, j]))
|
||||
for i in range(H):
|
||||
if bg_labels[i, 0] != 0:
|
||||
border.add(int(bg_labels[i, 0]))
|
||||
if bg_labels[i, W - 1] != 0:
|
||||
border.add(int(bg_labels[i, W - 1]))
|
||||
|
||||
counts = np.bincount(bg_labels.ravel())
|
||||
out = grid.copy()
|
||||
fill_labels = np.zeros(len(counts) + 1, dtype=bool)
|
||||
for k in range(1, len(counts)):
|
||||
if k in border:
|
||||
continue
|
||||
if counts[k] < min_keep_area_px:
|
||||
fill_labels[k] = True
|
||||
fill_mask = fill_labels[bg_labels]
|
||||
out[fill_mask] = True
|
||||
return out
|
||||
|
||||
|
||||
def build_grid(
|
||||
uv: np.ndarray,
|
||||
*,
|
||||
pixel_mm: float,
|
||||
pad_pixels: int = 4,
|
||||
closing_iters: int = 0,
|
||||
fill_bg_below_mm2: float = 0.0,
|
||||
) -> OccupancyGrid:
|
||||
"""Rasterise points to a binary grid.
|
||||
|
||||
``closing_iters`` is the number of binary-closing passes applied as a
|
||||
cheap defragmenter (also shrinks real holes by ``iters`` pixels — use
|
||||
sparingly).
|
||||
|
||||
``fill_bg_below_mm2`` is the preferred defragmenter: it fills every
|
||||
background component smaller than this area (in mm²) with foreground.
|
||||
Real cavities (holes the caller wants to detect) keep their original
|
||||
size because they exceed the threshold.
|
||||
"""
|
||||
if uv.shape[0] == 0:
|
||||
raise ValueError("empty point set")
|
||||
u_min = float(uv[:, 0].min()) - pad_pixels * pixel_mm
|
||||
v_min = float(uv[:, 1].min()) - pad_pixels * pixel_mm
|
||||
u_max = float(uv[:, 0].max()) + pad_pixels * pixel_mm
|
||||
v_max = float(uv[:, 1].max()) + pad_pixels * pixel_mm
|
||||
|
||||
W = int(np.ceil((u_max - u_min) / pixel_mm)) + 1
|
||||
H = int(np.ceil((v_max - v_min) / pixel_mm)) + 1
|
||||
|
||||
cols = np.clip(((uv[:, 0] - u_min) / pixel_mm).astype(np.int64), 0, W - 1)
|
||||
rows = np.clip(((uv[:, 1] - v_min) / pixel_mm).astype(np.int64), 0, H - 1)
|
||||
|
||||
occ = np.zeros((H, W), dtype=bool)
|
||||
occ[rows, cols] = True
|
||||
if closing_iters > 0:
|
||||
occ = _binary_close(occ, closing_iters)
|
||||
if fill_bg_below_mm2 > 0:
|
||||
min_keep_px = max(1, int(np.ceil(float(fill_bg_below_mm2) / (pixel_mm ** 2))))
|
||||
occ = _fill_small_bg_components(occ, min_keep_px)
|
||||
|
||||
return OccupancyGrid(
|
||||
occupied=occ,
|
||||
pixel_mm=float(pixel_mm),
|
||||
u_min=u_min,
|
||||
v_min=v_min,
|
||||
point_indices=np.empty(0),
|
||||
point_pixel_idx=np.column_stack([rows, cols]),
|
||||
)
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
|
||||
|
||||
def _flood_fill_label(grid: np.ndarray) -> np.ndarray:
|
||||
"""4-connected labelling. Background (False) gets label 0."""
|
||||
H, W = grid.shape
|
||||
labels = np.zeros((H, W), dtype=np.int32)
|
||||
next_label = 0
|
||||
stack: list[tuple[int, int]] = []
|
||||
for i in range(H):
|
||||
for j in range(W):
|
||||
if grid[i, j] and labels[i, j] == 0:
|
||||
next_label += 1
|
||||
stack.append((i, j))
|
||||
while stack:
|
||||
r, c = stack.pop()
|
||||
if r < 0 or r >= H or c < 0 or c >= W:
|
||||
continue
|
||||
if not grid[r, c] or labels[r, c] != 0:
|
||||
continue
|
||||
labels[r, c] = next_label
|
||||
stack.append((r - 1, c))
|
||||
stack.append((r + 1, c))
|
||||
stack.append((r, c - 1))
|
||||
stack.append((r, c + 1))
|
||||
return labels
|
||||
|
||||
|
||||
def _largest_component(labels: np.ndarray) -> int:
|
||||
if labels.max() == 0:
|
||||
return 0
|
||||
counts = np.bincount(labels.ravel())
|
||||
counts[0] = 0
|
||||
return int(np.argmax(counts))
|
||||
|
||||
|
||||
def _holes_inside(
|
||||
labels: np.ndarray,
|
||||
panel_label: int,
|
||||
) -> list[np.ndarray]:
|
||||
"""Return masks of background components fully enclosed by ``panel_label``.
|
||||
|
||||
A 'hole' is a 0-component that is not connected to the image border
|
||||
via 4-connectivity through other 0-pixels.
|
||||
"""
|
||||
H, W = labels.shape
|
||||
bg = labels == 0
|
||||
|
||||
# Label background components.
|
||||
bg_labels = _flood_fill_label(bg)
|
||||
|
||||
# Components that touch the border.
|
||||
border_components = set()
|
||||
for j in range(W):
|
||||
if bg_labels[0, j] != 0:
|
||||
border_components.add(int(bg_labels[0, j]))
|
||||
if bg_labels[H - 1, j] != 0:
|
||||
border_components.add(int(bg_labels[H - 1, j]))
|
||||
for i in range(H):
|
||||
if bg_labels[i, 0] != 0:
|
||||
border_components.add(int(bg_labels[i, 0]))
|
||||
if bg_labels[i, W - 1] != 0:
|
||||
border_components.add(int(bg_labels[i, W - 1]))
|
||||
|
||||
holes: list[np.ndarray] = []
|
||||
n_bg = int(bg_labels.max())
|
||||
for k in range(1, n_bg + 1):
|
||||
if k in border_components:
|
||||
continue
|
||||
mask = bg_labels == k
|
||||
# Sanity: at least one neighbouring pixel must belong to ``panel_label``.
|
||||
# (Otherwise it's enclosed by a different fg component, which we ignore.)
|
||||
# Quick neighbour test:
|
||||
from numpy import roll
|
||||
nbr = (
|
||||
(roll(labels, 1, axis=0) == panel_label)
|
||||
| (roll(labels, -1, axis=0) == panel_label)
|
||||
| (roll(labels, 1, axis=1) == panel_label)
|
||||
| (roll(labels, -1, axis=1) == panel_label)
|
||||
)
|
||||
if np.any(mask & nbr):
|
||||
holes.append(mask)
|
||||
return holes
|
||||
|
||||
|
||||
def _hole_boundary_pixels(panel_mask: np.ndarray, hole_mask: np.ndarray) -> np.ndarray:
|
||||
"""Pixels in ``panel_mask`` that touch a pixel in ``hole_mask``."""
|
||||
from numpy import roll
|
||||
|
||||
nbr = (
|
||||
roll(hole_mask, 1, axis=0)
|
||||
| roll(hole_mask, -1, axis=0)
|
||||
| roll(hole_mask, 1, axis=1)
|
||||
| roll(hole_mask, -1, axis=1)
|
||||
)
|
||||
return panel_mask & nbr
|
||||
|
||||
|
||||
def find_panel_holes(
|
||||
grid: OccupancyGrid,
|
||||
*,
|
||||
panel_min_area_ratio: float = 0.4,
|
||||
hole_min_area_mm2: float = 30.0,
|
||||
hole_max_area_mm2: float = 800.0,
|
||||
point_uv: np.ndarray | None = None,
|
||||
) -> tuple[np.ndarray, list[HoleCandidate]]:
|
||||
"""Return (panel_mask, list_of_holes).
|
||||
|
||||
``point_uv`` is optional but, if provided, lets us hand back the
|
||||
*original-cloud* point indices that fall inside each hole's boundary
|
||||
pixels (= proper boundary points for a 3D circle fit).
|
||||
"""
|
||||
H, W = grid.occupied.shape
|
||||
fg_labels = _flood_fill_label(grid.occupied)
|
||||
panel_label = _largest_component(fg_labels)
|
||||
if panel_label == 0:
|
||||
raise RuntimeError("no occupied pixels")
|
||||
|
||||
panel_mask = fg_labels == panel_label
|
||||
panel_area = panel_mask.sum() * grid.pixel_mm ** 2
|
||||
bbox_area = H * W * grid.pixel_mm ** 2
|
||||
if panel_area / max(bbox_area, 1e-9) < panel_min_area_ratio:
|
||||
# The panel CC is suspiciously small. We still proceed; downstream
|
||||
# code will report a quality flag.
|
||||
pass
|
||||
|
||||
holes_masks = _holes_inside(fg_labels, panel_label)
|
||||
out: list[HoleCandidate] = []
|
||||
|
||||
# Pre-compute (row, col) → list of original-cloud indices, for boundary recovery.
|
||||
pix_rc = grid.point_pixel_idx if point_uv is not None else None
|
||||
|
||||
for mask in holes_masks:
|
||||
area = float(mask.sum() * grid.pixel_mm ** 2)
|
||||
if area < hole_min_area_mm2 or area > hole_max_area_mm2:
|
||||
continue
|
||||
|
||||
bnd_mask = _hole_boundary_pixels(panel_mask, mask)
|
||||
if not bnd_mask.any():
|
||||
continue
|
||||
|
||||
bnd_rc = np.argwhere(bnd_mask) # (M, 2): (row, col)
|
||||
bnd_uv = np.column_stack(
|
||||
[
|
||||
grid.u_min + (bnd_rc[:, 1] + 0.5) * grid.pixel_mm,
|
||||
grid.v_min + (bnd_rc[:, 0] + 0.5) * grid.pixel_mm,
|
||||
]
|
||||
)
|
||||
|
||||
rows = np.argwhere(mask)
|
||||
cu = grid.u_min + (rows[:, 1] + 0.5).mean() * grid.pixel_mm
|
||||
cv = grid.v_min + (rows[:, 0] + 0.5).mean() * grid.pixel_mm
|
||||
|
||||
bbox = (
|
||||
grid.u_min + rows[:, 1].min() * grid.pixel_mm,
|
||||
grid.v_min + rows[:, 0].min() * grid.pixel_mm,
|
||||
grid.u_min + rows[:, 1].max() * grid.pixel_mm,
|
||||
grid.v_min + rows[:, 0].max() * grid.pixel_mm,
|
||||
)
|
||||
|
||||
if pix_rc is not None:
|
||||
# Map boundary pixels back to point indices.
|
||||
row_col_set = set(map(tuple, bnd_rc.tolist()))
|
||||
point_idx = np.array(
|
||||
[i for i, (r, c) in enumerate(map(tuple, pix_rc.tolist())) if (r, c) in row_col_set],
|
||||
dtype=np.int64,
|
||||
)
|
||||
else:
|
||||
point_idx = np.empty(0, dtype=np.int64)
|
||||
|
||||
out.append(
|
||||
HoleCandidate(
|
||||
boundary_uv=bnd_uv,
|
||||
boundary_point_idx=point_idx,
|
||||
centroid_uv=(float(cu), float(cv)),
|
||||
area_mm2=area,
|
||||
bbox_uv=bbox,
|
||||
)
|
||||
)
|
||||
|
||||
return panel_mask, out
|
||||
|
||||
|
||||
__all__ = [
|
||||
"OccupancyGrid",
|
||||
"HoleCandidate",
|
||||
"build_grid",
|
||||
"find_panel_holes",
|
||||
]
|
||||
@@ -0,0 +1,127 @@
|
||||
"""Build a local 2D frame on the panel and project / lift between it and 3D.
|
||||
|
||||
The panel's normal is roughly along ±x in the CAD frame, but the point
|
||||
cloud may be misaligned. We accept a ``normal_seed`` (from the GT or
|
||||
from a robust PCA on the point cloud) and orthonormalise around it.
|
||||
|
||||
Local 2D conventions
|
||||
--------------------
|
||||
|
||||
- ``u`` axis: closest to world +y inside the panel plane
|
||||
- ``v`` axis: closest to world +z inside the panel plane
|
||||
|
||||
So a top-down picture of the panel has u → right, v → up, and matches
|
||||
how the four locating holes are arranged in the y-z plane.
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
from dataclasses import dataclass
|
||||
|
||||
import numpy as np
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class LocalPanelFrame:
|
||||
origin: np.ndarray # (3,) — point on plane (typically the centroid)
|
||||
normal: np.ndarray # (3,) — unit, n_x > 0
|
||||
u_axis: np.ndarray # (3,)
|
||||
v_axis: np.ndarray # (3,)
|
||||
|
||||
def to_2d(self, points: np.ndarray) -> np.ndarray:
|
||||
"""Project ``points`` onto the panel plane in (u, v)."""
|
||||
rel = points - self.origin
|
||||
return np.column_stack([rel @ self.u_axis, rel @ self.v_axis])
|
||||
|
||||
def signed_distance(self, points: np.ndarray) -> np.ndarray:
|
||||
return (points - self.origin) @ self.normal
|
||||
|
||||
def from_2d(self, uv: np.ndarray, *, signed_distance: float = 0.0) -> np.ndarray:
|
||||
"""Lift ``(u, v)`` coords back into 3D, optionally with a normal offset."""
|
||||
uv = np.atleast_2d(uv)
|
||||
return (
|
||||
self.origin
|
||||
+ uv[:, 0:1] * self.u_axis
|
||||
+ uv[:, 1:2] * self.v_axis
|
||||
+ float(signed_distance) * self.normal
|
||||
)
|
||||
|
||||
|
||||
def _orthonormal_basis(normal: np.ndarray) -> tuple[np.ndarray, np.ndarray]:
|
||||
"""Return (u, v) so that {u, v, normal} is an RHC orthonormal basis,
|
||||
with ``u`` the in-plane vector closest to world +y.
|
||||
"""
|
||||
n = normal / max(np.linalg.norm(normal), 1e-12)
|
||||
y = np.array([0.0, 1.0, 0.0])
|
||||
u = y - np.dot(y, n) * n
|
||||
if np.linalg.norm(u) < 1e-6:
|
||||
# n is parallel to y; pick z instead.
|
||||
z = np.array([0.0, 0.0, 1.0])
|
||||
u = z - np.dot(z, n) * n
|
||||
u /= np.linalg.norm(u)
|
||||
v = np.cross(n, u)
|
||||
v /= np.linalg.norm(v)
|
||||
return u, v
|
||||
|
||||
|
||||
def _pca_normal(points: np.ndarray) -> np.ndarray:
|
||||
centroid = points.mean(axis=0)
|
||||
Q = points - centroid
|
||||
cov = (Q.T @ Q) / max(points.shape[0] - 1, 1)
|
||||
w, V = np.linalg.eigh(cov)
|
||||
# Smallest eigenvector ≈ panel normal.
|
||||
n = V[:, 0]
|
||||
if n[0] < 0:
|
||||
n = -n
|
||||
return n
|
||||
|
||||
|
||||
def project_to_panel(
|
||||
points: np.ndarray,
|
||||
*,
|
||||
normal_seed: np.ndarray | str = "pca",
|
||||
origin: np.ndarray | None = None,
|
||||
pca_kth: int = 0,
|
||||
) -> tuple[LocalPanelFrame, np.ndarray]:
|
||||
"""Project ``points`` onto a local panel frame.
|
||||
|
||||
Returns
|
||||
-------
|
||||
frame : :class:`LocalPanelFrame`
|
||||
uv : ``(N, 2)`` projected coordinates
|
||||
|
||||
Parameters
|
||||
----------
|
||||
normal_seed : str or (3,)
|
||||
``"pca"`` → estimate via PCA on the cloud (smallest variance axis).
|
||||
otherwise a 3-vector.
|
||||
origin : (3,) or None
|
||||
If ``None``, use the cloud centroid.
|
||||
"""
|
||||
pts = np.asarray(points, dtype=np.float64)
|
||||
|
||||
if isinstance(normal_seed, str):
|
||||
if normal_seed == "pca":
|
||||
normal = _pca_normal(pts)
|
||||
else:
|
||||
raise ValueError(f"unknown normal_seed string '{normal_seed}'")
|
||||
else:
|
||||
normal = np.asarray(normal_seed, dtype=np.float64).reshape(3)
|
||||
normal = normal / max(np.linalg.norm(normal), 1e-12)
|
||||
if normal[0] < 0:
|
||||
normal = -normal
|
||||
|
||||
if origin is None:
|
||||
origin = pts.mean(axis=0)
|
||||
origin = np.asarray(origin, dtype=np.float64).reshape(3)
|
||||
|
||||
u, v = _orthonormal_basis(normal)
|
||||
frame = LocalPanelFrame(origin=origin, normal=normal, u_axis=u, v_axis=v)
|
||||
return frame, frame.to_2d(pts)
|
||||
|
||||
|
||||
def lift_back(uv: np.ndarray, frame: LocalPanelFrame) -> np.ndarray:
|
||||
return frame.from_2d(uv, signed_distance=0.0)
|
||||
|
||||
|
||||
__all__ = ["LocalPanelFrame", "project_to_panel", "lift_back"]
|
||||
@@ -0,0 +1,55 @@
|
||||
"""Public exception hierarchy for coupler_panel.
|
||||
|
||||
Catch :class:`CouplerError` for any algorithm failure that should be
|
||||
surfaced to the user. Subclasses indicate what specifically went wrong so
|
||||
callers can render targeted error messages.
|
||||
|
||||
Non-fatal data-quality issues are **not** raised; they are reported via
|
||||
``CouplerMeasurement.quality_flags``.
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
|
||||
class CouplerError(Exception):
|
||||
"""Base class for all errors raised by the coupler-panel pipeline."""
|
||||
|
||||
|
||||
class InvalidPointCloudError(CouplerError):
|
||||
"""Raised when the input point cloud is malformed or unreadable."""
|
||||
|
||||
|
||||
class UnsupportedFormatError(InvalidPointCloudError):
|
||||
"""Raised when the input file extension is not supported."""
|
||||
|
||||
|
||||
class InsufficientPointsError(CouplerError):
|
||||
"""Raised when the input has too few points to run the pipeline."""
|
||||
|
||||
|
||||
class CADParseError(CouplerError):
|
||||
"""Raised when the STP file cannot be parsed or has unexpected topology."""
|
||||
|
||||
|
||||
class HoleDetectionError(CouplerError):
|
||||
"""Raised when the four locating holes cannot be reliably detected."""
|
||||
|
||||
|
||||
class RegistrationError(CouplerError):
|
||||
"""Raised when 4-hole Kabsch registration fails (e.g. degenerate pairing)."""
|
||||
|
||||
|
||||
class PlaneFitError(CouplerError):
|
||||
"""Raised when the panel plane cannot be fitted (too few inliers, etc.)."""
|
||||
|
||||
|
||||
__all__ = [
|
||||
"CouplerError",
|
||||
"InvalidPointCloudError",
|
||||
"UnsupportedFormatError",
|
||||
"InsufficientPointsError",
|
||||
"CADParseError",
|
||||
"HoleDetectionError",
|
||||
"RegistrationError",
|
||||
"PlaneFitError",
|
||||
]
|
||||
@@ -0,0 +1,11 @@
|
||||
"""Geometry construction & plane fitting for the coupler panel."""
|
||||
|
||||
from .reference import build_reference_frame
|
||||
from .roi import extract_panel_roi
|
||||
from .plane_fit import fit_panel_plane
|
||||
|
||||
__all__ = [
|
||||
"build_reference_frame",
|
||||
"extract_panel_roi",
|
||||
"fit_panel_plane",
|
||||
]
|
||||
@@ -0,0 +1,113 @@
|
||||
"""Robust panel-plane fitting.
|
||||
|
||||
Pipeline:
|
||||
|
||||
1. RANSAC: pick 3 random points, build a plane, count inliers within
|
||||
``inlier_thresh_mm``. Repeat ``ransac_iterations`` times; keep the
|
||||
best inlier set.
|
||||
2. SVD: refit the plane on the inliers (eigenvector with the smallest
|
||||
eigenvalue of the centred covariance).
|
||||
3. Sign convention: enforce ``n_x > 0``.
|
||||
|
||||
Returns a :class:`PanelPlane`.
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
from typing import Any
|
||||
|
||||
import numpy as np
|
||||
|
||||
from ..exceptions import PlaneFitError
|
||||
from ..structures import PanelPlane
|
||||
|
||||
|
||||
def _plane_from_three(p1, p2, p3) -> tuple[np.ndarray, float] | None:
|
||||
n = np.cross(p2 - p1, p3 - p1)
|
||||
norm = np.linalg.norm(n)
|
||||
if norm < 1e-9:
|
||||
return None
|
||||
n = n / norm
|
||||
d = -float(np.dot(n, p1))
|
||||
return n, d
|
||||
|
||||
|
||||
def _svd_plane(points: np.ndarray) -> tuple[np.ndarray, float]:
|
||||
centroid = points.mean(axis=0)
|
||||
Q = points - centroid
|
||||
cov = (Q.T @ Q) / max(points.shape[0] - 1, 1)
|
||||
w, V = np.linalg.eigh(cov)
|
||||
n = V[:, 0]
|
||||
n = n / max(np.linalg.norm(n), 1e-12)
|
||||
d = -float(np.dot(n, centroid))
|
||||
return n, d
|
||||
|
||||
|
||||
def fit_panel_plane(
|
||||
points: np.ndarray,
|
||||
*,
|
||||
plane_fit_cfg: dict[str, Any],
|
||||
seed: int | None = 7,
|
||||
) -> PanelPlane:
|
||||
pts = np.asarray(points, dtype=np.float64)
|
||||
n_pts = pts.shape[0]
|
||||
if n_pts < 3:
|
||||
raise PlaneFitError(f"need ≥ 3 points, got {n_pts}")
|
||||
|
||||
iterations = int(plane_fit_cfg.get("ransac_iterations", 200))
|
||||
thresh = plane_fit_cfg.get("ransac_inlier_thresh_mm", 1.5)
|
||||
if thresh in (None, "auto"):
|
||||
thresh = 1.5
|
||||
thresh = float(thresh)
|
||||
min_inlier_ratio = float(plane_fit_cfg.get("min_inlier_ratio", 0.6))
|
||||
enforce_nx_positive = bool(plane_fit_cfg.get("enforce_nx_positive", True))
|
||||
|
||||
rng = np.random.default_rng(seed)
|
||||
|
||||
best_inliers: np.ndarray | None = None
|
||||
best_count = -1
|
||||
|
||||
for _ in range(iterations):
|
||||
sample = rng.choice(n_pts, size=3, replace=False)
|
||||
plane = _plane_from_three(pts[sample[0]], pts[sample[1]], pts[sample[2]])
|
||||
if plane is None:
|
||||
continue
|
||||
n, d = plane
|
||||
dist = np.abs(pts @ n + d)
|
||||
inliers = dist <= thresh
|
||||
c = int(inliers.sum())
|
||||
if c > best_count:
|
||||
best_count = c
|
||||
best_inliers = inliers
|
||||
|
||||
if best_inliers is None or best_count < 3:
|
||||
raise PlaneFitError(
|
||||
f"RANSAC found {best_count} inliers in {iterations} iters at "
|
||||
f"thresh={thresh} mm; check noise level / threshold"
|
||||
)
|
||||
|
||||
inlier_ratio = best_count / n_pts
|
||||
inlier_pts = pts[best_inliers]
|
||||
n, d = _svd_plane(inlier_pts)
|
||||
if enforce_nx_positive and n[0] < 0:
|
||||
n = -n
|
||||
d = -d
|
||||
|
||||
residuals = inlier_pts @ n + d
|
||||
rms = float(np.sqrt(np.mean(residuals ** 2)))
|
||||
|
||||
if inlier_ratio < min_inlier_ratio:
|
||||
# Caller will log a quality flag; we still return the result.
|
||||
pass
|
||||
|
||||
centroid = inlier_pts.mean(axis=0)
|
||||
return PanelPlane(
|
||||
normal=tuple(n.tolist()),
|
||||
point_on_plane=tuple(centroid.tolist()),
|
||||
d_offset=float(d),
|
||||
inlier_ratio=float(inlier_ratio),
|
||||
inlier_residual_rms_mm=rms,
|
||||
)
|
||||
|
||||
|
||||
__all__ = ["fit_panel_plane"]
|
||||
@@ -0,0 +1,32 @@
|
||||
"""Construct the reference frame from four hole centres.
|
||||
|
||||
Given the four locating-hole centres :math:`H_1 .. H_4`:
|
||||
|
||||
- Panel centre :math:`C` = mean of the four centres (= diagonal
|
||||
intersection for a rectangle).
|
||||
- Reference line :math:`L = C + \\lambda \\hat e_x`.
|
||||
- Reference plane: :math:`z = C_z`.
|
||||
- Longitudinal mid-plane: :math:`y = C_y`.
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import numpy as np
|
||||
|
||||
from ..structures import HoleSet, ReferenceFrame
|
||||
|
||||
|
||||
def build_reference_frame(holes: HoleSet) -> ReferenceFrame:
|
||||
centers = holes.locating_centers()
|
||||
if centers.shape != (4, 3):
|
||||
raise ValueError(f"need 4 locating-hole centres, got shape {centers.shape}")
|
||||
C = centers.mean(axis=0)
|
||||
return ReferenceFrame(
|
||||
center_C=tuple(C.tolist()),
|
||||
line_direction=(1.0, 0.0, 0.0),
|
||||
reference_plane_z=float(C[2]),
|
||||
longitudinal_mid_y=float(C[1]),
|
||||
)
|
||||
|
||||
|
||||
__all__ = ["build_reference_frame"]
|
||||
@@ -0,0 +1,159 @@
|
||||
"""Extract the *clean* panel ROI from the registered point cloud.
|
||||
|
||||
Strategy:
|
||||
|
||||
1. Take points whose 2D projection falls inside the four-hole rectangle
|
||||
(shrunk inwards by ``edge_buffer_mm``).
|
||||
2. Drop points within ``hole_dilate_mm`` of any locating-hole centre
|
||||
(radius + dilation).
|
||||
3. Drop points within ``central_hole_dilate_mm`` of the central hole.
|
||||
4. Drop chamfer points (per-point normal deviates from panel normal by
|
||||
more than ``acos(chamfer_normal_dot_min)``). Normals are estimated
|
||||
on the fly via PCA on each point's k-neighbourhood.
|
||||
5. Drop z-score outliers along the panel normal.
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
from typing import Any
|
||||
|
||||
import numpy as np
|
||||
|
||||
from ..structures import HoleSet
|
||||
|
||||
|
||||
def _kth_normals(points: np.ndarray, k: int = 20) -> np.ndarray:
|
||||
"""Per-point normal via PCA on the k-NN. Returns ``(N, 3)``."""
|
||||
n = points.shape[0]
|
||||
if n < k + 1:
|
||||
return np.tile(np.array([1.0, 0.0, 0.0]), (n, 1))
|
||||
try:
|
||||
from scipy.spatial import cKDTree
|
||||
except ImportError:
|
||||
return np.tile(np.array([1.0, 0.0, 0.0]), (n, 1))
|
||||
|
||||
tree = cKDTree(points)
|
||||
_, idx = tree.query(points, k=k + 1)
|
||||
normals = np.empty((n, 3), dtype=np.float64)
|
||||
for i in range(n):
|
||||
nbh = points[idx[i, 1:]]
|
||||
c = nbh.mean(axis=0)
|
||||
Q = nbh - c
|
||||
cov = (Q.T @ Q) / max(k - 1, 1)
|
||||
try:
|
||||
w, V = np.linalg.eigh(cov)
|
||||
normals[i] = V[:, 0]
|
||||
except np.linalg.LinAlgError:
|
||||
normals[i] = np.array([1.0, 0.0, 0.0])
|
||||
return normals
|
||||
|
||||
|
||||
def _signed_to_plane(points: np.ndarray, normal: np.ndarray, offset: float) -> np.ndarray:
|
||||
"""Signed distance from each point to plane n·x + d = 0."""
|
||||
return points @ normal + offset
|
||||
|
||||
|
||||
def extract_panel_roi(
|
||||
points: np.ndarray,
|
||||
*,
|
||||
holes: HoleSet,
|
||||
panel_normal_seed: np.ndarray,
|
||||
roi_cfg: dict[str, Any],
|
||||
) -> tuple[np.ndarray, dict[str, Any]]:
|
||||
"""Return (ROI points, diagnostics dict).
|
||||
|
||||
``holes`` and ``panel_normal_seed`` are expected in the *same* frame
|
||||
as ``points`` — registration must already have happened before this
|
||||
is called.
|
||||
"""
|
||||
pts = np.asarray(points, dtype=np.float64)
|
||||
if pts.shape[0] == 0:
|
||||
return pts.copy(), {"n_in": 0, "n_out": 0, "stages": []}
|
||||
|
||||
n = np.asarray(panel_normal_seed, dtype=np.float64)
|
||||
n = n / max(np.linalg.norm(n), 1e-12)
|
||||
if n[0] < 0:
|
||||
n = -n
|
||||
|
||||
centers_3d = holes.locating_centers() # (4, 3)
|
||||
C = centers_3d.mean(axis=0)
|
||||
|
||||
# Build local 2D frame to test "inside the rectangle".
|
||||
y_world = np.array([0.0, 1.0, 0.0])
|
||||
u = y_world - np.dot(y_world, n) * n
|
||||
if np.linalg.norm(u) < 1e-6:
|
||||
u = np.array([0.0, 0.0, 1.0]) - np.dot(np.array([0.0, 0.0, 1.0]), n) * n
|
||||
u /= np.linalg.norm(u)
|
||||
v = np.cross(n, u)
|
||||
v /= np.linalg.norm(v)
|
||||
|
||||
rel = pts - C
|
||||
uv = np.column_stack([rel @ u, rel @ v])
|
||||
centers_uv = np.column_stack([(centers_3d - C) @ u, (centers_3d - C) @ v])
|
||||
|
||||
edge_buf = float(roi_cfg.get("edge_buffer_mm", 5.0))
|
||||
u_lo = centers_uv[:, 0].min() + edge_buf
|
||||
u_hi = centers_uv[:, 0].max() - edge_buf
|
||||
v_lo = centers_uv[:, 1].min() + edge_buf
|
||||
v_hi = centers_uv[:, 1].max() - edge_buf
|
||||
|
||||
stages: list[dict[str, Any]] = []
|
||||
n_in = pts.shape[0]
|
||||
|
||||
mask = (uv[:, 0] >= u_lo) & (uv[:, 0] <= u_hi) & (uv[:, 1] >= v_lo) & (uv[:, 1] <= v_hi)
|
||||
stages.append({"name": "rectangle", "n_kept": int(mask.sum())})
|
||||
|
||||
# 2. Locating hole exclusion (use 3D distance to each hole's centre,
|
||||
# with the hole's own radius + dilation).
|
||||
hole_dilate = float(roi_cfg.get("hole_dilate_mm", 3.0))
|
||||
for h in holes.locating:
|
||||
center = np.asarray(h.center)
|
||||
r = float(h.radius_mm) + hole_dilate
|
||||
d2 = np.sum((pts - center) ** 2, axis=1)
|
||||
mask &= d2 > r * r
|
||||
stages.append({"name": "locating_holes", "n_kept": int(mask.sum())})
|
||||
|
||||
# 3. Central hole exclusion.
|
||||
if holes.central is not None:
|
||||
center = np.asarray(holes.central.center)
|
||||
r = float(holes.central.radius_mm) + float(roi_cfg.get("central_hole_dilate_mm", 5.0))
|
||||
d2 = np.sum((pts - center) ** 2, axis=1)
|
||||
mask &= d2 > r * r
|
||||
stages.append({"name": "central_hole", "n_kept": int(mask.sum())})
|
||||
|
||||
# 4. Chamfer/normal filter — only run if we still have enough points to PCA.
|
||||
chamfer_dot_min = float(roi_cfg.get("chamfer_normal_dot_min", 0.85))
|
||||
if mask.sum() > 50 and chamfer_dot_min > 0:
|
||||
kept = pts[mask]
|
||||
normals = _kth_normals(kept, k=20)
|
||||
# Align all normals so they point along +n_panel (sign-invariant).
|
||||
flips = (normals @ n) < 0
|
||||
normals[flips] = -normals[flips]
|
||||
keep_local = (normals @ n) >= chamfer_dot_min
|
||||
# Map back into the full mask.
|
||||
idxs = np.where(mask)[0]
|
||||
new_mask = np.zeros_like(mask)
|
||||
new_mask[idxs[keep_local]] = True
|
||||
mask = new_mask
|
||||
stages.append({"name": "chamfer_normal", "n_kept": int(mask.sum())})
|
||||
|
||||
# 5. Z-score outlier rejection along panel normal.
|
||||
z_score = float(roi_cfg.get("outlier_z_score", 3.0))
|
||||
if mask.sum() > 50 and z_score > 0:
|
||||
kept = pts[mask]
|
||||
d = kept @ n
|
||||
mu = d.mean()
|
||||
sigma = d.std()
|
||||
if sigma > 1e-12:
|
||||
keep_local = np.abs(d - mu) <= z_score * sigma
|
||||
idxs = np.where(mask)[0]
|
||||
new_mask = np.zeros_like(mask)
|
||||
new_mask[idxs[keep_local]] = True
|
||||
mask = new_mask
|
||||
stages.append({"name": "z_score", "n_kept": int(mask.sum())})
|
||||
|
||||
out = pts[mask]
|
||||
return out, {"n_in": int(n_in), "n_out": int(out.shape[0]), "stages": stages}
|
||||
|
||||
|
||||
__all__ = ["extract_panel_roi"]
|
||||
@@ -0,0 +1,27 @@
|
||||
"""Point-cloud / artifact IO utilities for coupler_panel."""
|
||||
|
||||
from .pointcloud_io import (
|
||||
load_point_cloud,
|
||||
write_point_cloud,
|
||||
write_metadata_json,
|
||||
)
|
||||
from .ply_stream import crop_ply_bbox
|
||||
from .artifacts import (
|
||||
ensure_dir,
|
||||
write_json,
|
||||
write_yaml,
|
||||
write_csv_rows,
|
||||
Artifacts,
|
||||
)
|
||||
|
||||
__all__ = [
|
||||
"load_point_cloud",
|
||||
"write_point_cloud",
|
||||
"write_metadata_json",
|
||||
"crop_ply_bbox",
|
||||
"ensure_dir",
|
||||
"write_json",
|
||||
"write_yaml",
|
||||
"write_csv_rows",
|
||||
"Artifacts",
|
||||
]
|
||||
@@ -0,0 +1,168 @@
|
||||
"""Artifact directory layout for one pipeline run.
|
||||
|
||||
A run directory looks like::
|
||||
|
||||
runs/<ts>/
|
||||
├── 00_cad_mesh.ply
|
||||
├── 01_gt.json
|
||||
├── 02_sampled.ply
|
||||
├── 03_noisy.ply
|
||||
├── 04_transformed.ply
|
||||
├── 05_denoised.ply
|
||||
├── 06_registered.ply
|
||||
├── 07_holes.json
|
||||
├── 08_panel_roi.ply
|
||||
├── 09_plane.json
|
||||
├── 10_metrics.json
|
||||
├── images/
|
||||
├── config.resolved.yaml
|
||||
└── report.json
|
||||
|
||||
Most modules write through the :class:`Artifacts` helper so naming stays
|
||||
consistent.
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import csv
|
||||
import json
|
||||
from dataclasses import dataclass
|
||||
from pathlib import Path
|
||||
from typing import Any, Iterable, Sequence
|
||||
|
||||
import numpy as np
|
||||
import yaml
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# Generic helpers
|
||||
# ---------------------------------------------------------------------------
|
||||
|
||||
|
||||
def ensure_dir(path: str | Path) -> Path:
|
||||
p = Path(path)
|
||||
p.mkdir(parents=True, exist_ok=True)
|
||||
return p
|
||||
|
||||
|
||||
def write_json(data: Any, path: str | Path) -> Path:
|
||||
path = Path(path)
|
||||
path.parent.mkdir(parents=True, exist_ok=True)
|
||||
with path.open("w", encoding="utf-8") as fh:
|
||||
json.dump(data, fh, ensure_ascii=False, indent=2, default=_json_default)
|
||||
return path
|
||||
|
||||
|
||||
def write_yaml(data: Any, path: str | Path) -> Path:
|
||||
path = Path(path)
|
||||
path.parent.mkdir(parents=True, exist_ok=True)
|
||||
with path.open("w", encoding="utf-8") as fh:
|
||||
yaml.safe_dump(_yaml_default(data), fh, allow_unicode=True, sort_keys=False)
|
||||
return path
|
||||
|
||||
|
||||
def write_csv_rows(
|
||||
rows: Iterable[Sequence[Any]],
|
||||
path: str | Path,
|
||||
*,
|
||||
header: Sequence[str] | None = None,
|
||||
) -> Path:
|
||||
path = Path(path)
|
||||
path.parent.mkdir(parents=True, exist_ok=True)
|
||||
with path.open("w", encoding="utf-8", newline="") as fh:
|
||||
w = csv.writer(fh)
|
||||
if header is not None:
|
||||
w.writerow(header)
|
||||
for row in rows:
|
||||
w.writerow(row)
|
||||
return path
|
||||
|
||||
|
||||
def _json_default(obj: Any) -> Any:
|
||||
if isinstance(obj, np.ndarray):
|
||||
return obj.tolist()
|
||||
if isinstance(obj, (np.floating, np.integer)):
|
||||
return obj.item()
|
||||
if isinstance(obj, Path):
|
||||
return str(obj)
|
||||
raise TypeError(f"not JSON serialisable: {type(obj).__name__}")
|
||||
|
||||
|
||||
def _yaml_default(obj: Any) -> Any:
|
||||
if isinstance(obj, dict):
|
||||
return {k: _yaml_default(v) for k, v in obj.items()}
|
||||
if isinstance(obj, (list, tuple)):
|
||||
return [_yaml_default(v) for v in obj]
|
||||
if isinstance(obj, np.ndarray):
|
||||
return obj.tolist()
|
||||
if isinstance(obj, (np.floating, np.integer)):
|
||||
return obj.item()
|
||||
return obj
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# Stage-naming helper
|
||||
# ---------------------------------------------------------------------------
|
||||
|
||||
|
||||
@dataclass
|
||||
class Artifacts:
|
||||
"""Resolves stage filenames inside a run directory.
|
||||
|
||||
Use :meth:`path` to get the canonical name for a given stage:
|
||||
|
||||
>>> art = Artifacts("runs/demo")
|
||||
>>> art.path("sampled", ext="ply")
|
||||
PosixPath('runs/demo/02_sampled.ply')
|
||||
"""
|
||||
|
||||
root: Path
|
||||
|
||||
_STAGE_INDEX: dict[str, int] = None # type: ignore[assignment]
|
||||
|
||||
# Canonical stage ordering. Keys here are referenced from pipeline.py.
|
||||
STAGE_ORDER = (
|
||||
"cad_mesh", # 00
|
||||
"gt", # 01
|
||||
"sampled", # 02
|
||||
"noisy", # 03
|
||||
"transformed", # 04
|
||||
"denoised", # 05
|
||||
"registered", # 06
|
||||
"holes", # 07
|
||||
"panel_roi", # 08
|
||||
"plane", # 09
|
||||
"metrics", # 10
|
||||
)
|
||||
|
||||
def __init__(self, root: str | Path) -> None:
|
||||
self.root = ensure_dir(root)
|
||||
ensure_dir(self.root / "images")
|
||||
self._STAGE_INDEX = {name: i for i, name in enumerate(self.STAGE_ORDER)}
|
||||
|
||||
def path(self, stage: str, *, ext: str) -> Path:
|
||||
idx = self._STAGE_INDEX.get(stage)
|
||||
if idx is None:
|
||||
raise KeyError(f"unknown stage '{stage}'; valid: {list(self._STAGE_INDEX)}")
|
||||
ext = ext.lstrip(".")
|
||||
return self.root / f"{idx:02d}_{stage}.{ext}"
|
||||
|
||||
def image(self, name: str) -> Path:
|
||||
return self.root / "images" / name
|
||||
|
||||
@property
|
||||
def report_json(self) -> Path:
|
||||
return self.root / "report.json"
|
||||
|
||||
@property
|
||||
def resolved_config(self) -> Path:
|
||||
return self.root / "config.resolved.yaml"
|
||||
|
||||
|
||||
__all__ = [
|
||||
"ensure_dir",
|
||||
"write_json",
|
||||
"write_yaml",
|
||||
"write_csv_rows",
|
||||
"Artifacts",
|
||||
]
|
||||
@@ -0,0 +1,165 @@
|
||||
"""Streaming AABB crop for large binary PLY point clouds.
|
||||
|
||||
A full-vehicle scan PLY can be 1-12 GB. Loading it whole to apply a
|
||||
region-of-interest box would blow up memory, so this module parses the
|
||||
PLY header, memory-maps the vertex block, and filters it in chunks,
|
||||
keeping only points inside the ROI bounding box.
|
||||
|
||||
Supports binary little-/big-endian PLY with a single ``element vertex``
|
||||
block whose first three float properties are x, y, z (extra properties
|
||||
such as colours or normals are tolerated — the per-vertex stride is read
|
||||
from the header). ASCII PLY and other layouts fall back to the normal
|
||||
loader in :mod:`coupler_panel.io_utils.pointcloud_io`.
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
from dataclasses import dataclass
|
||||
from pathlib import Path
|
||||
from typing import Any
|
||||
|
||||
import numpy as np
|
||||
|
||||
from ..exceptions import InvalidPointCloudError
|
||||
|
||||
|
||||
# PLY property type -> (numpy dtype char, byte size)
|
||||
_PLY_TYPES = {
|
||||
"char": ("i1", 1), "int8": ("i1", 1),
|
||||
"uchar": ("u1", 1), "uint8": ("u1", 1),
|
||||
"short": ("i2", 2), "int16": ("i2", 2),
|
||||
"ushort": ("u2", 2), "uint16": ("u2", 2),
|
||||
"int": ("i4", 4), "int32": ("i4", 4),
|
||||
"uint": ("u4", 4), "uint32": ("u4", 4),
|
||||
"float": ("f4", 4), "float32": ("f4", 4),
|
||||
"double": ("f8", 8), "float64": ("f8", 8),
|
||||
}
|
||||
|
||||
|
||||
@dataclass
|
||||
class _PlyHeader:
|
||||
fmt: str # "binary_little_endian" | "binary_big_endian" | "ascii"
|
||||
n_vertices: int
|
||||
data_offset: int # byte offset where vertex data starts
|
||||
props: list[tuple[str, str]] # ordered (name, type) for the vertex element
|
||||
|
||||
|
||||
def _parse_header(path: Path) -> _PlyHeader:
|
||||
fmt = None
|
||||
n_vertices = 0
|
||||
props: list[tuple[str, str]] = []
|
||||
in_vertex = False
|
||||
# Read a generous prefix; PLY headers are tiny (well under 64 KB).
|
||||
with path.open("rb") as f:
|
||||
prefix = f.read(65536)
|
||||
marker = b"end_header"
|
||||
mpos = prefix.find(marker)
|
||||
if mpos < 0:
|
||||
raise InvalidPointCloudError(f"{path}: no end_header in first 64KB")
|
||||
# data starts right after the newline(s) following 'end_header'
|
||||
after = prefix[mpos + len(marker):]
|
||||
if after.startswith(b"\r\n"):
|
||||
nl_len = 2
|
||||
elif after.startswith(b"\n") or after.startswith(b"\r"):
|
||||
nl_len = 1
|
||||
else:
|
||||
nl_len = 0
|
||||
data_offset = mpos + len(marker) + nl_len
|
||||
|
||||
text = prefix[:mpos].decode("ascii", errors="replace")
|
||||
lines = text.splitlines()
|
||||
for ln in lines:
|
||||
parts = ln.split()
|
||||
if not parts:
|
||||
continue
|
||||
if parts[0] == "format":
|
||||
fmt = parts[1]
|
||||
elif parts[0] == "element":
|
||||
in_vertex = parts[1] == "vertex"
|
||||
if in_vertex:
|
||||
n_vertices = int(parts[2])
|
||||
elif parts[0] == "property" and in_vertex:
|
||||
# property <type> <name> (lists not supported for vertex)
|
||||
if parts[1] == "list":
|
||||
raise InvalidPointCloudError(f"{path}: list property in vertex unsupported")
|
||||
props.append((parts[-1], parts[1]))
|
||||
elif parts[0] == "element" and not in_vertex:
|
||||
in_vertex = False
|
||||
|
||||
if fmt is None or n_vertices <= 0 or not props:
|
||||
raise InvalidPointCloudError(f"{path}: malformed PLY header")
|
||||
return _PlyHeader(fmt=fmt, n_vertices=n_vertices, data_offset=data_offset, props=props)
|
||||
|
||||
|
||||
def crop_ply_bbox(
|
||||
path: str | Path,
|
||||
*,
|
||||
x_min: float, x_max: float,
|
||||
y_min: float, y_max: float,
|
||||
z_min: float, z_max: float,
|
||||
chunk: int = 10_000_000,
|
||||
) -> tuple[np.ndarray, dict[str, Any]]:
|
||||
"""Stream-crop a binary PLY to an AABB. Returns ``(points (M,3), diag)``.
|
||||
|
||||
Falls back to a full load (via the standard reader) for ASCII PLY or
|
||||
headers this fast path can't handle.
|
||||
"""
|
||||
path = Path(path)
|
||||
if not path.exists():
|
||||
raise InvalidPointCloudError(f"point cloud not found: {path}")
|
||||
|
||||
try:
|
||||
hdr = _parse_header(path)
|
||||
except InvalidPointCloudError:
|
||||
hdr = None
|
||||
|
||||
if hdr is None or not hdr.fmt.startswith("binary"):
|
||||
# Fallback: load whole then filter (ASCII / unknown layout).
|
||||
from .pointcloud_io import load_point_cloud
|
||||
pts = load_point_cloud(path)
|
||||
m = ((pts[:, 0] >= x_min) & (pts[:, 0] <= x_max)
|
||||
& (pts[:, 1] >= y_min) & (pts[:, 1] <= y_max)
|
||||
& (pts[:, 2] >= z_min) & (pts[:, 2] <= z_max))
|
||||
out = pts[m]
|
||||
return out, {"mode": "fallback_full_load", "n_in": int(pts.shape[0]),
|
||||
"n_out": int(out.shape[0])}
|
||||
|
||||
# Build a structured dtype matching the vertex layout.
|
||||
byteorder = "<" if "little" in hdr.fmt else ">"
|
||||
names = [p[0] for p in hdr.props]
|
||||
formats = []
|
||||
for _, t in hdr.props:
|
||||
if t not in _PLY_TYPES:
|
||||
raise InvalidPointCloudError(f"{path}: unsupported property type {t}")
|
||||
formats.append(byteorder + _PLY_TYPES[t][0])
|
||||
dt = np.dtype({"names": names, "formats": formats})
|
||||
if not all(c in names for c in ("x", "y", "z")):
|
||||
raise InvalidPointCloudError(f"{path}: vertex has no x/y/z properties")
|
||||
|
||||
mm = np.memmap(path, dtype=dt, mode="r", offset=hdr.data_offset, shape=(hdr.n_vertices,))
|
||||
|
||||
kept = []
|
||||
n_in = hdr.n_vertices
|
||||
for start in range(0, n_in, chunk):
|
||||
end = min(start + chunk, n_in)
|
||||
blk = mm[start:end]
|
||||
x = np.asarray(blk["x"], dtype=np.float64)
|
||||
y = np.asarray(blk["y"], dtype=np.float64)
|
||||
z = np.asarray(blk["z"], dtype=np.float64)
|
||||
m = ((x >= x_min) & (x <= x_max)
|
||||
& (y >= y_min) & (y <= y_max)
|
||||
& (z >= z_min) & (z <= z_max))
|
||||
if m.any():
|
||||
kept.append(np.column_stack([x[m], y[m], z[m]]))
|
||||
out = np.concatenate(kept, axis=0) if kept else np.zeros((0, 3))
|
||||
return out, {
|
||||
"mode": "streaming",
|
||||
"format": hdr.fmt,
|
||||
"n_in": int(n_in),
|
||||
"n_out": int(out.shape[0]),
|
||||
"bbox": {"x_min": x_min, "x_max": x_max, "y_min": y_min,
|
||||
"y_max": y_max, "z_min": z_min, "z_max": z_max},
|
||||
}
|
||||
|
||||
|
||||
__all__ = ["crop_ply_bbox"]
|
||||
@@ -0,0 +1,238 @@
|
||||
"""Point cloud read / write.
|
||||
|
||||
Supported formats:
|
||||
- ``.ply`` binary or ASCII (open3d if available, else minimal builtin reader)
|
||||
- ``.xyz`` whitespace-separated XYZ[ R G B][ NX NY NZ]
|
||||
- ``.txt`` alias for ``.xyz``
|
||||
- ``.csv`` comma-separated, first three columns must be X, Y, Z
|
||||
- ``.pcd`` open3d only
|
||||
|
||||
The XYZ reader also serves as a fallback for ``.ply`` if open3d is
|
||||
unavailable and the PLY is ASCII.
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import json
|
||||
from pathlib import Path
|
||||
from typing import Any
|
||||
|
||||
import numpy as np
|
||||
|
||||
from ..exceptions import InvalidPointCloudError, UnsupportedFormatError
|
||||
|
||||
|
||||
_TEXT_SUFFIXES = (".xyz", ".txt", ".csv")
|
||||
_OPEN3D_SUFFIXES = (".ply", ".pcd")
|
||||
|
||||
|
||||
def _try_import_open3d():
|
||||
try:
|
||||
import open3d as o3d # type: ignore
|
||||
|
||||
return o3d
|
||||
except Exception:
|
||||
return None
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# Loading
|
||||
# ---------------------------------------------------------------------------
|
||||
|
||||
|
||||
def load_point_cloud(path: str | Path) -> np.ndarray:
|
||||
"""Load a point cloud as a float64 ``(N, 3)`` ndarray.
|
||||
|
||||
Raises
|
||||
------
|
||||
UnsupportedFormatError
|
||||
If the file extension is not supported.
|
||||
InvalidPointCloudError
|
||||
If the file is missing, empty, or fails to parse.
|
||||
"""
|
||||
path = Path(path)
|
||||
if not path.exists():
|
||||
raise InvalidPointCloudError(f"point cloud not found: {path}")
|
||||
|
||||
suffix = path.suffix.lower()
|
||||
if suffix in _TEXT_SUFFIXES:
|
||||
return _load_text(path, sep="," if suffix == ".csv" else None)
|
||||
|
||||
if suffix in _OPEN3D_SUFFIXES:
|
||||
o3d = _try_import_open3d()
|
||||
if o3d is not None:
|
||||
pcd = o3d.io.read_point_cloud(str(path))
|
||||
arr = np.asarray(pcd.points, dtype=np.float64)
|
||||
if arr.size == 0:
|
||||
# PLY may also be ASCII XYZ-style; fall back.
|
||||
if suffix == ".ply":
|
||||
return _load_ascii_ply_fallback(path)
|
||||
raise InvalidPointCloudError(f"empty point cloud: {path}")
|
||||
return arr
|
||||
if suffix == ".ply":
|
||||
return _load_ascii_ply_fallback(path)
|
||||
raise InvalidPointCloudError(
|
||||
f"open3d is required to read {suffix}; install with `pip install open3d`"
|
||||
)
|
||||
|
||||
raise UnsupportedFormatError(f"unsupported extension: {suffix}")
|
||||
|
||||
|
||||
def _load_text(path: Path, sep: str | None) -> np.ndarray:
|
||||
try:
|
||||
if sep is None:
|
||||
arr = np.loadtxt(path, comments=("#", "//"))
|
||||
else:
|
||||
arr = np.loadtxt(path, comments=("#", "//"), delimiter=sep)
|
||||
except Exception as exc:
|
||||
raise InvalidPointCloudError(f"failed to parse {path}: {exc}") from exc
|
||||
if arr.ndim == 1:
|
||||
arr = arr.reshape(1, -1)
|
||||
if arr.shape[1] < 3:
|
||||
raise InvalidPointCloudError(
|
||||
f"{path} has fewer than 3 columns (got {arr.shape[1]})"
|
||||
)
|
||||
return arr[:, :3].astype(np.float64, copy=False)
|
||||
|
||||
|
||||
def _load_ascii_ply_fallback(path: Path) -> np.ndarray:
|
||||
"""Minimal ASCII-PLY reader for the 'just XYZ' case."""
|
||||
with path.open("r", encoding="utf-8", errors="replace") as fh:
|
||||
header_done = False
|
||||
n_vertices = 0
|
||||
x_idx = y_idx = z_idx = -1
|
||||
prop_idx = 0
|
||||
format_line: str | None = None
|
||||
for line in fh:
|
||||
line = line.strip()
|
||||
if line == "end_header":
|
||||
header_done = True
|
||||
break
|
||||
if line.startswith("format"):
|
||||
format_line = line
|
||||
elif line.startswith("element vertex"):
|
||||
n_vertices = int(line.split()[-1])
|
||||
prop_idx = 0
|
||||
elif line.startswith("property"):
|
||||
name = line.split()[-1]
|
||||
if name == "x":
|
||||
x_idx = prop_idx
|
||||
elif name == "y":
|
||||
y_idx = prop_idx
|
||||
elif name == "z":
|
||||
z_idx = prop_idx
|
||||
prop_idx += 1
|
||||
if not header_done or n_vertices <= 0 or -1 in (x_idx, y_idx, z_idx):
|
||||
raise InvalidPointCloudError(
|
||||
f"unsupported PLY header in {path}; install open3d for full PLY support"
|
||||
)
|
||||
if format_line and "binary" in format_line:
|
||||
raise InvalidPointCloudError(
|
||||
f"{path} is binary PLY; install open3d to read it"
|
||||
)
|
||||
out = np.empty((n_vertices, 3), dtype=np.float64)
|
||||
for i in range(n_vertices):
|
||||
parts = fh.readline().split()
|
||||
out[i, 0] = float(parts[x_idx])
|
||||
out[i, 1] = float(parts[y_idx])
|
||||
out[i, 2] = float(parts[z_idx])
|
||||
return out
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# Writing
|
||||
# ---------------------------------------------------------------------------
|
||||
|
||||
|
||||
def write_point_cloud(
|
||||
points: np.ndarray,
|
||||
path: str | Path,
|
||||
*,
|
||||
colors: np.ndarray | None = None,
|
||||
binary: bool = True,
|
||||
) -> Path:
|
||||
"""Write a point cloud. Format is inferred from the extension.
|
||||
|
||||
For ``.ply``/``.pcd`` open3d is preferred; if open3d is missing and the
|
||||
target is ``.ply``, we fall back to writing ASCII PLY ourselves.
|
||||
"""
|
||||
path = Path(path)
|
||||
path.parent.mkdir(parents=True, exist_ok=True)
|
||||
points = np.ascontiguousarray(points, dtype=np.float64)
|
||||
if points.ndim != 2 or points.shape[1] != 3:
|
||||
raise InvalidPointCloudError(
|
||||
f"expected (N,3) points, got shape {points.shape}"
|
||||
)
|
||||
|
||||
suffix = path.suffix.lower()
|
||||
if suffix in (".xyz", ".txt"):
|
||||
np.savetxt(path, points, fmt="%.6f")
|
||||
return path
|
||||
if suffix == ".csv":
|
||||
np.savetxt(path, points, fmt="%.6f", delimiter=",")
|
||||
return path
|
||||
|
||||
if suffix in _OPEN3D_SUFFIXES:
|
||||
o3d = _try_import_open3d()
|
||||
if o3d is not None:
|
||||
pcd = o3d.geometry.PointCloud()
|
||||
pcd.points = o3d.utility.Vector3dVector(points)
|
||||
if colors is not None:
|
||||
pcd.colors = o3d.utility.Vector3dVector(np.asarray(colors)[:, :3])
|
||||
o3d.io.write_point_cloud(str(path), pcd, write_ascii=not binary)
|
||||
return path
|
||||
if suffix == ".ply":
|
||||
_write_ascii_ply(points, path, colors=colors)
|
||||
return path
|
||||
raise InvalidPointCloudError(
|
||||
f"open3d is required to write {suffix}; install with `pip install open3d`"
|
||||
)
|
||||
|
||||
raise UnsupportedFormatError(f"unsupported extension: {suffix}")
|
||||
|
||||
|
||||
def _write_ascii_ply(
|
||||
points: np.ndarray,
|
||||
path: Path,
|
||||
colors: np.ndarray | None = None,
|
||||
) -> None:
|
||||
have_color = colors is not None
|
||||
n = points.shape[0]
|
||||
with path.open("w", encoding="ascii") as fh:
|
||||
fh.write("ply\nformat ascii 1.0\n")
|
||||
fh.write(f"element vertex {n}\n")
|
||||
fh.write("property float x\nproperty float y\nproperty float z\n")
|
||||
if have_color:
|
||||
fh.write("property uchar red\nproperty uchar green\nproperty uchar blue\n")
|
||||
fh.write("end_header\n")
|
||||
if have_color:
|
||||
cols = np.clip(np.asarray(colors), 0.0, 1.0)
|
||||
cols = (cols * 255).astype(np.uint8)
|
||||
for p, c in zip(points, cols):
|
||||
fh.write(f"{p[0]:.6f} {p[1]:.6f} {p[2]:.6f} {int(c[0])} {int(c[1])} {int(c[2])}\n")
|
||||
else:
|
||||
for p in points:
|
||||
fh.write(f"{p[0]:.6f} {p[1]:.6f} {p[2]:.6f}\n")
|
||||
|
||||
|
||||
def write_metadata_json(meta: dict[str, Any], path: str | Path) -> Path:
|
||||
path = Path(path)
|
||||
path.parent.mkdir(parents=True, exist_ok=True)
|
||||
with path.open("w", encoding="utf-8") as fh:
|
||||
json.dump(meta, fh, ensure_ascii=False, indent=2, default=_json_default)
|
||||
return path
|
||||
|
||||
|
||||
def _json_default(obj: Any) -> Any:
|
||||
if isinstance(obj, np.ndarray):
|
||||
return obj.tolist()
|
||||
if isinstance(obj, (np.floating, np.integer)):
|
||||
return obj.item()
|
||||
raise TypeError(f"not JSON serialisable: {type(obj).__name__}")
|
||||
|
||||
|
||||
__all__ = [
|
||||
"load_point_cloud",
|
||||
"write_point_cloud",
|
||||
"write_metadata_json",
|
||||
]
|
||||
@@ -0,0 +1,14 @@
|
||||
"""Deflection-angle and maximum-deviation computations + GT comparison."""
|
||||
|
||||
from .deflection import (
|
||||
DeflectionMetrics,
|
||||
compute_deflection_metrics,
|
||||
)
|
||||
from .compare import compare_to_gt, GTComparison
|
||||
|
||||
__all__ = [
|
||||
"DeflectionMetrics",
|
||||
"compute_deflection_metrics",
|
||||
"compare_to_gt",
|
||||
"GTComparison",
|
||||
]
|
||||
@@ -0,0 +1,60 @@
|
||||
"""Compare measured deflection against the GT (CAD-derived) values."""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
from dataclasses import dataclass
|
||||
|
||||
from .deflection import DeflectionMetrics
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class GTComparison:
|
||||
measured: DeflectionMetrics
|
||||
gt: DeflectionMetrics
|
||||
|
||||
@property
|
||||
def d_theta_right_deg(self) -> float:
|
||||
return self.measured.theta_right_deg - self.gt.theta_right_deg
|
||||
|
||||
@property
|
||||
def d_theta_ref_deg(self) -> float:
|
||||
return self.measured.theta_ref_deg - self.gt.theta_ref_deg
|
||||
|
||||
@property
|
||||
def d_D_y_max_mm(self) -> float:
|
||||
return self.measured.D_y_max_mm - self.gt.D_y_max_mm
|
||||
|
||||
@property
|
||||
def d_D_z_max_mm(self) -> float:
|
||||
return self.measured.D_z_max_mm - self.gt.D_z_max_mm
|
||||
|
||||
@property
|
||||
def d_dx_y_max_mm(self) -> float:
|
||||
return self.measured.dx_y_max_mm - self.gt.dx_y_max_mm
|
||||
|
||||
@property
|
||||
def d_dx_z_max_mm(self) -> float:
|
||||
return self.measured.dx_z_max_mm - self.gt.dx_z_max_mm
|
||||
|
||||
def to_dict(self) -> dict[str, float | dict]:
|
||||
return {
|
||||
"measured": self.measured.to_dict(),
|
||||
"gt": self.gt.to_dict(),
|
||||
"diff": {
|
||||
"theta_right_deg": self.d_theta_right_deg,
|
||||
"theta_ref_deg": self.d_theta_ref_deg,
|
||||
"D_y_max_mm": self.d_D_y_max_mm,
|
||||
"D_z_max_mm": self.d_D_z_max_mm,
|
||||
"dx_y_max_mm": self.d_dx_y_max_mm,
|
||||
"dx_z_max_mm": self.d_dx_z_max_mm,
|
||||
"abs_theta_right_deg": abs(self.d_theta_right_deg),
|
||||
"abs_theta_ref_deg": abs(self.d_theta_ref_deg),
|
||||
},
|
||||
}
|
||||
|
||||
|
||||
def compare_to_gt(measured: DeflectionMetrics, gt: DeflectionMetrics) -> GTComparison:
|
||||
return GTComparison(measured=measured, gt=gt)
|
||||
|
||||
|
||||
__all__ = ["GTComparison", "compare_to_gt"]
|
||||
@@ -0,0 +1,103 @@
|
||||
"""Deflection-angle and maximum-deviation formulas.
|
||||
|
||||
For a fitted panel-plane normal :math:`(n_x, n_y, n_z)` with
|
||||
:math:`n_x > 0`, the spec is::
|
||||
|
||||
theta_right_deg = atan2(n_y, n_x) * 180/pi (right-axis sign applied)
|
||||
theta_ref_deg = atan2(n_z, n_x) * 180/pi
|
||||
A = -n_y / n_x
|
||||
B = -n_z / n_x
|
||||
D_y_max = max |y_i - C_y|
|
||||
D_z_max = max |z_i - C_z|
|
||||
dx_y_max = |A| * D_y_max
|
||||
dx_z_max = |B| * D_z_max
|
||||
|
||||
``points`` should already be in the *world / CAD* frame, i.e. after the
|
||||
4-hole Kabsch registration, and clipped to the panel ROI so the maxima
|
||||
do not pick up edge or chamfer pixels.
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
from dataclasses import dataclass
|
||||
from typing import Any
|
||||
|
||||
import numpy as np
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class DeflectionMetrics:
|
||||
theta_right_deg: float
|
||||
theta_ref_deg: float
|
||||
D_y_max_mm: float
|
||||
D_z_max_mm: float
|
||||
dx_y_max_mm: float
|
||||
dx_z_max_mm: float
|
||||
A: float # -n_y / n_x
|
||||
B: float # -n_z / n_x
|
||||
|
||||
def to_dict(self) -> dict[str, Any]:
|
||||
return {
|
||||
"theta_right_deg": self.theta_right_deg,
|
||||
"theta_ref_deg": self.theta_ref_deg,
|
||||
"D_y_max_mm": self.D_y_max_mm,
|
||||
"D_z_max_mm": self.D_z_max_mm,
|
||||
"dx_y_max_mm": self.dx_y_max_mm,
|
||||
"dx_z_max_mm": self.dx_z_max_mm,
|
||||
"A": self.A,
|
||||
"B": self.B,
|
||||
}
|
||||
|
||||
|
||||
def compute_deflection_metrics(
|
||||
*,
|
||||
normal: tuple[float, float, float] | np.ndarray,
|
||||
center_C: tuple[float, float, float] | np.ndarray,
|
||||
panel_points: np.ndarray,
|
||||
right_axis_sign: int = 1,
|
||||
angle_clamp_deg: float = 0.0,
|
||||
) -> DeflectionMetrics:
|
||||
"""Compute the six measurands."""
|
||||
n = np.asarray(normal, dtype=np.float64).reshape(3)
|
||||
if n[0] <= 0:
|
||||
# Caller is expected to enforce n_x > 0 before this; we hard-fail
|
||||
# because every formula divides by n_x.
|
||||
raise ValueError(
|
||||
f"compute_deflection_metrics requires n_x > 0; got n = {tuple(n)}"
|
||||
)
|
||||
C = np.asarray(center_C, dtype=np.float64).reshape(3)
|
||||
pts = np.asarray(panel_points, dtype=np.float64)
|
||||
if pts.ndim != 2 or pts.shape[1] != 3:
|
||||
raise ValueError(f"panel_points must be (N,3); got {pts.shape}")
|
||||
if pts.shape[0] == 0:
|
||||
raise ValueError("panel_points is empty")
|
||||
|
||||
sign = 1.0 if int(right_axis_sign) >= 0 else -1.0
|
||||
|
||||
theta_right = sign * float(np.degrees(np.arctan2(n[1], n[0])))
|
||||
theta_ref = float(np.degrees(np.arctan2(n[2], n[0])))
|
||||
|
||||
if abs(theta_right) < float(angle_clamp_deg):
|
||||
theta_right = 0.0
|
||||
if abs(theta_ref) < float(angle_clamp_deg):
|
||||
theta_ref = 0.0
|
||||
|
||||
A = float(-n[1] / n[0])
|
||||
B = float(-n[2] / n[0])
|
||||
|
||||
D_y_max = float(np.max(np.abs(pts[:, 1] - C[1])))
|
||||
D_z_max = float(np.max(np.abs(pts[:, 2] - C[2])))
|
||||
|
||||
return DeflectionMetrics(
|
||||
theta_right_deg=theta_right,
|
||||
theta_ref_deg=theta_ref,
|
||||
D_y_max_mm=D_y_max,
|
||||
D_z_max_mm=D_z_max,
|
||||
dx_y_max_mm=abs(A) * D_y_max,
|
||||
dx_z_max_mm=abs(B) * D_z_max,
|
||||
A=A,
|
||||
B=B,
|
||||
)
|
||||
|
||||
|
||||
__all__ = ["DeflectionMetrics", "compute_deflection_metrics"]
|
||||
@@ -0,0 +1,364 @@
|
||||
"""End-to-end orchestration for *research / validation* runs.
|
||||
|
||||
Two entry points:
|
||||
|
||||
- :func:`run_coupler_measurement_pipeline` — the integration path:
|
||||
cloud (in CAD frame) + optional CAD STP + ROI → ROI clip → denoise →
|
||||
measure. **No registration.**
|
||||
- :func:`run_from_cad_pipeline` — the research path: STP → synthesise →
|
||||
denoise → 4-hole Kabsch (against oracle deflected GT) → measure → diff
|
||||
GT. Saves every intermediate artifact under a structured run directory.
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import math
|
||||
from pathlib import Path
|
||||
from typing import Any
|
||||
|
||||
import numpy as np
|
||||
|
||||
from .api import compute_coupler_measurement, ProgressCallback
|
||||
from .config import load_config, resolve_auto
|
||||
from .data_prep import (
|
||||
PreparedCloud,
|
||||
denoise_cloud,
|
||||
parse_cad,
|
||||
prepare_cloud_from_cad,
|
||||
prepare_cloud_from_scan,
|
||||
register_cloud,
|
||||
synthesize_cloud,
|
||||
)
|
||||
from .exceptions import InsufficientPointsError
|
||||
from .io_utils import (
|
||||
Artifacts,
|
||||
crop_ply_bbox,
|
||||
load_point_cloud,
|
||||
write_json,
|
||||
write_point_cloud,
|
||||
write_yaml,
|
||||
)
|
||||
from .metrics import compute_deflection_metrics
|
||||
from .detection.autotune import detection_overlay, relax_denoise_steps
|
||||
from .detection.layer_select import select_panel_layer
|
||||
from .roi_select import ROISpec, apply_roi, default_roi_spec, load_roi_json
|
||||
from .structures import CouplerMeasurement
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# Integration entry point
|
||||
# ---------------------------------------------------------------------------
|
||||
|
||||
|
||||
def run_coupler_measurement_pipeline(
|
||||
input_cloud: str | Path,
|
||||
out_dir: str | Path,
|
||||
*,
|
||||
config_path: str | Path | None = None,
|
||||
profile: str = "auto",
|
||||
cad_stp_path: str | Path | None = None,
|
||||
roi_spec: ROISpec | None = None,
|
||||
roi_json: str | Path | None = None,
|
||||
progress: ProgressCallback | None = None,
|
||||
) -> CouplerMeasurement:
|
||||
"""Main measurement entry: cloud (in CAD frame) → ROI → denoise → measure.
|
||||
|
||||
**No registration step.** The input cloud is assumed to already live
|
||||
in the CAD / world frame (scanner extrinsics calibrated upstream).
|
||||
If it doesn't, 4-hole Kabsch against nominal CAD holes would also
|
||||
cancel the panel-deflection signal we want to measure — so this
|
||||
pipeline deliberately does not register. For the research path that
|
||||
*does* register (using oracle deflected GT), see
|
||||
:func:`run_from_cad_pipeline`.
|
||||
|
||||
Inputs:
|
||||
|
||||
- ``cad_stp_path`` — optional STEP file. Parsed for the analytical
|
||||
panel-normal seed and, when no profile/config is given, to
|
||||
auto-tune the scale/density-dependent denoise + detection knobs
|
||||
(see below). The parsed GT is written as ``01_gt.json`` for audit.
|
||||
- ``roi_spec`` / ``roi_json`` — axis-aligned-bbox ROI applied to the
|
||||
cloud *before* denoising. If both omitted, the full cloud is kept.
|
||||
``roi_spec`` wins over ``roi_json``.
|
||||
|
||||
Writes every intermediate to ``out_dir``. Returns a
|
||||
:class:`CouplerMeasurement`.
|
||||
"""
|
||||
art = Artifacts(out_dir)
|
||||
cfg = load_config(config_path, profile=profile)
|
||||
cfg = resolve_auto(cfg)
|
||||
write_yaml(cfg, art.resolved_config)
|
||||
|
||||
# ---- Load + ROI clip --------------------------------------------------
|
||||
# The ROI JSON is the cropping hyper-parameter. When it is a finite
|
||||
# bounding box and the input is a (possibly multi-GB) binary PLY, crop
|
||||
# it by streaming the file so we never hold the whole vehicle cloud in
|
||||
# memory. Otherwise fall back to load-then-filter.
|
||||
if roi_spec is None:
|
||||
roi_spec = load_roi_json(roi_json) if roi_json is not None else default_roi_spec()
|
||||
|
||||
finite_bbox = all(
|
||||
math.isfinite(v) for v in (
|
||||
roi_spec.x_min, roi_spec.x_max,
|
||||
roi_spec.y_min, roi_spec.y_max,
|
||||
roi_spec.z_min, roi_spec.z_max,
|
||||
)
|
||||
)
|
||||
is_ply = str(input_cloud).lower().endswith(".ply")
|
||||
if finite_bbox and is_ply:
|
||||
pts, roi_diag = crop_ply_bbox(
|
||||
input_cloud,
|
||||
x_min=roi_spec.x_min, x_max=roi_spec.x_max,
|
||||
y_min=roi_spec.y_min, y_max=roi_spec.y_max,
|
||||
z_min=roi_spec.z_min, z_max=roi_spec.z_max,
|
||||
)
|
||||
roi_diag["kind"] = roi_spec.kind
|
||||
else:
|
||||
raw_pts = load_point_cloud(input_cloud)
|
||||
pts, roi_diag = apply_roi(raw_pts, roi_spec)
|
||||
|
||||
if pts.shape[0] == 0:
|
||||
raise InsufficientPointsError(
|
||||
f"ROI bbox kept 0 points from {input_cloud}; check the ROI JSON "
|
||||
f"bounds match the cloud's coordinate frame"
|
||||
)
|
||||
write_point_cloud(pts, art.path("sampled", ext="ply"))
|
||||
write_json({"spec": roi_spec.to_dict(), "diagnostics": roi_diag}, art.root / "roi.json")
|
||||
|
||||
# ---- Optional normal-direction layer isolation -----------------------
|
||||
# For a ROI cut from a full-vehicle scan, the box can slice through
|
||||
# several structures stacked along the panel normal (base plate +
|
||||
# raised mounting plate + roof truss). Detection and plane fit assume a
|
||||
# single thin plate, so we keep only one layer when the active config
|
||||
# turns this on (layer_select.enabled). Default config leaves it off,
|
||||
# so single-part clouds are unaffected.
|
||||
ls_cfg = cfg.get("layer_select", {})
|
||||
if ls_cfg.get("enabled", False):
|
||||
layer = select_panel_layer(
|
||||
pts,
|
||||
normal_seed=ls_cfg.get("normal_seed", "pca"),
|
||||
half_width_mm=float(ls_cfg.get("half_width_mm", 6.0)),
|
||||
bins=int(ls_cfg.get("bins", 240)),
|
||||
pick=str(ls_cfg.get("pick", "frontmost")),
|
||||
)
|
||||
pts = layer.points
|
||||
write_point_cloud(pts, art.path("transformed", ext="ply"))
|
||||
write_json(layer.diagnostics, art.root / "layer.json")
|
||||
|
||||
# Auto-tune the scale/density-dependent knobs from CAD + cloud when the
|
||||
# caller relies on the generic default config (no explicit config file
|
||||
# and no hand-tuned profile) but provides a CAD STP. This makes the
|
||||
# "CAD + cloud + ROI" contract self-sufficient for an adequately-sampled
|
||||
# scan of a panel of any scale, instead of (a) the default radius filter
|
||||
# deleting a sparse cloud or (b) 4-hole detection failing with the
|
||||
# small-panel defaults. A specific profile (e.g. lu1_af4) or an explicit
|
||||
# config is respected as-is (autotune skipped), so existing setups are
|
||||
# byte-for-byte unchanged.
|
||||
autotune = (
|
||||
cad_stp_path is not None
|
||||
and config_path is None
|
||||
and str(profile) in ("auto", "default", "none", "None")
|
||||
)
|
||||
|
||||
# ---- Optional CAD parse (panel_normal_seed + audit GT JSON) ----------
|
||||
panel_seed: np.ndarray | None = None
|
||||
parsed = None
|
||||
if cad_stp_path is not None:
|
||||
if autotune:
|
||||
# The default locating-hole band ([4,12] mm) selects the small
|
||||
# inner holes on a large panel, from which autotune would derive
|
||||
# too fine a grid. Widen it so parse_cad picks the large corner
|
||||
# locating holes (the reliably-scannable rectangle) and excludes
|
||||
# the big central pass-through hole.
|
||||
cfg["cad"] = {**cfg["cad"], "locating_hole_radius_mm": [6.0, 30.0]}
|
||||
parsed = parse_cad(cad_stp_path, cfg["cad"])
|
||||
write_json(parsed.gt.to_dict(), art.path("gt", ext="json"))
|
||||
panel_seed = np.asarray(parsed.gt.plane.normal, dtype=np.float64)
|
||||
|
||||
# ---- Denoise (no Kabsch, no prepare_cloud_from_scan) -----------------
|
||||
# Stage 1 of autotune (before denoise): drop the density-fragile radius
|
||||
# filter so a sparse cloud is not deleted.
|
||||
if autotune:
|
||||
cfg["denoise"] = relax_denoise_steps(cfg.get("denoise", {}))
|
||||
denoised = denoise_cloud(pts, cfg.get("denoise", {}))
|
||||
write_point_cloud(denoised.points, art.path("denoised", ext="ply"))
|
||||
|
||||
# Stage 2 of autotune (after denoise): size the detection grid / fill /
|
||||
# area band from the CAD locating-hole radius + the DENOISED point
|
||||
# spacing (the cloud detection actually sees).
|
||||
if autotune and parsed is not None:
|
||||
loc_r = float(np.median([h.radius_mm for h in parsed.gt.holes.locating]))
|
||||
cfg["detection"] = {
|
||||
**cfg.get("detection", {}),
|
||||
**detection_overlay(loc_r, denoised.points),
|
||||
}
|
||||
write_yaml(cfg, art.resolved_config)
|
||||
|
||||
# ---- Measurement ------------------------------------------------------
|
||||
result = compute_coupler_measurement(
|
||||
denoised.points,
|
||||
config=cfg,
|
||||
profile="custom",
|
||||
panel_normal_seed=panel_seed,
|
||||
progress=progress,
|
||||
)
|
||||
|
||||
write_json(result.to_dict(), art.report_json)
|
||||
write_json(result.to_dict()["report"], art.path("metrics", ext="json"))
|
||||
return result
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# CAD-driven research entry point
|
||||
# ---------------------------------------------------------------------------
|
||||
|
||||
|
||||
def run_from_cad_pipeline(
|
||||
stp_path: str | Path,
|
||||
out_dir: str | Path,
|
||||
*,
|
||||
config_path: str | Path | None = None,
|
||||
profile: str = "auto",
|
||||
progress: ProgressCallback | None = None,
|
||||
) -> dict[str, Any]:
|
||||
"""End-to-end research run: STP → synthesise → prepare → measure → diff GT.
|
||||
|
||||
Returns a summary dict containing the measurement, GT metrics, and
|
||||
measured-minus-GT differences. Writes every intermediate cloud and
|
||||
JSON to ``out_dir``.
|
||||
"""
|
||||
art = Artifacts(out_dir)
|
||||
cfg = load_config(config_path, profile=profile)
|
||||
cfg = resolve_auto(cfg)
|
||||
write_yaml(cfg, art.resolved_config)
|
||||
|
||||
# ---- Phase A.1 — parse + GT -------------------------------------------
|
||||
parsed = parse_cad(stp_path, cfg["cad"])
|
||||
write_point_cloud(parsed.mesh.vertices, art.path("cad_mesh", ext="ply"))
|
||||
write_json(parsed.gt.to_dict(), art.path("gt", ext="json"))
|
||||
|
||||
# ---- Phase A.2 — synthesise --------------------------------------------
|
||||
synth = synthesize_cloud(
|
||||
parsed,
|
||||
sampling_cfg=cfg["sampling"],
|
||||
noise_cfg=cfg.get("noise", {}),
|
||||
transform_cfg=cfg.get("transform", {}),
|
||||
scan_cfg=cfg.get("scan_simulator", {}),
|
||||
deflection_cfg=cfg.get("panel_deflection", {}),
|
||||
)
|
||||
write_point_cloud(synth.sampled, art.path("sampled", ext="ply"))
|
||||
write_point_cloud(synth.noisy, art.path("noisy", ext="ply"))
|
||||
write_point_cloud(synth.transformed, art.path("transformed", ext="ply"))
|
||||
|
||||
# ---- Phase A.3 — denoise ----------------------------------------------
|
||||
denoised = denoise_cloud(synth.transformed, cfg.get("denoise", {}))
|
||||
write_point_cloud(denoised.points, art.path("denoised", ext="ply"))
|
||||
|
||||
# ---- Phase A.4 — register ---------------------------------------------
|
||||
registered = register_cloud(
|
||||
denoised.points,
|
||||
target_holes=synth.gt_holes_after_deflection,
|
||||
detection_cfg=cfg["detection"],
|
||||
cad_cfg=cfg["cad"],
|
||||
registration_cfg=cfg["registration"],
|
||||
panel_normal_seed=None,
|
||||
)
|
||||
write_point_cloud(registered.points, art.path("registered", ext="ply"))
|
||||
|
||||
# ---- Phase B — measurement algorithm ----------------------------------
|
||||
result = compute_coupler_measurement(
|
||||
registered.points,
|
||||
config=cfg,
|
||||
profile="custom",
|
||||
panel_normal_seed=np.asarray(parsed.gt.plane.normal, dtype=np.float64),
|
||||
progress=progress,
|
||||
)
|
||||
|
||||
# ---- GT metrics --------------------------------------------------------
|
||||
gt_normal = synth.gt_normal_after_deflection
|
||||
centers = synth.gt_holes_after_deflection.locating_centers()
|
||||
gt_metrics = compute_deflection_metrics(
|
||||
normal=gt_normal,
|
||||
center_C=tuple(centers.mean(axis=0).tolist()),
|
||||
panel_points=centers,
|
||||
right_axis_sign=int(cfg["metrics"].get("right_axis_sign", 1)),
|
||||
angle_clamp_deg=float(cfg["metrics"].get("angle_clamp_deg", 0.0)),
|
||||
)
|
||||
|
||||
diff = {
|
||||
"theta_right_deg": result.theta_right_deg - gt_metrics.theta_right_deg,
|
||||
"theta_ref_deg": result.theta_ref_deg - gt_metrics.theta_ref_deg,
|
||||
"D_y_max_mm": result.D_y_max_mm - gt_metrics.D_y_max_mm,
|
||||
"D_z_max_mm": result.D_z_max_mm - gt_metrics.D_z_max_mm,
|
||||
"dx_y_max_mm": result.dx_y_max_mm - gt_metrics.dx_y_max_mm,
|
||||
"dx_z_max_mm": result.dx_z_max_mm - gt_metrics.dx_z_max_mm,
|
||||
}
|
||||
|
||||
summary = {
|
||||
"stp": str(stp_path),
|
||||
"panel_deflection_injected": {
|
||||
"theta_right_deg": synth.panel_deflection_theta_right_deg,
|
||||
"theta_ref_deg": synth.panel_deflection_theta_ref_deg,
|
||||
"R": synth.panel_deflection_R.tolist(),
|
||||
},
|
||||
"scan_pose_perturbation": synth.gt_transform_4x4.tolist(),
|
||||
"registration": {
|
||||
"matrix": registered.transform_4x4.tolist(),
|
||||
"rmse_mm": registered.rmse_mm,
|
||||
"max_residual_mm": registered.max_residual_mm,
|
||||
},
|
||||
"measured": result.to_dict(),
|
||||
"gt_cad": parsed.gt.to_dict(),
|
||||
"gt_after_deflection": {
|
||||
"normal": gt_normal.tolist(),
|
||||
"locating_centers": centers.tolist(),
|
||||
},
|
||||
"gt_metrics": gt_metrics.to_dict(),
|
||||
"diff": diff,
|
||||
"config": cfg,
|
||||
}
|
||||
write_json(summary, art.path("metrics", ext="json"))
|
||||
write_json(summary, art.report_json)
|
||||
|
||||
return summary
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# Helpers
|
||||
# ---------------------------------------------------------------------------
|
||||
|
||||
|
||||
def _load_holeset_from_json(path: str | Path):
|
||||
"""Reconstruct a HoleSet from the JSON written by ``GroundTruth.to_dict``."""
|
||||
import json
|
||||
|
||||
from .structures import Hole, HoleSet
|
||||
|
||||
with Path(path).open("r", encoding="utf-8") as fh:
|
||||
d = json.load(fh)
|
||||
holes = d["holes"]
|
||||
locating = tuple(
|
||||
Hole(
|
||||
center=tuple(h["center"]),
|
||||
radius_mm=float(h["radius_mm"]),
|
||||
fit_residual_rms_mm=float(h.get("fit_residual_rms_mm", 0.0)),
|
||||
role=h.get("role", "locating"),
|
||||
)
|
||||
for h in holes["locating"]
|
||||
)
|
||||
central = None
|
||||
if holes.get("central") is not None:
|
||||
c = holes["central"]
|
||||
central = Hole(
|
||||
center=tuple(c["center"]),
|
||||
radius_mm=float(c["radius_mm"]),
|
||||
fit_residual_rms_mm=float(c.get("fit_residual_rms_mm", 0.0)),
|
||||
role="central",
|
||||
)
|
||||
return HoleSet(locating=locating, central=central)
|
||||
|
||||
|
||||
__all__ = [
|
||||
"run_coupler_measurement_pipeline",
|
||||
"run_from_cad_pipeline",
|
||||
]
|
||||
@@ -0,0 +1,18 @@
|
||||
"""Rigid registration via 4 hole-centre correspondence (Kabsch / Umeyama).
|
||||
|
||||
This package deliberately ships **only** the four-hole method. We do
|
||||
*not* expose a panel-plane ICP variant because that would absorb the
|
||||
deflection signal we want to measure.
|
||||
"""
|
||||
|
||||
from .kabsch import (
|
||||
KabschResult,
|
||||
kabsch_umeyama,
|
||||
register_four_holes,
|
||||
)
|
||||
|
||||
__all__ = [
|
||||
"KabschResult",
|
||||
"kabsch_umeyama",
|
||||
"register_four_holes",
|
||||
]
|
||||
@@ -0,0 +1,114 @@
|
||||
"""Rigid registration from a small number of 3D point correspondences.
|
||||
|
||||
We use the closed-form Umeyama solution (Kabsch with the optional scale
|
||||
turned off — we want a rigid transform). Given 4 source points P (in the
|
||||
scan/world frame) and 4 target points Q (in the CAD/GT frame), we solve
|
||||
for the rigid T such that ``T @ P ≈ Q``.
|
||||
|
||||
The four locating holes give us four exact correspondences; their
|
||||
indexing must match (we ensure this in :mod:`coupler_panel.detection`
|
||||
via the canonical quadrant ordering).
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
from dataclasses import dataclass
|
||||
|
||||
import numpy as np
|
||||
|
||||
from ..exceptions import RegistrationError
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class KabschResult:
|
||||
"""4×4 transform plus quality stats."""
|
||||
|
||||
matrix: np.ndarray # (4, 4) — applies to source to align with target
|
||||
rmse_mm: float
|
||||
max_residual_mm: float
|
||||
n_pairs: int
|
||||
|
||||
def apply(self, points: np.ndarray) -> np.ndarray:
|
||||
R = self.matrix[:3, :3]
|
||||
t = self.matrix[:3, 3]
|
||||
return np.asarray(points, dtype=np.float64) @ R.T + t
|
||||
|
||||
|
||||
def kabsch_umeyama(
|
||||
source: np.ndarray,
|
||||
target: np.ndarray,
|
||||
*,
|
||||
allow_reflection: bool = False,
|
||||
) -> KabschResult:
|
||||
"""Solve for rigid T such that T·source ≈ target."""
|
||||
P = np.asarray(source, dtype=np.float64)
|
||||
Q = np.asarray(target, dtype=np.float64)
|
||||
if P.shape != Q.shape or P.ndim != 2 or P.shape[1] != 3:
|
||||
raise RegistrationError(
|
||||
f"shape mismatch: source={P.shape}, target={Q.shape}; "
|
||||
"expected (N,3) with the same N"
|
||||
)
|
||||
n = P.shape[0]
|
||||
if n < 3:
|
||||
raise RegistrationError(f"need ≥ 3 correspondences, got {n}")
|
||||
|
||||
p_bar = P.mean(axis=0)
|
||||
q_bar = Q.mean(axis=0)
|
||||
Pc = P - p_bar
|
||||
Qc = Q - q_bar
|
||||
|
||||
H = Pc.T @ Qc # (3, 3)
|
||||
U, _S, Vt = np.linalg.svd(H)
|
||||
R = Vt.T @ U.T
|
||||
|
||||
if not allow_reflection and np.linalg.det(R) < 0:
|
||||
Vt[-1, :] *= -1.0
|
||||
R = Vt.T @ U.T
|
||||
|
||||
t = q_bar - R @ p_bar
|
||||
|
||||
M = np.eye(4)
|
||||
M[:3, :3] = R
|
||||
M[:3, 3] = t
|
||||
|
||||
P_aligned = P @ R.T + t
|
||||
res = np.linalg.norm(P_aligned - Q, axis=1)
|
||||
return KabschResult(
|
||||
matrix=M,
|
||||
rmse_mm=float(np.sqrt(np.mean(res ** 2))),
|
||||
max_residual_mm=float(res.max()),
|
||||
n_pairs=int(n),
|
||||
)
|
||||
|
||||
|
||||
def register_four_holes(
|
||||
source_centers: np.ndarray,
|
||||
target_centers: np.ndarray,
|
||||
*,
|
||||
refuse_above_mm: float | None = None,
|
||||
) -> KabschResult:
|
||||
"""Wrapper for the canonical 4-hole pairing case.
|
||||
|
||||
The two arrays **must** be index-aligned (same canonical quadrant
|
||||
order). The detection / GT modules in this project are responsible
|
||||
for producing them in the same order.
|
||||
"""
|
||||
P = np.asarray(source_centers, dtype=np.float64)
|
||||
Q = np.asarray(target_centers, dtype=np.float64)
|
||||
if P.shape != (4, 3) or Q.shape != (4, 3):
|
||||
raise RegistrationError(
|
||||
f"register_four_holes requires (4,3) inputs; got {P.shape} and {Q.shape}"
|
||||
)
|
||||
|
||||
result = kabsch_umeyama(P, Q)
|
||||
|
||||
if refuse_above_mm is not None and result.rmse_mm > float(refuse_above_mm):
|
||||
raise RegistrationError(
|
||||
f"4-hole Kabsch RMSE {result.rmse_mm:.3f} mm exceeds {refuse_above_mm} mm — "
|
||||
"wrong correspondences, missed hole, or detection error"
|
||||
)
|
||||
|
||||
return result
|
||||
|
||||
|
||||
__all__ = ["KabschResult", "kabsch_umeyama", "register_four_holes"]
|
||||
@@ -0,0 +1,12 @@
|
||||
"""Batch-evaluation reporting helpers (xlsx output)."""
|
||||
|
||||
from .excel import EvalRow, write_excel_report, build_row_from_summary
|
||||
from .inspection import InspectionItem, write_inspection_report
|
||||
|
||||
__all__ = [
|
||||
"EvalRow",
|
||||
"write_excel_report",
|
||||
"build_row_from_summary",
|
||||
"InspectionItem",
|
||||
"write_inspection_report",
|
||||
]
|
||||
@@ -0,0 +1,285 @@
|
||||
"""Aggregate batch-eval results into an xlsx report.
|
||||
|
||||
Each row corresponds to one input PLY × one ROI JSON × one run.
|
||||
|
||||
Columns (kept stable across releases unless the field is removed from
|
||||
the upstream report):
|
||||
|
||||
sample_id, ply_path, roi_kind,
|
||||
theta_right_gt_deg, theta_ref_gt_deg,
|
||||
theta_right_pred_deg, theta_ref_pred_deg,
|
||||
theta_right_err_deg, theta_ref_err_deg,
|
||||
D_y_max_mm, D_z_max_mm,
|
||||
dx_y_max_mm, dx_z_max_mm,
|
||||
center_C_x_mm, center_C_y_mm, center_C_z_mm,
|
||||
normal_x, normal_y, normal_z,
|
||||
plane_inlier_ratio, plane_residual_rms_mm,
|
||||
registration_rmse_mm, registration_max_residual_mm,
|
||||
n_points_in, n_points_after_roi,
|
||||
quality_flags,
|
||||
pipeline_seconds, inner_stage_seconds,
|
||||
algorithm_version, schema_version
|
||||
|
||||
``pipeline_seconds`` is the **wall-clock** time from the moment the
|
||||
inputs (cloud PLY + ROI JSON, optionally CAD STP) enter
|
||||
``run_coupler_measurement_pipeline`` to the moment the
|
||||
``CouplerMeasurement`` is returned — i.e. the algorithm's total
|
||||
processing time per sample.
|
||||
|
||||
``inner_stage_seconds`` is the older, narrower number: the sum of
|
||||
``StageTiming.seconds`` recorded inside ``compute_coupler_measurement``
|
||||
itself (validate → detect_holes → … → metrics), excluding file IO,
|
||||
cloud load, ROI apply, denoise, and any CAD STP parse. It is kept for
|
||||
per-stage debugging only.
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
from dataclasses import asdict, dataclass, field
|
||||
from pathlib import Path
|
||||
from typing import Any
|
||||
|
||||
try:
|
||||
from openpyxl import Workbook
|
||||
from openpyxl.styles import Alignment, Font, PatternFill
|
||||
except ImportError as exc: # pragma: no cover
|
||||
raise ImportError(
|
||||
"openpyxl is required for Excel reporting; install with `pip install openpyxl`"
|
||||
) from exc
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# Row schema
|
||||
# ---------------------------------------------------------------------------
|
||||
|
||||
|
||||
COLUMNS: tuple[tuple[str, str], ...] = (
|
||||
("sample_id", "Sample ID"),
|
||||
("ply_path", "Input PLY"),
|
||||
("roi_kind", "ROI kind"),
|
||||
("theta_right_gt_deg", "θ_right GT (°)"),
|
||||
("theta_ref_gt_deg", "θ_ref GT (°)"),
|
||||
("theta_right_pred_deg", "θ_right Pred (°)"),
|
||||
("theta_ref_pred_deg", "θ_ref Pred (°)"),
|
||||
("theta_right_err_deg", "θ_right Err (°)"),
|
||||
("theta_ref_err_deg", "θ_ref Err (°)"),
|
||||
("D_y_max_mm", "D_y_max (mm)"),
|
||||
("D_z_max_mm", "D_z_max (mm)"),
|
||||
("dx_y_max_mm", "dx_y_max (mm)"),
|
||||
("dx_z_max_mm", "dx_z_max (mm)"),
|
||||
("center_C_x_mm", "C_x (mm)"),
|
||||
("center_C_y_mm", "C_y (mm)"),
|
||||
("center_C_z_mm", "C_z (mm)"),
|
||||
("normal_x", "n_x"),
|
||||
("normal_y", "n_y"),
|
||||
("normal_z", "n_z"),
|
||||
("plane_inlier_ratio", "plane inlier ratio"),
|
||||
("plane_residual_rms_mm", "plane res RMS (mm)"),
|
||||
("registration_rmse_mm", "Kabsch RMSE (mm)"),
|
||||
("registration_max_residual_mm", "Kabsch max res (mm)"),
|
||||
("n_points_in", "N in"),
|
||||
("n_points_after_roi", "N after ROI"),
|
||||
("quality_flags", "quality_flags"),
|
||||
("pipeline_seconds", "Algorithm runtime (s)"),
|
||||
("inner_stage_seconds", "compute_*_measurement (s)"),
|
||||
("algorithm_version", "algo ver"),
|
||||
("schema_version", "schema ver"),
|
||||
)
|
||||
|
||||
|
||||
@dataclass
|
||||
class EvalRow:
|
||||
"""One row in the Excel report (one PLY × one ROI × one config)."""
|
||||
|
||||
sample_id: str
|
||||
ply_path: str
|
||||
roi_kind: str
|
||||
theta_right_gt_deg: float | None
|
||||
theta_ref_gt_deg: float | None
|
||||
theta_right_pred_deg: float
|
||||
theta_ref_pred_deg: float
|
||||
theta_right_err_deg: float | None
|
||||
theta_ref_err_deg: float | None
|
||||
D_y_max_mm: float
|
||||
D_z_max_mm: float
|
||||
dx_y_max_mm: float
|
||||
dx_z_max_mm: float
|
||||
center_C_x_mm: float
|
||||
center_C_y_mm: float
|
||||
center_C_z_mm: float
|
||||
normal_x: float
|
||||
normal_y: float
|
||||
normal_z: float
|
||||
plane_inlier_ratio: float
|
||||
plane_residual_rms_mm: float
|
||||
registration_rmse_mm: float | None
|
||||
registration_max_residual_mm: float | None
|
||||
n_points_in: int
|
||||
n_points_after_roi: int
|
||||
quality_flags: str
|
||||
pipeline_seconds: float
|
||||
inner_stage_seconds: float
|
||||
algorithm_version: str
|
||||
schema_version: str
|
||||
extra: dict[str, Any] = field(default_factory=dict)
|
||||
|
||||
def to_dict(self) -> dict[str, Any]:
|
||||
d = asdict(self)
|
||||
d.pop("extra", None)
|
||||
return d
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# Row construction from pipeline outputs
|
||||
# ---------------------------------------------------------------------------
|
||||
|
||||
|
||||
def build_row_from_summary(
|
||||
*,
|
||||
sample_id: str,
|
||||
ply_path: str | Path,
|
||||
roi_kind: str,
|
||||
measured: dict[str, Any],
|
||||
gt_angles: tuple[float, float] | None,
|
||||
registration: dict[str, Any] | None,
|
||||
n_points_in: int,
|
||||
n_points_after_roi: int,
|
||||
pipeline_seconds: float,
|
||||
) -> EvalRow:
|
||||
"""Construct an :class:`EvalRow` from the pipeline's report dict.
|
||||
|
||||
``measured`` is the ``CouplerMeasurement.to_dict()`` payload. It
|
||||
embeds ``report`` (the inner dict ``compute_coupler_measurement``
|
||||
wrote). ``gt_angles`` is the injected ``(theta_right_deg,
|
||||
theta_ref_deg)`` or ``None`` when not known.
|
||||
|
||||
``pipeline_seconds`` should be the wall-clock duration of the
|
||||
``run_coupler_measurement_pipeline`` call — i.e. the algorithm's
|
||||
end-to-end runtime per sample. Caller is responsible for timing.
|
||||
"""
|
||||
report = measured.get("report", {})
|
||||
plane = report.get("plane", {})
|
||||
timings = report.get("timings", [])
|
||||
inner_s = float(sum(t.get("seconds", 0.0) for t in timings))
|
||||
|
||||
th_r_pred = float(measured["theta_right_deg"])
|
||||
th_z_pred = float(measured["theta_ref_deg"])
|
||||
if gt_angles is None:
|
||||
th_r_gt = th_z_gt = None
|
||||
th_r_err = th_z_err = None
|
||||
else:
|
||||
th_r_gt = float(gt_angles[0])
|
||||
th_z_gt = float(gt_angles[1])
|
||||
th_r_err = th_r_pred - th_r_gt
|
||||
th_z_err = th_z_pred - th_z_gt
|
||||
|
||||
cx, cy, cz = measured["center_C"]
|
||||
nx, ny, nz = measured["normal"]
|
||||
|
||||
return EvalRow(
|
||||
sample_id=sample_id,
|
||||
ply_path=str(ply_path),
|
||||
roi_kind=roi_kind,
|
||||
theta_right_gt_deg=th_r_gt,
|
||||
theta_ref_gt_deg=th_z_gt,
|
||||
theta_right_pred_deg=th_r_pred,
|
||||
theta_ref_pred_deg=th_z_pred,
|
||||
theta_right_err_deg=th_r_err,
|
||||
theta_ref_err_deg=th_z_err,
|
||||
D_y_max_mm=float(measured["D_y_max_mm"]),
|
||||
D_z_max_mm=float(measured["D_z_max_mm"]),
|
||||
dx_y_max_mm=float(measured["dx_y_max_mm"]),
|
||||
dx_z_max_mm=float(measured["dx_z_max_mm"]),
|
||||
center_C_x_mm=float(cx),
|
||||
center_C_y_mm=float(cy),
|
||||
center_C_z_mm=float(cz),
|
||||
normal_x=float(nx),
|
||||
normal_y=float(ny),
|
||||
normal_z=float(nz),
|
||||
plane_inlier_ratio=float(plane.get("inlier_ratio", float("nan"))),
|
||||
plane_residual_rms_mm=float(plane.get("inlier_residual_rms_mm", float("nan"))),
|
||||
registration_rmse_mm=(None if registration is None else float(registration.get("rmse_mm", float("nan")))),
|
||||
registration_max_residual_mm=(
|
||||
None
|
||||
if registration is None
|
||||
else float(registration.get("max_residual_mm", float("nan")))
|
||||
),
|
||||
n_points_in=int(n_points_in),
|
||||
n_points_after_roi=int(n_points_after_roi),
|
||||
quality_flags=";".join(measured.get("quality_flags", [])) or "",
|
||||
pipeline_seconds=float(pipeline_seconds),
|
||||
inner_stage_seconds=inner_s,
|
||||
algorithm_version=str(measured.get("algorithm_version", "")),
|
||||
schema_version=str(measured.get("schema_version", "")),
|
||||
)
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# Excel writer
|
||||
# ---------------------------------------------------------------------------
|
||||
|
||||
|
||||
def write_excel_report(rows: list[EvalRow], path: str | Path) -> Path:
|
||||
"""Write rows to an xlsx file. Returns the path."""
|
||||
out = Path(path)
|
||||
out.parent.mkdir(parents=True, exist_ok=True)
|
||||
|
||||
wb = Workbook()
|
||||
ws = wb.active
|
||||
ws.title = "results"
|
||||
|
||||
header_font = Font(bold=True)
|
||||
header_fill = PatternFill("solid", fgColor="DDDDDD")
|
||||
center = Alignment(horizontal="center")
|
||||
|
||||
# Header row.
|
||||
for col_idx, (_, label) in enumerate(COLUMNS, start=1):
|
||||
cell = ws.cell(row=1, column=col_idx, value=label)
|
||||
cell.font = header_font
|
||||
cell.fill = header_fill
|
||||
cell.alignment = center
|
||||
|
||||
# Data rows.
|
||||
for r_idx, row in enumerate(rows, start=2):
|
||||
d = row.to_dict()
|
||||
for c_idx, (key, _) in enumerate(COLUMNS, start=1):
|
||||
v = d.get(key)
|
||||
if isinstance(v, float) and (v != v): # NaN
|
||||
v = ""
|
||||
ws.cell(row=r_idx, column=c_idx, value=v)
|
||||
|
||||
# Number formatting for float-typed cells.
|
||||
float_keys = {
|
||||
"theta_right_gt_deg", "theta_ref_gt_deg",
|
||||
"theta_right_pred_deg", "theta_ref_pred_deg",
|
||||
"theta_right_err_deg", "theta_ref_err_deg",
|
||||
"D_y_max_mm", "D_z_max_mm", "dx_y_max_mm", "dx_z_max_mm",
|
||||
"center_C_x_mm", "center_C_y_mm", "center_C_z_mm",
|
||||
"normal_x", "normal_y", "normal_z",
|
||||
"plane_inlier_ratio", "plane_residual_rms_mm",
|
||||
"registration_rmse_mm", "registration_max_residual_mm",
|
||||
"pipeline_seconds", "inner_stage_seconds",
|
||||
}
|
||||
for c_idx, (key, _) in enumerate(COLUMNS, start=1):
|
||||
if key in float_keys:
|
||||
for r_idx in range(2, len(rows) + 2):
|
||||
ws.cell(row=r_idx, column=c_idx).number_format = "0.000000"
|
||||
|
||||
# Auto-width (cap at 32).
|
||||
for c_idx, (_, label) in enumerate(COLUMNS, start=1):
|
||||
col_letter = ws.cell(row=1, column=c_idx).column_letter
|
||||
max_len = len(str(label))
|
||||
for r_idx in range(2, len(rows) + 2):
|
||||
v = ws.cell(row=r_idx, column=c_idx).value
|
||||
if v is None:
|
||||
continue
|
||||
max_len = max(max_len, len(str(v)))
|
||||
ws.column_dimensions[col_letter].width = min(max_len + 2, 32)
|
||||
|
||||
ws.freeze_panes = "D2"
|
||||
|
||||
wb.save(out)
|
||||
return out
|
||||
|
||||
|
||||
__all__ = ["EvalRow", "COLUMNS", "build_row_from_summary", "write_excel_report"]
|
||||
@@ -0,0 +1,124 @@
|
||||
"""Inspection-item Excel reports (项点报表).
|
||||
|
||||
Each report is one measurement item with the six columns the quality
|
||||
department expects:
|
||||
|
||||
项点名称 | 理论尺寸 | 公差要求 | 测量结果 | 误差结果 | 测量结论
|
||||
|
||||
For the coupler panel, the two deflection angles are reported in a
|
||||
**slope form** ``value/L mm`` (e.g. ``0.8/375mm``) rather than degrees,
|
||||
because the tolerance is specified that way. ``value`` is the lateral
|
||||
offset over a baseline of ``L`` mm:
|
||||
|
||||
value = L * tan(theta) (theta in radians)
|
||||
|
||||
so a measured angle ``theta`` becomes ``(L*tan theta)/L mm``. The
|
||||
tolerance ``T/L mm`` is the angle whose offset over ``L`` is ``T``; the
|
||||
result passes when ``|L*tan(theta)| <= T``.
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import math
|
||||
from dataclasses import dataclass
|
||||
from pathlib import Path
|
||||
|
||||
try:
|
||||
from openpyxl import Workbook
|
||||
from openpyxl.styles import Alignment, Font, PatternFill
|
||||
except ImportError as exc: # pragma: no cover
|
||||
raise ImportError(
|
||||
"openpyxl is required for Excel reporting; install with `pip install openpyxl`"
|
||||
) from exc
|
||||
|
||||
|
||||
COLUMNS = ("项点名称", "理论尺寸", "公差要求", "测量结果", "误差结果", "测量结论")
|
||||
|
||||
|
||||
@dataclass
|
||||
class InspectionItem:
|
||||
"""One deflection-angle inspection item, reported in ``value/L mm`` form."""
|
||||
|
||||
name: str # 项点名称
|
||||
theta_deg: float # measured angle (degrees), theory = 0
|
||||
baseline_mm: float # L in the slope form
|
||||
tol_value_mm: float # tolerance numerator T (so tolerance = T/L mm)
|
||||
|
||||
# --- derived ---
|
||||
@property
|
||||
def offset_mm(self) -> float:
|
||||
"""L * tan(theta): lateral offset over the baseline, mm."""
|
||||
return self.baseline_mm * math.tan(math.radians(self.theta_deg))
|
||||
|
||||
@property
|
||||
def theoretical_str(self) -> str:
|
||||
return f"0/{self.baseline_mm:g}mm"
|
||||
|
||||
@property
|
||||
def tolerance_str(self) -> str:
|
||||
return f"{self.tol_value_mm:g}/{self.baseline_mm:g}mm"
|
||||
|
||||
@property
|
||||
def measured_str(self) -> str:
|
||||
return f"{self.offset_mm:.6f}/{self.baseline_mm:g}mm"
|
||||
|
||||
@property
|
||||
def error_str(self) -> str:
|
||||
# Error == measured result here (theory is 0).
|
||||
return self.measured_str
|
||||
|
||||
@property
|
||||
def passed(self) -> bool:
|
||||
return abs(self.offset_mm) <= self.tol_value_mm
|
||||
|
||||
@property
|
||||
def conclusion_str(self) -> str:
|
||||
return "合格" if self.passed else "不合格"
|
||||
|
||||
def row(self) -> tuple[str, str, str, str, str, str]:
|
||||
return (
|
||||
self.name,
|
||||
self.theoretical_str,
|
||||
self.tolerance_str,
|
||||
self.measured_str,
|
||||
self.error_str,
|
||||
self.conclusion_str,
|
||||
)
|
||||
|
||||
|
||||
def write_inspection_report(item: InspectionItem, path: str | Path) -> Path:
|
||||
"""Write a single-item inspection report to an xlsx file."""
|
||||
out = Path(path)
|
||||
out.parent.mkdir(parents=True, exist_ok=True)
|
||||
|
||||
wb = Workbook()
|
||||
ws = wb.active
|
||||
ws.title = "inspection"
|
||||
|
||||
header_font = Font(bold=True)
|
||||
header_fill = PatternFill("solid", fgColor="DDDDDD")
|
||||
center = Alignment(horizontal="center", vertical="center")
|
||||
|
||||
for c, label in enumerate(COLUMNS, start=1):
|
||||
cell = ws.cell(row=1, column=c, value=label)
|
||||
cell.font = header_font
|
||||
cell.fill = header_fill
|
||||
cell.alignment = center
|
||||
|
||||
for c, val in enumerate(item.row(), start=1):
|
||||
cell = ws.cell(row=2, column=c, value=val)
|
||||
cell.alignment = center
|
||||
|
||||
# red font for 不合格 conclusion
|
||||
if not item.passed:
|
||||
ws.cell(row=2, column=6).font = Font(color="CC0000", bold=True)
|
||||
|
||||
widths = (44, 12, 14, 16, 16, 12)
|
||||
for c, w in enumerate(widths, start=1):
|
||||
ws.column_dimensions[ws.cell(row=1, column=c).column_letter].width = w
|
||||
|
||||
wb.save(out)
|
||||
return out
|
||||
|
||||
|
||||
__all__ = ["InspectionItem", "write_inspection_report", "COLUMNS"]
|
||||
@@ -0,0 +1,25 @@
|
||||
"""Region-of-interest pre-filter for the input point cloud.
|
||||
|
||||
The main pipeline takes a *raw* point cloud (PLY) plus a ROI JSON
|
||||
descriptor (an axis-aligned bounding box in the CAD frame) that tells
|
||||
which subset of the cloud should actually feed the downstream algorithm.
|
||||
|
||||
ROI JSON layout — six flat keys, all in mm::
|
||||
|
||||
{
|
||||
"x_min": -12500.0, "x_max": 12500.0,
|
||||
"y_min": 150.0, "y_max": 4100.0,
|
||||
"z_min": -1680.0, "z_max": 200.0
|
||||
}
|
||||
"""
|
||||
|
||||
from .schema import ROISpec, default_roi_spec, load_roi_json, validate_roi_spec
|
||||
from .apply import apply_roi
|
||||
|
||||
__all__ = [
|
||||
"ROISpec",
|
||||
"default_roi_spec",
|
||||
"load_roi_json",
|
||||
"validate_roi_spec",
|
||||
"apply_roi",
|
||||
]
|
||||
@@ -0,0 +1,48 @@
|
||||
"""Apply an axis-aligned bounding-box :class:`ROISpec` to a point cloud."""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
from typing import Any
|
||||
|
||||
import numpy as np
|
||||
|
||||
from .schema import ROISpec
|
||||
|
||||
|
||||
def apply_roi(points: np.ndarray, spec: ROISpec) -> tuple[np.ndarray, dict[str, Any]]:
|
||||
"""Filter ``points`` by the axis-aligned bounding box in ``spec``.
|
||||
|
||||
Returns ``(filtered_points, diagnostics)``. ``diagnostics`` is a small
|
||||
JSON-friendly dict suitable for embedding in a run report.
|
||||
"""
|
||||
pts = np.asarray(points, dtype=np.float64)
|
||||
n_in = int(pts.shape[0])
|
||||
|
||||
if n_in == 0:
|
||||
return pts.copy(), {
|
||||
"kind": spec.kind,
|
||||
"n_in": 0,
|
||||
"n_out": 0,
|
||||
"kept_ratio": 0.0,
|
||||
"bounds": spec.to_dict(),
|
||||
}
|
||||
|
||||
x, y, z = pts[:, 0], pts[:, 1], pts[:, 2]
|
||||
mask = (
|
||||
(x >= spec.x_min) & (x <= spec.x_max)
|
||||
& (y >= spec.y_min) & (y <= spec.y_max)
|
||||
& (z >= spec.z_min) & (z <= spec.z_max)
|
||||
)
|
||||
out = pts[mask]
|
||||
n_out = int(out.shape[0])
|
||||
|
||||
return out, {
|
||||
"kind": spec.kind,
|
||||
"n_in": n_in,
|
||||
"n_out": n_out,
|
||||
"kept_ratio": float(n_out / n_in),
|
||||
"bounds": spec.to_dict(),
|
||||
}
|
||||
|
||||
|
||||
__all__ = ["apply_roi"]
|
||||
@@ -0,0 +1,127 @@
|
||||
"""ROI JSON schema definition + loader.
|
||||
|
||||
The ROI descriptor is a **flat axis-aligned bounding box** in the CAD
|
||||
frame. Six numbers, no nesting:
|
||||
|
||||
{
|
||||
"x_min": -12500.0,
|
||||
"x_max": 12500.0,
|
||||
"y_min": 150.0,
|
||||
"y_max": 4100.0,
|
||||
"z_min": -1680.0,
|
||||
"z_max": 200.0
|
||||
}
|
||||
|
||||
Semantics
|
||||
---------
|
||||
|
||||
- All units in **mm**, all coordinates in the CAD frame (x = longitudinal,
|
||||
y = width, z = height).
|
||||
- A point ``(x, y, z)`` is kept iff::
|
||||
|
||||
x_min <= x <= x_max and y_min <= y <= y_max and z_min <= z <= z_max
|
||||
|
||||
- To pass the whole cloud through unchanged, supply bounds wider than the
|
||||
cloud's own bbox (or use :func:`default_roi_spec`, which returns
|
||||
``±∞`` on every axis).
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import json
|
||||
import math
|
||||
from dataclasses import dataclass
|
||||
from pathlib import Path
|
||||
from typing import Any
|
||||
|
||||
|
||||
@dataclass
|
||||
class ROISpec:
|
||||
"""Axis-aligned bounding box in CAD-frame mm."""
|
||||
|
||||
x_min: float
|
||||
x_max: float
|
||||
y_min: float
|
||||
y_max: float
|
||||
z_min: float
|
||||
z_max: float
|
||||
|
||||
@property
|
||||
def kind(self) -> str:
|
||||
"""Stable identifier for logging / reporting."""
|
||||
return "aabb"
|
||||
|
||||
def to_dict(self) -> dict[str, float]:
|
||||
return {
|
||||
"x_min": float(self.x_min),
|
||||
"x_max": float(self.x_max),
|
||||
"y_min": float(self.y_min),
|
||||
"y_max": float(self.y_max),
|
||||
"z_min": float(self.z_min),
|
||||
"z_max": float(self.z_max),
|
||||
}
|
||||
|
||||
|
||||
def default_roi_spec() -> ROISpec:
|
||||
"""Pass-through ROI: infinite bounds on every axis."""
|
||||
inf = math.inf
|
||||
return ROISpec(
|
||||
x_min=-inf, x_max=inf,
|
||||
y_min=-inf, y_max=inf,
|
||||
z_min=-inf, z_max=inf,
|
||||
)
|
||||
|
||||
|
||||
def validate_roi_spec(spec: ROISpec) -> None:
|
||||
"""Raise ``ValueError`` if any axis has ``min >= max``."""
|
||||
pairs = (
|
||||
("x", spec.x_min, spec.x_max),
|
||||
("y", spec.y_min, spec.y_max),
|
||||
("z", spec.z_min, spec.z_max),
|
||||
)
|
||||
for axis, lo, hi in pairs:
|
||||
if not (lo < hi):
|
||||
raise ValueError(
|
||||
f"ROI {axis}_min must be strictly less than {axis}_max "
|
||||
f"(got {axis}_min={lo}, {axis}_max={hi})"
|
||||
)
|
||||
|
||||
|
||||
def load_roi_json(path: str | Path | None) -> ROISpec:
|
||||
"""Read a ROI JSON file. ``None`` → default pass-through spec.
|
||||
|
||||
The file must contain all six keys ``x_min`` / ``x_max`` / ``y_min`` /
|
||||
``y_max`` / ``z_min`` / ``z_max``. Extra keys are ignored (so you
|
||||
can attach a ``comment`` etc. without the loader complaining).
|
||||
"""
|
||||
if path is None:
|
||||
return default_roi_spec()
|
||||
path = Path(path)
|
||||
if not path.exists():
|
||||
raise FileNotFoundError(f"roi.json not found: {path}")
|
||||
with path.open("r", encoding="utf-8") as fh:
|
||||
data: dict[str, Any] = json.load(fh)
|
||||
|
||||
required = ("x_min", "x_max", "y_min", "y_max", "z_min", "z_max")
|
||||
missing = [k for k in required if k not in data]
|
||||
if missing:
|
||||
raise ValueError(f"roi.json {path} is missing required keys: {missing}")
|
||||
|
||||
spec = ROISpec(
|
||||
x_min=float(data["x_min"]),
|
||||
x_max=float(data["x_max"]),
|
||||
y_min=float(data["y_min"]),
|
||||
y_max=float(data["y_max"]),
|
||||
z_min=float(data["z_min"]),
|
||||
z_max=float(data["z_max"]),
|
||||
)
|
||||
validate_roi_spec(spec)
|
||||
return spec
|
||||
|
||||
|
||||
__all__ = [
|
||||
"ROISpec",
|
||||
"default_roi_spec",
|
||||
"validate_roi_spec",
|
||||
"load_roi_json",
|
||||
]
|
||||
@@ -0,0 +1,61 @@
|
||||
"""Mesh-to-point-cloud sampling strategies.
|
||||
|
||||
Unified entry point::
|
||||
|
||||
from coupler_panel.sampling import sample_mesh
|
||||
pts = sample_mesh(mesh, method="poisson_disk", num_points=100_000)
|
||||
|
||||
Each strategy lives in its own module; the dispatcher just routes by name.
|
||||
``mesh`` must expose ``vertices: (V, 3)`` and ``faces: (F, 3)`` as ndarrays
|
||||
(such as our :class:`coupler_panel.cad.mesh.TriangleMesh`).
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
from typing import Any
|
||||
|
||||
import numpy as np
|
||||
|
||||
from .uniform_area import sample_uniform_area
|
||||
from .poisson_disk import sample_poisson_disk
|
||||
from .adaptive import sample_adaptive
|
||||
from .grid_spacing import sample_grid_spacing, num_points_from_spacing
|
||||
|
||||
|
||||
_DISPATCH = {
|
||||
"uniform_area": sample_uniform_area,
|
||||
"poisson_disk": sample_poisson_disk,
|
||||
"adaptive": sample_adaptive,
|
||||
"grid_spacing": sample_grid_spacing,
|
||||
}
|
||||
|
||||
|
||||
def sample_mesh(mesh, method: str, **kwargs) -> np.ndarray:
|
||||
"""Dispatch to the named strategy. ``**kwargs`` are passed through."""
|
||||
if method not in _DISPATCH:
|
||||
raise ValueError(
|
||||
f"unknown sampling method '{method}'; valid: {sorted(_DISPATCH)}"
|
||||
)
|
||||
return _DISPATCH[method](mesh, **kwargs)
|
||||
|
||||
|
||||
def resolve_num_points(mesh, *, num_points: int | None, spacing_mm: float | None) -> int:
|
||||
"""If ``spacing_mm`` is given, convert it to a point count using the
|
||||
mesh surface area; otherwise return ``num_points``.
|
||||
"""
|
||||
if spacing_mm is not None and spacing_mm > 0:
|
||||
return num_points_from_spacing(mesh, spacing_mm)
|
||||
if num_points is None:
|
||||
raise ValueError("either num_points or spacing_mm must be set")
|
||||
return int(num_points)
|
||||
|
||||
|
||||
__all__ = [
|
||||
"sample_mesh",
|
||||
"sample_uniform_area",
|
||||
"sample_poisson_disk",
|
||||
"sample_adaptive",
|
||||
"sample_grid_spacing",
|
||||
"resolve_num_points",
|
||||
"num_points_from_spacing",
|
||||
]
|
||||
@@ -0,0 +1,104 @@
|
||||
"""Curvature-adaptive sampling.
|
||||
|
||||
Idea: weight every triangle by ``area * (1 + k * curvature_proxy)``, then
|
||||
do area-weighted sampling on the *weighted* areas. The curvature proxy is
|
||||
``1 - mean_dot(face_normal, neighbour_face_normals)``, normalised to
|
||||
[0, 1] across the mesh.
|
||||
|
||||
This produces denser samples on rounded edges, hole boundaries, and small
|
||||
features without needing a real curvature operator.
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
from typing import Any
|
||||
|
||||
import numpy as np
|
||||
|
||||
from .uniform_area import _triangle_areas, sample_uniform_area
|
||||
|
||||
|
||||
def _face_normals(vertices: np.ndarray, faces: np.ndarray) -> np.ndarray:
|
||||
a = vertices[faces[:, 0]]
|
||||
b = vertices[faces[:, 1]]
|
||||
c = vertices[faces[:, 2]]
|
||||
n = np.cross(b - a, c - a)
|
||||
norms = np.linalg.norm(n, axis=1, keepdims=True)
|
||||
norms[norms < 1e-12] = 1.0
|
||||
return n / norms
|
||||
|
||||
|
||||
def _vertex_face_adjacency(faces: np.ndarray, n_vertices: int) -> list[list[int]]:
|
||||
adj: list[list[int]] = [[] for _ in range(n_vertices)]
|
||||
for i, f in enumerate(faces):
|
||||
adj[f[0]].append(i)
|
||||
adj[f[1]].append(i)
|
||||
adj[f[2]].append(i)
|
||||
return adj
|
||||
|
||||
|
||||
def _curvature_proxy(vertices: np.ndarray, faces: np.ndarray) -> np.ndarray:
|
||||
"""Per-face roughness in [0, 1]."""
|
||||
fn = _face_normals(vertices, faces)
|
||||
adj = _vertex_face_adjacency(faces, vertices.shape[0])
|
||||
proxy = np.zeros(faces.shape[0], dtype=np.float64)
|
||||
for i, f in enumerate(faces):
|
||||
neighbours: set[int] = set()
|
||||
for v in f:
|
||||
neighbours.update(adj[v])
|
||||
neighbours.discard(i)
|
||||
if not neighbours:
|
||||
continue
|
||||
nb = fn[list(neighbours)]
|
||||
d = np.clip(nb @ fn[i], -1.0, 1.0)
|
||||
proxy[i] = 1.0 - np.mean(d)
|
||||
if proxy.max() > 0:
|
||||
proxy = proxy / proxy.max()
|
||||
return proxy
|
||||
|
||||
|
||||
def sample_adaptive(
|
||||
mesh,
|
||||
*,
|
||||
num_points: int,
|
||||
seed: int | None = 42,
|
||||
curvature_weight: float = 1.0,
|
||||
**_: Any,
|
||||
) -> np.ndarray:
|
||||
"""Curvature-weighted sampling."""
|
||||
if num_points <= 0:
|
||||
return np.empty((0, 3), dtype=np.float64)
|
||||
|
||||
v = np.asarray(mesh.vertices, dtype=np.float64)
|
||||
f = np.asarray(mesh.faces, dtype=np.int64)
|
||||
if f.shape[0] == 0:
|
||||
return np.empty((0, 3), dtype=np.float64)
|
||||
|
||||
# On large meshes the O(F·avg_degree) curvature pass is expensive; if we
|
||||
# are above ~200k faces, fall back to plain uniform_area, which is the
|
||||
# natural definition when curvature_weight = 0.
|
||||
if f.shape[0] > 200_000:
|
||||
return sample_uniform_area(mesh, num_points=num_points, seed=seed)
|
||||
|
||||
areas = _triangle_areas(v, f)
|
||||
proxy = _curvature_proxy(v, f)
|
||||
weights = areas * (1.0 + float(curvature_weight) * proxy)
|
||||
total = weights.sum()
|
||||
if total <= 0:
|
||||
return sample_uniform_area(mesh, num_points=num_points, seed=seed)
|
||||
|
||||
rng = np.random.default_rng(seed)
|
||||
probs = weights / total
|
||||
tri_idx = rng.choice(f.shape[0], size=num_points, p=probs)
|
||||
u = rng.random(num_points)
|
||||
w = rng.random(num_points)
|
||||
flip = u + w > 1.0
|
||||
u[flip] = 1.0 - u[flip]
|
||||
w[flip] = 1.0 - w[flip]
|
||||
a = v[f[tri_idx, 0]]
|
||||
b = v[f[tri_idx, 1]]
|
||||
c = v[f[tri_idx, 2]]
|
||||
return a + (b - a) * u[:, None] + (c - a) * w[:, None]
|
||||
|
||||
|
||||
__all__ = ["sample_adaptive"]
|
||||
@@ -0,0 +1,56 @@
|
||||
"""Spacing-driven sampling.
|
||||
|
||||
Translates a target inter-point spacing in mm into a point count, then
|
||||
defers to area-weighted sampling. Useful when you want to sweep over
|
||||
``spacing_mm ∈ {0.25, 0.5, 1, 2, 5}`` rather than over absolute counts.
|
||||
|
||||
For an isotropic surface sample, the mean nearest-neighbour distance is
|
||||
roughly ``sqrt(area / n)`` (square cells). We invert that:
|
||||
|
||||
n ≈ area / spacing²
|
||||
|
||||
and add a small fudge factor of 1.15 to compensate for sample variance.
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
from typing import Any
|
||||
|
||||
import numpy as np
|
||||
|
||||
from .uniform_area import _triangle_areas, sample_uniform_area
|
||||
|
||||
|
||||
def num_points_from_spacing(mesh, spacing_mm: float, *, fudge: float = 1.15) -> int:
|
||||
if spacing_mm <= 0:
|
||||
raise ValueError("spacing_mm must be > 0")
|
||||
v = np.asarray(mesh.vertices, dtype=np.float64)
|
||||
f = np.asarray(mesh.faces, dtype=np.int64)
|
||||
total_area = float(_triangle_areas(v, f).sum())
|
||||
n = int(round(fudge * total_area / (spacing_mm ** 2)))
|
||||
return max(n, 1)
|
||||
|
||||
|
||||
def sample_grid_spacing(
|
||||
mesh,
|
||||
*,
|
||||
spacing_mm: float | None = None,
|
||||
num_points: int | None = None,
|
||||
seed: int | None = 42,
|
||||
fudge: float = 1.15,
|
||||
**_: Any,
|
||||
) -> np.ndarray:
|
||||
"""If ``spacing_mm`` is given, derive ``num_points`` from it.
|
||||
|
||||
Falls through to area-weighted uniform sampling.
|
||||
"""
|
||||
if spacing_mm is not None:
|
||||
n = num_points_from_spacing(mesh, float(spacing_mm), fudge=fudge)
|
||||
elif num_points is not None:
|
||||
n = int(num_points)
|
||||
else:
|
||||
raise ValueError("either spacing_mm or num_points must be set")
|
||||
return sample_uniform_area(mesh, num_points=n, seed=seed)
|
||||
|
||||
|
||||
__all__ = ["sample_grid_spacing", "num_points_from_spacing"]
|
||||
@@ -0,0 +1,95 @@
|
||||
"""Poisson-disk surface sampling.
|
||||
|
||||
Uses ``open3d.geometry.TriangleMesh.sample_points_poisson_disk`` when
|
||||
open3d is available; falls back to area-weighted uniform sampling
|
||||
followed by an O(N²) Bridson-style culling on small meshes.
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
from typing import Any
|
||||
|
||||
import numpy as np
|
||||
|
||||
from .uniform_area import sample_uniform_area
|
||||
|
||||
|
||||
def _try_open3d():
|
||||
try:
|
||||
import open3d as o3d # type: ignore
|
||||
|
||||
return o3d
|
||||
except ImportError:
|
||||
return None
|
||||
|
||||
|
||||
def sample_poisson_disk(
|
||||
mesh,
|
||||
*,
|
||||
num_points: int,
|
||||
seed: int | None = 42,
|
||||
init_factor: int = 5,
|
||||
**_: Any,
|
||||
) -> np.ndarray:
|
||||
"""Poisson-disk samples on a triangle mesh."""
|
||||
if num_points <= 0:
|
||||
return np.empty((0, 3), dtype=np.float64)
|
||||
|
||||
o3d = _try_open3d()
|
||||
if o3d is not None:
|
||||
# open3d wants its own mesh class.
|
||||
m = o3d.geometry.TriangleMesh()
|
||||
m.vertices = o3d.utility.Vector3dVector(np.asarray(mesh.vertices, dtype=np.float64))
|
||||
m.triangles = o3d.utility.Vector3iVector(np.asarray(mesh.faces, dtype=np.int32))
|
||||
m.compute_vertex_normals()
|
||||
# Open3D's API takes seed via 'seed' kwarg (0.18+) — passing it best-effort.
|
||||
try:
|
||||
pcd = m.sample_points_poisson_disk(
|
||||
number_of_points=int(num_points),
|
||||
init_factor=int(init_factor),
|
||||
seed=int(seed) if seed is not None else -1,
|
||||
)
|
||||
except TypeError:
|
||||
pcd = m.sample_points_poisson_disk(
|
||||
number_of_points=int(num_points),
|
||||
init_factor=int(init_factor),
|
||||
)
|
||||
return np.asarray(pcd.points, dtype=np.float64)
|
||||
|
||||
# ---- Fallback: oversample + grid cull ---------------------------------
|
||||
over = sample_uniform_area(
|
||||
mesh,
|
||||
num_points=num_points * max(init_factor, 5),
|
||||
seed=seed,
|
||||
)
|
||||
# Estimate target spacing from mesh bbox.
|
||||
bbox = over.max(axis=0) - over.min(axis=0)
|
||||
if num_points <= 1:
|
||||
return over[:num_points]
|
||||
target_spacing = float(np.cbrt(np.prod(bbox)) / max(num_points, 1) ** (1 / 3))
|
||||
return _grid_cull(over, target_spacing, num_points, seed)
|
||||
|
||||
|
||||
def _grid_cull(
|
||||
points: np.ndarray,
|
||||
spacing: float,
|
||||
keep_n: int,
|
||||
seed: int | None,
|
||||
) -> np.ndarray:
|
||||
"""Pick at most one point per voxel of size ``spacing``."""
|
||||
rng = np.random.default_rng(seed)
|
||||
perm = rng.permutation(points.shape[0])
|
||||
voxel = np.floor(points[perm] / max(spacing, 1e-6)).astype(np.int64)
|
||||
seen: set[tuple[int, int, int]] = set()
|
||||
out_idx: list[int] = []
|
||||
for k, key in enumerate(map(tuple, voxel)):
|
||||
if key in seen:
|
||||
continue
|
||||
seen.add(key)
|
||||
out_idx.append(perm[k])
|
||||
if len(out_idx) >= keep_n:
|
||||
break
|
||||
return points[np.asarray(out_idx)]
|
||||
|
||||
|
||||
__all__ = ["sample_poisson_disk"]
|
||||
@@ -0,0 +1,53 @@
|
||||
"""Area-weighted uniform triangle sampling."""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
from typing import Any
|
||||
|
||||
import numpy as np
|
||||
|
||||
|
||||
def _triangle_areas(vertices: np.ndarray, faces: np.ndarray) -> np.ndarray:
|
||||
a = vertices[faces[:, 0]]
|
||||
b = vertices[faces[:, 1]]
|
||||
c = vertices[faces[:, 2]]
|
||||
return 0.5 * np.linalg.norm(np.cross(b - a, c - a), axis=1)
|
||||
|
||||
|
||||
def sample_uniform_area(
|
||||
mesh,
|
||||
*,
|
||||
num_points: int,
|
||||
seed: int | None = 42,
|
||||
**_: Any,
|
||||
) -> np.ndarray:
|
||||
"""Sample ``num_points`` from a triangle mesh, weighted by area.
|
||||
|
||||
Returns an ``(N, 3) float64`` ndarray.
|
||||
"""
|
||||
if num_points <= 0:
|
||||
return np.empty((0, 3), dtype=np.float64)
|
||||
|
||||
v = np.asarray(mesh.vertices, dtype=np.float64)
|
||||
f = np.asarray(mesh.faces, dtype=np.int64)
|
||||
areas = _triangle_areas(v, f)
|
||||
total = float(areas.sum())
|
||||
if total <= 0:
|
||||
raise ValueError("mesh has zero total area")
|
||||
|
||||
rng = np.random.default_rng(seed)
|
||||
|
||||
probs = areas / total
|
||||
tri_idx = rng.choice(len(f), size=num_points, p=probs)
|
||||
u = rng.random(num_points)
|
||||
w = rng.random(num_points)
|
||||
flip = u + w > 1.0
|
||||
u[flip] = 1.0 - u[flip]
|
||||
w[flip] = 1.0 - w[flip]
|
||||
a = v[f[tri_idx, 0]]
|
||||
b = v[f[tri_idx, 1]]
|
||||
c = v[f[tri_idx, 2]]
|
||||
return a + (b - a) * u[:, None] + (c - a) * w[:, None]
|
||||
|
||||
|
||||
__all__ = ["sample_uniform_area"]
|
||||
@@ -0,0 +1,21 @@
|
||||
"""Synthetic-scan utilities: noise, rigid transform, optional view simulator."""
|
||||
|
||||
from .noise import add_gaussian_noise
|
||||
from .transform import (
|
||||
RandomRigidTransform,
|
||||
apply_rigid,
|
||||
compose_rigid,
|
||||
invert_rigid,
|
||||
sample_random_rigid,
|
||||
)
|
||||
from .scan_simulator import simulate_scan_view
|
||||
|
||||
__all__ = [
|
||||
"add_gaussian_noise",
|
||||
"RandomRigidTransform",
|
||||
"apply_rigid",
|
||||
"compose_rigid",
|
||||
"invert_rigid",
|
||||
"sample_random_rigid",
|
||||
"simulate_scan_view",
|
||||
]
|
||||
@@ -0,0 +1,57 @@
|
||||
"""Synthetic Gaussian noise generation."""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import numpy as np
|
||||
|
||||
|
||||
def add_gaussian_noise(
|
||||
points: np.ndarray,
|
||||
*,
|
||||
sigma_mm: float,
|
||||
mode: str = "isotropic",
|
||||
normals: np.ndarray | None = None,
|
||||
seed: int | None = 43,
|
||||
) -> np.ndarray:
|
||||
"""Return a noisy copy of ``points``.
|
||||
|
||||
Parameters
|
||||
----------
|
||||
points : (N, 3)
|
||||
sigma_mm : float
|
||||
Standard deviation in mm. ``0`` is a no-op (returns a copy).
|
||||
mode : {"isotropic", "along_normal"}
|
||||
``isotropic`` — N(0, σ²) noise added independently to x/y/z.
|
||||
``along_normal`` — N(0, σ²) along ``normals`` only. Requires
|
||||
a per-point normal of shape ``(N, 3)``.
|
||||
normals : (N, 3) or None
|
||||
Required when ``mode == "along_normal"``.
|
||||
"""
|
||||
points = np.asarray(points, dtype=np.float64)
|
||||
if sigma_mm == 0:
|
||||
return points.copy()
|
||||
if sigma_mm < 0:
|
||||
raise ValueError("sigma_mm must be ≥ 0")
|
||||
|
||||
rng = np.random.default_rng(seed)
|
||||
if mode == "isotropic":
|
||||
return points + rng.normal(0.0, sigma_mm, size=points.shape)
|
||||
|
||||
if mode == "along_normal":
|
||||
if normals is None:
|
||||
raise ValueError("along_normal mode requires per-point normals")
|
||||
normals = np.asarray(normals, dtype=np.float64)
|
||||
if normals.shape != points.shape:
|
||||
raise ValueError(
|
||||
f"normals shape {normals.shape} != points shape {points.shape}"
|
||||
)
|
||||
norms = np.linalg.norm(normals, axis=1, keepdims=True)
|
||||
norms[norms < 1e-12] = 1.0
|
||||
unit = normals / norms
|
||||
offset = rng.normal(0.0, sigma_mm, size=points.shape[0])[:, None]
|
||||
return points + unit * offset
|
||||
|
||||
raise ValueError(f"unknown noise mode '{mode}'")
|
||||
|
||||
|
||||
__all__ = ["add_gaussian_noise"]
|
||||
@@ -0,0 +1,80 @@
|
||||
"""Single-view scan simulator (optional).
|
||||
|
||||
Models a structured-light / line-laser scanner that only sees points
|
||||
visible from a single camera origin. Implementation is the *hidden-point
|
||||
removal* (HPR) of Katz, Tal & Basri (2007), which works directly on a
|
||||
point cloud — no mesh raycasting needed.
|
||||
|
||||
Open3D ships an HPR primitive; we route to that when available, with a
|
||||
plain-numpy fallback for the simple convex-hull formulation.
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import numpy as np
|
||||
|
||||
|
||||
def _try_open3d():
|
||||
try:
|
||||
import open3d as o3d # type: ignore
|
||||
|
||||
return o3d
|
||||
except ImportError:
|
||||
return None
|
||||
|
||||
|
||||
def simulate_scan_view(
|
||||
points: np.ndarray,
|
||||
*,
|
||||
view_origin,
|
||||
hpr_radius_mm: float,
|
||||
) -> np.ndarray:
|
||||
"""Return only the points visible from ``view_origin``.
|
||||
|
||||
Parameters
|
||||
----------
|
||||
view_origin : (3,)
|
||||
hpr_radius_mm : float
|
||||
HPR sphere radius. Larger = more points kept. Pick something
|
||||
much larger than the cloud diameter (e.g. 100× diameter) to
|
||||
approximate a "all front-facing surfaces" view.
|
||||
"""
|
||||
pts = np.asarray(points, dtype=np.float64)
|
||||
o = np.asarray(view_origin, dtype=np.float64).reshape(3)
|
||||
|
||||
o3d = _try_open3d()
|
||||
if o3d is not None:
|
||||
pcd = o3d.geometry.PointCloud()
|
||||
pcd.points = o3d.utility.Vector3dVector(pts)
|
||||
_, idx = pcd.hidden_point_removal(o, float(hpr_radius_mm))
|
||||
return pts[np.asarray(idx, dtype=np.int64)]
|
||||
|
||||
# ---- numpy fallback: spherical-flip + convex-hull selection ----------
|
||||
# Flip every point through a sphere of radius R centred at the view.
|
||||
diff = pts - o
|
||||
norms = np.linalg.norm(diff, axis=1, keepdims=True)
|
||||
norms[norms < 1e-12] = 1.0
|
||||
R = float(hpr_radius_mm)
|
||||
flipped = pts + 2.0 * (R - norms) * (diff / norms)
|
||||
# Add the viewpoint itself as a phantom point so its convex hull
|
||||
# encloses the visible-from-origin set.
|
||||
augmented = np.vstack([flipped, o[None, :]])
|
||||
try:
|
||||
from scipy.spatial import ConvexHull
|
||||
|
||||
hull = ConvexHull(augmented, qhull_options="QJ")
|
||||
keep = np.asarray(hull.vertices, dtype=np.int64)
|
||||
# Drop the phantom point.
|
||||
keep = keep[keep < pts.shape[0]]
|
||||
return pts[keep]
|
||||
except Exception:
|
||||
# Last-resort: keep points whose dot with (point - origin) along
|
||||
# the centroid axis is positive. Crude but never crashes.
|
||||
centroid = pts.mean(axis=0)
|
||||
view_dir = centroid - o
|
||||
view_dir /= max(np.linalg.norm(view_dir), 1e-12)
|
||||
d = (pts - o) @ view_dir
|
||||
return pts[d > 0]
|
||||
|
||||
|
||||
__all__ = ["simulate_scan_view"]
|
||||
@@ -0,0 +1,162 @@
|
||||
"""Random rigid transforms used to mimic mounting jitter / scan pose noise.
|
||||
|
||||
The transform is stored as a 4×4 homogeneous matrix; the GT used for
|
||||
validation is exactly this matrix. The 4-hole Kabsch registration step
|
||||
should recover its inverse up to noise.
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
from dataclasses import dataclass
|
||||
from typing import Sequence
|
||||
|
||||
import numpy as np
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class RandomRigidTransform:
|
||||
"""Outcome of one random rigid sample."""
|
||||
|
||||
matrix: np.ndarray # (4, 4) float64
|
||||
rotation_deg: float
|
||||
rotation_axis: tuple[float, float, float]
|
||||
translation_mm: float
|
||||
translation_dir: tuple[float, float, float]
|
||||
seed: int | None
|
||||
|
||||
def to_dict(self) -> dict:
|
||||
return {
|
||||
"matrix": self.matrix.tolist(),
|
||||
"rotation_deg": self.rotation_deg,
|
||||
"rotation_axis": list(self.rotation_axis),
|
||||
"translation_mm": self.translation_mm,
|
||||
"translation_dir": list(self.translation_dir),
|
||||
"seed": self.seed,
|
||||
}
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# Construction
|
||||
# ---------------------------------------------------------------------------
|
||||
|
||||
|
||||
def _rodrigues(axis: np.ndarray, angle_rad: float) -> np.ndarray:
|
||||
"""Rodrigues rotation around a unit ``axis``."""
|
||||
axis = axis / max(np.linalg.norm(axis), 1e-12)
|
||||
K = np.array(
|
||||
[
|
||||
[0.0, -axis[2], axis[1]],
|
||||
[axis[2], 0.0, -axis[0]],
|
||||
[-axis[1], axis[0], 0.0],
|
||||
],
|
||||
dtype=np.float64,
|
||||
)
|
||||
s = np.sin(angle_rad)
|
||||
c = np.cos(angle_rad)
|
||||
return np.eye(3) + s * K + (1.0 - c) * (K @ K)
|
||||
|
||||
|
||||
def _resolve_axis(axis_spec, rng) -> np.ndarray:
|
||||
if isinstance(axis_spec, str):
|
||||
spec = axis_spec.lower()
|
||||
if spec == "x":
|
||||
return np.array([1.0, 0.0, 0.0])
|
||||
if spec == "y":
|
||||
return np.array([0.0, 1.0, 0.0])
|
||||
if spec == "z":
|
||||
return np.array([0.0, 0.0, 1.0])
|
||||
if spec == "random":
|
||||
v = rng.normal(size=3)
|
||||
return v / max(np.linalg.norm(v), 1e-12)
|
||||
raise ValueError(f"unknown rotation_axis '{axis_spec}'")
|
||||
arr = np.asarray(axis_spec, dtype=np.float64)
|
||||
if arr.shape != (3,):
|
||||
raise ValueError("rotation_axis must be 'x'/'y'/'z'/'random' or a 3-vector")
|
||||
n = np.linalg.norm(arr)
|
||||
if n < 1e-12:
|
||||
raise ValueError("rotation_axis vector is degenerate")
|
||||
return arr / n
|
||||
|
||||
|
||||
def _resolve_dir(dir_spec, rng) -> np.ndarray:
|
||||
if isinstance(dir_spec, str):
|
||||
if dir_spec == "random":
|
||||
v = rng.normal(size=3)
|
||||
return v / max(np.linalg.norm(v), 1e-12)
|
||||
raise ValueError(f"unknown translation_dir '{dir_spec}'")
|
||||
arr = np.asarray(dir_spec, dtype=np.float64)
|
||||
if arr.shape != (3,):
|
||||
raise ValueError("translation_dir must be 'random' or a 3-vector")
|
||||
n = np.linalg.norm(arr)
|
||||
if n < 1e-12:
|
||||
raise ValueError("translation_dir vector is degenerate")
|
||||
return arr / n
|
||||
|
||||
|
||||
def sample_random_rigid(
|
||||
*,
|
||||
rotation_deg: float,
|
||||
translation_mm: float,
|
||||
rotation_axis: str | Sequence[float] = "random",
|
||||
translation_dir: str | Sequence[float] = "random",
|
||||
seed: int | None = 44,
|
||||
) -> RandomRigidTransform:
|
||||
rng = np.random.default_rng(seed)
|
||||
axis = _resolve_axis(rotation_axis, rng)
|
||||
direction = _resolve_dir(translation_dir, rng)
|
||||
|
||||
R = _rodrigues(axis, np.deg2rad(float(rotation_deg)))
|
||||
t = direction * float(translation_mm)
|
||||
|
||||
M = np.eye(4)
|
||||
M[:3, :3] = R
|
||||
M[:3, 3] = t
|
||||
|
||||
return RandomRigidTransform(
|
||||
matrix=M,
|
||||
rotation_deg=float(rotation_deg),
|
||||
rotation_axis=tuple(axis.tolist()),
|
||||
translation_mm=float(translation_mm),
|
||||
translation_dir=tuple(direction.tolist()),
|
||||
seed=seed,
|
||||
)
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# Application
|
||||
# ---------------------------------------------------------------------------
|
||||
|
||||
|
||||
def apply_rigid(points: np.ndarray, T: np.ndarray) -> np.ndarray:
|
||||
"""Apply a 4×4 homogeneous transform to an ``(N, 3)`` cloud."""
|
||||
P = np.asarray(points, dtype=np.float64)
|
||||
if P.ndim != 2 or P.shape[1] != 3:
|
||||
raise ValueError(f"expected (N,3), got {P.shape}")
|
||||
R = np.asarray(T, dtype=np.float64)[:3, :3]
|
||||
t = np.asarray(T, dtype=np.float64)[:3, 3]
|
||||
return P @ R.T + t
|
||||
|
||||
|
||||
def compose_rigid(*Ts: np.ndarray) -> np.ndarray:
|
||||
out = np.eye(4)
|
||||
for T in Ts:
|
||||
out = out @ np.asarray(T, dtype=np.float64)
|
||||
return out
|
||||
|
||||
|
||||
def invert_rigid(T: np.ndarray) -> np.ndarray:
|
||||
R = T[:3, :3]
|
||||
t = T[:3, 3]
|
||||
inv = np.eye(4)
|
||||
inv[:3, :3] = R.T
|
||||
inv[:3, 3] = -R.T @ t
|
||||
return inv
|
||||
|
||||
|
||||
__all__ = [
|
||||
"RandomRigidTransform",
|
||||
"sample_random_rigid",
|
||||
"apply_rigid",
|
||||
"compose_rigid",
|
||||
"invert_rigid",
|
||||
]
|
||||
@@ -0,0 +1,148 @@
|
||||
"""Result dataclasses for coupler_panel.
|
||||
|
||||
These types form the **stable public schema** of the package; their
|
||||
field names and semantics are guarded by ``SCHEMA_VERSION`` (see
|
||||
``_version.py``).
|
||||
|
||||
All numeric units are **mm** for distances and **degree** for angles
|
||||
unless otherwise noted.
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
from dataclasses import asdict, dataclass, field
|
||||
from typing import Any
|
||||
|
||||
import numpy as np
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# Building blocks
|
||||
# ---------------------------------------------------------------------------
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class Hole:
|
||||
"""A single detected (or GT) circular hole on the panel."""
|
||||
|
||||
center: tuple[float, float, float] # (x, y, z), mm
|
||||
radius_mm: float
|
||||
fit_residual_rms_mm: float # 0 for GT
|
||||
role: str # "locating" | "central" | "unknown"
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class HoleSet:
|
||||
"""The four locating holes plus an optional central hole.
|
||||
|
||||
``locating`` is ordered consistently between GT and detection so that
|
||||
Kabsch pairing is index-aligned.
|
||||
"""
|
||||
|
||||
locating: tuple[Hole, Hole, Hole, Hole]
|
||||
central: Hole | None = None
|
||||
|
||||
def locating_centers(self) -> np.ndarray:
|
||||
"""Return a (4, 3) ndarray of locating-hole centres."""
|
||||
return np.asarray([h.center for h in self.locating], dtype=np.float64)
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class PanelPlane:
|
||||
"""The fitted (or GT) panel plane."""
|
||||
|
||||
normal: tuple[float, float, float] # unit vector, n_x > 0 enforced
|
||||
point_on_plane: tuple[float, float, float]
|
||||
d_offset: float # n . x + d = 0
|
||||
inlier_ratio: float # 1.0 for GT
|
||||
inlier_residual_rms_mm: float # 0 for GT
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class ReferenceFrame:
|
||||
"""Reference geometry derived from the four-hole panel centre."""
|
||||
|
||||
center_C: tuple[float, float, float]
|
||||
# Reference line L = C + lam * e_x (always parallel to the +x axis).
|
||||
line_direction: tuple[float, float, float] = (1.0, 0.0, 0.0)
|
||||
# Reference plane: z = C_z. Stored as plane normal + offset.
|
||||
reference_plane_z: float = 0.0
|
||||
# Longitudinal mid-plane: y = C_y.
|
||||
longitudinal_mid_y: float = 0.0
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class StageTiming:
|
||||
"""Per-stage wall-clock and peak-memory cost."""
|
||||
|
||||
stage: str
|
||||
seconds: float
|
||||
peak_kib: float
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# Top-level result
|
||||
# ---------------------------------------------------------------------------
|
||||
|
||||
|
||||
@dataclass
|
||||
class CouplerMeasurement:
|
||||
"""Top-level result of the coupler-panel measurement pipeline.
|
||||
|
||||
See ``docs/USAGE.md`` for the stable-API contract.
|
||||
"""
|
||||
|
||||
schema_version: str
|
||||
algorithm_version: str
|
||||
|
||||
# --- Primary measurands ---
|
||||
theta_right_deg: float # atan2(n_y, n_x), with right_axis_sign applied
|
||||
theta_ref_deg: float # atan2(n_z, n_x)
|
||||
D_y_max_mm: float # max |y_i - C_y| over panel ROI
|
||||
D_z_max_mm: float # max |z_i - C_z| over panel ROI
|
||||
dx_y_max_mm: float # |A| * D_y_max, A = -n_y / n_x
|
||||
dx_z_max_mm: float # |B| * D_z_max, B = -n_z / n_x
|
||||
|
||||
# --- Geometry ---
|
||||
center_C: tuple[float, float, float]
|
||||
normal: tuple[float, float, float] # n_x > 0
|
||||
holes: HoleSet
|
||||
plane: PanelPlane
|
||||
frame: ReferenceFrame
|
||||
|
||||
# --- Diagnostics ---
|
||||
quality_flags: list[str] = field(default_factory=list)
|
||||
timings: list[StageTiming] = field(default_factory=list)
|
||||
report: dict[str, Any] = field(default_factory=dict)
|
||||
|
||||
def to_dict(self) -> dict[str, Any]:
|
||||
"""Return a fully JSON-serialisable dict."""
|
||||
return _to_jsonable(asdict(self))
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# Helpers
|
||||
# ---------------------------------------------------------------------------
|
||||
|
||||
|
||||
def _to_jsonable(obj: Any) -> Any:
|
||||
"""Recursively convert numpy / tuple structures into plain Python types."""
|
||||
if isinstance(obj, dict):
|
||||
return {k: _to_jsonable(v) for k, v in obj.items()}
|
||||
if isinstance(obj, (list, tuple)):
|
||||
return [_to_jsonable(v) for v in obj]
|
||||
if isinstance(obj, np.ndarray):
|
||||
return obj.tolist()
|
||||
if isinstance(obj, (np.floating, np.integer)):
|
||||
return obj.item()
|
||||
return obj
|
||||
|
||||
|
||||
__all__ = [
|
||||
"Hole",
|
||||
"HoleSet",
|
||||
"PanelPlane",
|
||||
"ReferenceFrame",
|
||||
"StageTiming",
|
||||
"CouplerMeasurement",
|
||||
]
|
||||
@@ -0,0 +1,21 @@
|
||||
"""Diagnostic visualisation utilities (matplotlib + open3d offscreen).
|
||||
|
||||
All renderers degrade gracefully if matplotlib / open3d are missing —
|
||||
they raise :class:`ImportError` only when actually called. The pipeline
|
||||
calls them via :func:`render_all`, which silently skips on missing deps.
|
||||
"""
|
||||
|
||||
from .cloud_view import render_cloud, render_cloud_compare
|
||||
from .holes_view import render_holes
|
||||
from .plane_view import render_plane_with_refs
|
||||
from .heatmap import render_panel_heatmap
|
||||
from .render_all import render_all
|
||||
|
||||
__all__ = [
|
||||
"render_cloud",
|
||||
"render_cloud_compare",
|
||||
"render_holes",
|
||||
"render_plane_with_refs",
|
||||
"render_panel_heatmap",
|
||||
"render_all",
|
||||
]
|
||||
@@ -0,0 +1,27 @@
|
||||
"""Open3D offscreen renderer factory.
|
||||
|
||||
Wraps the shifting open3d offscreen API so callers don't have to. Returns
|
||||
``None`` if open3d is not available — callers should fall back to
|
||||
matplotlib in that case.
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
from typing import Any
|
||||
|
||||
|
||||
def make_offscreen_renderer(width: int = 1024, height: int = 768) -> Any | None:
|
||||
try:
|
||||
import open3d as o3d # type: ignore
|
||||
except ImportError:
|
||||
return None
|
||||
|
||||
# open3d's offscreen path moved between 0.16 / 0.18.
|
||||
try:
|
||||
renderer = o3d.visualization.rendering.OffscreenRenderer(width, height)
|
||||
return renderer
|
||||
except Exception:
|
||||
return None
|
||||
|
||||
|
||||
__all__ = ["make_offscreen_renderer"]
|
||||
@@ -0,0 +1,134 @@
|
||||
"""Point cloud rendering using matplotlib.
|
||||
|
||||
By default we render a 2D (y, z) projection — the coupler panel is a thin
|
||||
sheet whose normal is along x, so a top-down y-z view is more readable
|
||||
than a 3D scatter. Pass ``mode="3d"`` to force a 3D scatter (slower and
|
||||
broken on some matplotlib versions, kept for completeness).
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
from pathlib import Path
|
||||
|
||||
import numpy as np
|
||||
|
||||
|
||||
def _import_plt():
|
||||
import matplotlib
|
||||
|
||||
matplotlib.use("Agg")
|
||||
import matplotlib.pyplot as plt # noqa: WPS433
|
||||
|
||||
return plt
|
||||
|
||||
|
||||
def _decimate(points: np.ndarray, max_points: int) -> np.ndarray:
|
||||
n = points.shape[0]
|
||||
if n <= max_points:
|
||||
return points
|
||||
rng = np.random.default_rng(0)
|
||||
idx = rng.choice(n, size=max_points, replace=False)
|
||||
return points[idx]
|
||||
|
||||
|
||||
def render_cloud(
|
||||
points: np.ndarray,
|
||||
out_path: str | Path,
|
||||
*,
|
||||
title: str = "",
|
||||
color: str | np.ndarray = "tab:blue",
|
||||
max_points: int = 80_000,
|
||||
dpi: int = 150,
|
||||
mode: str = "2d",
|
||||
) -> Path:
|
||||
plt = _import_plt()
|
||||
pts = _decimate(np.asarray(points, dtype=np.float64), max_points)
|
||||
|
||||
if mode == "3d":
|
||||
return _render_3d(pts, out_path, title=title, color=color, dpi=dpi)
|
||||
|
||||
fig, axes = plt.subplots(1, 2, figsize=(14, 6))
|
||||
axes[0].scatter(pts[:, 1], pts[:, 2], s=0.5, c=color)
|
||||
axes[0].set_xlabel("y (mm)")
|
||||
axes[0].set_ylabel("z (mm)")
|
||||
axes[0].set_aspect("equal")
|
||||
axes[0].set_title("y-z projection (panel face)")
|
||||
|
||||
axes[1].scatter(pts[:, 0], pts[:, 2], s=0.5, c=color)
|
||||
axes[1].set_xlabel("x (mm)")
|
||||
axes[1].set_ylabel("z (mm)")
|
||||
axes[1].set_aspect("equal")
|
||||
axes[1].set_title("x-z projection (side view)")
|
||||
|
||||
if title:
|
||||
fig.suptitle(title)
|
||||
|
||||
out_path = Path(out_path)
|
||||
out_path.parent.mkdir(parents=True, exist_ok=True)
|
||||
fig.tight_layout()
|
||||
fig.savefig(out_path, dpi=dpi)
|
||||
plt.close(fig)
|
||||
return out_path
|
||||
|
||||
|
||||
def render_cloud_compare(
|
||||
clouds: dict[str, np.ndarray],
|
||||
out_path: str | Path,
|
||||
*,
|
||||
title: str = "",
|
||||
max_points: int = 50_000,
|
||||
dpi: int = 150,
|
||||
) -> Path:
|
||||
"""Side-by-side y-z projections of several clouds."""
|
||||
plt = _import_plt()
|
||||
n = len(clouds)
|
||||
fig, axes = plt.subplots(1, n, figsize=(6 * n, 6), squeeze=False)
|
||||
cmap = plt.get_cmap("tab10")
|
||||
for i, (name, pts) in enumerate(clouds.items()):
|
||||
ax = axes[0, i]
|
||||
sub = _decimate(np.asarray(pts, dtype=np.float64), max_points)
|
||||
ax.scatter(sub[:, 1], sub[:, 2], s=0.5, c=[cmap(i)])
|
||||
ax.set_title(name)
|
||||
ax.set_xlabel("y (mm)")
|
||||
ax.set_ylabel("z (mm)")
|
||||
ax.set_aspect("equal")
|
||||
if title:
|
||||
fig.suptitle(title)
|
||||
out_path = Path(out_path)
|
||||
out_path.parent.mkdir(parents=True, exist_ok=True)
|
||||
fig.tight_layout()
|
||||
fig.savefig(out_path, dpi=dpi)
|
||||
plt.close(fig)
|
||||
return out_path
|
||||
|
||||
|
||||
def _render_3d(pts, out_path, *, title, color, dpi):
|
||||
plt = _import_plt()
|
||||
fig = plt.figure(figsize=(8, 6))
|
||||
try:
|
||||
ax = fig.add_subplot(111, projection="3d")
|
||||
ax.scatter(pts[:, 0], pts[:, 1], pts[:, 2], s=0.5, c=color)
|
||||
ax.set_xlabel("x (mm)")
|
||||
ax.set_ylabel("y (mm)")
|
||||
ax.set_zlabel("z (mm)")
|
||||
if title:
|
||||
ax.set_title(title)
|
||||
except Exception:
|
||||
# Fallback to 2D when matplotlib's 3D backend is broken (e.g. mpl 3.10
|
||||
# has a known incompatibility with mpl_toolkits.mplot3d).
|
||||
ax = fig.add_subplot(111)
|
||||
ax.scatter(pts[:, 1], pts[:, 2], s=0.5, c=color)
|
||||
ax.set_aspect("equal")
|
||||
ax.set_xlabel("y (mm)")
|
||||
ax.set_ylabel("z (mm)")
|
||||
if title:
|
||||
ax.set_title(f"{title} (3D backend unavailable)")
|
||||
out_path = Path(out_path)
|
||||
out_path.parent.mkdir(parents=True, exist_ok=True)
|
||||
fig.tight_layout()
|
||||
fig.savefig(out_path, dpi=dpi)
|
||||
plt.close(fig)
|
||||
return out_path
|
||||
|
||||
|
||||
__all__ = ["render_cloud", "render_cloud_compare"]
|
||||
@@ -0,0 +1,73 @@
|
||||
"""Panel deviation heatmap.
|
||||
|
||||
Colours each panel-ROI point by its signed perpendicular distance to
|
||||
the fitted plane, projected onto a 2D image of the panel face.
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
from pathlib import Path
|
||||
|
||||
import numpy as np
|
||||
|
||||
|
||||
def _import_plt():
|
||||
import matplotlib
|
||||
|
||||
matplotlib.use("Agg")
|
||||
import matplotlib.pyplot as plt
|
||||
|
||||
return plt
|
||||
|
||||
|
||||
def render_panel_heatmap(
|
||||
panel_points: np.ndarray,
|
||||
*,
|
||||
plane_normal: tuple[float, float, float],
|
||||
plane_d: float,
|
||||
center_C: tuple[float, float, float],
|
||||
out_path: str | Path,
|
||||
title: str = "",
|
||||
cmap: str = "viridis",
|
||||
dpi: int = 150,
|
||||
) -> Path:
|
||||
plt = _import_plt()
|
||||
|
||||
pts = np.asarray(panel_points, dtype=np.float64)
|
||||
n = np.asarray(plane_normal, dtype=np.float64)
|
||||
n = n / max(np.linalg.norm(n), 1e-12)
|
||||
C = np.asarray(center_C, dtype=np.float64)
|
||||
|
||||
# Local 2D frame on the panel.
|
||||
y_world = np.array([0.0, 1.0, 0.0])
|
||||
u = y_world - np.dot(y_world, n) * n
|
||||
if np.linalg.norm(u) < 1e-6:
|
||||
u = np.array([0.0, 0.0, 1.0]) - np.dot(np.array([0.0, 0.0, 1.0]), n) * n
|
||||
u /= np.linalg.norm(u)
|
||||
v = np.cross(n, u)
|
||||
|
||||
rel = pts - C
|
||||
uu = rel @ u
|
||||
vv = rel @ v
|
||||
signed_dist = pts @ n + float(plane_d)
|
||||
|
||||
fig, ax = plt.subplots(figsize=(8, 7))
|
||||
sc = ax.scatter(uu, vv, c=signed_dist, cmap=cmap, s=2)
|
||||
cb = plt.colorbar(sc, ax=ax)
|
||||
cb.set_label("signed distance to fitted plane (mm)")
|
||||
|
||||
ax.set_aspect("equal")
|
||||
ax.set_xlabel("panel u (≈ y axis, mm)")
|
||||
ax.set_ylabel("panel v (≈ z axis, mm)")
|
||||
if title:
|
||||
ax.set_title(title)
|
||||
|
||||
out_path = Path(out_path)
|
||||
out_path.parent.mkdir(parents=True, exist_ok=True)
|
||||
fig.tight_layout()
|
||||
fig.savefig(out_path, dpi=dpi)
|
||||
plt.close(fig)
|
||||
return out_path
|
||||
|
||||
|
||||
__all__ = ["render_panel_heatmap"]
|
||||
@@ -0,0 +1,70 @@
|
||||
"""4-hole detection diagnostic plot.
|
||||
|
||||
Shows the panel cloud projected to its local 2D frame, with each
|
||||
detected circle drawn on top and the four selected locating holes
|
||||
highlighted.
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
from pathlib import Path
|
||||
|
||||
import numpy as np
|
||||
|
||||
|
||||
def _import_plt():
|
||||
import matplotlib
|
||||
|
||||
matplotlib.use("Agg")
|
||||
import matplotlib.pyplot as plt
|
||||
from matplotlib.patches import Circle # noqa: WPS433
|
||||
|
||||
return plt, Circle
|
||||
|
||||
|
||||
def render_holes(
|
||||
uv_points: np.ndarray,
|
||||
candidates: list,
|
||||
selected_indices: list[int],
|
||||
out_path: str | Path,
|
||||
*,
|
||||
title: str = "",
|
||||
dpi: int = 150,
|
||||
) -> Path:
|
||||
plt, Circle = _import_plt()
|
||||
fig, ax = plt.subplots(figsize=(8, 8))
|
||||
if uv_points.shape[0] > 50_000:
|
||||
idx = np.random.default_rng(0).choice(uv_points.shape[0], 50_000, replace=False)
|
||||
sub = uv_points[idx]
|
||||
else:
|
||||
sub = uv_points
|
||||
ax.scatter(sub[:, 0], sub[:, 1], s=0.3, c="lightgray")
|
||||
|
||||
for i, cand in enumerate(candidates):
|
||||
is_sel = i in set(selected_indices)
|
||||
col = "tab:red" if is_sel else "tab:blue"
|
||||
ax.add_patch(
|
||||
Circle(
|
||||
(cand.fit.cx, cand.fit.cy),
|
||||
cand.fit.radius,
|
||||
fill=False,
|
||||
edgecolor=col,
|
||||
lw=2 if is_sel else 1,
|
||||
)
|
||||
)
|
||||
ax.plot(cand.fit.cx, cand.fit.cy, "+", color=col)
|
||||
|
||||
ax.set_aspect("equal")
|
||||
ax.set_xlabel("u (mm)")
|
||||
ax.set_ylabel("v (mm)")
|
||||
if title:
|
||||
ax.set_title(title)
|
||||
out_path = Path(out_path)
|
||||
out_path.parent.mkdir(parents=True, exist_ok=True)
|
||||
fig.tight_layout()
|
||||
fig.savefig(out_path, dpi=dpi)
|
||||
plt.close(fig)
|
||||
return out_path
|
||||
|
||||
|
||||
__all__ = ["render_holes"]
|
||||
@@ -0,0 +1,105 @@
|
||||
"""Plane + reference geometry overlay (2D projection).
|
||||
|
||||
Renders the y-z view of the panel ROI with the fitted plane normal,
|
||||
reference line L, reference plane z=C_z, and longitudinal mid y=C_y
|
||||
overlaid as annotations.
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
from pathlib import Path
|
||||
|
||||
import numpy as np
|
||||
|
||||
|
||||
def _import_plt():
|
||||
import matplotlib
|
||||
|
||||
matplotlib.use("Agg")
|
||||
import matplotlib.pyplot as plt
|
||||
|
||||
return plt
|
||||
|
||||
|
||||
def render_plane_with_refs(
|
||||
panel_points: np.ndarray,
|
||||
*,
|
||||
plane_normal: tuple[float, float, float],
|
||||
center_C: tuple[float, float, float],
|
||||
out_path: str | Path,
|
||||
title: str = "",
|
||||
line_length_mm: float = 200.0,
|
||||
dpi: int = 150,
|
||||
) -> Path:
|
||||
plt = _import_plt()
|
||||
|
||||
pts = np.asarray(panel_points, dtype=np.float64)
|
||||
n = np.asarray(plane_normal, dtype=np.float64)
|
||||
n = n / max(np.linalg.norm(n), 1e-12)
|
||||
C = np.asarray(center_C, dtype=np.float64)
|
||||
|
||||
if pts.shape[0] > 80_000:
|
||||
idx = np.random.default_rng(0).choice(pts.shape[0], 80_000, replace=False)
|
||||
pts = pts[idx]
|
||||
|
||||
fig, axes = plt.subplots(1, 2, figsize=(14, 6))
|
||||
|
||||
# Panel face view: y-z, with a small arrow for the normal projected
|
||||
# into y-z (the in-plane part of n).
|
||||
ax = axes[0]
|
||||
ax.scatter(pts[:, 1], pts[:, 2], s=0.5, c="lightgray")
|
||||
# Reference plane z = C_z
|
||||
ax.axhline(C[2], color="tab:green", lw=1, alpha=0.7, label=f"ref plane z = {C[2]:.2f}")
|
||||
# Longitudinal mid y = C_y
|
||||
ax.axvline(C[1], color="tab:orange", lw=1, alpha=0.7, label=f"long. mid y = {C[1]:.2f}")
|
||||
ax.scatter([C[1]], [C[2]], color="black", s=40, zorder=5, label=f"C = ({C[1]:.1f}, {C[2]:.1f})")
|
||||
# Project normal into y-z
|
||||
n_yz = np.array([n[1], n[2]])
|
||||
if np.linalg.norm(n_yz) > 1e-3:
|
||||
scale = line_length_mm * 0.5
|
||||
ax.annotate(
|
||||
"",
|
||||
xy=(C[1] + n_yz[0] * scale, C[2] + n_yz[1] * scale),
|
||||
xytext=(C[1], C[2]),
|
||||
arrowprops=dict(arrowstyle="->", color="tab:red", lw=2),
|
||||
)
|
||||
ax.set_aspect("equal")
|
||||
ax.set_xlabel("y (mm)")
|
||||
ax.set_ylabel("z (mm)")
|
||||
ax.set_title("y-z view: ROI + reference geometry + n's y-z projection")
|
||||
ax.legend(loc="upper right", fontsize="x-small")
|
||||
|
||||
# x-y side view: shows the panel tilt out of x.
|
||||
ax = axes[1]
|
||||
ax.scatter(pts[:, 0], pts[:, 1], s=0.5, c="lightgray")
|
||||
# Reference line L (along +x at y=C_y, z=C_z, projected to x-y → horizontal at y=C_y)
|
||||
L = float(line_length_mm)
|
||||
ax.plot([C[0] - L, C[0] + L], [C[1], C[1]], color="tab:blue", lw=2, label="ref line L")
|
||||
# Plane normal (x, y components)
|
||||
n_xy = np.array([n[0], n[1]])
|
||||
scale = line_length_mm * 0.5
|
||||
ax.annotate(
|
||||
"",
|
||||
xy=(C[0] + n_xy[0] * scale, C[1] + n_xy[1] * scale),
|
||||
xytext=(C[0], C[1]),
|
||||
arrowprops=dict(arrowstyle="->", color="tab:red", lw=2),
|
||||
)
|
||||
ax.scatter([C[0]], [C[1]], color="black", s=40, zorder=5)
|
||||
ax.set_aspect("equal")
|
||||
ax.set_xlabel("x (mm)")
|
||||
ax.set_ylabel("y (mm)")
|
||||
ax.set_title(f"x-y view: n=({n[0]:.4f},{n[1]:.4f},{n[2]:.4f})")
|
||||
ax.legend(loc="upper right", fontsize="x-small")
|
||||
|
||||
if title:
|
||||
fig.suptitle(title)
|
||||
|
||||
out_path = Path(out_path)
|
||||
out_path.parent.mkdir(parents=True, exist_ok=True)
|
||||
fig.tight_layout()
|
||||
fig.savefig(out_path, dpi=dpi)
|
||||
plt.close(fig)
|
||||
return out_path
|
||||
|
||||
|
||||
__all__ = ["render_plane_with_refs"]
|
||||
@@ -0,0 +1,112 @@
|
||||
"""One-shot rendering of every diagnostic image for a single run.
|
||||
|
||||
Reads the artifact directory written by :mod:`coupler_panel.pipeline` and
|
||||
fills in ``images/`` with PNG plots. Silently skips any stage that
|
||||
cannot be loaded (e.g. ``denoised.ply`` if denoise was disabled).
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
from pathlib import Path
|
||||
from typing import Any
|
||||
|
||||
import numpy as np
|
||||
|
||||
from ..io_utils import load_point_cloud, Artifacts
|
||||
|
||||
|
||||
def _safe_load(path: Path) -> np.ndarray | None:
|
||||
try:
|
||||
return load_point_cloud(path) if path.exists() else None
|
||||
except Exception:
|
||||
return None
|
||||
|
||||
|
||||
def render_all(run_dir: str | Path, *, dpi: int = 150) -> dict[str, Path]:
|
||||
"""Render every diagnostic image we know how to make for this run."""
|
||||
art = Artifacts(run_dir)
|
||||
out: dict[str, Path] = {}
|
||||
|
||||
# Lazy imports — matplotlib not strictly required to run pipeline.
|
||||
try:
|
||||
from .cloud_view import render_cloud, render_cloud_compare
|
||||
from .plane_view import render_plane_with_refs
|
||||
from .heatmap import render_panel_heatmap
|
||||
except ImportError:
|
||||
return out
|
||||
|
||||
stages = ("cad_mesh", "sampled", "noisy", "transformed", "denoised", "registered")
|
||||
clouds: dict[str, np.ndarray] = {}
|
||||
for st in stages:
|
||||
pts = _safe_load(art.path(st, ext="ply"))
|
||||
if pts is not None and pts.shape[0] > 0:
|
||||
clouds[st] = pts
|
||||
try:
|
||||
out[st] = render_cloud(
|
||||
pts, art.image(f"{st}.png"), title=st, dpi=dpi,
|
||||
)
|
||||
except Exception:
|
||||
pass
|
||||
|
||||
# Per-stage 2-up comparisons of selected pairs.
|
||||
if "sampled" in clouds and "noisy" in clouds:
|
||||
try:
|
||||
out["sampled_vs_noisy"] = render_cloud_compare(
|
||||
{"sampled": clouds["sampled"], "noisy": clouds["noisy"]},
|
||||
art.image("sampled_vs_noisy.png"),
|
||||
dpi=dpi,
|
||||
)
|
||||
except Exception:
|
||||
pass
|
||||
|
||||
if "transformed" in clouds and "registered" in clouds:
|
||||
try:
|
||||
out["transformed_vs_registered"] = render_cloud_compare(
|
||||
{"transformed": clouds["transformed"], "registered": clouds["registered"]},
|
||||
art.image("transformed_vs_registered.png"),
|
||||
dpi=dpi,
|
||||
)
|
||||
except Exception:
|
||||
pass
|
||||
|
||||
# Plane + reference geometry (read from report.json).
|
||||
report_path = art.report_json
|
||||
if report_path.exists():
|
||||
try:
|
||||
import json
|
||||
|
||||
with report_path.open("r", encoding="utf-8") as fh:
|
||||
report: dict[str, Any] = json.load(fh)
|
||||
|
||||
measured = report.get("measured", report)
|
||||
plane = measured.get("plane")
|
||||
frame = measured.get("frame")
|
||||
roi_pts_path = art.path("panel_roi", ext="ply")
|
||||
roi_pts = _safe_load(roi_pts_path)
|
||||
if roi_pts is None and "registered" in clouds:
|
||||
roi_pts = clouds["registered"]
|
||||
if plane and frame and roi_pts is not None and roi_pts.shape[0] > 0:
|
||||
out["plane_with_refs"] = render_plane_with_refs(
|
||||
roi_pts,
|
||||
plane_normal=tuple(plane["normal"]),
|
||||
center_C=tuple(frame["center_C"]),
|
||||
out_path=art.image("plane_with_refs.png"),
|
||||
title="fitted plane + reference geometry",
|
||||
dpi=dpi,
|
||||
)
|
||||
out["heatmap"] = render_panel_heatmap(
|
||||
roi_pts,
|
||||
plane_normal=tuple(plane["normal"]),
|
||||
plane_d=float(plane["d_offset"]),
|
||||
center_C=tuple(frame["center_C"]),
|
||||
out_path=art.image("deviation_heatmap.png"),
|
||||
title="panel deviation heatmap",
|
||||
dpi=dpi,
|
||||
)
|
||||
except Exception:
|
||||
pass
|
||||
|
||||
return out
|
||||
|
||||
|
||||
__all__ = ["render_all"]
|
||||
@@ -0,0 +1,128 @@
|
||||
"""Generate synthetic test point clouds at varying injected panel-deflection
|
||||
angles. Each cloud is produced by the *existing* CAD synthesis path
|
||||
(parse → sample → deflect → noise) **with the scanner-pose jitter step
|
||||
turned off**, so each PLY already lives in the CAD frame — which is the
|
||||
contract the main measurement pipeline expects (no registration step).
|
||||
|
||||
Each PLY is paired with one row in ``manifest.json`` so the batch
|
||||
evaluator knows the GT angle to compare against.
|
||||
|
||||
Run::
|
||||
|
||||
python examples/generate_test_clouds.py \
|
||||
--stp test_data/lu1.stp \
|
||||
--out test_data/synth \
|
||||
--profile lu1_af4
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import argparse
|
||||
import logging
|
||||
from pathlib import Path
|
||||
|
||||
from coupler_panel.config import load_config, resolve_auto
|
||||
from coupler_panel.data_prep import parse_cad, synthesize_cloud
|
||||
from coupler_panel.io_utils import write_point_cloud, write_json
|
||||
|
||||
|
||||
# (theta_right_deg, theta_ref_deg) pairs — mirrors the spirit of the
|
||||
# existing lu1_deflection_sweep matrix.
|
||||
DEFAULT_INJECTIONS = [
|
||||
(0.0, 0.0),
|
||||
(0.1, 0.0),
|
||||
(0.3, 0.0),
|
||||
(0.5, 0.0),
|
||||
(1.0, 0.0),
|
||||
(2.0, 0.0),
|
||||
(0.0, 0.5),
|
||||
(0.5, 0.5),
|
||||
(1.0, 1.0),
|
||||
]
|
||||
|
||||
|
||||
def _build_parser() -> argparse.ArgumentParser:
|
||||
p = argparse.ArgumentParser(description=__doc__)
|
||||
p.add_argument("--stp", type=Path, required=True, help="Path to the panel STEP file.")
|
||||
p.add_argument("--out", type=Path, required=True, help="Output directory for PLYs + manifest.")
|
||||
p.add_argument("--profile", type=str, default="lu1_af4", help="Config profile.")
|
||||
p.add_argument(
|
||||
"--config",
|
||||
type=Path,
|
||||
default=None,
|
||||
help="Optional YAML override; takes precedence over --profile.",
|
||||
)
|
||||
return p
|
||||
|
||||
|
||||
def main(argv: list[str] | None = None) -> int:
|
||||
logging.basicConfig(level=logging.INFO, format="%(levelname)s %(name)s: %(message)s")
|
||||
log = logging.getLogger("generate_test_clouds")
|
||||
args = _build_parser().parse_args(argv)
|
||||
|
||||
cfg = load_config(args.config, profile=args.profile)
|
||||
cfg = resolve_auto(cfg)
|
||||
|
||||
log.info("parsing CAD: %s", args.stp)
|
||||
parsed = parse_cad(args.stp, cfg["cad"])
|
||||
|
||||
out_dir = args.out
|
||||
out_dir.mkdir(parents=True, exist_ok=True)
|
||||
|
||||
# CAD nominal GT — kept as an audit reference (not consumed by the
|
||||
# main pipeline, which only needs the STP).
|
||||
write_json(parsed.gt.to_dict(), out_dir / "gt.json")
|
||||
|
||||
manifest: list[dict] = []
|
||||
for i, (th_r, th_z) in enumerate(DEFAULT_INJECTIONS):
|
||||
cfg_i = dict(cfg)
|
||||
cfg_i["panel_deflection"] = {
|
||||
"enabled": True,
|
||||
"theta_right_deg": float(th_r),
|
||||
"theta_ref_deg": float(th_z),
|
||||
}
|
||||
# Scanner-pose jitter is OFF: the cloud must already be in the
|
||||
# CAD frame because the main pipeline does not register.
|
||||
cfg_i["transform"] = {"enabled": False}
|
||||
|
||||
log.info("[%d/%d] synthesising θ_right=%+.2f° θ_ref=%+.2f°",
|
||||
i + 1, len(DEFAULT_INJECTIONS), th_r, th_z)
|
||||
synth = synthesize_cloud(
|
||||
parsed,
|
||||
sampling_cfg=cfg_i["sampling"],
|
||||
noise_cfg=cfg_i.get("noise", {}),
|
||||
transform_cfg=cfg_i["transform"],
|
||||
scan_cfg=cfg_i.get("scan_simulator", {}),
|
||||
deflection_cfg=cfg_i["panel_deflection"],
|
||||
)
|
||||
sample_id = (
|
||||
f"r{th_r:+.2f}_z{th_z:+.2f}"
|
||||
.replace("+", "p").replace("-", "n").replace(".", "")
|
||||
)
|
||||
ply_path = out_dir / f"cloud_{sample_id}.ply"
|
||||
# synth.transformed == synth.noisy when transform is disabled.
|
||||
write_point_cloud(synth.transformed, ply_path)
|
||||
|
||||
manifest.append({
|
||||
"sample_id": sample_id,
|
||||
"ply": ply_path.name,
|
||||
"theta_right_deg": float(th_r),
|
||||
"theta_ref_deg": float(th_z),
|
||||
"n_points": int(synth.transformed.shape[0]),
|
||||
})
|
||||
|
||||
write_json(
|
||||
{
|
||||
"stp": str(args.stp),
|
||||
"profile": args.profile,
|
||||
"gt": "gt.json",
|
||||
"samples": manifest,
|
||||
},
|
||||
out_dir / "manifest.json",
|
||||
)
|
||||
log.info("done: %d clouds + gt.json + manifest.json under %s", len(manifest), out_dir)
|
||||
return 0
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
raise SystemExit(main())
|
||||
@@ -0,0 +1,58 @@
|
||||
"""Minimal integration example for software developers.
|
||||
|
||||
This script shows the *only* code path a production caller needs:
|
||||
|
||||
load a clean, registered point cloud → call the API → use the result
|
||||
|
||||
Run::
|
||||
|
||||
python examples/integrate_minimal.py path/to/scan.ply
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import logging
|
||||
import sys
|
||||
from pathlib import Path
|
||||
|
||||
from coupler_panel import (
|
||||
CouplerError,
|
||||
compute_coupler_measurement,
|
||||
load_point_cloud,
|
||||
)
|
||||
|
||||
|
||||
def main(input_path: Path) -> int:
|
||||
logging.basicConfig(level=logging.INFO, format="%(levelname)s %(name)s: %(message)s")
|
||||
log = logging.getLogger("integrate_minimal")
|
||||
|
||||
points = load_point_cloud(input_path)
|
||||
log.info("loaded %d points from %s", points.shape[0], input_path)
|
||||
|
||||
try:
|
||||
result = compute_coupler_measurement(
|
||||
points,
|
||||
progress=lambda s, f: log.info("[%5.1f%%] %s", f * 100, s),
|
||||
)
|
||||
except CouplerError as exc:
|
||||
log.error("measurement failed: %s", exc)
|
||||
return 2
|
||||
|
||||
print(f"theta_right_deg = {result.theta_right_deg:+.4f}")
|
||||
print(f"theta_ref_deg = {result.theta_ref_deg:+.4f}")
|
||||
print(f"D_y_max_mm = {result.D_y_max_mm:.3f}")
|
||||
print(f"D_z_max_mm = {result.D_z_max_mm:.3f}")
|
||||
print(f"dx_y_max_mm = {result.dx_y_max_mm:.3f}")
|
||||
print(f"dx_z_max_mm = {result.dx_z_max_mm:.3f}")
|
||||
print(f"center_C = {tuple(round(c, 3) for c in result.center_C)}")
|
||||
print(f"normal = {tuple(round(c, 6) for c in result.normal)}")
|
||||
print(f"quality_flags = {result.quality_flags}")
|
||||
|
||||
return 0
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
if len(sys.argv) != 2:
|
||||
print(__doc__.strip())
|
||||
sys.exit(1)
|
||||
sys.exit(main(Path(sys.argv[1])))
|
||||
@@ -0,0 +1,8 @@
|
||||
{
|
||||
"x_min": -12000.0,
|
||||
"x_max": 100.0,
|
||||
"y_min": -300.0,
|
||||
"y_max": 1200.0,
|
||||
"z_min": -1000.0,
|
||||
"z_max": 800.0
|
||||
}
|
||||
@@ -0,0 +1,9 @@
|
||||
{
|
||||
"x_min": -11753.0,
|
||||
"x_max": -11506.0,
|
||||
"y_min": -950.0,
|
||||
"y_max": 950.0,
|
||||
"z_min": 788.0,
|
||||
"z_max": 1192.0,
|
||||
"comment": "Coupler mounting panel ROI on train_1 full-vehicle scan (m000004802409t02_asm). Six numbers are the cropping hyper-parameters consumed directly by the algorithm; coordinates are in the cloud's own frame."
|
||||
}
|
||||
@@ -0,0 +1,9 @@
|
||||
{
|
||||
"x_min": 11526.0,
|
||||
"x_max": 11733.0,
|
||||
"y_min": 588.0,
|
||||
"y_max": 1192.0,
|
||||
"z_min": -950.0,
|
||||
"z_max": 950.0,
|
||||
"comment": "Coupler mounting panel ROI on train_2 full-vehicle scan. Dataset frame: x=length, y=height, z=width. These six numbers are the cropping hyper-parameters consumed directly by the algorithm."
|
||||
}
|
||||
@@ -0,0 +1,147 @@
|
||||
"""Batch evaluator: main measurement program with CAD + cloud + ROI inputs.
|
||||
|
||||
For each PLY listed in the manifest:
|
||||
|
||||
1. Load the cloud (PLY).
|
||||
2. Apply the ROI JSON (currently ``mode='all'`` placeholder).
|
||||
3. Feed (CAD STP, ROI'd cloud, GT JSON) into the existing measurement
|
||||
pipeline — internal stages (denoise / Kabsch / detection / ROI /
|
||||
plane fit / metrics) are unchanged.
|
||||
4. Compare θ_right / θ_ref against the injected GT recorded in the
|
||||
manifest.
|
||||
5. Append a row to the Excel report.
|
||||
|
||||
Run::
|
||||
|
||||
python examples/run_batch_eval.py \
|
||||
--cad test_data/lu1.stp \
|
||||
--data test_data/synth \
|
||||
--roi examples/roi_all.json \
|
||||
--out runs/batch_eval \
|
||||
--profile lu1_af4
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import argparse
|
||||
import json
|
||||
import logging
|
||||
import time
|
||||
from pathlib import Path
|
||||
|
||||
from coupler_panel.exceptions import CouplerError
|
||||
from coupler_panel.io_utils import load_point_cloud
|
||||
from coupler_panel.pipeline import run_coupler_measurement_pipeline
|
||||
from coupler_panel.reporting import build_row_from_summary, write_excel_report
|
||||
from coupler_panel.roi_select import load_roi_json
|
||||
|
||||
|
||||
def _build_parser() -> argparse.ArgumentParser:
|
||||
p = argparse.ArgumentParser(description=__doc__)
|
||||
p.add_argument("--cad", type=Path, required=True, help="STEP file (panel CAD).")
|
||||
p.add_argument("--data", type=Path, required=True, help="Directory with PLYs + manifest.json.")
|
||||
p.add_argument("--roi", type=Path, required=True, help="ROI JSON file.")
|
||||
p.add_argument("--out", type=Path, required=True, help="Output directory for runs + xlsx.")
|
||||
p.add_argument("--profile", type=str, default="lu1_af4", help="Config profile.")
|
||||
p.add_argument("--config", type=Path, default=None, help="Optional YAML override.")
|
||||
return p
|
||||
|
||||
|
||||
def main(argv: list[str] | None = None) -> int:
|
||||
logging.basicConfig(level=logging.INFO, format="%(levelname)s %(name)s: %(message)s")
|
||||
log = logging.getLogger("run_batch_eval")
|
||||
args = _build_parser().parse_args(argv)
|
||||
|
||||
args.out.mkdir(parents=True, exist_ok=True)
|
||||
|
||||
manifest_path = args.data / "manifest.json"
|
||||
with manifest_path.open("r", encoding="utf-8") as fh:
|
||||
manifest = json.load(fh)
|
||||
samples = manifest["samples"]
|
||||
roi_spec = load_roi_json(args.roi)
|
||||
log.info(
|
||||
"loaded ROI spec (%s): x∈[%g,%g] y∈[%g,%g] z∈[%g,%g]",
|
||||
roi_spec.kind,
|
||||
roi_spec.x_min, roi_spec.x_max,
|
||||
roi_spec.y_min, roi_spec.y_max,
|
||||
roi_spec.z_min, roi_spec.z_max,
|
||||
)
|
||||
log.info("samples in manifest: %d", len(samples))
|
||||
|
||||
rows = []
|
||||
for i, sample in enumerate(samples, start=1):
|
||||
sid = sample["sample_id"]
|
||||
ply = args.data / sample["ply"]
|
||||
log.info("[%d/%d] %s — %s", i, len(samples), sid, ply.name)
|
||||
|
||||
run_dir = args.out / sid
|
||||
try:
|
||||
# Pre-count points to record n_points_in (before ROI).
|
||||
n_in = int(load_point_cloud(ply).shape[0])
|
||||
|
||||
# Main pipeline: CAD STP is parsed for the panel-normal seed
|
||||
# only; the cloud is assumed to be in the CAD frame already
|
||||
# (scanner extrinsics calibrated upstream). No Kabsch.
|
||||
# The wall-clock duration from "inputs enter the algorithm"
|
||||
# to "result returned" goes straight into the Excel report.
|
||||
t_start = time.perf_counter()
|
||||
result = run_coupler_measurement_pipeline(
|
||||
input_cloud=ply,
|
||||
out_dir=run_dir,
|
||||
config_path=args.config,
|
||||
profile=args.profile,
|
||||
cad_stp_path=args.cad,
|
||||
roi_spec=roi_spec,
|
||||
)
|
||||
pipeline_seconds = time.perf_counter() - t_start
|
||||
except CouplerError as exc:
|
||||
log.error(" -> FAILED: %s", exc)
|
||||
continue
|
||||
except Exception as exc:
|
||||
log.exception(" -> CRASH: %s", exc)
|
||||
continue
|
||||
|
||||
# n_after_roi is recorded in run_dir/roi.json.diagnostics.n_out
|
||||
roi_diag_path = run_dir / "roi.json"
|
||||
n_after = n_in
|
||||
if roi_diag_path.exists():
|
||||
with roi_diag_path.open("r", encoding="utf-8") as fh:
|
||||
n_after = int(json.load(fh).get("diagnostics", {}).get("n_out", n_in))
|
||||
|
||||
# Kabsch RMSE: pull from the prepared/registration log if present
|
||||
# (the persisted pipeline does not currently expose this in the
|
||||
# report dict; it's available via report.json side-files but for a
|
||||
# minimum-touch wiring we leave it None when not surfaced).
|
||||
registration = None
|
||||
|
||||
row = build_row_from_summary(
|
||||
sample_id=sid,
|
||||
ply_path=str(ply),
|
||||
roi_kind=roi_spec.kind,
|
||||
measured=result.to_dict(),
|
||||
gt_angles=(float(sample["theta_right_deg"]), float(sample["theta_ref_deg"])),
|
||||
registration=registration,
|
||||
n_points_in=n_in,
|
||||
n_points_after_roi=n_after,
|
||||
pipeline_seconds=pipeline_seconds,
|
||||
)
|
||||
rows.append(row)
|
||||
log.info(" θ_right pred=%+.4f° GT=%+.2f° err=%+.4f°",
|
||||
row.theta_right_pred_deg, row.theta_right_gt_deg, row.theta_right_err_deg)
|
||||
log.info(" θ_ref pred=%+.4f° GT=%+.2f° err=%+.4f°",
|
||||
row.theta_ref_pred_deg, row.theta_ref_gt_deg, row.theta_ref_err_deg)
|
||||
log.info(" runtime: %.3f s (inner stages: %.3f s)",
|
||||
row.pipeline_seconds, row.inner_stage_seconds)
|
||||
|
||||
if not rows:
|
||||
log.error("no successful runs; nothing to write")
|
||||
return 2
|
||||
|
||||
xlsx_path = args.out / "eval_report.xlsx"
|
||||
write_excel_report(rows, xlsx_path)
|
||||
log.info("wrote %s (%d rows)", xlsx_path, len(rows))
|
||||
return 0
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
raise SystemExit(main())
|
||||
@@ -0,0 +1,43 @@
|
||||
"""Full research demo: STP → synthetic cloud → measure → diff GT.
|
||||
|
||||
Run::
|
||||
|
||||
python examples/run_full_validation.py path/to/panel.stp runs/demo
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import json
|
||||
import logging
|
||||
import sys
|
||||
from pathlib import Path
|
||||
|
||||
from coupler_panel.pipeline import run_from_cad_pipeline
|
||||
|
||||
|
||||
def main(stp: Path, out: Path) -> int:
|
||||
logging.basicConfig(level=logging.INFO, format="%(levelname)s %(name)s: %(message)s")
|
||||
summary = run_from_cad_pipeline(
|
||||
stp_path=stp,
|
||||
out_dir=out,
|
||||
progress=lambda s, f: print(f"[{f*100:5.1f}%] {s}", flush=True),
|
||||
)
|
||||
print("\n=== diff (measured - GT) ===")
|
||||
print(json.dumps(summary["diff"], indent=2))
|
||||
|
||||
# Render diagnostic PNGs.
|
||||
try:
|
||||
from coupler_panel.viz import render_all
|
||||
|
||||
render_all(out)
|
||||
except ImportError:
|
||||
print("(matplotlib not installed; skip rendering)")
|
||||
|
||||
return 0
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
if len(sys.argv) != 3:
|
||||
print(__doc__.strip())
|
||||
sys.exit(1)
|
||||
sys.exit(main(Path(sys.argv[1]), Path(sys.argv[2])))
|
||||
@@ -0,0 +1,117 @@
|
||||
"""Integration entry point — the contract for the software integrator.
|
||||
|
||||
Three input files, one Excel output:
|
||||
|
||||
--roi ROI JSON (6-key AABB crop hyper-parameters, in the cloud frame)
|
||||
--cloud full-vehicle point cloud PLY (may be multi-GB; cropped by streaming)
|
||||
--stp theoretical CAD STEP file (recorded for audit/traceability)
|
||||
--out output directory (gets eval_report.xlsx + intermediates)
|
||||
|
||||
The cloud is cropped to the ROI inside the algorithm (no external
|
||||
pre-processing needed), the coupler panel is measured, and the two
|
||||
deflection angles + intermediate values are written to an .xlsx report.
|
||||
|
||||
Example::
|
||||
|
||||
python examples/run_integration.py \
|
||||
--roi examples/roi_train2_coupler.json \
|
||||
--cloud E:/BaiduNetdiskDownload/pointsCloud_data/train_2_1T.ply \
|
||||
--stp E:/BaiduNetdiskDownload/pointsCloud_data/m000004802418tc01_asm.stp \
|
||||
--out runs/train2_integration \
|
||||
--profile train2_coupler
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import argparse
|
||||
import logging
|
||||
import math
|
||||
import time
|
||||
from pathlib import Path
|
||||
|
||||
import numpy as np
|
||||
|
||||
from coupler_panel.exceptions import CouplerError
|
||||
from coupler_panel.pipeline import run_coupler_measurement_pipeline
|
||||
from coupler_panel.reporting import InspectionItem, write_inspection_report
|
||||
from coupler_panel.roi_select import load_roi_json
|
||||
|
||||
|
||||
def _build_parser() -> argparse.ArgumentParser:
|
||||
p = argparse.ArgumentParser(description=__doc__)
|
||||
p.add_argument("--roi", type=Path, required=True, help="ROI JSON (6-key AABB).")
|
||||
p.add_argument("--cloud", type=Path, required=True, help="Full-vehicle point cloud PLY.")
|
||||
p.add_argument("--stp", type=Path, default=None, help="Theoretical CAD STEP (audit).")
|
||||
p.add_argument("--out", type=Path, required=True, help="Output directory.")
|
||||
p.add_argument("--profile", type=str, default="train2_coupler",
|
||||
help="Config profile (default: train2_coupler).")
|
||||
p.add_argument("--config", type=Path, default=None, help="Optional YAML override.")
|
||||
p.add_argument("--gt-theta-right", type=float, default=0.0,
|
||||
help="Known/expected theta_right (deg) for the error column. Default 0.")
|
||||
p.add_argument("--gt-theta-ref", type=float, default=0.0,
|
||||
help="Known/expected theta_ref (deg) for the error column. Default 0.")
|
||||
p.add_argument("--sample-id", type=str, default=None, help="Row label in the report.")
|
||||
return p
|
||||
|
||||
|
||||
def main(argv: list[str] | None = None) -> int:
|
||||
logging.basicConfig(level=logging.INFO, format="%(levelname)s %(name)s: %(message)s")
|
||||
log = logging.getLogger("run_integration")
|
||||
args = _build_parser().parse_args(argv)
|
||||
|
||||
roi_spec = load_roi_json(args.roi)
|
||||
log.info("ROI bbox: x[%g,%g] y[%g,%g] z[%g,%g]",
|
||||
roi_spec.x_min, roi_spec.x_max, roi_spec.y_min,
|
||||
roi_spec.y_max, roi_spec.z_min, roi_spec.z_max)
|
||||
log.info("cloud: %s", args.cloud)
|
||||
log.info("stp (audit): %s", args.stp)
|
||||
|
||||
sample_id = args.sample_id or args.cloud.stem
|
||||
|
||||
t0 = time.perf_counter()
|
||||
try:
|
||||
result = run_coupler_measurement_pipeline(
|
||||
input_cloud=args.cloud,
|
||||
out_dir=args.out,
|
||||
config_path=args.config,
|
||||
profile=args.profile,
|
||||
roi_spec=roi_spec,
|
||||
progress=lambda s, f: log.info("[%5.1f%%] %s", f * 100, s),
|
||||
)
|
||||
except CouplerError as exc:
|
||||
log.error("measurement failed: %s", exc)
|
||||
return 2
|
||||
pipeline_seconds = time.perf_counter() - t0
|
||||
|
||||
# --- Raw (pre-clamp) deflection angles from the fitted plane normal ----
|
||||
# Use the un-clamped normal so a tiny non-zero angle is NOT rounded to 0.
|
||||
n = np.asarray(result.report["plane"]["normal"], dtype=np.float64)
|
||||
theta_right_deg = math.degrees(math.atan2(n[1], n[0])) # 距纵向中心 (y)
|
||||
theta_ref_deg = math.degrees(math.atan2(n[2], n[0])) # 距参考面 (z)
|
||||
|
||||
# --- Two inspection-item Excel reports (slope form value/L mm) ---------
|
||||
item_ref = InspectionItem(
|
||||
name="车钩面板距参考面的右偏转角（头车一位端处车钩）",
|
||||
theta_deg=theta_ref_deg,
|
||||
baseline_mm=375.0,
|
||||
tol_value_mm=0.8, # 公差 0.8/375mm
|
||||
)
|
||||
item_long = InspectionItem(
|
||||
name="车钩面板距纵向中心的右偏转角(头车一位端处车钩)",
|
||||
theta_deg=theta_right_deg,
|
||||
baseline_mm=700.0,
|
||||
tol_value_mm=1.0, # 公差 1/700mm
|
||||
)
|
||||
|
||||
out_ref = write_inspection_report(item_ref, args.out / "report_ref_plane.xlsx")
|
||||
out_long = write_inspection_report(item_long, args.out / "report_longitudinal.xlsx")
|
||||
|
||||
for it, p in ((item_ref, out_ref), (item_long, out_long)):
|
||||
log.info("%s | 测量结果=%s | 公差=%s | %s",
|
||||
it.name, it.measured_str, it.tolerance_str, it.conclusion_str)
|
||||
log.info("wrote %s , %s", out_ref, out_long)
|
||||
return 0
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
raise SystemExit(main())
|
||||
89
alg/delivery_v1.2.1/coupler_panel_release/pyproject.toml
Normal file
89
alg/delivery_v1.2.1/coupler_panel_release/pyproject.toml
Normal file
@@ -0,0 +1,89 @@
|
||||
[build-system]
|
||||
requires = ["setuptools>=68", "wheel"]
|
||||
build-backend = "setuptools.build_meta"
|
||||
|
||||
[project]
|
||||
name = "coupler-panel-validation"
|
||||
version = "0.1.0"
|
||||
description = "CAD-STP to point-cloud measurement validation system for coupler panel deflection."
|
||||
readme = "README.md"
|
||||
requires-python = ">=3.10"
|
||||
license = { text = "Proprietary" }
|
||||
authors = [{ name = "PKU High-Speed Train Project" }]
|
||||
keywords = ["point-cloud", "cad", "step", "metrology", "rail", "coupler"]
|
||||
classifiers = [
|
||||
"Development Status :: 3 - Alpha",
|
||||
"Intended Audience :: Manufacturing",
|
||||
"Programming Language :: Python :: 3",
|
||||
"Programming Language :: Python :: 3.10",
|
||||
"Programming Language :: Python :: 3.11",
|
||||
"Programming Language :: Python :: 3.12",
|
||||
"Topic :: Scientific/Engineering",
|
||||
]
|
||||
|
||||
# Core: pure compute (no plotting, no CAD).
|
||||
dependencies = [
|
||||
"numpy>=1.24",
|
||||
"scipy>=1.10",
|
||||
"pyyaml>=6.0",
|
||||
"tqdm>=4.65",
|
||||
]
|
||||
|
||||
[project.optional-dependencies]
|
||||
# Required to run the full simulation / measurement pipeline:
|
||||
# point-cloud IO, sampling, denoising, registration helpers.
|
||||
cloud = [
|
||||
"open3d>=0.18",
|
||||
"trimesh>=4.0",
|
||||
]
|
||||
# Required to parse STP files into a tessellated mesh and to extract
|
||||
# topological GT (hole circles, panel plane).
|
||||
cad = [
|
||||
"cadquery-ocp>=7.7",
|
||||
"trimesh>=4.0",
|
||||
]
|
||||
# Visualisation (PNG diagnostics + offscreen 3D renders).
|
||||
viz = [
|
||||
"matplotlib>=3.7",
|
||||
"open3d>=0.18",
|
||||
]
|
||||
# Excel batch-evaluation reporting (reporting.excel).
|
||||
report = [
|
||||
"openpyxl>=3.1",
|
||||
]
|
||||
# Convenience: install everything (compute + cad + cloud + plotting + report).
|
||||
all = [
|
||||
"open3d>=0.18",
|
||||
"trimesh>=4.0",
|
||||
"cadquery-ocp>=7.7",
|
||||
"matplotlib>=3.7",
|
||||
"openpyxl>=3.1",
|
||||
]
|
||||
dev = [
|
||||
"pytest>=7",
|
||||
"pytest-cov",
|
||||
]
|
||||
|
||||
[project.scripts]
|
||||
coupler-measure = "coupler_panel.cli:main"
|
||||
|
||||
[tool.setuptools]
|
||||
packages = [
|
||||
"coupler_panel",
|
||||
"coupler_panel.cad",
|
||||
"coupler_panel.sampling",
|
||||
"coupler_panel.simulation",
|
||||
"coupler_panel.denoise",
|
||||
"coupler_panel.registration",
|
||||
"coupler_panel.detection",
|
||||
"coupler_panel.geometry",
|
||||
"coupler_panel.metrics",
|
||||
"coupler_panel.viz",
|
||||
"coupler_panel.io_utils",
|
||||
"coupler_panel.roi_select",
|
||||
"coupler_panel.reporting",
|
||||
]
|
||||
include-package-data = true
|
||||
|
||||
[tool.setuptools.package-data]
|
||||
coupler_panel = ["py.typed"]
|
||||
30
alg/delivery_v1.2.1/coupler_panel_release/requirements.txt
Normal file
30
alg/delivery_v1.2.1/coupler_panel_release/requirements.txt
Normal file
@@ -0,0 +1,30 @@
|
||||
# coupler-panel-validation — locked dependency list (Python 3.10, end-to-end verified).
|
||||
#
|
||||
# python -m pip install -r requirements.txt
|
||||
# pip install -e .
|
||||
#
|
||||
# Or install only a subset via the extras in pyproject.toml:
|
||||
# pip install -e ".[cloud]" # core + point-cloud IO + denoise
|
||||
# pip install -e ".[cad]" # core + STP parsing + GT extraction
|
||||
# pip install -e ".[viz]" # core + matplotlib diagnostics
|
||||
# pip install -e ".[report]" # core + Excel batch reporting
|
||||
# pip install -e ".[all]" # everything
|
||||
|
||||
# --- Core compute (always required) ---
|
||||
numpy==1.26.4
|
||||
scipy==1.15.3
|
||||
pyyaml==6.0.2
|
||||
tqdm==4.65.0
|
||||
|
||||
# --- Point-cloud IO + denoise (PLY/PCD read/write, voxel/SOR/ROR) ---
|
||||
open3d==0.19.0
|
||||
trimesh==4.12.2
|
||||
|
||||
# --- STP/STEP parsing (only needed when passing cad_stp_path) ---
|
||||
cadquery-ocp==7.8.1.1.post1
|
||||
|
||||
# --- Diagnostic PNG renders (coupler_panel.viz, optional) ---
|
||||
matplotlib==3.10.8
|
||||
|
||||
# --- Excel batch-eval reporting (coupler_panel.reporting) ---
|
||||
openpyxl==3.1.2
|
||||
73
alg/delivery_v1.2.1/underframe_deflection/README.md
Normal file
73
alg/delivery_v1.2.1/underframe_deflection/README.md
Normal file
@@ -0,0 +1,73 @@
|
||||
# underframe-deflection
|
||||
|
||||
**高铁底架纵梁挠度测量算法（基于点云）。** 输入待测点云 + 理论 CAD + ROI，输出含挠度核心数据的 Excel。
|
||||
|
||||
[]() []()
|
||||
|
||||
---
|
||||
|
||||
## 这是什么
|
||||
|
||||
两种测量模式：
|
||||
|
||||
- **单梁模式（默认）** — ROI 已框住单根纵梁（左右各一个 ROI），裁剪后直接测量。
|
||||
支持流式裁剪超大点云（实测 12GB / 10 亿点整车 PLY），左右各出一份 Excel。**这是当前交付的主模式。**
|
||||
- **双梁模式（`--mode dual`）** — 一片含左右两梁的车底点云 + CAD 配准 + 双梁追踪（v1.1 算法，保留）。
|
||||
|
||||
原理（单梁）：沿车长切片 → 每片取下表面主平面高度 → 鲁棒拟合直线作理论基准 → 挠度 = 下表面 − 基准。
|
||||
理论平直的梁，挠度 ≈ 0。
|
||||
|
||||
---
|
||||
|
||||
## 快速开始
|
||||
|
||||
```bash
|
||||
pip install -e .[all]
|
||||
|
||||
underframe-measure \
|
||||
--input-cloud train.ply \
|
||||
--left-roi roi_left_beam.json \
|
||||
--right-roi roi_right_beam.json \
|
||||
--cad theoretical.stp \
|
||||
--out-dir out/
|
||||
```
|
||||
|
||||
→ `out/left/report.xlsx` 和 `out/right/report.xlsx`（左右各一份独立结果）。
|
||||
|
||||
`test_data/roi_left_beam.json` / `roi_right_beam.json` 是可直接用的样例 ROI。
|
||||
|
||||
---
|
||||
|
||||
## 文档
|
||||
|
||||
| 文档 | 面向 | 内容 |
|
||||
| --- | --- | --- |
|
||||
| **README.md**（本文件） | 所有人 | 项目总览、快速开始 |
|
||||
| **[USAGE.md](./USAGE.md)** | 集成开发者 | 输入三件套、调用方式、输出 Excel 字段、配准开关、异常 |
|
||||
| **[DEV_NOTES.md](./DEV_NOTES.md)** | 维护者 | 本次修改记录：改了什么、为什么、遇到什么问题、怎么解决 |
|
||||
|
||||
---
|
||||
|
||||
## 坐标系约定
|
||||
|
||||
`x = 车长（纵向）`，`y = 车高（挠度方向）`，`z = 车宽（横向）`，单位 mm。
|
||||
ROI JSON 的 bbox 字段用同一坐标系。详见 USAGE.md。
|
||||
|
||||
---
|
||||
|
||||
## 安装
|
||||
|
||||
```bash
|
||||
pip install -e .[all] # core + matplotlib(图) + openpyxl(Excel)
|
||||
pip install -e . # 仅核心 numpy + scipy
|
||||
```
|
||||
|
||||
安装后注册命令 `underframe-measure`，Python 内 `import underframe_deflection`。
|
||||
|
||||
---
|
||||
|
||||
## 状态与版本
|
||||
|
||||
- **当前版本**：1.2.0（单梁直接测量模式 + 流式裁剪超大点云）
|
||||
- **Python**：≥ 3.10（Windows / Linux / macOS）
|
||||
- License：Proprietary — PKU High-Speed Train Project
|
||||
200
alg/delivery_v1.2.1/underframe_deflection/USAGE.md
Normal file
200
alg/delivery_v1.2.1/underframe_deflection/USAGE.md
Normal file
@@ -0,0 +1,200 @@
|
||||
# underframe_deflection 使用指南
|
||||
|
||||
> 面向软件集成方。说明如何用「**待测整车点云 + 理论 CAD（stp）+ ROI JSON**」三件套输入，
|
||||
> 调用算法，得到含挠度核心数据的 Excel 输出。
|
||||
|
||||
---
|
||||
|
||||
## 1. 输入与输出
|
||||
|
||||
### 输入（集成方提供）
|
||||
|
||||
| 文件 | 格式 | 必需 | 说明 |
|
||||
| --- | --- | --- | --- |
|
||||
| **待测整车点云** | `.ply`（ASCII 或 binary） | ✅ | 现场扫描的整车点云。支持超大文件（实测 12GB / 10 亿点，流式裁剪不爆内存） |
|
||||
| **理论 CAD** | `.stp` | ✅（按需用） | 理论模型。**当前实例点云已配准**，CAD 仅记录路径作溯源；需重新配准时见 §4 |
|
||||
| **ROI JSON** | `.json` | ✅ | 每根梁一个，描述该梁的轴对齐裁剪框（AABB） |
|
||||
|
||||
> 左右梁是**两次独立调用**：左梁一个 ROI → 一份输出；右梁一个 ROI → 一份输出。
|
||||
|
||||
### 输出（左右各一份）
|
||||
|
||||
```
|
||||
out_dir/
|
||||
├── left/
|
||||
│ ├── report.xlsx ← 挠度核心数据 Excel（Summary + Deflection_Curve）
|
||||
│ ├── report.json ← 完整结果（机器读）
|
||||
│ ├── deflection_curve.csv ← 沿车长的逐点挠度
|
||||
│ └── deflection_curve.png ← 诊断图
|
||||
├── right/ ← 同上结构
|
||||
└── summary.json ← 左右汇总
|
||||
```
|
||||
|
||||
---
|
||||
|
||||
## 2. ROI JSON 格式
|
||||
|
||||
坐标系：**x = 车长（纵向），y = 车高（挠度方向），z = 车宽（横向）**，单位 mm。
|
||||
|
||||
```json
|
||||
{
|
||||
"x_min": -100000.0,
|
||||
"x_max": 100000.0,
|
||||
"y_min": 919.950,
|
||||
"y_max": 1399.950,
|
||||
"z_min": 948.623,
|
||||
"z_max": 1752.212
|
||||
}
|
||||
```
|
||||
|
||||
- `x` 给一个覆盖整车长度的大区间即可。
|
||||
- `y`（车高）框住梁所在的高度带。
|
||||
- `z`（车宽）分左右：左梁取正侧，右梁取负侧。
|
||||
- 仓库 `test_data/roi_left_beam.json` / `roi_right_beam.json` 是可直接用的样例。
|
||||
|
||||
约束：每轴 `min < max`，均为有限实数；非法 → `InvalidPointCloudError`。
|
||||
|
||||
---
|
||||
|
||||
## 3. 调用方式
|
||||
|
||||
### 3.1 命令行
|
||||
|
||||
```bash
|
||||
underframe-measure \
|
||||
--input-cloud train.ply \
|
||||
--left-roi roi_left_beam.json \
|
||||
--right-roi roi_right_beam.json \
|
||||
--cad theoretical.stp \
|
||||
--out-dir out/
|
||||
```
|
||||
|
||||
默认 `--mode single`、默认**不配准**（点云已配准），`--cad` 的 stp 路径记录进报告。
|
||||
|
||||
可选开关：`--enable-registration`（§4）、`--config FILE.json`（覆盖参数）、
|
||||
`--skip-excel`、`--skip-plot`、`--side left|right`（配 `--roi` 单根时用）。
|
||||
|
||||
### 3.2 Python API（推荐集成）
|
||||
|
||||
```python
|
||||
from underframe_deflection import run_both_beams
|
||||
|
||||
summary = run_both_beams(
|
||||
input_cloud="train.ply",
|
||||
left_roi="roi_left_beam.json",
|
||||
right_roi="roi_right_beam.json",
|
||||
out_dir="out/",
|
||||
cad_path="theoretical.stp", # 记录路径；配准默认关
|
||||
enable_registration=False, # 见 §4
|
||||
)
|
||||
|
||||
for side in ("left", "right"):
|
||||
d = summary[side]["deflection"]
|
||||
print(side,
|
||||
"最大挠度(带符号) =", d["selected_signed_mm"], "mm",
|
||||
"@ x =", d["selected_x_mm"], "mm",
|
||||
"| RMS =", d["rms_mm"], "mm")
|
||||
```
|
||||
|
||||
单根梁独立调用：
|
||||
|
||||
```python
|
||||
from underframe_deflection import run_single_beam_pipeline
|
||||
|
||||
report = run_single_beam_pipeline(
|
||||
input_cloud="train.ply",
|
||||
roi_path="roi_left_beam.json",
|
||||
out_dir="out/left",
|
||||
side="left",
|
||||
cad_path="theoretical.stp",
|
||||
)
|
||||
```
|
||||
|
||||
---
|
||||
|
||||
## 4. 配准开关
|
||||
|
||||
挠度测量的完整物理流程：**扫描点云 → 用 CAD 配准到标准姿态 → 测梁下表面相对理想基准的偏差**。
|
||||
配准消除安装 / 摆放 / 扫描姿态引入的整体倾斜。
|
||||
|
||||
| `enable_registration` | 行为 | 适用 |
|
||||
| --- | --- | --- |
|
||||
| `False`（默认） | 不配准，点云按原样裁剪测量 | **点云已预配准**（本实例） |
|
||||
| `True` | 先把整车点云刚性对齐到 CAD，再裁剪测量 | 点云未配准 |
|
||||
|
||||
配准开启时对 CAD 的要求：
|
||||
- 必须是**可读的点云 / 网格**（`.ply / .xyz / .stl / .obj`）。
|
||||
- `.stp` **不直接解析**（STEP 解析需重型依赖）。若只有 stp，先用
|
||||
`tools/cad_sampling/step_to_pointcloud.py` 离线采样成 `.ply` 再传 `--cad`。
|
||||
- 传 stp + 开配准时**不会崩溃**，而是记录提示"STEP not parsed; convert first"并跳过配准。
|
||||
|
||||
> 本次交付的测试样本点云**已配准**，所以默认关。配准算法（ICP）已完整保留，随时可启用。
|
||||
|
||||
---
|
||||
|
||||
## 5. 输出 Excel（report.xlsx，左右各一份）
|
||||
|
||||
### Sheet「检测报表」（核心交付，6 列）
|
||||
|
||||
| 列 | 本项目取值 |
|
||||
| --- | --- |
|
||||
| 项点名称 | 纵向梁到直线的垂直偏差（挠度） |
|
||||
| 理论尺寸 | 0 |
|
||||
| 公差要求 | 左梁 ±7mm / 右梁 ±12mm |
|
||||
| 测量结果 | 实测最大挠度（mm） |
|
||||
| 误差结果 | 与理论 0 的误差（= 测量结果，因理论值为 0） |
|
||||
| 测量结论 | 误差 ≤ 公差 → **合格**，否则 → **不合格** |
|
||||
|
||||
**左右判定**：面朝 x 正方向，`z > 0` 为左梁（公差 7mm），`z < 0` 为右梁（公差 12mm）。
|
||||
程序按 ROI 的 z 中心**自动判定**，不依赖文件名——即使左右 ROI 文件传反也不会用错公差。
|
||||
|
||||
公差可覆盖：命令行 `--left-tolerance` / `--right-tolerance`（两梁模式）或 `--tolerance`（单梁模式）；
|
||||
Python API 同名参数 `left_tolerance_mm` / `right_tolerance_mm` / `tolerance_mm`。
|
||||
|
||||
### Sheet「明细」（溯源，key/value）
|
||||
|
||||
输入文件路径、是否配准、ROI 命中点数、切片数、参考线斜率、最大下凹/上凸、RMS、质量警告等。
|
||||
|
||||
### Sheet「挠度曲线」
|
||||
|
||||
沿车长逐点表：`x_mm, underside_height_mm, reference_height_mm, deflection_mm, kept`。
|
||||
|
||||
---
|
||||
|
||||
## 6. 异常与质量标记
|
||||
|
||||
| 异常 | 触发 |
|
||||
| --- | --- |
|
||||
| `InvalidPointCloudError` | 点云 / ROI 格式非法、AABB 反转 |
|
||||
| `InsufficientPointsError` | ROI 命中点过少（< 2000） |
|
||||
|
||||
非致命质量标记（写进 `quality_flags`，不抛异常）：
|
||||
`many_underside_spikes`（悬挂物/噪点偏多，已鲁棒剔除）、`low_surface_support`（下表面支撑点偏少）、
|
||||
`insufficient_valid_slices` / `fit_unstable`（有效切片不足）。
|
||||
|
||||
集成方建议：外层 `try / except DeflectionError` 兜底，并把 `quality_flags` 落日志。
|
||||
|
||||
---
|
||||
|
||||
## 7. 算法原理
|
||||
|
||||
沿车长（x）分细切片 → 每片取**下表面主平面高度**（下半部直方图主峰，对悬挂物 / 噪点鲁棒）→
|
||||
鲁棒拟合一条直线作理论基准 → 挠度 = 下表面高度 − 基准线。理论平直的梁，挠度 ≈ 0。
|
||||
|
||||
> 设计与验证的完整记录见 [`DEV_NOTES.md`](./DEV_NOTES.md)。
|
||||
|
||||
---
|
||||
|
||||
## 附录：双梁模式（legacy）
|
||||
|
||||
输入一片含左右两梁的车底点云 + CAD 基准 + ROI，自动配准 + 双梁追踪 + 样条拟合 + 孔参考线。
|
||||
|
||||
```bash
|
||||
underframe-measure --mode dual \
|
||||
--input-cloud scan.ply --cad reference.ply --roi roi.json \
|
||||
--out-dir out/ --input-axis y_up
|
||||
```
|
||||
|
||||
- `--input-axis {y_up,z_up}`：高度轴约定，默认 `y_up`（CAD/扫描约定 y=高度）。
|
||||
- 输出 5-sheet Excel + 5 PNG + 4 PLY 叠加图。
|
||||
- 该模式针对"双梁车底slab"，与单梁模式互不影响。
|
||||
67
alg/delivery_v1.2.1/underframe_deflection/__init__.py
Normal file
67
alg/delivery_v1.2.1/underframe_deflection/__init__.py
Normal file
@@ -0,0 +1,67 @@
|
||||
"""Point-cloud-only underframe beam deflection measurement.
|
||||
|
||||
Public API (stable across patch releases — see USAGE.md):
|
||||
|
||||
- :func:`compute_deflection` — pure, in-memory computation; returns a
|
||||
:class:`DeflectionResult`.
|
||||
- :func:`load_point_cloud` — read a PLY/XYZ/CSV file into a numpy array.
|
||||
- :class:`DeflectionResult`, :class:`SideDeflection`, :class:`DeflectionCurves`
|
||||
— the result schema.
|
||||
- :mod:`underframe_deflection.artifacts` — optional file writers
|
||||
(JSON / CSV / PNG plots / coloured PLY overlays).
|
||||
- :mod:`underframe_deflection.exceptions` — exception hierarchy.
|
||||
- :func:`run_measurement_pipeline` — convenience wrapper that does
|
||||
read + compute + write-all in one call.
|
||||
"""
|
||||
|
||||
from ._version import ALGORITHM_VERSION, SCHEMA_VERSION, __version__
|
||||
from .api import (
|
||||
DeflectionCurves,
|
||||
DeflectionResult,
|
||||
ProgressCallback,
|
||||
SideDeflection,
|
||||
compute_deflection,
|
||||
load_point_cloud,
|
||||
)
|
||||
from .cad_io import read_cad_reference
|
||||
from .exceptions import (
|
||||
DeflectionError,
|
||||
FrameInferenceError,
|
||||
InsufficientPointsError,
|
||||
InvalidPointCloudError,
|
||||
UnsupportedFormatError,
|
||||
)
|
||||
from .pipeline import run_measurement_pipeline
|
||||
from .preprocess import bilateral_filter_points
|
||||
from .registration import RegistrationResult, register_to_cad
|
||||
from .roi import apply_passthrough, load_roi_aabb
|
||||
from .single_beam import SingleBeamResult, measure_single_beam
|
||||
from .single_beam_pipeline import run_both_beams, run_single_beam_pipeline
|
||||
|
||||
__all__ = [
|
||||
"__version__",
|
||||
"ALGORITHM_VERSION",
|
||||
"SCHEMA_VERSION",
|
||||
"compute_deflection",
|
||||
"load_point_cloud",
|
||||
"DeflectionResult",
|
||||
"SideDeflection",
|
||||
"DeflectionCurves",
|
||||
"ProgressCallback",
|
||||
"run_measurement_pipeline",
|
||||
"DeflectionError",
|
||||
"InvalidPointCloudError",
|
||||
"InsufficientPointsError",
|
||||
"FrameInferenceError",
|
||||
"UnsupportedFormatError",
|
||||
"read_cad_reference",
|
||||
"load_roi_aabb",
|
||||
"apply_passthrough",
|
||||
"bilateral_filter_points",
|
||||
"register_to_cad",
|
||||
"RegistrationResult",
|
||||
"measure_single_beam",
|
||||
"SingleBeamResult",
|
||||
"run_single_beam_pipeline",
|
||||
"run_both_beams",
|
||||
]
|
||||
18
alg/delivery_v1.2.1/underframe_deflection/_version.py
Normal file
18
alg/delivery_v1.2.1/underframe_deflection/_version.py
Normal file
@@ -0,0 +1,18 @@
|
||||
"""Version metadata for the underframe_deflection package.
|
||||
|
||||
`__version__` tracks the package release; bump on any user-visible change.
|
||||
`ALGORITHM_VERSION` tracks the deflection algorithm itself; bump whenever
|
||||
internal computation changes in a way that may shift numeric results.
|
||||
`SCHEMA_VERSION` tracks the structure of `DeflectionResult.to_dict()` /
|
||||
`report.json`; bump on any field rename, removal, or semantic change.
|
||||
Adding new optional fields is a non-breaking change and does NOT require a
|
||||
schema version bump.
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
__version__ = "1.2.0"
|
||||
ALGORITHM_VERSION = "0.1.0"
|
||||
SCHEMA_VERSION = "1.0"
|
||||
|
||||
__all__ = ["__version__", "ALGORITHM_VERSION", "SCHEMA_VERSION"]
|
||||
795
alg/delivery_v1.2.1/underframe_deflection/api.py
Normal file
795
alg/delivery_v1.2.1/underframe_deflection/api.py
Normal file
@@ -0,0 +1,795 @@
|
||||
"""Public, stable API for the underframe deflection algorithm.
|
||||
|
||||
This module is the contract integrators should depend on. The internal
|
||||
algorithm modules (`beams`, `fitting`, `reference`, ...) are implementation
|
||||
details and may change between releases without notice; this module's
|
||||
function signatures and `DeflectionResult` schema are the stable surface.
|
||||
|
||||
Typical usage::
|
||||
|
||||
from underframe_deflection import compute_deflection, load_point_cloud
|
||||
|
||||
points = load_point_cloud("scan.ply")
|
||||
result = compute_deflection(points)
|
||||
print(result.left.selected_signed_mm, result.right.selected_signed_mm)
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
from dataclasses import asdict, dataclass, field
|
||||
from pathlib import Path
|
||||
from typing import Any, Callable, Sequence
|
||||
|
||||
import numpy as np
|
||||
|
||||
from . import io as _io
|
||||
from ._version import ALGORITHM_VERSION, SCHEMA_VERSION
|
||||
from .beams import candidate_rows, extract_beam_slices, track_side_candidates
|
||||
from .config import cloud_geometry, load_config, resolve_numeric_parameters
|
||||
from .exceptions import InsufficientPointsError, InvalidPointCloudError
|
||||
from .fitting import (
|
||||
extract_representative_points,
|
||||
fit_representative_line,
|
||||
representatives_to_rows,
|
||||
evaluate_line,
|
||||
)
|
||||
from .frame import (
|
||||
build_bottom_roi_mask,
|
||||
frame_hypotheses_to_dict,
|
||||
infer_vehicle_frame,
|
||||
transform_points,
|
||||
transform_vectors,
|
||||
)
|
||||
from .preprocess import estimate_spacing, preprocess_point_cloud
|
||||
from .reference import detect_reference_line, evaluate_reference_line
|
||||
from .structures import (
|
||||
FittedLine,
|
||||
FrameHypothesis,
|
||||
ReferenceLine,
|
||||
RepresentativePoint,
|
||||
SliceResult,
|
||||
TrackSolution,
|
||||
)
|
||||
|
||||
|
||||
ProgressCallback = Callable[[str, float], None]
|
||||
"""Signature for progress callbacks: ``callback(stage_name, fraction_in_[0,1])``."""
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# Public result types
|
||||
# ---------------------------------------------------------------------------
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class SideDeflection:
|
||||
"""Per-side deflection summary.
|
||||
|
||||
All distances are millimetres. ``signed`` values are positive when the
|
||||
beam sits above the reference line and negative when it sags below.
|
||||
Fields whose value is ``None`` were not measurable on this side; check
|
||||
``available`` and the parent result's ``quality_flags`` for the reason.
|
||||
"""
|
||||
|
||||
side: str
|
||||
available: bool
|
||||
reference_available: bool
|
||||
reference_confidence: float
|
||||
max_downward_mm: float | None
|
||||
max_downward_x_mm: float | None
|
||||
max_upward_mm: float | None
|
||||
max_upward_x_mm: float | None
|
||||
selected_x_mm: float | None
|
||||
selected_signed_mm: float | None
|
||||
selected_absolute_mm: float | None
|
||||
beam_fit_residual_rms_z_mm: float | None
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class DeflectionCurves:
|
||||
"""Sampled deflection along the longitudinal axis.
|
||||
|
||||
Arrays are aligned by index to ``x_mm``. Deflection arrays may contain
|
||||
``NaN`` where the curve is undefined (no overlap of beam fit and
|
||||
reference line, or low confidence).
|
||||
"""
|
||||
|
||||
x_mm: np.ndarray
|
||||
left_deflection_mm: np.ndarray
|
||||
right_deflection_mm: np.ndarray
|
||||
left_confidence: np.ndarray
|
||||
right_confidence: np.ndarray
|
||||
|
||||
|
||||
@dataclass
|
||||
class _ComputeArtifacts:
|
||||
"""Internal payload consumed by ``artifacts`` writers.
|
||||
|
||||
Not part of the public API. Fields may change between releases and
|
||||
should only be accessed via the artifact writer functions in
|
||||
``underframe_deflection.artifacts``.
|
||||
"""
|
||||
|
||||
raw_points: np.ndarray
|
||||
filtered_vehicle: np.ndarray
|
||||
work_vehicle: np.ndarray
|
||||
work_roi: np.ndarray
|
||||
filtered_roi: np.ndarray
|
||||
frame_hypotheses: list[FrameHypothesis]
|
||||
best_frame: FrameHypothesis
|
||||
slices: list[SliceResult]
|
||||
left_tracks: list[TrackSolution]
|
||||
right_tracks: list[TrackSolution]
|
||||
best_left: TrackSolution | None
|
||||
best_right: TrackSolution | None
|
||||
left_reps: list[RepresentativePoint]
|
||||
right_reps: list[RepresentativePoint]
|
||||
left_line: FittedLine | None
|
||||
right_line: FittedLine | None
|
||||
left_reference: ReferenceLine
|
||||
right_reference: ReferenceLine
|
||||
x_grid: np.ndarray
|
||||
left_payload: dict[str, np.ndarray]
|
||||
right_payload: dict[str, np.ndarray]
|
||||
deflection_rows: list[dict]
|
||||
left_summary: dict
|
||||
right_summary: dict
|
||||
resolved_config: dict[str, Any]
|
||||
|
||||
|
||||
@dataclass
|
||||
class DeflectionResult:
|
||||
"""The result of :func:`compute_deflection`.
|
||||
|
||||
Public, stable fields:
|
||||
|
||||
- :attr:`schema_version`, :attr:`algorithm_version` — version stamps
|
||||
- :attr:`left`, :attr:`right` — :class:`SideDeflection` headline results
|
||||
- :attr:`curves` — :class:`DeflectionCurves` sampled curves
|
||||
- :attr:`quality_flags` — list of non-fatal data-quality warnings
|
||||
- :attr:`report` — full nested dict (same content as ``report.json``)
|
||||
|
||||
The private attribute :attr:`_artifacts` carries internal data needed
|
||||
by :mod:`underframe_deflection.artifacts` writers and should not be
|
||||
accessed directly by integrators.
|
||||
"""
|
||||
|
||||
schema_version: str
|
||||
algorithm_version: str
|
||||
left: SideDeflection
|
||||
right: SideDeflection
|
||||
curves: DeflectionCurves
|
||||
quality_flags: list[str]
|
||||
report: dict
|
||||
_artifacts: _ComputeArtifacts | None = field(default=None, repr=False, compare=False)
|
||||
|
||||
def to_dict(self) -> dict:
|
||||
"""Return a fully-serializable dict (same content as ``report.json``)."""
|
||||
return self.report
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# Public entry points
|
||||
# ---------------------------------------------------------------------------
|
||||
|
||||
|
||||
def load_point_cloud(path: str | Path) -> np.ndarray:
|
||||
"""Load a point cloud from disk into an ``(N, 3)`` ``float64`` array.
|
||||
|
||||
Supported formats: ``.ply`` (ASCII), ``.xyz``, ``.txt``, ``.csv``.
|
||||
|
||||
Raises:
|
||||
UnsupportedFormatError: extension is not recognised.
|
||||
InvalidPointCloudError: file is malformed.
|
||||
"""
|
||||
try:
|
||||
return _io.read_point_cloud(path)
|
||||
except ValueError as exc:
|
||||
# io.read_point_cloud raises ValueError for both unsupported format
|
||||
# and malformed PLY headers; surface as a structured exception.
|
||||
from .exceptions import UnsupportedFormatError
|
||||
|
||||
if "Unsupported point cloud format" in str(exc):
|
||||
raise UnsupportedFormatError(str(exc)) from exc
|
||||
raise InvalidPointCloudError(str(exc)) from exc
|
||||
|
||||
|
||||
def compute_deflection(
|
||||
points: np.ndarray | Sequence[Sequence[float]],
|
||||
config: dict | None = None,
|
||||
profile: str = "auto",
|
||||
*,
|
||||
progress: ProgressCallback | None = None,
|
||||
roi_aabb: dict | None = None,
|
||||
) -> DeflectionResult:
|
||||
"""Run the full deflection pipeline on an in-memory point cloud.
|
||||
|
||||
This function performs no file I/O. To persist the result use the
|
||||
writers in :mod:`underframe_deflection.artifacts`.
|
||||
|
||||
Args:
|
||||
points: ``(N, 3)`` array-like of XYZ coordinates in millimetres.
|
||||
The X axis is assumed longitudinal, Y lateral, Z vertical
|
||||
(positive up). If your scan uses a different convention,
|
||||
transform it before calling this function.
|
||||
config: Optional config override dict. Use the same shape as
|
||||
:data:`underframe_deflection.config.DEFAULT_CONFIG`. Values
|
||||
you omit fall back to the profile defaults.
|
||||
profile: Tuning profile name. ``"auto"`` works for most scans.
|
||||
See :data:`underframe_deflection.config.PROFILE_OVERRIDES`
|
||||
for the available profiles.
|
||||
progress: Optional callback invoked at each major stage with the
|
||||
stage name and a ``fraction in [0, 1]``.
|
||||
|
||||
Returns:
|
||||
:class:`DeflectionResult` with the headline numbers, sampled
|
||||
curves, full report dict, and quality flags.
|
||||
|
||||
Raises:
|
||||
InvalidPointCloudError: ``points`` is not a 2D ``(N, 3)`` array.
|
||||
InsufficientPointsError: too few points to run the pipeline.
|
||||
"""
|
||||
raw_points = _validate_points(points)
|
||||
|
||||
def _emit(stage: str, fraction: float) -> None:
|
||||
if progress is not None:
|
||||
progress(stage, float(fraction))
|
||||
|
||||
_emit("preprocess", 0.0)
|
||||
base_config = load_config(None, profile)
|
||||
if config:
|
||||
from .config import deep_merge
|
||||
|
||||
base_config = deep_merge(base_config, config)
|
||||
|
||||
if roi_aabb is not None:
|
||||
from .roi import normalize_aabb
|
||||
|
||||
normalized = normalize_aabb(roi_aabb)
|
||||
base_config["roi"]["aabb"] = normalized
|
||||
passthrough = base_config["preprocess"].setdefault(
|
||||
"passthrough", {"enabled": False, "aabb": None}
|
||||
)
|
||||
if passthrough.get("aabb") is None:
|
||||
passthrough["enabled"] = True
|
||||
passthrough["aabb"] = normalized
|
||||
|
||||
initial_geometry = cloud_geometry(raw_points)
|
||||
initial_spacing = estimate_spacing(
|
||||
raw_points,
|
||||
sample_size=base_config["preprocess"]["spacing_sample_size"],
|
||||
)
|
||||
preprocess_config = resolve_numeric_parameters(base_config, initial_geometry, initial_spacing)
|
||||
preprocess_result = preprocess_point_cloud(raw_points, preprocess_config)
|
||||
_emit("preprocess", 1.0)
|
||||
|
||||
_emit("frame", 0.0)
|
||||
if base_config["frame"].get("use_input_axes", False):
|
||||
best_frame = FrameHypothesis(
|
||||
name="input_axes_identity",
|
||||
axes=np.eye(3, dtype=np.float64),
|
||||
origin=np.zeros(3, dtype=np.float64),
|
||||
score=1.0,
|
||||
bottom_score=1.0,
|
||||
symmetry_score=1.0,
|
||||
flatness_score=1.0,
|
||||
hole_score=1.0,
|
||||
continuity_score=1.0,
|
||||
)
|
||||
frame_hypotheses = [best_frame]
|
||||
else:
|
||||
frame_hypotheses, best_frame = infer_vehicle_frame(
|
||||
preprocess_result.work_points,
|
||||
preprocess_result.work_normals,
|
||||
preprocess_result.work_curvature,
|
||||
preprocess_config,
|
||||
)
|
||||
_emit("frame", 1.0)
|
||||
|
||||
filtered_vehicle = transform_points(
|
||||
preprocess_result.filtered_points, best_frame.axes, best_frame.origin
|
||||
)
|
||||
work_vehicle = transform_points(
|
||||
preprocess_result.work_points, best_frame.axes, best_frame.origin
|
||||
)
|
||||
work_normals_vehicle = transform_vectors(preprocess_result.work_normals, best_frame.axes)
|
||||
|
||||
vehicle_geometry = cloud_geometry(work_vehicle)
|
||||
resolved_config = resolve_numeric_parameters(
|
||||
base_config, vehicle_geometry, preprocess_result.spacing
|
||||
)
|
||||
roi_mask_work, roi_payload = build_bottom_roi_mask(work_vehicle, resolved_config)
|
||||
roi_mask_filtered = _apply_roi_mask(filtered_vehicle, roi_payload)
|
||||
|
||||
work_roi = work_vehicle[roi_mask_work]
|
||||
work_roi_normals = work_normals_vehicle[roi_mask_work]
|
||||
filtered_roi = filtered_vehicle[roi_mask_filtered]
|
||||
|
||||
_emit("beam_tracking", 0.0)
|
||||
slices = extract_beam_slices(work_roi, work_roi_normals, resolved_config)
|
||||
left_tracks = track_side_candidates(slices, "left", resolved_config)
|
||||
right_tracks = track_side_candidates(slices, "right", resolved_config)
|
||||
best_left = left_tracks[0] if left_tracks else None
|
||||
best_right = right_tracks[0] if right_tracks else None
|
||||
_emit("beam_tracking", 1.0)
|
||||
|
||||
_emit("line_fitting", 0.0)
|
||||
left_reps = (
|
||||
extract_representative_points(filtered_roi, best_left, resolved_config)
|
||||
if best_left
|
||||
else []
|
||||
)
|
||||
right_reps = (
|
||||
extract_representative_points(filtered_roi, best_right, resolved_config)
|
||||
if best_right
|
||||
else []
|
||||
)
|
||||
left_line = fit_representative_line(left_reps, resolved_config, "left")
|
||||
right_line = fit_representative_line(right_reps, resolved_config, "right")
|
||||
_emit("line_fitting", 1.0)
|
||||
|
||||
_emit("reference", 0.0)
|
||||
left_reference = (
|
||||
detect_reference_line(filtered_roi, left_line, resolved_config, "left")
|
||||
if left_line is not None
|
||||
else ReferenceLine("left", False, None, None, None, 0.0, None)
|
||||
)
|
||||
right_reference = (
|
||||
detect_reference_line(filtered_roi, right_line, resolved_config, "right")
|
||||
if right_line is not None
|
||||
else ReferenceLine("right", False, None, None, None, 0.0, None)
|
||||
)
|
||||
_emit("reference", 1.0)
|
||||
|
||||
_emit("deflection", 0.0)
|
||||
line_candidates = [line for line in (left_line, right_line) if line is not None]
|
||||
if line_candidates:
|
||||
grid_step = (
|
||||
resolved_config["representative"]["grid_step_mm"]
|
||||
* resolved_config["deflection"]["evaluation_grid_factor"]
|
||||
)
|
||||
x_min = min(float(line.x_support.min()) for line in line_candidates)
|
||||
x_max = max(float(line.x_support.max()) for line in line_candidates)
|
||||
x_grid = np.arange(x_min, x_max + grid_step, grid_step)
|
||||
else:
|
||||
x_grid = np.zeros(0, dtype=np.float64)
|
||||
|
||||
left_summary, left_payload = _evaluate_side_deflection("left", left_line, left_reference, x_grid)
|
||||
right_summary, right_payload = _evaluate_side_deflection(
|
||||
"right", right_line, right_reference, x_grid
|
||||
)
|
||||
deflection_rows = (
|
||||
_build_deflection_rows(x_grid, left_payload, right_payload) if len(x_grid) else []
|
||||
)
|
||||
_emit("deflection", 1.0)
|
||||
|
||||
symmetry = _side_symmetry_metrics(best_left, best_right)
|
||||
quality_flags = _collect_quality_flags(
|
||||
best_left,
|
||||
best_right,
|
||||
left_line,
|
||||
right_line,
|
||||
left_reference,
|
||||
right_reference,
|
||||
symmetry,
|
||||
resolved_config,
|
||||
)
|
||||
|
||||
report = {
|
||||
"schema_version": SCHEMA_VERSION,
|
||||
"algorithm_version": ALGORITHM_VERSION,
|
||||
"axis_convention": {"x": "longitudinal", "y": "lateral", "z": "height"},
|
||||
"profile": profile,
|
||||
"resolved_config": resolved_config,
|
||||
"preprocess": preprocess_result.stats,
|
||||
"frame_hypotheses": frame_hypotheses_to_dict(frame_hypotheses),
|
||||
"selected_frame": frame_hypotheses_to_dict([best_frame])[0],
|
||||
"roi": roi_payload,
|
||||
"symmetry_check": symmetry,
|
||||
"left_track": _track_to_dict(best_left),
|
||||
"right_track": _track_to_dict(best_right),
|
||||
"representative_lines": {
|
||||
"left": _fitted_line_to_dict(left_line),
|
||||
"right": _fitted_line_to_dict(right_line),
|
||||
},
|
||||
"reference_lines": {
|
||||
"left": _reference_to_dict(left_reference),
|
||||
"right": _reference_to_dict(right_reference),
|
||||
},
|
||||
"alternate_solutions": {
|
||||
"left_tracks": [_track_to_dict(track) for track in left_tracks[1:]],
|
||||
"right_tracks": [_track_to_dict(track) for track in right_tracks[1:]],
|
||||
"left_reference": [dict(item) for item in left_reference.alternates],
|
||||
"right_reference": [dict(item) for item in right_reference.alternates],
|
||||
},
|
||||
"deflection": {"left": left_summary, "right": right_summary},
|
||||
"quality_flags": quality_flags,
|
||||
"representative_points": {
|
||||
"left": representatives_to_rows(left_reps),
|
||||
"right": representatives_to_rows(right_reps),
|
||||
},
|
||||
}
|
||||
|
||||
artifacts = _ComputeArtifacts(
|
||||
raw_points=raw_points,
|
||||
filtered_vehicle=filtered_vehicle,
|
||||
work_vehicle=work_vehicle,
|
||||
work_roi=work_roi,
|
||||
filtered_roi=filtered_roi,
|
||||
frame_hypotheses=frame_hypotheses,
|
||||
best_frame=best_frame,
|
||||
slices=slices,
|
||||
left_tracks=left_tracks,
|
||||
right_tracks=right_tracks,
|
||||
best_left=best_left,
|
||||
best_right=best_right,
|
||||
left_reps=left_reps,
|
||||
right_reps=right_reps,
|
||||
left_line=left_line,
|
||||
right_line=right_line,
|
||||
left_reference=left_reference,
|
||||
right_reference=right_reference,
|
||||
x_grid=x_grid,
|
||||
left_payload=left_payload,
|
||||
right_payload=right_payload,
|
||||
deflection_rows=deflection_rows,
|
||||
left_summary=left_summary,
|
||||
right_summary=right_summary,
|
||||
resolved_config=resolved_config,
|
||||
)
|
||||
|
||||
return DeflectionResult(
|
||||
schema_version=SCHEMA_VERSION,
|
||||
algorithm_version=ALGORITHM_VERSION,
|
||||
left=_side_summary_to_public("left", left_summary, left_line),
|
||||
right=_side_summary_to_public("right", right_summary, right_line),
|
||||
curves=DeflectionCurves(
|
||||
x_mm=x_grid,
|
||||
left_deflection_mm=left_payload["deflection"],
|
||||
right_deflection_mm=right_payload["deflection"],
|
||||
left_confidence=np.minimum(left_payload["beam_conf"], left_payload["ref_conf"]),
|
||||
right_confidence=np.minimum(right_payload["beam_conf"], right_payload["ref_conf"]),
|
||||
),
|
||||
quality_flags=quality_flags,
|
||||
report=report,
|
||||
_artifacts=artifacts,
|
||||
)
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# Helpers (private)
|
||||
# ---------------------------------------------------------------------------
|
||||
|
||||
|
||||
def _validate_points(points: np.ndarray | Sequence[Sequence[float]]) -> np.ndarray:
|
||||
array = np.asarray(points, dtype=np.float64)
|
||||
if array.ndim != 2 or array.shape[1] < 3:
|
||||
raise InvalidPointCloudError(
|
||||
f"Point cloud must be a 2D (N, 3+) array, got shape {array.shape}."
|
||||
)
|
||||
if array.shape[1] > 3:
|
||||
array = array[:, :3]
|
||||
if len(array) < 256:
|
||||
raise InsufficientPointsError(
|
||||
f"Need at least 256 points to run the pipeline, got {len(array)}."
|
||||
)
|
||||
return np.ascontiguousarray(array)
|
||||
|
||||
|
||||
def _apply_roi_mask(points_vehicle: np.ndarray, roi_payload: dict) -> np.ndarray:
|
||||
mask = (
|
||||
(points_vehicle[:, 0] >= roi_payload["x_min"])
|
||||
& (points_vehicle[:, 0] <= roi_payload["x_max"])
|
||||
& (points_vehicle[:, 2] <= roi_payload["z_ceiling"])
|
||||
)
|
||||
if roi_payload.get("source") == "external_aabb":
|
||||
mask &= (
|
||||
(points_vehicle[:, 1] >= roi_payload["y_min"])
|
||||
& (points_vehicle[:, 1] <= roi_payload["y_max"])
|
||||
& (points_vehicle[:, 2] >= roi_payload["z_min"])
|
||||
)
|
||||
return mask
|
||||
|
||||
|
||||
def _track_to_dict(track: TrackSolution | None) -> dict | None:
|
||||
if track is None:
|
||||
return None
|
||||
return {
|
||||
"side": track.side,
|
||||
"rank": track.rank,
|
||||
"score": track.score,
|
||||
"coverage_ratio": track.coverage_ratio,
|
||||
"coverage_count": track.coverage_count,
|
||||
"score_margin": track.score_margin,
|
||||
"confidence": track.confidence,
|
||||
"candidate_ids": track.candidate_ids,
|
||||
"candidates": [
|
||||
{
|
||||
"candidate_id": candidate.candidate_id,
|
||||
"slice_index": candidate.slice_index,
|
||||
"x_center": candidate.x_center,
|
||||
"y_center": candidate.y_center,
|
||||
"z_low": candidate.z_low,
|
||||
"width": candidate.width,
|
||||
"score": candidate.score,
|
||||
}
|
||||
for candidate in track.candidates
|
||||
],
|
||||
}
|
||||
|
||||
|
||||
def _fitted_line_to_dict(line: FittedLine | None) -> dict | None:
|
||||
if line is None:
|
||||
return None
|
||||
return {
|
||||
"side": line.side,
|
||||
"y_residual_rms": line.y_residual_rms,
|
||||
"z_residual_rms": line.z_residual_rms,
|
||||
"support_x_min": float(line.x_support.min()),
|
||||
"support_x_max": float(line.x_support.max()),
|
||||
"support_count": int(len(line.x_support)),
|
||||
"low_confidence_segments": [list(segment) for segment in line.low_confidence_segments],
|
||||
"gaps": [list(segment) for segment in line.gaps],
|
||||
}
|
||||
|
||||
|
||||
def _reference_to_dict(reference: ReferenceLine) -> dict:
|
||||
return {
|
||||
"side": reference.side,
|
||||
"available": reference.available,
|
||||
"confidence": reference.confidence,
|
||||
"residual": reference.residual,
|
||||
"point_front": None if reference.point_front is None else reference.point_front.tolist(),
|
||||
"point_back": None if reference.point_back is None else reference.point_back.tolist(),
|
||||
"direction": None if reference.direction is None else reference.direction.tolist(),
|
||||
"candidates_front": [
|
||||
asdict(candidate) | {"center": candidate.center.tolist()}
|
||||
for candidate in reference.candidates_front
|
||||
],
|
||||
"candidates_back": [
|
||||
asdict(candidate) | {"center": candidate.center.tolist()}
|
||||
for candidate in reference.candidates_back
|
||||
],
|
||||
"alternates": [dict(item) for item in reference.alternates],
|
||||
}
|
||||
|
||||
|
||||
def _side_symmetry_metrics(
|
||||
left_track: TrackSolution | None, right_track: TrackSolution | None
|
||||
) -> dict:
|
||||
if not left_track or not right_track or not left_track.candidates or not right_track.candidates:
|
||||
return {"available": False}
|
||||
y_mid_samples = [
|
||||
candidate.y_mid for candidate in left_track.candidates + right_track.candidates
|
||||
]
|
||||
y_mid = float(np.median(y_mid_samples))
|
||||
left_offset = float(
|
||||
np.median([abs(candidate.y_center - y_mid) for candidate in left_track.candidates])
|
||||
)
|
||||
right_offset = float(
|
||||
np.median([abs(candidate.y_center - y_mid) for candidate in right_track.candidates])
|
||||
)
|
||||
symmetry_error = abs(left_offset - right_offset)
|
||||
return {
|
||||
"available": True,
|
||||
"midline_y": y_mid,
|
||||
"left_offset": left_offset,
|
||||
"right_offset": right_offset,
|
||||
"symmetry_error": symmetry_error,
|
||||
"relative_symmetry_error": symmetry_error / max((left_offset + right_offset) * 0.5, 1.0),
|
||||
}
|
||||
|
||||
|
||||
def _evaluate_side_deflection(
|
||||
side: str,
|
||||
line: FittedLine | None,
|
||||
reference: ReferenceLine,
|
||||
x_grid: np.ndarray,
|
||||
) -> tuple[dict, dict[str, np.ndarray]]:
|
||||
if line is None:
|
||||
empty = np.full_like(x_grid, np.nan, dtype=np.float64)
|
||||
return (
|
||||
{
|
||||
"side": side,
|
||||
"available": False,
|
||||
"reason": "representative_line_unavailable",
|
||||
"selected_measurement": {
|
||||
"available": False,
|
||||
"reason": "representative_line_unavailable",
|
||||
},
|
||||
},
|
||||
{
|
||||
"beam_y": empty,
|
||||
"beam_z": empty,
|
||||
"beam_conf": np.zeros_like(x_grid),
|
||||
"ref_y": empty,
|
||||
"ref_z": empty,
|
||||
"ref_conf": np.zeros_like(x_grid),
|
||||
"deflection": empty,
|
||||
},
|
||||
)
|
||||
|
||||
beam_y, beam_z, beam_conf = evaluate_line(line, x_grid)
|
||||
ref_eval, ref_conf = evaluate_reference_line(reference, x_grid)
|
||||
if np.ndim(ref_eval) != 2:
|
||||
ref_y = np.full_like(x_grid, np.nan, dtype=np.float64)
|
||||
ref_z = np.full_like(x_grid, np.nan, dtype=np.float64)
|
||||
else:
|
||||
ref_y = ref_eval[:, 0]
|
||||
ref_z = ref_eval[:, 1]
|
||||
|
||||
deflection = beam_z - ref_z
|
||||
invalid = np.isnan(ref_z) | (beam_conf <= 0.0) | (ref_conf <= 0.0)
|
||||
deflection[invalid] = np.nan
|
||||
valid = ~np.isnan(deflection)
|
||||
summary = {
|
||||
"side": side,
|
||||
"available": bool(np.any(valid)),
|
||||
"reference_available": reference.available,
|
||||
"beam_fit_residual_rms_y": line.y_residual_rms,
|
||||
"beam_fit_residual_rms_z": line.z_residual_rms,
|
||||
"reference_confidence": reference.confidence,
|
||||
"reference_residual": reference.residual,
|
||||
}
|
||||
if np.any(valid):
|
||||
valid_x = x_grid[valid]
|
||||
valid_deflection = deflection[valid]
|
||||
valid_indices = np.flatnonzero(valid)
|
||||
down_index = int(np.argmin(valid_deflection))
|
||||
up_index = int(np.argmax(valid_deflection))
|
||||
abs_index = int(np.argmax(np.abs(valid_deflection)))
|
||||
selected_x = float(valid_x[abs_index])
|
||||
global_index = int(valid_indices[abs_index])
|
||||
beam_point = np.array(
|
||||
[selected_x, float(beam_y[global_index]), float(beam_z[global_index])],
|
||||
dtype=np.float64,
|
||||
)
|
||||
reference_point = np.array(
|
||||
[selected_x, float(ref_y[global_index]), float(ref_z[global_index])],
|
||||
dtype=np.float64,
|
||||
)
|
||||
vertical_anchor = np.array(
|
||||
[selected_x, float(beam_y[global_index]), float(ref_z[global_index])],
|
||||
dtype=np.float64,
|
||||
)
|
||||
selected_value = float(valid_deflection[abs_index])
|
||||
summary.update(
|
||||
{
|
||||
"max_downward_deflection_mm": float(valid_deflection[down_index]),
|
||||
"max_downward_x_mm": float(valid_x[down_index]),
|
||||
"max_upward_deflection_mm": float(valid_deflection[up_index]),
|
||||
"max_upward_x_mm": float(valid_x[up_index]),
|
||||
"selected_measurement": {
|
||||
"available": True,
|
||||
"selection_rule": "maximum_absolute_vertical_deflection",
|
||||
"selected_type": "downward" if selected_value < 0.0 else "upward",
|
||||
"x_mm": selected_x,
|
||||
"signed_deflection_mm": selected_value,
|
||||
"absolute_deflection_mm": float(abs(selected_value)),
|
||||
"beam_point": beam_point.tolist(),
|
||||
"reference_point": reference_point.tolist(),
|
||||
"vertical_anchor_point": vertical_anchor.tolist(),
|
||||
"vertical_distance_mm": float(abs(selected_value)),
|
||||
},
|
||||
}
|
||||
)
|
||||
else:
|
||||
summary["reason"] = "reference_unavailable_or_no_valid_overlap"
|
||||
summary["selected_measurement"] = {
|
||||
"available": False,
|
||||
"reason": "reference_unavailable_or_no_valid_overlap",
|
||||
}
|
||||
return (
|
||||
summary,
|
||||
{
|
||||
"beam_y": beam_y,
|
||||
"beam_z": beam_z,
|
||||
"beam_conf": beam_conf,
|
||||
"ref_y": ref_y,
|
||||
"ref_z": ref_z,
|
||||
"ref_conf": ref_conf,
|
||||
"deflection": deflection,
|
||||
},
|
||||
)
|
||||
|
||||
|
||||
def _build_deflection_rows(
|
||||
x_grid: np.ndarray,
|
||||
left_payload: dict[str, np.ndarray],
|
||||
right_payload: dict[str, np.ndarray],
|
||||
) -> list[dict]:
|
||||
rows: list[dict] = []
|
||||
for index, x_value in enumerate(x_grid):
|
||||
rows.append(
|
||||
{
|
||||
"x": float(x_value),
|
||||
"left_beam_y": _nan_safe(left_payload["beam_y"][index]),
|
||||
"left_beam_z": _nan_safe(left_payload["beam_z"][index]),
|
||||
"left_ref_y": _nan_safe(left_payload["ref_y"][index]),
|
||||
"left_ref_z": _nan_safe(left_payload["ref_z"][index]),
|
||||
"left_deflection": _nan_safe(left_payload["deflection"][index]),
|
||||
"left_confidence": float(
|
||||
min(left_payload["beam_conf"][index], left_payload["ref_conf"][index])
|
||||
),
|
||||
"right_beam_y": _nan_safe(right_payload["beam_y"][index]),
|
||||
"right_beam_z": _nan_safe(right_payload["beam_z"][index]),
|
||||
"right_ref_y": _nan_safe(right_payload["ref_y"][index]),
|
||||
"right_ref_z": _nan_safe(right_payload["ref_z"][index]),
|
||||
"right_deflection": _nan_safe(right_payload["deflection"][index]),
|
||||
"right_confidence": float(
|
||||
min(right_payload["beam_conf"][index], right_payload["ref_conf"][index])
|
||||
),
|
||||
}
|
||||
)
|
||||
return rows
|
||||
|
||||
|
||||
def _nan_safe(value: float) -> float:
|
||||
return float(value) if not np.isnan(value) else float("nan")
|
||||
|
||||
|
||||
def _collect_quality_flags(
|
||||
best_left: TrackSolution | None,
|
||||
best_right: TrackSolution | None,
|
||||
left_line: FittedLine | None,
|
||||
right_line: FittedLine | None,
|
||||
left_reference: ReferenceLine,
|
||||
right_reference: ReferenceLine,
|
||||
symmetry: dict,
|
||||
resolved_config: dict,
|
||||
) -> list[str]:
|
||||
flags: list[str] = []
|
||||
low_conf = resolved_config["reference"]["low_confidence_threshold"]
|
||||
if best_left is None:
|
||||
flags.append("left_track_missing")
|
||||
if best_right is None:
|
||||
flags.append("right_track_missing")
|
||||
if left_line is None:
|
||||
flags.append("left_representative_line_missing")
|
||||
if right_line is None:
|
||||
flags.append("right_representative_line_missing")
|
||||
if not left_reference.available:
|
||||
flags.append("left_reference_unavailable")
|
||||
if not right_reference.available:
|
||||
flags.append("right_reference_unavailable")
|
||||
if left_reference.available and left_reference.confidence < low_conf:
|
||||
flags.append("left_reference_low_confidence")
|
||||
if right_reference.available and right_reference.confidence < low_conf:
|
||||
flags.append("right_reference_low_confidence")
|
||||
if symmetry.get("available") and symmetry["relative_symmetry_error"] > 0.18:
|
||||
flags.append("left_right_symmetry_warning")
|
||||
if best_left and best_left.coverage_ratio < 0.45:
|
||||
flags.append("left_track_low_coverage")
|
||||
if best_right and best_right.coverage_ratio < 0.45:
|
||||
flags.append("right_track_low_coverage")
|
||||
return flags
|
||||
|
||||
|
||||
def _side_summary_to_public(
|
||||
side: str, summary: dict, line: FittedLine | None
|
||||
) -> SideDeflection:
|
||||
selected = summary.get("selected_measurement", {}) or {}
|
||||
return SideDeflection(
|
||||
side=side,
|
||||
available=bool(summary.get("available", False)),
|
||||
reference_available=bool(summary.get("reference_available", False)),
|
||||
reference_confidence=float(summary.get("reference_confidence", 0.0) or 0.0),
|
||||
max_downward_mm=summary.get("max_downward_deflection_mm"),
|
||||
max_downward_x_mm=summary.get("max_downward_x_mm"),
|
||||
max_upward_mm=summary.get("max_upward_deflection_mm"),
|
||||
max_upward_x_mm=summary.get("max_upward_x_mm"),
|
||||
selected_x_mm=selected.get("x_mm"),
|
||||
selected_signed_mm=selected.get("signed_deflection_mm"),
|
||||
selected_absolute_mm=selected.get("absolute_deflection_mm"),
|
||||
beam_fit_residual_rms_z_mm=line.z_residual_rms if line is not None else None,
|
||||
)
|
||||
|
||||
|
||||
__all__ = [
|
||||
"DeflectionResult",
|
||||
"SideDeflection",
|
||||
"DeflectionCurves",
|
||||
"ProgressCallback",
|
||||
"compute_deflection",
|
||||
"load_point_cloud",
|
||||
]
|
||||
270
alg/delivery_v1.2.1/underframe_deflection/artifacts.py
Normal file
270
alg/delivery_v1.2.1/underframe_deflection/artifacts.py
Normal file
@@ -0,0 +1,270 @@
|
||||
"""Optional output writers for a :class:`DeflectionResult`.
|
||||
|
||||
The functions in this module are entirely optional — calling
|
||||
:func:`underframe_deflection.compute_deflection` does not write anything
|
||||
to disk. Pull in only the writers you need. Plot rendering requires
|
||||
matplotlib (install with the ``viz`` extra: ``pip install
|
||||
underframe-deflection[viz]``); JSON/CSV writers have no extra deps.
|
||||
|
||||
All writers expect the ``DeflectionResult`` to come from the same package
|
||||
version that produced it; do not pass in a result whose ``_artifacts``
|
||||
field has been tampered with.
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
from pathlib import Path
|
||||
from typing import TYPE_CHECKING
|
||||
|
||||
from .api import DeflectionResult
|
||||
from .beams import candidate_rows
|
||||
from .io import ensure_output_dir, write_csv, write_json
|
||||
from .ply_overlay import export_measurement_overlay_plys
|
||||
from .structures import TrackSolution
|
||||
|
||||
if TYPE_CHECKING:
|
||||
pass
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# JSON / CSV writers (stdlib only)
|
||||
# ---------------------------------------------------------------------------
|
||||
|
||||
|
||||
def write_report_json(result: DeflectionResult, path: str | Path) -> Path:
|
||||
"""Write the full report to ``path`` as JSON.
|
||||
|
||||
The structure matches ``DeflectionResult.to_dict()``. Schema is
|
||||
versioned by ``result.schema_version``.
|
||||
"""
|
||||
output_path = Path(path)
|
||||
ensure_output_dir(output_path.parent)
|
||||
write_json(output_path, result.to_dict())
|
||||
return output_path
|
||||
|
||||
|
||||
def write_curves_csv(result: DeflectionResult, path: str | Path) -> Path:
|
||||
"""Write the sampled deflection curves to a CSV file.
|
||||
|
||||
Columns: ``x``, plus per-side beam/reference Y/Z, deflection, and
|
||||
confidence. NaN values indicate gaps where the curve is undefined.
|
||||
"""
|
||||
output_path = Path(path)
|
||||
ensure_output_dir(output_path.parent)
|
||||
artifacts = _require_artifacts(result)
|
||||
rows = artifacts.deflection_rows
|
||||
fieldnames = (
|
||||
list(rows[0].keys())
|
||||
if rows
|
||||
else [
|
||||
"x",
|
||||
"left_beam_y",
|
||||
"left_beam_z",
|
||||
"left_ref_y",
|
||||
"left_ref_z",
|
||||
"left_deflection",
|
||||
"left_confidence",
|
||||
"right_beam_y",
|
||||
"right_beam_z",
|
||||
"right_ref_y",
|
||||
"right_ref_z",
|
||||
"right_deflection",
|
||||
"right_confidence",
|
||||
]
|
||||
)
|
||||
write_csv(output_path, fieldnames=fieldnames, rows=rows)
|
||||
return output_path
|
||||
|
||||
|
||||
def write_beam_tracks_csv(result: DeflectionResult, path: str | Path) -> Path:
|
||||
"""Write the per-slice beam candidate table to a CSV file.
|
||||
|
||||
Useful when debugging tracking behaviour or visualising candidate
|
||||
bands in external tools.
|
||||
"""
|
||||
output_path = Path(path)
|
||||
ensure_output_dir(output_path.parent)
|
||||
artifacts = _require_artifacts(result)
|
||||
rows = _annotate_beam_track_rows(
|
||||
candidate_rows(artifacts.slices),
|
||||
artifacts.left_tracks,
|
||||
artifacts.right_tracks,
|
||||
)
|
||||
fieldnames = (
|
||||
list(rows[0].keys())
|
||||
if rows
|
||||
else [
|
||||
"slice_index",
|
||||
"candidate_id",
|
||||
"side",
|
||||
"x_start",
|
||||
"x_end",
|
||||
"x_center",
|
||||
"y_min",
|
||||
"y_max",
|
||||
"y_center",
|
||||
"z_low",
|
||||
"width",
|
||||
"point_count",
|
||||
"low_point_count",
|
||||
"flatness",
|
||||
"normal_alignment",
|
||||
"score",
|
||||
"y_mid",
|
||||
"selected_rank",
|
||||
"selected_side",
|
||||
]
|
||||
)
|
||||
write_csv(output_path, fieldnames=fieldnames, rows=rows)
|
||||
return output_path
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# Plots (matplotlib — optional dependency)
|
||||
# ---------------------------------------------------------------------------
|
||||
|
||||
|
||||
def render_plots(
|
||||
result: DeflectionResult,
|
||||
out_dir: str | Path,
|
||||
*,
|
||||
dpi: int | None = None,
|
||||
max_scatter_points: int | None = None,
|
||||
) -> dict[str, Path]:
|
||||
"""Render diagnostic PNG plots into ``out_dir``.
|
||||
|
||||
Returns a dict mapping plot name to the written file path. Requires
|
||||
matplotlib; the import is deferred so this module can be imported
|
||||
without it as long as ``render_plots`` is not called.
|
||||
"""
|
||||
out_dir_path = ensure_output_dir(out_dir)
|
||||
artifacts = _require_artifacts(result)
|
||||
cfg = artifacts.resolved_config["visualization"]
|
||||
resolved_dpi = dpi if dpi is not None else cfg["dpi"]
|
||||
resolved_limit = (
|
||||
max_scatter_points if max_scatter_points is not None else cfg["max_scatter_points"]
|
||||
)
|
||||
|
||||
from .visualization import (
|
||||
plot_beam_candidates,
|
||||
plot_deflection_curves,
|
||||
plot_frame_hypotheses,
|
||||
plot_representatives_and_lines,
|
||||
plot_roi_overview,
|
||||
)
|
||||
|
||||
paths: dict[str, Path] = {}
|
||||
paths["frame_hypotheses"] = out_dir_path / "frame_hypotheses.png"
|
||||
plot_frame_hypotheses(artifacts.frame_hypotheses, paths["frame_hypotheses"], dpi=resolved_dpi)
|
||||
|
||||
paths["bottom_roi"] = out_dir_path / "bottom_roi.png"
|
||||
plot_roi_overview(
|
||||
artifacts.work_vehicle,
|
||||
artifacts.work_roi,
|
||||
paths["bottom_roi"],
|
||||
dpi=resolved_dpi,
|
||||
limit=resolved_limit,
|
||||
)
|
||||
|
||||
paths["beam_tracks"] = out_dir_path / "beam_tracks.png"
|
||||
plot_beam_candidates(
|
||||
artifacts.slices,
|
||||
artifacts.best_left,
|
||||
artifacts.best_right,
|
||||
paths["beam_tracks"],
|
||||
dpi=resolved_dpi,
|
||||
)
|
||||
|
||||
paths["representative_lines"] = out_dir_path / "representative_lines.png"
|
||||
plot_representatives_and_lines(
|
||||
artifacts.left_reps,
|
||||
artifacts.right_reps,
|
||||
artifacts.left_line,
|
||||
artifacts.right_line,
|
||||
artifacts.left_reference,
|
||||
artifacts.right_reference,
|
||||
paths["representative_lines"],
|
||||
dpi=resolved_dpi,
|
||||
)
|
||||
|
||||
paths["deflection_curves"] = out_dir_path / "deflection_curves.png"
|
||||
plot_deflection_curves(artifacts.deflection_rows, paths["deflection_curves"], dpi=resolved_dpi)
|
||||
|
||||
return paths
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# PLY overlays
|
||||
# ---------------------------------------------------------------------------
|
||||
|
||||
|
||||
def export_overlay_plys(result: DeflectionResult, out_dir: str | Path) -> dict[str, str]:
|
||||
"""Export coloured PLY overlays of the ROI, fitted lines, and measurements.
|
||||
|
||||
Produces ``roi_cropped_cloud.ply`` and three measurement overlays
|
||||
(full / left-only / right-only). Returns a mapping of name to path.
|
||||
"""
|
||||
out_dir_path = ensure_output_dir(out_dir)
|
||||
artifacts = _require_artifacts(result)
|
||||
return export_measurement_overlay_plys(
|
||||
output_dir=out_dir_path,
|
||||
roi_points=artifacts.filtered_roi,
|
||||
left_track=artifacts.best_left,
|
||||
right_track=artifacts.best_right,
|
||||
left_line=artifacts.left_line,
|
||||
right_line=artifacts.right_line,
|
||||
left_reference=artifacts.left_reference,
|
||||
right_reference=artifacts.right_reference,
|
||||
left_measurement=artifacts.left_summary.get("selected_measurement"),
|
||||
right_measurement=artifacts.right_summary.get("selected_measurement"),
|
||||
config=artifacts.resolved_config,
|
||||
)
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# Helpers
|
||||
# ---------------------------------------------------------------------------
|
||||
|
||||
|
||||
def _require_artifacts(result: DeflectionResult):
|
||||
if result._artifacts is None:
|
||||
raise ValueError(
|
||||
"DeflectionResult has no internal artifacts; it was likely "
|
||||
"reconstructed from a serialized report. Re-run compute_deflection "
|
||||
"to obtain a writable result."
|
||||
)
|
||||
return result._artifacts
|
||||
|
||||
|
||||
def _annotate_beam_track_rows(
|
||||
rows: list[dict],
|
||||
left_tracks: list[TrackSolution],
|
||||
right_tracks: list[TrackSolution],
|
||||
) -> list[dict]:
|
||||
rank_map: dict[str, tuple[str, int]] = {}
|
||||
for track in left_tracks + right_tracks:
|
||||
for candidate in track.candidates:
|
||||
key = f"{track.side}:{candidate.candidate_id}"
|
||||
if key not in rank_map:
|
||||
rank_map[key] = (track.side, track.rank)
|
||||
annotated = []
|
||||
for row in rows:
|
||||
key = f"{row['side']}:{row['candidate_id']}"
|
||||
annotation = rank_map.get(key)
|
||||
annotated.append(
|
||||
row
|
||||
| {
|
||||
"selected_rank": annotation[1] if annotation else "",
|
||||
"selected_side": annotation[0] if annotation else "",
|
||||
}
|
||||
)
|
||||
return annotated
|
||||
|
||||
|
||||
__all__ = [
|
||||
"write_report_json",
|
||||
"write_curves_csv",
|
||||
"write_beam_tracks_csv",
|
||||
"render_plots",
|
||||
"export_overlay_plys",
|
||||
]
|
||||
563
alg/delivery_v1.2.1/underframe_deflection/beams.py
Normal file
563
alg/delivery_v1.2.1/underframe_deflection/beams.py
Normal file
@@ -0,0 +1,563 @@
|
||||
from __future__ import annotations
|
||||
|
||||
from dataclasses import dataclass
|
||||
from math import acos
|
||||
|
||||
import numpy as np
|
||||
from scipy.ndimage import gaussian_filter1d
|
||||
|
||||
from .structures import CandidateBand, SliceResult, TrackSolution
|
||||
|
||||
|
||||
def _contiguous_ranges(mask: np.ndarray) -> list[tuple[int, int]]:
|
||||
ranges: list[tuple[int, int]] = []
|
||||
start = None
|
||||
for idx, value in enumerate(mask):
|
||||
if value and start is None:
|
||||
start = idx
|
||||
elif not value and start is not None:
|
||||
ranges.append((start, idx - 1))
|
||||
start = None
|
||||
if start is not None:
|
||||
ranges.append((start, len(mask) - 1))
|
||||
return ranges
|
||||
|
||||
|
||||
def _candidate_score(
|
||||
z_low: float,
|
||||
slice_bottom_ref: float,
|
||||
z_scale: float,
|
||||
flatness: float,
|
||||
flat_thresh: float,
|
||||
normal_alignment: float,
|
||||
point_count: int,
|
||||
width: float,
|
||||
preferred_width: float,
|
||||
edge_ratio: float,
|
||||
edge_preference_weight: float,
|
||||
) -> float:
|
||||
low_term = np.clip((slice_bottom_ref - z_low) / max(z_scale, 1.0) + 1.0, 0.0, 3.0)
|
||||
flat_term = np.clip(1.0 - flatness / max(flat_thresh, 1e-6), 0.0, 1.5)
|
||||
width_term = np.clip(
|
||||
1.0 - abs(width - preferred_width) / max(preferred_width, 1.0),
|
||||
-0.5,
|
||||
1.0,
|
||||
)
|
||||
density_term = np.clip(np.log1p(point_count) / 6.0, 0.0, 1.2)
|
||||
edge_term = np.clip(edge_ratio, 0.0, 1.0)
|
||||
return float(
|
||||
1.2 * low_term
|
||||
+ 0.9 * flat_term
|
||||
+ 0.8 * normal_alignment
|
||||
+ 0.5 * width_term
|
||||
+ 0.4 * density_term
|
||||
+ edge_preference_weight * edge_term
|
||||
)
|
||||
|
||||
|
||||
def extract_beam_slices(
|
||||
points_vehicle: np.ndarray,
|
||||
normals_vehicle: np.ndarray,
|
||||
config: dict,
|
||||
) -> list[SliceResult]:
|
||||
beam_cfg = config["beam"]
|
||||
x_min = float(points_vehicle[:, 0].min())
|
||||
x_max = float(points_vehicle[:, 0].max())
|
||||
slice_edges = np.arange(x_min, x_max + beam_cfg["slice_step_mm"], beam_cfg["slice_step_mm"])
|
||||
if slice_edges[-1] < x_max:
|
||||
slice_edges = np.append(slice_edges, x_max)
|
||||
y_global_lo, y_global_hi = np.quantile(
|
||||
points_vehicle[:, 1],
|
||||
[beam_cfg["midline_trim_quantile"], 1.0 - beam_cfg["midline_trim_quantile"]],
|
||||
)
|
||||
flat_thresh = beam_cfg["flatness_mm"]
|
||||
results: list[SliceResult] = []
|
||||
|
||||
for slice_index, x_start in enumerate(slice_edges[:-1]):
|
||||
x_center = x_start + beam_cfg["slice_thickness_mm"] * 0.5
|
||||
x_end = x_start + beam_cfg["slice_thickness_mm"]
|
||||
mask = (points_vehicle[:, 0] >= x_start) & (points_vehicle[:, 0] < x_end)
|
||||
slice_points = points_vehicle[mask]
|
||||
if len(slice_points) < beam_cfg["slice_min_points"]:
|
||||
results.append(
|
||||
SliceResult(
|
||||
slice_index=slice_index,
|
||||
x_start=float(x_start),
|
||||
x_end=float(x_end),
|
||||
x_center=float(x_center),
|
||||
y_mid=float(0.5 * (y_global_lo + y_global_hi)),
|
||||
point_count=int(len(slice_points)),
|
||||
)
|
||||
)
|
||||
continue
|
||||
|
||||
slice_normals = normals_vehicle[mask]
|
||||
y_lo, y_hi = np.quantile(
|
||||
slice_points[:, 1],
|
||||
[beam_cfg["midline_trim_quantile"], 1.0 - beam_cfg["midline_trim_quantile"]],
|
||||
)
|
||||
y_mid = float(0.5 * (y_lo + y_hi))
|
||||
y_edges = np.arange(y_lo, y_hi + beam_cfg["y_bin_width_mm"], beam_cfg["y_bin_width_mm"])
|
||||
if len(y_edges) < 3:
|
||||
y_edges = np.linspace(y_lo, y_hi, 4)
|
||||
bin_count = len(y_edges) - 1
|
||||
low_z = np.full(bin_count, np.nan)
|
||||
flatness = np.full(bin_count, np.nan)
|
||||
normal_alignment = np.full(bin_count, np.nan)
|
||||
counts = np.zeros(bin_count, dtype=np.int32)
|
||||
low_counts = np.zeros(bin_count, dtype=np.int32)
|
||||
|
||||
for bin_index in range(bin_count):
|
||||
bin_mask = (slice_points[:, 1] >= y_edges[bin_index]) & (
|
||||
slice_points[:, 1] < y_edges[bin_index + 1]
|
||||
)
|
||||
counts[bin_index] = int(np.count_nonzero(bin_mask))
|
||||
if counts[bin_index] < beam_cfg["min_bin_points"]:
|
||||
continue
|
||||
bin_points = slice_points[bin_mask]
|
||||
z_threshold = np.quantile(bin_points[:, 2], beam_cfg["low_quantile"])
|
||||
low_mask = bin_points[:, 2] <= z_threshold
|
||||
low_points = bin_points[low_mask]
|
||||
low_counts[bin_index] = int(np.count_nonzero(low_mask))
|
||||
if len(low_points) == 0:
|
||||
continue
|
||||
low_z[bin_index] = float(np.median(low_points[:, 2]))
|
||||
flatness[bin_index] = float(
|
||||
np.median(np.abs(low_points[:, 2] - low_z[bin_index]))
|
||||
)
|
||||
normal_alignment[bin_index] = float(
|
||||
np.mean(np.abs(slice_normals[bin_mask][low_mask, 2]))
|
||||
)
|
||||
|
||||
valid = ~np.isnan(low_z)
|
||||
if valid.sum() < 5:
|
||||
results.append(
|
||||
SliceResult(
|
||||
slice_index=slice_index,
|
||||
x_start=float(x_start),
|
||||
x_end=float(x_end),
|
||||
x_center=float(x_center),
|
||||
y_mid=y_mid,
|
||||
point_count=int(len(slice_points)),
|
||||
)
|
||||
)
|
||||
continue
|
||||
|
||||
smoothed_low_z = low_z.copy()
|
||||
smoothed_low_z[valid] = gaussian_filter1d(low_z[valid], sigma=1.0)
|
||||
z_cut = float(
|
||||
np.quantile(smoothed_low_z[valid], 0.25) + beam_cfg["low_band_margin_mm"]
|
||||
)
|
||||
qualified_mask = (
|
||||
valid
|
||||
& (smoothed_low_z <= z_cut)
|
||||
& (flatness <= flat_thresh)
|
||||
& (normal_alignment >= beam_cfg["normal_alignment_min"])
|
||||
& (counts >= beam_cfg["min_bin_points"])
|
||||
)
|
||||
slice_result = SliceResult(
|
||||
slice_index=slice_index,
|
||||
x_start=float(x_start),
|
||||
x_end=float(x_end),
|
||||
x_center=float(x_center),
|
||||
y_mid=y_mid,
|
||||
point_count=int(len(slice_points)),
|
||||
)
|
||||
z_scale = float(np.std(slice_points[:, 2]) + 1e-6)
|
||||
slice_bottom_ref = float(np.quantile(slice_points[:, 2], 0.15))
|
||||
candidate_counter = 0
|
||||
|
||||
def append_candidate(start: int, end: int, side: str) -> None:
|
||||
nonlocal candidate_counter
|
||||
y_min = float(y_edges[start])
|
||||
y_max = float(y_edges[end + 1])
|
||||
width = float(y_max - y_min)
|
||||
if width < beam_cfg["width_min_mm"] or width > beam_cfg["width_max_mm"]:
|
||||
return
|
||||
weighted_counts = counts[start : end + 1].astype(np.float64)
|
||||
weighted_y = 0.5 * (y_edges[start : end + 1] + y_edges[start + 1 : end + 2])
|
||||
y_center = float(np.average(weighted_y, weights=weighted_counts))
|
||||
half_span = max(0.5 * (y_hi - y_lo), 1.0)
|
||||
edge_ratio = float(np.clip(abs(y_center - y_mid) / half_span, 0.0, 1.0))
|
||||
z_low_group = float(np.nanmedian(smoothed_low_z[start : end + 1]))
|
||||
flatness_group = float(np.nanmean(flatness[start : end + 1]))
|
||||
normal_group = float(np.nanmean(normal_alignment[start : end + 1]))
|
||||
point_count = int(np.sum(counts[start : end + 1]))
|
||||
low_point_count = int(np.sum(low_counts[start : end + 1]))
|
||||
score = _candidate_score(
|
||||
z_low=z_low_group,
|
||||
slice_bottom_ref=slice_bottom_ref,
|
||||
z_scale=z_scale,
|
||||
flatness=flatness_group,
|
||||
flat_thresh=flat_thresh,
|
||||
normal_alignment=normal_group,
|
||||
point_count=point_count,
|
||||
width=width,
|
||||
preferred_width=beam_cfg["width_preferred_mm"],
|
||||
edge_ratio=edge_ratio,
|
||||
edge_preference_weight=beam_cfg["edge_preference_weight"],
|
||||
)
|
||||
candidate = CandidateBand(
|
||||
candidate_id=f"s{slice_index:03d}_c{candidate_counter:02d}",
|
||||
slice_index=slice_index,
|
||||
x_start=float(x_start),
|
||||
x_end=float(x_end),
|
||||
x_center=float(x_center),
|
||||
y_min=y_min,
|
||||
y_max=y_max,
|
||||
y_center=y_center,
|
||||
z_low=z_low_group,
|
||||
width=width,
|
||||
point_count=point_count,
|
||||
low_point_count=low_point_count,
|
||||
flatness=flatness_group,
|
||||
normal_alignment=normal_group,
|
||||
score=score,
|
||||
edge_ratio=edge_ratio,
|
||||
side=side,
|
||||
y_mid=y_mid,
|
||||
bin_start=start,
|
||||
bin_end=end,
|
||||
)
|
||||
candidate_counter += 1
|
||||
slice_result.candidate_bands.append(candidate)
|
||||
if side == "left":
|
||||
slice_result.left_candidates.append(candidate)
|
||||
else:
|
||||
slice_result.right_candidates.append(candidate)
|
||||
|
||||
def side_candidates(side_mask: np.ndarray, side: str) -> None:
|
||||
indices = np.where(qualified_mask & side_mask)[0]
|
||||
if len(indices) == 0:
|
||||
return
|
||||
index_set = set(indices.tolist())
|
||||
minima: list[int] = []
|
||||
for idx in indices:
|
||||
left_neighbor = idx - 1 if (idx - 1) in index_set else None
|
||||
right_neighbor = idx + 1 if (idx + 1) in index_set else None
|
||||
left_ok = left_neighbor is None or smoothed_low_z[idx] <= smoothed_low_z[left_neighbor]
|
||||
right_ok = right_neighbor is None or smoothed_low_z[idx] <= smoothed_low_z[right_neighbor]
|
||||
if left_ok and right_ok:
|
||||
minima.append(idx)
|
||||
if not minima:
|
||||
minima = [int(indices[np.argmin(smoothed_low_z[indices])])]
|
||||
minima = sorted(minima, key=lambda idx: smoothed_low_z[idx])
|
||||
used_bins: set[int] = set()
|
||||
rise_limit_extra = max(beam_cfg["flatness_mm"] * 2.0, beam_cfg["low_band_margin_mm"] * 0.6)
|
||||
for min_idx in minima[:3]:
|
||||
if min_idx in used_bins:
|
||||
continue
|
||||
start = end = min_idx
|
||||
rise_limit = smoothed_low_z[min_idx] + rise_limit_extra
|
||||
while (
|
||||
(start - 1) in index_set
|
||||
and smoothed_low_z[start - 1] <= rise_limit
|
||||
and (y_edges[end + 1] - y_edges[start - 1]) <= beam_cfg["width_max_mm"]
|
||||
):
|
||||
start -= 1
|
||||
while (
|
||||
(end + 1) in index_set
|
||||
and smoothed_low_z[end + 1] <= rise_limit
|
||||
and (y_edges[end + 2] - y_edges[start]) <= beam_cfg["width_max_mm"]
|
||||
):
|
||||
end += 1
|
||||
append_candidate(start, end, side)
|
||||
used_bins.update(range(start, end + 1))
|
||||
|
||||
# The beam usually sits on the outer bottom edge, so seed one
|
||||
# boundary-driven candidate even if that edge is not a local minimum.
|
||||
outer_idx = int(indices[0] if side == "left" else indices[-1])
|
||||
if outer_idx not in used_bins:
|
||||
start = end = outer_idx
|
||||
rise_limit = smoothed_low_z[outer_idx] + rise_limit_extra
|
||||
if side == "left":
|
||||
while (
|
||||
(end + 1) in index_set
|
||||
and smoothed_low_z[end + 1] <= rise_limit
|
||||
and (y_edges[end + 2] - y_edges[start]) <= beam_cfg["width_max_mm"]
|
||||
):
|
||||
end += 1
|
||||
else:
|
||||
while (
|
||||
(start - 1) in index_set
|
||||
and smoothed_low_z[start - 1] <= rise_limit
|
||||
and (y_edges[end + 1] - y_edges[start - 1]) <= beam_cfg["width_max_mm"]
|
||||
):
|
||||
start -= 1
|
||||
append_candidate(start, end, side)
|
||||
used_bins.update(range(start, end + 1))
|
||||
|
||||
side_candidates(np.asarray([0.5 * (y_edges[i] + y_edges[i + 1]) < y_mid for i in range(bin_count)]), "left")
|
||||
side_candidates(np.asarray([0.5 * (y_edges[i] + y_edges[i + 1]) >= y_mid for i in range(bin_count)]), "right")
|
||||
for attr_name in ("left_candidates", "right_candidates"):
|
||||
side_list = getattr(slice_result, attr_name)
|
||||
if not side_list:
|
||||
continue
|
||||
strongest_edge = max(candidate.edge_ratio for candidate in side_list)
|
||||
edge_floor = max(
|
||||
beam_cfg["inner_reject_ratio"],
|
||||
beam_cfg["edge_keep_min_ratio"],
|
||||
)
|
||||
if strongest_edge >= beam_cfg["strong_edge_ratio"]:
|
||||
edge_floor = max(edge_floor, strongest_edge - beam_cfg["relative_edge_window"])
|
||||
filtered = [
|
||||
candidate
|
||||
for candidate in side_list
|
||||
if candidate.edge_ratio >= edge_floor
|
||||
]
|
||||
if filtered or beam_cfg.get("drop_weak_edge_slices", False):
|
||||
setattr(slice_result, attr_name, filtered)
|
||||
slice_result.candidate_bands = slice_result.left_candidates + slice_result.right_candidates
|
||||
|
||||
results.append(slice_result)
|
||||
return results
|
||||
|
||||
|
||||
@dataclass(slots=True)
|
||||
class _PathState:
|
||||
score: float
|
||||
candidate_ids: tuple[str | None, ...]
|
||||
selected: tuple[CandidateBand, ...]
|
||||
coverage_count: int
|
||||
last_y: float | None
|
||||
last_z: float | None
|
||||
last_width: float | None
|
||||
last_normal: np.ndarray | None
|
||||
prev_dy: float | None
|
||||
prev_dz: float | None
|
||||
gap_run: int
|
||||
|
||||
|
||||
def _normal_to_vector(candidate: CandidateBand) -> np.ndarray:
|
||||
lateral = 1.0 if candidate.side == "right" else -1.0
|
||||
return np.array([0.0, lateral * np.sqrt(max(1.0 - candidate.normal_alignment**2, 0.0)), candidate.normal_alignment])
|
||||
|
||||
|
||||
def _transition_penalty(
|
||||
prev_state: _PathState,
|
||||
candidate: CandidateBand,
|
||||
config: dict,
|
||||
) -> float:
|
||||
weights = config["tracking"]["weights"]
|
||||
beam_cfg = config["beam"]
|
||||
penalty = 0.0
|
||||
if prev_state.last_y is not None:
|
||||
penalty += weights["y_penalty"] * abs(candidate.y_center - prev_state.last_y) / max(
|
||||
beam_cfg["y_bin_width_mm"],
|
||||
1.0,
|
||||
)
|
||||
if prev_state.last_z is not None:
|
||||
penalty += weights["z_penalty"] * abs(candidate.z_low - prev_state.last_z) / max(
|
||||
beam_cfg["flatness_mm"],
|
||||
1.0,
|
||||
)
|
||||
if prev_state.last_width is not None:
|
||||
penalty += weights["width_penalty"] * abs(candidate.width - prev_state.last_width) / max(
|
||||
beam_cfg["width_preferred_mm"],
|
||||
1.0,
|
||||
)
|
||||
current_normal = _normal_to_vector(candidate)
|
||||
if prev_state.last_normal is not None:
|
||||
cosine = float(
|
||||
np.clip(
|
||||
np.dot(prev_state.last_normal, current_normal)
|
||||
/ max(np.linalg.norm(prev_state.last_normal) * np.linalg.norm(current_normal), 1e-6),
|
||||
-1.0,
|
||||
1.0,
|
||||
)
|
||||
)
|
||||
penalty += weights["normal_penalty"] * acos(cosine)
|
||||
if prev_state.prev_dy is not None and prev_state.last_y is not None:
|
||||
dy = candidate.y_center - prev_state.last_y
|
||||
penalty += weights["jitter_penalty"] * abs(dy - prev_state.prev_dy) / max(
|
||||
beam_cfg["y_bin_width_mm"],
|
||||
1.0,
|
||||
)
|
||||
if prev_state.prev_dz is not None and prev_state.last_z is not None:
|
||||
dz = candidate.z_low - prev_state.last_z
|
||||
penalty += weights["jitter_penalty"] * abs(dz - prev_state.prev_dz) / max(
|
||||
beam_cfg["flatness_mm"],
|
||||
1.0,
|
||||
)
|
||||
return penalty
|
||||
|
||||
|
||||
def _insert_topk(states: list[_PathState], state: _PathState, k: int) -> list[_PathState]:
|
||||
states.append(state)
|
||||
states.sort(key=lambda item: item.score, reverse=True)
|
||||
deduped: list[_PathState] = []
|
||||
seen = set()
|
||||
for candidate in states:
|
||||
key = candidate.candidate_ids
|
||||
if key in seen:
|
||||
continue
|
||||
deduped.append(candidate)
|
||||
seen.add(key)
|
||||
if len(deduped) >= k:
|
||||
break
|
||||
return deduped
|
||||
|
||||
|
||||
def track_side_candidates(
|
||||
slices: list[SliceResult],
|
||||
side: str,
|
||||
config: dict,
|
||||
) -> list[TrackSolution]:
|
||||
top_k = config["tracking"]["top_k"]
|
||||
beam_width = config["tracking"]["beam_width"]
|
||||
weights = config["tracking"]["weights"]
|
||||
states: dict[str, list[_PathState]] = {
|
||||
"gap": [
|
||||
_PathState(
|
||||
score=0.0,
|
||||
candidate_ids=tuple(),
|
||||
selected=tuple(),
|
||||
coverage_count=0,
|
||||
last_y=None,
|
||||
last_z=None,
|
||||
last_width=None,
|
||||
last_normal=None,
|
||||
prev_dy=None,
|
||||
prev_dz=None,
|
||||
gap_run=0,
|
||||
)
|
||||
]
|
||||
}
|
||||
|
||||
for slice_result in slices:
|
||||
candidates = (
|
||||
slice_result.left_candidates if side == "left" else slice_result.right_candidates
|
||||
)
|
||||
current_states: dict[str, list[_PathState]] = {"gap": []}
|
||||
for candidate in candidates:
|
||||
current_states[candidate.candidate_id] = []
|
||||
|
||||
for previous_group in states.values():
|
||||
for prev_state in previous_group:
|
||||
gap_score = prev_state.score - weights["gap_penalty"] - weights["gap_growth_penalty"] * prev_state.gap_run
|
||||
gap_state = _PathState(
|
||||
score=gap_score,
|
||||
candidate_ids=prev_state.candidate_ids + (None,),
|
||||
selected=prev_state.selected,
|
||||
coverage_count=prev_state.coverage_count,
|
||||
last_y=prev_state.last_y,
|
||||
last_z=prev_state.last_z,
|
||||
last_width=prev_state.last_width,
|
||||
last_normal=prev_state.last_normal,
|
||||
prev_dy=prev_state.prev_dy,
|
||||
prev_dz=prev_state.prev_dz,
|
||||
gap_run=prev_state.gap_run + 1,
|
||||
)
|
||||
current_states["gap"] = _insert_topk(current_states["gap"], gap_state, top_k)
|
||||
|
||||
for candidate in candidates:
|
||||
penalty = _transition_penalty(prev_state, candidate, config)
|
||||
dy = None if prev_state.last_y is None else candidate.y_center - prev_state.last_y
|
||||
dz = None if prev_state.last_z is None else candidate.z_low - prev_state.last_z
|
||||
score = (
|
||||
prev_state.score
|
||||
+ weights["coverage_reward"]
|
||||
+ weights["node_reward"] * candidate.score
|
||||
- penalty
|
||||
)
|
||||
candidate_state = _PathState(
|
||||
score=score,
|
||||
candidate_ids=prev_state.candidate_ids + (candidate.candidate_id,),
|
||||
selected=prev_state.selected + (candidate,),
|
||||
coverage_count=prev_state.coverage_count + 1,
|
||||
last_y=candidate.y_center,
|
||||
last_z=candidate.z_low,
|
||||
last_width=candidate.width,
|
||||
last_normal=_normal_to_vector(candidate),
|
||||
prev_dy=dy,
|
||||
prev_dz=dz,
|
||||
gap_run=0,
|
||||
)
|
||||
current_states[candidate.candidate_id] = _insert_topk(
|
||||
current_states[candidate.candidate_id],
|
||||
candidate_state,
|
||||
top_k,
|
||||
)
|
||||
|
||||
flat_states = [item for group in current_states.values() for item in group]
|
||||
flat_states.sort(key=lambda item: item.score, reverse=True)
|
||||
states = {"gap": [], **{key: [] for key in current_states if key != "gap"}}
|
||||
for state in flat_states[:beam_width]:
|
||||
state_key = "gap" if state.candidate_ids[-1] is None else state.candidate_ids[-1]
|
||||
states[state_key] = _insert_topk(states[state_key], state, top_k)
|
||||
|
||||
solutions: list[_PathState] = [item for group in states.values() for item in group]
|
||||
solutions.sort(key=lambda item: item.score, reverse=True)
|
||||
deduped: list[_PathState] = []
|
||||
seen = set()
|
||||
for solution in solutions:
|
||||
if solution.candidate_ids in seen:
|
||||
continue
|
||||
seen.add(solution.candidate_ids)
|
||||
deduped.append(solution)
|
||||
if len(deduped) >= top_k:
|
||||
break
|
||||
|
||||
final_solutions: list[TrackSolution] = []
|
||||
min_coverage = config["tracking"]["min_coverage_ratio"] * max(len(slices), 1)
|
||||
if not deduped:
|
||||
return final_solutions
|
||||
score_baseline = deduped[0].score
|
||||
for rank, state in enumerate(deduped, start=1):
|
||||
if state.coverage_count < min_coverage:
|
||||
continue
|
||||
margin = None
|
||||
if rank < len(deduped):
|
||||
margin = float(state.score - deduped[rank].score)
|
||||
confidence = float(
|
||||
np.clip(
|
||||
0.5
|
||||
+ 0.25 * state.coverage_count / max(len(slices), 1)
|
||||
+ 0.25 * (state.score / max(abs(score_baseline), 1e-6)),
|
||||
0.0,
|
||||
1.0,
|
||||
)
|
||||
)
|
||||
final_solutions.append(
|
||||
TrackSolution(
|
||||
side=side,
|
||||
rank=rank,
|
||||
score=float(state.score),
|
||||
coverage_ratio=float(state.coverage_count / max(len(slices), 1)),
|
||||
coverage_count=int(state.coverage_count),
|
||||
candidate_ids=list(state.candidate_ids),
|
||||
candidates=list(state.selected),
|
||||
score_margin=margin,
|
||||
confidence=confidence,
|
||||
)
|
||||
)
|
||||
return final_solutions
|
||||
|
||||
|
||||
def candidate_rows(slices: list[SliceResult]) -> list[dict]:
|
||||
rows: list[dict] = []
|
||||
for slice_result in slices:
|
||||
for candidate in slice_result.candidate_bands:
|
||||
rows.append(
|
||||
{
|
||||
"slice_index": candidate.slice_index,
|
||||
"candidate_id": candidate.candidate_id,
|
||||
"side": candidate.side,
|
||||
"x_start": candidate.x_start,
|
||||
"x_end": candidate.x_end,
|
||||
"x_center": candidate.x_center,
|
||||
"y_min": candidate.y_min,
|
||||
"y_max": candidate.y_max,
|
||||
"y_center": candidate.y_center,
|
||||
"z_low": candidate.z_low,
|
||||
"width": candidate.width,
|
||||
"point_count": candidate.point_count,
|
||||
"low_point_count": candidate.low_point_count,
|
||||
"flatness": candidate.flatness,
|
||||
"normal_alignment": candidate.normal_alignment,
|
||||
"score": candidate.score,
|
||||
"edge_ratio": candidate.edge_ratio,
|
||||
"y_mid": candidate.y_mid,
|
||||
}
|
||||
)
|
||||
return rows
|
||||
164
alg/delivery_v1.2.1/underframe_deflection/cad_io.py
Normal file
164
alg/delivery_v1.2.1/underframe_deflection/cad_io.py
Normal file
@@ -0,0 +1,164 @@
|
||||
"""CAD reference loader.
|
||||
|
||||
Accepts pre-tessellated CAD geometry — either a sampled point cloud
|
||||
(``.ply / .xyz / .txt / .csv``) or a triangle mesh (``.stl`` ASCII or
|
||||
binary, ``.obj``). For meshes, only the vertex positions are kept; face
|
||||
connectivity is discarded because the downstream registration is
|
||||
point-to-point ICP.
|
||||
|
||||
STEP / IGES files are not parsed here — convert them offline to STL or a
|
||||
sampled PLY first.
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import struct
|
||||
from pathlib import Path
|
||||
|
||||
import numpy as np
|
||||
|
||||
from .exceptions import InvalidPointCloudError, UnsupportedFormatError
|
||||
from .io import _read_ascii_ply
|
||||
|
||||
|
||||
_SUPPORTED_SUFFIXES = {".ply", ".xyz", ".txt", ".csv", ".stl", ".obj"}
|
||||
|
||||
|
||||
def read_cad_reference(path: str | Path) -> np.ndarray:
|
||||
"""Load a CAD reference and return an ``(N, 3) float64`` array.
|
||||
|
||||
The result is a unique set of vertex / point coordinates in
|
||||
millimetres. Faces / normals / colors in the source file are
|
||||
ignored.
|
||||
"""
|
||||
path = Path(path)
|
||||
suffix = path.suffix.lower()
|
||||
if suffix not in _SUPPORTED_SUFFIXES:
|
||||
raise UnsupportedFormatError(
|
||||
f"Unsupported CAD reference format: {path.suffix} "
|
||||
f"(supported: {sorted(_SUPPORTED_SUFFIXES)})"
|
||||
)
|
||||
if suffix == ".ply":
|
||||
points = _read_ascii_ply(path)
|
||||
elif suffix in {".xyz", ".txt", ".csv"}:
|
||||
try:
|
||||
data = np.loadtxt(path, dtype=np.float64)
|
||||
except Exception as exc: # numpy raises a few different things
|
||||
raise InvalidPointCloudError(
|
||||
f"Failed to read CAD reference text file {path}: {exc}"
|
||||
) from exc
|
||||
if data.ndim == 1:
|
||||
data = data[None, :]
|
||||
if data.shape[1] < 3:
|
||||
raise InvalidPointCloudError(
|
||||
f"CAD reference {path} does not contain XYZ columns."
|
||||
)
|
||||
points = data[:, :3]
|
||||
elif suffix == ".stl":
|
||||
points = _read_stl_vertices(path)
|
||||
elif suffix == ".obj":
|
||||
points = _read_obj_vertices(path)
|
||||
else: # pragma: no cover - guarded above
|
||||
raise UnsupportedFormatError(f"Unsupported CAD format: {suffix}")
|
||||
|
||||
if points.ndim != 2 or points.shape[1] != 3 or len(points) == 0:
|
||||
raise InvalidPointCloudError(
|
||||
f"CAD reference {path} produced an empty or malformed point set."
|
||||
)
|
||||
return np.ascontiguousarray(points, dtype=np.float64)
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# STL / OBJ readers
|
||||
# ---------------------------------------------------------------------------
|
||||
|
||||
|
||||
def _read_stl_vertices(path: Path) -> np.ndarray:
|
||||
"""Read vertex positions from an STL file (ASCII or binary)."""
|
||||
# ASCII STL must start with "solid" and contain "facet" keywords.
|
||||
# Binary STL has an 80-byte header followed by a uint32 triangle
|
||||
# count, so we sniff the first 6 bytes for "solid " AND verify by
|
||||
# scanning a few more lines for "facet" to avoid false positives.
|
||||
with path.open("rb") as fh:
|
||||
head = fh.read(256)
|
||||
if head[:5].lower() == b"solid" and b"facet" in head.lower():
|
||||
return _read_stl_ascii(path)
|
||||
return _read_stl_binary(path)
|
||||
|
||||
|
||||
def _read_stl_ascii(path: Path) -> np.ndarray:
|
||||
vertices: list[tuple[float, float, float]] = []
|
||||
try:
|
||||
with path.open("r", encoding="utf-8", errors="replace") as fh:
|
||||
for line in fh:
|
||||
stripped = line.strip()
|
||||
if stripped.startswith("vertex"):
|
||||
parts = stripped.split()
|
||||
if len(parts) >= 4:
|
||||
vertices.append(
|
||||
(float(parts[1]), float(parts[2]), float(parts[3]))
|
||||
)
|
||||
except (OSError, ValueError) as exc:
|
||||
raise InvalidPointCloudError(f"Malformed ASCII STL {path}: {exc}") from exc
|
||||
if not vertices:
|
||||
raise InvalidPointCloudError(f"ASCII STL {path} contains no vertices.")
|
||||
return _unique_rows(np.asarray(vertices, dtype=np.float64))
|
||||
|
||||
|
||||
def _read_stl_binary(path: Path) -> np.ndarray:
|
||||
try:
|
||||
with path.open("rb") as fh:
|
||||
fh.read(80) # header
|
||||
count_bytes = fh.read(4)
|
||||
if len(count_bytes) < 4:
|
||||
raise InvalidPointCloudError(
|
||||
f"Binary STL {path} truncated before triangle count."
|
||||
)
|
||||
(triangle_count,) = struct.unpack("<I", count_bytes)
|
||||
if triangle_count == 0:
|
||||
raise InvalidPointCloudError(f"Binary STL {path} has zero triangles.")
|
||||
payload = fh.read(triangle_count * 50)
|
||||
if len(payload) < triangle_count * 50:
|
||||
raise InvalidPointCloudError(
|
||||
f"Binary STL {path} truncated (expected "
|
||||
f"{triangle_count * 50} bytes, got {len(payload)})."
|
||||
)
|
||||
except (OSError, struct.error) as exc:
|
||||
raise InvalidPointCloudError(f"Failed to read binary STL {path}: {exc}") from exc
|
||||
|
||||
# Each 50-byte record: 12 floats (normal + 3 vertices) + 2-byte attr.
|
||||
arr = np.frombuffer(payload, dtype=np.uint8).reshape(triangle_count, 50)
|
||||
# vertices occupy bytes 12..48 -> 36 bytes -> 9 floats.
|
||||
floats = np.frombuffer(arr[:, 12:48].tobytes(), dtype="<f4").reshape(-1, 3)
|
||||
if not np.isfinite(floats).all():
|
||||
raise InvalidPointCloudError(f"Binary STL {path} contains non-finite values.")
|
||||
return _unique_rows(floats.astype(np.float64))
|
||||
|
||||
|
||||
def _read_obj_vertices(path: Path) -> np.ndarray:
|
||||
vertices: list[tuple[float, float, float]] = []
|
||||
try:
|
||||
with path.open("r", encoding="utf-8", errors="replace") as fh:
|
||||
for line in fh:
|
||||
if not line.startswith("v "):
|
||||
continue
|
||||
parts = line.split()
|
||||
if len(parts) >= 4:
|
||||
vertices.append(
|
||||
(float(parts[1]), float(parts[2]), float(parts[3]))
|
||||
)
|
||||
except (OSError, ValueError) as exc:
|
||||
raise InvalidPointCloudError(f"Malformed OBJ {path}: {exc}") from exc
|
||||
if not vertices:
|
||||
raise InvalidPointCloudError(f"OBJ {path} contains no vertex positions.")
|
||||
return np.asarray(vertices, dtype=np.float64)
|
||||
|
||||
|
||||
def _unique_rows(points: np.ndarray) -> np.ndarray:
|
||||
if len(points) == 0:
|
||||
return points
|
||||
# Vertices from triangle meshes have ~3x duplication; collapse them.
|
||||
return np.unique(points, axis=0)
|
||||
|
||||
|
||||
__all__ = ["read_cad_reference"]
|
||||
199
alg/delivery_v1.2.1/underframe_deflection/cli.py
Normal file
199
alg/delivery_v1.2.1/underframe_deflection/cli.py
Normal file
@@ -0,0 +1,199 @@
|
||||
"""Command-line entry point installed by ``pyproject.toml``.
|
||||
|
||||
The DEFAULT mode is single-beam measurement: the ROI(s) already isolate one
|
||||
beam each, the cloud is cropped and measured directly (no registration, no
|
||||
dual-beam tracking). Provide ``--roi`` for one beam, or ``--left-roi`` and
|
||||
``--right-roi`` to measure both in one invocation.
|
||||
|
||||
The legacy dual-beam pipeline (registration + left/right tracking on a
|
||||
car-bottom slab) is still available via ``--mode dual``.
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import argparse
|
||||
|
||||
|
||||
def parse_args() -> argparse.Namespace:
|
||||
parser = argparse.ArgumentParser(
|
||||
description="Measure underframe longitudinal beam deflection from a point cloud.",
|
||||
)
|
||||
parser.add_argument(
|
||||
"--mode",
|
||||
default="single",
|
||||
choices=["single", "dual"],
|
||||
help="'single' (default): ROI isolates one beam, measure directly. "
|
||||
"'dual': legacy car-bottom slab with left+right tracking + registration.",
|
||||
)
|
||||
parser.add_argument(
|
||||
"--input-cloud",
|
||||
required=True,
|
||||
help="Path to input scan point cloud (.ply or .xyz). Large binary PLYs "
|
||||
"are stream-cropped by the ROI in single mode.",
|
||||
)
|
||||
parser.add_argument(
|
||||
"--out-dir",
|
||||
required=True,
|
||||
help="Output directory for report.json/.xlsx, CSV, and PNG.",
|
||||
)
|
||||
# --- single mode ---
|
||||
parser.add_argument(
|
||||
"--roi",
|
||||
default=None,
|
||||
help="Single mode: ROI JSON isolating ONE beam (native frame "
|
||||
"x=length, y=height, z=width).",
|
||||
)
|
||||
parser.add_argument(
|
||||
"--left-roi",
|
||||
default=None,
|
||||
help="Single mode: ROI JSON for the left beam (use with --right-roi).",
|
||||
)
|
||||
parser.add_argument(
|
||||
"--right-roi",
|
||||
default=None,
|
||||
help="Single mode: ROI JSON for the right beam (use with --left-roi).",
|
||||
)
|
||||
parser.add_argument(
|
||||
"--side",
|
||||
default=None,
|
||||
help="Single mode label for --roi (left/right); inferred from the ROI's "
|
||||
"lateral z (x-forward: z>0 left, z<0 right) if omitted.",
|
||||
)
|
||||
parser.add_argument(
|
||||
"--tolerance",
|
||||
type=float,
|
||||
default=None,
|
||||
help="Single mode --roi: pass/fail tolerance in mm. Default by side "
|
||||
"(left 7, right 12).",
|
||||
)
|
||||
parser.add_argument(
|
||||
"--left-tolerance",
|
||||
type=float,
|
||||
default=None,
|
||||
help="Both-beam mode: left-beam tolerance in mm (default 7).",
|
||||
)
|
||||
parser.add_argument(
|
||||
"--right-tolerance",
|
||||
type=float,
|
||||
default=None,
|
||||
help="Both-beam mode: right-beam tolerance in mm (default 12).",
|
||||
)
|
||||
parser.add_argument(
|
||||
"--enable-registration",
|
||||
action="store_true",
|
||||
help="Single mode: rigidly align the cloud to --cad before measuring. "
|
||||
"Default OFF (cloud assumed already registered). Requires --cad as a "
|
||||
"sampled .ply/.stl/.obj (.stp must be converted first).",
|
||||
)
|
||||
# --- shared ---
|
||||
parser.add_argument(
|
||||
"--config",
|
||||
default=None,
|
||||
help="Optional JSON config override file.",
|
||||
)
|
||||
parser.add_argument(
|
||||
"--skip-excel",
|
||||
action="store_true",
|
||||
help="Skip writing the .xlsx report.",
|
||||
)
|
||||
parser.add_argument(
|
||||
"--skip-plot",
|
||||
action="store_true",
|
||||
help="Single mode: skip the PNG plot.",
|
||||
)
|
||||
# --- dual (legacy) mode ---
|
||||
parser.add_argument(
|
||||
"--cad",
|
||||
default=None,
|
||||
help="Dual mode: CAD reference (.ply/.xyz/.stl/.obj) for registration.",
|
||||
)
|
||||
parser.add_argument(
|
||||
"--profile",
|
||||
default="auto",
|
||||
choices=["auto", "M", "TC", "TP", "custom"],
|
||||
help="Dual mode: profile for scale-related tuning.",
|
||||
)
|
||||
parser.add_argument(
|
||||
"--skip-registration",
|
||||
action="store_true",
|
||||
help="Dual mode: bypass ICP even when --cad is supplied.",
|
||||
)
|
||||
parser.add_argument(
|
||||
"--no-bilateral",
|
||||
action="store_true",
|
||||
help="Dual mode: disable bilateral denoising.",
|
||||
)
|
||||
parser.add_argument(
|
||||
"--input-axis",
|
||||
default="y_up",
|
||||
choices=["z_up", "y_up"],
|
||||
help="Dual mode: height axis convention (default y_up).",
|
||||
)
|
||||
return parser.parse_args()
|
||||
|
||||
|
||||
def _load_config(path):
|
||||
if not path:
|
||||
return None
|
||||
import json
|
||||
from pathlib import Path
|
||||
return json.loads(Path(path).read_text(encoding="utf-8"))
|
||||
|
||||
|
||||
def main() -> None:
|
||||
args = parse_args()
|
||||
|
||||
if args.mode == "single":
|
||||
from .single_beam_pipeline import run_single_beam_pipeline, run_both_beams
|
||||
config = _load_config(args.config)
|
||||
if args.left_roi and args.right_roi:
|
||||
run_both_beams(
|
||||
input_cloud=args.input_cloud,
|
||||
left_roi=args.left_roi,
|
||||
right_roi=args.right_roi,
|
||||
out_dir=args.out_dir,
|
||||
config=config,
|
||||
cad_path=args.cad,
|
||||
enable_registration=args.enable_registration,
|
||||
left_tolerance_mm=args.left_tolerance,
|
||||
right_tolerance_mm=args.right_tolerance,
|
||||
skip_excel=args.skip_excel,
|
||||
skip_plot=args.skip_plot,
|
||||
)
|
||||
elif args.roi:
|
||||
run_single_beam_pipeline(
|
||||
input_cloud=args.input_cloud,
|
||||
roi_path=args.roi,
|
||||
out_dir=args.out_dir,
|
||||
side=args.side,
|
||||
tolerance_mm=args.tolerance,
|
||||
config=config,
|
||||
cad_path=args.cad,
|
||||
enable_registration=args.enable_registration,
|
||||
skip_excel=args.skip_excel,
|
||||
skip_plot=args.skip_plot,
|
||||
)
|
||||
else:
|
||||
raise SystemExit(
|
||||
"single mode requires --roi (one beam) or both --left-roi and --right-roi."
|
||||
)
|
||||
return
|
||||
|
||||
# dual (legacy) mode
|
||||
from .pipeline import run_measurement_pipeline
|
||||
run_measurement_pipeline(
|
||||
input_cloud=args.input_cloud,
|
||||
out_dir=args.out_dir,
|
||||
config_path=args.config,
|
||||
profile=args.profile,
|
||||
cad_path=args.cad,
|
||||
roi_path=args.roi,
|
||||
skip_registration=args.skip_registration,
|
||||
enable_bilateral=not args.no_bilateral,
|
||||
skip_excel=args.skip_excel,
|
||||
input_axis=args.input_axis,
|
||||
)
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
369
alg/delivery_v1.2.1/underframe_deflection/config.py
Normal file
369
alg/delivery_v1.2.1/underframe_deflection/config.py
Normal file
@@ -0,0 +1,369 @@
|
||||
from __future__ import annotations
|
||||
|
||||
import copy
|
||||
import json
|
||||
from pathlib import Path
|
||||
from typing import Any
|
||||
|
||||
import numpy as np
|
||||
|
||||
|
||||
DEFAULT_CONFIG: dict[str, Any] = {
|
||||
"preprocess": {
|
||||
"stat_k": 18,
|
||||
"stat_std_ratio": 2.8,
|
||||
"radius_spacing_multiplier": 2.8,
|
||||
"radius_min_mm": 12.0,
|
||||
"radius_min_neighbors": 4,
|
||||
"voxel_spacing_multiplier": 1.5,
|
||||
"voxel_min_mm": 8.0,
|
||||
"normal_k": 20,
|
||||
"spacing_sample_size": 4000,
|
||||
# Optional passthrough crop performed before any other filter.
|
||||
# ``aabb`` is populated from the external ROI JSON when supplied.
|
||||
"passthrough": {
|
||||
"enabled": False,
|
||||
"aabb": None,
|
||||
},
|
||||
# Point-cloud bilateral filter applied after radius filtering.
|
||||
"bilateral": {
|
||||
"enabled": True,
|
||||
"k": 24,
|
||||
"sigma_d_spacing_multiplier": 2.0,
|
||||
"sigma_d_min_mm": 2.0,
|
||||
"sigma_n_deg": 15.0,
|
||||
"iterations": 1,
|
||||
},
|
||||
},
|
||||
"frame": {
|
||||
"use_input_axes": True,
|
||||
"coarse_slice_count": 28,
|
||||
"coarse_y_bins": 30,
|
||||
"bottom_quantile": 0.14,
|
||||
"low_band_margin_spacing_multiplier": 6.0,
|
||||
"candidate_bin_min_points": 12,
|
||||
"normal_alignment_min": 0.55,
|
||||
"flatness_spacing_multiplier": 4.0,
|
||||
"flatness_min_mm": 8.0,
|
||||
"symmetry_weight": 1.25,
|
||||
"bottom_weight": 1.3,
|
||||
"flatness_weight": 1.0,
|
||||
"hole_weight": 0.55,
|
||||
},
|
||||
"roi": {
|
||||
"x_trim_quantile": 0.004,
|
||||
"bottom_height_ratio": 0.24,
|
||||
"bottom_quantile": 0.18,
|
||||
"z_margin_spacing_multiplier": 3.0,
|
||||
"min_z_ceiling_mm": 1500.0,
|
||||
# External axis-aligned bbox supplied via JSON.
|
||||
# Fields: x_min, x_max, y_min, y_max, z_min, z_max (mm, CAD frame).
|
||||
"aabb": None,
|
||||
},
|
||||
"beam": {
|
||||
"slice_thickness_length_ratio": 0.012,
|
||||
"slice_thickness_spacing_multiplier": 22.0,
|
||||
"slice_step_ratio": 0.5,
|
||||
"slice_min_points": 80,
|
||||
"y_bin_width_width_ratio": 0.03,
|
||||
"y_bin_width_spacing_multiplier": 3.5,
|
||||
"low_quantile": 0.18,
|
||||
"low_core_quantile": 0.28,
|
||||
"min_bin_points": 10,
|
||||
"width_min_ratio": 0.018,
|
||||
"width_max_ratio": 0.18,
|
||||
"width_preferred_ratio": 0.065,
|
||||
"flatness_spacing_multiplier": 4.5,
|
||||
"flatness_min_mm": 10.0,
|
||||
"normal_alignment_min": 0.6,
|
||||
"low_band_margin_spacing_multiplier": 8.0,
|
||||
"midline_trim_quantile": 0.02,
|
||||
"edge_preference_weight": 3.2,
|
||||
"strong_edge_ratio": 0.58,
|
||||
"inner_reject_ratio": 0.32,
|
||||
"edge_keep_min_ratio": 0.72,
|
||||
"relative_edge_window": 0.18,
|
||||
"drop_weak_edge_slices": True,
|
||||
},
|
||||
"tracking": {
|
||||
"top_k": 3,
|
||||
"beam_width": 24,
|
||||
"min_coverage_ratio": 0.28,
|
||||
"weights": {
|
||||
"coverage_reward": 2.2,
|
||||
"node_reward": 1.0,
|
||||
"y_penalty": 1.6,
|
||||
"z_penalty": 1.4,
|
||||
"width_penalty": 0.7,
|
||||
"normal_penalty": 0.8,
|
||||
"gap_penalty": 2.0,
|
||||
"gap_growth_penalty": 0.7,
|
||||
"jitter_penalty": 0.9,
|
||||
},
|
||||
},
|
||||
"representative": {
|
||||
"band_y_margin_spacing_multiplier": 4.0,
|
||||
"band_z_margin_spacing_multiplier": 10.0,
|
||||
"core_quantile": 0.28,
|
||||
"core_margin_spacing_multiplier": 5.0,
|
||||
"underside_profile_bin_count": 14,
|
||||
"underside_quantile": 0.2,
|
||||
"underside_outer_fraction": 0.4,
|
||||
"underside_rise_spacing_multiplier": 0.55,
|
||||
"underside_core_spacing_multiplier": 0.7,
|
||||
"underside_profile_margin_spacing_multiplier": 1.0,
|
||||
"underside_min_bin_points": 18,
|
||||
"max_iter": 5,
|
||||
"spline_smoothing_y_spacing_multiplier": 3.0,
|
||||
"spline_smoothing_z_spacing_multiplier": 8.0,
|
||||
"max_gap_slices": 4,
|
||||
"grid_step_slice_ratio": 1.0,
|
||||
# Robust rejection of representative points contaminated by gussets /
|
||||
# cross-members before the spline fit. Window is in units of beam
|
||||
# slice steps; sigma is the robust (MAD-based) rejection threshold.
|
||||
"outlier_rejection_enabled": True,
|
||||
"outlier_window_slices": 2.5,
|
||||
"outlier_sigma": 2.5,
|
||||
},
|
||||
"reference": {
|
||||
"mode": "line_endpoints",
|
||||
"stable_region_fraction": 0.14,
|
||||
"search_extension_spacing_multiplier": 12.0,
|
||||
"search_extension_min_mm": 650.0,
|
||||
"lateral_margin_width_ratio": 0.11,
|
||||
"vertical_margin_height_ratio": 0.22,
|
||||
"underside_lower_margin_spacing_multiplier": 1.2,
|
||||
"underside_upper_margin_spacing_multiplier": 3.8,
|
||||
"circle_trials": 360,
|
||||
"circle_sample_size": 700,
|
||||
"normal_axis_max_alignment": 0.65,
|
||||
"preferred_axes": ["y"],
|
||||
"radius_min_mm": 35.0,
|
||||
"radius_max_width_ratio": 0.04,
|
||||
"fit_tolerance_spacing_multiplier": 0.38,
|
||||
"coverage_bin_count": 20,
|
||||
"min_inliers": 18,
|
||||
"cluster_eps_spacing_multiplier": 2.5,
|
||||
"cluster_min_samples": 2,
|
||||
"x_window_count": 6,
|
||||
"x_window_overlap_ratio": 0.35,
|
||||
"axis_bonus": {
|
||||
"y": 0.42,
|
||||
"z": 0.18,
|
||||
"x": -0.85
|
||||
},
|
||||
"extremity_reward": 2.2,
|
||||
"underside_penalty_weight": 1.2,
|
||||
"vertical_center_penalty_weight": 0.25,
|
||||
"lateral_penalty_weight": 0.55,
|
||||
"low_confidence_threshold": 0.6,
|
||||
},
|
||||
"deflection": {
|
||||
"evaluation_grid_factor": 1.0,
|
||||
},
|
||||
"registration": {
|
||||
"enabled": True,
|
||||
"skip": False,
|
||||
"icp_max_iter": 60,
|
||||
"icp_tol_mm": 0.05,
|
||||
"icp_tol_rel": 1.0e-4,
|
||||
"icp_max_corr_spacing_multiplier": 6.0,
|
||||
"icp_max_corr_min_mm": 25.0,
|
||||
"icp_downsample_target": 30000,
|
||||
"trim_ratio": 0.2,
|
||||
"pca_sign_disambiguation": True,
|
||||
# If the recovered transform is within these tolerances of identity
|
||||
# (and snapping does not worsen correspondence), return exact identity
|
||||
# so an already-aligned scan is left untouched.
|
||||
"identity_rot_tol_deg": 0.5,
|
||||
"identity_trans_tol_mm": 5.0,
|
||||
},
|
||||
"cad": {
|
||||
"path": None,
|
||||
"downsample_voxel_mm": 4.0,
|
||||
},
|
||||
"visualization": {
|
||||
"max_scatter_points": 40000,
|
||||
"dpi": 180,
|
||||
},
|
||||
}
|
||||
|
||||
|
||||
PROFILE_OVERRIDES: dict[str, dict[str, Any]] = {
|
||||
"auto": {},
|
||||
"M": {
|
||||
"beam": {
|
||||
"slice_thickness_length_ratio": 0.011,
|
||||
"width_preferred_ratio": 0.06,
|
||||
}
|
||||
},
|
||||
"TC": {
|
||||
"beam": {
|
||||
"slice_thickness_length_ratio": 0.014,
|
||||
"width_preferred_ratio": 0.07,
|
||||
},
|
||||
"reference": {"stable_region_fraction": 0.16},
|
||||
},
|
||||
"TP": {
|
||||
"beam": {
|
||||
"slice_thickness_length_ratio": 0.013,
|
||||
"width_preferred_ratio": 0.072,
|
||||
},
|
||||
"reference": {"vertical_margin_height_ratio": 0.24},
|
||||
},
|
||||
"custom": {},
|
||||
}
|
||||
|
||||
|
||||
def deep_merge(base: dict[str, Any], override: dict[str, Any]) -> dict[str, Any]:
|
||||
merged = copy.deepcopy(base)
|
||||
for key, value in override.items():
|
||||
if isinstance(value, dict) and isinstance(merged.get(key), dict):
|
||||
merged[key] = deep_merge(merged[key], value)
|
||||
else:
|
||||
merged[key] = copy.deepcopy(value)
|
||||
return merged
|
||||
|
||||
|
||||
def load_config(config_path: str | None, profile: str) -> dict[str, Any]:
|
||||
profile_name = profile if profile in PROFILE_OVERRIDES else "auto"
|
||||
config = deep_merge(DEFAULT_CONFIG, PROFILE_OVERRIDES[profile_name])
|
||||
if config_path:
|
||||
user_config = json.loads(Path(config_path).read_text(encoding="utf-8"))
|
||||
config = deep_merge(config, user_config)
|
||||
return config
|
||||
|
||||
|
||||
def cloud_geometry(points: np.ndarray) -> dict[str, float]:
|
||||
mins = points.min(axis=0)
|
||||
maxs = points.max(axis=0)
|
||||
spans = maxs - mins
|
||||
return {
|
||||
"x_min": float(mins[0]),
|
||||
"x_max": float(maxs[0]),
|
||||
"y_min": float(mins[1]),
|
||||
"y_max": float(maxs[1]),
|
||||
"z_min": float(mins[2]),
|
||||
"z_max": float(maxs[2]),
|
||||
"length": float(spans[0]),
|
||||
"width": float(spans[1]),
|
||||
"height": float(spans[2]),
|
||||
}
|
||||
|
||||
|
||||
def resolve_numeric_parameters(
|
||||
config: dict[str, Any],
|
||||
geometry: dict[str, float],
|
||||
spacing: float,
|
||||
) -> dict[str, Any]:
|
||||
resolved = copy.deepcopy(config)
|
||||
length = geometry["length"]
|
||||
width = geometry["width"]
|
||||
height = geometry["height"]
|
||||
|
||||
preprocess = resolved["preprocess"]
|
||||
preprocess["radius_mm"] = max(
|
||||
preprocess["radius_min_mm"],
|
||||
preprocess["radius_spacing_multiplier"] * spacing,
|
||||
)
|
||||
preprocess["voxel_mm"] = max(
|
||||
preprocess["voxel_min_mm"],
|
||||
preprocess["voxel_spacing_multiplier"] * spacing,
|
||||
)
|
||||
bilateral = preprocess.get("bilateral")
|
||||
if isinstance(bilateral, dict):
|
||||
bilateral["sigma_d_mm"] = max(
|
||||
float(bilateral.get("sigma_d_min_mm", 2.0)),
|
||||
float(bilateral.get("sigma_d_spacing_multiplier", 2.0)) * spacing,
|
||||
)
|
||||
bilateral["sigma_n_rad"] = float(
|
||||
np.deg2rad(float(bilateral.get("sigma_n_deg", 15.0)))
|
||||
)
|
||||
|
||||
frame = resolved["frame"]
|
||||
frame["flatness_mm"] = max(
|
||||
frame["flatness_min_mm"],
|
||||
frame["flatness_spacing_multiplier"] * spacing,
|
||||
)
|
||||
frame["low_band_margin_mm"] = (
|
||||
frame["low_band_margin_spacing_multiplier"] * spacing
|
||||
)
|
||||
|
||||
roi = resolved["roi"]
|
||||
roi["z_margin_mm"] = roi["z_margin_spacing_multiplier"] * spacing
|
||||
|
||||
beam = resolved["beam"]
|
||||
beam["slice_thickness_mm"] = max(
|
||||
beam["slice_thickness_length_ratio"] * length,
|
||||
beam["slice_thickness_spacing_multiplier"] * spacing,
|
||||
)
|
||||
beam["slice_step_mm"] = beam["slice_thickness_mm"] * beam["slice_step_ratio"]
|
||||
beam["y_bin_width_mm"] = max(
|
||||
beam["y_bin_width_width_ratio"] * width,
|
||||
beam["y_bin_width_spacing_multiplier"] * spacing,
|
||||
)
|
||||
beam["width_min_mm"] = beam["width_min_ratio"] * width
|
||||
beam["width_max_mm"] = beam["width_max_ratio"] * width
|
||||
beam["width_preferred_mm"] = beam["width_preferred_ratio"] * width
|
||||
beam["flatness_mm"] = max(
|
||||
beam["flatness_min_mm"],
|
||||
beam["flatness_spacing_multiplier"] * spacing,
|
||||
)
|
||||
beam["low_band_margin_mm"] = (
|
||||
beam["low_band_margin_spacing_multiplier"] * spacing
|
||||
)
|
||||
|
||||
representative = resolved["representative"]
|
||||
representative["band_y_margin_mm"] = (
|
||||
representative["band_y_margin_spacing_multiplier"] * spacing
|
||||
)
|
||||
representative["band_z_margin_mm"] = (
|
||||
representative["band_z_margin_spacing_multiplier"] * spacing
|
||||
)
|
||||
representative["core_margin_mm"] = (
|
||||
representative["core_margin_spacing_multiplier"] * spacing
|
||||
)
|
||||
representative["grid_step_mm"] = (
|
||||
representative["grid_step_slice_ratio"] * beam["slice_step_mm"]
|
||||
)
|
||||
|
||||
reference = resolved["reference"]
|
||||
reference["lateral_margin_mm"] = (
|
||||
reference["lateral_margin_width_ratio"] * width
|
||||
)
|
||||
reference["vertical_margin_mm"] = (
|
||||
reference["vertical_margin_height_ratio"] * height
|
||||
)
|
||||
reference["search_extension_mm"] = max(
|
||||
reference["search_extension_min_mm"],
|
||||
reference["search_extension_spacing_multiplier"] * spacing,
|
||||
)
|
||||
reference["underside_lower_margin_mm"] = (
|
||||
reference["underside_lower_margin_spacing_multiplier"] * spacing
|
||||
)
|
||||
reference["underside_upper_margin_mm"] = (
|
||||
reference["underside_upper_margin_spacing_multiplier"] * spacing
|
||||
)
|
||||
reference["radius_max_mm"] = max(
|
||||
reference["radius_min_mm"] * 1.5,
|
||||
reference["radius_max_width_ratio"] * width,
|
||||
)
|
||||
reference["fit_tolerance_mm"] = (
|
||||
reference["fit_tolerance_spacing_multiplier"] * spacing
|
||||
)
|
||||
reference["cluster_eps_mm"] = (
|
||||
reference["cluster_eps_spacing_multiplier"] * spacing
|
||||
)
|
||||
|
||||
registration = resolved.get("registration")
|
||||
if isinstance(registration, dict):
|
||||
registration["icp_max_corr_mm"] = max(
|
||||
float(registration.get("icp_max_corr_min_mm", 25.0)),
|
||||
float(registration.get("icp_max_corr_spacing_multiplier", 6.0)) * spacing,
|
||||
)
|
||||
|
||||
resolved["runtime"] = {
|
||||
"spacing_mm": float(spacing),
|
||||
"geometry": geometry,
|
||||
}
|
||||
return resolved
|
||||
251
alg/delivery_v1.2.1/underframe_deflection/excel_report.py
Normal file
251
alg/delivery_v1.2.1/underframe_deflection/excel_report.py
Normal file
@@ -0,0 +1,251 @@
|
||||
"""Excel error report writer (.xlsx).
|
||||
|
||||
Optional dependency: ``openpyxl``. When the package is missing the
|
||||
caller is expected to fall back gracefully (e.g. log a warning and
|
||||
continue without the xlsx artifact).
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
from pathlib import Path
|
||||
from typing import Any
|
||||
|
||||
from .api import DeflectionResult
|
||||
from .artifacts import _require_artifacts, _annotate_beam_track_rows
|
||||
from .beams import candidate_rows
|
||||
from .io import ensure_output_dir
|
||||
|
||||
|
||||
SUMMARY_SHEET = "Summary"
|
||||
CURVES_SHEET = "Deflection_Curves"
|
||||
TRACKS_SHEET = "Beam_Tracks"
|
||||
REFERENCE_SHEET = "Reference_Lines"
|
||||
REPRESENTATIVES_SHEET = "Representative_Points"
|
||||
|
||||
|
||||
def write_excel_report(
|
||||
result: DeflectionResult,
|
||||
path: str | Path,
|
||||
*,
|
||||
inputs: dict[str, Any] | None = None,
|
||||
registration_info: dict[str, Any] | None = None,
|
||||
) -> Path:
|
||||
"""Write a 5-sheet Excel error report next to the JSON artifacts.
|
||||
|
||||
``inputs`` is a free-form dict of input paths (scan / cad / roi) and
|
||||
profile; it is rendered verbatim into the Summary sheet.
|
||||
``registration_info`` may include ``rms_mm``, ``iterations``,
|
||||
``inlier_ratio``, ``converged``, ``initial_rms_mm``.
|
||||
"""
|
||||
try:
|
||||
from openpyxl import Workbook
|
||||
except ImportError as exc: # pragma: no cover - exercised at runtime only
|
||||
raise ImportError(
|
||||
"Writing Excel reports requires the 'openpyxl' package. "
|
||||
"Install with: pip install openpyxl"
|
||||
) from exc
|
||||
|
||||
output_path = Path(path)
|
||||
ensure_output_dir(output_path.parent)
|
||||
artifacts = _require_artifacts(result)
|
||||
|
||||
wb = Workbook()
|
||||
wb.remove(wb.active)
|
||||
|
||||
_write_summary(wb, result, artifacts, inputs or {}, registration_info or {})
|
||||
_write_curves(wb, artifacts)
|
||||
_write_tracks(wb, artifacts)
|
||||
_write_reference_lines(wb, result)
|
||||
_write_representatives(wb, result)
|
||||
|
||||
wb.save(output_path)
|
||||
return output_path
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# Sheet builders
|
||||
# ---------------------------------------------------------------------------
|
||||
|
||||
|
||||
def _write_summary(
|
||||
wb,
|
||||
result: DeflectionResult,
|
||||
artifacts,
|
||||
inputs: dict[str, Any],
|
||||
reg: dict[str, Any],
|
||||
) -> None:
|
||||
ws = wb.create_sheet(SUMMARY_SHEET)
|
||||
ws.append(["key", "value"])
|
||||
rows: list[tuple[str, Any]] = []
|
||||
|
||||
rows.append(("schema_version", result.schema_version))
|
||||
rows.append(("algorithm_version", result.algorithm_version))
|
||||
rows.append(("profile", inputs.get("profile", "")))
|
||||
rows.append(("scan_path", inputs.get("scan_path", "")))
|
||||
rows.append(("cad_path", inputs.get("cad_path", "")))
|
||||
rows.append(("roi_path", inputs.get("roi_path", "")))
|
||||
rows.append(("config_path", inputs.get("config_path", "")))
|
||||
|
||||
rows.append(("registration_enabled", reg.get("enabled", False)))
|
||||
rows.append(("registration_rms_mm", reg.get("rms_mm", "")))
|
||||
rows.append(("registration_initial_rms_mm", reg.get("initial_rms_mm", "")))
|
||||
rows.append(("registration_iterations", reg.get("iterations", "")))
|
||||
rows.append(("registration_inlier_ratio", reg.get("inlier_ratio", "")))
|
||||
rows.append(("registration_converged", reg.get("converged", "")))
|
||||
|
||||
preprocess = result.report.get("preprocess", {}) or {}
|
||||
for key in (
|
||||
"raw_count",
|
||||
"after_passthrough",
|
||||
"passthrough_applied",
|
||||
"after_statistical_filter",
|
||||
"after_radius_filter",
|
||||
"after_bilateral",
|
||||
"bilateral_applied",
|
||||
"downsampled_count",
|
||||
"spacing_mm",
|
||||
"statistical_keep_ratio",
|
||||
"radius_keep_ratio",
|
||||
):
|
||||
if key in preprocess:
|
||||
rows.append((f"preprocess.{key}", preprocess[key]))
|
||||
|
||||
deflection = result.report.get("deflection", {}) or {}
|
||||
for side in ("left", "right"):
|
||||
side_data = deflection.get(side, {}) or {}
|
||||
selected = side_data.get("selected_measurement", {}) or {}
|
||||
rows.append((f"{side}.available", side_data.get("available", False)))
|
||||
rows.append((f"{side}.reference_available", side_data.get("reference_available", False)))
|
||||
rows.append((f"{side}.reference_confidence", side_data.get("reference_confidence", "")))
|
||||
rows.append((f"{side}.max_downward_deflection_mm", side_data.get("max_downward_deflection_mm", "")))
|
||||
rows.append((f"{side}.max_downward_x_mm", side_data.get("max_downward_x_mm", "")))
|
||||
rows.append((f"{side}.max_upward_deflection_mm", side_data.get("max_upward_deflection_mm", "")))
|
||||
rows.append((f"{side}.max_upward_x_mm", side_data.get("max_upward_x_mm", "")))
|
||||
rows.append((f"{side}.selected_x_mm", selected.get("x_mm", "")))
|
||||
rows.append((f"{side}.selected_signed_deflection_mm", selected.get("signed_deflection_mm", "")))
|
||||
rows.append((f"{side}.selected_absolute_deflection_mm", selected.get("absolute_deflection_mm", "")))
|
||||
rows.append((f"{side}.beam_fit_residual_rms_z", side_data.get("beam_fit_residual_rms_z", "")))
|
||||
|
||||
rows.append(("quality_flags", "; ".join(result.quality_flags)))
|
||||
|
||||
roi_payload = result.report.get("roi", {}) or {}
|
||||
for key in ("source", "x_min", "x_max", "y_min", "y_max", "z_min", "z_max", "z_ceiling", "point_count"):
|
||||
if key in roi_payload:
|
||||
rows.append((f"roi.{key}", roi_payload[key]))
|
||||
|
||||
for key, value in rows:
|
||||
ws.append([key, _coerce_cell(value)])
|
||||
|
||||
ws.column_dimensions["A"].width = 42
|
||||
ws.column_dimensions["B"].width = 32
|
||||
|
||||
|
||||
def _write_curves(wb, artifacts) -> None:
|
||||
ws = wb.create_sheet(CURVES_SHEET)
|
||||
rows = artifacts.deflection_rows
|
||||
if not rows:
|
||||
ws.append(["x"])
|
||||
return
|
||||
fieldnames = list(rows[0].keys())
|
||||
ws.append(fieldnames)
|
||||
for row in rows:
|
||||
ws.append([_coerce_cell(row.get(name)) for name in fieldnames])
|
||||
|
||||
|
||||
def _write_tracks(wb, artifacts) -> None:
|
||||
ws = wb.create_sheet(TRACKS_SHEET)
|
||||
rows = _annotate_beam_track_rows(
|
||||
candidate_rows(artifacts.slices),
|
||||
artifacts.left_tracks,
|
||||
artifacts.right_tracks,
|
||||
)
|
||||
if not rows:
|
||||
ws.append(["slice_index", "candidate_id", "side"])
|
||||
return
|
||||
fieldnames = list(rows[0].keys())
|
||||
ws.append(fieldnames)
|
||||
for row in rows:
|
||||
ws.append([_coerce_cell(row.get(name)) for name in fieldnames])
|
||||
|
||||
|
||||
def _write_reference_lines(wb, result: DeflectionResult) -> None:
|
||||
ws = wb.create_sheet(REFERENCE_SHEET)
|
||||
header = [
|
||||
"side",
|
||||
"available",
|
||||
"confidence",
|
||||
"residual",
|
||||
"front_x",
|
||||
"front_y",
|
||||
"front_z",
|
||||
"back_x",
|
||||
"back_y",
|
||||
"back_z",
|
||||
"dir_x",
|
||||
"dir_y",
|
||||
"dir_z",
|
||||
]
|
||||
ws.append(header)
|
||||
for side in ("left", "right"):
|
||||
ref = (result.report.get("reference_lines") or {}).get(side) or {}
|
||||
front = ref.get("point_front") or [None, None, None]
|
||||
back = ref.get("point_back") or [None, None, None]
|
||||
direction = ref.get("direction") or [None, None, None]
|
||||
ws.append(
|
||||
[
|
||||
side,
|
||||
_coerce_cell(ref.get("available")),
|
||||
_coerce_cell(ref.get("confidence")),
|
||||
_coerce_cell(ref.get("residual")),
|
||||
_coerce_cell(front[0] if len(front) > 0 else None),
|
||||
_coerce_cell(front[1] if len(front) > 1 else None),
|
||||
_coerce_cell(front[2] if len(front) > 2 else None),
|
||||
_coerce_cell(back[0] if len(back) > 0 else None),
|
||||
_coerce_cell(back[1] if len(back) > 1 else None),
|
||||
_coerce_cell(back[2] if len(back) > 2 else None),
|
||||
_coerce_cell(direction[0] if len(direction) > 0 else None),
|
||||
_coerce_cell(direction[1] if len(direction) > 1 else None),
|
||||
_coerce_cell(direction[2] if len(direction) > 2 else None),
|
||||
]
|
||||
)
|
||||
|
||||
|
||||
def _write_representatives(wb, result: DeflectionResult) -> None:
|
||||
ws = wb.create_sheet(REPRESENTATIVES_SHEET)
|
||||
header = ["side", "slice_index", "x", "y", "z", "width", "point_count", "residual", "confidence"]
|
||||
ws.append(header)
|
||||
rep_payload = result.report.get("representative_points") or {}
|
||||
for side in ("left", "right"):
|
||||
for item in rep_payload.get(side, []) or []:
|
||||
ws.append(
|
||||
[
|
||||
side,
|
||||
_coerce_cell(item.get("slice_index")),
|
||||
_coerce_cell(item.get("x")),
|
||||
_coerce_cell(item.get("y")),
|
||||
_coerce_cell(item.get("z")),
|
||||
_coerce_cell(item.get("width")),
|
||||
_coerce_cell(item.get("point_count")),
|
||||
_coerce_cell(item.get("residual")),
|
||||
_coerce_cell(item.get("confidence")),
|
||||
]
|
||||
)
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# Helpers
|
||||
# ---------------------------------------------------------------------------
|
||||
|
||||
|
||||
def _coerce_cell(value: Any) -> Any:
|
||||
"""Map non-primitive types to plain Python so openpyxl can write them."""
|
||||
if value is None:
|
||||
return ""
|
||||
if isinstance(value, (str, int, float, bool)):
|
||||
return value
|
||||
if isinstance(value, (list, tuple)):
|
||||
return ",".join(str(_coerce_cell(v)) for v in value)
|
||||
return str(value)
|
||||
|
||||
|
||||
__all__ = ["write_excel_report"]
|
||||
40
alg/delivery_v1.2.1/underframe_deflection/exceptions.py
Normal file
40
alg/delivery_v1.2.1/underframe_deflection/exceptions.py
Normal file
@@ -0,0 +1,40 @@
|
||||
"""Public exception hierarchy for underframe_deflection.
|
||||
|
||||
Integrators should catch `DeflectionError` for any algorithm failure that
|
||||
should be surfaced to the user. Subclasses indicate what specifically went
|
||||
wrong so callers can render targeted error messages.
|
||||
|
||||
Non-fatal data-quality issues are not raised as exceptions; they are
|
||||
reported via `DeflectionResult.quality_flags`.
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
|
||||
class DeflectionError(Exception):
|
||||
"""Base class for all errors raised by the deflection pipeline."""
|
||||
|
||||
|
||||
class InvalidPointCloudError(DeflectionError):
|
||||
"""Raised when the input point cloud is malformed or unreadable."""
|
||||
|
||||
|
||||
class InsufficientPointsError(DeflectionError):
|
||||
"""Raised when the input has too few points to run the pipeline."""
|
||||
|
||||
|
||||
class FrameInferenceError(DeflectionError):
|
||||
"""Raised when the vehicle reference frame cannot be determined."""
|
||||
|
||||
|
||||
class UnsupportedFormatError(InvalidPointCloudError):
|
||||
"""Raised when the input file extension is not supported."""
|
||||
|
||||
|
||||
__all__ = [
|
||||
"DeflectionError",
|
||||
"InvalidPointCloudError",
|
||||
"InsufficientPointsError",
|
||||
"FrameInferenceError",
|
||||
"UnsupportedFormatError",
|
||||
]
|
||||
308
alg/delivery_v1.2.1/underframe_deflection/fitting.py
Normal file
308
alg/delivery_v1.2.1/underframe_deflection/fitting.py
Normal file
@@ -0,0 +1,308 @@
|
||||
from __future__ import annotations
|
||||
|
||||
from dataclasses import asdict
|
||||
|
||||
import numpy as np
|
||||
from scipy.interpolate import UnivariateSpline
|
||||
|
||||
from .structures import CandidateBand, FittedLine, RepresentativePoint, TrackSolution
|
||||
|
||||
|
||||
def _robust_center_2d(points_yz: np.ndarray, max_iter: int) -> tuple[np.ndarray, float]:
|
||||
center = np.median(points_yz, axis=0)
|
||||
residual = 0.0
|
||||
for _ in range(max_iter):
|
||||
deltas = points_yz - center
|
||||
scales = np.median(np.abs(deltas), axis=0) + 1e-6
|
||||
radius = np.sqrt(np.sum((deltas / scales) ** 2, axis=1))
|
||||
c = 4.685
|
||||
weights = np.square(1.0 - np.square(np.clip(radius / c, 0.0, 1.0)))
|
||||
weights[radius >= c] = 0.0
|
||||
if not np.any(weights > 0):
|
||||
break
|
||||
center = np.average(points_yz, axis=0, weights=weights)
|
||||
residual = float(np.sqrt(np.average(np.sum(deltas**2, axis=1), weights=weights)))
|
||||
return center, residual
|
||||
|
||||
|
||||
def _extract_underside_core_points(
|
||||
region_points: np.ndarray,
|
||||
candidate: CandidateBand,
|
||||
config: dict,
|
||||
) -> np.ndarray:
|
||||
rep_cfg = config["representative"]
|
||||
spacing = config["runtime"]["spacing_mm"]
|
||||
profile_margin = max(
|
||||
rep_cfg["underside_profile_margin_spacing_multiplier"] * spacing,
|
||||
0.2 * rep_cfg["band_y_margin_mm"],
|
||||
)
|
||||
y_lo = max(float(region_points[:, 1].min()), candidate.y_min - profile_margin)
|
||||
y_hi = min(float(region_points[:, 1].max()), candidate.y_max + profile_margin)
|
||||
if not np.isfinite(y_lo) or not np.isfinite(y_hi) or (y_hi - y_lo) <= 1e-6:
|
||||
return np.zeros((0, 3), dtype=np.float64)
|
||||
|
||||
bin_count = max(int(rep_cfg["underside_profile_bin_count"]), 8)
|
||||
y_edges = np.linspace(y_lo, y_hi, bin_count + 1)
|
||||
valid_bins: list[dict[str, float | int]] = []
|
||||
min_bin_points = int(rep_cfg["underside_min_bin_points"])
|
||||
underside_quantile = float(rep_cfg["underside_quantile"])
|
||||
|
||||
for index in range(bin_count):
|
||||
bin_mask = (region_points[:, 1] >= y_edges[index]) & (region_points[:, 1] < y_edges[index + 1])
|
||||
bin_points = region_points[bin_mask]
|
||||
if len(bin_points) < min_bin_points:
|
||||
continue
|
||||
z_floor = float(np.quantile(bin_points[:, 2], underside_quantile))
|
||||
valid_bins.append(
|
||||
{
|
||||
"index": index,
|
||||
"y_min": float(y_edges[index]),
|
||||
"y_max": float(y_edges[index + 1]),
|
||||
"y_center": float(0.5 * (y_edges[index] + y_edges[index + 1])),
|
||||
"z_floor": z_floor,
|
||||
"count": int(len(bin_points)),
|
||||
}
|
||||
)
|
||||
|
||||
if len(valid_bins) < 3:
|
||||
return np.zeros((0, 3), dtype=np.float64)
|
||||
|
||||
outer_bin_count = max(int(np.ceil(len(valid_bins) * rep_cfg["underside_outer_fraction"])), 2)
|
||||
if candidate.side == "left":
|
||||
outer_bins = valid_bins[:outer_bin_count]
|
||||
else:
|
||||
outer_bins = valid_bins[-outer_bin_count:]
|
||||
anchor_bin = min(outer_bins, key=lambda item: item["z_floor"])
|
||||
anchor_index = int(anchor_bin["index"])
|
||||
floor_ref = float(anchor_bin["z_floor"])
|
||||
rise_limit = floor_ref + max(
|
||||
rep_cfg["underside_rise_spacing_multiplier"] * spacing,
|
||||
35.0,
|
||||
)
|
||||
|
||||
valid_by_index = {int(item["index"]): item for item in valid_bins}
|
||||
selected_bins = [anchor_bin]
|
||||
expand_index = anchor_index - 1
|
||||
while expand_index in valid_by_index and float(valid_by_index[expand_index]["z_floor"]) <= rise_limit:
|
||||
selected_bins.append(valid_by_index[expand_index])
|
||||
expand_index -= 1
|
||||
expand_index = anchor_index + 1
|
||||
while expand_index in valid_by_index and float(valid_by_index[expand_index]["z_floor"]) <= rise_limit:
|
||||
selected_bins.append(valid_by_index[expand_index])
|
||||
expand_index += 1
|
||||
if not selected_bins:
|
||||
return np.zeros((0, 3), dtype=np.float64)
|
||||
|
||||
y_min = min(float(item["y_min"]) for item in selected_bins)
|
||||
y_max = max(float(item["y_max"]) for item in selected_bins)
|
||||
core_ceiling = floor_ref + max(
|
||||
rep_cfg["underside_core_spacing_multiplier"] * spacing,
|
||||
45.0,
|
||||
)
|
||||
core_mask = (
|
||||
(region_points[:, 1] >= y_min)
|
||||
& (region_points[:, 1] <= y_max)
|
||||
& (region_points[:, 2] <= core_ceiling)
|
||||
)
|
||||
core_points = region_points[core_mask]
|
||||
if len(core_points) < 6:
|
||||
return np.zeros((0, 3), dtype=np.float64)
|
||||
return core_points
|
||||
|
||||
|
||||
def extract_representative_points(
|
||||
points_vehicle: np.ndarray,
|
||||
track: TrackSolution,
|
||||
config: dict,
|
||||
) -> list[RepresentativePoint]:
|
||||
rep_cfg = config["representative"]
|
||||
representatives: list[RepresentativePoint] = []
|
||||
for candidate in track.candidates:
|
||||
region_mask = (
|
||||
(points_vehicle[:, 0] >= candidate.x_start)
|
||||
& (points_vehicle[:, 0] < candidate.x_end)
|
||||
& (points_vehicle[:, 1] >= candidate.y_min - rep_cfg["band_y_margin_mm"])
|
||||
& (points_vehicle[:, 1] <= candidate.y_max + rep_cfg["band_y_margin_mm"])
|
||||
& (
|
||||
points_vehicle[:, 2]
|
||||
<= candidate.z_low + rep_cfg["band_z_margin_mm"]
|
||||
)
|
||||
)
|
||||
region_points = points_vehicle[region_mask]
|
||||
if len(region_points) < max(8, candidate.low_point_count):
|
||||
continue
|
||||
core_points = _extract_underside_core_points(region_points, candidate, config)
|
||||
if len(core_points) < 6:
|
||||
z_quantile = float(np.quantile(region_points[:, 2], rep_cfg["core_quantile"]))
|
||||
core_ceiling = min(z_quantile, candidate.z_low + rep_cfg["core_margin_mm"])
|
||||
core_points = region_points[region_points[:, 2] <= core_ceiling]
|
||||
if len(core_points) < 6:
|
||||
core_points = region_points[np.argsort(region_points[:, 2])[: max(6, len(region_points) // 4)]]
|
||||
center_yz, residual = _robust_center_2d(core_points[:, 1:3], rep_cfg["max_iter"])
|
||||
confidence = float(
|
||||
np.clip(
|
||||
0.35
|
||||
+ 0.3 * min(len(core_points) / max(candidate.low_point_count, 1), 1.5)
|
||||
+ 0.35 * max(0.0, 1.0 - residual / max(candidate.width, 1.0)),
|
||||
0.0,
|
||||
1.0,
|
||||
)
|
||||
)
|
||||
representatives.append(
|
||||
RepresentativePoint(
|
||||
side=track.side,
|
||||
slice_index=candidate.slice_index,
|
||||
x=float(np.median(core_points[:, 0])),
|
||||
y=float(center_yz[0]),
|
||||
z=float(center_yz[1]),
|
||||
width=float(candidate.width),
|
||||
point_count=int(len(core_points)),
|
||||
residual=residual,
|
||||
confidence=confidence,
|
||||
x_start=candidate.x_start,
|
||||
x_end=candidate.x_end,
|
||||
)
|
||||
)
|
||||
representatives.sort(key=lambda item: item.x)
|
||||
return representatives
|
||||
|
||||
|
||||
def _fit_spline(
|
||||
x: np.ndarray,
|
||||
values: np.ndarray,
|
||||
smooth_scale: float,
|
||||
iterations: int = 4,
|
||||
) -> tuple[UnivariateSpline, float]:
|
||||
weights = np.ones_like(values)
|
||||
spline = None
|
||||
residual_rms = 0.0
|
||||
smoothing = max(len(x) * smooth_scale**2, 1e-6)
|
||||
for _ in range(iterations):
|
||||
spline = UnivariateSpline(x, values, w=weights, s=smoothing)
|
||||
fitted = spline(x)
|
||||
residual = values - fitted
|
||||
scale = np.median(np.abs(residual)) * 1.4826 + 1e-6
|
||||
ratio = np.abs(residual) / (4.685 * scale)
|
||||
weights = np.square(1.0 - np.square(np.clip(ratio, 0.0, 1.0)))
|
||||
weights[ratio >= 1.0] = 0.05
|
||||
residual_rms = float(np.sqrt(np.mean(residual**2)))
|
||||
assert spline is not None
|
||||
return spline, residual_rms
|
||||
|
||||
|
||||
def _reject_contaminated_representatives(
|
||||
x: np.ndarray,
|
||||
z: np.ndarray,
|
||||
window_mm: float,
|
||||
sigma: float,
|
||||
) -> np.ndarray:
|
||||
"""Flag representative points that deviate from a robust local baseline.
|
||||
|
||||
When the cloud is sampled directly from CAD, gussets and cross-members
|
||||
intermittently raise the detected beam underside by tens of mm at
|
||||
isolated X positions. A sliding-window median is robust to such localized
|
||||
spikes (up to ~50% local contamination) while passing smooth, genuine
|
||||
deflection through almost unchanged. Points whose height deviates from the
|
||||
median baseline by more than ``sigma`` robust standard deviations are
|
||||
rejected before the spline fit.
|
||||
"""
|
||||
n = len(x)
|
||||
keep = np.ones(n, dtype=bool)
|
||||
if n < 6:
|
||||
return keep
|
||||
baseline = np.empty(n, dtype=np.float64)
|
||||
for index in range(n):
|
||||
local = z[(x >= x[index] - window_mm) & (x <= x[index] + window_mm)]
|
||||
baseline[index] = float(np.median(local))
|
||||
residual = z - baseline
|
||||
mad = float(np.median(np.abs(residual - np.median(residual))))
|
||||
scale = 1.4826 * mad + 1e-6
|
||||
keep = np.abs(residual) <= sigma * scale
|
||||
if np.count_nonzero(keep) < 4:
|
||||
return np.ones(n, dtype=bool)
|
||||
return keep
|
||||
|
||||
|
||||
def fit_representative_line(
|
||||
representatives: list[RepresentativePoint],
|
||||
config: dict,
|
||||
side: str,
|
||||
) -> FittedLine | None:
|
||||
if len(representatives) < 4:
|
||||
return None
|
||||
x = np.asarray([item.x for item in representatives], dtype=np.float64)
|
||||
y = np.asarray([item.y for item in representatives], dtype=np.float64)
|
||||
z = np.asarray([item.z for item in representatives], dtype=np.float64)
|
||||
order = np.argsort(x)
|
||||
x = x[order]
|
||||
y = y[order]
|
||||
z = z[order]
|
||||
reps_sorted = [representatives[index] for index in order]
|
||||
if np.any(np.diff(x) <= 0):
|
||||
unique_mask = np.ones_like(x, dtype=bool)
|
||||
unique_mask[1:] = np.diff(x) > 1e-6
|
||||
x = x[unique_mask]
|
||||
y = y[unique_mask]
|
||||
z = z[unique_mask]
|
||||
reps_sorted = [item for item, keep in zip(reps_sorted, unique_mask) if keep]
|
||||
if len(x) < 4:
|
||||
return None
|
||||
|
||||
rep_cfg = config["representative"]
|
||||
spacing = config["runtime"]["spacing_mm"]
|
||||
|
||||
if rep_cfg.get("outlier_rejection_enabled", True) and len(x) >= 6:
|
||||
window_mm = rep_cfg.get("outlier_window_slices", 2.5) * config["beam"]["slice_step_mm"]
|
||||
sigma = rep_cfg.get("outlier_sigma", 2.5)
|
||||
keep = _reject_contaminated_representatives(x, z, window_mm, sigma)
|
||||
if np.count_nonzero(keep) >= 4:
|
||||
x = x[keep]
|
||||
y = y[keep]
|
||||
z = z[keep]
|
||||
reps_sorted = [item for item, flag in zip(reps_sorted, keep) if flag]
|
||||
y_spline, y_residual = _fit_spline(
|
||||
x,
|
||||
y,
|
||||
smooth_scale=rep_cfg["spline_smoothing_y_spacing_multiplier"] * spacing,
|
||||
)
|
||||
z_spline, z_residual = _fit_spline(
|
||||
x,
|
||||
z,
|
||||
smooth_scale=rep_cfg["spline_smoothing_z_spacing_multiplier"] * spacing,
|
||||
)
|
||||
max_gap = rep_cfg["max_gap_slices"] * config["beam"]["slice_step_mm"]
|
||||
gaps: list[tuple[float, float]] = []
|
||||
for x0, x1 in zip(x[:-1], x[1:]):
|
||||
if (x1 - x0) > max_gap:
|
||||
gaps.append((float(x0), float(x1)))
|
||||
low_confidence_segments = list(gaps)
|
||||
return FittedLine(
|
||||
side=side,
|
||||
x_support=x,
|
||||
y_support=y,
|
||||
z_support=z,
|
||||
y_spline=y_spline,
|
||||
z_spline=z_spline,
|
||||
y_residual_rms=y_residual,
|
||||
z_residual_rms=z_residual,
|
||||
low_confidence_segments=low_confidence_segments,
|
||||
gaps=gaps,
|
||||
)
|
||||
|
||||
|
||||
def evaluate_line(line: FittedLine, x_grid: np.ndarray) -> tuple[np.ndarray, np.ndarray, np.ndarray]:
|
||||
y = line.y_spline(x_grid)
|
||||
z = line.z_spline(x_grid)
|
||||
confidence = np.ones_like(x_grid, dtype=np.float64)
|
||||
support_min = float(line.x_support.min())
|
||||
support_max = float(line.x_support.max())
|
||||
outside = (x_grid < support_min) | (x_grid > support_max)
|
||||
confidence[outside] = 0.0
|
||||
for start, end in line.low_confidence_segments:
|
||||
mask = (x_grid >= start) & (x_grid <= end)
|
||||
confidence[mask] = np.minimum(confidence[mask], 0.25)
|
||||
return y, z, confidence
|
||||
|
||||
|
||||
def representatives_to_rows(representatives: list[RepresentativePoint]) -> list[dict]:
|
||||
return [asdict(item) for item in representatives]
|
||||
335
alg/delivery_v1.2.1/underframe_deflection/frame.py
Normal file
335
alg/delivery_v1.2.1/underframe_deflection/frame.py
Normal file
@@ -0,0 +1,335 @@
|
||||
from __future__ import annotations
|
||||
|
||||
from dataclasses import asdict
|
||||
|
||||
import numpy as np
|
||||
from scipy.ndimage import gaussian_filter1d
|
||||
|
||||
from .structures import FrameHypothesis
|
||||
|
||||
|
||||
def normalize(vector: np.ndarray) -> np.ndarray:
|
||||
norm = float(np.linalg.norm(vector))
|
||||
if norm == 0:
|
||||
return vector.copy()
|
||||
return vector / norm
|
||||
|
||||
|
||||
def orthonormalize_axes(x_axis: np.ndarray, z_axis_seed: np.ndarray) -> np.ndarray:
|
||||
x_axis = normalize(x_axis)
|
||||
z_axis_seed = normalize(z_axis_seed)
|
||||
y_axis = normalize(np.cross(z_axis_seed, x_axis))
|
||||
z_axis = normalize(np.cross(x_axis, y_axis))
|
||||
return np.vstack([x_axis, y_axis, z_axis])
|
||||
|
||||
|
||||
def transform_points(points: np.ndarray, axes: np.ndarray, origin: np.ndarray) -> np.ndarray:
|
||||
return (points - origin) @ axes.T
|
||||
|
||||
|
||||
def transform_vectors(vectors: np.ndarray, axes: np.ndarray) -> np.ndarray:
|
||||
return vectors @ axes.T
|
||||
|
||||
|
||||
def generate_frame_hypotheses(points: np.ndarray) -> tuple[np.ndarray, list[np.ndarray]]:
|
||||
centered = points - points.mean(axis=0, keepdims=True)
|
||||
covariance = centered.T @ centered / max(len(points), 1)
|
||||
eigenvalues, eigenvectors = np.linalg.eigh(covariance)
|
||||
order = np.argsort(eigenvalues)[::-1]
|
||||
eigenvectors = eigenvectors[:, order]
|
||||
primary = eigenvectors[:, 0]
|
||||
residual_axes = [eigenvectors[:, 1], eigenvectors[:, 2]]
|
||||
return primary, residual_axes
|
||||
|
||||
|
||||
def _contiguous_ranges(mask: np.ndarray) -> list[tuple[int, int]]:
|
||||
ranges: list[tuple[int, int]] = []
|
||||
start = None
|
||||
for idx, value in enumerate(mask):
|
||||
if value and start is None:
|
||||
start = idx
|
||||
elif not value and start is not None:
|
||||
ranges.append((start, idx - 1))
|
||||
start = None
|
||||
if start is not None:
|
||||
ranges.append((start, len(mask) - 1))
|
||||
return ranges
|
||||
|
||||
|
||||
def _score_frame_hypothesis(
|
||||
coords: np.ndarray,
|
||||
normals: np.ndarray,
|
||||
curvature: np.ndarray,
|
||||
config: dict,
|
||||
) -> tuple[float, float, float, float, float, float]:
|
||||
frame_cfg = config["frame"]
|
||||
coarse_slice_count = frame_cfg["coarse_slice_count"]
|
||||
x_min, x_max = float(coords[:, 0].min()), float(coords[:, 0].max())
|
||||
y_min, y_max = float(coords[:, 1].min()), float(coords[:, 1].max())
|
||||
width = max(y_max - y_min, 1e-6)
|
||||
slice_edges = np.linspace(x_min, x_max, coarse_slice_count + 1)
|
||||
flat_thresh = frame_cfg["flatness_mm"]
|
||||
|
||||
bottom_scores: list[float] = []
|
||||
symmetry_scores: list[float] = []
|
||||
flatness_scores: list[float] = []
|
||||
continuity_flags: list[float] = []
|
||||
hole_scores: list[float] = []
|
||||
|
||||
global_curvature_scale = float(np.quantile(curvature, 0.75) + 1e-6)
|
||||
|
||||
for slice_index in range(coarse_slice_count):
|
||||
mask = (coords[:, 0] >= slice_edges[slice_index]) & (
|
||||
coords[:, 0] < slice_edges[slice_index + 1]
|
||||
)
|
||||
slice_points = coords[mask]
|
||||
if len(slice_points) < frame_cfg["candidate_bin_min_points"] * 6:
|
||||
continue
|
||||
slice_normals = normals[mask]
|
||||
slice_curvature = curvature[mask]
|
||||
trim_q = 0.02
|
||||
y_lo, y_hi = np.quantile(slice_points[:, 1], [trim_q, 1.0 - trim_q])
|
||||
y_mid = 0.5 * (y_lo + y_hi)
|
||||
y_edges = np.linspace(y_lo, y_hi, frame_cfg["coarse_y_bins"] + 1)
|
||||
|
||||
low_z = np.full(frame_cfg["coarse_y_bins"], np.nan)
|
||||
flatness = np.full(frame_cfg["coarse_y_bins"], np.nan)
|
||||
normal_alignment = np.full(frame_cfg["coarse_y_bins"], np.nan)
|
||||
counts = np.zeros(frame_cfg["coarse_y_bins"], dtype=np.int32)
|
||||
curvature_score = np.zeros(frame_cfg["coarse_y_bins"], dtype=np.float64)
|
||||
|
||||
for bin_index in range(frame_cfg["coarse_y_bins"]):
|
||||
bin_mask = (slice_points[:, 1] >= y_edges[bin_index]) & (
|
||||
slice_points[:, 1] < y_edges[bin_index + 1]
|
||||
)
|
||||
counts[bin_index] = int(np.count_nonzero(bin_mask))
|
||||
if counts[bin_index] < frame_cfg["candidate_bin_min_points"]:
|
||||
continue
|
||||
bin_points = slice_points[bin_mask]
|
||||
z_threshold = np.quantile(bin_points[:, 2], frame_cfg["bottom_quantile"])
|
||||
low_mask = bin_points[:, 2] <= z_threshold
|
||||
low_points = bin_points[low_mask]
|
||||
low_z[bin_index] = float(np.median(low_points[:, 2]))
|
||||
flatness[bin_index] = float(
|
||||
np.median(np.abs(low_points[:, 2] - low_z[bin_index]))
|
||||
)
|
||||
normal_alignment[bin_index] = float(
|
||||
np.mean(np.abs(slice_normals[bin_mask][low_mask, 2]))
|
||||
)
|
||||
curvature_score[bin_index] = float(
|
||||
np.mean(slice_curvature[bin_mask][low_mask]) / global_curvature_scale
|
||||
)
|
||||
|
||||
valid = ~np.isnan(low_z)
|
||||
if valid.sum() < 4:
|
||||
continue
|
||||
smoothed_low_z = low_z.copy()
|
||||
smoothed_low_z[valid] = gaussian_filter1d(low_z[valid], sigma=1.0)
|
||||
z_cut = float(
|
||||
np.quantile(smoothed_low_z[valid], 0.25) + frame_cfg["low_band_margin_mm"]
|
||||
)
|
||||
candidate_mask = (
|
||||
valid
|
||||
& (smoothed_low_z <= z_cut)
|
||||
& (flatness <= flat_thresh)
|
||||
& (normal_alignment >= frame_cfg["normal_alignment_min"])
|
||||
)
|
||||
groups = _contiguous_ranges(candidate_mask)
|
||||
if not groups:
|
||||
continue
|
||||
|
||||
group_summaries = []
|
||||
for start, end in groups:
|
||||
y_center = 0.5 * (y_edges[start] + y_edges[end + 1])
|
||||
group_summaries.append(
|
||||
{
|
||||
"start": start,
|
||||
"end": end,
|
||||
"y_center": y_center,
|
||||
"z_low": float(np.nanmedian(smoothed_low_z[start : end + 1])),
|
||||
"flatness": float(np.nanmean(flatness[start : end + 1])),
|
||||
"normal": float(np.nanmean(normal_alignment[start : end + 1])),
|
||||
"count": int(np.sum(counts[start : end + 1])),
|
||||
"curvature": float(np.nanmean(curvature_score[start : end + 1])),
|
||||
}
|
||||
)
|
||||
|
||||
left = [group for group in group_summaries if group["y_center"] < y_mid]
|
||||
right = [group for group in group_summaries if group["y_center"] >= y_mid]
|
||||
if not left or not right:
|
||||
continuity_flags.append(0.35)
|
||||
continue
|
||||
left_best = min(left, key=lambda item: item["z_low"])
|
||||
right_best = min(right, key=lambda item: item["z_low"])
|
||||
|
||||
slice_bottom_ref = float(np.quantile(slice_points[:, 2], 0.15))
|
||||
low_scale = max(np.std(slice_points[:, 2]), 1.0)
|
||||
bottom_scores.append(
|
||||
np.clip(
|
||||
((slice_bottom_ref - left_best["z_low"]) + (slice_bottom_ref - right_best["z_low"]))
|
||||
/ (2.0 * low_scale)
|
||||
+ 1.0,
|
||||
0.0,
|
||||
2.0,
|
||||
)
|
||||
)
|
||||
sym_error = abs(
|
||||
abs(left_best["y_center"] - y_mid) - abs(right_best["y_center"] - y_mid)
|
||||
) / max(width * 0.5, 1.0)
|
||||
symmetry_scores.append(float(np.clip(1.0 - sym_error, 0.0, 1.0)))
|
||||
flatness_scores.append(
|
||||
float(
|
||||
np.clip(
|
||||
(
|
||||
(1.0 - left_best["flatness"] / max(flat_thresh, 1e-6))
|
||||
+ (1.0 - right_best["flatness"] / max(flat_thresh, 1e-6))
|
||||
)
|
||||
/ 2.0
|
||||
* (left_best["normal"] + right_best["normal"])
|
||||
/ 2.0,
|
||||
0.0,
|
||||
1.5,
|
||||
)
|
||||
)
|
||||
)
|
||||
continuity_flags.append(1.0)
|
||||
hole_scores.append(
|
||||
float(
|
||||
np.clip(
|
||||
(left_best["curvature"] + right_best["curvature"]) / 2.0,
|
||||
0.0,
|
||||
1.5,
|
||||
)
|
||||
)
|
||||
)
|
||||
|
||||
if not continuity_flags:
|
||||
return (0.0, 0.0, 0.0, 0.0, 0.0, 0.0)
|
||||
|
||||
bottom_score = float(np.mean(bottom_scores)) if bottom_scores else 0.0
|
||||
symmetry_score = float(np.mean(symmetry_scores)) if symmetry_scores else 0.0
|
||||
flatness_score = float(np.mean(flatness_scores)) if flatness_scores else 0.0
|
||||
hole_score = float(np.mean(hole_scores)) if hole_scores else 0.0
|
||||
continuity_score = float(np.mean(continuity_flags))
|
||||
|
||||
total_score = (
|
||||
frame_cfg["bottom_weight"] * bottom_score
|
||||
+ frame_cfg["symmetry_weight"] * symmetry_score
|
||||
+ frame_cfg["flatness_weight"] * flatness_score
|
||||
+ frame_cfg["hole_weight"] * hole_score
|
||||
+ continuity_score
|
||||
)
|
||||
return (
|
||||
total_score,
|
||||
bottom_score,
|
||||
symmetry_score,
|
||||
flatness_score,
|
||||
hole_score,
|
||||
continuity_score,
|
||||
)
|
||||
|
||||
|
||||
def infer_vehicle_frame(
|
||||
points: np.ndarray,
|
||||
normals: np.ndarray,
|
||||
curvature: np.ndarray,
|
||||
config: dict,
|
||||
) -> tuple[list[FrameHypothesis], FrameHypothesis]:
|
||||
origin = points.mean(axis=0)
|
||||
primary_axis, residual_axes = generate_frame_hypotheses(points)
|
||||
hypotheses: list[FrameHypothesis] = []
|
||||
for x_sign in (-1.0, 1.0):
|
||||
for z_base_index, z_base in enumerate(residual_axes):
|
||||
for z_sign in (-1.0, 1.0):
|
||||
axes = orthonormalize_axes(x_sign * primary_axis, z_sign * z_base)
|
||||
coords = transform_points(points, axes, origin)
|
||||
transformed_normals = transform_vectors(normals, axes)
|
||||
score_tuple = _score_frame_hypothesis(
|
||||
coords,
|
||||
transformed_normals,
|
||||
curvature,
|
||||
config,
|
||||
)
|
||||
name = f"x{x_sign:+.0f}_z{z_base_index + 2}{z_sign:+.0f}"
|
||||
hypotheses.append(
|
||||
FrameHypothesis(
|
||||
name=name,
|
||||
axes=axes,
|
||||
origin=origin,
|
||||
score=score_tuple[0],
|
||||
bottom_score=score_tuple[1],
|
||||
symmetry_score=score_tuple[2],
|
||||
flatness_score=score_tuple[3],
|
||||
hole_score=score_tuple[4],
|
||||
continuity_score=score_tuple[5],
|
||||
)
|
||||
)
|
||||
hypotheses.sort(key=lambda item: item.score, reverse=True)
|
||||
return hypotheses, hypotheses[0]
|
||||
|
||||
|
||||
def build_bottom_roi_mask(points_vehicle: np.ndarray, config: dict) -> tuple[np.ndarray, dict]:
|
||||
roi_cfg = config["roi"]
|
||||
aabb = roi_cfg.get("aabb") if isinstance(roi_cfg, dict) else None
|
||||
z_floor = float(points_vehicle[:, 2].min())
|
||||
if aabb is not None:
|
||||
x_lo = float(aabb["x_min"])
|
||||
x_hi = float(aabb["x_max"])
|
||||
y_lo = float(aabb["y_min"])
|
||||
y_hi = float(aabb["y_max"])
|
||||
z_lo = float(aabb["z_min"])
|
||||
z_ceiling = float(aabb["z_max"])
|
||||
mask = (
|
||||
(points_vehicle[:, 0] >= x_lo)
|
||||
& (points_vehicle[:, 0] <= x_hi)
|
||||
& (points_vehicle[:, 1] >= y_lo)
|
||||
& (points_vehicle[:, 1] <= y_hi)
|
||||
& (points_vehicle[:, 2] >= z_lo)
|
||||
& (points_vehicle[:, 2] <= z_ceiling)
|
||||
)
|
||||
payload = {
|
||||
"source": "external_aabb",
|
||||
"x_min": x_lo,
|
||||
"x_max": x_hi,
|
||||
"y_min": y_lo,
|
||||
"y_max": y_hi,
|
||||
"z_min": z_lo,
|
||||
"z_floor": z_floor,
|
||||
"z_ceiling": z_ceiling,
|
||||
"point_count": int(np.count_nonzero(mask)),
|
||||
}
|
||||
return mask, payload
|
||||
|
||||
x_lo = float(np.quantile(points_vehicle[:, 0], roi_cfg["x_trim_quantile"]))
|
||||
x_hi = float(np.quantile(points_vehicle[:, 0], 1.0 - roi_cfg["x_trim_quantile"]))
|
||||
z_quantile = float(np.quantile(points_vehicle[:, 2], roi_cfg["bottom_quantile"]))
|
||||
z_span = float(points_vehicle[:, 2].max() - z_floor)
|
||||
z_ceiling = min(
|
||||
z_quantile + roi_cfg["z_margin_mm"],
|
||||
z_floor + roi_cfg["bottom_height_ratio"] * z_span,
|
||||
)
|
||||
z_ceiling = max(z_ceiling, roi_cfg["min_z_ceiling_mm"])
|
||||
mask = (
|
||||
(points_vehicle[:, 0] >= x_lo)
|
||||
& (points_vehicle[:, 0] <= x_hi)
|
||||
& (points_vehicle[:, 2] <= z_ceiling)
|
||||
)
|
||||
payload = {
|
||||
"source": "auto",
|
||||
"x_min": x_lo,
|
||||
"x_max": x_hi,
|
||||
"z_floor": z_floor,
|
||||
"z_ceiling": z_ceiling,
|
||||
"point_count": int(np.count_nonzero(mask)),
|
||||
}
|
||||
return mask, payload
|
||||
|
||||
|
||||
def frame_hypotheses_to_dict(hypotheses: list[FrameHypothesis]) -> list[dict]:
|
||||
payload = []
|
||||
for hypothesis in hypotheses:
|
||||
item = asdict(hypothesis)
|
||||
item["axes"] = hypothesis.axes.tolist()
|
||||
item["origin"] = hypothesis.origin.tolist()
|
||||
payload.append(item)
|
||||
return payload
|
||||
278
alg/delivery_v1.2.1/underframe_deflection/io.py
Normal file
278
alg/delivery_v1.2.1/underframe_deflection/io.py
Normal file
@@ -0,0 +1,278 @@
|
||||
from __future__ import annotations
|
||||
|
||||
import csv
|
||||
import json
|
||||
from pathlib import Path
|
||||
from typing import Iterable
|
||||
|
||||
import numpy as np
|
||||
|
||||
|
||||
def read_point_cloud(path: str | Path) -> np.ndarray:
|
||||
path = Path(path)
|
||||
suffix = path.suffix.lower()
|
||||
if suffix == ".ply":
|
||||
return _read_ply(path)
|
||||
if suffix in {".xyz", ".txt", ".csv"}:
|
||||
return np.loadtxt(path, dtype=np.float64)
|
||||
raise ValueError(f"Unsupported point cloud format: {path.suffix}")
|
||||
|
||||
|
||||
# PLY scalar property type -> numpy dtype code.
|
||||
_PLY_TYPE_TO_NP = {
|
||||
"char": "i1",
|
||||
"int8": "i1",
|
||||
"uchar": "u1",
|
||||
"uint8": "u1",
|
||||
"short": "i2",
|
||||
"int16": "i2",
|
||||
"ushort": "u2",
|
||||
"uint16": "u2",
|
||||
"int": "i4",
|
||||
"int32": "i4",
|
||||
"uint": "u4",
|
||||
"uint32": "u4",
|
||||
"float": "f4",
|
||||
"float32": "f4",
|
||||
"double": "f8",
|
||||
"float64": "f8",
|
||||
}
|
||||
|
||||
|
||||
def _parse_ply_header(
|
||||
path: Path,
|
||||
) -> tuple[str, int, int, int, list[tuple[str, str]]]:
|
||||
"""Parse a PLY header.
|
||||
|
||||
Returns ``(format, header_lines, header_bytes, vertex_count,
|
||||
vertex_props)`` where ``format`` is one of ``ascii`` /
|
||||
``binary_little_endian`` / ``binary_big_endian`` and ``vertex_props`` is
|
||||
the ordered ``(type, name)`` list for the vertex element. The file is
|
||||
read in binary so the byte offset of the data block is exact (needed for
|
||||
binary PLY).
|
||||
"""
|
||||
header_lines = 0
|
||||
header_bytes = 0
|
||||
vertex_count: int | None = None
|
||||
vertex_props: list[tuple[str, str]] = []
|
||||
in_vertex = False
|
||||
fmt = "ascii"
|
||||
with path.open("rb") as fh:
|
||||
first = fh.readline()
|
||||
header_bytes += len(first)
|
||||
header_lines += 1
|
||||
if first.strip().lower() != b"ply":
|
||||
raise ValueError(f"Not a PLY file (missing 'ply' magic): {path}")
|
||||
while True:
|
||||
raw = fh.readline()
|
||||
if not raw:
|
||||
raise ValueError(f"PLY header missing end_header: {path}")
|
||||
header_bytes += len(raw)
|
||||
header_lines += 1
|
||||
stripped = raw.decode("ascii", errors="replace").strip()
|
||||
if not stripped:
|
||||
continue
|
||||
tokens = stripped.split()
|
||||
keyword = tokens[0].lower()
|
||||
if keyword == "format":
|
||||
fmt = tokens[1].lower()
|
||||
elif keyword == "element":
|
||||
in_vertex = tokens[1].lower() == "vertex"
|
||||
if in_vertex:
|
||||
vertex_count = int(tokens[2])
|
||||
elif keyword == "property" and in_vertex:
|
||||
if tokens[1].lower() == "list":
|
||||
vertex_props.append(("list", tokens[-1]))
|
||||
else:
|
||||
vertex_props.append((tokens[1].lower(), tokens[-1]))
|
||||
elif keyword == "end_header":
|
||||
break
|
||||
if vertex_count is None:
|
||||
raise ValueError(f"PLY file has no vertex element: {path}")
|
||||
return fmt, header_lines, header_bytes, vertex_count, vertex_props
|
||||
|
||||
|
||||
def _read_ply(path: Path) -> np.ndarray:
|
||||
"""Read a PLY point cloud (ASCII or binary) into an ``(N, 3)`` array."""
|
||||
fmt, header_lines, header_bytes, vertex_count, vertex_props = _parse_ply_header(path)
|
||||
if fmt == "ascii":
|
||||
points = np.loadtxt(path, skiprows=header_lines, dtype=np.float64)
|
||||
if points.ndim == 1:
|
||||
points = points[None, :]
|
||||
if points.shape[1] < 3:
|
||||
raise ValueError(f"PLY file does not contain XYZ coordinates: {path}")
|
||||
points = points[:, :3]
|
||||
if len(points) != vertex_count:
|
||||
raise ValueError(
|
||||
f"PLY vertex count mismatch: header={vertex_count}, data={len(points)}"
|
||||
)
|
||||
return points
|
||||
if fmt in {"binary_little_endian", "binary_big_endian"}:
|
||||
return _read_binary_ply(path, header_bytes, vertex_count, vertex_props, fmt)
|
||||
raise ValueError(f"Unsupported PLY format '{fmt}': {path}")
|
||||
|
||||
|
||||
def _read_binary_ply(
|
||||
path: Path,
|
||||
header_bytes: int,
|
||||
vertex_count: int,
|
||||
vertex_props: list[tuple[str, str]],
|
||||
fmt: str,
|
||||
) -> np.ndarray:
|
||||
if any(ptype == "list" for ptype, _ in vertex_props):
|
||||
raise ValueError(
|
||||
f"Binary PLY with list properties in the vertex element is unsupported: {path}"
|
||||
)
|
||||
missing = {"x", "y", "z"} - {name for _, name in vertex_props}
|
||||
if missing:
|
||||
raise ValueError(
|
||||
f"PLY vertex element missing coordinate field(s) {sorted(missing)}: {path}"
|
||||
)
|
||||
endian = "<" if fmt == "binary_little_endian" else ">"
|
||||
try:
|
||||
record_dtype = np.dtype(
|
||||
[(name, endian + _PLY_TYPE_TO_NP[ptype]) for ptype, name in vertex_props]
|
||||
)
|
||||
except KeyError as exc:
|
||||
raise ValueError(f"Unsupported PLY property type {exc} in {path}") from exc
|
||||
expected = record_dtype.itemsize * vertex_count
|
||||
with path.open("rb") as fh:
|
||||
fh.seek(header_bytes)
|
||||
buffer = fh.read(expected)
|
||||
if len(buffer) < expected:
|
||||
raise ValueError(
|
||||
f"Binary PLY truncated: expected {expected} bytes of vertex data, "
|
||||
f"got {len(buffer)}: {path}"
|
||||
)
|
||||
records = np.frombuffer(buffer, dtype=record_dtype, count=vertex_count)
|
||||
return np.column_stack([records["x"], records["y"], records["z"]]).astype(np.float64)
|
||||
|
||||
|
||||
# Backwards-compatible alias. ``cad_io.py`` imports this name; it now reads
|
||||
# both ASCII and binary PLY transparently.
|
||||
_read_ascii_ply = _read_ply
|
||||
|
||||
|
||||
def read_point_cloud_cropped(
|
||||
path: str | Path,
|
||||
aabb: dict,
|
||||
*,
|
||||
chunk_vertices: int = 4_000_000,
|
||||
) -> np.ndarray:
|
||||
"""Stream a (possibly huge) PLY and return only points inside ``aabb``.
|
||||
|
||||
For multi-GB clouds (hundreds of millions / billions of points) loading
|
||||
the whole file is infeasible. This reads the vertex block in chunks,
|
||||
keeps only points within the axis-aligned bbox, and concatenates the
|
||||
survivors. Falls back to a full read + mask for ASCII / text formats.
|
||||
|
||||
``aabb`` keys: ``x_min/x_max/y_min/y_max/z_min/z_max`` (same frame as the
|
||||
file). Returns an ``(M, 3) float64`` array.
|
||||
"""
|
||||
path = Path(path)
|
||||
suffix = path.suffix.lower()
|
||||
bounds = (
|
||||
float(aabb["x_min"]), float(aabb["x_max"]),
|
||||
float(aabb["y_min"]), float(aabb["y_max"]),
|
||||
float(aabb["z_min"]), float(aabb["z_max"]),
|
||||
)
|
||||
if suffix != ".ply":
|
||||
pts = read_point_cloud(path)
|
||||
return _crop_points(pts, bounds)
|
||||
|
||||
fmt, _, header_bytes, vertex_count, vertex_props = _parse_ply_header(path)
|
||||
if fmt == "ascii":
|
||||
pts = _read_ply(path)
|
||||
return _crop_points(pts, bounds)
|
||||
if fmt not in {"binary_little_endian", "binary_big_endian"}:
|
||||
raise ValueError(f"Unsupported PLY format '{fmt}': {path}")
|
||||
if any(ptype == "list" for ptype, _ in vertex_props):
|
||||
raise ValueError(f"Binary PLY with list vertex properties unsupported: {path}")
|
||||
missing = {"x", "y", "z"} - {name for _, name in vertex_props}
|
||||
if missing:
|
||||
raise ValueError(f"PLY missing coordinate field(s) {sorted(missing)}: {path}")
|
||||
|
||||
endian = "<" if fmt == "binary_little_endian" else ">"
|
||||
record_dtype = np.dtype(
|
||||
[(name, endian + _PLY_TYPE_TO_NP[ptype]) for ptype, name in vertex_props]
|
||||
)
|
||||
record_bytes = record_dtype.itemsize
|
||||
kept_chunks: list[np.ndarray] = []
|
||||
with path.open("rb") as fh:
|
||||
fh.seek(header_bytes)
|
||||
remaining = vertex_count
|
||||
while remaining > 0:
|
||||
n = min(chunk_vertices, remaining)
|
||||
buf = fh.read(n * record_bytes)
|
||||
if len(buf) < n * record_bytes:
|
||||
n = len(buf) // record_bytes
|
||||
if n == 0:
|
||||
break
|
||||
buf = buf[: n * record_bytes]
|
||||
recs = np.frombuffer(buf, dtype=record_dtype, count=n)
|
||||
xyz = np.column_stack(
|
||||
[recs["x"], recs["y"], recs["z"]]
|
||||
).astype(np.float64)
|
||||
crop = _crop_points(xyz, bounds)
|
||||
if len(crop):
|
||||
kept_chunks.append(crop)
|
||||
remaining -= n
|
||||
if not kept_chunks:
|
||||
return np.zeros((0, 3), dtype=np.float64)
|
||||
return np.concatenate(kept_chunks, axis=0)
|
||||
|
||||
|
||||
def _crop_points(points: np.ndarray, bounds: tuple) -> np.ndarray:
|
||||
x_lo, x_hi, y_lo, y_hi, z_lo, z_hi = bounds
|
||||
mask = (
|
||||
(points[:, 0] >= x_lo) & (points[:, 0] <= x_hi)
|
||||
& (points[:, 1] >= y_lo) & (points[:, 1] <= y_hi)
|
||||
& (points[:, 2] >= z_lo) & (points[:, 2] <= z_hi)
|
||||
)
|
||||
return points[mask]
|
||||
|
||||
|
||||
def ensure_output_dir(path: str | Path) -> Path:
|
||||
out_dir = Path(path)
|
||||
out_dir.mkdir(parents=True, exist_ok=True)
|
||||
return out_dir
|
||||
|
||||
|
||||
def write_json(path: str | Path, payload: dict) -> None:
|
||||
Path(path).write_text(
|
||||
json.dumps(payload, ensure_ascii=False, indent=2),
|
||||
encoding="utf-8",
|
||||
)
|
||||
|
||||
|
||||
def write_csv(path: str | Path, fieldnames: list[str], rows: Iterable[dict]) -> None:
|
||||
with Path(path).open("w", encoding="utf-8", newline="") as fh:
|
||||
writer = csv.DictWriter(fh, fieldnames=fieldnames)
|
||||
writer.writeheader()
|
||||
for row in rows:
|
||||
writer.writerow(row)
|
||||
|
||||
|
||||
def write_colored_ply(
|
||||
path: str | Path,
|
||||
points: np.ndarray,
|
||||
colors: np.ndarray,
|
||||
) -> None:
|
||||
path = Path(path)
|
||||
if len(points) != len(colors):
|
||||
raise ValueError("Point/color length mismatch for PLY export.")
|
||||
points = np.asarray(points, dtype=np.float64)
|
||||
colors = np.asarray(colors, dtype=np.uint8)
|
||||
with path.open("w", encoding="ascii", newline="\n") as fh:
|
||||
fh.write("ply\n")
|
||||
fh.write("format ascii 1.0\n")
|
||||
fh.write(f"element vertex {len(points)}\n")
|
||||
fh.write("property float x\n")
|
||||
fh.write("property float y\n")
|
||||
fh.write("property float z\n")
|
||||
fh.write("property uchar red\n")
|
||||
fh.write("property uchar green\n")
|
||||
fh.write("property uchar blue\n")
|
||||
fh.write("end_header\n")
|
||||
for (x, y, z), (r, g, b) in zip(points, colors, strict=True):
|
||||
fh.write(f"{x:.6f} {y:.6f} {z:.6f} {int(r)} {int(g)} {int(b)}\n")
|
||||
288
alg/delivery_v1.2.1/underframe_deflection/pipeline.py
Normal file
288
alg/delivery_v1.2.1/underframe_deflection/pipeline.py
Normal file
@@ -0,0 +1,288 @@
|
||||
"""High-level convenience: read inputs, compute, write all artifacts.
|
||||
|
||||
The pipeline now accepts three optional companion inputs in addition to
|
||||
the scan point cloud:
|
||||
|
||||
- ``cad_path``: CAD reference geometry (PLY/XYZ/STL/OBJ) used as the
|
||||
baseline coordinate frame; the scan is rigidly aligned to it.
|
||||
- ``roi_path``: JSON file holding an axis-aligned bbox (in the CAD
|
||||
frame) that replaces the automatic ROI selection.
|
||||
- ``--skip-excel`` / ``skip_excel``: opt out of the Excel artifact.
|
||||
|
||||
When all of these are absent the pipeline behaves like the legacy
|
||||
single-cloud workflow.
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import json
|
||||
import logging
|
||||
from pathlib import Path
|
||||
from typing import Any
|
||||
|
||||
import numpy as np
|
||||
|
||||
from .api import DeflectionResult, compute_deflection, load_point_cloud
|
||||
from .artifacts import (
|
||||
export_overlay_plys,
|
||||
render_plots,
|
||||
write_beam_tracks_csv,
|
||||
write_curves_csv,
|
||||
write_report_json,
|
||||
)
|
||||
from .cad_io import read_cad_reference
|
||||
from .config import load_config, resolve_numeric_parameters, cloud_geometry
|
||||
from .io import ensure_output_dir
|
||||
from .preprocess import estimate_spacing, preprocess_point_cloud
|
||||
from .registration import RegistrationResult, apply_transform, register_to_cad
|
||||
from .roi import load_roi_aabb
|
||||
|
||||
|
||||
log = logging.getLogger(__name__)
|
||||
|
||||
|
||||
def _swap_y_z(points: np.ndarray) -> np.ndarray:
|
||||
"""Reorder columns x,y,z -> x,z,y.
|
||||
|
||||
Bridges between the CAD/scan convention (``y`` = height) and the
|
||||
algorithm's canonical frame (``z`` = height). Pure column permutation;
|
||||
no values are altered.
|
||||
"""
|
||||
return points[:, [0, 2, 1]]
|
||||
|
||||
|
||||
def _swap_roi_y_z(roi: dict) -> dict:
|
||||
"""Swap the y/z bounds of an AABB to match :func:`_swap_y_z`."""
|
||||
return {
|
||||
"x_min": roi["x_min"], "x_max": roi["x_max"],
|
||||
"y_min": roi["z_min"], "y_max": roi["z_max"],
|
||||
"z_min": roi["y_min"], "z_max": roi["y_max"],
|
||||
}
|
||||
|
||||
|
||||
def run_measurement_pipeline(
|
||||
input_cloud: str | Path,
|
||||
out_dir: str | Path,
|
||||
config_path: str | None = None,
|
||||
profile: str = "auto",
|
||||
*,
|
||||
cad_path: str | Path | None = None,
|
||||
roi_path: str | Path | None = None,
|
||||
skip_registration: bool = False,
|
||||
enable_bilateral: bool = True,
|
||||
skip_excel: bool = False,
|
||||
input_axis: str = "y_up",
|
||||
) -> dict:
|
||||
"""Read inputs, run the algorithm, and write every artifact.
|
||||
|
||||
Returns the same dict as ``DeflectionResult.to_dict()``; it is also
|
||||
persisted to ``<out_dir>/report.json``. CSV files land in
|
||||
``out_dir`` directly, plots in ``out_dir/images/``, PLY overlays in
|
||||
``out_dir/ply/`` and (unless suppressed) the Excel error report in
|
||||
``out_dir/report.xlsx``.
|
||||
"""
|
||||
out_dir_path = ensure_output_dir(out_dir)
|
||||
|
||||
scan_raw = load_point_cloud(input_cloud)
|
||||
|
||||
user_config: dict[str, Any] = {}
|
||||
if config_path:
|
||||
user_config = json.loads(Path(config_path).read_text(encoding="utf-8"))
|
||||
|
||||
roi_aabb: dict[str, float] | None = None
|
||||
if roi_path is not None:
|
||||
roi_aabb = load_roi_aabb(roi_path)
|
||||
|
||||
if not enable_bilateral:
|
||||
user_config.setdefault("preprocess", {}).setdefault("bilateral", {})["enabled"] = False
|
||||
|
||||
cad_points: np.ndarray | None = None
|
||||
if cad_path is not None:
|
||||
cad_points = read_cad_reference(cad_path)
|
||||
|
||||
if input_axis == "y_up":
|
||||
# Bridge from the CAD/scan convention (y = height) into the
|
||||
# algorithm's canonical frame (z = height). The core algorithm is
|
||||
# unchanged; this is a pure input adapter at the I/O boundary.
|
||||
scan_raw = _swap_y_z(scan_raw)
|
||||
if cad_points is not None:
|
||||
cad_points = _swap_y_z(cad_points)
|
||||
if roi_aabb is not None:
|
||||
roi_aabb = _swap_roi_y_z(roi_aabb)
|
||||
elif input_axis != "z_up":
|
||||
raise ValueError(f"input_axis must be 'z_up' or 'y_up', got {input_axis!r}")
|
||||
|
||||
registration_meta: dict[str, Any] = {
|
||||
"enabled": cad_points is not None and not skip_registration,
|
||||
"skipped": cad_points is not None and skip_registration,
|
||||
"performed": False,
|
||||
}
|
||||
registration_result: RegistrationResult | None = None
|
||||
|
||||
scan_for_compute = scan_raw
|
||||
if cad_points is not None and not skip_registration:
|
||||
registration_result, scan_for_compute = _register_scan(
|
||||
scan_raw,
|
||||
cad_points,
|
||||
profile,
|
||||
user_config,
|
||||
roi_aabb,
|
||||
)
|
||||
registration_meta.update(
|
||||
performed=True,
|
||||
rms_mm=registration_result.rms_mm,
|
||||
initial_rms_mm=registration_result.initial_rms_mm,
|
||||
iterations=registration_result.iterations,
|
||||
inlier_ratio=registration_result.inlier_ratio,
|
||||
converged=registration_result.converged,
|
||||
R=registration_result.R.tolist(),
|
||||
t=registration_result.t.tolist(),
|
||||
)
|
||||
|
||||
result = compute_deflection(
|
||||
scan_for_compute,
|
||||
config=user_config,
|
||||
profile=profile,
|
||||
roi_aabb=roi_aabb,
|
||||
)
|
||||
|
||||
_augment_report(
|
||||
result,
|
||||
input_cloud=input_cloud,
|
||||
cad_path=cad_path,
|
||||
roi_path=roi_path,
|
||||
roi_aabb=roi_aabb,
|
||||
registration_meta=registration_meta,
|
||||
)
|
||||
|
||||
_apply_registration_quality_flags(result, registration_result)
|
||||
|
||||
write_report_json(result, out_dir_path / "report.json")
|
||||
write_beam_tracks_csv(result, out_dir_path / "beam_tracks.csv")
|
||||
write_curves_csv(result, out_dir_path / "deflection_curves.csv")
|
||||
ply_exports = export_overlay_plys(result, out_dir_path / "ply")
|
||||
result.report["ply_exports"] = ply_exports
|
||||
write_report_json(result, out_dir_path / "report.json")
|
||||
render_plots(result, out_dir_path / "images")
|
||||
|
||||
if not skip_excel:
|
||||
_write_excel_safely(
|
||||
result,
|
||||
out_dir_path / "report.xlsx",
|
||||
inputs={
|
||||
"scan_path": str(Path(input_cloud).resolve()),
|
||||
"cad_path": str(Path(cad_path).resolve()) if cad_path else "",
|
||||
"roi_path": str(Path(roi_path).resolve()) if roi_path else "",
|
||||
"config_path": str(Path(config_path).resolve()) if config_path else "",
|
||||
"profile": profile,
|
||||
},
|
||||
registration_info=registration_meta,
|
||||
)
|
||||
|
||||
return result.report
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# Internals
|
||||
# ---------------------------------------------------------------------------
|
||||
|
||||
|
||||
def _register_scan(
|
||||
scan_raw: np.ndarray,
|
||||
cad: np.ndarray,
|
||||
profile: str,
|
||||
user_config: dict[str, Any],
|
||||
roi_aabb: dict[str, float] | None,
|
||||
) -> tuple[RegistrationResult, np.ndarray]:
|
||||
"""Run a lightweight preprocess + ICP and apply the transform."""
|
||||
from .config import deep_merge
|
||||
|
||||
base_config = load_config(None, profile)
|
||||
if user_config:
|
||||
base_config = deep_merge(base_config, user_config)
|
||||
if roi_aabb is not None:
|
||||
# Avoid letting the passthrough crop the cloud before registration
|
||||
# (the AABB is expressed in the *post-registration* frame).
|
||||
base_config["preprocess"]["passthrough"] = {"enabled": False, "aabb": None}
|
||||
base_config["preprocess"]["bilateral"] = dict(
|
||||
base_config["preprocess"].get("bilateral", {})
|
||||
)
|
||||
base_config["preprocess"]["bilateral"]["enabled"] = False
|
||||
|
||||
geometry = cloud_geometry(scan_raw)
|
||||
spacing = estimate_spacing(
|
||||
scan_raw,
|
||||
sample_size=base_config["preprocess"]["spacing_sample_size"],
|
||||
)
|
||||
resolved = resolve_numeric_parameters(base_config, geometry, spacing)
|
||||
work = preprocess_point_cloud(scan_raw, resolved)
|
||||
|
||||
reg = register_to_cad(work.work_points, cad, resolved)
|
||||
scan_aligned = apply_transform(scan_raw, reg.R, reg.t)
|
||||
return reg, scan_aligned
|
||||
|
||||
|
||||
def _augment_report(
|
||||
result: DeflectionResult,
|
||||
*,
|
||||
input_cloud: str | Path,
|
||||
cad_path: str | Path | None,
|
||||
roi_path: str | Path | None,
|
||||
roi_aabb: dict[str, float] | None,
|
||||
registration_meta: dict[str, Any],
|
||||
) -> None:
|
||||
result.report["input_cloud"] = str(Path(input_cloud).resolve())
|
||||
result.report["cad_reference"] = str(Path(cad_path).resolve()) if cad_path else None
|
||||
result.report["roi_source"] = str(Path(roi_path).resolve()) if roi_path else None
|
||||
result.report["roi_aabb"] = roi_aabb
|
||||
result.report["registration"] = registration_meta
|
||||
|
||||
|
||||
def _apply_registration_quality_flags(
|
||||
result: DeflectionResult,
|
||||
registration: RegistrationResult | None,
|
||||
) -> None:
|
||||
if registration is None:
|
||||
return
|
||||
if not registration.converged:
|
||||
result.quality_flags.append("registration_not_converged")
|
||||
# Use spacing-relative threshold: NN distance between sparse clouds is
|
||||
# bounded below by half the spacing, so an absolute 10 mm gate
|
||||
# misfires on coarsely-sampled scans. Treat >2x spacing as poor.
|
||||
spacing_mm = float(
|
||||
(result.report.get("preprocess") or {}).get("spacing_mm", 0.0) or 0.0
|
||||
)
|
||||
poor_threshold = max(10.0, 2.0 * spacing_mm)
|
||||
if registration.rms_mm > poor_threshold:
|
||||
result.quality_flags.append("registration_poor_quality")
|
||||
if registration.inlier_ratio < 0.5:
|
||||
result.quality_flags.append("registration_low_inlier_ratio")
|
||||
result.report["quality_flags"] = list(result.quality_flags)
|
||||
|
||||
|
||||
def _write_excel_safely(
|
||||
result: DeflectionResult,
|
||||
path: Path,
|
||||
*,
|
||||
inputs: dict[str, Any],
|
||||
registration_info: dict[str, Any],
|
||||
) -> None:
|
||||
try:
|
||||
from .excel_report import write_excel_report
|
||||
except ImportError as exc:
|
||||
log.warning("Skipping Excel report — %s", exc)
|
||||
return
|
||||
try:
|
||||
write_excel_report(
|
||||
result,
|
||||
path,
|
||||
inputs=inputs,
|
||||
registration_info=registration_info,
|
||||
)
|
||||
except ImportError as exc:
|
||||
log.warning("Skipping Excel report — openpyxl missing (%s)", exc)
|
||||
except Exception as exc: # pragma: no cover - defensive
|
||||
log.warning("Skipping Excel report — %s", exc)
|
||||
|
||||
|
||||
__all__ = ["run_measurement_pipeline"]
|
||||
216
alg/delivery_v1.2.1/underframe_deflection/ply_overlay.py
Normal file
216
alg/delivery_v1.2.1/underframe_deflection/ply_overlay.py
Normal file
@@ -0,0 +1,216 @@
|
||||
from __future__ import annotations
|
||||
|
||||
from pathlib import Path
|
||||
|
||||
import numpy as np
|
||||
|
||||
from .fitting import evaluate_line
|
||||
from .io import write_colored_ply
|
||||
from .reference import evaluate_reference_line
|
||||
from .structures import FittedLine, ReferenceLine, TrackSolution
|
||||
|
||||
|
||||
COLOR_BASE = np.array([170, 170, 170], dtype=np.uint8)
|
||||
COLOR_LEFT_BAND = np.array([0, 114, 189], dtype=np.uint8)
|
||||
COLOR_RIGHT_BAND = np.array([217, 83, 25], dtype=np.uint8)
|
||||
COLOR_LEFT_REP_LINE = np.array([0, 220, 255], dtype=np.uint8)
|
||||
COLOR_RIGHT_REP_LINE = np.array([255, 196, 0], dtype=np.uint8)
|
||||
COLOR_LEFT_REF_LINE = np.array([0, 200, 120], dtype=np.uint8)
|
||||
COLOR_RIGHT_REF_LINE = np.array([200, 0, 160], dtype=np.uint8)
|
||||
COLOR_LEFT_MEASURE = np.array([0, 255, 120], dtype=np.uint8)
|
||||
COLOR_RIGHT_MEASURE = np.array([255, 80, 200], dtype=np.uint8)
|
||||
COLOR_CORRESPONDENCE = np.array([255, 255, 255], dtype=np.uint8)
|
||||
COLOR_ANCHOR = np.array([255, 64, 64], dtype=np.uint8)
|
||||
|
||||
|
||||
def _points_and_colors(
|
||||
points: np.ndarray,
|
||||
color: np.ndarray,
|
||||
) -> tuple[np.ndarray, np.ndarray]:
|
||||
if len(points) == 0:
|
||||
return np.zeros((0, 3), dtype=np.float64), np.zeros((0, 3), dtype=np.uint8)
|
||||
colors = np.repeat(color[None, :], len(points), axis=0)
|
||||
return points, colors
|
||||
|
||||
|
||||
def _sample_segment(
|
||||
start: np.ndarray,
|
||||
end: np.ndarray,
|
||||
step_mm: float,
|
||||
) -> np.ndarray:
|
||||
start = np.asarray(start, dtype=np.float64)
|
||||
end = np.asarray(end, dtype=np.float64)
|
||||
length = float(np.linalg.norm(end - start))
|
||||
if length <= 1e-9:
|
||||
return start[None, :]
|
||||
count = max(int(np.ceil(length / max(step_mm, 1.0))) + 1, 2)
|
||||
t = np.linspace(0.0, 1.0, count)
|
||||
return start[None, :] + (end - start)[None, :] * t[:, None]
|
||||
|
||||
|
||||
def _sample_polyline(points: np.ndarray, step_mm: float) -> np.ndarray:
|
||||
if len(points) == 0:
|
||||
return np.zeros((0, 3), dtype=np.float64)
|
||||
if len(points) == 1:
|
||||
return points.copy()
|
||||
segments = []
|
||||
for start, end in zip(points[:-1], points[1:], strict=True):
|
||||
segment = _sample_segment(start, end, step_mm)
|
||||
if segments:
|
||||
segment = segment[1:]
|
||||
segments.append(segment)
|
||||
return np.vstack(segments)
|
||||
|
||||
|
||||
def _marker_cross(
|
||||
center: np.ndarray,
|
||||
size_mm: float,
|
||||
step_mm: float,
|
||||
) -> np.ndarray:
|
||||
center = np.asarray(center, dtype=np.float64)
|
||||
offsets = np.arange(-size_mm, size_mm + step_mm, step_mm)
|
||||
samples = [center[None, :]]
|
||||
for axis in range(3):
|
||||
points = np.repeat(center[None, :], len(offsets), axis=0)
|
||||
points[:, axis] += offsets
|
||||
samples.append(points)
|
||||
return np.vstack(samples)
|
||||
|
||||
|
||||
def _track_band_mask(
|
||||
points: np.ndarray,
|
||||
track: TrackSolution | None,
|
||||
config: dict,
|
||||
) -> np.ndarray:
|
||||
if track is None:
|
||||
return np.zeros(len(points), dtype=bool)
|
||||
rep_cfg = config["representative"]
|
||||
mask = np.zeros(len(points), dtype=bool)
|
||||
for candidate in track.candidates:
|
||||
candidate_mask = (
|
||||
(points[:, 0] >= candidate.x_start)
|
||||
& (points[:, 0] < candidate.x_end)
|
||||
& (points[:, 1] >= candidate.y_min - 0.5 * rep_cfg["band_y_margin_mm"])
|
||||
& (points[:, 1] <= candidate.y_max + 0.5 * rep_cfg["band_y_margin_mm"])
|
||||
& (points[:, 2] <= candidate.z_low + rep_cfg["band_z_margin_mm"])
|
||||
)
|
||||
mask |= candidate_mask
|
||||
return mask
|
||||
|
||||
|
||||
def _sample_fitted_line(
|
||||
line: FittedLine | None,
|
||||
step_mm: float,
|
||||
) -> np.ndarray:
|
||||
if line is None:
|
||||
return np.zeros((0, 3), dtype=np.float64)
|
||||
x_min = float(line.x_support.min())
|
||||
x_max = float(line.x_support.max())
|
||||
x_grid = np.arange(x_min, x_max + step_mm, step_mm)
|
||||
y, z, conf = evaluate_line(line, x_grid)
|
||||
keep = conf > 0.0
|
||||
return np.column_stack([x_grid[keep], y[keep], z[keep]])
|
||||
|
||||
|
||||
def _sample_reference_line(
|
||||
reference_line: ReferenceLine,
|
||||
step_mm: float,
|
||||
) -> np.ndarray:
|
||||
if not reference_line.available or reference_line.point_front is None or reference_line.point_back is None:
|
||||
return np.zeros((0, 3), dtype=np.float64)
|
||||
return _sample_segment(reference_line.point_front, reference_line.point_back, step_mm)
|
||||
|
||||
|
||||
def _measurement_overlay_geometry(
|
||||
measurement: dict | None,
|
||||
measure_color: np.ndarray,
|
||||
step_mm: float,
|
||||
marker_size_mm: float,
|
||||
) -> list[tuple[np.ndarray, np.ndarray]]:
|
||||
layers: list[tuple[np.ndarray, np.ndarray]] = []
|
||||
if not measurement or not measurement.get("available", False):
|
||||
return layers
|
||||
beam_point = np.asarray(measurement["beam_point"], dtype=np.float64)
|
||||
ref_point = np.asarray(measurement["reference_point"], dtype=np.float64)
|
||||
vertical_anchor = np.asarray(measurement["vertical_anchor_point"], dtype=np.float64)
|
||||
|
||||
layers.append(_points_and_colors(_marker_cross(beam_point, marker_size_mm, step_mm), measure_color))
|
||||
layers.append(_points_and_colors(_marker_cross(ref_point, marker_size_mm, step_mm), COLOR_CORRESPONDENCE))
|
||||
layers.append(_points_and_colors(_marker_cross(vertical_anchor, marker_size_mm, step_mm), COLOR_ANCHOR))
|
||||
layers.append(_points_and_colors(_sample_segment(beam_point, vertical_anchor, step_mm), measure_color))
|
||||
layers.append(_points_and_colors(_sample_segment(vertical_anchor, ref_point, step_mm), COLOR_CORRESPONDENCE))
|
||||
return layers
|
||||
|
||||
|
||||
def export_measurement_overlay_plys(
|
||||
output_dir: str | Path,
|
||||
roi_points: np.ndarray,
|
||||
left_track: TrackSolution | None,
|
||||
right_track: TrackSolution | None,
|
||||
left_line: FittedLine | None,
|
||||
right_line: FittedLine | None,
|
||||
left_reference: ReferenceLine,
|
||||
right_reference: ReferenceLine,
|
||||
left_measurement: dict | None,
|
||||
right_measurement: dict | None,
|
||||
config: dict,
|
||||
coordinate_transform=None,
|
||||
) -> dict[str, str]:
|
||||
output_dir = Path(output_dir)
|
||||
output_dir.mkdir(parents=True, exist_ok=True)
|
||||
step_mm = max(config["runtime"]["spacing_mm"] * 0.8, 20.0)
|
||||
marker_size_mm = max(config["runtime"]["spacing_mm"] * 2.5, 60.0)
|
||||
|
||||
roi_colors = np.repeat(COLOR_BASE[None, :], len(roi_points), axis=0)
|
||||
roi_path = output_dir / "roi_cropped_cloud.ply"
|
||||
|
||||
left_band_mask = _track_band_mask(roi_points, left_track, config)
|
||||
right_band_mask = _track_band_mask(roi_points, right_track, config)
|
||||
left_band_points = roi_points[left_band_mask]
|
||||
right_band_points = roi_points[right_band_mask]
|
||||
|
||||
left_rep_line = _sample_fitted_line(left_line, step_mm)
|
||||
right_rep_line = _sample_fitted_line(right_line, step_mm)
|
||||
left_ref_line = _sample_reference_line(left_reference, step_mm)
|
||||
right_ref_line = _sample_reference_line(right_reference, step_mm)
|
||||
|
||||
def build_overlay(include_left: bool, include_right: bool) -> tuple[np.ndarray, np.ndarray]:
|
||||
layers: list[tuple[np.ndarray, np.ndarray]] = [(_points_and_colors(roi_points, COLOR_BASE))]
|
||||
if include_left:
|
||||
layers.append(_points_and_colors(left_band_points, COLOR_LEFT_BAND))
|
||||
layers.append(_points_and_colors(left_rep_line, COLOR_LEFT_REP_LINE))
|
||||
layers.append(_points_and_colors(left_ref_line, COLOR_LEFT_REF_LINE))
|
||||
layers.extend(_measurement_overlay_geometry(left_measurement, COLOR_LEFT_MEASURE, step_mm, marker_size_mm))
|
||||
if include_right:
|
||||
layers.append(_points_and_colors(right_band_points, COLOR_RIGHT_BAND))
|
||||
layers.append(_points_and_colors(right_rep_line, COLOR_RIGHT_REP_LINE))
|
||||
layers.append(_points_and_colors(right_ref_line, COLOR_RIGHT_REF_LINE))
|
||||
layers.extend(_measurement_overlay_geometry(right_measurement, COLOR_RIGHT_MEASURE, step_mm, marker_size_mm))
|
||||
points = np.vstack([layer_points for layer_points, _ in layers if len(layer_points) > 0])
|
||||
colors = np.vstack([layer_colors for layer_points, layer_colors in layers if len(layer_points) > 0])
|
||||
return points, colors
|
||||
|
||||
full_points, full_colors = build_overlay(include_left=True, include_right=True)
|
||||
left_points, left_colors = build_overlay(include_left=True, include_right=False)
|
||||
right_points, right_colors = build_overlay(include_left=False, include_right=True)
|
||||
if coordinate_transform is not None:
|
||||
roi_points_out = coordinate_transform(roi_points)
|
||||
full_points = coordinate_transform(full_points)
|
||||
left_points = coordinate_transform(left_points)
|
||||
right_points = coordinate_transform(right_points)
|
||||
else:
|
||||
roi_points_out = roi_points
|
||||
|
||||
full_path = output_dir / "roi_measurement_overlay_full.ply"
|
||||
left_path = output_dir / "roi_measurement_overlay_left.ply"
|
||||
right_path = output_dir / "roi_measurement_overlay_right.ply"
|
||||
write_colored_ply(roi_path, roi_points_out, roi_colors)
|
||||
write_colored_ply(full_path, full_points, full_colors)
|
||||
write_colored_ply(left_path, left_points, left_colors)
|
||||
write_colored_ply(right_path, right_points, right_colors)
|
||||
return {
|
||||
"roi_cropped_cloud": str(roi_path),
|
||||
"roi_measurement_overlay_full": str(full_path),
|
||||
"roi_measurement_overlay_left": str(left_path),
|
||||
"roi_measurement_overlay_right": str(right_path),
|
||||
}
|
||||
247
alg/delivery_v1.2.1/underframe_deflection/preprocess.py
Normal file
247
alg/delivery_v1.2.1/underframe_deflection/preprocess.py
Normal file
@@ -0,0 +1,247 @@
|
||||
from __future__ import annotations
|
||||
|
||||
from dataclasses import dataclass
|
||||
|
||||
import numpy as np
|
||||
from scipy.spatial import cKDTree
|
||||
|
||||
from .roi import apply_passthrough, normalize_aabb
|
||||
|
||||
|
||||
@dataclass(slots=True)
|
||||
class PreprocessResult:
|
||||
raw_points: np.ndarray
|
||||
filtered_points: np.ndarray
|
||||
work_points: np.ndarray
|
||||
work_normals: np.ndarray
|
||||
work_curvature: np.ndarray
|
||||
spacing: float
|
||||
stats: dict
|
||||
|
||||
|
||||
def estimate_spacing(points: np.ndarray, sample_size: int = 4000) -> float:
|
||||
if len(points) < 2:
|
||||
return 1.0
|
||||
if len(points) > sample_size:
|
||||
indices = np.linspace(0, len(points) - 1, sample_size, dtype=int)
|
||||
sample = points[indices]
|
||||
else:
|
||||
sample = points
|
||||
tree = cKDTree(sample)
|
||||
distances, _ = tree.query(sample, k=min(2, len(sample)))
|
||||
if distances.ndim == 1:
|
||||
return float(np.median(distances))
|
||||
return float(np.quantile(distances[:, -1], 0.3))
|
||||
|
||||
|
||||
def statistical_outlier_filter(
|
||||
points: np.ndarray,
|
||||
k: int,
|
||||
std_ratio: float,
|
||||
) -> tuple[np.ndarray, np.ndarray]:
|
||||
if len(points) <= k + 1:
|
||||
mask = np.ones(len(points), dtype=bool)
|
||||
return points, mask
|
||||
tree = cKDTree(points)
|
||||
distances, _ = tree.query(points, k=k + 1)
|
||||
mean_dist = distances[:, 1:].mean(axis=1)
|
||||
threshold = mean_dist.mean() + std_ratio * mean_dist.std(ddof=0)
|
||||
mask = mean_dist <= threshold
|
||||
return points[mask], mask
|
||||
|
||||
|
||||
def radius_filter(
|
||||
points: np.ndarray,
|
||||
radius: float,
|
||||
min_neighbors: int,
|
||||
) -> tuple[np.ndarray, np.ndarray]:
|
||||
if len(points) == 0:
|
||||
return points, np.zeros(0, dtype=bool)
|
||||
tree = cKDTree(points)
|
||||
counts = tree.query_ball_point(points, r=radius, return_length=True)
|
||||
mask = counts >= min_neighbors
|
||||
return points[mask], mask
|
||||
|
||||
|
||||
def voxel_downsample(points: np.ndarray, voxel_size: float) -> np.ndarray:
|
||||
if len(points) == 0 or voxel_size <= 0:
|
||||
return points.copy()
|
||||
mins = points.min(axis=0)
|
||||
grid = np.floor((points - mins) / voxel_size).astype(np.int64)
|
||||
unique, inverse = np.unique(grid, axis=0, return_inverse=True)
|
||||
sums = np.zeros((len(unique), 3), dtype=np.float64)
|
||||
counts = np.bincount(inverse)
|
||||
np.add.at(sums, inverse, points)
|
||||
return sums / counts[:, None]
|
||||
|
||||
|
||||
def estimate_normals(
|
||||
points: np.ndarray,
|
||||
k: int,
|
||||
) -> tuple[np.ndarray, np.ndarray]:
|
||||
if len(points) == 0:
|
||||
return np.zeros((0, 3)), np.zeros(0)
|
||||
neighbor_count = min(k + 1, len(points))
|
||||
tree = cKDTree(points)
|
||||
_, indices = tree.query(points, k=neighbor_count)
|
||||
normals = np.zeros((len(points), 3), dtype=np.float64)
|
||||
curvature = np.zeros(len(points), dtype=np.float64)
|
||||
for idx, neigh in enumerate(indices):
|
||||
neighborhood = points[np.atleast_1d(neigh)]
|
||||
centered = neighborhood - neighborhood.mean(axis=0, keepdims=True)
|
||||
cov = centered.T @ centered / max(len(centered), 1)
|
||||
eigenvalues, eigenvectors = np.linalg.eigh(cov)
|
||||
order = np.argsort(eigenvalues)
|
||||
eigenvalues = eigenvalues[order]
|
||||
eigenvectors = eigenvectors[:, order]
|
||||
normals[idx] = eigenvectors[:, 0]
|
||||
curvature[idx] = float(eigenvalues[0] / max(eigenvalues.sum(), 1e-9))
|
||||
return normals, curvature
|
||||
|
||||
|
||||
def bilateral_filter_points(
|
||||
points: np.ndarray,
|
||||
normals: np.ndarray,
|
||||
*,
|
||||
k: int,
|
||||
sigma_d_mm: float,
|
||||
sigma_n_rad: float,
|
||||
iterations: int = 1,
|
||||
) -> np.ndarray:
|
||||
"""Point-cloud bilateral filter that smooths along estimated normals.
|
||||
|
||||
Each point is shifted *along its own normal* by the weighted average of
|
||||
the signed normal-direction offsets to its k-nearest neighbours, using a
|
||||
Gaussian on Euclidean distance (``sigma_d_mm``) and a Gaussian on the
|
||||
angular deviation between point normals (``sigma_n_rad``). The tangential
|
||||
position is preserved, so edges survive while noise along the surface
|
||||
normal is suppressed.
|
||||
|
||||
The per-point shift is clamped to ``sigma_d_mm`` so that degenerate
|
||||
neighbourhoods or bad normals cannot collapse the cloud.
|
||||
"""
|
||||
if len(points) == 0 or iterations <= 0:
|
||||
return points.copy()
|
||||
effective_k = int(min(max(k, 2), len(points)))
|
||||
sigma_d = max(float(sigma_d_mm), 1e-6)
|
||||
sigma_n = max(float(sigma_n_rad), 1e-6)
|
||||
inv_2sd2 = 1.0 / (2.0 * sigma_d * sigma_d)
|
||||
inv_2sn2 = 1.0 / (2.0 * sigma_n * sigma_n)
|
||||
max_shift = sigma_d # clamp: a bilateral step never moves a point further than sigma_d
|
||||
|
||||
current = points.astype(np.float64, copy=True)
|
||||
unit_normals = normals.astype(np.float64, copy=True)
|
||||
norm_len = np.linalg.norm(unit_normals, axis=1)
|
||||
valid_normal = norm_len > 1e-9
|
||||
unit_normals[valid_normal] /= norm_len[valid_normal, None]
|
||||
|
||||
for _ in range(int(iterations)):
|
||||
tree = cKDTree(current)
|
||||
distances, indices = tree.query(current, k=effective_k)
|
||||
if distances.ndim == 1:
|
||||
distances = distances[:, None]
|
||||
indices = indices[:, None]
|
||||
own_n = unit_normals[:, None, :] # (N, 1, 3)
|
||||
offsets = current[indices] - current[:, None, :] # (N, k, 3)
|
||||
normal_offset = np.einsum("nkd,nkd->nk", offsets, own_n)
|
||||
spatial_w = np.exp(-(distances ** 2) * inv_2sd2)
|
||||
dots = np.clip(np.einsum("nkd,nkd->nk", unit_normals[indices], own_n), -1.0, 1.0)
|
||||
angle = np.arccos(np.abs(dots))
|
||||
normal_w = np.exp(-(angle ** 2) * inv_2sn2)
|
||||
weights = spatial_w * normal_w
|
||||
weight_sum = weights.sum(axis=1)
|
||||
move = weight_sum > 1e-12
|
||||
move &= valid_normal
|
||||
delta = np.zeros(len(current), dtype=np.float64)
|
||||
delta[move] = (weights[move] * normal_offset[move]).sum(axis=1) / weight_sum[move]
|
||||
delta = np.clip(delta, -max_shift, max_shift)
|
||||
current[move] = current[move] + delta[move, None] * unit_normals[move]
|
||||
return current
|
||||
|
||||
|
||||
def preprocess_point_cloud(points: np.ndarray, config: dict) -> PreprocessResult:
|
||||
preprocess = config["preprocess"]
|
||||
flags: list[str] = []
|
||||
spacing_estimate = estimate_spacing(
|
||||
points,
|
||||
sample_size=preprocess["spacing_sample_size"],
|
||||
)
|
||||
|
||||
passthrough_cfg = preprocess.get("passthrough") or {}
|
||||
passthrough_aabb = passthrough_cfg.get("aabb") if passthrough_cfg.get("enabled") else None
|
||||
raw_count = int(len(points))
|
||||
if passthrough_aabb is not None:
|
||||
aabb = normalize_aabb(passthrough_aabb)
|
||||
keep_mask = apply_passthrough(points, aabb)
|
||||
kept = points[keep_mask]
|
||||
if len(kept) >= max(64, raw_count // 1000):
|
||||
passthrough_points = kept
|
||||
else:
|
||||
passthrough_points = points
|
||||
flags.append("passthrough_empty")
|
||||
else:
|
||||
passthrough_points = points
|
||||
|
||||
stat_points, stat_mask = statistical_outlier_filter(
|
||||
passthrough_points,
|
||||
k=preprocess["stat_k"],
|
||||
std_ratio=preprocess["stat_std_ratio"],
|
||||
)
|
||||
radius_points, radius_mask = radius_filter(
|
||||
stat_points,
|
||||
radius=preprocess["radius_mm"],
|
||||
min_neighbors=preprocess["radius_min_neighbors"],
|
||||
)
|
||||
|
||||
bilateral_cfg = preprocess.get("bilateral") or {}
|
||||
bilateral_count = len(radius_points)
|
||||
bilateral_applied = False
|
||||
if bilateral_cfg.get("enabled") and len(radius_points) >= 8:
|
||||
k_requested = int(bilateral_cfg.get("k", 24))
|
||||
k_effective = min(k_requested, len(radius_points))
|
||||
if k_effective < k_requested:
|
||||
flags.append("bilateral_degraded")
|
||||
pre_normals, _ = estimate_normals(radius_points, k=min(k_effective, 32))
|
||||
radius_points = bilateral_filter_points(
|
||||
radius_points,
|
||||
pre_normals,
|
||||
k=k_effective,
|
||||
sigma_d_mm=float(bilateral_cfg.get("sigma_d_mm", max(2.0 * spacing_estimate, 2.0))),
|
||||
sigma_n_rad=float(
|
||||
bilateral_cfg.get(
|
||||
"sigma_n_rad",
|
||||
np.deg2rad(float(bilateral_cfg.get("sigma_n_deg", 15.0))),
|
||||
)
|
||||
),
|
||||
iterations=int(bilateral_cfg.get("iterations", 1)),
|
||||
)
|
||||
bilateral_applied = True
|
||||
|
||||
work_points = voxel_downsample(radius_points, preprocess["voxel_mm"])
|
||||
work_normals, work_curvature = estimate_normals(
|
||||
work_points,
|
||||
k=preprocess["normal_k"],
|
||||
)
|
||||
stats = {
|
||||
"raw_count": raw_count,
|
||||
"after_passthrough": int(len(passthrough_points)),
|
||||
"passthrough_applied": passthrough_aabb is not None,
|
||||
"after_statistical_filter": int(len(stat_points)),
|
||||
"after_radius_filter": int(len(radius_points)),
|
||||
"after_bilateral": int(bilateral_count),
|
||||
"bilateral_applied": bilateral_applied,
|
||||
"downsampled_count": int(len(work_points)),
|
||||
"spacing_mm": float(spacing_estimate),
|
||||
"statistical_keep_ratio": float(np.mean(stat_mask)) if len(stat_mask) else 1.0,
|
||||
"radius_keep_ratio": float(np.mean(radius_mask)) if len(radius_mask) else 1.0,
|
||||
"flags": flags,
|
||||
}
|
||||
return PreprocessResult(
|
||||
raw_points=points,
|
||||
filtered_points=radius_points,
|
||||
work_points=work_points,
|
||||
work_normals=work_normals,
|
||||
work_curvature=work_curvature,
|
||||
spacing=spacing_estimate,
|
||||
stats=stats,
|
||||
)
|
||||
0
alg/delivery_v1.2.1/underframe_deflection/py.typed
Normal file
0
alg/delivery_v1.2.1/underframe_deflection/py.typed
Normal file
Some files were not shown because too many files have changed in this diff Show More
Reference in New Issue
Block a user