feat: 升级算法包 delivery_v1.1 → v1.2,新增任务级 CAD 与测距面板
- 算法包替换为 delivery_v1.2(coupler_panel 新增 autotune/layer_select,underframe_deflection 新增 single_beam 管线),删除 delivery_v1.1 - manifests/adapters 的 PYTHONPATH 与版本说明同步更新到 v1.2 - 新增任务级 CAD 文件支持(task_cad_files 表、上传/读取路由、TaskManager 全选与加载显示) - 新增 train-measurement 自定义前端面板(YAML 编辑器 + 可视化 ROI 配置 + 3D 取点) - PointCloudViewer 支持 PLY 叠加层渲染与 ROI 拾取 - Dockerfile 改用 Miniforge 安装 pythonocc-core,修复 libgomp/libstdc++ 依赖
This commit is contained in:
@@ -0,0 +1,18 @@
|
||||
"""Rigid registration via 4 hole-centre correspondence (Kabsch / Umeyama).
|
||||
|
||||
This package deliberately ships **only** the four-hole method. We do
|
||||
*not* expose a panel-plane ICP variant because that would absorb the
|
||||
deflection signal we want to measure.
|
||||
"""
|
||||
|
||||
from .kabsch import (
|
||||
KabschResult,
|
||||
kabsch_umeyama,
|
||||
register_four_holes,
|
||||
)
|
||||
|
||||
__all__ = [
|
||||
"KabschResult",
|
||||
"kabsch_umeyama",
|
||||
"register_four_holes",
|
||||
]
|
||||
@@ -0,0 +1,114 @@
|
||||
"""Rigid registration from a small number of 3D point correspondences.
|
||||
|
||||
We use the closed-form Umeyama solution (Kabsch with the optional scale
|
||||
turned off — we want a rigid transform). Given 4 source points P (in the
|
||||
scan/world frame) and 4 target points Q (in the CAD/GT frame), we solve
|
||||
for the rigid T such that ``T @ P ≈ Q``.
|
||||
|
||||
The four locating holes give us four exact correspondences; their
|
||||
indexing must match (we ensure this in :mod:`coupler_panel.detection`
|
||||
via the canonical quadrant ordering).
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
from dataclasses import dataclass
|
||||
|
||||
import numpy as np
|
||||
|
||||
from ..exceptions import RegistrationError
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class KabschResult:
|
||||
"""4×4 transform plus quality stats."""
|
||||
|
||||
matrix: np.ndarray # (4, 4) — applies to source to align with target
|
||||
rmse_mm: float
|
||||
max_residual_mm: float
|
||||
n_pairs: int
|
||||
|
||||
def apply(self, points: np.ndarray) -> np.ndarray:
|
||||
R = self.matrix[:3, :3]
|
||||
t = self.matrix[:3, 3]
|
||||
return np.asarray(points, dtype=np.float64) @ R.T + t
|
||||
|
||||
|
||||
def kabsch_umeyama(
|
||||
source: np.ndarray,
|
||||
target: np.ndarray,
|
||||
*,
|
||||
allow_reflection: bool = False,
|
||||
) -> KabschResult:
|
||||
"""Solve for rigid T such that T·source ≈ target."""
|
||||
P = np.asarray(source, dtype=np.float64)
|
||||
Q = np.asarray(target, dtype=np.float64)
|
||||
if P.shape != Q.shape or P.ndim != 2 or P.shape[1] != 3:
|
||||
raise RegistrationError(
|
||||
f"shape mismatch: source={P.shape}, target={Q.shape}; "
|
||||
"expected (N,3) with the same N"
|
||||
)
|
||||
n = P.shape[0]
|
||||
if n < 3:
|
||||
raise RegistrationError(f"need ≥ 3 correspondences, got {n}")
|
||||
|
||||
p_bar = P.mean(axis=0)
|
||||
q_bar = Q.mean(axis=0)
|
||||
Pc = P - p_bar
|
||||
Qc = Q - q_bar
|
||||
|
||||
H = Pc.T @ Qc # (3, 3)
|
||||
U, _S, Vt = np.linalg.svd(H)
|
||||
R = Vt.T @ U.T
|
||||
|
||||
if not allow_reflection and np.linalg.det(R) < 0:
|
||||
Vt[-1, :] *= -1.0
|
||||
R = Vt.T @ U.T
|
||||
|
||||
t = q_bar - R @ p_bar
|
||||
|
||||
M = np.eye(4)
|
||||
M[:3, :3] = R
|
||||
M[:3, 3] = t
|
||||
|
||||
P_aligned = P @ R.T + t
|
||||
res = np.linalg.norm(P_aligned - Q, axis=1)
|
||||
return KabschResult(
|
||||
matrix=M,
|
||||
rmse_mm=float(np.sqrt(np.mean(res ** 2))),
|
||||
max_residual_mm=float(res.max()),
|
||||
n_pairs=int(n),
|
||||
)
|
||||
|
||||
|
||||
def register_four_holes(
|
||||
source_centers: np.ndarray,
|
||||
target_centers: np.ndarray,
|
||||
*,
|
||||
refuse_above_mm: float | None = None,
|
||||
) -> KabschResult:
|
||||
"""Wrapper for the canonical 4-hole pairing case.
|
||||
|
||||
The two arrays **must** be index-aligned (same canonical quadrant
|
||||
order). The detection / GT modules in this project are responsible
|
||||
for producing them in the same order.
|
||||
"""
|
||||
P = np.asarray(source_centers, dtype=np.float64)
|
||||
Q = np.asarray(target_centers, dtype=np.float64)
|
||||
if P.shape != (4, 3) or Q.shape != (4, 3):
|
||||
raise RegistrationError(
|
||||
f"register_four_holes requires (4,3) inputs; got {P.shape} and {Q.shape}"
|
||||
)
|
||||
|
||||
result = kabsch_umeyama(P, Q)
|
||||
|
||||
if refuse_above_mm is not None and result.rmse_mm > float(refuse_above_mm):
|
||||
raise RegistrationError(
|
||||
f"4-hole Kabsch RMSE {result.rmse_mm:.3f} mm exceeds {refuse_above_mm} mm — "
|
||||
"wrong correspondences, missed hole, or detection error"
|
||||
)
|
||||
|
||||
return result
|
||||
|
||||
|
||||
__all__ = ["KabschResult", "kabsch_umeyama", "register_four_holes"]
|
||||
Reference in New Issue
Block a user