feat: initial commit - massive-cloud point cloud processing system
This commit is contained in:
200
alg/delivery_v1.1/underframe_deflection/README.md
Normal file
200
alg/delivery_v1.1/underframe_deflection/README.md
Normal file
@@ -0,0 +1,200 @@
|
||||
# underframe-deflection
|
||||
|
||||
**从扫描点云测量高铁底架纵梁挠度的 Python 算法包：接受"CAD 基准 + 扫描点云 + ROI JSON"三件套输入，自动配准、去噪、识别梁、出 Excel 误差报表。**
|
||||
|
||||
[]() []() []()
|
||||
|
||||
---
|
||||
|
||||
## 这是什么
|
||||
|
||||
输入一份现场扫描点云（`.ply / .xyz`），一份 CAD 基准（`.ply / .stl / .obj`），一份描述测量区域的 ROI JSON，本包会：
|
||||
|
||||
1. **去噪**：passthrough → 统计离群 → 半径离群 → 双边滤波 → 体素降采样
|
||||
2. **配准**：用 ICP 把扫描点云刚性对齐到 CAD 坐标系
|
||||
3. **识别梁**：沿车长切片、Beam-search 跟踪左右纵梁
|
||||
4. **拟合**：鲁棒样条拟合代表线
|
||||
5. **找参考**：基于车体孔特征或线端点定参考线
|
||||
6. **算挠度**：沿 X 轴逐点计算并选出绝对值最大的测量点
|
||||
|
||||
输出：完整 JSON 报告、Excel 误差报表（5 sheet）、沿 X 的挠度 CSV、候选切片诊断 CSV、5 张诊断 PNG、4 个着色 PLY 叠加图。
|
||||
|
||||
---
|
||||
|
||||
## 安装
|
||||
|
||||
```bash
|
||||
# 进入本目录后
|
||||
pip install -e .[all] # 推荐：core + matplotlib + openpyxl
|
||||
|
||||
# 或仅安装核心 + 自己选 extra
|
||||
pip install -e . # 仅 numpy + scipy（只能调用 compute_deflection）
|
||||
pip install -e .[viz] # +matplotlib（PNG 诊断图）
|
||||
pip install -e .[excel] # +openpyxl（.xlsx 报表）
|
||||
|
||||
# 或直接用 requirements 锁版本
|
||||
pip install -r requirements.txt
|
||||
```
|
||||
|
||||
安装后会注册命令行 `underframe-measure`，Python 内可直接 `import underframe_deflection`。
|
||||
|
||||
---
|
||||
|
||||
## 上手
|
||||
|
||||
```bash
|
||||
underframe-measure \
|
||||
--input-cloud scan.ply \
|
||||
--cad reference.ply \
|
||||
--roi roi.json \
|
||||
--out-dir out/
|
||||
```
|
||||
|
||||
或者 Python 调用：
|
||||
|
||||
```python
|
||||
from underframe_deflection import run_measurement_pipeline
|
||||
|
||||
report = run_measurement_pipeline(
|
||||
input_cloud="scan.ply",
|
||||
cad_path="reference.ply",
|
||||
roi_path="roi.json",
|
||||
out_dir="out/",
|
||||
)
|
||||
|
||||
print(report["deflection"]["left"]["selected_measurement"]["signed_deflection_mm"])
|
||||
print(report["deflection"]["right"]["selected_measurement"]["signed_deflection_mm"])
|
||||
```
|
||||
|
||||
`test_data/` 自带可直接运行的样例：
|
||||
|
||||
```bash
|
||||
underframe-measure \
|
||||
--input-cloud test_data/m000004802409t02_asm_100000pts.ply \
|
||||
--cad test_data/m000004802409t02_asm_100000pts.ply \
|
||||
--roi test_data/sample_roi.json \
|
||||
--out-dir test_data/runs/demo
|
||||
```
|
||||
|
||||
---
|
||||
|
||||
## 文档导览
|
||||
|
||||
| 文件 | 用途 |
|
||||
| --- | --- |
|
||||
| **`README.md`** (本文件) | 仓库概览，30 秒上手 |
|
||||
| **`USAGE.md`** | 集成开发者完整指南：三种调用方式、输入格式、读结果、异常、配置覆盖、模板 |
|
||||
| `requirements.txt` | 锁定版本的依赖清单 |
|
||||
| `pyproject.toml` | 打包描述（pip install 的入口） |
|
||||
|
||||
---
|
||||
|
||||
## 仓库结构
|
||||
|
||||
```
|
||||
underframe_deflection/
|
||||
├── README.md ← 本文件
|
||||
├── USAGE.md ← 集成开发者文档
|
||||
├── requirements.txt
|
||||
├── pyproject.toml ← pip 打包描述（无需 setup.py）
|
||||
├── _version.py ← __version__ / ALGORITHM_VERSION / SCHEMA_VERSION
|
||||
├── __init__.py ← 公开 API 出口
|
||||
│
|
||||
├── api.py ← compute_deflection() 主入口
|
||||
├── pipeline.py ← run_measurement_pipeline() 端到端编排
|
||||
├── cli.py ← 命令行入口（underframe-measure）
|
||||
│
|
||||
├── config.py ← DEFAULT_CONFIG 与档位
|
||||
├── preprocess.py ← SOR / radius / voxel / normals / bilateral
|
||||
├── cad_io.py ← CAD 读取 (PLY/XYZ/STL/OBJ)
|
||||
├── registration.py ← PCA 粗对齐 + trimmed ICP
|
||||
├── roi.py ← ROI JSON 读取 + AABB passthrough
|
||||
│
|
||||
├── frame.py ← 车体坐标系推断 + ROI mask
|
||||
├── beams.py ← 切片 + 候选条带 + Beam-search 跟踪
|
||||
├── fitting.py ← 代表点 + 鲁棒样条拟合
|
||||
├── reference.py ← 参考线检测（孔 / 端点）
|
||||
│
|
||||
├── artifacts.py ← JSON / CSV / PNG / PLY writer
|
||||
├── excel_report.py ← Excel 写入器 (5 sheet)
|
||||
├── ply_overlay.py ← 着色 PLY 叠加图
|
||||
├── visualization.py ← 诊断 PNG 绘图
|
||||
│
|
||||
├── io.py ← 文件 I/O 工具
|
||||
├── exceptions.py ← 公开异常层级
|
||||
├── structures.py ← 内部数据类
|
||||
│
|
||||
└── test_data/
|
||||
├── m000004802409t02_asm.stp ← 原始 STEP（仅参考，不被解析）
|
||||
├── m000004802409t02_asm_100000pts.ply ← STEP 采样得到的 CAD 基准点云
|
||||
└── sample_roi.json ← 样例 ROI（AABB）
|
||||
```
|
||||
|
||||
---
|
||||
|
||||
## 输入约定
|
||||
|
||||
所有点云的坐标轴必须：
|
||||
|
||||
- **X**：纵向（车长）
|
||||
- **Y**：横向（车宽）
|
||||
- **Z**：高度，**向上为正**
|
||||
- 单位：**毫米**
|
||||
|
||||
CAD 基准也必须遵循同样的轴向；ROI JSON 中的 bbox 字段在 CAD 坐标系下。
|
||||
如果你的扫描设备轴向不同，请在调用前自行做旋转/置换。
|
||||
|
||||
---
|
||||
|
||||
## 公开 API
|
||||
|
||||
```python
|
||||
from underframe_deflection import (
|
||||
# 主入口
|
||||
compute_deflection, # 纯内存计算
|
||||
run_measurement_pipeline, # 文件读 + 算 + 文件写
|
||||
|
||||
# 单步工具（高级用法）
|
||||
load_point_cloud, # 读扫描点云
|
||||
read_cad_reference, # 读 CAD
|
||||
load_roi_aabb, # 读 ROI JSON
|
||||
register_to_cad, # ICP 配准
|
||||
bilateral_filter_points, # 双边滤波
|
||||
apply_passthrough, # AABB 过滤
|
||||
|
||||
# 结果数据类
|
||||
DeflectionResult,
|
||||
SideDeflection,
|
||||
DeflectionCurves,
|
||||
RegistrationResult,
|
||||
|
||||
# 异常
|
||||
DeflectionError,
|
||||
InvalidPointCloudError,
|
||||
InsufficientPointsError,
|
||||
UnsupportedFormatError,
|
||||
FrameInferenceError,
|
||||
|
||||
# 版本
|
||||
__version__,
|
||||
ALGORITHM_VERSION,
|
||||
SCHEMA_VERSION,
|
||||
)
|
||||
```
|
||||
|
||||
详见 [`USAGE.md`](./USAGE.md)。
|
||||
|
||||
---
|
||||
|
||||
## 状态与版本
|
||||
|
||||
- **当前版本**：1.1.0
|
||||
- **算法版本**：0.1.0（v1.1 没有动算法主流程，仅扩展 I/O / 预处理 / 配准）
|
||||
- **Schema 版本**：1.0（v1.1 仅新增可选字段，向后兼容）
|
||||
- **Python**：≥ 3.10（Windows / Linux / macOS 通用）
|
||||
|
||||
---
|
||||
|
||||
## License
|
||||
|
||||
Proprietary — PKU High-Speed Train Project.
|
||||
363
alg/delivery_v1.1/underframe_deflection/USAGE.md
Normal file
363
alg/delivery_v1.1/underframe_deflection/USAGE.md
Normal file
@@ -0,0 +1,363 @@
|
||||
# underframe_deflection 集成调用指南 (v1.1)
|
||||
|
||||
> 高铁底架纵梁挠度测量算法 —— 基于点云的计算。
|
||||
> 本文档面向把本算法集成进上层软件的开发者：怎么准备 **CAD 基准 + 扫描点云 + ROI JSON** 三件套输入、怎么调用、怎么读结果。
|
||||
|
||||
---
|
||||
|
||||
## 0. TL;DR
|
||||
|
||||
```python
|
||||
from underframe_deflection import run_measurement_pipeline
|
||||
|
||||
report = run_measurement_pipeline(
|
||||
input_cloud="scan.ply",
|
||||
cad_path="reference.ply",
|
||||
roi_path="roi.json",
|
||||
out_dir="out/",
|
||||
)
|
||||
|
||||
# 关心的两个核心数值
|
||||
print(report["deflection"]["left"]["selected_measurement"]["signed_deflection_mm"])
|
||||
print(report["deflection"]["right"]["selected_measurement"]["signed_deflection_mm"])
|
||||
```
|
||||
|
||||
落盘后会得到：
|
||||
|
||||
```
|
||||
out/
|
||||
├── report.json # 完整结果
|
||||
├── report.xlsx # 误差报表（Summary + 4 张中间表）
|
||||
├── deflection_curves.csv # 沿 X 的挠度曲线
|
||||
├── beam_tracks.csv # 候选梁切片诊断
|
||||
├── images/ # 5 张诊断 PNG
|
||||
└── ply/ # 4 个着色 PLY 叠加图
|
||||
```
|
||||
|
||||
---
|
||||
|
||||
## 1. 算法的三个输入
|
||||
|
||||
| 输入 | 类型 | 必需 | 说明 |
|
||||
| --- | --- | --- | --- |
|
||||
| **扫描点云** | `.ply / .xyz / .txt / .csv` | ✅ 必须 | 现场采集得到的 (N, 3) XYZ 点云，单位 mm |
|
||||
| **CAD 基准** | `.ply / .xyz / .stl / .obj` | 可选但强烈建议 | 预先 tessellate / 采样过的 CAD 几何，作为配准基准坐标系；不直接吃 STEP/IGES |
|
||||
| **ROI JSON** | `.json` | 可选但强烈建议 | 描述车底测量区域的轴对齐 bbox，坐标基于 CAD 坐标系 |
|
||||
|
||||
> 三者**全给**时走 v1.1 完整流程：去噪 → 配准到 CAD 系 → 按 ROI JSON 截区 → 算挠度 → 出 Excel。
|
||||
> 只给扫描点云时走 legacy 流程，与本算法 v1.0 行为完全一致（仅多出 bilateral 这一步可用 `--no-bilateral` 关掉）。
|
||||
|
||||
### 1.1 ROI JSON 文件格式
|
||||
|
||||
固定 6 个 float 字段，单位 mm，CAD 坐标系下：
|
||||
|
||||
```json
|
||||
{
|
||||
"x_min": -12500.0,
|
||||
"x_max": 12500.0,
|
||||
"y_min": 150.0,
|
||||
"y_max": 4100.0,
|
||||
"z_min": -1680.0,
|
||||
"z_max": 200.0
|
||||
}
|
||||
```
|
||||
|
||||
约束：每个轴 `min < max`，且都是有限实数；任一字段缺失或非法 → `InvalidPointCloudError`。
|
||||
|
||||
`test_data/sample_roi.json` 是一个可直接使用的样例。
|
||||
|
||||
### 1.2 CAD 基准
|
||||
|
||||
- **不解析 STEP / IGES**。如果你只有 `.stp`，请用 CAD 软件或离线脚本先把它转成 `.stl` / `.ply` / `.obj`，再喂给本算法。
|
||||
- 三角网格的话只读顶点位置，face 信息忽略 —— 算法内部把 CAD 当作"参考点集"用 ICP 对齐。
|
||||
- `test_data/m000004802409t02_asm_100000pts.ply` 是从原始 STEP 采样得到的 10 万点 PLY，可直接当 CAD 基准。
|
||||
|
||||
---
|
||||
|
||||
## 2. 三种调用方式
|
||||
|
||||
### 2.1 命令行（最简单的集成方式）
|
||||
|
||||
```bash
|
||||
underframe-measure \
|
||||
--input-cloud scan.ply \
|
||||
--cad reference.ply \
|
||||
--roi roi.json \
|
||||
--out-dir out/
|
||||
```
|
||||
|
||||
可选开关：
|
||||
|
||||
| 开关 | 含义 |
|
||||
| --- | --- |
|
||||
| `--config CONFIG.json` | 覆盖部分内部超参数（结构同 `DEFAULT_CONFIG`） |
|
||||
| `--profile {auto,M,TC,TP,custom}` | 调参档位 |
|
||||
| `--skip-registration` | 给了 `--cad` 但不跑 ICP（适合调试或扫描已经预对齐） |
|
||||
| `--no-bilateral` | 关闭双边滤波（v1.0 行为） |
|
||||
| `--skip-excel` | 不写 `.xlsx` |
|
||||
|
||||
未安装本包时，等价命令：
|
||||
|
||||
```bash
|
||||
python -m underframe_deflection.cli --input-cloud scan.ply ...
|
||||
```
|
||||
|
||||
### 2.2 一次性 Python 函数（推荐）
|
||||
|
||||
```python
|
||||
from underframe_deflection import run_measurement_pipeline
|
||||
|
||||
report = run_measurement_pipeline(
|
||||
input_cloud="scan.ply",
|
||||
cad_path="reference.ply", # 可选
|
||||
roi_path="roi.json", # 可选
|
||||
out_dir="out/",
|
||||
config_path=None, # 可选 JSON 配置覆盖
|
||||
profile="auto", # 可选档位
|
||||
skip_registration=False,
|
||||
enable_bilateral=True,
|
||||
skip_excel=False,
|
||||
)
|
||||
```
|
||||
|
||||
**返回**：`report` 是一个 `dict`，结构与 `out/report.json` 完全一致。
|
||||
|
||||
### 2.3 纯内存计算（不读写文件）
|
||||
|
||||
如果你已经从其它来源拿到了点云和 ROI（例如直接从相机驱动 / 数据库取数据），可以避开文件 I/O：
|
||||
|
||||
```python
|
||||
import numpy as np
|
||||
from underframe_deflection import compute_deflection, load_point_cloud
|
||||
|
||||
points = load_point_cloud("scan.ply") # 或自己构造 (N, 3) ndarray
|
||||
roi_aabb = { # 与 ROI JSON 同结构的 dict
|
||||
"x_min": -12500, "x_max": 12500,
|
||||
"y_min": 150, "y_max": 4100,
|
||||
"z_min": -1680, "z_max": 200,
|
||||
}
|
||||
|
||||
result = compute_deflection(points, roi_aabb=roi_aabb)
|
||||
print(result.left.selected_signed_mm, result.right.selected_signed_mm)
|
||||
```
|
||||
|
||||
> 注意：`compute_deflection` 本身**不做 CAD 配准**。如果你想自己控制配准过程，先调用 `register_to_cad`，把变换应用到点云后再喂给 `compute_deflection`，配合 `config["frame"]["use_input_axes"]=True`（这本来就是默认值）即可：
|
||||
|
||||
```python
|
||||
from underframe_deflection import (
|
||||
read_cad_reference, register_to_cad, compute_deflection,
|
||||
)
|
||||
from underframe_deflection.registration import apply_transform
|
||||
|
||||
scan = load_point_cloud("scan.ply")
|
||||
cad = read_cad_reference("reference.ply")
|
||||
|
||||
# 你需要一份预解算好的 config（spacing 之类）。最简单的办法：
|
||||
from underframe_deflection.config import load_config, resolve_numeric_parameters, cloud_geometry
|
||||
from underframe_deflection.preprocess import estimate_spacing
|
||||
cfg = load_config(None, "auto")
|
||||
cfg = resolve_numeric_parameters(cfg, cloud_geometry(scan), estimate_spacing(scan))
|
||||
|
||||
reg = register_to_cad(scan, cad, cfg)
|
||||
scan_aligned = apply_transform(scan, reg.R, reg.t)
|
||||
|
||||
result = compute_deflection(scan_aligned, roi_aabb=roi_aabb)
|
||||
```
|
||||
|
||||
---
|
||||
|
||||
## 3. 读取输出
|
||||
|
||||
### 3.1 核心数值（最常用）
|
||||
|
||||
```python
|
||||
left = report["deflection"]["left"]["selected_measurement"]
|
||||
right = report["deflection"]["right"]["selected_measurement"]
|
||||
|
||||
if left["available"]:
|
||||
x_mm = left["x_mm"]
|
||||
signed = left["signed_deflection_mm"] # 带符号，向下为负
|
||||
absolute = left["absolute_deflection_mm"]
|
||||
type_ = left["selected_type"] # "downward" / "upward"
|
||||
beam_pt = left["beam_point"] # [x, y, z]
|
||||
ref_pt = left["reference_point"] # [x, y, z]
|
||||
```
|
||||
|
||||
如果你用的是 `compute_deflection`，公开数据类更直接：
|
||||
|
||||
```python
|
||||
result.left.selected_signed_mm
|
||||
result.left.max_downward_mm
|
||||
result.left.max_downward_x_mm
|
||||
result.left.max_upward_mm
|
||||
result.left.reference_confidence
|
||||
result.curves.x_mm # ndarray
|
||||
result.curves.left_deflection_mm # ndarray, NaN 表示该 X 处无效
|
||||
```
|
||||
|
||||
### 3.2 质量警告 `quality_flags`
|
||||
|
||||
**非致命**问题列在这里，调用方应判断是否要重测：
|
||||
|
||||
| Flag | 含义 |
|
||||
| --- | --- |
|
||||
| `left_track_missing` / `right_track_missing` | 单侧梁追踪失败 |
|
||||
| `left_representative_line_missing` / `right_...` | 单侧代表线拟合失败 |
|
||||
| `left_reference_unavailable` / `right_...` | 单侧参考线不可用 |
|
||||
| `left_reference_low_confidence` / `right_...` | 参考线置信度低 |
|
||||
| `left_track_low_coverage` / `right_...` | 单侧追踪覆盖率 < 45% |
|
||||
| `left_right_symmetry_warning` | 左右对称性偏差 > 18% |
|
||||
| **`registration_not_converged`** | ICP 未收敛到容差内 |
|
||||
| **`registration_poor_quality`** | ICP 最终 RMS > max(10 mm, 2×点间距) |
|
||||
| **`registration_low_inlier_ratio`** | ICP 内点比例 < 50% |
|
||||
| **`passthrough_empty`** | ROI passthrough 后点过少，已自动回退 |
|
||||
| **`bilateral_degraded`** | bilateral k 邻域被裁剪 |
|
||||
|
||||
加粗的几项是 v1.1 新增。
|
||||
|
||||
### 3.3 配准信息
|
||||
|
||||
```python
|
||||
reg = report["registration"]
|
||||
if reg["performed"]:
|
||||
reg["rms_mm"] # 最终对齐残差
|
||||
reg["initial_rms_mm"] # 粗对齐后的初始残差
|
||||
reg["iterations"]
|
||||
reg["inlier_ratio"]
|
||||
reg["converged"] # bool
|
||||
reg["R"] # 3x3 list
|
||||
reg["t"] # length-3 list
|
||||
```
|
||||
|
||||
### 3.4 Excel 误差报表
|
||||
|
||||
`out/report.xlsx` 5 个 sheet：
|
||||
|
||||
| Sheet | 内容 |
|
||||
| --- | --- |
|
||||
| **Summary** | key/value 形式：版本、输入路径、配准统计、预处理各阶段计数、左右挠度核心数值、质量警告 |
|
||||
| **Deflection_Curves** | 沿 X 的逐点表：x / 左右 beam_y, beam_z / 左右 ref_y, ref_z / 左右 deflection / 左右 confidence |
|
||||
| **Beam_Tracks** | 每片候选条带：slice_index、y_center、z_low、width、score、selected_rank 等 |
|
||||
| **Reference_Lines** | 左右参考线 front/back 端点、direction、residual、confidence |
|
||||
| **Representative_Points** | 左右每个代表点的 x/y/z/width/residual/confidence |
|
||||
|
||||
> 用 `openpyxl`、`pandas.read_excel`、Excel/LibreOffice 都可以直接读。
|
||||
|
||||
### 3.5 异常
|
||||
|
||||
| 异常类 | 触发条件 |
|
||||
| --- | --- |
|
||||
| `InvalidPointCloudError` | 点云 / CAD / ROI JSON 文件格式非法、字段缺失、AABB 反转 |
|
||||
| `UnsupportedFormatError` | 文件后缀不在支持列表 |
|
||||
| `InsufficientPointsError` | 点数过少（点云 < 256）或 ROI 过滤后无重叠 |
|
||||
| `FrameInferenceError` | 自动坐标系推断失败（仅 legacy 路径） |
|
||||
|
||||
集成方应在外层 `try / except DeflectionError` 兜底，并把 `quality_flags` 落入日志或上层告警。
|
||||
|
||||
---
|
||||
|
||||
## 4. 配置覆盖（高级）
|
||||
|
||||
绝大多数场景不需要碰配置。需要微调时，通过 `--config my.json` 或 `config=` 传入嵌套 dict，与 `DEFAULT_CONFIG` 深度合并：
|
||||
|
||||
```python
|
||||
config = {
|
||||
"preprocess": {
|
||||
"bilateral": {"enabled": True, "k": 32, "sigma_n_deg": 10.0},
|
||||
# 也可以从外部传 passthrough，但通常用 ROI JSON 即可
|
||||
},
|
||||
"registration": {
|
||||
"icp_max_iter": 80,
|
||||
"trim_ratio": 0.15,
|
||||
},
|
||||
"reference": {
|
||||
"low_confidence_threshold": 0.5,
|
||||
},
|
||||
"deflection": {
|
||||
"evaluation_grid_factor": 0.5, # 更密的挠度采样
|
||||
},
|
||||
}
|
||||
```
|
||||
|
||||
完整字段定义见 `underframe_deflection/config.py: DEFAULT_CONFIG`。
|
||||
|
||||
---
|
||||
|
||||
## 5. 进度回调
|
||||
|
||||
`compute_deflection` 支持 `progress` 回调：
|
||||
|
||||
```python
|
||||
def on_progress(stage: str, fraction: float) -> None:
|
||||
print(f"[{fraction:5.1%}] {stage}")
|
||||
|
||||
result = compute_deflection(points, roi_aabb=roi_aabb, progress=on_progress)
|
||||
```
|
||||
|
||||
阶段顺序：`preprocess → frame → beam_tracking → line_fitting → reference → deflection`。
|
||||
|
||||
`run_measurement_pipeline` 当前未透传 progress 回调，需要时请直接调 `compute_deflection`。
|
||||
|
||||
---
|
||||
|
||||
## 6. 坐标系约定
|
||||
|
||||
输入点云**必须**满足以下约定，否则结果无意义：
|
||||
|
||||
- **X 轴**：纵向（车长方向）
|
||||
- **Y 轴**：横向（车宽方向）
|
||||
- **Z 轴**：高度方向，**向上为正**
|
||||
- 单位：毫米
|
||||
|
||||
CAD 基准也必须遵循同样的轴向；ROI JSON 中的 bbox 字段在 CAD 坐标系下。
|
||||
扫描点云轴向不同 → 调用前自行做置换 / 旋转。
|
||||
|
||||
---
|
||||
|
||||
## 7. 版本兼容性
|
||||
|
||||
- `__version__` —— 包版本
|
||||
- `ALGORITHM_VERSION` —— 算法版本（升级会改变数值结果）
|
||||
- `SCHEMA_VERSION` —— `report.json` 结构版本（仅在破坏性变更时递增；新字段不递增）
|
||||
|
||||
v1.1 新增了非破坏性的可选字段（`cad_reference / roi_source / roi_aabb / registration / preprocess.passthrough / preprocess.bilateral / roi.source / quality_flags 新值`），`SCHEMA_VERSION` 保持 `"1.0"` 不变。
|
||||
|
||||
---
|
||||
|
||||
## 8. 集成模板
|
||||
|
||||
```python
|
||||
import logging
|
||||
from underframe_deflection import (
|
||||
run_measurement_pipeline,
|
||||
DeflectionError,
|
||||
)
|
||||
|
||||
log = logging.getLogger(__name__)
|
||||
|
||||
|
||||
def measure(scan_path: str, cad_path: str, roi_path: str, out_dir: str) -> dict | None:
|
||||
try:
|
||||
report = run_measurement_pipeline(
|
||||
input_cloud=scan_path,
|
||||
cad_path=cad_path,
|
||||
roi_path=roi_path,
|
||||
out_dir=out_dir,
|
||||
)
|
||||
except DeflectionError as exc:
|
||||
log.error("deflection failed: %s", exc)
|
||||
return None
|
||||
|
||||
flags = report.get("quality_flags") or []
|
||||
if flags:
|
||||
log.warning("quality flags: %s", flags)
|
||||
|
||||
return {
|
||||
"schema_version": report["schema_version"],
|
||||
"algorithm_version": report["algorithm_version"],
|
||||
"left_signed_mm": report["deflection"]["left"].get("selected_measurement", {}).get("signed_deflection_mm"),
|
||||
"right_signed_mm": report["deflection"]["right"].get("selected_measurement", {}).get("signed_deflection_mm"),
|
||||
"registration_rms_mm": report["registration"].get("rms_mm"),
|
||||
"flags": flags,
|
||||
}
|
||||
```
|
||||
61
alg/delivery_v1.1/underframe_deflection/__init__.py
Normal file
61
alg/delivery_v1.1/underframe_deflection/__init__.py
Normal file
@@ -0,0 +1,61 @@
|
||||
"""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
|
||||
|
||||
__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",
|
||||
]
|
||||
18
alg/delivery_v1.1/underframe_deflection/_version.py
Normal file
18
alg/delivery_v1.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.1.0"
|
||||
ALGORITHM_VERSION = "0.1.0"
|
||||
SCHEMA_VERSION = "1.0"
|
||||
|
||||
__all__ = ["__version__", "ALGORITHM_VERSION", "SCHEMA_VERSION"]
|
||||
795
alg/delivery_v1.1/underframe_deflection/api.py
Normal file
795
alg/delivery_v1.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.1/underframe_deflection/artifacts.py
Normal file
270
alg/delivery_v1.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.1/underframe_deflection/beams.py
Normal file
563
alg/delivery_v1.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.1/underframe_deflection/cad_io.py
Normal file
164
alg/delivery_v1.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"]
|
||||
83
alg/delivery_v1.1/underframe_deflection/cli.py
Normal file
83
alg/delivery_v1.1/underframe_deflection/cli.py
Normal file
@@ -0,0 +1,83 @@
|
||||
"""Command-line entry point installed by ``pyproject.toml``.
|
||||
|
||||
This mirrors the project-root ``run_underframe_measurement.py`` script so
|
||||
that ``pip install`` exposes a console command (``underframe-measure``).
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import argparse
|
||||
|
||||
from .pipeline import run_measurement_pipeline
|
||||
|
||||
|
||||
def parse_args() -> argparse.Namespace:
|
||||
parser = argparse.ArgumentParser(
|
||||
description="Measure underframe longitudinal beam deflection from a point cloud.",
|
||||
)
|
||||
parser.add_argument(
|
||||
"--input-cloud",
|
||||
required=True,
|
||||
help="Path to input scan point cloud (.ply or .xyz).",
|
||||
)
|
||||
parser.add_argument(
|
||||
"--out-dir",
|
||||
required=True,
|
||||
help="Directory for report.json/.xlsx, CSV files, PLY overlays, and PNG plots.",
|
||||
)
|
||||
parser.add_argument(
|
||||
"--cad",
|
||||
default=None,
|
||||
help="Optional CAD reference (.ply/.xyz/.stl/.obj) used as the registration baseline.",
|
||||
)
|
||||
parser.add_argument(
|
||||
"--roi",
|
||||
default=None,
|
||||
help="Optional ROI JSON file holding an axis-aligned bbox in the CAD frame.",
|
||||
)
|
||||
parser.add_argument(
|
||||
"--config",
|
||||
default=None,
|
||||
help="Optional JSON config override file.",
|
||||
)
|
||||
parser.add_argument(
|
||||
"--profile",
|
||||
default="auto",
|
||||
choices=["auto", "M", "TC", "TP", "custom"],
|
||||
help="Optional profile for scale-related tuning.",
|
||||
)
|
||||
parser.add_argument(
|
||||
"--skip-registration",
|
||||
action="store_true",
|
||||
help="Bypass ICP even when --cad is supplied (useful for debugging).",
|
||||
)
|
||||
parser.add_argument(
|
||||
"--no-bilateral",
|
||||
action="store_true",
|
||||
help="Disable the bilateral denoising step.",
|
||||
)
|
||||
parser.add_argument(
|
||||
"--skip-excel",
|
||||
action="store_true",
|
||||
help="Skip writing the .xlsx error report.",
|
||||
)
|
||||
return parser.parse_args()
|
||||
|
||||
|
||||
def main() -> None:
|
||||
args = parse_args()
|
||||
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,
|
||||
)
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
358
alg/delivery_v1.1/underframe_deflection/config.py
Normal file
358
alg/delivery_v1.1/underframe_deflection/config.py
Normal file
@@ -0,0 +1,358 @@
|
||||
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,
|
||||
},
|
||||
"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,
|
||||
},
|
||||
"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.1/underframe_deflection/excel_report.py
Normal file
251
alg/delivery_v1.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.1/underframe_deflection/exceptions.py
Normal file
40
alg/delivery_v1.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",
|
||||
]
|
||||
265
alg/delivery_v1.1/underframe_deflection/fitting.py
Normal file
265
alg/delivery_v1.1/underframe_deflection/fitting.py
Normal file
@@ -0,0 +1,265 @@
|
||||
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 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"]
|
||||
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.1/underframe_deflection/frame.py
Normal file
335
alg/delivery_v1.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
|
||||
88
alg/delivery_v1.1/underframe_deflection/io.py
Normal file
88
alg/delivery_v1.1/underframe_deflection/io.py
Normal file
@@ -0,0 +1,88 @@
|
||||
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_ascii_ply(path)
|
||||
if suffix in {".xyz", ".txt", ".csv"}:
|
||||
return np.loadtxt(path, dtype=np.float64)
|
||||
raise ValueError(f"Unsupported point cloud format: {path.suffix}")
|
||||
|
||||
|
||||
def _read_ascii_ply(path: Path) -> np.ndarray:
|
||||
header_lines = 0
|
||||
vertex_count = None
|
||||
with path.open("r", encoding="ascii", errors="strict") as fh:
|
||||
for line in fh:
|
||||
header_lines += 1
|
||||
stripped = line.strip()
|
||||
if stripped.startswith("element vertex"):
|
||||
vertex_count = int(stripped.split()[-1])
|
||||
if stripped == "end_header":
|
||||
break
|
||||
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 vertex_count is not None and len(points) != vertex_count:
|
||||
raise ValueError(
|
||||
f"PLY vertex count mismatch: header={vertex_count}, data={len(points)}"
|
||||
)
|
||||
return points
|
||||
|
||||
|
||||
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")
|
||||
256
alg/delivery_v1.1/underframe_deflection/pipeline.py
Normal file
256
alg/delivery_v1.1/underframe_deflection/pipeline.py
Normal file
@@ -0,0 +1,256 @@
|
||||
"""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 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,
|
||||
) -> 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)
|
||||
|
||||
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.1/underframe_deflection/ply_overlay.py
Normal file
216
alg/delivery_v1.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),
|
||||
}
|
||||
238
alg/delivery_v1.1/underframe_deflection/preprocess.py
Normal file
238
alg/delivery_v1.1/underframe_deflection/preprocess.py
Normal file
@@ -0,0 +1,238 @@
|
||||
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 from 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 sensor noise along the surface normal is suppressed.
|
||||
"""
|
||||
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)
|
||||
|
||||
current = points.astype(np.float64, copy=True)
|
||||
for _ in range(int(iterations)):
|
||||
tree = cKDTree(current)
|
||||
distances, indices = tree.query(current, k=effective_k)
|
||||
if distances.ndim == 1: # k == 1 edge case
|
||||
distances = distances[:, None]
|
||||
indices = indices[:, None]
|
||||
neighbor_points = current[indices] # (N, k, 3)
|
||||
neighbor_normals = normals[indices] # (N, k, 3)
|
||||
offsets = neighbor_points - current[:, None, :] # (N, k, 3)
|
||||
own_normals = normals[:, None, :] # (N, 1, 3)
|
||||
normal_offset = np.einsum("nkd,nkd->nk", offsets, own_normals)
|
||||
spatial_w = np.exp(-(distances ** 2) * inv_2sd2)
|
||||
dots = np.clip(np.einsum("nkd,nkd->nk", neighbor_normals, own_normals), -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)
|
||||
valid = weight_sum > 1e-12
|
||||
delta = np.zeros(len(current), dtype=np.float64)
|
||||
delta[valid] = (
|
||||
(weights[valid] * normal_offset[valid]).sum(axis=1) / weight_sum[valid]
|
||||
)
|
||||
current = current + delta[:, None] * normals
|
||||
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)
|
||||
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)),
|
||||
)
|
||||
|
||||
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": bool(bilateral_cfg.get("enabled") and len(radius_points) >= 8),
|
||||
"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.1/underframe_deflection/py.typed
Normal file
0
alg/delivery_v1.1/underframe_deflection/py.typed
Normal file
68
alg/delivery_v1.1/underframe_deflection/pyproject.toml
Normal file
68
alg/delivery_v1.1/underframe_deflection/pyproject.toml
Normal file
@@ -0,0 +1,68 @@
|
||||
[build-system]
|
||||
requires = ["setuptools>=68", "wheel"]
|
||||
build-backend = "setuptools.build_meta"
|
||||
|
||||
[project]
|
||||
name = "underframe-deflection"
|
||||
version = "1.1.0"
|
||||
description = "High-speed train underframe longitudinal beam deflection measurement from scan point clouds + CAD reference."
|
||||
readme = "README.md"
|
||||
requires-python = ">=3.10"
|
||||
license = { text = "Proprietary" }
|
||||
authors = [
|
||||
{ name = "PKU High-Speed Train Project Team" },
|
||||
]
|
||||
keywords = ["point-cloud", "deflection", "ICP", "CAD", "metrology", "high-speed-train"]
|
||||
classifiers = [
|
||||
"Development Status :: 4 - Beta",
|
||||
"Intended Audience :: Manufacturing",
|
||||
"Programming Language :: Python :: 3",
|
||||
"Programming Language :: Python :: 3.10",
|
||||
"Programming Language :: Python :: 3.11",
|
||||
"Programming Language :: Python :: 3.12",
|
||||
"Operating System :: OS Independent",
|
||||
"Topic :: Scientific/Engineering",
|
||||
"Topic :: Scientific/Engineering :: Image Processing",
|
||||
]
|
||||
|
||||
# Core dependencies — required for compute_deflection() and registration.
|
||||
dependencies = [
|
||||
"numpy>=1.23,<3.0",
|
||||
"scipy>=1.10,<2.0",
|
||||
]
|
||||
|
||||
[project.optional-dependencies]
|
||||
# Required by render_plots() and run_measurement_pipeline() (writes PNGs).
|
||||
viz = ["matplotlib>=3.5,<4.0"]
|
||||
|
||||
# Required by write_excel_report(); pipeline degrades gracefully if missing.
|
||||
excel = ["openpyxl>=3.1,<4.0"]
|
||||
|
||||
# Convenience meta-extra that pulls everything an integrator usually wants.
|
||||
all = [
|
||||
"matplotlib>=3.5,<4.0",
|
||||
"openpyxl>=3.1,<4.0",
|
||||
]
|
||||
|
||||
[project.scripts]
|
||||
underframe-measure = "underframe_deflection.cli:main"
|
||||
|
||||
[project.urls]
|
||||
# Fill in your internal hosting URLs before publishing.
|
||||
# Homepage = "https://example.invalid/underframe-deflection"
|
||||
# Issues = "https://example.invalid/underframe-deflection/issues"
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# Package layout
|
||||
#
|
||||
# This pyproject.toml lives inside the package directory itself
|
||||
# (delivery_v1.1/underframe_deflection/), so the source root and the
|
||||
# package source are the same folder.
|
||||
# ---------------------------------------------------------------------------
|
||||
[tool.setuptools]
|
||||
packages = ["underframe_deflection"]
|
||||
package-dir = { "underframe_deflection" = "." }
|
||||
include-package-data = false
|
||||
|
||||
[tool.setuptools.package-data]
|
||||
underframe_deflection = ["py.typed"]
|
||||
606
alg/delivery_v1.1/underframe_deflection/reference.py
Normal file
606
alg/delivery_v1.1/underframe_deflection/reference.py
Normal file
@@ -0,0 +1,606 @@
|
||||
from __future__ import annotations
|
||||
|
||||
from itertools import product
|
||||
|
||||
import numpy as np
|
||||
|
||||
from .fitting import evaluate_line
|
||||
from .preprocess import estimate_normals
|
||||
from .structures import FittedLine, HoleCandidate, ReferenceLine
|
||||
|
||||
|
||||
AXIS_INDEX = {"x": 0, "y": 1, "z": 2}
|
||||
|
||||
|
||||
def _circle_from_three_points(points_2d: np.ndarray) -> tuple[np.ndarray, float] | None:
|
||||
p1, p2, p3 = points_2d
|
||||
temp = p2[0] ** 2 + p2[1] ** 2
|
||||
bc = (p1[0] ** 2 + p1[1] ** 2 - temp) / 2.0
|
||||
cd = (temp - p3[0] ** 2 - p3[1] ** 2) / 2.0
|
||||
det = (p1[0] - p2[0]) * (p2[1] - p3[1]) - (p2[0] - p3[0]) * (p1[1] - p2[1])
|
||||
if abs(det) < 1e-6:
|
||||
return None
|
||||
center_x = (bc * (p2[1] - p3[1]) - cd * (p1[1] - p2[1])) / det
|
||||
center_y = ((p1[0] - p2[0]) * cd - (p2[0] - p3[0]) * bc) / det
|
||||
center = np.array([center_x, center_y], dtype=np.float64)
|
||||
radius = float(np.linalg.norm(center - p1))
|
||||
return center, radius
|
||||
|
||||
|
||||
def _circle_coverage(angles: np.ndarray, bins: int) -> float:
|
||||
wrapped = np.mod(angles, 2.0 * np.pi)
|
||||
hist, _ = np.histogram(wrapped, bins=bins, range=(0.0, 2.0 * np.pi))
|
||||
return float(np.count_nonzero(hist) / max(bins, 1))
|
||||
|
||||
|
||||
def _cluster_labels(features: np.ndarray, eps: float, min_samples: int) -> np.ndarray:
|
||||
if len(features) == 0:
|
||||
return np.zeros(0, dtype=np.int32)
|
||||
labels = np.full(len(features), -1, dtype=np.int32)
|
||||
cluster_id = 0
|
||||
for index in range(len(features)):
|
||||
if labels[index] >= 0:
|
||||
continue
|
||||
seed_neighbors = np.where(
|
||||
np.linalg.norm(features - features[index], axis=1) <= eps
|
||||
)[0]
|
||||
if len(seed_neighbors) < min_samples:
|
||||
continue
|
||||
labels[seed_neighbors] = cluster_id
|
||||
queue = list(seed_neighbors.tolist())
|
||||
while queue:
|
||||
current = queue.pop()
|
||||
current_neighbors = np.where(
|
||||
np.linalg.norm(features - features[current], axis=1) <= eps
|
||||
)[0]
|
||||
if len(current_neighbors) < min_samples:
|
||||
continue
|
||||
for neighbor in current_neighbors:
|
||||
if labels[neighbor] < 0:
|
||||
labels[neighbor] = cluster_id
|
||||
queue.append(int(neighbor))
|
||||
cluster_id += 1
|
||||
return labels
|
||||
|
||||
|
||||
def _detect_circle_proposals(
|
||||
points: np.ndarray,
|
||||
normals: np.ndarray,
|
||||
config: dict,
|
||||
*,
|
||||
allowed_axes: list[str] | None = None,
|
||||
) -> list[HoleCandidate]:
|
||||
ref_cfg = config["reference"]
|
||||
rng = np.random.default_rng(42)
|
||||
proposals: list[dict] = []
|
||||
if len(points) < ref_cfg["min_inliers"]:
|
||||
return []
|
||||
allowed_axis_set = set(allowed_axes or AXIS_INDEX.keys())
|
||||
|
||||
for axis_name, axis_index in AXIS_INDEX.items():
|
||||
if axis_name not in allowed_axis_set:
|
||||
continue
|
||||
mask = np.abs(normals[:, axis_index]) <= ref_cfg["normal_axis_max_alignment"]
|
||||
axis_points = points[mask] if np.count_nonzero(mask) >= ref_cfg["min_inliers"] else points
|
||||
if len(axis_points) < ref_cfg["min_inliers"]:
|
||||
continue
|
||||
other_axes = [idx for idx in range(3) if idx != axis_index]
|
||||
projected = axis_points[:, other_axes]
|
||||
if len(projected) > ref_cfg["circle_sample_size"]:
|
||||
sample_indices = rng.choice(len(projected), size=ref_cfg["circle_sample_size"], replace=False)
|
||||
projected_sample = projected[sample_indices]
|
||||
else:
|
||||
projected_sample = projected
|
||||
|
||||
for _ in range(ref_cfg["circle_trials"]):
|
||||
triple_indices = rng.choice(len(projected_sample), size=3, replace=False)
|
||||
circle = _circle_from_three_points(projected_sample[triple_indices])
|
||||
if circle is None:
|
||||
continue
|
||||
center_2d, radius = circle
|
||||
if not (ref_cfg["radius_min_mm"] <= radius <= ref_cfg["radius_max_mm"]):
|
||||
continue
|
||||
radial_distance = np.linalg.norm(projected - center_2d[None, :], axis=1)
|
||||
residual = np.abs(radial_distance - radius)
|
||||
inlier_mask = residual <= ref_cfg["fit_tolerance_mm"]
|
||||
inlier_count = int(np.count_nonzero(inlier_mask))
|
||||
if inlier_count < ref_cfg["min_inliers"]:
|
||||
continue
|
||||
angles = np.arctan2(
|
||||
projected[inlier_mask, 1] - center_2d[1],
|
||||
projected[inlier_mask, 0] - center_2d[0],
|
||||
)
|
||||
coverage_ratio = _circle_coverage(angles, ref_cfg["coverage_bin_count"])
|
||||
mean_residual = float(np.mean(residual[inlier_mask]))
|
||||
score = (
|
||||
inlier_count / max(len(projected), 1)
|
||||
+ 1.35 * coverage_ratio
|
||||
- mean_residual / max(ref_cfg["fit_tolerance_mm"], 1.0)
|
||||
)
|
||||
center_3d = np.zeros(3, dtype=np.float64)
|
||||
center_3d[other_axes] = center_2d
|
||||
center_3d[axis_index] = float(np.median(axis_points[inlier_mask, axis_index]))
|
||||
proposals.append(
|
||||
{
|
||||
"axis": axis_name,
|
||||
"center": center_3d,
|
||||
"radius": radius,
|
||||
"score": score,
|
||||
"inlier_count": inlier_count,
|
||||
"coverage_ratio": coverage_ratio,
|
||||
"residual": mean_residual,
|
||||
}
|
||||
)
|
||||
|
||||
if not proposals:
|
||||
return []
|
||||
|
||||
proposal_features = np.asarray(
|
||||
[
|
||||
np.hstack(
|
||||
[
|
||||
proposal["center"] / max(ref_cfg["cluster_eps_mm"], 1.0),
|
||||
proposal["radius"] / max(ref_cfg["cluster_eps_mm"], 1.0),
|
||||
]
|
||||
)
|
||||
for proposal in proposals
|
||||
]
|
||||
)
|
||||
candidates: list[HoleCandidate] = []
|
||||
labels = _cluster_labels(
|
||||
proposal_features,
|
||||
eps=1.0,
|
||||
min_samples=ref_cfg["cluster_min_samples"],
|
||||
)
|
||||
for label in sorted(set(labels)):
|
||||
if label < 0:
|
||||
continue
|
||||
members = [proposal for proposal, member_label in zip(proposals, labels) if member_label == label]
|
||||
scores = np.asarray([member["score"] for member in members], dtype=np.float64)
|
||||
weights = np.clip(scores - scores.min() + 1e-3, 1e-3, None)
|
||||
centers = np.asarray([member["center"] for member in members], dtype=np.float64)
|
||||
radii = np.asarray([member["radius"] for member in members], dtype=np.float64)
|
||||
best_member = max(members, key=lambda item: item["score"])
|
||||
center = np.average(centers, axis=0, weights=weights)
|
||||
radius = float(np.average(radii, weights=weights))
|
||||
residual_factor = float(
|
||||
np.exp(
|
||||
-best_member["residual"]
|
||||
/ max(ref_cfg["fit_tolerance_mm"] * 3.5, 1.0)
|
||||
)
|
||||
)
|
||||
radius_factor = float(
|
||||
np.exp(
|
||||
-max(radius - ref_cfg["radius_max_mm"] * 0.45, 0.0)
|
||||
/ max(ref_cfg["radius_max_mm"] * 0.35, 1.0)
|
||||
)
|
||||
)
|
||||
confidence = float(
|
||||
np.clip(
|
||||
(
|
||||
0.25
|
||||
+ 0.22 * best_member["coverage_ratio"]
|
||||
+ 0.18 * best_member["inlier_count"] / max(len(points), 1)
|
||||
+ 0.15 * (best_member["score"] / 3.0)
|
||||
)
|
||||
* residual_factor
|
||||
* radius_factor
|
||||
+ 0.1,
|
||||
0.0,
|
||||
1.0,
|
||||
)
|
||||
)
|
||||
candidates.append(
|
||||
HoleCandidate(
|
||||
side="",
|
||||
region="",
|
||||
axis=best_member["axis"],
|
||||
center=center,
|
||||
radius=radius,
|
||||
score=float(best_member["score"]),
|
||||
inlier_count=int(best_member["inlier_count"]),
|
||||
coverage_ratio=float(best_member["coverage_ratio"]),
|
||||
residual=float(best_member["residual"]),
|
||||
confidence=confidence,
|
||||
)
|
||||
)
|
||||
candidates.sort(key=lambda item: item.score, reverse=True)
|
||||
return candidates
|
||||
|
||||
|
||||
def _score_candidates_against_beam(
|
||||
candidates: list[HoleCandidate],
|
||||
fitted_line: FittedLine,
|
||||
side: str,
|
||||
region: str,
|
||||
config: dict,
|
||||
region_bounds: dict[str, float],
|
||||
) -> list[HoleCandidate]:
|
||||
if not candidates:
|
||||
return []
|
||||
x_values = np.asarray([candidate.center[0] for candidate in candidates], dtype=np.float64)
|
||||
y_expected, z_expected, _ = evaluate_line(fitted_line, x_values)
|
||||
ref_cfg = config["reference"]
|
||||
scored: list[HoleCandidate] = []
|
||||
region_span = max(region_bounds["x_hi"] - region_bounds["x_lo"], 1.0)
|
||||
preferred_axes = set(ref_cfg.get("preferred_axes", []))
|
||||
axis_bonus_cfg = ref_cfg.get("axis_bonus", {})
|
||||
for index, candidate in enumerate(candidates):
|
||||
lateral_distance = abs(candidate.center[1] - y_expected[index])
|
||||
vertical_distance = abs(candidate.center[2] - z_expected[index])
|
||||
underside_bottom = candidate.center[2] - candidate.radius
|
||||
underside_distance = abs(underside_bottom - z_expected[index])
|
||||
if region == "front":
|
||||
extremity = np.clip(
|
||||
(region_bounds["x_hi"] - candidate.center[0]) / region_span,
|
||||
0.0,
|
||||
1.0,
|
||||
)
|
||||
else:
|
||||
extremity = np.clip(
|
||||
(candidate.center[0] - region_bounds["x_lo"]) / region_span,
|
||||
0.0,
|
||||
1.0,
|
||||
)
|
||||
penalty = (
|
||||
ref_cfg["lateral_penalty_weight"]
|
||||
* lateral_distance
|
||||
/ max(ref_cfg["lateral_margin_mm"], 1.0)
|
||||
+ ref_cfg["vertical_center_penalty_weight"]
|
||||
* vertical_distance
|
||||
/ max(ref_cfg["underside_upper_margin_mm"], 1.0)
|
||||
+ ref_cfg["underside_penalty_weight"]
|
||||
* underside_distance
|
||||
/ max(ref_cfg["underside_upper_margin_mm"], 1.0)
|
||||
)
|
||||
adjusted_score = (
|
||||
candidate.score
|
||||
+ axis_bonus_cfg.get(candidate.axis, 0.0)
|
||||
+ ref_cfg["extremity_reward"] * extremity
|
||||
- penalty
|
||||
)
|
||||
axis_factor = 1.0 if candidate.axis in preferred_axes else 0.72
|
||||
confidence = float(
|
||||
np.clip(
|
||||
candidate.confidence
|
||||
* axis_factor
|
||||
* (0.72 + 0.28 * extremity)
|
||||
* np.exp(-0.22 * penalty),
|
||||
0.0,
|
||||
1.0,
|
||||
)
|
||||
)
|
||||
scored.append(
|
||||
HoleCandidate(
|
||||
side=side,
|
||||
region=region,
|
||||
axis=candidate.axis,
|
||||
center=candidate.center,
|
||||
radius=candidate.radius,
|
||||
score=float(adjusted_score),
|
||||
inlier_count=candidate.inlier_count,
|
||||
coverage_ratio=candidate.coverage_ratio,
|
||||
residual=candidate.residual,
|
||||
confidence=confidence,
|
||||
)
|
||||
)
|
||||
scored.sort(key=lambda item: item.score, reverse=True)
|
||||
return scored
|
||||
|
||||
|
||||
def _collect_region_hole_candidates(
|
||||
region_points: np.ndarray,
|
||||
region_normals: np.ndarray,
|
||||
config: dict,
|
||||
allowed_axes: list[str],
|
||||
) -> list[HoleCandidate]:
|
||||
if len(region_points) == 0:
|
||||
return []
|
||||
ref_cfg = config["reference"]
|
||||
candidate_pool: list[HoleCandidate] = []
|
||||
x_min = float(region_points[:, 0].min())
|
||||
x_max = float(region_points[:, 0].max())
|
||||
span = max(x_max - x_min, 1.0)
|
||||
window_count = max(int(ref_cfg.get("x_window_count", 1)), 1)
|
||||
overlap = float(np.clip(ref_cfg.get("x_window_overlap_ratio", 0.0), 0.0, 0.9))
|
||||
base_window = span / window_count
|
||||
window_length = base_window * (1.0 + overlap)
|
||||
|
||||
candidate_pool.extend(
|
||||
_detect_circle_proposals(
|
||||
region_points,
|
||||
region_normals,
|
||||
config,
|
||||
allowed_axes=allowed_axes,
|
||||
)
|
||||
)
|
||||
|
||||
if window_count <= 1:
|
||||
return candidate_pool
|
||||
|
||||
for window_index in range(window_count):
|
||||
x_lo = x_min + window_index * base_window
|
||||
x_hi = x_lo + window_length
|
||||
mask = (region_points[:, 0] >= x_lo) & (region_points[:, 0] <= x_hi)
|
||||
sub_points = region_points[mask]
|
||||
if len(sub_points) < ref_cfg["min_inliers"] * 2:
|
||||
continue
|
||||
sub_normals = region_normals[mask]
|
||||
candidate_pool.extend(
|
||||
_detect_circle_proposals(
|
||||
sub_points,
|
||||
sub_normals,
|
||||
config,
|
||||
allowed_axes=allowed_axes,
|
||||
)
|
||||
)
|
||||
|
||||
deduped: list[HoleCandidate] = []
|
||||
for candidate in sorted(candidate_pool, key=lambda item: item.score, reverse=True):
|
||||
if any(
|
||||
candidate.axis == existing.axis
|
||||
and np.linalg.norm(candidate.center - existing.center) <= max(ref_cfg["cluster_eps_mm"] * 0.6, 80.0)
|
||||
and abs(candidate.radius - existing.radius) <= max(ref_cfg["cluster_eps_mm"] * 0.25, 20.0)
|
||||
for existing in deduped
|
||||
):
|
||||
continue
|
||||
deduped.append(candidate)
|
||||
if len(deduped) >= 12:
|
||||
break
|
||||
return deduped
|
||||
|
||||
|
||||
def _region_points_for_hole_search(
|
||||
points_vehicle: np.ndarray,
|
||||
fitted_line: FittedLine,
|
||||
region: str,
|
||||
config: dict,
|
||||
) -> tuple[np.ndarray, dict[str, float]]:
|
||||
ref_cfg = config["reference"]
|
||||
x_min = float(fitted_line.x_support.min())
|
||||
x_max = float(fitted_line.x_support.max())
|
||||
span = max(x_max - x_min, 1.0)
|
||||
region_length = ref_cfg["stable_region_fraction"] * span
|
||||
extension = ref_cfg["search_extension_mm"]
|
||||
vehicle_x_min = float(points_vehicle[:, 0].min())
|
||||
vehicle_x_max = float(points_vehicle[:, 0].max())
|
||||
if region == "front":
|
||||
x_lo = max(vehicle_x_min, x_min - extension)
|
||||
x_hi = min(vehicle_x_max, x_min + region_length)
|
||||
x_eval_lo, x_eval_hi = x_min, min(x_max, x_min + region_length)
|
||||
else:
|
||||
x_lo = max(vehicle_x_min, x_max - region_length)
|
||||
x_hi = min(vehicle_x_max, x_max + extension)
|
||||
x_eval_lo, x_eval_hi = max(x_min, x_max - region_length), x_max
|
||||
if x_eval_hi <= x_eval_lo:
|
||||
x_grid = np.asarray([x_min if region == "front" else x_max], dtype=np.float64)
|
||||
else:
|
||||
x_grid = np.linspace(x_eval_lo, x_eval_hi, 16)
|
||||
y_ref, z_ref, _ = evaluate_line(fitted_line, x_grid)
|
||||
y_center = float(np.median(y_ref))
|
||||
z_center = float(np.median(z_ref))
|
||||
z_lo = max(
|
||||
float(points_vehicle[:, 2].min()),
|
||||
z_center - ref_cfg["underside_lower_margin_mm"],
|
||||
)
|
||||
z_hi = min(
|
||||
float(points_vehicle[:, 2].max()),
|
||||
z_center + ref_cfg["underside_upper_margin_mm"],
|
||||
)
|
||||
mask = (
|
||||
(points_vehicle[:, 0] >= x_lo)
|
||||
& (points_vehicle[:, 0] <= x_hi)
|
||||
& (np.abs(points_vehicle[:, 1] - y_center) <= ref_cfg["lateral_margin_mm"])
|
||||
& (points_vehicle[:, 2] >= z_lo)
|
||||
& (points_vehicle[:, 2] <= z_hi)
|
||||
)
|
||||
return points_vehicle[mask], {
|
||||
"x_lo": float(x_lo),
|
||||
"x_hi": float(x_hi),
|
||||
"y_center": y_center,
|
||||
"z_center": z_center,
|
||||
"z_lo": float(z_lo),
|
||||
"z_hi": float(z_hi),
|
||||
}
|
||||
|
||||
|
||||
def _build_endpoint_reference_line(
|
||||
fitted_line: FittedLine,
|
||||
side: str,
|
||||
) -> ReferenceLine:
|
||||
x_front = float(fitted_line.x_support.min())
|
||||
x_back = float(fitted_line.x_support.max())
|
||||
yz_front, _ = evaluate_reference_line_from_fitted(fitted_line, np.asarray([x_front], dtype=np.float64))
|
||||
yz_back, _ = evaluate_reference_line_from_fitted(fitted_line, np.asarray([x_back], dtype=np.float64))
|
||||
point_front = np.array([x_front, yz_front[0, 0], yz_front[0, 1]], dtype=np.float64)
|
||||
point_back = np.array([x_back, yz_back[0, 0], yz_back[0, 1]], dtype=np.float64)
|
||||
direction = point_back - point_front
|
||||
norm = float(np.linalg.norm(direction))
|
||||
if norm <= 1e-9:
|
||||
return ReferenceLine(
|
||||
side=side,
|
||||
available=False,
|
||||
point_front=None,
|
||||
point_back=None,
|
||||
direction=None,
|
||||
confidence=0.0,
|
||||
residual=None,
|
||||
candidates_front=[],
|
||||
candidates_back=[],
|
||||
alternates=[],
|
||||
)
|
||||
confidence = 1.0
|
||||
residual = float(0.5 * (fitted_line.y_residual_rms + fitted_line.z_residual_rms))
|
||||
return ReferenceLine(
|
||||
side=side,
|
||||
available=True,
|
||||
point_front=point_front,
|
||||
point_back=point_back,
|
||||
direction=direction / norm,
|
||||
confidence=confidence,
|
||||
residual=residual,
|
||||
candidates_front=[],
|
||||
candidates_back=[],
|
||||
alternates=[],
|
||||
)
|
||||
|
||||
|
||||
def evaluate_reference_line_from_fitted(
|
||||
fitted_line: FittedLine,
|
||||
x_grid: np.ndarray,
|
||||
) -> tuple[np.ndarray, np.ndarray]:
|
||||
y, z, confidence = evaluate_line(fitted_line, x_grid)
|
||||
return np.column_stack([y, z]), confidence
|
||||
|
||||
|
||||
def detect_reference_line(
|
||||
points_vehicle: np.ndarray,
|
||||
fitted_line: FittedLine,
|
||||
config: dict,
|
||||
side: str,
|
||||
) -> ReferenceLine:
|
||||
ref_cfg = config["reference"]
|
||||
if ref_cfg.get("mode") == "line_endpoints":
|
||||
return _build_endpoint_reference_line(fitted_line, side)
|
||||
preferred_axes = ref_cfg.get("preferred_axes", ["y", "z"])
|
||||
front_region, front_bounds = _region_points_for_hole_search(points_vehicle, fitted_line, "front", config)
|
||||
back_region, back_bounds = _region_points_for_hole_search(points_vehicle, fitted_line, "back", config)
|
||||
|
||||
front_normals, _ = estimate_normals(front_region, k=min(18, max(len(front_region) - 1, 1)))
|
||||
back_normals, _ = estimate_normals(back_region, k=min(18, max(len(back_region) - 1, 1)))
|
||||
|
||||
front_candidates = _score_candidates_against_beam(
|
||||
_collect_region_hole_candidates(
|
||||
front_region,
|
||||
front_normals,
|
||||
config,
|
||||
allowed_axes=preferred_axes,
|
||||
),
|
||||
fitted_line,
|
||||
side,
|
||||
"front",
|
||||
config,
|
||||
front_bounds,
|
||||
)
|
||||
back_candidates = _score_candidates_against_beam(
|
||||
_collect_region_hole_candidates(
|
||||
back_region,
|
||||
back_normals,
|
||||
config,
|
||||
allowed_axes=preferred_axes,
|
||||
),
|
||||
fitted_line,
|
||||
side,
|
||||
"back",
|
||||
config,
|
||||
back_bounds,
|
||||
)
|
||||
|
||||
if not front_candidates or not back_candidates:
|
||||
return ReferenceLine(
|
||||
side=side,
|
||||
available=False,
|
||||
point_front=None,
|
||||
point_back=None,
|
||||
direction=None,
|
||||
confidence=0.0,
|
||||
residual=None,
|
||||
candidates_front=front_candidates,
|
||||
candidates_back=back_candidates,
|
||||
alternates=[],
|
||||
)
|
||||
|
||||
alternates = []
|
||||
best = None
|
||||
for front_candidate, back_candidate in product(front_candidates[:3], back_candidates[:3]):
|
||||
direction = back_candidate.center - front_candidate.center
|
||||
norm = float(np.linalg.norm(direction))
|
||||
if norm < 1e-6:
|
||||
continue
|
||||
direction_unit = direction / norm
|
||||
x_alignment = abs(direction_unit[0])
|
||||
front_bottom = front_candidate.center[2] - front_candidate.radius
|
||||
back_bottom = back_candidate.center[2] - back_candidate.radius
|
||||
combo_score = (
|
||||
front_candidate.score
|
||||
+ back_candidate.score
|
||||
+ 1.2 * x_alignment
|
||||
- abs(front_candidate.center[1] - back_candidate.center[1]) / max(ref_cfg["lateral_margin_mm"], 1.0)
|
||||
- 0.6 * abs(front_bottom - back_bottom) / max(ref_cfg["underside_upper_margin_mm"], 1.0)
|
||||
+ (0.18 if front_candidate.axis == back_candidate.axis else 0.0)
|
||||
)
|
||||
residual_mean = 0.5 * (front_candidate.residual + back_candidate.residual)
|
||||
combo_confidence = float(
|
||||
np.clip(
|
||||
min(front_candidate.confidence, back_candidate.confidence)
|
||||
* x_alignment
|
||||
* np.exp(-residual_mean / max(ref_cfg["fit_tolerance_mm"] * 3.0, 1.0)),
|
||||
0.0,
|
||||
1.0,
|
||||
)
|
||||
)
|
||||
alternate = {
|
||||
"front_center": front_candidate.center.tolist(),
|
||||
"back_center": back_candidate.center.tolist(),
|
||||
"direction": direction_unit.tolist(),
|
||||
"score": float(combo_score),
|
||||
"confidence": combo_confidence,
|
||||
}
|
||||
alternates.append(alternate)
|
||||
if best is None or combo_score > best[0]:
|
||||
best = (combo_score, front_candidate, back_candidate, direction_unit)
|
||||
|
||||
alternates.sort(key=lambda item: item["score"], reverse=True)
|
||||
if best is None:
|
||||
return ReferenceLine(
|
||||
side=side,
|
||||
available=False,
|
||||
point_front=None,
|
||||
point_back=None,
|
||||
direction=None,
|
||||
confidence=0.0,
|
||||
residual=None,
|
||||
candidates_front=front_candidates,
|
||||
candidates_back=back_candidates,
|
||||
alternates=alternates[:5],
|
||||
)
|
||||
|
||||
_, front_best, back_best, direction = best
|
||||
residual = float((front_best.residual + back_best.residual) / 2.0)
|
||||
confidence = float(
|
||||
np.clip(
|
||||
min(front_best.confidence, back_best.confidence)
|
||||
* abs(direction[0])
|
||||
* np.exp(-residual / max(config["reference"]["fit_tolerance_mm"] * 3.0, 1.0)),
|
||||
0.0,
|
||||
1.0,
|
||||
)
|
||||
)
|
||||
return ReferenceLine(
|
||||
side=side,
|
||||
available=True,
|
||||
point_front=front_best.center,
|
||||
point_back=back_best.center,
|
||||
direction=direction,
|
||||
confidence=confidence,
|
||||
residual=residual,
|
||||
candidates_front=front_candidates,
|
||||
candidates_back=back_candidates,
|
||||
alternates=alternates[:5],
|
||||
)
|
||||
|
||||
|
||||
def evaluate_reference_line(reference_line: ReferenceLine, x_grid: np.ndarray) -> tuple[np.ndarray, np.ndarray]:
|
||||
if not reference_line.available or reference_line.point_front is None or reference_line.point_back is None:
|
||||
return np.full_like(x_grid, np.nan, dtype=np.float64), np.zeros_like(x_grid, dtype=np.float64)
|
||||
front = reference_line.point_front
|
||||
direction = reference_line.direction
|
||||
assert direction is not None
|
||||
dx = direction[0]
|
||||
if abs(dx) < 1e-6:
|
||||
return np.full_like(x_grid, np.nan, dtype=np.float64), np.zeros_like(x_grid, dtype=np.float64)
|
||||
t = (x_grid - front[0]) / dx
|
||||
y = front[1] + t * direction[1]
|
||||
z = front[2] + t * direction[2]
|
||||
confidence = np.full_like(x_grid, reference_line.confidence, dtype=np.float64)
|
||||
return np.column_stack([y, z]), confidence
|
||||
259
alg/delivery_v1.1/underframe_deflection/registration.py
Normal file
259
alg/delivery_v1.1/underframe_deflection/registration.py
Normal file
@@ -0,0 +1,259 @@
|
||||
"""Rigid registration from scan point cloud to CAD reference.
|
||||
|
||||
Two-stage approach:
|
||||
|
||||
1. **Coarse**: centroid alignment + PCA axis matching with sign
|
||||
disambiguation via per-axis skew (third central moment). Resolves
|
||||
global orientation flips without requiring a good initial guess.
|
||||
2. **Fine**: trimmed iterative closest point (ICP) on a downsampled CAD
|
||||
target using ``scipy.spatial.cKDTree`` for nearest-neighbour
|
||||
queries. Reflection-free rotation via SVD with the standard
|
||||
determinant guard.
|
||||
|
||||
The result transforms scan points into the CAD coordinate frame:
|
||||
``cad_point ≈ R @ scan_point + t``.
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
from dataclasses import dataclass
|
||||
|
||||
import numpy as np
|
||||
from scipy.spatial import cKDTree
|
||||
|
||||
from .preprocess import voxel_downsample
|
||||
|
||||
|
||||
@dataclass(slots=True)
|
||||
class RegistrationResult:
|
||||
R: np.ndarray
|
||||
t: np.ndarray
|
||||
rms_mm: float
|
||||
iterations: int
|
||||
inlier_ratio: float
|
||||
converged: bool
|
||||
coarse_R: np.ndarray
|
||||
coarse_t: np.ndarray
|
||||
initial_rms_mm: float
|
||||
|
||||
|
||||
def register_to_cad(
|
||||
scan: np.ndarray,
|
||||
cad: np.ndarray,
|
||||
config: dict,
|
||||
) -> RegistrationResult:
|
||||
"""Rigidly align ``scan`` to ``cad`` and return the transformation.
|
||||
|
||||
``config`` is a fully resolved config dict (must contain the
|
||||
``registration`` section with ``icp_max_corr_mm`` already filled by
|
||||
:func:`underframe_deflection.config.resolve_numeric_parameters`).
|
||||
"""
|
||||
if len(scan) < 3 or len(cad) < 3:
|
||||
raise ValueError("register_to_cad requires at least 3 points in scan and cad.")
|
||||
|
||||
reg_cfg = config["registration"]
|
||||
cad_cfg = config.get("cad", {})
|
||||
|
||||
# CAD-side downsampling — keeps NN queries cheap when the CAD has
|
||||
# millions of points.
|
||||
cad_voxel = float(cad_cfg.get("downsample_voxel_mm", 0.0) or 0.0)
|
||||
target_n = int(reg_cfg.get("icp_downsample_target", 30000))
|
||||
cad_sampled = cad
|
||||
if cad_voxel > 0.0:
|
||||
cad_sampled = voxel_downsample(cad_sampled, cad_voxel)
|
||||
if target_n > 0 and len(cad_sampled) > target_n:
|
||||
idx = np.linspace(0, len(cad_sampled) - 1, target_n, dtype=int)
|
||||
cad_sampled = cad_sampled[idx]
|
||||
|
||||
R0, t0 = _coarse_align_pca(
|
||||
scan,
|
||||
cad_sampled,
|
||||
disambiguate_sign=bool(reg_cfg.get("pca_sign_disambiguation", True)),
|
||||
)
|
||||
scan_coarse = scan @ R0.T + t0
|
||||
|
||||
max_corr = float(reg_cfg.get("icp_max_corr_mm", 25.0))
|
||||
max_iter = int(reg_cfg.get("icp_max_iter", 60))
|
||||
tol_mm = float(reg_cfg.get("icp_tol_mm", 0.05))
|
||||
tol_rel = float(reg_cfg.get("icp_tol_rel", 1.0e-4))
|
||||
trim = float(reg_cfg.get("trim_ratio", 0.2))
|
||||
|
||||
# ICP source downsampling — keeps query time bounded.
|
||||
src_target = target_n if target_n > 0 else len(scan_coarse)
|
||||
if len(scan_coarse) > src_target:
|
||||
idx = np.linspace(0, len(scan_coarse) - 1, src_target, dtype=int)
|
||||
scan_for_icp = scan_coarse[idx]
|
||||
else:
|
||||
scan_for_icp = scan_coarse
|
||||
|
||||
tree = cKDTree(cad_sampled)
|
||||
R = np.eye(3, dtype=np.float64)
|
||||
t = np.zeros(3, dtype=np.float64)
|
||||
prev_rms = float("inf")
|
||||
current = scan_for_icp.copy()
|
||||
iterations_run = 0
|
||||
inlier_ratio = 0.0
|
||||
rms = float("inf")
|
||||
converged = False
|
||||
|
||||
initial_rms = _correspondence_rms(current, tree, max_corr)
|
||||
|
||||
for iteration in range(1, max_iter + 1):
|
||||
iterations_run = iteration
|
||||
distances, neighbours = tree.query(current, k=1)
|
||||
keep = distances <= max_corr
|
||||
if trim > 0.0 and np.any(keep):
|
||||
kept_d = distances[keep]
|
||||
if len(kept_d) > 8:
|
||||
cutoff = float(np.quantile(kept_d, 1.0 - trim))
|
||||
keep = keep & (distances <= cutoff)
|
||||
if np.count_nonzero(keep) < 3:
|
||||
break
|
||||
src = current[keep]
|
||||
dst = cad_sampled[neighbours[keep]]
|
||||
delta_R, delta_t = _kabsch(src, dst)
|
||||
current = current @ delta_R.T + delta_t
|
||||
R = delta_R @ R
|
||||
t = delta_R @ t + delta_t
|
||||
|
||||
residuals = current - cad_sampled[neighbours]
|
||||
residual_norms = np.linalg.norm(residuals[keep], axis=1)
|
||||
rms = float(np.sqrt(np.mean(residual_norms ** 2))) if len(residual_norms) else float("inf")
|
||||
inlier_ratio = float(np.count_nonzero(keep) / max(len(current), 1))
|
||||
|
||||
if abs(prev_rms - rms) <= tol_mm or (
|
||||
prev_rms > 0.0 and abs(prev_rms - rms) / max(prev_rms, 1e-9) <= tol_rel
|
||||
):
|
||||
converged = True
|
||||
break
|
||||
prev_rms = rms
|
||||
|
||||
R_total = R @ R0
|
||||
t_total = R @ t0 + t
|
||||
return RegistrationResult(
|
||||
R=R_total,
|
||||
t=t_total,
|
||||
rms_mm=rms,
|
||||
iterations=iterations_run,
|
||||
inlier_ratio=inlier_ratio,
|
||||
converged=converged,
|
||||
coarse_R=R0,
|
||||
coarse_t=t0,
|
||||
initial_rms_mm=initial_rms,
|
||||
)
|
||||
|
||||
|
||||
def apply_transform(points: np.ndarray, R: np.ndarray, t: np.ndarray) -> np.ndarray:
|
||||
"""Apply ``R @ p + t`` to a point cloud (broadcasted)."""
|
||||
return points @ R.T + t
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# Internals
|
||||
# ---------------------------------------------------------------------------
|
||||
|
||||
|
||||
def _coarse_align_pca(
|
||||
scan: np.ndarray,
|
||||
cad: np.ndarray,
|
||||
*,
|
||||
disambiguate_sign: bool,
|
||||
) -> tuple[np.ndarray, np.ndarray]:
|
||||
"""Return (R, t) that map ``scan`` into the CAD PCA frame."""
|
||||
scan_axes, scan_center = _pca_basis(scan)
|
||||
cad_axes, cad_center = _pca_basis(cad)
|
||||
|
||||
if disambiguate_sign:
|
||||
scan_axes = _resolve_sign_by_skew(scan, scan_center, scan_axes, cad, cad_center, cad_axes)
|
||||
|
||||
# R maps scan → CAD: scan_axes^T rotates scan into its own PCA basis,
|
||||
# cad_axes rotates back into the CAD frame.
|
||||
R = cad_axes @ scan_axes.T
|
||||
if np.linalg.det(R) < 0:
|
||||
scan_axes[:, -1] *= -1.0
|
||||
R = cad_axes @ scan_axes.T
|
||||
t = cad_center - R @ scan_center
|
||||
return R, t
|
||||
|
||||
|
||||
def _pca_basis(points: np.ndarray) -> tuple[np.ndarray, np.ndarray]:
|
||||
center = points.mean(axis=0)
|
||||
centered = points - center
|
||||
covariance = centered.T @ centered / max(len(points), 1)
|
||||
eigenvalues, eigenvectors = np.linalg.eigh(covariance)
|
||||
order = np.argsort(eigenvalues)[::-1]
|
||||
axes = eigenvectors[:, order]
|
||||
# ``axes`` is (3, 3) where columns are unit eigenvectors sorted by
|
||||
# decreasing variance. Treat columns as the new basis directions.
|
||||
return axes, center
|
||||
|
||||
|
||||
def _resolve_sign_by_skew(
|
||||
scan: np.ndarray,
|
||||
scan_center: np.ndarray,
|
||||
scan_axes: np.ndarray,
|
||||
cad: np.ndarray,
|
||||
cad_center: np.ndarray,
|
||||
cad_axes: np.ndarray,
|
||||
) -> np.ndarray:
|
||||
"""Pick eigenvector signs that match the chirality of ``cad``."""
|
||||
scan_proj = (scan - scan_center) @ scan_axes
|
||||
cad_proj = (cad - cad_center) @ cad_axes
|
||||
cad_skew = np.array([_third_moment(cad_proj[:, i]) for i in range(3)])
|
||||
best_signs = np.array([1.0, 1.0, 1.0])
|
||||
best_score = np.inf
|
||||
for s0 in (-1.0, 1.0):
|
||||
for s1 in (-1.0, 1.0):
|
||||
for s2 in (-1.0, 1.0):
|
||||
signs = np.array([s0, s1, s2])
|
||||
if np.prod(signs) < 0:
|
||||
# Disallow reflections; the kabsch det guard will
|
||||
# cover the residual cases.
|
||||
continue
|
||||
scan_skew = np.array(
|
||||
[signs[i] ** 3 * _third_moment(scan_proj[:, i]) for i in range(3)]
|
||||
)
|
||||
score = float(np.sum(np.abs(scan_skew - cad_skew)))
|
||||
if score < best_score:
|
||||
best_score = score
|
||||
best_signs = signs
|
||||
return scan_axes * best_signs[None, :]
|
||||
|
||||
|
||||
def _third_moment(values: np.ndarray) -> float:
|
||||
if len(values) == 0:
|
||||
return 0.0
|
||||
std = float(np.std(values))
|
||||
if std < 1e-9:
|
||||
return 0.0
|
||||
centered = values - values.mean()
|
||||
return float(np.mean(centered ** 3) / (std ** 3))
|
||||
|
||||
|
||||
def _kabsch(src: np.ndarray, dst: np.ndarray) -> tuple[np.ndarray, np.ndarray]:
|
||||
"""Closed-form rigid-body fit (no reflections)."""
|
||||
src_center = src.mean(axis=0)
|
||||
dst_center = dst.mean(axis=0)
|
||||
src_centered = src - src_center
|
||||
dst_centered = dst - dst_center
|
||||
H = src_centered.T @ dst_centered
|
||||
U, _, Vt = np.linalg.svd(H)
|
||||
D = np.eye(3, dtype=np.float64)
|
||||
if np.linalg.det(Vt.T @ U.T) < 0:
|
||||
D[-1, -1] = -1.0
|
||||
R = Vt.T @ D @ U.T
|
||||
t = dst_center - R @ src_center
|
||||
return R, t
|
||||
|
||||
|
||||
def _correspondence_rms(points: np.ndarray, tree: cKDTree, max_corr: float) -> float:
|
||||
if len(points) == 0:
|
||||
return float("inf")
|
||||
distances, _ = tree.query(points, k=1)
|
||||
keep = distances <= max_corr
|
||||
if not np.any(keep):
|
||||
return float(np.sqrt(np.mean(distances ** 2)))
|
||||
return float(np.sqrt(np.mean(distances[keep] ** 2)))
|
||||
|
||||
|
||||
__all__ = ["RegistrationResult", "register_to_cad", "apply_transform"]
|
||||
21
alg/delivery_v1.1/underframe_deflection/requirements.txt
Normal file
21
alg/delivery_v1.1/underframe_deflection/requirements.txt
Normal file
@@ -0,0 +1,21 @@
|
||||
# underframe_deflection v1.1 — runtime dependencies
|
||||
#
|
||||
# Core (mandatory for compute_deflection and registration):
|
||||
numpy>=1.23,<3.0
|
||||
scipy>=1.10,<2.0
|
||||
|
||||
# Visualization (required by artifacts.render_plots and the
|
||||
# `run_measurement_pipeline` end-to-end flow that writes PNGs):
|
||||
matplotlib>=3.5,<4.0
|
||||
|
||||
# Excel report writer (required by artifacts/excel_report.write_excel_report;
|
||||
# pipeline degrades gracefully if missing, but production deployments should
|
||||
# install it):
|
||||
openpyxl>=3.1,<4.0
|
||||
|
||||
# --- Reference versions used for validation ----------------------------------
|
||||
# numpy 1.26.4
|
||||
# scipy 1.15.3
|
||||
# matplotlib 3.10.8
|
||||
# openpyxl 3.1.2
|
||||
# Python 3.10.9 (Windows 11 x64)
|
||||
97
alg/delivery_v1.1/underframe_deflection/roi.py
Normal file
97
alg/delivery_v1.1/underframe_deflection/roi.py
Normal file
@@ -0,0 +1,97 @@
|
||||
"""Region-of-interest helpers driven by an external JSON file.
|
||||
|
||||
Supports a single axis-aligned bounding box (AABB) in the CAD coordinate
|
||||
frame, units millimetres. The AABB is used in two places:
|
||||
|
||||
1. ``preprocess_point_cloud`` applies it as an early passthrough crop
|
||||
(performance + noise reduction).
|
||||
2. ``frame.build_bottom_roi_mask`` consumes the same AABB to build the
|
||||
canonical ROI mask used by the deflection stages.
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import json
|
||||
from pathlib import Path
|
||||
from typing import Any
|
||||
|
||||
import numpy as np
|
||||
|
||||
from .exceptions import InvalidPointCloudError
|
||||
|
||||
|
||||
_REQUIRED_KEYS = ("x_min", "x_max", "y_min", "y_max", "z_min", "z_max")
|
||||
|
||||
|
||||
def load_roi_aabb(path: str | Path) -> dict[str, float]:
|
||||
"""Load an AABB ROI specification from a JSON file.
|
||||
|
||||
Returns a normalised dict with the six float fields. Raises
|
||||
``InvalidPointCloudError`` for missing keys, non-numeric values, or
|
||||
inverted ranges (min >= max).
|
||||
"""
|
||||
path = Path(path)
|
||||
try:
|
||||
raw = json.loads(path.read_text(encoding="utf-8"))
|
||||
except (OSError, json.JSONDecodeError) as exc:
|
||||
raise InvalidPointCloudError(f"Failed to read ROI JSON {path}: {exc}") from exc
|
||||
if not isinstance(raw, dict):
|
||||
raise InvalidPointCloudError(
|
||||
f"ROI JSON {path} must be a JSON object with AABB fields."
|
||||
)
|
||||
aabb: dict[str, float] = {}
|
||||
for key in _REQUIRED_KEYS:
|
||||
if key not in raw:
|
||||
raise InvalidPointCloudError(
|
||||
f"ROI JSON {path} missing required key '{key}'."
|
||||
)
|
||||
try:
|
||||
aabb[key] = float(raw[key])
|
||||
except (TypeError, ValueError) as exc:
|
||||
raise InvalidPointCloudError(
|
||||
f"ROI JSON {path} key '{key}' is not numeric: {raw[key]!r}"
|
||||
) from exc
|
||||
_validate_aabb(aabb, source=str(path))
|
||||
return aabb
|
||||
|
||||
|
||||
def normalize_aabb(aabb: dict[str, Any]) -> dict[str, float]:
|
||||
"""Validate and coerce an in-memory AABB dict to ``{key: float}``."""
|
||||
coerced: dict[str, float] = {}
|
||||
for key in _REQUIRED_KEYS:
|
||||
if key not in aabb:
|
||||
raise InvalidPointCloudError(f"AABB dict missing required key '{key}'.")
|
||||
coerced[key] = float(aabb[key])
|
||||
_validate_aabb(coerced, source="<inline>")
|
||||
return coerced
|
||||
|
||||
|
||||
def _validate_aabb(aabb: dict[str, float], *, source: str) -> None:
|
||||
for axis in ("x", "y", "z"):
|
||||
lo = aabb[f"{axis}_min"]
|
||||
hi = aabb[f"{axis}_max"]
|
||||
if not (np.isfinite(lo) and np.isfinite(hi)):
|
||||
raise InvalidPointCloudError(
|
||||
f"AABB from {source} has non-finite bound on axis {axis}."
|
||||
)
|
||||
if lo >= hi:
|
||||
raise InvalidPointCloudError(
|
||||
f"AABB from {source} has inverted range on axis {axis}: "
|
||||
f"{axis}_min={lo} >= {axis}_max={hi}."
|
||||
)
|
||||
|
||||
|
||||
def apply_passthrough(points: np.ndarray, aabb: dict[str, float]) -> np.ndarray:
|
||||
"""Return a boolean mask of points inside the AABB (inclusive)."""
|
||||
aabb = normalize_aabb(aabb)
|
||||
return (
|
||||
(points[:, 0] >= aabb["x_min"])
|
||||
& (points[:, 0] <= aabb["x_max"])
|
||||
& (points[:, 1] >= aabb["y_min"])
|
||||
& (points[:, 1] <= aabb["y_max"])
|
||||
& (points[:, 2] >= aabb["z_min"])
|
||||
& (points[:, 2] <= aabb["z_max"])
|
||||
)
|
||||
|
||||
|
||||
__all__ = ["load_roi_aabb", "normalize_aabb", "apply_passthrough"]
|
||||
126
alg/delivery_v1.1/underframe_deflection/structures.py
Normal file
126
alg/delivery_v1.1/underframe_deflection/structures.py
Normal file
@@ -0,0 +1,126 @@
|
||||
from __future__ import annotations
|
||||
|
||||
from dataclasses import dataclass, field
|
||||
from typing import Any
|
||||
|
||||
import numpy as np
|
||||
|
||||
|
||||
@dataclass(slots=True)
|
||||
class FrameHypothesis:
|
||||
name: str
|
||||
axes: np.ndarray
|
||||
origin: np.ndarray
|
||||
score: float
|
||||
bottom_score: float
|
||||
symmetry_score: float
|
||||
flatness_score: float
|
||||
hole_score: float
|
||||
continuity_score: float
|
||||
|
||||
|
||||
@dataclass(slots=True)
|
||||
class CandidateBand:
|
||||
candidate_id: str
|
||||
slice_index: int
|
||||
x_start: float
|
||||
x_end: float
|
||||
x_center: float
|
||||
y_min: float
|
||||
y_max: float
|
||||
y_center: float
|
||||
z_low: float
|
||||
width: float
|
||||
point_count: int
|
||||
low_point_count: int
|
||||
flatness: float
|
||||
normal_alignment: float
|
||||
score: float
|
||||
edge_ratio: float
|
||||
side: str
|
||||
y_mid: float
|
||||
bin_start: int
|
||||
bin_end: int
|
||||
|
||||
|
||||
@dataclass(slots=True)
|
||||
class SliceResult:
|
||||
slice_index: int
|
||||
x_start: float
|
||||
x_end: float
|
||||
x_center: float
|
||||
y_mid: float
|
||||
candidate_bands: list[CandidateBand] = field(default_factory=list)
|
||||
left_candidates: list[CandidateBand] = field(default_factory=list)
|
||||
right_candidates: list[CandidateBand] = field(default_factory=list)
|
||||
point_count: int = 0
|
||||
|
||||
|
||||
@dataclass(slots=True)
|
||||
class TrackSolution:
|
||||
side: str
|
||||
rank: int
|
||||
score: float
|
||||
coverage_ratio: float
|
||||
coverage_count: int
|
||||
candidate_ids: list[str | None]
|
||||
candidates: list[CandidateBand]
|
||||
score_margin: float | None
|
||||
confidence: float
|
||||
|
||||
|
||||
@dataclass(slots=True)
|
||||
class RepresentativePoint:
|
||||
side: str
|
||||
slice_index: int
|
||||
x: float
|
||||
y: float
|
||||
z: float
|
||||
width: float
|
||||
point_count: int
|
||||
residual: float
|
||||
confidence: float
|
||||
x_start: float
|
||||
x_end: float
|
||||
|
||||
|
||||
@dataclass(slots=True)
|
||||
class FittedLine:
|
||||
side: str
|
||||
x_support: np.ndarray
|
||||
y_support: np.ndarray
|
||||
z_support: np.ndarray
|
||||
y_spline: Any
|
||||
z_spline: Any
|
||||
y_residual_rms: float
|
||||
z_residual_rms: float
|
||||
low_confidence_segments: list[tuple[float, float]]
|
||||
gaps: list[tuple[float, float]]
|
||||
|
||||
|
||||
@dataclass(slots=True)
|
||||
class HoleCandidate:
|
||||
side: str
|
||||
region: str
|
||||
axis: str
|
||||
center: np.ndarray
|
||||
radius: float
|
||||
score: float
|
||||
inlier_count: int
|
||||
coverage_ratio: float
|
||||
residual: float
|
||||
confidence: float
|
||||
|
||||
|
||||
@dataclass(slots=True)
|
||||
class ReferenceLine:
|
||||
side: str
|
||||
available: bool
|
||||
point_front: np.ndarray | None
|
||||
point_back: np.ndarray | None
|
||||
direction: np.ndarray | None
|
||||
confidence: float
|
||||
residual: float | None
|
||||
candidates_front: list[HoleCandidate] = field(default_factory=list)
|
||||
candidates_back: list[HoleCandidate] = field(default_factory=list)
|
||||
alternates: list[dict[str, Any]] = field(default_factory=list)
|
||||
@@ -0,0 +1,8 @@
|
||||
{
|
||||
"x_min": -12500.0,
|
||||
"x_max": 12500.0,
|
||||
"y_min": 150.0,
|
||||
"y_max": 4100.0,
|
||||
"z_min": -1680.0,
|
||||
"z_max": 200.0
|
||||
}
|
||||
180
alg/delivery_v1.1/underframe_deflection/visualization.py
Normal file
180
alg/delivery_v1.1/underframe_deflection/visualization.py
Normal file
@@ -0,0 +1,180 @@
|
||||
from __future__ import annotations
|
||||
|
||||
from pathlib import Path
|
||||
|
||||
import matplotlib
|
||||
|
||||
matplotlib.use("Agg")
|
||||
|
||||
import matplotlib.pyplot as plt
|
||||
import numpy as np
|
||||
|
||||
from .beams import candidate_rows
|
||||
from .fitting import evaluate_line
|
||||
from .reference import evaluate_reference_line
|
||||
from .structures import FittedLine, FrameHypothesis, RepresentativePoint, SliceResult, TrackSolution
|
||||
|
||||
|
||||
def _sample_points(points: np.ndarray, limit: int) -> np.ndarray:
|
||||
if len(points) <= limit:
|
||||
return points
|
||||
indices = np.linspace(0, len(points) - 1, limit, dtype=int)
|
||||
return points[indices]
|
||||
|
||||
|
||||
def plot_frame_hypotheses(
|
||||
hypotheses: list[FrameHypothesis],
|
||||
output_path: str | Path,
|
||||
dpi: int,
|
||||
) -> None:
|
||||
fig, ax = plt.subplots(figsize=(10, 4.5))
|
||||
names = [item.name for item in hypotheses]
|
||||
scores = [item.score for item in hypotheses]
|
||||
ax.bar(names, scores, color="#2f5c7e")
|
||||
ax.set_title("Frame Hypothesis Scores")
|
||||
ax.set_ylabel("Score")
|
||||
ax.tick_params(axis="x", rotation=30)
|
||||
fig.tight_layout()
|
||||
fig.savefig(output_path, dpi=dpi)
|
||||
plt.close(fig)
|
||||
|
||||
|
||||
def plot_roi_overview(
|
||||
work_points_vehicle: np.ndarray,
|
||||
roi_points_vehicle: np.ndarray,
|
||||
output_path: str | Path,
|
||||
dpi: int,
|
||||
limit: int,
|
||||
) -> None:
|
||||
full_sample = _sample_points(work_points_vehicle, limit)
|
||||
roi_sample = _sample_points(roi_points_vehicle, limit)
|
||||
fig, axes = plt.subplots(1, 2, figsize=(12, 5))
|
||||
axes[0].scatter(full_sample[:, 0], full_sample[:, 2], s=1, c="#d3d3d3", alpha=0.6)
|
||||
axes[0].scatter(roi_sample[:, 0], roi_sample[:, 2], s=1.5, c="#1f77b4", alpha=0.7)
|
||||
axes[0].set_title("ROI in X-Z")
|
||||
axes[0].set_xlabel("x (mm)")
|
||||
axes[0].set_ylabel("z (mm)")
|
||||
axes[1].scatter(full_sample[:, 1], full_sample[:, 2], s=1, c="#d3d3d3", alpha=0.6)
|
||||
axes[1].scatter(roi_sample[:, 1], roi_sample[:, 2], s=1.5, c="#1f77b4", alpha=0.7)
|
||||
axes[1].set_title("ROI in Y-Z")
|
||||
axes[1].set_xlabel("y (mm)")
|
||||
axes[1].set_ylabel("z (mm)")
|
||||
fig.tight_layout()
|
||||
fig.savefig(output_path, dpi=dpi)
|
||||
plt.close(fig)
|
||||
|
||||
|
||||
def plot_beam_candidates(
|
||||
slices: list[SliceResult],
|
||||
left_track: TrackSolution | None,
|
||||
right_track: TrackSolution | None,
|
||||
output_path: str | Path,
|
||||
dpi: int,
|
||||
) -> None:
|
||||
rows = candidate_rows(slices)
|
||||
fig, axes = plt.subplots(1, 2, figsize=(13, 5))
|
||||
if rows:
|
||||
x = np.asarray([row["x_center"] for row in rows])
|
||||
y = np.asarray([row["y_center"] for row in rows])
|
||||
z = np.asarray([row["z_low"] for row in rows])
|
||||
colors = ["#4a90e2" if row["side"] == "left" else "#ff7f50" for row in rows]
|
||||
axes[0].scatter(x, y, c=colors, s=10, alpha=0.45)
|
||||
axes[1].scatter(x, z, c=colors, s=10, alpha=0.45)
|
||||
|
||||
for track, color in ((left_track, "#0b5394"), (right_track, "#b45f06")):
|
||||
if not track:
|
||||
continue
|
||||
track_x = [candidate.x_center for candidate in track.candidates]
|
||||
track_y = [candidate.y_center for candidate in track.candidates]
|
||||
track_z = [candidate.z_low for candidate in track.candidates]
|
||||
axes[0].plot(track_x, track_y, color=color, linewidth=2.5)
|
||||
axes[1].plot(track_x, track_z, color=color, linewidth=2.5)
|
||||
|
||||
axes[0].set_title("Beam Candidate Bands and Tracks (X-Y)")
|
||||
axes[0].set_xlabel("x (mm)")
|
||||
axes[0].set_ylabel("y (mm)")
|
||||
axes[1].set_title("Beam Candidate Bands and Tracks (X-Z)")
|
||||
axes[1].set_xlabel("x (mm)")
|
||||
axes[1].set_ylabel("z (mm)")
|
||||
fig.tight_layout()
|
||||
fig.savefig(output_path, dpi=dpi)
|
||||
plt.close(fig)
|
||||
|
||||
|
||||
def plot_representatives_and_lines(
|
||||
left_points: list[RepresentativePoint],
|
||||
right_points: list[RepresentativePoint],
|
||||
left_line: FittedLine | None,
|
||||
right_line: FittedLine | None,
|
||||
left_reference,
|
||||
right_reference,
|
||||
output_path: str | Path,
|
||||
dpi: int,
|
||||
) -> None:
|
||||
fig, axes = plt.subplots(1, 2, figsize=(13, 5))
|
||||
for reps, color, label in (
|
||||
(left_points, "#0b5394", "Left beam"),
|
||||
(right_points, "#b45f06", "Right beam"),
|
||||
):
|
||||
if not reps:
|
||||
continue
|
||||
x = np.asarray([item.x for item in reps])
|
||||
y = np.asarray([item.y for item in reps])
|
||||
z = np.asarray([item.z for item in reps])
|
||||
axes[0].scatter(x, y, s=12, color=color, alpha=0.7, label=f"{label} reps")
|
||||
axes[1].scatter(x, z, s=12, color=color, alpha=0.7, label=f"{label} reps")
|
||||
|
||||
for line, reference, color, label in (
|
||||
(left_line, left_reference, "#0b5394", "Left beam"),
|
||||
(right_line, right_reference, "#b45f06", "Right beam"),
|
||||
):
|
||||
if line is None:
|
||||
continue
|
||||
x_grid = np.linspace(float(line.x_support.min()), float(line.x_support.max()), 300)
|
||||
y, z, _ = evaluate_line(line, x_grid)
|
||||
axes[0].plot(x_grid, y, color=color, linewidth=2.2, label=f"{label} fit")
|
||||
axes[1].plot(x_grid, z, color=color, linewidth=2.2, label=f"{label} fit")
|
||||
ref_eval, _ = evaluate_reference_line(reference, x_grid)
|
||||
if np.ndim(ref_eval) == 2 and not np.isnan(ref_eval[:, 0]).all():
|
||||
axes[0].plot(x_grid, ref_eval[:, 0], color=color, linestyle="--", linewidth=1.4)
|
||||
axes[1].plot(x_grid, ref_eval[:, 1], color=color, linestyle="--", linewidth=1.4)
|
||||
|
||||
axes[0].set_title("Representative Points and Lines (X-Y)")
|
||||
axes[0].set_xlabel("x (mm)")
|
||||
axes[0].set_ylabel("y (mm)")
|
||||
axes[1].set_title("Representative Points and Lines (X-Z)")
|
||||
axes[1].set_xlabel("x (mm)")
|
||||
axes[1].set_ylabel("z (mm)")
|
||||
handles, labels = axes[0].get_legend_handles_labels()
|
||||
if handles:
|
||||
fig.legend(handles, labels, loc="upper center", ncol=4)
|
||||
fig.tight_layout()
|
||||
fig.savefig(output_path, dpi=dpi)
|
||||
plt.close(fig)
|
||||
|
||||
|
||||
def plot_deflection_curves(
|
||||
deflection_rows: list[dict],
|
||||
output_path: str | Path,
|
||||
dpi: int,
|
||||
) -> None:
|
||||
fig, ax = plt.subplots(figsize=(11, 4.5))
|
||||
if deflection_rows:
|
||||
rows = deflection_rows
|
||||
x = np.asarray([row["x"] for row in rows], dtype=np.float64)
|
||||
left = np.asarray([row["left_deflection"] for row in rows], dtype=np.float64)
|
||||
right = np.asarray([row["right_deflection"] for row in rows], dtype=np.float64)
|
||||
if not np.isnan(left).all():
|
||||
ax.plot(x, left, color="#0b5394", linewidth=2.0, label="Left deflection")
|
||||
if not np.isnan(right).all():
|
||||
ax.plot(x, right, color="#b45f06", linewidth=2.0, label="Right deflection")
|
||||
ax.axhline(0.0, color="#666666", linewidth=1.0, linestyle=":")
|
||||
ax.set_title("Deflection Curves")
|
||||
ax.set_xlabel("x (mm)")
|
||||
ax.set_ylabel("Deflection Δz (mm)")
|
||||
handles, labels = ax.get_legend_handles_labels()
|
||||
if handles:
|
||||
ax.legend(loc="best")
|
||||
fig.tight_layout()
|
||||
fig.savefig(output_path, dpi=dpi)
|
||||
plt.close(fig)
|
||||
Reference in New Issue
Block a user