commit 16811100d16e3909f4affbd1e0b70ac6c2dbeb92 Author: lipan Date: Wed May 27 09:51:42 2026 +0000 feat: initial commit - massive-cloud point cloud processing system diff --git a/.dockerignore b/.dockerignore new file mode 100644 index 0000000..52a2ecf --- /dev/null +++ b/.dockerignore @@ -0,0 +1,61 @@ +# 宏量点云处理软件 - Docker构建忽略文件 + +# 依赖目录 +node_modules +.pnpm-store + +# 构建输出 +dist +build +.next +.nuxt + +# 开发文件 +.git +.gitignore +.github +.vscode +.idea + +# 环境文件 +.env +.env.* +!.env.example + +# 日志 +logs +*.log +npm-debug.log* +pnpm-debug.log* + +# 测试 +coverage +.nyc_output +*.test.ts +*.spec.ts +__tests__ + +# 文档 +*.md +!README.md +docs + +# Docker相关 +Dockerfile* +docker-compose* +.docker + +# 本地大数据目录（开发 compose 挂载到 /data，勿打入构建上下文） +data + +# 算法源码与样例数据（13.5GB 量级）——镜像里通过卷或下载脚本注入， +# 不打进构建上下文。在容器内通过 ALG_ROOT 环境变量指向挂载点。 +alg +src/alg + +# 其他 +.DS_Store +Thumbs.db +*.swp +*.swo +.manus-logs diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..ba15763 --- /dev/null +++ b/.gitignore @@ -0,0 +1,147 @@ +# Dependencies +**/node_modules +.pnpm-store/ + +# Build outputs +dist/ +build/ +*.dist + +# Environment variables +.env +.env.local +.env.development.local +.env.test.local +.env.production.local + +# IDE and editor files +.vscode/ +.idea/ +*.swp +*.swo +*~ + +# OS generated files +.DS_Store +.DS_Store? +._* +.Spotlight-V100 +.Trashes +ehthumbs.db +Thumbs.db + +# Logs +logs +*.log +npm-debug.log* +yarn-debug.log* +yarn-error.log* +pnpm-debug.log* +lerna-debug.log* + +# Runtime data +pids +*.pid +*.seed +*.pid.lock +*.bak + +# Coverage directory used by tools like istanbul +coverage/ +*.lcov + +# nyc test coverage +.nyc_output + +# Dependency directories +jspm_packages/ + +# TypeScript cache +*.tsbuildinfo + +# Optional npm cache directory +.npm + +# Optional eslint cache +.eslintcache + +# Microbundle cache +.rpt2_cache/ +.rts2_cache_cjs/ +.rts2_cache_es/ +.rts2_cache_umd/ + +# Optional REPL history +.node_repl_history + +# Output of 'npm pack' +*.tgz + +# Yarn Integrity file +.yarn-integrity + +# parcel-bundler cache (https://parceljs.org/) +.cache +.parcel-cache + +# Next.js build output +.next + +# Nuxt.js build / generate output +.nuxt + +# Gatsby files +.cache/ + +# Storybook build outputs +.out +.storybook-out + +# Temporary folders +tmp/ +temp/ + +# Database +*.db +*.sqlite +*.sqlite3 + +# ─── 算法原始压缩包（大文件，不入库）────────────────────────────────────────── +alg/*.zip +alg/*.rar +alg/*.tar.gz +alg/*.7z + +# ─── 算法测试数据（大文件，不入库）──────────────────────────────────────────── +alg/**/test_data/**/*.ply +alg/**/test_data/**/*.stp +alg/**/test_data/**/*.step +alg/**/test_data/runs/ + +# ─── 运行时数据（不入库）──────────────────────────────────────────────────────── +# 点云、分析结果、上传文件等运行时产生的数据 +data/ + +# ─── 算法示例数据 / 运行产物（不入库）────────────────────────────────────────── +# flatness-suite 示例数据集、可视化输出、运行结果 +alg/point_porj/datasets/ +alg/point_porj/vis/ +alg/point_porj/results/ +alg/point_porj/excel/ + +# 点云测距示例点云文件（大文件） +alg/点云测距v1.1/*.ply +alg/点云测距v1.1/train_measurement_system/visualizations/ +alg/点云测距v1.1/train_measurement_system/results/ + +# Python 构建产物 +**/__pycache__/ +**/*.pyc +**/*.pyo +**/*.egg-info/ +**/.eggs/ +**/dist-info/ + +# 算法运行中间产物 +alg/delivery_v1.1/coupler_panel_release/runs/ +alg/delivery_v1.1/underframe_deflection/runs/ diff --git a/.gitkeep b/.gitkeep new file mode 100644 index 0000000..e69de29 diff --git a/.prettierignore b/.prettierignore new file mode 100644 index 0000000..7284259 --- /dev/null +++ b/.prettierignore @@ -0,0 +1,35 @@ +# Dependencies +node_modules/ +.pnpm-store/ + +# Build outputs +dist/ +build/ +*.dist + +# Generated files +*.tsbuildinfo +coverage/ + +# Package files +package-lock.json +pnpm-lock.yaml + +# Database +*.db +*.sqlite +*.sqlite3 + +# Logs +*.log + +# Environment files +.env* + +# IDE files +.vscode/ +.idea/ + +# OS files +.DS_Store +Thumbs.db diff --git a/.prettierrc b/.prettierrc new file mode 100644 index 0000000..67c0bc8 --- /dev/null +++ b/.prettierrc @@ -0,0 +1,15 @@ +{ + "semi": true, + "trailingComma": "es5", + "singleQuote": false, + "printWidth": 80, + "tabWidth": 2, + "useTabs": false, + "bracketSpacing": true, + "bracketSameLine": false, + "arrowParens": "avoid", + "endOfLine": "lf", + "quoteProps": "as-needed", + "jsxSingleQuote": false, + "proseWrap": "preserve" +} diff --git a/=1.10 b/=1.10 new file mode 100644 index 0000000..e69de29 diff --git a/=3.1 b/=3.1 new file mode 100644 index 0000000..3bccad2 --- /dev/null +++ b/=3.1 @@ -0,0 +1,16 @@ +Looking in indexes: https://pypi.tuna.tsinghua.edu.cn/simple +Collecting scipy + Downloading https://pypi.tuna.tsinghua.edu.cn/packages/8e/6d/41991e503e51fc1134502694c5fa7a1671501a17ffa12716a4a9151af3df/scipy-1.15.3-cp310-cp310-manylinux_2_17_x86_64.manylinux2014_x86_64.whl (37.7 MB) + ━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━ 37.7/37.7 MB 3.8 MB/s 0:00:09 +Collecting openpyxl + Downloading https://pypi.tuna.tsinghua.edu.cn/packages/c0/da/977ded879c29cbd04de313843e76868e6e13408a94ed6b987245dc7c8506/openpyxl-3.1.5-py2.py3-none-any.whl (250 kB) +Requirement already satisfied: numpy<2.5,>=1.23.5 in /usr/local/lib/python3.10/dist-packages (from scipy) (2.2.6) +Collecting et-xmlfile (from openpyxl) + Downloading https://pypi.tuna.tsinghua.edu.cn/packages/c1/8b/5fe2cc11fee489817272089c4203e679c63b570a5aaeb18d852ae3cbba6a/et_xmlfile-2.0.0-py3-none-any.whl (18 kB) +Installing collected packages: scipy, et-xmlfile, openpyxl + +Successfully installed et-xmlfile-2.0.0 openpyxl-3.1.5 scipy-1.15.3 +WARNING: Running pip as the 'root' user can result in broken permissions and conflicting behaviour with the system package manager, possibly rendering your system unusable. It is recommended to use a virtual environment instead: https://pip.pypa.io/warnings/venv. Use the --root-user-action option if you know what you are doing and want to suppress this warning. + +[notice] A new release of pip is available: 26.0.1 -> 26.1.1 +[notice] To update, run: python3 -m pip install --upgrade pip diff --git a/DEPLOYMENT.md b/DEPLOYMENT.md new file mode 100644 index 0000000..84f75d6 --- /dev/null +++ b/DEPLOYMENT.md @@ -0,0 +1,264 @@ +# 宏量点云处理软件 - 内网Docker部署指南 + +本文档详细说明如何在内网服务器上使用Docker部署宏量点云处理软件。 + +## 系统要求 + +### 硬件要求 + +| 配置项 | 最低要求 | 推荐配置 | +|--------|----------|----------| +| CPU | 4核 | 8核+ | +| 内存 | 8GB | 16GB+ | +| 存储 | 100GB SSD | 500GB+ SSD | +| 网络 | 100Mbps | 1Gbps | + +### 软件要求 + +- 操作系统: Linux (Ubuntu 20.04+, CentOS 7+, Debian 10+) +- Docker: 20.10+ +- Docker Compose: 2.0+ + +## 快速开始 + +### 1. 安装Docker和Docker Compose + +```bash +# Ubuntu/Debian +curl -fsSL https://get.docker.com | sh +sudo usermod -aG docker $USER + +# 安装Docker Compose +sudo curl -L "https://github.com/docker/compose/releases/latest/download/docker-compose-$(uname -s)-$(uname -m)" -o /usr/local/bin/docker-compose +sudo chmod +x /usr/local/bin/docker-compose + +# 重新登录以应用用户组更改 +``` + +### 2. 获取项目代码 + +```bash +# 从GitHub克隆 (如果已导出到GitHub) +git clone https://github.com/your-org/massive-point-cloud.git +cd massive-point-cloud + +# 或者直接上传项目文件到服务器 +``` + +### 3. 配置环境变量 + +```bash +# 创建环境变量文件 +cat > .env << 'EOF' +# 安全配置 - 请修改为随机值 +JWT_SECRET=$(openssl rand -base64 32) +MYSQL_ROOT_PASSWORD=$(openssl rand -base64 16) + +# MinIO配置 +MINIO_ROOT_USER=minioadmin +MINIO_ROOT_PASSWORD=$(openssl rand -base64 16) + +# 应用配置 +VITE_APP_TITLE=宏量点云处理系统 +EOF + +# 或者手动编辑 +nano .env +``` + +### 4. 启动服务 + +```bash +# 基础部署 (应用 + MySQL + MinIO) +docker-compose up -d + +# 查看服务状态 +docker-compose ps + +# 查看日志 +docker-compose logs -f +``` + +### 5. 访问应用 + +- **Web应用**: http://your-server-ip:3000 +- **MinIO控制台**: http://your-server-ip:9001 + +## 部署架构 + +``` +┌─────────────────────────────────────────────────────────────┐ +│ 内网服务器 │ +│ ┌─────────────────────────────────────────────────────┐ │ +│ │ Docker Network │ │ +│ │ │ │ +│ │ ┌──────────┐ ┌──────────┐ ┌──────────────────┐ │ │ +│ │ │ App │ │ MySQL │ │ MinIO │ │ │ +│ │ │ :3000 │ │ :3306 │ │ :9000 / :9001 │ │ │ +│ │ └────┬─────┘ └────┬─────┘ └────────┬─────────┘ │ │ +│ │ │ │ │ │ │ +│ │ └─────────────┴──────────────────┘ │ │ +│ │ │ │ │ +│ └──────────────────────┼──────────────────────────────┘ │ +│ │ │ +│ ┌──────────────────────┴──────────────────────────────┐ │ +│ │ Nginx (可选, 生产环境) │ │ +│ │ :80 / :443 │ │ +│ └──────────────────────────────────────────────────────┘ │ +└─────────────────────────────────────────────────────────────┘ +``` + +## 服务说明 + +| 服务 | 端口 | 说明 | +|------|------|------| +| app | 3000 | 主应用服务 | +| mysql | 3306 | MySQL数据库 | +| minio | 9000 | MinIO API | +| minio | 9001 | MinIO控制台 | +| nginx | 80/443 | 反向代理 (可选) | +| redis | 6379 | 缓存服务 (可选) | + +## 高级配置 + +### 启用Nginx反向代理 + +```bash +# 使用production profile启动 +docker-compose --profile production up -d +``` + +### 启用Redis缓存 + +```bash +# 使用with-cache profile启动 +docker-compose --profile with-cache up -d +``` + +### 配置HTTPS + +1. 将SSL证书放置到 `docker/nginx/ssl/` 目录: + - `cert.pem` - 证书文件 + - `key.pem` - 私钥文件 + +2. 编辑 `docker/nginx/nginx.conf`，取消HTTPS服务器配置的注释 + +3. 重启Nginx: +```bash +docker-compose restart nginx +``` + +### 数据持久化 + +所有数据存储在Docker卷中: + +| 卷名 | 用途 | +|------|------| +| pointcloud-mysql-data | MySQL数据 | +| pointcloud-minio-data | 点云文件存储 | +| pointcloud-redis-data | Redis缓存 | +| pointcloud-app-logs | 应用日志 | + +备份数据: +```bash +# 备份MySQL +docker exec massive-point-cloud-mysql mysqldump -u root -p pointcloud_db > backup.sql + +# 备份MinIO数据 +docker run --rm -v pointcloud-minio-data:/data -v $(pwd):/backup alpine tar cvf /backup/minio-backup.tar /data +``` + +## 运维命令 + +```bash +# 查看所有服务状态 +docker-compose ps + +# 查看实时日志 +docker-compose logs -f app + +# 重启单个服务 +docker-compose restart app + +# 停止所有服务 +docker-compose down + +# 停止并删除数据卷 (危险!) +docker-compose down -v + +# 更新应用 +git pull +docker-compose build app +docker-compose up -d app + +# 进入容器调试 +docker exec -it massive-point-cloud-app sh +docker exec -it massive-point-cloud-mysql mysql -u root -p +``` + +## 故障排查 + +### 应用无法启动 + +```bash +# 检查日志 +docker-compose logs app + +# 检查数据库连接 +docker exec massive-point-cloud-app curl -f http://mysql:3306 || echo "MySQL not ready" +``` + +### 数据库连接失败 + +```bash +# 检查MySQL状态 +docker-compose logs mysql + +# 手动测试连接 +docker exec -it massive-point-cloud-mysql mysql -u pointcloud -p -e "SELECT 1" +``` + +### MinIO无法访问 + +```bash +# 检查MinIO状态 +docker-compose logs minio + +# 检查存储桶 +docker exec massive-point-cloud-minio-init mc ls myminio/ +``` + +## 性能优化 + +### MySQL优化 + +编辑 `docker/mysql/my.cnf`: +```ini +[mysqld] +innodb_buffer_pool_size = 2G +innodb_log_file_size = 512M +max_connections = 200 +``` + +### 增加文件上传限制 + +修改 `docker/nginx/nginx.conf`: +```nginx +client_max_body_size 5G; +``` + +## 安全建议 + +1. **修改默认密码**: 务必修改 `.env` 中的所有默认密码 +2. **限制端口暴露**: 生产环境只暴露80/443端口 +3. **启用防火墙**: 配置iptables或firewalld +4. **定期备份**: 设置自动备份任务 +5. **更新镜像**: 定期更新Docker镜像修复安全漏洞 + +## 联系支持 + +如遇到部署问题，请检查: +1. Docker和Docker Compose版本是否满足要求 +2. 服务器资源是否充足 +3. 端口是否被占用 +4. 防火墙规则是否正确 diff --git a/Dockerfile b/Dockerfile new file mode 100644 index 0000000..f5d3f85 --- /dev/null +++ b/Dockerfile @@ -0,0 +1,133 @@ +# 宏量点云处理软件 - 生产环境Dockerfile +# 多阶段构建，优化镜像大小 + +# ============================================ +# 阶段1: 依赖安装 +# ============================================ +FROM node:22-alpine AS deps + +WORKDIR /app + +# 安装pnpm +RUN corepack enable && corepack prepare pnpm@latest --activate + +# 复制依赖文件 +COPY package.json pnpm-lock.yaml* ./ +COPY patches ./patches + +# 安装依赖 +RUN pnpm install --frozen-lockfile --prod=false + +# ============================================ +# 阶段2: 构建应用 +# ============================================ +FROM node:22-alpine AS builder + +WORKDIR /app + +# 安装pnpm +RUN corepack enable && corepack prepare pnpm@latest --activate + +# 复制依赖 +COPY --from=deps /app/node_modules ./node_modules +COPY . . + +# 设置构建时环境变量 +ARG VITE_APP_TITLE="宏量点云处理系统" +ARG VITE_OAUTH_PORTAL_URL="disabled" +ARG VITE_APP_ID="" +ENV VITE_APP_TITLE=$VITE_APP_TITLE +ENV VITE_OAUTH_PORTAL_URL=$VITE_OAUTH_PORTAL_URL +ENV VITE_APP_ID=$VITE_APP_ID + +# 构建应用 +RUN pnpm build + +# ============================================ +# 阶段3: 生产运行环境（bookworm：与 dev 一致，便于 cadquery manylinux 轮子） +# ============================================ +FROM node:22-bookworm-slim AS runner + +WORKDIR /app + +RUN apt-get update && apt-get install -y --no-install-recommends \ + curl \ + dumb-init \ + python3 \ + python3-pip \ + ca-certificates \ + libgl1 \ + libglib2.0-0 \ + libxrender1 \ + libxext6 \ + libsm6 \ + libice6 \ + libx11-6 \ + libgomp1 \ + && rm -rf /var/lib/apt/lists/* + +# cadquery-ocp 轮子较大，须加大 pip 超时；覆盖：--build-arg PIP_DEFAULT_TIMEOUT=600 +ARG PIP_INDEX_URL=https://pypi.tuna.tsinghua.edu.cn/simple +ARG PIP_DEFAULT_TIMEOUT=300 +COPY requirements-geometry.txt /tmp/requirements-geometry.txt +RUN python3 -m pip install --no-cache-dir --break-system-packages \ + --default-timeout="${PIP_DEFAULT_TIMEOUT}" \ + --retries 10 \ + -i "${PIP_INDEX_URL}" \ + -r /tmp/requirements-geometry.txt \ + && rm /tmp/requirements-geometry.txt \ + && python3 -c "import cadquery; print('cadquery OK')" + +# 安装 Miniforge（conda-forge 原生，无 Anaconda ToS 限制）用于 pythonocc-core +ARG CONDA_INSTALLER_URL=https://mirrors.tuna.tsinghua.edu.cn/github-release/conda-forge/miniforge/LatestRelease/Miniforge3-Linux-x86_64.sh +RUN curl -fsSL "${CONDA_INSTALLER_URL}" -o /tmp/miniforge.sh \ + && bash /tmp/miniforge.sh -b -p /opt/conda \ + && rm /tmp/miniforge.sh \ + && PY_VER=$(python3 -c "import sys; print(f'{sys.version_info.major}.{sys.version_info.minor}')") \ + && /opt/conda/bin/conda config --set channel_priority strict \ + && /opt/conda/bin/conda create -y -n occ python="${PY_VER}" pythonocc-core=7.9.3 -c conda-forge \ + && /opt/conda/bin/conda clean -afy +RUN PY_VER=$(python3 -c "import sys; print(f'{sys.version_info.major}.{sys.version_info.minor}')") \ + && CONDA_SP="/opt/conda/envs/occ/lib/python${PY_VER}/site-packages" \ + && SYS_SP=$(python3 -c "import site; print(site.getsitepackages()[0])") \ + && ln -sf "${CONDA_SP}/OCC" "${SYS_SP}/OCC" + +ENV PYTHON3_PATH=/usr/bin/python3 + +# 创建非 root 用户（bookworm 用 groupadd/useradd） +RUN groupadd --gid 1001 nodejs && \ + useradd --uid 1001 --gid nodejs --system --shell /usr/sbin/nologin appuser + +# 安装pnpm (用于运行时) +RUN corepack enable && corepack prepare pnpm@latest --activate + +# 复制构建产物 +COPY --from=builder /app/dist ./dist +COPY --from=builder /app/node_modules ./node_modules +COPY --from=builder /app/package.json ./package.json +COPY --from=builder /app/drizzle ./drizzle +# STEP 装配检测：子进程脚本（runStepParseMvp 从 dist 相对定位 ../scripts） +COPY --from=builder /app/scripts ./scripts +# 通用算法插件（Route B）：manifests / Python 适配器 / mock fixtures。 +# registry.ts 通过 PLUGIN_DIR 环境变量定位；默认从 dist/plugins/ 加载。 +COPY --from=builder /app/server/plugins ./dist/plugins +ENV PLUGIN_DIR=/app/dist/plugins + +# 创建日志目录 +RUN mkdir -p /app/logs && chown -R appuser:nodejs /app + +# 切换到非root用户 +USER appuser + +# 暴露端口 +EXPOSE 3000 + +# 健康检查 (使用环境变量中的端口，默认3000) +HEALTHCHECK --interval=30s --timeout=10s --start-period=5s --retries=3 \ + CMD curl -f http://localhost:${PORT:-3000}/api/health || exit 1 + +# 使用dumb-init作为PID 1进程 +ENTRYPOINT ["dumb-init", "--"] + +# 启动应用 +CMD ["node", "dist/index.js"] diff --git a/alg/delivery_v1.1/coupler_panel_release/README.md b/alg/delivery_v1.1/coupler_panel_release/README.md new file mode 100644 index 0000000..45646ec --- /dev/null +++ b/alg/delivery_v1.1/coupler_panel_release/README.md @@ -0,0 +1,61 @@ +# coupler-panel-validation + +车钩面板 CAD-STP 到点云测量验证系统。 + +从 CAD STP 文件出发，构造 GT、合成模拟点云、跑完整测量流水线， +对比 GT，输出实验报告与可视化。 + +## 一句话用法 + +```python +from coupler_panel import compute_coupler_measurement, load_point_cloud + +points = load_point_cloud("scan.ply") # (N, 3) ndarray, 单位 mm +result = compute_coupler_measurement(points) +print(result.theta_right_deg, result.theta_ref_deg) +print(result.dx_y_max_mm, result.dx_z_max_mm) +``` + +完整文档见 [docs/USAGE.md](docs/USAGE.md)。 + +## 安装 + +```bash +# 仅核心计算（numpy/scipy） +pip install -e . + +# 加上点云 IO + 去噪 + 配准 +pip install -e ".[cloud]" + +# 加上 STP 解析 +pip install -e ".[cad]" + +# 加上可视化 +pip install -e ".[viz]" + +# 全装 +pip install -e ".[all]" +``` + +## 命令行 + +```bash +# 1) 已有点云，直接测量 +coupler-measure --input-cloud scan.ply --out-dir runs/demo + +# 2) 从 CAD STP 生成模拟点云并测量 +coupler-measure --stp panel.stp --out-dir runs/from_cad + +# 3) 跑参数 sweep（采样×密度×噪声×变换矩阵） +coupler-measure --sweep configs/experiments/full_sweep.yaml --out-dir runs/sweep +``` + +## 坐标系约定 + +- x = 车体纵向 +- y = 车体宽度 +- z = 车体高度 +- 单位 mm +- 拟合面板法向 `n = (n_x, n_y, n_z)` 强制 `n_x > 0` + +详见 USAGE.md 第 5 节。 diff --git a/alg/delivery_v1.1/coupler_panel_release/USAGE.md b/alg/delivery_v1.1/coupler_panel_release/USAGE.md new file mode 100644 index 0000000..16c0327 --- /dev/null +++ b/alg/delivery_v1.1/coupler_panel_release/USAGE.md @@ -0,0 +1,376 @@ +# coupler_panel — 集成开发者使用指南 + +> 高铁车钩面板偏转角与最大偏差测量算法 +> +> 主输入：**CAD STP + 点云 PLY + ROI JSON** +> +> 主输出：两个方向偏转角 `θ_right` / `θ_ref`、最大 x 向偏差 `dx_y_max` / `dx_z_max`，以及一份完整可序列化的 `report.json`。 + +--- + +## 1. 一句话集成 + +```python +from coupler_panel.pipeline import run_coupler_measurement_pipeline + +result = run_coupler_measurement_pipeline( + input_cloud = "scan.ply", # 你的测量点云（已在 CAD 帧） + cad_stp_path = "panel.stp", # 同款面板的 CAD STP + roi_json = "roi.json", # ROI 描述（不裁剪就传 mode=all） + out_dir = "runs/today", # 中间产物 + report.json 落盘目录 +) + +print(result.theta_right_deg, result.theta_ref_deg) # 两个偏转角 +print(result.dx_y_max_mm, result.dx_z_max_mm) # 最大 x 偏差 +``` + +返回 `CouplerMeasurement`，全字段语义见 §4。`out_dir` 下额外得到 8 个文件（含 `report.json`，便于排查 / 后处理）。 + +--- + +## 2. 安装 + +```bash +# 全量安装（推荐）— core + 点云 IO + STP 解析 + 可视化 + Excel +python -m pip install -r requirements.txt +pip install -e ".[all]" + +# 也可以按需取子集（见 pyproject.toml 的 extras）： +pip install -e ".[cloud]" # 只要点云 + 去噪 +pip install -e ".[cad]" # 需要解析 STP +pip install -e ".[report]" # 需要 Excel 报表 +``` + +`requirements.txt` 锁定了端到端跑通过的版本（Python 3.10 验证）。 + +--- + +## 3. 三件输入的准备 + +### 3.1 CAD STP + +- 提供面板的 STEP 文件路径。 +- 算法用它做两件事： + 1. 从拓扑直接提 4 个定位孔 + 中心大孔 + 面板法向（写入 `01_gt.json`，作审计用） + 2. 把 CAD 法向作为 `panel_normal_seed` 喂给 hole-detection 的投影几何，加速并稳定 2D 网格化 +- **可以不传**（`cad_stp_path=None`）—— 此时 hole detection 走 PCA 自行估法向；适用于你已经预先把法向算好/扫描坐标系本来就规范的场景。建议生产里**始终传**，便于审计。 + +### 3.2 点云 PLY + +- 支持 `.ply` / `.pcd` / `.xyz` / `.txt` / `.csv`（PCD 与二进制 PLY 需要 open3d）。 +- 数组形状 `(N, 3)`，单位 **mm**。 +- **必须在 CAD 坐标系**： + - x = 车体纵向，y = 车体宽度，z = 车体高度 + - 算法默认面板法向 `n_x > 0` +- **不再做配准**（参见 §6 "为什么不配准"）。这意味着上游扫描端外参必须已经标定 —— 把点云对齐到 CAD 帧是扫描软件的职责，不是本算法的职责。 +- 可以带颜色 / 法向等额外列；算法只读前 3 列。 + +### 3.3 ROI JSON + +定义"PLY 里哪些点要纳入后续算法"，**轴对齐 bounding box（AABB）**格式： + +```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 +} +``` + +六个 key 全部必填，单位 mm，坐标系 CAD 帧。点 `(x, y, z)` 被保留的条件是： + +``` +x_min <= x <= x_max ∧ y_min <= y <= y_max ∧ z_min <= z <= z_max +``` + +- **想保留全部点云**：把 6 个边界写得比点云自身 bbox 还宽即可（不需要任何 mode 切换）。 +- **想不写文件**：传 `roi_json=None`，算法走"±∞" 默认（pass-through）。 +- **想程序内构造**：传 `roi_spec=ROISpec(x_min=..., ...)`。 + +--- + +## 4. 公共 API 详解 + +### 4.1 主入口 `run_coupler_measurement_pipeline` + +```python +def run_coupler_measurement_pipeline( + input_cloud : str | Path, # 必填：点云路径 + out_dir : str | Path, # 必填：输出目录（自动创建） + *, + config_path : str | Path | None = None, # 可选：自定义 YAML + profile : str = "auto", # 可选：preset profile + cad_stp_path: str | Path | None = None, # 推荐：CAD STP 路径 + roi_spec : ROISpec | None = None, # 二选一：内存 ROI 对象 + roi_json : str | Path | None = None, # 二选一：ROI JSON 路径 + progress : Callable[[str, float], None] | None = None, +) -> CouplerMeasurement +``` + +**算法链路（固定不可绕过）**： +``` +load_point_cloud → apply_roi → [optional CAD parse → panel_normal_seed] + → denoise_cloud → compute_coupler_measurement + ├─ validate + ├─ detect 4 holes + ├─ build reference frame + ├─ extract panel ROI + ├─ fit panel plane (RANSAC + SVD) + └─ compute metrics +``` + +**Profile**：传 `"auto"` 即可；若你的面板型号对应到一个特定 profile，可指定 `profile="lu1_af4"` 等（详见 `configs/profiles/`）。 + +**配置覆盖**： +```python +result = run_coupler_measurement_pipeline( + input_cloud="scan.ply", + out_dir="runs/x", + config_path={ + # 任何在 configs/default.yaml 里的字段都可以这样覆盖 + "denoise": {"steps": [{"type": "voxel", "voxel_mm": 0.3}]}, + "plane_fit": {"ransac_inlier_thresh_mm": 1.0}, + "metrics": {"right_axis_sign": -1}, + }, +) +``` + +### 4.2 返回值 `CouplerMeasurement` + +```python +@dataclass +class CouplerMeasurement: + # 版本号 + schema_version: str # 字段语义稳定性 → SCHEMA_VERSION + algorithm_version: str # 数值结果稳定性 → ALGORITHM_VERSION + + # === 6 个主测量量 === + theta_right_deg: float # 右偏转角 atan2(n_y, n_x)，applied right_axis_sign + theta_ref_deg: float # 上偏转角 atan2(n_z, n_x) + D_y_max_mm: float # max |y_i - C_y| + D_z_max_mm: float # max |z_i - C_z| + dx_y_max_mm: float # |A| · D_y_max, A = -n_y/n_x + dx_z_max_mm: float # |B| · D_z_max, B = -n_z/n_x + + # === 几何 === + center_C: tuple[float, float, float] # 面板中心 (4 孔均值) + normal: tuple[float, float, float] # 拟合面板法向，n_x > 0 + + # === 详细子结构 === + holes: HoleSet # 检测到的 4 个定位孔 + 可选中心孔 + plane: PanelPlane # normal + offset + inlier_ratio + RMS + frame: ReferenceFrame # C + 参考面 z=C_z + 纵向中面 y=C_y + + # === 诊断 === + quality_flags: list[str] # 例如 'plane_inlier_ratio_low' + timings: list[StageTiming] # 每阶段秒数 + 峰值内存 + report: dict # 全量可序列化，与 report.json 一致 +``` + +```python +result.to_dict() # 完全 JSON 化的 dict（落 DB / 网络传输用） +``` + +### 4.3 异常 + +``` +CouplerError # 所有可预见异常的基类 +├── InvalidPointCloudError # 输入形状/dtype/NaN 非法 +│ └── UnsupportedFormatError # 文件后缀不支持 +├── InsufficientPointsError # 点数 < 200 或 ROI 后 < 100 +├── CADParseError # STP 解析失败 / 找不到 4 孔 +├── HoleDetectionError # 点云上 4 孔检测失败 +└── PlaneFitError # RANSAC 找不到平面 +``` + +非致命问题通过 `result.quality_flags` 暴露： + +| Flag | 含义 | +|---|---| +| `plane_inlier_ratio_low` | 平面拟合内点率低于 `plane_fit.min_inlier_ratio` | + +### 4.4 进度回调 + +```python +def on_progress(stage: str, fraction: float) -> None: + print(f"[{fraction:5.1%}] {stage}") + +result = run_coupler_measurement_pipeline(..., progress=on_progress) +``` + +阶段顺序：`validate → detect_holes → build_frame → extract_roi → fit_plane → metrics`。 + +--- + +## 5. ROI JSON schema 详细 + +**完整字段**（6 个 key，全部必填，单位 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 +} +``` + +**规则**： + +- 必须满足 `x_min < x_max` 且 `y_min < y_max` 且 `z_min < z_max`，否则 `load_roi_json` 抛 `ValueError` +- 不在 [min, max] 闭区间内的点被丢弃（用 `>=` 与 `<=`，包含边界） +- 多余的 key（例如 `comment`、`source`）会被忽略，方便挂元数据 + +**手动构造 ROI（不读文件）**： + +```python +from coupler_panel.roi_select import ROISpec, default_roi_spec + +# 全保留默认（六个轴都是 ±∞） +spec = default_roi_spec() + +# 自定义 bbox +spec = ROISpec( + x_min=-12000, x_max= 100, + y_min= -300, y_max= 1200, + z_min= -1000, z_max= 800, +) + +result = run_coupler_measurement_pipeline(..., roi_spec=spec) +``` + +**诊断输出**： + +每次运行后 `out_dir/roi.json` 写出实际生效的 spec + 应用诊断： + +```jsonc +{ + "spec": { "x_min": -12000.0, ... }, + "diagnostics": { + "kind": "aabb", + "n_in": 96662, + "n_out": 96662, + "kept_ratio": 1.0, + "bounds": { "x_min": -12000.0, ... } + } +} +``` + +--- + +## 6. 为什么主路径**不做配准** + +车钩面板上的 4 个定位孔与面板一起刚体旋转。如果用"扫到的 4 孔 → CAD 原始 4 孔"做 Kabsch 配准，数学上会**精确解出**"扫描器位姿误差 · 面板偏转"的复合刚体变换并反向作用 —— 把面板偏转随同位姿误差**一起消掉**，下游平面拟合再读不到偏转信号。 + +这是几何性质，不是实现 bug。 + +工程上，主算法假设扫描端外参标定让点云已经在 CAD 帧。如果你的设备做不到位姿标定，要么： +1. 在面板**外**装基准（不动）做 Kabsch +2. 用 ICP 或其他不依赖 4 孔的方法 + +研究模式（`coupler_panel.pipeline.run_from_cad_pipeline`）保留了 4 孔 Kabsch + 注入偏转 oracle 的能力，用于上限对照实验；生产集成请用 `run_coupler_measurement_pipeline`。 + +--- + +## 7. 落盘产物（`out_dir/`） + +| 文件 | 内容 | +|---|---| +| `config.resolved.yaml` | 解析合并后实际生效的完整配置 | +| `roi.json` | ROI spec + 应用诊断（n_in / n_out / kept_ratio） | +| `01_gt.json` | （传了 cad_stp_path 时）从 STP 提的审计用 GT | +| `02_sampled.ply` | ROI 裁剪后的点云 | +| `05_denoised.ply` | 去噪后点云（喂给测量算法的最终点云） | +| `10_metrics.json` | 等价于 `report.json` 的内部 report 部分 | +| `report.json` | **完整结果**，等于 `result.to_dict()`；schema 见 §4.2 | +| `images/` | 占位空目录（仅 `viz` 模块写图时会用） | + +--- + +## 8. 集成模板 + +```python +import logging +from pathlib import Path + +from coupler_panel.pipeline import run_coupler_measurement_pipeline +from coupler_panel.exceptions import CouplerError + +log = logging.getLogger(__name__) + + +def measure_coupler( + cloud_path: Path, + cad_stp: Path, + roi_json: Path, + out_dir: Path, +) -> dict | None: + """主集成入口。失败时返回 None；成功时返回 6 个测量量 + flags。""" + try: + result = run_coupler_measurement_pipeline( + input_cloud = cloud_path, + cad_stp_path = cad_stp, + roi_json = roi_json, + out_dir = out_dir, + progress = lambda s, f: log.debug("%s %.0f%%", s, f * 100), + ) + except CouplerError as exc: + log.error("coupler measurement failed: %s", exc) + return None + + if result.quality_flags: + log.warning("quality flags: %s", result.quality_flags) + + return { + "schema_version": result.schema_version, + "algorithm_version": result.algorithm_version, + "theta_right_deg": result.theta_right_deg, + "theta_ref_deg": result.theta_ref_deg, + "dx_y_max_mm": result.dx_y_max_mm, + "dx_z_max_mm": result.dx_z_max_mm, + "center_C_mm": list(result.center_C), + "normal": list(result.normal), + "flags": result.quality_flags, + } +``` + +--- + +## 9. 命令行（备用，非主集成路径） + +```bash +# 等价于 run_coupler_measurement_pipeline(input_cloud=..., out_dir=...) +coupler-measure --input-cloud scan.ply --out-dir runs/today +``` + +> 当前 CLI 暂未把 `--cad` 与 `--roi` 暴露出来（设计上 CLI 主要给研究流程用）；集成方请直接用 Python API。 + +--- + +## 10. 版本兼容性 + +| 标识 | 在哪 | 含义 | +|---|---|---| +| `coupler_panel.__version__` | `coupler_panel._version` | 包版本，遵循语义化版本 | +| `coupler_panel.ALGORITHM_VERSION` | 同上 | 算法版本；升级算法时递增，**数值结果可能变化** | +| `coupler_panel.SCHEMA_VERSION` | 同上 | `CouplerMeasurement` / `report.json` 字段结构；只在破坏性变更时递增；**新增可选字段不递增** | + +集成建议： +1. 落库时把 `algorithm_version` 写进每条记录，便于历史复现 +2. 反序列化 `report.json` 时校验 `schema_version` 主版本号 + +--- + +## 11. 坐标 & 单位速查 + +| 量 | 单位 | 约定 | +|---|---|---| +| 点云坐标 | mm | x=纵向、y=宽度、z=高度 | +| 角度 | 度 (deg) | `θ_right` = atan2(n_y, n_x)、`θ_ref` = atan2(n_z, n_x) | +| 法向 | 单位向量 | 强制 `n_x > 0` | +| `right_axis_sign` | ±1 | 如果车体"右"是 +y 留 1；如果是 -y 则配置 -1 | diff --git a/alg/delivery_v1.1/coupler_panel_release/configs/default.yaml b/alg/delivery_v1.1/coupler_panel_release/configs/default.yaml new file mode 100644 index 0000000..414a185 --- /dev/null +++ b/alg/delivery_v1.1/coupler_panel_release/configs/default.yaml @@ -0,0 +1,147 @@ +# Default configuration for coupler panel measurement pipeline. +# All numeric units in mm / degree unless otherwise stated. +# Override fields by passing a dict or a YAML path with the same nested shape. + +schema_version: "1.0" + +# --- CAD parsing & GT extraction -------------------------------------------- +cad: + # OpenCASCADE tessellation quality. + linear_deflection_mm: 0.5 + angular_deflection_rad: 0.1 + # Hole topology classification. + # The four locating holes are expected to be small and equal-radius; + # the central pass-through hole is expected to be much larger. + locating_hole_radius_mm: [4.0, 12.0] # [min, max] of expected locating hole radius + central_hole_radius_min_mm: 20.0 # any circle larger than this is treated as the centre hole + rectangle_aspect_tol: 0.15 # tolerance for "the 4 hole centres form a rectangle" + +# --- Sampling --------------------------------------------------------------- +sampling: + method: poisson_disk # uniform_area | poisson_disk | adaptive | grid_spacing + # Use either spacing_mm OR num_points; if both set, spacing_mm wins. + spacing_mm: 1.0 + num_points: 100000 + seed: 42 + adaptive: + curvature_weight: 1.0 + min_spacing_factor: 0.25 # smallest spacing = spacing_mm * factor + max_spacing_factor: 4.0 + +# --- Simulated scan (optional) ---------------------------------------------- +scan_simulator: + enabled: false + view_origin: [10000.0, 0.0, 0.0] # camera position in CAD frame, mm + hpr_radius_mm: 50000.0 # hidden-point-removal radius + +# --- Panel deflection injection (the SIGNAL the algorithm should measure) -- +# This rotates the panel (and its 4 locating holes) by a known small angle. +# After running the pipeline, theta_right_deg / theta_ref_deg in the result +# should match these values within the algorithm's recovery accuracy. +panel_deflection: + enabled: false + theta_right_deg: 0.0 # atan2(n_y, n_x) target (rotation around z) + theta_ref_deg: 0.0 # atan2(n_z, n_x) target (rotation around -y) + +# --- Synthetic noise -------------------------------------------------------- +# Realistic LIDAR / structured-light noise on a ~2 m × 2 m panel is ≈ 50-80 +# µm = 0.05-0.08 mm. Larger sigma is mostly to test robustness, not to +# match a real sensor. +noise: + enabled: true + sigma_mm: 0.05 + mode: isotropic # isotropic | along_normal + seed: 43 + +# --- Synthetic scan-pose perturbation --------------------------------------- +# Mounting / scanner-stage jitter. 4-hole Kabsch should cancel this; if it +# leaks into the final angles that's a registration error, not a measurement +# of panel deflection. Set rotation_deg=0, translation_mm=0 for "ideal pose" +# experiments that isolate the deflection-recovery accuracy. +transform: + enabled: true + rotation_deg: 0.5 # magnitude; axis is randomised + rotation_axis: random # random | x | y | z | [ax, ay, az] + translation_mm: 2.0 # magnitude; direction is randomised + translation_dir: random + seed: 44 + +# --- Denoise pipeline ------------------------------------------------------- +# Steps are executed in the listed order. Set steps to [] to disable. +denoise: + steps: + - type: passthrough + bbox_padding_mm: 50.0 + - type: voxel + voxel_mm: 0.5 + - type: statistical + nb_neighbors: 30 + std_ratio: 2.0 + - type: radius + nb_points: 8 + radius_mm: 2.0 + # Optional MLS smoothing (requires open3d ≥ 0.18 with ml extras). + smoothing: + enabled: false + search_radius_mm: 2.0 + polynomial_order: 2 + +# --- Registration ----------------------------------------------------------- +registration: + method: kabsch_four_holes # only this method is implemented; documented & enforced + # The four GT hole centres come from CAD; the four scan hole centres come + # from the detection module. Pairing is by greedy nearest-neighbour after + # a coarse PCA-based pre-alignment of the centroid. + pre_align: pca_centroid + refuse_if_residual_above_mm: 5.0 # quality flag if RMS pairing residual exceeds this + +# --- 4-hole detection in point cloud ---------------------------------------- +detection: + # Local panel-plane projection. + panel_normal_seed: from_cad # from_cad | pca | given + panel_normal_pca_kth: 1 # use k-th smallest PCA eigenvector as panel normal + # 2D occupancy grid. + grid_pixel_mm: 0.3 + panel_min_area_ratio: 0.4 # largest connected component must be ≥ this fraction of bbox + hole_min_area_mm2: 10.0 + hole_max_area_mm2: 1500.0 + closing_iters: 2 # 原来没有，加形态学闭运算填补边界断裂 + fill_bg_below_mm2: 5.0 # 原来没有，填充小碎片 + # Circle fitting on hole boundary. + circle_fit: + method: algebraic_then_lsq # algebraic_then_lsq | hyper | taubin + inlier_residual_mm: 0.5 + # Selection. + select_top_k_by_radius_match: 4 + rectangle_aspect_tol: 0.20 + +# --- Panel ROI extraction --------------------------------------------------- +roi: + hole_dilate_mm: 3.0 # exclude points within (hole_radius + dilate) + central_hole_dilate_mm: 5.0 + edge_buffer_mm: 5.0 # shrink ROI rectangle inward by this + chamfer_normal_dot_min: 0.85 # exclude points whose normal deviates more than acos(0.85) from panel normal + outlier_z_score: 3.0 + +# --- Plane fit -------------------------------------------------------------- +plane_fit: + method: ransac_svd + ransac_iterations: 200 + ransac_inlier_thresh_mm: auto # auto = 3*sigma_mm + min_inlier_ratio: 0.6 + enforce_nx_positive: true + +# --- Metrics ---------------------------------------------------------------- +metrics: + # If your right axis is +y, leave this 1. If right is -y, set to -1. + right_axis_sign: 1 + # Angles smaller than this are clamped to 0 (purely cosmetic; raw value is + # always in the report). + angle_clamp_deg: 0.0 + +# --- Output / artifacts ----------------------------------------------------- +artifacts: + save_intermediate: true + ply_format: binary # binary | ascii + image_dpi: 150 + heatmap_colormap: viridis diff --git a/alg/delivery_v1.1/coupler_panel_release/configs/experiments/density_sweep.yaml b/alg/delivery_v1.1/coupler_panel_release/configs/experiments/density_sweep.yaml new file mode 100644 index 0000000..a502e97 --- /dev/null +++ b/alg/delivery_v1.1/coupler_panel_release/configs/experiments/density_sweep.yaml @@ -0,0 +1,13 @@ +# Parameter sweep over point cloud density. +matrix: + sampling.spacing_mm: + - 0.25 + - 0.5 + - 1.0 + - 2.0 + - 5.0 +fixed: + sampling.method: poisson_disk + noise.sigma_mm: 0.5 + transform.rotation_deg: 3.0 + transform.translation_mm: 5.0 diff --git a/alg/delivery_v1.1/coupler_panel_release/configs/experiments/full_sweep.yaml b/alg/delivery_v1.1/coupler_panel_release/configs/experiments/full_sweep.yaml new file mode 100644 index 0000000..aac5ddf --- /dev/null +++ b/alg/delivery_v1.1/coupler_panel_release/configs/experiments/full_sweep.yaml @@ -0,0 +1,34 @@ +# Full cartesian product. WARNING: 4 * 5 * 8 * 4 * 4 = 2560 runs. +# Use only when you have time and disk. +matrix: + sampling.method: + - uniform_area + - poisson_disk + - adaptive + - grid_spacing + sampling.spacing_mm: + - 0.25 + - 0.5 + - 1.0 + - 2.0 + - 5.0 + noise.sigma_mm: + - 0.0 + - 0.25 + - 0.5 + - 1.0 + - 2.0 + - 4.0 + - 10.0 + - 20.0 + transform.rotation_deg: + - 1.0 + - 3.0 + - 5.0 + - 10.0 + transform.translation_mm: + - 0.5 + - 1.0 + - 5.0 + - 10.0 +fixed: {} diff --git a/alg/delivery_v1.1/coupler_panel_release/configs/experiments/lu1_af4_sweep.yaml b/alg/delivery_v1.1/coupler_panel_release/configs/experiments/lu1_af4_sweep.yaml new file mode 100644 index 0000000..63ccbda --- /dev/null +++ b/alg/delivery_v1.1/coupler_panel_release/configs/experiments/lu1_af4_sweep.yaml @@ -0,0 +1,47 @@ +# Sweep matrix for the lu1-AF4 coupler panel STP. Total 9 runs. +stp_path: D:/myDocuments/PKU/high_speed_train_proj/lu1-M000004835653_AF4_ASMP072708187_N_AF4.stp + +matrix: + noise.sigma_mm: [0.0, 0.5, 2.0] + transform.rotation_deg: [1.0, 3.0, 10.0] + +fixed: + sampling.method: poisson_disk + sampling.spacing_mm: null + sampling.num_points: 500000 + sampling.seed: 42 + scan_simulator.enabled: true + scan_simulator.view_origin: [10000.0, 0.0, 0.0] + scan_simulator.hpr_radius_mm: 200000.0 + noise.seed: 43 + transform.translation_mm: 5.0 + transform.seed: 44 + cad.locating_hole_radius_mm: [14.0, 18.0] + cad.central_hole_radius_min_mm: 80.0 + detection.grid_pixel_mm: 2.0 + detection.fill_bg_below_mm2: 100.0 + detection.hole_min_area_mm2: 600.0 + detection.hole_max_area_mm2: 5000.0 + detection.locating_hole_radius_mm: [14.0, 22.0] + detection.panel_min_area_ratio: 0.15 + detection.rectangle_aspect_tol: 0.40 + detection.circle_fit.method: algebraic_then_lsq + detection.circle_fit.inlier_residual_mm: 1.5 + roi.hole_dilate_mm: 5.0 + roi.central_hole_dilate_mm: 10.0 + roi.edge_buffer_mm: 8.0 + roi.chamfer_normal_dot_min: 0.80 + roi.outlier_z_score: 3.0 + plane_fit.ransac_iterations: 300 + plane_fit.ransac_inlier_thresh_mm: auto + plane_fit.min_inlier_ratio: 0.5 + plane_fit.enforce_nx_positive: true + metrics.right_axis_sign: 1 + denoise.steps: + - type: passthrough + bbox_padding_mm: 50.0 + - type: voxel + voxel_mm: 1.0 + - type: statistical + nb_neighbors: 20 + std_ratio: 2.5 diff --git a/alg/delivery_v1.1/coupler_panel_release/configs/experiments/lu1_deflection_sweep.yaml b/alg/delivery_v1.1/coupler_panel_release/configs/experiments/lu1_deflection_sweep.yaml new file mode 100644 index 0000000..d034b5a --- /dev/null +++ b/alg/delivery_v1.1/coupler_panel_release/configs/experiments/lu1_deflection_sweep.yaml @@ -0,0 +1,74 @@ +# Deflection-recovery sweep: vary the INJECTED panel-deflection angle and +# verify the algorithm reads it back. Single-axis at a time so the +# response of theta_right and theta_ref can be analysed independently. +stp_path: D:/myDocuments/PKU/high_speed_train_proj/lu1-M000004835653_AF4_ASMP072708187_N_AF4.stp + +matrix: + panel_deflection.theta_right_deg: [0.0, 0.1, 0.3, 0.5, 1.0, 2.0] + +fixed: + panel_deflection.enabled: true + panel_deflection.theta_ref_deg: 0.0 + + sampling.method: poisson_disk + sampling.spacing_mm: null + sampling.num_points: 500000 + sampling.seed: 42 + + scan_simulator.enabled: true + scan_simulator.view_origin: [10000.0, 0.0, 0.0] + scan_simulator.hpr_radius_mm: 200000.0 + + noise.enabled: true + noise.sigma_mm: 0.05 + noise.mode: isotropic + noise.seed: 43 + + transform.enabled: true + transform.rotation_deg: 0.5 + transform.translation_mm: 2.0 + transform.rotation_axis: random + transform.translation_dir: random + transform.seed: 44 + + cad.locating_hole_radius_mm: [14.0, 18.0] + cad.central_hole_radius_min_mm: 80.0 + cad.linear_deflection_mm: 0.5 + cad.angular_deflection_rad: 0.1 + + detection.panel_normal_seed: pca + detection.grid_pixel_mm: 2.0 + detection.fill_bg_below_mm2: 100.0 + detection.hole_min_area_mm2: 600.0 + detection.hole_max_area_mm2: 5000.0 + detection.locating_hole_radius_mm: [14.0, 22.0] + detection.panel_min_area_ratio: 0.15 + detection.rectangle_aspect_tol: 0.40 + detection.circle_fit.method: algebraic_then_lsq + detection.circle_fit.inlier_residual_mm: 1.5 + + roi.hole_dilate_mm: 5.0 + roi.central_hole_dilate_mm: 10.0 + roi.edge_buffer_mm: 8.0 + roi.chamfer_normal_dot_min: 0.80 + roi.outlier_z_score: 3.0 + + plane_fit.ransac_iterations: 300 + plane_fit.ransac_inlier_thresh_mm: auto + plane_fit.min_inlier_ratio: 0.5 + plane_fit.enforce_nx_positive: true + + metrics.right_axis_sign: 1 + metrics.angle_clamp_deg: 0.0 + + registration.method: kabsch_four_holes + registration.refuse_if_residual_above_mm: 5.0 + + denoise.steps: + - type: passthrough + bbox_padding_mm: 50.0 + - type: voxel + voxel_mm: 0.5 + - type: statistical + nb_neighbors: 20 + std_ratio: 2.5 diff --git a/alg/delivery_v1.1/coupler_panel_release/configs/experiments/lu1_density_sweep.yaml b/alg/delivery_v1.1/coupler_panel_release/configs/experiments/lu1_density_sweep.yaml new file mode 100644 index 0000000..87e6f02 --- /dev/null +++ b/alg/delivery_v1.1/coupler_panel_release/configs/experiments/lu1_density_sweep.yaml @@ -0,0 +1,73 @@ +# Point-cloud density sweep: fix deflection + noise, vary num_points to +# show how many points the algorithm needs. +stp_path: D:/myDocuments/PKU/high_speed_train_proj/lu1-M000004835653_AF4_ASMP072708187_N_AF4.stp + +matrix: + sampling.num_points: [200000, 350000, 500000, 750000, 1000000] + +fixed: + panel_deflection.enabled: true + panel_deflection.theta_right_deg: 0.5 + panel_deflection.theta_ref_deg: 0.3 + + sampling.method: poisson_disk + sampling.spacing_mm: null + sampling.seed: 42 + + scan_simulator.enabled: true + scan_simulator.view_origin: [10000.0, 0.0, 0.0] + scan_simulator.hpr_radius_mm: 200000.0 + + noise.enabled: true + noise.sigma_mm: 0.05 + noise.mode: isotropic + noise.seed: 43 + + transform.enabled: true + transform.rotation_deg: 0.5 + transform.translation_mm: 2.0 + transform.rotation_axis: random + transform.translation_dir: random + transform.seed: 44 + + cad.locating_hole_radius_mm: [14.0, 18.0] + cad.central_hole_radius_min_mm: 80.0 + cad.linear_deflection_mm: 0.5 + cad.angular_deflection_rad: 0.1 + + detection.panel_normal_seed: pca + detection.grid_pixel_mm: 2.0 + detection.fill_bg_below_mm2: 100.0 + detection.hole_min_area_mm2: 600.0 + detection.hole_max_area_mm2: 5000.0 + detection.locating_hole_radius_mm: [14.0, 28.0] # wider window for low-density tests + detection.panel_min_area_ratio: 0.15 + detection.rectangle_aspect_tol: 0.40 + detection.circle_fit.method: algebraic_then_lsq + detection.circle_fit.inlier_residual_mm: 1.5 + + roi.hole_dilate_mm: 5.0 + roi.central_hole_dilate_mm: 10.0 + roi.edge_buffer_mm: 8.0 + roi.chamfer_normal_dot_min: 0.80 + roi.outlier_z_score: 3.0 + + plane_fit.ransac_iterations: 300 + plane_fit.ransac_inlier_thresh_mm: auto + plane_fit.min_inlier_ratio: 0.5 + plane_fit.enforce_nx_positive: true + + metrics.right_axis_sign: 1 + metrics.angle_clamp_deg: 0.0 + + registration.method: kabsch_four_holes + registration.refuse_if_residual_above_mm: 5.0 + + denoise.steps: + - type: passthrough + bbox_padding_mm: 50.0 + - type: voxel + voxel_mm: 0.5 + - type: statistical + nb_neighbors: 20 + std_ratio: 2.5 diff --git a/alg/delivery_v1.1/coupler_panel_release/configs/experiments/lu1_noise_sweep.yaml b/alg/delivery_v1.1/coupler_panel_release/configs/experiments/lu1_noise_sweep.yaml new file mode 100644 index 0000000..0a97453 --- /dev/null +++ b/alg/delivery_v1.1/coupler_panel_release/configs/experiments/lu1_noise_sweep.yaml @@ -0,0 +1,73 @@ +# Noise sweep: fix the panel deflection at (theta_right=0.5°, theta_ref=0.3°) +# and vary Gaussian noise sigma to characterise robustness. +stp_path: D:/myDocuments/PKU/high_speed_train_proj/lu1-M000004835653_AF4_ASMP072708187_N_AF4.stp + +matrix: + noise.sigma_mm: [0.0, 0.025, 0.05, 0.1, 0.2, 0.5] + +fixed: + panel_deflection.enabled: true + panel_deflection.theta_right_deg: 0.5 + panel_deflection.theta_ref_deg: 0.3 + + sampling.method: poisson_disk + sampling.spacing_mm: null + sampling.num_points: 500000 + sampling.seed: 42 + + scan_simulator.enabled: true + scan_simulator.view_origin: [10000.0, 0.0, 0.0] + scan_simulator.hpr_radius_mm: 200000.0 + + noise.enabled: true + noise.mode: isotropic + noise.seed: 43 + + transform.enabled: true + transform.rotation_deg: 0.5 + transform.translation_mm: 2.0 + transform.rotation_axis: random + transform.translation_dir: random + transform.seed: 44 + + cad.locating_hole_radius_mm: [14.0, 18.0] + cad.central_hole_radius_min_mm: 80.0 + cad.linear_deflection_mm: 0.5 + cad.angular_deflection_rad: 0.1 + + detection.panel_normal_seed: pca + detection.grid_pixel_mm: 2.0 + detection.fill_bg_below_mm2: 100.0 + detection.hole_min_area_mm2: 600.0 + detection.hole_max_area_mm2: 5000.0 + detection.locating_hole_radius_mm: [14.0, 22.0] + detection.panel_min_area_ratio: 0.15 + detection.rectangle_aspect_tol: 0.40 + detection.circle_fit.method: algebraic_then_lsq + detection.circle_fit.inlier_residual_mm: 1.5 + + roi.hole_dilate_mm: 5.0 + roi.central_hole_dilate_mm: 10.0 + roi.edge_buffer_mm: 8.0 + roi.chamfer_normal_dot_min: 0.80 + roi.outlier_z_score: 3.0 + + plane_fit.ransac_iterations: 300 + plane_fit.ransac_inlier_thresh_mm: auto + plane_fit.min_inlier_ratio: 0.5 + plane_fit.enforce_nx_positive: true + + metrics.right_axis_sign: 1 + metrics.angle_clamp_deg: 0.0 + + registration.method: kabsch_four_holes + registration.refuse_if_residual_above_mm: 5.0 + + denoise.steps: + - type: passthrough + bbox_padding_mm: 50.0 + - type: voxel + voxel_mm: 0.5 + - type: statistical + nb_neighbors: 20 + std_ratio: 2.5 diff --git a/alg/delivery_v1.1/coupler_panel_release/configs/experiments/lu1_sampling_sweep.yaml b/alg/delivery_v1.1/coupler_panel_release/configs/experiments/lu1_sampling_sweep.yaml new file mode 100644 index 0000000..e28a5de --- /dev/null +++ b/alg/delivery_v1.1/coupler_panel_release/configs/experiments/lu1_sampling_sweep.yaml @@ -0,0 +1,78 @@ +# Sampling-strategy sweep: 4 methods × same other settings, so the saved +# intermediates per run//02_sampled.ply, 03_noisy.ply, ... can be +# compared apples-to-apples. +stp_path: D:/myDocuments/PKU/high_speed_train_proj/lu1-M000004835653_AF4_ASMP072708187_N_AF4.stp + +matrix: + sampling.method: + - uniform_area + - poisson_disk + - adaptive + - grid_spacing + +fixed: + panel_deflection.enabled: true + panel_deflection.theta_right_deg: 0.5 + panel_deflection.theta_ref_deg: 0.3 + + sampling.spacing_mm: null + sampling.num_points: 500000 + sampling.seed: 42 + + scan_simulator.enabled: true + scan_simulator.view_origin: [10000.0, 0.0, 0.0] + scan_simulator.hpr_radius_mm: 200000.0 + + noise.enabled: true + noise.sigma_mm: 0.05 + noise.mode: isotropic + noise.seed: 43 + + transform.enabled: true + transform.rotation_deg: 0.5 + transform.translation_mm: 2.0 + transform.rotation_axis: random + transform.translation_dir: random + transform.seed: 44 + + cad.locating_hole_radius_mm: [14.0, 18.0] + cad.central_hole_radius_min_mm: 80.0 + cad.linear_deflection_mm: 0.5 + cad.angular_deflection_rad: 0.1 + + detection.panel_normal_seed: pca + detection.grid_pixel_mm: 2.0 + detection.fill_bg_below_mm2: 100.0 + detection.hole_min_area_mm2: 600.0 + detection.hole_max_area_mm2: 5000.0 + detection.locating_hole_radius_mm: [14.0, 22.0] + detection.panel_min_area_ratio: 0.15 + detection.rectangle_aspect_tol: 0.40 + detection.circle_fit.method: algebraic_then_lsq + detection.circle_fit.inlier_residual_mm: 1.5 + + roi.hole_dilate_mm: 5.0 + roi.central_hole_dilate_mm: 10.0 + roi.edge_buffer_mm: 8.0 + roi.chamfer_normal_dot_min: 0.80 + roi.outlier_z_score: 3.0 + + plane_fit.ransac_iterations: 300 + plane_fit.ransac_inlier_thresh_mm: auto + plane_fit.min_inlier_ratio: 0.5 + plane_fit.enforce_nx_positive: true + + metrics.right_axis_sign: 1 + metrics.angle_clamp_deg: 0.0 + + registration.method: kabsch_four_holes + registration.refuse_if_residual_above_mm: 5.0 + + denoise.steps: + - type: passthrough + bbox_padding_mm: 50.0 + - type: voxel + voxel_mm: 0.5 + - type: statistical + nb_neighbors: 20 + std_ratio: 2.5 diff --git a/alg/delivery_v1.1/coupler_panel_release/configs/experiments/noise_sweep.yaml b/alg/delivery_v1.1/coupler_panel_release/configs/experiments/noise_sweep.yaml new file mode 100644 index 0000000..0eed81c --- /dev/null +++ b/alg/delivery_v1.1/coupler_panel_release/configs/experiments/noise_sweep.yaml @@ -0,0 +1,16 @@ +# Parameter sweep over Gaussian noise sigma. +matrix: + noise.sigma_mm: + - 0.0 + - 0.25 + - 0.5 + - 1.0 + - 2.0 + - 4.0 + - 10.0 + - 20.0 +fixed: + sampling.method: poisson_disk + sampling.spacing_mm: 1.0 + transform.rotation_deg: 3.0 + transform.translation_mm: 5.0 diff --git a/alg/delivery_v1.1/coupler_panel_release/configs/experiments/sampling_sweep.yaml b/alg/delivery_v1.1/coupler_panel_release/configs/experiments/sampling_sweep.yaml new file mode 100644 index 0000000..e1b7f40 --- /dev/null +++ b/alg/delivery_v1.1/coupler_panel_release/configs/experiments/sampling_sweep.yaml @@ -0,0 +1,12 @@ +# Parameter sweep over sampling methods (other knobs fixed at default). +matrix: + sampling.method: + - uniform_area + - poisson_disk + - adaptive + - grid_spacing +fixed: + sampling.spacing_mm: 1.0 + noise.sigma_mm: 0.5 + transform.rotation_deg: 3.0 + transform.translation_mm: 5.0 diff --git a/alg/delivery_v1.1/coupler_panel_release/configs/experiments/transform_sweep.yaml b/alg/delivery_v1.1/coupler_panel_release/configs/experiments/transform_sweep.yaml new file mode 100644 index 0000000..eecdce1 --- /dev/null +++ b/alg/delivery_v1.1/coupler_panel_release/configs/experiments/transform_sweep.yaml @@ -0,0 +1,16 @@ +# Parameter sweep over GT rigid transform magnitude. +matrix: + transform.rotation_deg: + - 1.0 + - 3.0 + - 5.0 + - 10.0 + transform.translation_mm: + - 0.5 + - 1.0 + - 5.0 + - 10.0 +fixed: + sampling.method: poisson_disk + sampling.spacing_mm: 1.0 + noise.sigma_mm: 0.5 diff --git a/alg/delivery_v1.1/coupler_panel_release/configs/profiles/high_density.yaml b/alg/delivery_v1.1/coupler_panel_release/configs/profiles/high_density.yaml new file mode 100644 index 0000000..395392f --- /dev/null +++ b/alg/delivery_v1.1/coupler_panel_release/configs/profiles/high_density.yaml @@ -0,0 +1,17 @@ +# Profile: high-density scan (≈100w points, sub-mm spacing). +sampling: + spacing_mm: 0.25 + num_points: 1000000 +denoise: + steps: + - type: passthrough + bbox_padding_mm: 50.0 + - type: voxel + voxel_mm: 0.25 + - type: statistical + nb_neighbors: 50 + std_ratio: 2.0 +detection: + grid_pixel_mm: 0.25 +plane_fit: + ransac_iterations: 500 diff --git a/alg/delivery_v1.1/coupler_panel_release/configs/profiles/low_density.yaml b/alg/delivery_v1.1/coupler_panel_release/configs/profiles/low_density.yaml new file mode 100644 index 0000000..e1734f2 --- /dev/null +++ b/alg/delivery_v1.1/coupler_panel_release/configs/profiles/low_density.yaml @@ -0,0 +1,18 @@ +# Profile: low-density scan (≈1w points, ≈2 mm spacing). +sampling: + spacing_mm: 2.0 + num_points: 10000 +denoise: + steps: + - type: passthrough + bbox_padding_mm: 50.0 + - type: voxel + voxel_mm: 1.0 + - type: statistical + nb_neighbors: 15 + std_ratio: 2.5 +detection: + grid_pixel_mm: 1.0 + hole_min_area_mm2: 20.0 +plane_fit: + ransac_iterations: 100 diff --git a/alg/delivery_v1.1/coupler_panel_release/configs/profiles/lu1_af4.yaml b/alg/delivery_v1.1/coupler_panel_release/configs/profiles/lu1_af4.yaml new file mode 100644 index 0000000..d6dce44 --- /dev/null +++ b/alg/delivery_v1.1/coupler_panel_release/configs/profiles/lu1_af4.yaml @@ -0,0 +1,106 @@ +# Profile tuned for the lu1-M000004835653_AF4_ASMP072708187_N_AF4.stp panel. +# +# Topology summary (from cad/iter_circular_edges): +# - locating holes: r = 16 mm × 4 corners at (y=±100, z=±180) +# - central hole: r = 100 mm at (y≈1, z=0) +# - panel size: ~200 × 360 mm in y-z, ~76 mm thick along x +# +# Use: +# coupler-measure --stp --out-dir --profile lu1_af4 + +cad: + linear_deflection_mm: 0.5 + angular_deflection_rad: 0.1 + # CAD-side: strict band that selects only the r=16 mm corner holes. + locating_hole_radius_mm: [14.0, 18.0] + central_hole_radius_min_mm: 80.0 + rectangle_aspect_tol: 0.30 + +sampling: + method: poisson_disk + spacing_mm: null + num_points: 500000 + seed: 42 + +# Critical: simulate a single-view "+x looking at -x" scan. Without this, +# back-face points project into the hole regions and detection fails. +scan_simulator: + enabled: true + view_origin: [10000.0, 0.0, 0.0] + hpr_radius_mm: 200000.0 + +# --- Panel deflection injection ------------------------------------------- +# This is the *signal* we want the algorithm to recover. By default it is +# off — sweeps override these. +panel_deflection: + enabled: true + theta_right_deg: 0.5 # rotation around z so n_y > 0 + theta_ref_deg: 0.3 # rotation around -y so n_z > 0 + +# --- Realistic LiDAR / structured-light noise ----------------------------- +noise: + enabled: true + sigma_mm: 0.05 # ≈ 50 µm; matches typical industrial scanners + mode: isotropic + seed: 43 + +# --- Scanner-pose jitter -------------------------------------------------- +# Modest values: 4-hole Kabsch should remove this completely. +transform: + enabled: true + rotation_deg: 0.5 + translation_mm: 2.0 + rotation_axis: random + translation_dir: random + seed: 44 + +denoise: + steps: + - type: passthrough + bbox_padding_mm: 50.0 + - type: voxel + voxel_mm: 0.5 # finer voxel for low-noise data + - type: statistical + nb_neighbors: 20 + std_ratio: 2.5 + +detection: + panel_normal_seed: pca # critical: use cloud's own PCA, not CAD seed + grid_pixel_mm: 2.0 + closing_iters: 0 + fill_bg_below_mm2: 100.0 + panel_min_area_ratio: 0.15 + hole_min_area_mm2: 600.0 + hole_max_area_mm2: 5000.0 + locating_hole_radius_mm: [14.0, 22.0] + circle_fit: + method: algebraic_then_lsq + inlier_residual_mm: 1.5 + rectangle_aspect_tol: 0.40 + +roi: + hole_dilate_mm: 5.0 + central_hole_dilate_mm: 10.0 + edge_buffer_mm: 8.0 + chamfer_normal_dot_min: 0.80 + outlier_z_score: 3.0 + +plane_fit: + ransac_iterations: 300 + ransac_inlier_thresh_mm: auto + min_inlier_ratio: 0.5 + enforce_nx_positive: true + +metrics: + right_axis_sign: 1 + angle_clamp_deg: 0.0 + +artifacts: + save_intermediate: true + ply_format: binary + image_dpi: 150 + heatmap_colormap: viridis + +registration: + method: kabsch_four_holes + refuse_if_residual_above_mm: 5.0 diff --git a/alg/delivery_v1.1/coupler_panel_release/coupler_panel/__init__.py b/alg/delivery_v1.1/coupler_panel_release/coupler_panel/__init__.py new file mode 100644 index 0000000..bb7eb0d --- /dev/null +++ b/alg/delivery_v1.1/coupler_panel_release/coupler_panel/__init__.py @@ -0,0 +1,116 @@ +"""Coupler-panel CAD-STP to point-cloud measurement validation system. + +The package is split into two distinct layers: + +**1. Public measurement algorithm (integration API).** + The single function software developers should call from their + product code. Takes a clean point cloud, returns a + :class:`CouplerMeasurement`. Stable schema across patch releases — + see ``docs/USAGE.md``. + + Entry points: + + - :func:`compute_coupler_measurement` — pure, in-memory. + - :class:`CouplerMeasurement`, :class:`HoleSet`, :class:`PanelPlane`, + :class:`ReferenceFrame`, :class:`Hole`. + - :mod:`coupler_panel.exceptions`. + - :func:`load_point_cloud`, :func:`write_point_cloud` for IO. + +**2. Validation / data-preparation harness (research only).** + Used to generate synthetic point clouds from CAD, denoise them, and + register them into the CAD frame. Not part of the integration + surface; layout and signatures may change. + + Entry points: + + - :mod:`coupler_panel.data_prep` — STP → mesh → sample → noise → + transform → denoise → register. + - :mod:`coupler_panel.pipeline` — convenience wrappers that save + every artifact to disk. +""" + +from __future__ import annotations + +# --- Stable, integration-safe public surface -------------------------------- +from ._version import ALGORITHM_VERSION, SCHEMA_VERSION, __version__ +from .exceptions import ( + CADParseError, + CouplerError, + HoleDetectionError, + InsufficientPointsError, + InvalidPointCloudError, + PlaneFitError, + RegistrationError, + UnsupportedFormatError, +) +from .io_utils import load_point_cloud, write_point_cloud +from .structures import ( + CouplerMeasurement, + Hole, + HoleSet, + PanelPlane, + ReferenceFrame, + StageTiming, +) + + +def __getattr__(name: str): + """Lazy export of heavy / optional symbols. + + Avoids importing matplotlib / open3d / OCP at ``import coupler_panel`` + time so a thin integration deployment keeps a fast, dependency-light + startup. + """ + if name in ("compute_coupler_measurement", "ProgressCallback"): + from .api import compute_coupler_measurement, ProgressCallback + + return {"compute_coupler_measurement": compute_coupler_measurement, "ProgressCallback": ProgressCallback}[name] + if name == "run_coupler_measurement_pipeline": + from .pipeline import run_coupler_measurement_pipeline + + return run_coupler_measurement_pipeline + if name == "run_from_cad_pipeline": + from .pipeline import run_from_cad_pipeline + + return run_from_cad_pipeline + if name == "prepare_cloud_from_cad": + from .data_prep import prepare_cloud_from_cad + + return prepare_cloud_from_cad + if name == "prepare_cloud_from_scan": + from .data_prep import prepare_cloud_from_scan + + return prepare_cloud_from_scan + raise AttributeError(name) + + +__all__ = [ + "__version__", + "ALGORITHM_VERSION", + "SCHEMA_VERSION", + # Stable measurement API + "compute_coupler_measurement", + "ProgressCallback", + "CouplerMeasurement", + "Hole", + "HoleSet", + "PanelPlane", + "ReferenceFrame", + "StageTiming", + "load_point_cloud", + "write_point_cloud", + # Exceptions + "CouplerError", + "InvalidPointCloudError", + "UnsupportedFormatError", + "InsufficientPointsError", + "CADParseError", + "HoleDetectionError", + "RegistrationError", + "PlaneFitError", + # Research-only convenience (data prep + persisting orchestration) + "run_coupler_measurement_pipeline", + "run_from_cad_pipeline", + "prepare_cloud_from_cad", + "prepare_cloud_from_scan", +] diff --git a/alg/delivery_v1.1/coupler_panel_release/coupler_panel/_version.py b/alg/delivery_v1.1/coupler_panel_release/coupler_panel/_version.py new file mode 100644 index 0000000..08287e5 --- /dev/null +++ b/alg/delivery_v1.1/coupler_panel_release/coupler_panel/_version.py @@ -0,0 +1,19 @@ +"""Version markers for coupler_panel. + +- ``__version__`` — package version (semver). +- ``ALGORITHM_VERSION`` — algorithm/numerics version. Numerical results may + change when this increments. Increment whenever any computation in + ``compute_coupler_measurement`` changes. +- ``SCHEMA_VERSION`` — result-schema version. Only increments on a + *breaking* change to the ``CouplerMeasurement`` dataclass or to + ``report.json`` field semantics. New optional fields do **not** bump + this. +""" + +from __future__ import annotations + +__version__ = "0.1.0" +ALGORITHM_VERSION = "0.1.0" +SCHEMA_VERSION = "1.0" + +__all__ = ["__version__", "ALGORITHM_VERSION", "SCHEMA_VERSION"] diff --git a/alg/delivery_v1.1/coupler_panel_release/coupler_panel/api.py b/alg/delivery_v1.1/coupler_panel_release/coupler_panel/api.py new file mode 100644 index 0000000..e728877 --- /dev/null +++ b/alg/delivery_v1.1/coupler_panel_release/coupler_panel/api.py @@ -0,0 +1,251 @@ +"""Public Python API — **measurement algorithm only**. + +This module is what software developers integrate against. It takes a +*clean, already-registered* point cloud (in the CAD / world frame) of +the coupler panel and returns a :class:`CouplerMeasurement`. + +Responsibilities of this layer: + + validate → detect 4 holes → build reference frame + → extract panel ROI → fit panel plane → compute metrics + +Things that are **not** in this layer (they live in +:mod:`coupler_panel.data_prep`): + +- Reading STEP files / generating synthetic point clouds +- Adding noise / random rigid transforms +- Denoising (passthrough / SOR / ROR / voxel / smoothing) +- 4-hole Kabsch registration into the CAD frame + +The reason: those steps are *engineering* steps that vary per sensor +setup; the production caller usually has their own scan + cleanup +pipeline and just wants the deflection numbers out the other side. +""" + +from __future__ import annotations + +import time +import tracemalloc +from typing import Any, Callable + +import numpy as np + +from ._version import ALGORITHM_VERSION, SCHEMA_VERSION +from .config import load_config, resolve_auto +from .detection import detect_four_holes +from .exceptions import ( + InsufficientPointsError, + InvalidPointCloudError, + PlaneFitError, +) +from .geometry import build_reference_frame, extract_panel_roi, fit_panel_plane +from .metrics import compute_deflection_metrics +from .structures import ( + CouplerMeasurement, + HoleSet, + StageTiming, +) + + +ProgressCallback = Callable[[str, float], None] + + +_STAGES = ( + "validate", + "detect_holes", + "build_frame", + "extract_roi", + "fit_plane", + "metrics", +) + + +def _ping(progress: ProgressCallback | None, stage: str, frac: float) -> None: + if progress is not None: + try: + progress(stage, float(frac)) + except Exception: + pass + + +def _validate_points(points: Any) -> np.ndarray: + try: + pts = np.asarray(points, dtype=np.float64) + except Exception as exc: + raise InvalidPointCloudError(f"could not coerce points to ndarray: {exc}") from exc + if pts.ndim != 2 or pts.shape[1] != 3: + raise InvalidPointCloudError(f"expected (N, 3) points; got shape {pts.shape}") + if pts.shape[0] < 200: + raise InsufficientPointsError(f"too few points: {pts.shape[0]} (need ≥ 200)") + if not np.isfinite(pts).all(): + raise InvalidPointCloudError("input contains NaN/Inf") + return pts + + +def compute_coupler_measurement( + points: Any, + config: str | dict[str, Any] | None = None, + profile: str = "auto", + *, + panel_normal_seed: np.ndarray | None = None, + progress: ProgressCallback | None = None, +) -> CouplerMeasurement: + """Run the **measurement algorithm** on an already-prepared point cloud. + + Parameters + ---------- + points : array-like + ``(N, 3)`` cloud in the CAD / world frame, units mm. + The cloud is expected to be denoised and rigidly aligned to the + CAD frame before this call. See :mod:`coupler_panel.data_prep` + for a reference implementation of that preparation step. + config : path / dict / None + Optional override of the default YAML config. + profile : str + ``"auto"`` / ``"default"`` skip profile loading. Other names are + resolved against ``configs/profiles/.yaml``. + panel_normal_seed : (3,) or None + Optional prior on the panel normal (e.g. the GT normal). Speeds + up and stabilises the in-plane projection used by hole detection. + ``None`` falls back to PCA on the cloud. + progress : callable or None + ``progress(stage_name, fraction)`` called once per stage. + + Returns + ------- + :class:`CouplerMeasurement` + + Raises + ------ + InvalidPointCloudError, InsufficientPointsError, HoleDetectionError, + PlaneFitError, CouplerError + """ + cfg = load_config(config, profile=profile) + cfg = resolve_auto(cfg) + + quality_flags: list[str] = [] + timings: list[StageTiming] = [] + + # ---- validate ---------------------------------------------------------- + pts = _validate_points(points) + _ping(progress, "validate", 1.0 / len(_STAGES)) + + # ---- detect 4 holes ---------------------------------------------------- + t0 = time.perf_counter() + tracemalloc.start() + detection = detect_four_holes( + pts, + detection_cfg=cfg["detection"], + cad_cfg=cfg["cad"], + panel_normal_seed=panel_normal_seed, + ) + _, peak = tracemalloc.get_traced_memory() + tracemalloc.stop() + timings.append(StageTiming("detect_holes", time.perf_counter() - t0, peak / 1024)) + _ping(progress, "detect_holes", 2.0 / len(_STAGES)) + + detected_holes = detection.holes_3d + + # ---- reference frame --------------------------------------------------- + t0 = time.perf_counter() + frame = build_reference_frame(detected_holes) + timings.append(StageTiming("build_frame", time.perf_counter() - t0, 0.0)) + _ping(progress, "build_frame", 3.0 / len(_STAGES)) + + # ---- extract panel ROI ------------------------------------------------- + t0 = time.perf_counter() + tracemalloc.start() + if panel_normal_seed is not None: + seed3 = np.asarray(panel_normal_seed, dtype=np.float64) + else: + seed3 = np.array([1.0, 0.0, 0.0]) + roi_pts, roi_diag = extract_panel_roi( + pts, + holes=detected_holes, + panel_normal_seed=seed3, + roi_cfg=cfg["roi"], + ) + if roi_pts.shape[0] < 100: + raise InsufficientPointsError( + f"only {roi_pts.shape[0]} points survived ROI extraction" + ) + _, peak = tracemalloc.get_traced_memory() + tracemalloc.stop() + timings.append(StageTiming("extract_roi", time.perf_counter() - t0, peak / 1024)) + _ping(progress, "extract_roi", 4.0 / len(_STAGES)) + + # ---- fit panel plane --------------------------------------------------- + t0 = time.perf_counter() + tracemalloc.start() + plane = fit_panel_plane(roi_pts, plane_fit_cfg=cfg["plane_fit"]) + if plane.inlier_ratio < float(cfg["plane_fit"].get("min_inlier_ratio", 0.6)): + quality_flags.append("plane_inlier_ratio_low") + _, peak = tracemalloc.get_traced_memory() + tracemalloc.stop() + timings.append(StageTiming("fit_plane", time.perf_counter() - t0, peak / 1024)) + _ping(progress, "fit_plane", 5.0 / len(_STAGES)) + + # ---- metrics ----------------------------------------------------------- + t0 = time.perf_counter() + metrics_cfg = cfg.get("metrics", {}) + metrics = compute_deflection_metrics( + normal=plane.normal, + center_C=frame.center_C, + panel_points=roi_pts, + right_axis_sign=int(metrics_cfg.get("right_axis_sign", 1)), + angle_clamp_deg=float(metrics_cfg.get("angle_clamp_deg", 0.0)), + ) + timings.append(StageTiming("metrics", time.perf_counter() - t0, 0.0)) + _ping(progress, "metrics", 1.0) + + report = { + "schema_version": SCHEMA_VERSION, + "algorithm_version": ALGORITHM_VERSION, + "config": cfg, + "n_points_in": int(pts.shape[0]), + "detection": detection.diagnostics, + "roi": roi_diag, + "plane": { + "normal": list(plane.normal), + "point_on_plane": list(plane.point_on_plane), + "d_offset": plane.d_offset, + "inlier_ratio": plane.inlier_ratio, + "inlier_residual_rms_mm": plane.inlier_residual_rms_mm, + }, + "frame": { + "center_C": list(frame.center_C), + "reference_plane_z": frame.reference_plane_z, + "longitudinal_mid_y": frame.longitudinal_mid_y, + }, + "metrics": metrics.to_dict(), + "quality_flags": quality_flags, + "timings": [ + {"stage": t.stage, "seconds": t.seconds, "peak_kib": t.peak_kib} + for t in timings + ], + } + + return CouplerMeasurement( + schema_version=SCHEMA_VERSION, + algorithm_version=ALGORITHM_VERSION, + theta_right_deg=metrics.theta_right_deg, + theta_ref_deg=metrics.theta_ref_deg, + D_y_max_mm=metrics.D_y_max_mm, + D_z_max_mm=metrics.D_z_max_mm, + dx_y_max_mm=metrics.dx_y_max_mm, + dx_z_max_mm=metrics.dx_z_max_mm, + center_C=frame.center_C, + normal=plane.normal, + holes=detected_holes, + plane=plane, + frame=frame, + quality_flags=quality_flags, + timings=timings, + report=report, + ) + + +__all__ = [ + "compute_coupler_measurement", + "ProgressCallback", +] diff --git a/alg/delivery_v1.1/coupler_panel_release/coupler_panel/cad/__init__.py b/alg/delivery_v1.1/coupler_panel_release/coupler_panel/cad/__init__.py new file mode 100644 index 0000000..36b2326 --- /dev/null +++ b/alg/delivery_v1.1/coupler_panel_release/coupler_panel/cad/__init__.py @@ -0,0 +1,21 @@ +"""CAD STP parsing + GT extraction. + +This package depends on ``cadquery-ocp`` (OCP) for OpenCascade access. If +OCP is not installed the modules will raise :class:`ImportError` at import +time only when actually used; pure point-cloud workflows do not need it. +""" + +from .stp_parser import StepShape, load_step +from .mesh import tessellate_to_trimesh +from .gt_extractor import ( + GroundTruth, + extract_ground_truth, +) + +__all__ = [ + "StepShape", + "load_step", + "tessellate_to_trimesh", + "GroundTruth", + "extract_ground_truth", +] diff --git a/alg/delivery_v1.1/coupler_panel_release/coupler_panel/cad/gt_extractor.py b/alg/delivery_v1.1/coupler_panel_release/coupler_panel/cad/gt_extractor.py new file mode 100644 index 0000000..8f1a2eb --- /dev/null +++ b/alg/delivery_v1.1/coupler_panel_release/coupler_panel/cad/gt_extractor.py @@ -0,0 +1,310 @@ +"""Extract Ground Truth geometry from a parsed STEP shape. + +Strategy +-------- + +1. Walk every circular edge in the shape (``iter_circular_edges``). +2. Cluster by radius: + - radii inside ``locating_hole_radius_mm`` → candidate locating holes + - radii ≥ ``central_hole_radius_min_mm`` → candidate central hole +3. Discard duplicates (front + back face of the same hole share centre and + axis up to a sign). +4. Find a set of four locating holes whose centres form a near-rectangle: + pairwise sort by axis-distance, then verify diagonals roughly equal. +5. Order the four locating holes consistently (anti-clockwise when viewed + from +x looking down −x). This ordering is the canonical pairing for + Kabsch later. +6. Define: + - ``C`` = mean of the four locating-hole centres (== diagonal + intersection for a rectangle) + - ``n`` = panel normal = unit axis of the locating circles, signed + so that ``n_x > 0`` + - reference line, plane, longitudinal mid-plane in :class:`ReferenceFrame` + +This GT can be used as +- the gold standard the measurement is compared against, AND +- a prior law that bootstraps point-cloud-side hole detection (we know the + panel normal a priori). +""" + +from __future__ import annotations + +from dataclasses import dataclass, field +from typing import Any + +import numpy as np + +from ..exceptions import CADParseError +from ..structures import Hole, HoleSet, PanelPlane, ReferenceFrame +from .stp_parser import StepShape, iter_circular_edges + + +@dataclass +class GroundTruth: + """All CAD-derived reference values used as the GT for validation.""" + + holes: HoleSet + plane: PanelPlane + frame: ReferenceFrame + raw_circles: list[dict[str, Any]] = field(default_factory=list) + + def to_dict(self) -> dict[str, Any]: + return { + "holes": { + "locating": [ + { + "center": list(h.center), + "radius_mm": h.radius_mm, + "fit_residual_rms_mm": h.fit_residual_rms_mm, + "role": h.role, + } + for h in self.holes.locating + ], + "central": ( + None + if self.holes.central is None + else { + "center": list(self.holes.central.center), + "radius_mm": self.holes.central.radius_mm, + "fit_residual_rms_mm": self.holes.central.fit_residual_rms_mm, + "role": self.holes.central.role, + } + ), + }, + "plane": { + "normal": list(self.plane.normal), + "point_on_plane": list(self.plane.point_on_plane), + "d_offset": self.plane.d_offset, + "inlier_ratio": self.plane.inlier_ratio, + "inlier_residual_rms_mm": self.plane.inlier_residual_rms_mm, + }, + "frame": { + "center_C": list(self.frame.center_C), + "line_direction": list(self.frame.line_direction), + "reference_plane_z": self.frame.reference_plane_z, + "longitudinal_mid_y": self.frame.longitudinal_mid_y, + }, + "raw_circles": self.raw_circles, + } + + +# --------------------------------------------------------------------------- +# Public entry point +# --------------------------------------------------------------------------- + + +def extract_ground_truth( + step: StepShape, + cad_cfg: dict[str, Any], +) -> GroundTruth: + """Extract GT from a parsed STEP file using the ``cad`` config block.""" + r_lo, r_hi = cad_cfg["locating_hole_radius_mm"] + r_central_min = float(cad_cfg["central_hole_radius_min_mm"]) + rect_aspect_tol = float(cad_cfg.get("rectangle_aspect_tol", 0.15)) + + raw: list[dict[str, Any]] = [] + locating: list[dict[str, Any]] = [] + central: list[dict[str, Any]] = [] + + for center, axis, radius, _edge in iter_circular_edges(step.shape): + item = { + "center": center, + "axis": axis, + "radius": radius, + } + raw.append(item) + if r_lo <= radius <= r_hi: + locating.append(item) + elif radius >= r_central_min: + central.append(item) + + if not locating: + raise CADParseError( + f"no circular edge with radius in [{r_lo}, {r_hi}] mm; " + "adjust cad.locating_hole_radius_mm to match your panel" + ) + + locating = _dedupe_circles(locating, r_tol=0.05, c_tol=0.5) + central = _dedupe_circles(central, r_tol=0.5, c_tol=2.0) + + four = _select_rectangle(locating, rect_aspect_tol=rect_aspect_tol) + four_ordered = _order_rectangle(four) + + panel_normal = _consensus_axis(four_ordered) + panel_normal = _enforce_nx_positive(panel_normal) + + centers = np.asarray([c["center"] for c in four_ordered], dtype=np.float64) + C = centers.mean(axis=0) + + holes = HoleSet( + locating=tuple( # type: ignore[arg-type] + Hole( + center=tuple(c["center"]), + radius_mm=float(c["radius"]), + fit_residual_rms_mm=0.0, + role="locating", + ) + for c in four_ordered + ), + central=( + Hole( + center=tuple(central[0]["center"]), + radius_mm=float(central[0]["radius"]), + fit_residual_rms_mm=0.0, + role="central", + ) + if central + else None + ), + ) + + plane = PanelPlane( + normal=tuple(panel_normal), + point_on_plane=tuple(C), + d_offset=float(-np.dot(panel_normal, C)), + inlier_ratio=1.0, + inlier_residual_rms_mm=0.0, + ) + + frame = ReferenceFrame( + center_C=tuple(C), + line_direction=(1.0, 0.0, 0.0), + reference_plane_z=float(C[2]), + longitudinal_mid_y=float(C[1]), + ) + + return GroundTruth(holes=holes, plane=plane, frame=frame, raw_circles=raw) + + +# --------------------------------------------------------------------------- +# Helpers +# --------------------------------------------------------------------------- + + +def _dedupe_circles( + items: list[dict[str, Any]], + *, + r_tol: float, + c_tol: float, +) -> list[dict[str, Any]]: + """Drop circles that share centre and radius (front/back face copies).""" + out: list[dict[str, Any]] = [] + for it in items: + c = np.asarray(it["center"]) + r = it["radius"] + is_dup = False + for kept in out: + if abs(kept["radius"] - r) <= r_tol: + if np.linalg.norm(np.asarray(kept["center"]) - c) <= c_tol: + is_dup = True + break + if not is_dup: + out.append(it) + return out + + +def _select_rectangle( + items: list[dict[str, Any]], + *, + rect_aspect_tol: float, +) -> list[dict[str, Any]]: + """Pick four items whose centres look most like a rectangle. + + For tractability we (a) require ≥ 4 candidates, (b) if exactly 4, return + them, (c) otherwise enumerate all 4-subsets and pick the one with the + smallest "rectangleness" residual. + """ + if len(items) < 4: + raise CADParseError( + f"only {len(items)} locating-hole candidates found; need ≥ 4" + ) + if len(items) == 4: + return items + + from itertools import combinations + + best_score = float("inf") + best: list[dict[str, Any]] | None = None + for combo in combinations(items, 4): + s = _rectangle_score(combo) + if s < best_score: + best_score = s + best = list(combo) + assert best is not None + if best_score > rect_aspect_tol: + # Still accept the best — the tolerance is informational; warn via + # raw_circles in the GT report. + pass + return best + + +def _rectangle_score(items) -> float: + """0 = perfect rectangle. Larger = worse.""" + c = np.asarray([i["center"] for i in items], dtype=np.float64) + # Use distances between the 4 centres. For a rectangle, the 6 pairwise + # distances are: two "long" sides equal, two "short" sides equal, two + # diagonals equal. + from itertools import combinations + + d = sorted(np.linalg.norm(c[i] - c[j]) for i, j in combinations(range(4), 2)) + if d[0] < 1e-6: + return 1e6 + s1, s2 = d[0], d[1] # short sides + l1, l2 = d[2], d[3] # long sides + g1, g2 = d[4], d[5] # diagonals + score = ( + abs(s1 - s2) / max(s1, s2) + + abs(l1 - l2) / max(l1, l2) + + abs(g1 - g2) / max(g1, g2) + ) + return score + + +def _order_rectangle(items: list[dict[str, Any]]) -> list[dict[str, Any]]: + """Return four hole dicts in a canonical (-y/-z, +y/-z, +y/+z, -y/+z) order. + + The ordering is taken in the y-z plane (panel normal is along ±x). This + matches how a panel is normally described: bottom-left, bottom-right, + top-right, top-left when viewed from +x looking down −x. + """ + c = np.asarray([i["center"] for i in items], dtype=np.float64) + cy = c[:, 1].mean() + cz = c[:, 2].mean() + quadrant: dict[tuple[bool, bool], int] = {} + for idx, p in enumerate(c): + key = (p[1] >= cy, p[2] >= cz) + quadrant[key] = idx + order = [ + (False, False), + (True, False), + (True, True), + (False, True), + ] + if not all(q in quadrant for q in order): + # Geometry is degenerate; fall back to angular sort. + ang = np.arctan2(c[:, 2] - cz, c[:, 1] - cy) + idxs = np.argsort(ang) + return [items[i] for i in idxs] + return [items[quadrant[q]] for q in order] + + +def _consensus_axis(items: list[dict[str, Any]]) -> np.ndarray: + """Average and re-normalise the circle axis directions.""" + axes = np.asarray([i["axis"] for i in items], dtype=np.float64) + # Flip any axes that disagree with the first. + ref = axes[0] + for k in range(1, len(axes)): + if np.dot(axes[k], ref) < 0: + axes[k] = -axes[k] + n = axes.mean(axis=0) + n /= max(np.linalg.norm(n), 1e-12) + return n + + +def _enforce_nx_positive(n: np.ndarray) -> np.ndarray: + if n[0] < 0: + return -n + return n + + +__all__ = ["GroundTruth", "extract_ground_truth"] diff --git a/alg/delivery_v1.1/coupler_panel_release/coupler_panel/cad/mesh.py b/alg/delivery_v1.1/coupler_panel_release/coupler_panel/cad/mesh.py new file mode 100644 index 0000000..3aaf926 --- /dev/null +++ b/alg/delivery_v1.1/coupler_panel_release/coupler_panel/cad/mesh.py @@ -0,0 +1,143 @@ +"""Tessellation of a TopoDS_Shape into a (V, F) triangle mesh. + +The output is a small dataclass — we do not import ``trimesh`` until the +caller actually asks for it, to keep the core dependency surface minimal. +""" + +from __future__ import annotations + +from dataclasses import dataclass +from typing import Any + +import numpy as np + +from .stp_parser import StepShape, _import_ocp, iter_faces + + +@dataclass +class TriangleMesh: + """Vertex / face arrays. + + Attributes + ---------- + vertices : ``(V, 3) float64`` + faces : ``(F, 3) int64`` (vertex indices) + face_normals : ``(F, 3) float64`` + """ + + vertices: np.ndarray + faces: np.ndarray + face_normals: np.ndarray + + @property + def n_vertices(self) -> int: + return int(self.vertices.shape[0]) + + @property + def n_faces(self) -> int: + return int(self.faces.shape[0]) + + def to_trimesh(self): + """Return a ``trimesh.Trimesh`` (lazy import).""" + try: + import trimesh + except ImportError as exc: + raise ImportError( + "trimesh is required for to_trimesh(); " + "install with: pip install 'coupler-panel-validation[cad]'" + ) from exc + return trimesh.Trimesh( + vertices=self.vertices, + faces=self.faces, + face_normals=self.face_normals, + process=False, + ) + + +def tessellate_to_trimesh( + step: StepShape, + *, + linear_deflection_mm: float = 0.5, + angular_deflection_rad: float = 0.1, +) -> TriangleMesh: + """Tessellate a :class:`StepShape` into a single combined triangle mesh. + + Each face is tessellated independently (with the location of the edge + transferred onto the vertices), then concatenated. Triangle winding is + flipped where the OCP face is reversed, so :attr:`face_normals` is the + *outward* normal in the assembly frame. + """ + occ = _import_ocp() + BRep_Tool = occ["BRep_Tool"] + TopLoc_Location = occ["TopLoc_Location"] + BRepMesh_IncrementalMesh = occ["BRepMesh_IncrementalMesh"] + + BRepMesh_IncrementalMesh( + step.shape, + float(linear_deflection_mm), + False, + float(angular_deflection_rad), + True, + ) + + all_v: list[np.ndarray] = [] + all_f: list[np.ndarray] = [] + all_n: list[np.ndarray] = [] + v_offset = 0 + + for face in iter_faces(step.shape): + loc = TopLoc_Location() + triangulation = BRep_Tool.Triangulation_s(face, loc) + if triangulation is None: + continue + + trsf = loc.Transformation() + n_nodes = triangulation.NbNodes() + n_tri = triangulation.NbTriangles() + if n_nodes == 0 or n_tri == 0: + continue + + v = np.empty((n_nodes, 3), dtype=np.float64) + for i in range(n_nodes): + p = triangulation.Node(i + 1).Transformed(trsf) + v[i, 0] = p.X() + v[i, 1] = p.Y() + v[i, 2] = p.Z() + + f = np.empty((n_tri, 3), dtype=np.int64) + is_reversed = face.Orientation() == 1 # TopAbs_REVERSED + for j in range(n_tri): + tri = triangulation.Triangle(j + 1) + i1, i2, i3 = tri.Get() + if is_reversed: + f[j] = (i3 - 1, i2 - 1, i1 - 1) + else: + f[j] = (i1 - 1, i2 - 1, i3 - 1) + + # Outward face normal per triangle. + a = v[f[:, 0]] + b = v[f[:, 1]] + c = v[f[:, 2]] + normals = np.cross(b - a, c - a) + norms = np.linalg.norm(normals, axis=1, keepdims=True) + norms[norms < 1e-12] = 1.0 + normals = normals / norms + + all_v.append(v) + all_f.append(f + v_offset) + all_n.append(normals) + v_offset += n_nodes + + if not all_v: + raise RuntimeError( + "tessellation produced no triangles; " + "try a finer linear_deflection_mm" + ) + + vertices = np.concatenate(all_v, axis=0) + faces = np.concatenate(all_f, axis=0) + face_normals = np.concatenate(all_n, axis=0) + return TriangleMesh(vertices=vertices, faces=faces, face_normals=face_normals) + + +__all__ = ["TriangleMesh", "tessellate_to_trimesh"] diff --git a/alg/delivery_v1.1/coupler_panel_release/coupler_panel/cad/stp_parser.py b/alg/delivery_v1.1/coupler_panel_release/coupler_panel/cad/stp_parser.py new file mode 100644 index 0000000..116fdce --- /dev/null +++ b/alg/delivery_v1.1/coupler_panel_release/coupler_panel/cad/stp_parser.py @@ -0,0 +1,219 @@ +"""STP file loading via OCP (OpenCascade Python bindings). + +The OCP import is deferred so the rest of the package does not require +OpenCascade to be installed. + +The parser exposes a thin :class:`StepShape` wrapper that gives the rest +of the package what it needs: the underlying ``TopoDS_Shape`` for +tessellation and topology traversal. +""" + +from __future__ import annotations + +from dataclasses import dataclass +from pathlib import Path +from typing import Any + +from ..exceptions import CADParseError + + +@dataclass +class StepShape: + """Result of loading a STEP file. + + Attributes + ---------- + shape : ``OCP.TopoDS.TopoDS_Shape`` + The root shape (assembly compound or single solid). + source : ``Path`` + Source file path, for debugging. + """ + + shape: Any + source: Path + + +def _import_ocp() -> dict[str, Any]: + """Import the OCP modules we need. Raises a clean error if missing.""" + try: + from OCP.STEPControl import STEPControl_Reader # type: ignore + from OCP.IFSelect import IFSelect_RetDone # type: ignore + from OCP.TopoDS import TopoDS, TopoDS_Shape # type: ignore + from OCP.TopAbs import ( # type: ignore + TopAbs_FACE, + TopAbs_EDGE, + TopAbs_VERTEX, + ) + from OCP.TopExp import TopExp_Explorer # type: ignore + from OCP.TopLoc import TopLoc_Location # type: ignore + from OCP.BRep import BRep_Tool # type: ignore + from OCP.BRepMesh import BRepMesh_IncrementalMesh # type: ignore + from OCP.BRepGProp import BRepGProp # type: ignore + from OCP.GProp import GProp_GProps # type: ignore + from OCP.GeomAbs import GeomAbs_Circle # type: ignore + from OCP.BRepAdaptor import BRepAdaptor_Curve, BRepAdaptor_Surface # type: ignore + from OCP.gp import gp_Pnt, gp_Dir, gp_Vec # type: ignore + except ImportError as exc: + raise ImportError( + "OCP (cadquery-ocp) is required to parse STEP files. " + "Install with: pip install 'coupler-panel-validation[cad]'" + ) from exc + + return { + "STEPControl_Reader": STEPControl_Reader, + "IFSelect_RetDone": IFSelect_RetDone, + "TopoDS": TopoDS, + "TopoDS_Shape": TopoDS_Shape, + "TopAbs_FACE": TopAbs_FACE, + "TopAbs_EDGE": TopAbs_EDGE, + "TopAbs_VERTEX": TopAbs_VERTEX, + "TopExp_Explorer": TopExp_Explorer, + "TopLoc_Location": TopLoc_Location, + "BRep_Tool": BRep_Tool, + "BRepMesh_IncrementalMesh": BRepMesh_IncrementalMesh, + "BRepGProp": BRepGProp, + "GProp_GProps": GProp_GProps, + "GeomAbs_Circle": GeomAbs_Circle, + "BRepAdaptor_Curve": BRepAdaptor_Curve, + "BRepAdaptor_Surface": BRepAdaptor_Surface, + "gp_Pnt": gp_Pnt, + "gp_Dir": gp_Dir, + "gp_Vec": gp_Vec, + } + + +def load_step(path: str | Path) -> StepShape: + """Read a STEP file and return its root :class:`TopoDS_Shape`. + + Raises :class:`CADParseError` on read failure. + """ + p = Path(path) + if not p.exists(): + raise CADParseError(f"STEP file not found: {p}") + + occ = _import_ocp() + reader = occ["STEPControl_Reader"]() + status = reader.ReadFile(str(p)) + if status != occ["IFSelect_RetDone"]: + raise CADParseError(f"OCP failed to read {p} (status={status})") + n_roots = reader.TransferRoots() + if n_roots <= 0: + raise CADParseError(f"no roots transferred from {p}") + + shape = reader.OneShape() + return StepShape(shape=shape, source=p) + + +# --------------------------------------------------------------------------- +# Topology iteration helpers (used by gt_extractor) +# --------------------------------------------------------------------------- + + +def _shape_key(shape) -> int: + """Stable identity key for a TopoDS_Shape across OCP versions. + + OCP 7.8+ removed ``HashCode``. We fall back to the underlying TShape's + Python id, which is stable per OpenCASCADE topology object. + """ + try: + return shape.HashCode(2**31 - 1) + except AttributeError: + try: + return id(shape.TShape()) + except Exception: + return id(shape) + + +def iter_faces(shape: Any): + """Yield every TopoDS_Face inside ``shape``. + + Two visits of the *same underlying surface with opposite orientations* + are treated as distinct because the orientation governs the outward + normal of the produced triangulation. + """ + occ = _import_ocp() + TopoDS = occ["TopoDS"] + explorer = occ["TopExp_Explorer"](shape, occ["TopAbs_FACE"]) + while explorer.More(): + raw = explorer.Current() + try: + yield TopoDS.Face_s(raw) + except Exception: + pass + explorer.Next() + + +def iter_circular_edges(shape: Any): + """Yield ``(center_xyz, axis_xyz, radius_mm, edge)`` for every circular + edge in ``shape``. Duplicates (same circle on two adjacent faces) are + NOT removed here — downstream ``_dedupe_circles`` does that by + geometry (centre + radius). + """ + occ = _import_ocp() + explorer = occ["TopExp_Explorer"](shape, occ["TopAbs_EDGE"]) + TopoDS = occ["TopoDS"] + BRepAdaptor_Curve = occ["BRepAdaptor_Curve"] + GeomAbs_Circle = occ["GeomAbs_Circle"] + while explorer.More(): + raw = explorer.Current() + try: + edge = TopoDS.Edge_s(raw) + curve = BRepAdaptor_Curve(edge) + if curve.GetType() == GeomAbs_Circle: + circ = curve.Circle() + loc = circ.Location() + ax = circ.Axis().Direction() + radius = float(circ.Radius()) + center = (float(loc.X()), float(loc.Y()), float(loc.Z())) + axis = (float(ax.X()), float(ax.Y()), float(ax.Z())) + yield center, axis, radius, edge + except Exception: + pass + explorer.Next() + + +def iter_cylindrical_faces(shape: Any): + """Yield ``(center, axis, radius, face)`` for every cylinder face. + + Cylinder faces are typically the *walls* of through-holes; their + radii match the circular edges that bound them. + """ + occ = _import_ocp() + TopoDS = occ["TopoDS"] + BRepAdaptor_Surface = occ["BRepAdaptor_Surface"] + explorer = occ["TopExp_Explorer"](shape, occ["TopAbs_FACE"]) + while explorer.More(): + raw = explorer.Current() + try: + face = TopoDS.Face_s(raw) + surf = BRepAdaptor_Surface(face) + if int(surf.GetType()) == 1: + cyl = surf.Cylinder() + loc = cyl.Location() + ax = cyl.Axis().Direction() + radius = float(cyl.Radius()) + yield ( + (float(loc.X()), float(loc.Y()), float(loc.Z())), + (float(ax.X()), float(ax.Y()), float(ax.Z())), + radius, + face, + ) + except Exception: + pass + explorer.Next() + + +def face_area(face: Any) -> float: + occ = _import_ocp() + props = occ["GProp_GProps"]() + occ["BRepGProp"].SurfaceProperties_s(face, props) + return float(props.Mass()) + + +__all__ = [ + "StepShape", + "load_step", + "iter_faces", + "iter_circular_edges", + "face_area", +] diff --git a/alg/delivery_v1.1/coupler_panel_release/coupler_panel/cli.py b/alg/delivery_v1.1/coupler_panel_release/coupler_panel/cli.py new file mode 100644 index 0000000..b4119c5 --- /dev/null +++ b/alg/delivery_v1.1/coupler_panel_release/coupler_panel/cli.py @@ -0,0 +1,161 @@ +"""``coupler-measure`` command-line entry point. + +Three modes:: + + # measure an existing point cloud (production-like flow) + coupler-measure --input-cloud scan.ply --out-dir runs/demo + + # research: synthesise a cloud from a STEP file and measure it + coupler-measure --stp panel.stp --out-dir runs/from_cad + + # research: parameter sweep + coupler-measure --sweep matrix.yaml --out-dir runs/sweep_xx +""" + +from __future__ import annotations + +import argparse +import json +import logging +import sys +from pathlib import Path + + +log = logging.getLogger("coupler_panel.cli") + + +def _build_parser() -> argparse.ArgumentParser: + p = argparse.ArgumentParser( + prog="coupler-measure", + description="Coupler-panel CAD-STP to point-cloud measurement & validation.", + ) + src = p.add_mutually_exclusive_group(required=True) + src.add_argument("--input-cloud", type=Path, help="Path to a PLY/XYZ/CSV/PCD point cloud.") + src.add_argument("--stp", type=Path, help="Path to a STEP file (research mode).") + src.add_argument("--sweep", type=Path, help="Path to a sweep matrix YAML.") + + p.add_argument("--out-dir", type=Path, required=True, help="Output directory.") + p.add_argument("--config", type=Path, default=None, help="YAML config overrides.") + p.add_argument( + "--profile", + type=str, + default="auto", + help="auto | default | high_density | low_density | custom", + ) + p.add_argument( + "--target-holes", + type=Path, + default=None, + help="DEPRECATED — the main pipeline no longer performs 4-hole Kabsch " + "registration (it cancels the panel deflection signal). The flag " + "is accepted for backward compatibility but is ignored.", + ) + p.add_argument( + "--render", + action="store_true", + help="After measurement, render PNG diagnostics into /images/.", + ) + p.add_argument("-v", "--verbose", action="count", default=0, help="-v: INFO, -vv: DEBUG") + return p + + +def _setup_logging(verbose: int) -> None: + level = logging.WARNING + if verbose == 1: + level = logging.INFO + elif verbose >= 2: + level = logging.DEBUG + logging.basicConfig( + level=level, + format="%(asctime)s [%(levelname)s] %(name)s: %(message)s", + ) + + +def _on_progress(stage: str, fraction: float) -> None: + log.info("[%5.1f%%] %s", fraction * 100, stage) + + +def _maybe_render(out_dir: Path) -> None: + try: + from .viz import render_all + + render_all(out_dir) + except ImportError: + log.warning("matplotlib not installed; skip --render") + except Exception as exc: + log.warning("render_all failed: %s", exc) + + +def main(argv: list[str] | None = None) -> int: + args = _build_parser().parse_args(argv) + _setup_logging(args.verbose) + + try: + if args.input_cloud is not None: + from .pipeline import run_coupler_measurement_pipeline + + if args.target_holes is not None: + log.warning( + "--target-holes is deprecated and ignored; the main pipeline " + "no longer performs Kabsch registration." + ) + result = run_coupler_measurement_pipeline( + input_cloud=args.input_cloud, + out_dir=args.out_dir, + config_path=args.config, + profile=args.profile, + progress=_on_progress, + ) + print( + json.dumps( + { + "theta_right_deg": result.theta_right_deg, + "theta_ref_deg": result.theta_ref_deg, + "dx_y_max_mm": result.dx_y_max_mm, + "dx_z_max_mm": result.dx_z_max_mm, + "quality_flags": result.quality_flags, + }, + indent=2, + ensure_ascii=False, + ) + ) + + elif args.stp is not None: + from .pipeline import run_from_cad_pipeline + + summary = run_from_cad_pipeline( + stp_path=args.stp, + out_dir=args.out_dir, + config_path=args.config, + profile=args.profile, + progress=_on_progress, + ) + print( + json.dumps( + { + "diff": summary["diff"], + "quality_flags": summary["measured"]["quality_flags"], + }, + indent=2, + ensure_ascii=False, + ) + ) + + elif args.sweep is not None: + from importlib import import_module + + mod = import_module("experiments.sweep_runner") + mod.main(matrix_path=args.sweep, out_dir=args.out_dir, config_path=args.config) + + if args.render: + _maybe_render(args.out_dir) + + except Exception as exc: + log.exception("pipeline failed: %s", exc) + return 2 + + return 0 + + +if __name__ == "__main__": + sys.exit(main()) diff --git a/alg/delivery_v1.1/coupler_panel_release/coupler_panel/config.py b/alg/delivery_v1.1/coupler_panel_release/coupler_panel/config.py new file mode 100644 index 0000000..515a375 --- /dev/null +++ b/alg/delivery_v1.1/coupler_panel_release/coupler_panel/config.py @@ -0,0 +1,209 @@ +"""Configuration loading and merging. + +Configs are nested dicts. The single source of truth for defaults is +``configs/default.yaml`` (shipped alongside the package); ``DEFAULT_CONFIG`` +mirrors its structure for in-memory programmatic overrides. + +Public functions: + +- :func:`load_config` — load a YAML file or accept an already-built dict. +- :func:`load_profile` — load ``configs/profiles/.yaml`` and merge + on top of the defaults. +- :func:`deep_merge` — recursive dict merge (override wins). +- :func:`apply_dotted` — set a value via ``"sampling.spacing_mm"``. +- :func:`resolve_auto` — fill in fields whose value is the literal + string ``"auto"`` (currently: ``plane_fit.ransac_inlier_thresh_mm``). +""" + +from __future__ import annotations + +import copy +from pathlib import Path +from typing import Any + +import yaml + + +_PKG_ROOT = Path(__file__).resolve().parent +_DEFAULT_YAML = _PKG_ROOT.parent / "configs" / "default.yaml" + + +def _load_yaml(path: Path) -> dict[str, Any]: + with path.open("r", encoding="utf-8") as fh: + data = yaml.safe_load(fh) + if not isinstance(data, dict): + raise ValueError(f"{path}: expected a mapping at the top level") + return data + + +def _load_default() -> dict[str, Any]: + if _DEFAULT_YAML.exists(): + return _load_yaml(_DEFAULT_YAML) + # Package may have been installed without configs/. Fall back to a minimal + # in-code default so library use without the YAML still works. + return _IN_CODE_FALLBACK + + +# Loaded lazily; recomputed on each call to keep tests deterministic. +def default_config() -> dict[str, Any]: + """Return a fresh copy of the default configuration.""" + return copy.deepcopy(_load_default()) + + +DEFAULT_CONFIG: dict[str, Any] # populated below for convenience + + +def load_config( + source: str | Path | dict[str, Any] | None, + *, + profile: str = "auto", +) -> dict[str, Any]: + """Resolve the final configuration to use. + + The merge order is:: + + default.yaml < profile (if any) < source (path or dict) + + Parameters + ---------- + source : path, dict, or None + If ``None``, only defaults (and the profile) are used. + profile : str + ``"auto"`` / ``"default"`` skip the profile step. Any other name + is looked up at ``configs/profiles/.yaml``. + """ + cfg = default_config() + + if profile not in (None, "auto", "default", "custom"): + prof_path = _PKG_ROOT.parent / "configs" / "profiles" / f"{profile}.yaml" + if not prof_path.exists(): + raise FileNotFoundError(f"profile '{profile}' not found at {prof_path}") + cfg = deep_merge(cfg, _load_yaml(prof_path)) + + if source is None: + pass + elif isinstance(source, dict): + cfg = deep_merge(cfg, source) + else: + cfg = deep_merge(cfg, _load_yaml(Path(source))) + + return cfg + + +def load_profile(name: str) -> dict[str, Any]: + """Load a profile YAML directly; useful for tests.""" + path = _PKG_ROOT.parent / "configs" / "profiles" / f"{name}.yaml" + return _load_yaml(path) + + +def deep_merge(base: dict[str, Any], override: dict[str, Any]) -> dict[str, Any]: + """Recursive dict merge. ``override`` wins on leaf collisions.""" + out = copy.deepcopy(base) + for key, val in override.items(): + if ( + key in out + and isinstance(out[key], dict) + and isinstance(val, dict) + ): + out[key] = deep_merge(out[key], val) + else: + out[key] = copy.deepcopy(val) + return out + + +def apply_dotted(cfg: dict[str, Any], dotted_key: str, value: Any) -> dict[str, Any]: + """Set ``cfg["a"]["b"]["c"] = value`` from ``"a.b.c"``. Returns ``cfg``.""" + parts = dotted_key.split(".") + node = cfg + for p in parts[:-1]: + node = node.setdefault(p, {}) + if not isinstance(node, dict): + raise TypeError(f"cannot apply '{dotted_key}': '{p}' is not a dict") + node[parts[-1]] = value + return cfg + + +def resolve_auto(cfg: dict[str, Any]) -> dict[str, Any]: + """Replace literal ``"auto"`` markers with concrete values. + + Currently: + + - ``plane_fit.ransac_inlier_thresh_mm == "auto"`` → ``3 * noise.sigma_mm`` + (or 0.5 mm if ``noise.sigma_mm == 0``). + """ + out = copy.deepcopy(cfg) + pf = out.get("plane_fit", {}) + if pf.get("ransac_inlier_thresh_mm", None) == "auto": + sigma = float(out.get("noise", {}).get("sigma_mm", 0.0) or 0.0) + pf["ransac_inlier_thresh_mm"] = max(3.0 * sigma, 0.5) + out["plane_fit"] = pf + return out + + +# Minimal in-code fallback used only when the YAML is missing. +_IN_CODE_FALLBACK: dict[str, Any] = { + "schema_version": "1.0", + "cad": { + "linear_deflection_mm": 0.5, + "angular_deflection_rad": 0.1, + "locating_hole_radius_mm": [4.0, 12.0], + "central_hole_radius_min_mm": 20.0, + "rectangle_aspect_tol": 0.15, + }, + "sampling": {"method": "poisson_disk", "spacing_mm": 1.0, "num_points": 100000, "seed": 42}, + "noise": {"enabled": True, "sigma_mm": 0.5, "mode": "isotropic", "seed": 43}, + "transform": { + "enabled": True, + "rotation_deg": 3.0, + "rotation_axis": "random", + "translation_mm": 5.0, + "translation_dir": "random", + "seed": 44, + }, + "denoise": {"steps": []}, + "registration": {"method": "kabsch_four_holes", "pre_align": "pca_centroid"}, + "detection": { + "panel_normal_seed": "from_cad", + "grid_pixel_mm": 0.5, + "panel_min_area_ratio": 0.4, + "hole_min_area_mm2": 30.0, + "hole_max_area_mm2": 800.0, + "circle_fit": {"method": "algebraic_then_lsq", "inlier_residual_mm": 0.5}, + "select_top_k_by_radius_match": 4, + "rectangle_aspect_tol": 0.20, + }, + "roi": { + "hole_dilate_mm": 3.0, + "central_hole_dilate_mm": 5.0, + "edge_buffer_mm": 5.0, + "chamfer_normal_dot_min": 0.85, + "outlier_z_score": 3.0, + }, + "plane_fit": { + "method": "ransac_svd", + "ransac_iterations": 200, + "ransac_inlier_thresh_mm": "auto", + "min_inlier_ratio": 0.6, + "enforce_nx_positive": True, + }, + "metrics": {"right_axis_sign": 1, "angle_clamp_deg": 0.0}, + "artifacts": { + "save_intermediate": True, + "ply_format": "binary", + "image_dpi": 150, + "heatmap_colormap": "viridis", + }, +} + +DEFAULT_CONFIG = copy.deepcopy(_IN_CODE_FALLBACK) + + +__all__ = [ + "DEFAULT_CONFIG", + "default_config", + "load_config", + "load_profile", + "deep_merge", + "apply_dotted", + "resolve_auto", +] diff --git a/alg/delivery_v1.1/coupler_panel_release/coupler_panel/data_prep.py b/alg/delivery_v1.1/coupler_panel_release/coupler_panel/data_prep.py new file mode 100644 index 0000000..a47d70e --- /dev/null +++ b/alg/delivery_v1.1/coupler_panel_release/coupler_panel/data_prep.py @@ -0,0 +1,528 @@ +"""Data-preparation pipeline (research / validation use only). + +This module is **not** part of the integration API. It exists so that +researchers can take a CAD STP file, synthesise scan-like point clouds, +denoise them, and align them to the CAD frame — i.e. it produces an +input that :func:`coupler_panel.api.compute_coupler_measurement` can +then run on. + +Stages performed (each one is independently re-runnable so the experiment +sweep can iterate over individual knobs):: + + parse_cad STP → mesh + GT + synthesize_cloud mesh → sampled (multi-strategy) → noisy → transformed + denoise_cloud cloud → cloud (configurable pipeline) + register_cloud cloud → cloud (4-hole Kabsch into CAD frame) + +The end-to-end convenience wrapper :func:`prepare_cloud_from_cad` glues +the four stages together and returns a :class:`PreparedCloud` ready to +hand to the measurement API. +""" + +from __future__ import annotations + +from dataclasses import dataclass, field +from pathlib import Path +from typing import Any + +import numpy as np + +from .config import load_config, resolve_auto +from .denoise import run_pipeline as run_denoise +from .detection import detect_four_holes +from .exceptions import RegistrationError +from .registration import register_four_holes +from .structures import Hole, HoleSet + + +# --------------------------------------------------------------------------- +# Result types +# --------------------------------------------------------------------------- + + +@dataclass +class CADParsed: + """Output of :func:`parse_cad`.""" + + mesh: Any # coupler_panel.cad.mesh.TriangleMesh + gt: Any # coupler_panel.cad.gt_extractor.GroundTruth + + +@dataclass +class SynthesizedCloud: + """Output of :func:`synthesize_cloud`.""" + + sampled: np.ndarray # (Ns, 3) fresh from the mesh, optional HPR applied + deflected: np.ndarray # (Ns, 3) + panel-deflection injection (known GT) + noisy: np.ndarray # (Ns, 3) + Gaussian noise + transformed: np.ndarray # (Ns, 3) + scanner-pose rigid transform + gt_transform_4x4: np.ndarray # (4, 4) scanner-pose transform applied at end + gt_holes_in_scan_frame: HoleSet # deflected GT holes after T_jitter — Kabsch target + # ---- Panel-deflection ground truth ---------------------------------- + panel_deflection_R: np.ndarray # (3, 3) rotation that tilts the panel + panel_deflection_theta_right_deg: float + panel_deflection_theta_ref_deg: float + gt_holes_after_deflection: HoleSet # in CAD frame, post-deflection + gt_normal_after_deflection: np.ndarray # (3,) deflected panel normal in CAD frame + info: dict[str, Any] = field(default_factory=dict) + + +@dataclass +class DenoisedCloud: + """Output of :func:`denoise_cloud`.""" + + points: np.ndarray + log: list[dict[str, Any]] + + +@dataclass +class RegisteredCloud: + """Output of :func:`register_cloud`.""" + + points: np.ndarray # cloud aligned into the CAD frame + transform_4x4: np.ndarray # the rigid alignment (scan → CAD) + rmse_mm: float + max_residual_mm: float + detected_holes_in_cad_frame: HoleSet + + +@dataclass +class PreparedCloud: + """End-to-end output, ready to feed into the public measurement API. + + ``points`` is in the CAD frame, denoised, with all the meta-data the + experiment / validation harness needs to score the measurement. + """ + + points: np.ndarray + gt: Any # GroundTruth + gt_transform_4x4: np.ndarray | None + registration_transform_4x4: np.ndarray | None + registration_rmse_mm: float | None + detected_holes_in_cad_frame: HoleSet | None + intermediates: dict[str, Any] = field(default_factory=dict) + + +# --------------------------------------------------------------------------- +# Stage 1 — STP → mesh + GT +# --------------------------------------------------------------------------- + + +def parse_cad(stp_path: str | Path, cad_cfg: dict[str, Any]) -> CADParsed: + from .cad import extract_ground_truth, load_step, tessellate_to_trimesh + + step = load_step(stp_path) + mesh = tessellate_to_trimesh( + step, + linear_deflection_mm=float(cad_cfg["linear_deflection_mm"]), + angular_deflection_rad=float(cad_cfg["angular_deflection_rad"]), + ) + gt = extract_ground_truth(step, cad_cfg) + return CADParsed(mesh=mesh, gt=gt) + + +# --------------------------------------------------------------------------- +# Stage 2 — mesh → sampled → noisy → transformed +# --------------------------------------------------------------------------- + + +def _build_panel_deflection_R(theta_right_deg: float, theta_ref_deg: float) -> np.ndarray: + """Build the 3×3 rotation that tilts a panel whose normal is (1,0,0). + + The post-rotation normal is:: + + n = (cos(th_ref)·cos(th_right), cos(th_ref)·sin(th_right), sin(th_ref)) + + so that ``atan2(n_y, n_x) = th_right`` and ``atan2(n_z, n_x) = th_ref`` + exactly. + + R = R_z(th_right) · R_y(-th_ref) + """ + a = np.deg2rad(float(theta_right_deg)) + b = np.deg2rad(float(theta_ref_deg)) + Rz = np.array( + [ + [np.cos(a), -np.sin(a), 0.0], + [np.sin(a), np.cos(a), 0.0], + [0.0, 0.0, 1.0], + ] + ) + Ry_neg = np.array( + [ + [np.cos(b), 0.0, -np.sin(b)], + [0.0, 1.0, 0.0], + [np.sin(b), 0.0, np.cos(b)], + ] + ) + return Rz @ Ry_neg + + +def _rotate_holeset(holes: HoleSet, R: np.ndarray) -> HoleSet: + locating = tuple( + Hole( + center=tuple((R @ np.asarray(h.center)).tolist()), + radius_mm=h.radius_mm, + fit_residual_rms_mm=h.fit_residual_rms_mm, + role=h.role, + ) + for h in holes.locating + ) + central = ( + None + if holes.central is None + else Hole( + center=tuple((R @ np.asarray(holes.central.center)).tolist()), + radius_mm=holes.central.radius_mm, + fit_residual_rms_mm=holes.central.fit_residual_rms_mm, + role="central", + ) + ) + return HoleSet(locating=locating, central=central) + + +def synthesize_cloud( + parsed: CADParsed, + *, + sampling_cfg: dict[str, Any], + noise_cfg: dict[str, Any], + transform_cfg: dict[str, Any], + scan_cfg: dict[str, Any] | None = None, + deflection_cfg: dict[str, Any] | None = None, +) -> SynthesizedCloud: + from .sampling import resolve_num_points, sample_mesh + from .simulation import ( + add_gaussian_noise, + apply_rigid, + sample_random_rigid, + simulate_scan_view, + ) + + n = resolve_num_points( + parsed.mesh, + num_points=sampling_cfg.get("num_points"), + spacing_mm=sampling_cfg.get("spacing_mm"), + ) + sampled = sample_mesh( + parsed.mesh, + method=sampling_cfg["method"], + num_points=n, + spacing_mm=sampling_cfg.get("spacing_mm"), + seed=int(sampling_cfg.get("seed", 42)), + curvature_weight=float(sampling_cfg.get("adaptive", {}).get("curvature_weight", 1.0)), + ) + + # Optional: simulate a single-view scan via hidden-point removal. + # Without this, the sampler returns points from BOTH sides of the panel + # (front face, back face, cylinder hole walls, …); the projected + # occupancy grid then has no cavities because the back-face points fill + # the hole region. Enable this for any "facing-scan" simulation. + if scan_cfg and bool(scan_cfg.get("enabled", False)): + view = scan_cfg.get("view_origin", [10000.0, 0.0, 0.0]) + sampled = simulate_scan_view( + sampled, + view_origin=view, + hpr_radius_mm=float(scan_cfg.get("hpr_radius_mm", 50000.0)), + ) + + # ---- Panel deflection injection --------------------------------------- + # This is the *signal* the algorithm should later recover. We rotate + # the entire panel (and its 4 locating holes + central hole) by a known + # small angle so that the GT panel normal becomes + # n_new = R · (1, 0, 0) + # with atan2(n_new.y, n_new.x) = theta_right_deg (rotation around z) + # and atan2(n_new.z, n_new.x) = theta_ref_deg (rotation around -y). + # The 4 hole centres rotate with the panel, so 4-hole Kabsch (which + # targets the *deflected* GT holes) does NOT undo the deflection — the + # panel orientation in the registered cloud still carries the signal. + th_right = 0.0 + th_ref = 0.0 + if deflection_cfg and bool(deflection_cfg.get("enabled", False)): + th_right = float(deflection_cfg.get("theta_right_deg", 0.0)) + th_ref = float(deflection_cfg.get("theta_ref_deg", 0.0)) + R_panel = _build_panel_deflection_R(th_right, th_ref) + deflected = sampled @ R_panel.T + gt_holes_after = _rotate_holeset(parsed.gt.holes, R_panel) + gt_normal_after = R_panel @ np.asarray(parsed.gt.plane.normal, dtype=np.float64) + sampled = deflected # downstream uses the deflected cloud as the new ground + + if bool(noise_cfg.get("enabled", True)) and float(noise_cfg.get("sigma_mm", 0.0)) > 0: + noisy = add_gaussian_noise( + sampled, + sigma_mm=float(noise_cfg["sigma_mm"]), + mode=noise_cfg.get("mode", "isotropic"), + seed=int(noise_cfg.get("seed", 43)), + ) + else: + noisy = sampled.copy() + + # The "GT-side" scan-pose perturbation: shifts and rotates the cloud as + # a whole to mimic mounting / scanner-stage repeatability error. It is + # NOT the deflection we want to measure; 4-hole Kabsch should cancel it. + if bool(transform_cfg.get("enabled", True)): + T = sample_random_rigid( + rotation_deg=float(transform_cfg.get("rotation_deg", 0.0)), + translation_mm=float(transform_cfg.get("translation_mm", 0.0)), + rotation_axis=transform_cfg.get("rotation_axis", "random"), + translation_dir=transform_cfg.get("translation_dir", "random"), + seed=int(transform_cfg.get("seed", 44)), + ) + transformed = apply_rigid(noisy, T.matrix) + Mat = T.matrix + R = Mat[:3, :3] + t = Mat[:3, 3] + # Deflected GT holes, then jitter — same composition the cloud went through. + gt_holes_scan = HoleSet( + locating=tuple( # type: ignore[arg-type] + Hole( + center=tuple((R @ np.asarray(h.center) + t).tolist()), + radius_mm=h.radius_mm, + fit_residual_rms_mm=0.0, + role=h.role, + ) + for h in gt_holes_after.locating + ), + central=( + None + if gt_holes_after.central is None + else Hole( + center=tuple( + (R @ np.asarray(gt_holes_after.central.center) + t).tolist() + ), + radius_mm=gt_holes_after.central.radius_mm, + fit_residual_rms_mm=0.0, + role="central", + ) + ), + ) + info = {"transform": T.to_dict()} + gt_T = T.matrix + else: + transformed = noisy.copy() + gt_holes_scan = gt_holes_after + info = {"transform": {"enabled": False}} + gt_T = np.eye(4) + + return SynthesizedCloud( + sampled=sampled, + deflected=deflected, + noisy=noisy, + transformed=transformed, + gt_transform_4x4=gt_T, + gt_holes_in_scan_frame=gt_holes_scan, + panel_deflection_R=R_panel, + panel_deflection_theta_right_deg=th_right, + panel_deflection_theta_ref_deg=th_ref, + gt_holes_after_deflection=gt_holes_after, + gt_normal_after_deflection=gt_normal_after, + info=info, + ) + + +# --------------------------------------------------------------------------- +# Stage 3 — denoise +# --------------------------------------------------------------------------- + + +def denoise_cloud( + points: np.ndarray, + denoise_cfg: dict[str, Any], +) -> DenoisedCloud: + pts, log = run_denoise(points, denoise_cfg.get("steps", [])) + return DenoisedCloud(points=pts, log=log) + + +# --------------------------------------------------------------------------- +# Stage 4 — register (4-hole Kabsch) +# --------------------------------------------------------------------------- + + +def register_cloud( + points: np.ndarray, + *, + target_holes: HoleSet, + detection_cfg: dict[str, Any], + cad_cfg: dict[str, Any], + registration_cfg: dict[str, Any], + panel_normal_seed: np.ndarray | None = None, +) -> RegisteredCloud: + """Detect 4 holes in ``points`` and Kabsch-align them to ``target_holes``. + + ``target_holes`` is the GT (CAD-frame) hole set. The output cloud is + in the CAD frame. The 4-hole pairing is canonical-quadrant-ordered + on both sides, so no greedy NN matching is needed. + """ + detection = detect_four_holes( + points, + detection_cfg=detection_cfg, + cad_cfg=cad_cfg, + panel_normal_seed=panel_normal_seed, + ) + + src = detection.holes_3d.locating_centers() + tgt = target_holes.locating_centers() + kabsch = register_four_holes( + src, + tgt, + refuse_above_mm=registration_cfg.get("refuse_if_residual_above_mm"), + ) + + R = kabsch.matrix[:3, :3] + t = kabsch.matrix[:3, 3] + aligned = points @ R.T + t + + detected_in_cad = HoleSet( + locating=tuple( # type: ignore[arg-type] + Hole( + center=tuple((R @ np.asarray(h.center) + t).tolist()), + radius_mm=h.radius_mm, + fit_residual_rms_mm=h.fit_residual_rms_mm, + role=h.role, + ) + for h in detection.holes_3d.locating + ), + central=( + None + if detection.holes_3d.central is None + else Hole( + center=tuple( + (R @ np.asarray(detection.holes_3d.central.center) + t).tolist() + ), + radius_mm=detection.holes_3d.central.radius_mm, + fit_residual_rms_mm=detection.holes_3d.central.fit_residual_rms_mm, + role="central", + ) + ), + ) + + return RegisteredCloud( + points=aligned, + transform_4x4=kabsch.matrix, + rmse_mm=kabsch.rmse_mm, + max_residual_mm=kabsch.max_residual_mm, + detected_holes_in_cad_frame=detected_in_cad, + ) + + +# --------------------------------------------------------------------------- +# End-to-end convenience: STP → PreparedCloud +# --------------------------------------------------------------------------- + + +def prepare_cloud_from_cad( + stp_path: str | Path, + config: str | Path | dict[str, Any] | None = None, + profile: str = "auto", +) -> PreparedCloud: + """Run all four stages and return a :class:`PreparedCloud`. + + The returned cloud is denoised and aligned into the CAD frame; pass + ``prepared.points`` to + :func:`coupler_panel.api.compute_coupler_measurement`. + """ + cfg = load_config(config, profile=profile) + cfg = resolve_auto(cfg) + + parsed = parse_cad(stp_path, cfg["cad"]) + synth = synthesize_cloud( + parsed, + sampling_cfg=cfg["sampling"], + noise_cfg=cfg.get("noise", {}), + transform_cfg=cfg.get("transform", {}), + scan_cfg=cfg.get("scan_simulator", {}), + deflection_cfg=cfg.get("panel_deflection", {}), + ) + denoised = denoise_cloud(synth.transformed, cfg.get("denoise", {})) + # Important: register against the *deflected* GT — the holes have moved + # with the panel, so Kabsch only undoes the scan-pose jitter, NOT the + # injected deflection. The plane fit downstream then sees the tilted + # panel and can recover theta_right / theta_ref. + registered = register_cloud( + denoised.points, + target_holes=synth.gt_holes_after_deflection, + detection_cfg=cfg["detection"], + cad_cfg=cfg["cad"], + registration_cfg=cfg["registration"], + ) + + return PreparedCloud( + points=registered.points, + gt=parsed.gt, + gt_transform_4x4=synth.gt_transform_4x4, + registration_transform_4x4=registered.transform_4x4, + registration_rmse_mm=registered.rmse_mm, + detected_holes_in_cad_frame=registered.detected_holes_in_cad_frame, + intermediates={ + "config": cfg, + "sampled": synth.sampled, + "noisy": synth.noisy, + "transformed": synth.transformed, + "denoised": denoised.points, + "denoise_log": denoised.log, + }, + ) + + +def prepare_cloud_from_scan( + points: np.ndarray, + *, + config: str | Path | dict[str, Any] | None = None, + profile: str = "auto", + target_holes: HoleSet | None = None, +) -> PreparedCloud: + """Same as :func:`prepare_cloud_from_cad` but starting from a real scan. + + Skips parsing / synthesis. If ``target_holes`` is given, runs the + 4-hole Kabsch registration; otherwise assumes the cloud is already + in the CAD frame. + """ + cfg = load_config(config, profile=profile) + cfg = resolve_auto(cfg) + + denoised = denoise_cloud(np.asarray(points, dtype=np.float64), cfg.get("denoise", {})) + + if target_holes is None: + return PreparedCloud( + points=denoised.points, + gt=None, + gt_transform_4x4=None, + registration_transform_4x4=None, + registration_rmse_mm=None, + detected_holes_in_cad_frame=None, + intermediates={"config": cfg, "denoise_log": denoised.log}, + ) + + registered = register_cloud( + denoised.points, + target_holes=target_holes, + detection_cfg=cfg["detection"], + cad_cfg=cfg["cad"], + registration_cfg=cfg["registration"], + ) + return PreparedCloud( + points=registered.points, + gt=None, + gt_transform_4x4=None, + registration_transform_4x4=registered.transform_4x4, + registration_rmse_mm=registered.rmse_mm, + detected_holes_in_cad_frame=registered.detected_holes_in_cad_frame, + intermediates={ + "config": cfg, + "denoised": denoised.points, + "denoise_log": denoised.log, + }, + ) + + +__all__ = [ + "CADParsed", + "SynthesizedCloud", + "DenoisedCloud", + "RegisteredCloud", + "PreparedCloud", + "parse_cad", + "synthesize_cloud", + "denoise_cloud", + "register_cloud", + "prepare_cloud_from_cad", + "prepare_cloud_from_scan", +] diff --git a/alg/delivery_v1.1/coupler_panel_release/coupler_panel/denoise/__init__.py b/alg/delivery_v1.1/coupler_panel_release/coupler_panel/denoise/__init__.py new file mode 100644 index 0000000..7c726ff --- /dev/null +++ b/alg/delivery_v1.1/coupler_panel_release/coupler_panel/denoise/__init__.py @@ -0,0 +1,29 @@ +"""Composable point cloud denoising pipeline. + +Each step is a pure function ``points -> points``. Steps are configured +via YAML and chained by :func:`run_pipeline`. + +Available steps (matching ``denoise.steps[].type``): + +- ``passthrough`` — keep points inside an axis-aligned bounding box +- ``voxel`` — voxel-grid downsample +- ``statistical`` — statistical outlier removal (SOR) +- ``radius`` — radius outlier removal (ROR) +- ``smoothing`` — MLS smoothing (only when explicitly enabled) +""" + +from .passthrough import passthrough_filter +from .voxel import voxel_downsample +from .statistical import statistical_outlier_removal +from .radius import radius_outlier_removal +from .smoothing import mls_smoothing +from .pipeline import run_pipeline + +__all__ = [ + "passthrough_filter", + "voxel_downsample", + "statistical_outlier_removal", + "radius_outlier_removal", + "mls_smoothing", + "run_pipeline", +] diff --git a/alg/delivery_v1.1/coupler_panel_release/coupler_panel/denoise/passthrough.py b/alg/delivery_v1.1/coupler_panel_release/coupler_panel/denoise/passthrough.py new file mode 100644 index 0000000..929b109 --- /dev/null +++ b/alg/delivery_v1.1/coupler_panel_release/coupler_panel/denoise/passthrough.py @@ -0,0 +1,41 @@ +"""Passthrough (axis-aligned bounding-box) filter.""" + +from __future__ import annotations + +import numpy as np + + +def passthrough_filter( + points: np.ndarray, + *, + bbox: tuple[tuple[float, float], tuple[float, float], tuple[float, float]] | None = None, + bbox_padding_mm: float | None = None, +) -> np.ndarray: + """Keep only points inside ``bbox``. + + ``bbox`` is ``((xmin, xmax), (ymin, ymax), (zmin, zmax))``. + + If ``bbox`` is ``None`` and ``bbox_padding_mm`` is given, the bbox is + derived from the cloud itself, padded by ``bbox_padding_mm`` on each + side — which is a useful no-op default that nonetheless drops far-out + NaNs / spurious points. + """ + pts = np.asarray(points, dtype=np.float64) + if bbox is None: + if bbox_padding_mm is None: + return pts.copy() + pad = float(bbox_padding_mm) + lo = pts.min(axis=0) - pad + hi = pts.max(axis=0) + pad + bbox = ((lo[0], hi[0]), (lo[1], hi[1]), (lo[2], hi[2])) + + (xlo, xhi), (ylo, yhi), (zlo, zhi) = bbox + mask = ( + (pts[:, 0] >= xlo) & (pts[:, 0] <= xhi) + & (pts[:, 1] >= ylo) & (pts[:, 1] <= yhi) + & (pts[:, 2] >= zlo) & (pts[:, 2] <= zhi) + ) + return pts[mask] + + +__all__ = ["passthrough_filter"] diff --git a/alg/delivery_v1.1/coupler_panel_release/coupler_panel/denoise/pipeline.py b/alg/delivery_v1.1/coupler_panel_release/coupler_panel/denoise/pipeline.py new file mode 100644 index 0000000..66d5693 --- /dev/null +++ b/alg/delivery_v1.1/coupler_panel_release/coupler_panel/denoise/pipeline.py @@ -0,0 +1,71 @@ +"""Compose denoising steps from a YAML-style step list. + +Example config:: + + denoise: + steps: + - type: passthrough + bbox_padding_mm: 50.0 + - type: voxel + voxel_mm: 0.5 + - type: statistical + nb_neighbors: 30 + std_ratio: 2.0 +""" + +from __future__ import annotations + +from typing import Any, Callable + +import numpy as np + +from .passthrough import passthrough_filter +from .radius import radius_outlier_removal +from .smoothing import mls_smoothing +from .statistical import statistical_outlier_removal +from .voxel import voxel_downsample + + +_STEP_FNS: dict[str, Callable[..., np.ndarray]] = { + "passthrough": passthrough_filter, + "voxel": voxel_downsample, + "statistical": statistical_outlier_removal, + "radius": radius_outlier_removal, + "smoothing": mls_smoothing, +} + + +def run_pipeline( + points: np.ndarray, + steps: list[dict[str, Any]] | None, + *, + on_step: Callable[[str, np.ndarray], None] | None = None, +) -> tuple[np.ndarray, list[dict[str, Any]]]: + """Apply each step in order and return the final cloud + a per-step log. + + Each log entry is:: + + {"type": ..., "params": {...}, "n_in": int, "n_out": int} + """ + pts = np.asarray(points, dtype=np.float64) + log: list[dict[str, Any]] = [] + if not steps: + return pts.copy(), log + + for raw in steps: + cfg = dict(raw) + kind = cfg.pop("type") + if kind not in _STEP_FNS: + raise ValueError(f"unknown denoise step '{kind}'; valid: {sorted(_STEP_FNS)}") + fn = _STEP_FNS[kind] + n_in = pts.shape[0] + pts = fn(pts, **cfg) + log.append( + {"type": kind, "params": cfg, "n_in": n_in, "n_out": int(pts.shape[0])} + ) + if on_step is not None: + on_step(kind, pts) + return pts, log + + +__all__ = ["run_pipeline"] diff --git a/alg/delivery_v1.1/coupler_panel_release/coupler_panel/denoise/radius.py b/alg/delivery_v1.1/coupler_panel_release/coupler_panel/denoise/radius.py new file mode 100644 index 0000000..6b096cd --- /dev/null +++ b/alg/delivery_v1.1/coupler_panel_release/coupler_panel/denoise/radius.py @@ -0,0 +1,38 @@ +"""Radius outlier removal (ROR). + +A point is kept iff at least ``nb_points`` other points lie within +``radius_mm``. Useful for getting rid of spray noise in scanner data. +""" + +from __future__ import annotations + +import numpy as np + + +def radius_outlier_removal( + points: np.ndarray, + *, + nb_points: int = 8, + radius_mm: float = 2.0, +) -> np.ndarray: + pts = np.asarray(points, dtype=np.float64) + n = pts.shape[0] + if n == 0: + return pts.copy() + + try: + from scipy.spatial import cKDTree + + tree = cKDTree(pts) + counts = tree.query_ball_point(pts, r=float(radius_mm), return_length=True) + # `counts` includes the point itself; threshold accordingly. + keep = counts >= int(nb_points) + 1 + except ImportError: + diff = pts[:, None, :] - pts[None, :, :] + d = np.sqrt(np.sum(diff * diff, axis=2)) + keep = (d <= float(radius_mm)).sum(axis=1) >= int(nb_points) + 1 + + return pts[keep] + + +__all__ = ["radius_outlier_removal"] diff --git a/alg/delivery_v1.1/coupler_panel_release/coupler_panel/denoise/smoothing.py b/alg/delivery_v1.1/coupler_panel_release/coupler_panel/denoise/smoothing.py new file mode 100644 index 0000000..6c89ad0 --- /dev/null +++ b/alg/delivery_v1.1/coupler_panel_release/coupler_panel/denoise/smoothing.py @@ -0,0 +1,84 @@ +"""Optional moving-least-squares (MLS) smoothing. + +For each point, fit a local polynomial in the neighbour set's tangent +plane and project the point onto it. Implementation here is a degree-2 +local fit using only numpy + scipy.cKDTree. For most coupler-panel work +this stage is *off* by default — leave it disabled unless you actually +need smoothing in addition to SOR/ROR. +""" + +from __future__ import annotations + +import numpy as np + + +def mls_smoothing( + points: np.ndarray, + *, + search_radius_mm: float = 2.0, + polynomial_order: int = 2, + min_neighbors: int = 8, +) -> np.ndarray: + pts = np.asarray(points, dtype=np.float64) + n = pts.shape[0] + if n == 0: + return pts.copy() + + try: + from scipy.spatial import cKDTree + except ImportError: + # Without a tree this is hopelessly slow; skip safely. + return pts.copy() + + tree = cKDTree(pts) + out = pts.copy() + + for i in range(n): + idx = tree.query_ball_point(pts[i], r=float(search_radius_mm)) + if len(idx) < min_neighbors: + continue + + nbh = pts[idx] + center = nbh.mean(axis=0) + # Local frame from PCA on the neighbourhood. + Q = nbh - center + cov = (Q.T @ Q) / max(len(idx) - 1, 1) + w, V = np.linalg.eigh(cov) + # Smallest eigenvector ≈ surface normal; the other two span the tangent. + normal = V[:, 0] + tangents = V[:, 1:] + + local_xy = Q @ tangents # (k, 2) + local_z = Q @ normal # (k,) + + if polynomial_order == 1: + A = np.column_stack([np.ones(len(idx)), local_xy]) + else: + x, y = local_xy[:, 0], local_xy[:, 1] + A = np.column_stack([np.ones(len(idx)), x, y, x * x, x * y, y * y]) + + try: + coef, *_ = np.linalg.lstsq(A, local_z, rcond=None) + except np.linalg.LinAlgError: + continue + + # Project pts[i] onto the surface in local frame. + rel = pts[i] - center + xy_i = np.array([rel @ tangents[:, 0], rel @ tangents[:, 1]]) + if polynomial_order == 1: + z_i = coef[0] + coef[1] * xy_i[0] + coef[2] * xy_i[1] + else: + z_i = ( + coef[0] + + coef[1] * xy_i[0] + + coef[2] * xy_i[1] + + coef[3] * xy_i[0] ** 2 + + coef[4] * xy_i[0] * xy_i[1] + + coef[5] * xy_i[1] ** 2 + ) + out[i] = center + tangents[:, 0] * xy_i[0] + tangents[:, 1] * xy_i[1] + normal * z_i + + return out + + +__all__ = ["mls_smoothing"] diff --git a/alg/delivery_v1.1/coupler_panel_release/coupler_panel/denoise/statistical.py b/alg/delivery_v1.1/coupler_panel_release/coupler_panel/denoise/statistical.py new file mode 100644 index 0000000..180d30a --- /dev/null +++ b/alg/delivery_v1.1/coupler_panel_release/coupler_panel/denoise/statistical.py @@ -0,0 +1,44 @@ +"""Statistical outlier removal (SOR). + +For each point, compute the mean distance to its ``k`` nearest neighbours. +A point whose mean distance exceeds the global mean by ``std_ratio`` × +the global std is dropped. +""" + +from __future__ import annotations + +import numpy as np + + +def statistical_outlier_removal( + points: np.ndarray, + *, + nb_neighbors: int = 30, + std_ratio: float = 2.0, +) -> np.ndarray: + pts = np.asarray(points, dtype=np.float64) + n = pts.shape[0] + if n <= nb_neighbors + 1: + return pts.copy() + + try: + from scipy.spatial import cKDTree + + tree = cKDTree(pts) + dist, _ = tree.query(pts, k=nb_neighbors + 1) + # First column is the point itself (distance 0). + mean_d = dist[:, 1:].mean(axis=1) + except ImportError: + # Plain numpy fallback (O(N²); fine for N up to a few thousand). + diff = pts[:, None, :] - pts[None, :, :] + d = np.sqrt(np.sum(diff * diff, axis=2)) + d.sort(axis=1) + mean_d = d[:, 1 : nb_neighbors + 1].mean(axis=1) + + mu = mean_d.mean() + sigma = mean_d.std() + keep = mean_d <= mu + float(std_ratio) * sigma + return pts[keep] + + +__all__ = ["statistical_outlier_removal"] diff --git a/alg/delivery_v1.1/coupler_panel_release/coupler_panel/denoise/voxel.py b/alg/delivery_v1.1/coupler_panel_release/coupler_panel/denoise/voxel.py new file mode 100644 index 0000000..b35cc83 --- /dev/null +++ b/alg/delivery_v1.1/coupler_panel_release/coupler_panel/denoise/voxel.py @@ -0,0 +1,37 @@ +"""Voxel-grid downsampling.""" + +from __future__ import annotations + +import numpy as np + + +def voxel_downsample(points: np.ndarray, *, voxel_mm: float) -> np.ndarray: + """Replace each occupied voxel with the centroid of its points.""" + if voxel_mm <= 0: + raise ValueError("voxel_mm must be > 0") + pts = np.asarray(points, dtype=np.float64) + if pts.shape[0] == 0: + return pts.copy() + + keys = np.floor(pts / float(voxel_mm)).astype(np.int64) + # Compose a single 1-D key for fast unique() grouping. + kmin = keys.min(axis=0) + keys = keys - kmin + span = keys.max(axis=0) + 1 + flat = keys[:, 0] + span[0] * (keys[:, 1] + span[1] * keys[:, 2]) + + order = np.argsort(flat, kind="stable") + flat_sorted = flat[order] + pts_sorted = pts[order] + + # Group by run-length on the sorted flat keys. + boundaries = np.concatenate(([0], np.where(np.diff(flat_sorted) != 0)[0] + 1, [pts.shape[0]])) + n_groups = len(boundaries) - 1 + out = np.empty((n_groups, 3), dtype=np.float64) + for i in range(n_groups): + s, e = boundaries[i], boundaries[i + 1] + out[i] = pts_sorted[s:e].mean(axis=0) + return out + + +__all__ = ["voxel_downsample"] diff --git a/alg/delivery_v1.1/coupler_panel_release/coupler_panel/detection/__init__.py b/alg/delivery_v1.1/coupler_panel_release/coupler_panel/detection/__init__.py new file mode 100644 index 0000000..94f98bd --- /dev/null +++ b/alg/delivery_v1.1/coupler_panel_release/coupler_panel/detection/__init__.py @@ -0,0 +1,36 @@ +"""Four-hole detection on a panel point cloud. + +Pipeline (see ``detect_four_holes``):: + + 1. plane_local.project_to_panel(...) # cloud -> 2D coords + 2. occupancy_grid.build(...) # 2D grid + connected components + 3. occupancy_grid.find_holes(...) # holes (cavities) inside main panel CC + 4. circle_fit.fit_circle(...) # boundary point fit + 5. four_hole_selector.select(...) # keep the 4 best, ordered +""" + +from .plane_local import LocalPanelFrame, project_to_panel, lift_back +from .circle_fit import fit_circle, fit_circle_lsq +from .occupancy_grid import ( + OccupancyGrid, + HoleCandidate, + build_grid, + find_panel_holes, +) +from .four_hole_selector import select_four_holes +from .detect_holes import detect_four_holes, FourHoleDetection + +__all__ = [ + "LocalPanelFrame", + "project_to_panel", + "lift_back", + "fit_circle", + "fit_circle_lsq", + "OccupancyGrid", + "HoleCandidate", + "build_grid", + "find_panel_holes", + "select_four_holes", + "detect_four_holes", + "FourHoleDetection", +] diff --git a/alg/delivery_v1.1/coupler_panel_release/coupler_panel/detection/circle_fit.py b/alg/delivery_v1.1/coupler_panel_release/coupler_panel/detection/circle_fit.py new file mode 100644 index 0000000..caf0f4d --- /dev/null +++ b/alg/delivery_v1.1/coupler_panel_release/coupler_panel/detection/circle_fit.py @@ -0,0 +1,106 @@ +"""2D circle fitting. + +Two routines: + +- :func:`fit_circle_algebraic` — Pratt-style algebraic fit (closed-form). + Fast, biased, but a great initial guess. +- :func:`fit_circle_lsq` — Levenberg-Marquardt geometric fit. + Unbiased, requires an initial guess. +- :func:`fit_circle` — algebraic → geometric, returns the result + along with the per-point residuals. +""" + +from __future__ import annotations + +from dataclasses import dataclass + +import numpy as np + + +@dataclass(frozen=True) +class CircleFit: + cx: float + cy: float + radius: float + residual_rms_mm: float + residual_max_mm: float + n_points: int + + +def fit_circle_algebraic(uv: np.ndarray) -> tuple[float, float, float]: + """Return (cx, cy, radius) by Pratt's algebraic fit.""" + if uv.shape[0] < 3: + raise ValueError("need ≥ 3 points to fit a circle") + x = uv[:, 0] + y = uv[:, 1] + A = np.column_stack([2.0 * x, 2.0 * y, np.ones_like(x)]) + b = x ** 2 + y ** 2 + sol, *_ = np.linalg.lstsq(A, b, rcond=None) + cx, cy, c = sol + r2 = c + cx ** 2 + cy ** 2 + if r2 <= 0: + raise ValueError("algebraic fit produced non-positive squared radius") + return float(cx), float(cy), float(np.sqrt(r2)) + + +def fit_circle_lsq( + uv: np.ndarray, + *, + init: tuple[float, float, float] | None = None, + max_iter: int = 30, + tol: float = 1e-8, +) -> tuple[float, float, float]: + """Geometric (orthogonal-residual) least-squares circle fit. + + Uses Gauss–Newton on f_i(p) = sqrt((x_i-cx)² + (y_i-cy)²) - r. + """ + if init is None: + cx, cy, r = fit_circle_algebraic(uv) + else: + cx, cy, r = init + + x = uv[:, 0] + y = uv[:, 1] + for _ in range(max_iter): + dx = x - cx + dy = y - cy + d = np.sqrt(dx * dx + dy * dy) + d_safe = np.where(d > 1e-12, d, 1e-12) + # Residual. + f = d - r + # Jacobian wrt (cx, cy, r). + J = np.column_stack([-dx / d_safe, -dy / d_safe, -np.ones_like(d)]) + try: + delta, *_ = np.linalg.lstsq(J, -f, rcond=None) + except np.linalg.LinAlgError: + break + cx += delta[0] + cy += delta[1] + r += delta[2] + if np.max(np.abs(delta)) < tol: + break + return float(cx), float(cy), float(r) + + +def fit_circle(uv: np.ndarray, *, refine: bool = True) -> CircleFit: + cx, cy, r = fit_circle_algebraic(uv) + if refine: + cx, cy, r = fit_circle_lsq(uv, init=(cx, cy, r)) + d = np.sqrt((uv[:, 0] - cx) ** 2 + (uv[:, 1] - cy) ** 2) + res = d - r + return CircleFit( + cx=cx, + cy=cy, + radius=r, + residual_rms_mm=float(np.sqrt(np.mean(res ** 2))), + residual_max_mm=float(np.max(np.abs(res))), + n_points=int(uv.shape[0]), + ) + + +__all__ = [ + "CircleFit", + "fit_circle", + "fit_circle_algebraic", + "fit_circle_lsq", +] diff --git a/alg/delivery_v1.1/coupler_panel_release/coupler_panel/detection/detect_holes.py b/alg/delivery_v1.1/coupler_panel_release/coupler_panel/detection/detect_holes.py new file mode 100644 index 0000000..61f0392 --- /dev/null +++ b/alg/delivery_v1.1/coupler_panel_release/coupler_panel/detection/detect_holes.py @@ -0,0 +1,154 @@ +"""High-level orchestration: cloud → 4 ordered hole centres in 3D.""" + +from __future__ import annotations + +from dataclasses import dataclass +from typing import Any + +import numpy as np + +from ..exceptions import HoleDetectionError +from ..structures import Hole, HoleSet +from .circle_fit import fit_circle +from .four_hole_selector import CircleCandidate, select_four_holes +from .occupancy_grid import build_grid, find_panel_holes +from .plane_local import LocalPanelFrame, project_to_panel + + +@dataclass +class FourHoleDetection: + holes_3d: HoleSet + frame: LocalPanelFrame + candidates: list[CircleCandidate] + diagnostics: dict[str, Any] + + +def detect_four_holes( + points: np.ndarray, + *, + detection_cfg: dict[str, Any], + cad_cfg: dict[str, Any], + panel_normal_seed: np.ndarray | None = None, +) -> FourHoleDetection: + """Detect the four locating-hole centres in 3D from a panel point cloud.""" + pts = np.asarray(points, dtype=np.float64) + if pts.shape[0] < 100: + raise HoleDetectionError(f"too few points for hole detection: {pts.shape[0]}") + + # 1. Project onto local panel plane. + seed_mode = detection_cfg.get("panel_normal_seed", "from_cad") + if panel_normal_seed is not None and seed_mode != "pca": + seed = np.asarray(panel_normal_seed, dtype=np.float64) + else: + seed = "pca" + frame, uv = project_to_panel(pts, normal_seed=seed) + + # 2. Build 2D occupancy grid. + pixel_mm = float(detection_cfg["grid_pixel_mm"]) + closing_iters = int(detection_cfg.get("closing_iters", 0)) + fill_bg_below = float(detection_cfg.get("fill_bg_below_mm2", 0.0)) + grid = build_grid( + uv, + pixel_mm=pixel_mm, + closing_iters=closing_iters, + fill_bg_below_mm2=fill_bg_below, + ) + + # 3. Find panel + hole cavities. + panel_mask, holes = find_panel_holes( + grid, + panel_min_area_ratio=float(detection_cfg.get("panel_min_area_ratio", 0.4)), + hole_min_area_mm2=float(detection_cfg.get("hole_min_area_mm2", 30.0)), + hole_max_area_mm2=float(detection_cfg.get("hole_max_area_mm2", 800.0)), + point_uv=uv, + ) + if len(holes) < 4: + raise HoleDetectionError( + f"only {len(holes)} hole-shaped cavities found in occupancy grid; " + "consider lowering grid_pixel_mm or relaxing hole_*_area_mm2" + ) + + # 4. Fit a circle to each cavity boundary. + candidates: list[CircleCandidate] = [] + for h in holes: + try: + cf = fit_circle(h.boundary_uv) + except Exception: + continue + candidates.append( + CircleCandidate( + fit=cf, + centroid_uv=h.centroid_uv, + area_mm2=h.area_mm2, + n_boundary_points=int(h.boundary_uv.shape[0]), + ) + ) + if len(candidates) < 4: + raise HoleDetectionError( + f"only {len(candidates)} circles fitted (need ≥ 4)" + ) + + # 5. Pick the 4 most rectangle-like, equal-radius circles. + # Detection can override the CAD-side radius range (rasterisation + # biases the fitted radius outwards). + r_lo, r_hi = detection_cfg.get( + "locating_hole_radius_mm", + cad_cfg["locating_hole_radius_mm"], + ) + ordered, sel_idx, diag = select_four_holes( + candidates, + target_radius_lo_mm=float(r_lo), + target_radius_hi_mm=float(r_hi), + rectangle_aspect_tol=float( + detection_cfg.get( + "rectangle_aspect_tol", + cad_cfg.get("rectangle_aspect_tol", 0.20), + ) + ), + ) + + # 6. Lift the four hole centres back to 3D (signed_distance = 0). + centers_uv = np.array([(c.fit.cx, c.fit.cy) for c in ordered]) + centers_3d = frame.from_2d(centers_uv, signed_distance=0.0) + + # 7. Identify a central hole (radius ≥ central_hole_radius_min_mm). + central_hole = None + central_min = float(cad_cfg.get("central_hole_radius_min_mm", 20.0)) + for cand in candidates: + if cand.fit.radius >= central_min: + uv_c = np.array([[cand.fit.cx, cand.fit.cy]]) + xyz_c = frame.from_2d(uv_c, signed_distance=0.0)[0] + central_hole = Hole( + center=tuple(xyz_c.tolist()), + radius_mm=float(cand.fit.radius), + fit_residual_rms_mm=float(cand.fit.residual_rms_mm), + role="central", + ) + break + + holes_3d = HoleSet( + locating=tuple( # type: ignore[arg-type] + Hole( + center=tuple(centers_3d[i].tolist()), + radius_mm=float(c.fit.radius), + fit_residual_rms_mm=float(c.fit.residual_rms_mm), + role="locating", + ) + for i, c in enumerate(ordered) + ), + central=central_hole, + ) + + return FourHoleDetection( + holes_3d=holes_3d, + frame=frame, + candidates=candidates, + diagnostics={ + "n_cavities": len(holes), + "n_circle_fits": len(candidates), + "selection": diag, + }, + ) + + +__all__ = ["FourHoleDetection", "detect_four_holes"] diff --git a/alg/delivery_v1.1/coupler_panel_release/coupler_panel/detection/four_hole_selector.py b/alg/delivery_v1.1/coupler_panel_release/coupler_panel/detection/four_hole_selector.py new file mode 100644 index 0000000..5397b89 --- /dev/null +++ b/alg/delivery_v1.1/coupler_panel_release/coupler_panel/detection/four_hole_selector.py @@ -0,0 +1,145 @@ +"""Pick four locating holes out of a list of fitted-circle candidates. + +Given more than four candidates, choose the 4-subset that: + +1. Has the smallest standard deviation of radius (the four locating + holes are equal-radius by spec). +2. Forms a near-rectangle (small `rectangle_score`, see CAD GT module). +3. Each radius is within ``[r_lo, r_hi]``. + +We score the cartesian product of "best radii" and "best rectangle" +jointly, weighted. +""" + +from __future__ import annotations + +from dataclasses import dataclass +from itertools import combinations + +import numpy as np + +from .circle_fit import CircleFit + + +@dataclass +class CircleCandidate: + """A candidate locating hole, in the panel's local 2D frame.""" + + fit: CircleFit + centroid_uv: tuple[float, float] # from occupancy grid (used as initial guess) + area_mm2: float + n_boundary_points: int + + +def _rectangle_score(centers_uv: np.ndarray) -> float: + """0 = perfect rectangle. Larger = worse.""" + if centers_uv.shape[0] != 4: + return float("inf") + d = sorted( + float(np.linalg.norm(centers_uv[i] - centers_uv[j])) + for i, j in combinations(range(4), 2) + ) + if d[0] < 1e-6: + return 1e6 + s1, s2 = d[0], d[1] + l1, l2 = d[2], d[3] + g1, g2 = d[4], d[5] + return ( + abs(s1 - s2) / max(s1, s2) + + abs(l1 - l2) / max(l1, l2) + + abs(g1 - g2) / max(g1, g2) + ) + + +def _radius_score(radii: np.ndarray, target_lo: float, target_hi: float) -> float: + if np.any(radii < target_lo) or np.any(radii > target_hi): + return float("inf") + if radii.mean() <= 0: + return float("inf") + return float(radii.std() / max(radii.mean(), 1e-6)) + + +def _order_quadrants(centers_uv: np.ndarray, candidates) -> tuple[list, list[int]]: + """Order 4 candidates as (-u/-v, +u/-v, +u/+v, -u/+v) in the local frame.""" + cu = centers_uv[:, 0].mean() + cv = centers_uv[:, 1].mean() + quadrant: dict[tuple[bool, bool], int] = {} + for idx, p in enumerate(centers_uv): + quadrant[(p[0] >= cu, p[1] >= cv)] = idx + order = [(False, False), (True, False), (True, True), (False, True)] + if not all(q in quadrant for q in order): + # Degenerate, fall back to angular sort. + ang = np.arctan2(centers_uv[:, 1] - cv, centers_uv[:, 0] - cu) + idxs = np.argsort(ang).tolist() + return [candidates[i] for i in idxs], idxs + idxs = [quadrant[q] for q in order] + return [candidates[i] for i in idxs], idxs + + +def select_four_holes( + candidates: list[CircleCandidate], + *, + target_radius_lo_mm: float, + target_radius_hi_mm: float, + rectangle_aspect_tol: float = 0.20, + radius_weight: float = 1.0, + rectangle_weight: float = 1.0, +) -> tuple[list[CircleCandidate], list[int], dict]: + """Pick the best 4-subset; return (ordered subset, indices into input, diag). + + Raises ``RuntimeError`` if no valid 4-subset exists. + """ + n = len(candidates) + if n < 4: + raise RuntimeError(f"need ≥ 4 candidates, got {n}") + + best_idx: tuple[int, ...] | None = None + best_score = float("inf") + best_diag: dict = {} + + for combo in combinations(range(n), 4): + radii = np.array([candidates[i].fit.radius for i in combo]) + rs = _radius_score(radii, target_radius_lo_mm, target_radius_hi_mm) + if not np.isfinite(rs): + continue + centers = np.array([candidates[i].fit.cx for i in combo]), np.array( + [candidates[i].fit.cy for i in combo] + ) + centers_uv = np.column_stack(centers) + rect = _rectangle_score(centers_uv) + score = radius_weight * rs + rectangle_weight * rect + if score < best_score: + best_score = score + best_idx = combo + best_diag = { + "radius_score": rs, + "rectangle_score": rect, + "score": score, + "radii_mean": float(radii.mean()), + "radii_std": float(radii.std()), + } + + if best_idx is None: + raise RuntimeError( + "no valid 4-subset (radius constraints excluded all combinations); " + f"target_radius=[{target_radius_lo_mm}, {target_radius_hi_mm}], " + f"got {sorted(round(c.fit.radius, 3) for c in candidates)}" + ) + + subset = [candidates[i] for i in best_idx] + centers_uv = np.array([(c.fit.cx, c.fit.cy) for c in subset]) + ordered, perm = _order_quadrants(centers_uv, subset) + final_idx = [best_idx[p] for p in perm] + best_diag["selected_indices"] = list(final_idx) + best_diag["selected_radii"] = [c.fit.radius for c in ordered] + + if best_diag["rectangle_score"] > rectangle_aspect_tol: + best_diag.setdefault("warnings", []).append("rectangle_score_above_tol") + + return ordered, final_idx, best_diag + + +__all__ = [ + "CircleCandidate", + "select_four_holes", +] diff --git a/alg/delivery_v1.1/coupler_panel_release/coupler_panel/detection/occupancy_grid.py b/alg/delivery_v1.1/coupler_panel_release/coupler_panel/detection/occupancy_grid.py new file mode 100644 index 0000000..a158373 --- /dev/null +++ b/alg/delivery_v1.1/coupler_panel_release/coupler_panel/detection/occupancy_grid.py @@ -0,0 +1,350 @@ +"""2D occupancy grid + connected-component analysis on panel-projected points. + +Steps used by :func:`detect_four_holes`: + +1. ``build_grid`` — rasterise (u, v) points to a binary grid. +2. ``find_panel_holes`` — find the largest connected component (= panel), + then enumerate its internal cavities (= holes). Each cavity gets its + boundary points (in the original 3D order) so we can fit a circle. + +Implementation is pure numpy + a small flood-fill — no scikit-image +dependency. +""" + +from __future__ import annotations + +from dataclasses import dataclass + +import numpy as np + + +@dataclass(frozen=True) +class OccupancyGrid: + occupied: np.ndarray # (H, W) bool + pixel_mm: float + u_min: float + v_min: float + point_indices: np.ndarray # (H, W, K_max)? No — see point_pixel_idx instead + point_pixel_idx: np.ndarray # (N, 2) int (row, col) + + +@dataclass(frozen=True) +class HoleCandidate: + """One internal cavity of the panel CC.""" + + boundary_uv: np.ndarray # (M, 2) float — boundary pixel centres + boundary_point_idx: np.ndarray # (M,) int — indices into the original cloud, may be empty + centroid_uv: tuple[float, float] + area_mm2: float + bbox_uv: tuple[float, float, float, float] # (umin, vmin, umax, vmax) + + +# --------------------------------------------------------------------------- + + +def _binary_dilate(grid: np.ndarray, iters: int = 1) -> np.ndarray: + """4-connected dilation, ``iters`` times. Pure numpy.""" + if iters <= 0: + return grid.copy() + out = grid.copy() + for _ in range(iters): + nxt = out.copy() + nxt[1:, :] |= out[:-1, :] + nxt[:-1, :] |= out[1:, :] + nxt[:, 1:] |= out[:, :-1] + nxt[:, :-1] |= out[:, 1:] + out = nxt + return out + + +def _binary_erode(grid: np.ndarray, iters: int = 1) -> np.ndarray: + if iters <= 0: + return grid.copy() + inv = ~grid + inv = _binary_dilate(inv, iters) + return ~inv + + +def _binary_close(grid: np.ndarray, iters: int = 1) -> np.ndarray: + """Close = dilate then erode. Fills small gaps without growing the mask.""" + return _binary_erode(_binary_dilate(grid, iters), iters) + + +def _fill_small_bg_components(grid: np.ndarray, min_keep_area_px: int) -> np.ndarray: + """Set every 0-component smaller than ``min_keep_area_px`` to 1. + + Equivalent to "area opening of the background" — fills sparse-coverage + gaps without shrinking the real cavities the caller wants to detect. + Border-touching background components are always kept (they're outside + the panel, not real holes). + """ + H, W = grid.shape + bg_labels = _flood_fill_label(~grid) + if bg_labels.max() == 0: + return grid.copy() + + # Identify border components. + border = set() + for j in range(W): + if bg_labels[0, j] != 0: + border.add(int(bg_labels[0, j])) + if bg_labels[H - 1, j] != 0: + border.add(int(bg_labels[H - 1, j])) + for i in range(H): + if bg_labels[i, 0] != 0: + border.add(int(bg_labels[i, 0])) + if bg_labels[i, W - 1] != 0: + border.add(int(bg_labels[i, W - 1])) + + counts = np.bincount(bg_labels.ravel()) + out = grid.copy() + fill_labels = np.zeros(len(counts) + 1, dtype=bool) + for k in range(1, len(counts)): + if k in border: + continue + if counts[k] < min_keep_area_px: + fill_labels[k] = True + fill_mask = fill_labels[bg_labels] + out[fill_mask] = True + return out + + +def build_grid( + uv: np.ndarray, + *, + pixel_mm: float, + pad_pixels: int = 4, + closing_iters: int = 0, + fill_bg_below_mm2: float = 0.0, +) -> OccupancyGrid: + """Rasterise points to a binary grid. + + ``closing_iters`` is the number of binary-closing passes applied as a + cheap defragmenter (also shrinks real holes by ``iters`` pixels — use + sparingly). + + ``fill_bg_below_mm2`` is the preferred defragmenter: it fills every + background component smaller than this area (in mm²) with foreground. + Real cavities (holes the caller wants to detect) keep their original + size because they exceed the threshold. + """ + if uv.shape[0] == 0: + raise ValueError("empty point set") + u_min = float(uv[:, 0].min()) - pad_pixels * pixel_mm + v_min = float(uv[:, 1].min()) - pad_pixels * pixel_mm + u_max = float(uv[:, 0].max()) + pad_pixels * pixel_mm + v_max = float(uv[:, 1].max()) + pad_pixels * pixel_mm + + W = int(np.ceil((u_max - u_min) / pixel_mm)) + 1 + H = int(np.ceil((v_max - v_min) / pixel_mm)) + 1 + + cols = np.clip(((uv[:, 0] - u_min) / pixel_mm).astype(np.int64), 0, W - 1) + rows = np.clip(((uv[:, 1] - v_min) / pixel_mm).astype(np.int64), 0, H - 1) + + occ = np.zeros((H, W), dtype=bool) + occ[rows, cols] = True + if closing_iters > 0: + occ = _binary_close(occ, closing_iters) + if fill_bg_below_mm2 > 0: + min_keep_px = max(1, int(np.ceil(float(fill_bg_below_mm2) / (pixel_mm ** 2)))) + occ = _fill_small_bg_components(occ, min_keep_px) + + return OccupancyGrid( + occupied=occ, + pixel_mm=float(pixel_mm), + u_min=u_min, + v_min=v_min, + point_indices=np.empty(0), + point_pixel_idx=np.column_stack([rows, cols]), + ) + + +# --------------------------------------------------------------------------- + + +def _flood_fill_label(grid: np.ndarray) -> np.ndarray: + """4-connected labelling. Background (False) gets label 0.""" + H, W = grid.shape + labels = np.zeros((H, W), dtype=np.int32) + next_label = 0 + stack: list[tuple[int, int]] = [] + for i in range(H): + for j in range(W): + if grid[i, j] and labels[i, j] == 0: + next_label += 1 + stack.append((i, j)) + while stack: + r, c = stack.pop() + if r < 0 or r >= H or c < 0 or c >= W: + continue + if not grid[r, c] or labels[r, c] != 0: + continue + labels[r, c] = next_label + stack.append((r - 1, c)) + stack.append((r + 1, c)) + stack.append((r, c - 1)) + stack.append((r, c + 1)) + return labels + + +def _largest_component(labels: np.ndarray) -> int: + if labels.max() == 0: + return 0 + counts = np.bincount(labels.ravel()) + counts[0] = 0 + return int(np.argmax(counts)) + + +def _holes_inside( + labels: np.ndarray, + panel_label: int, +) -> list[np.ndarray]: + """Return masks of background components fully enclosed by ``panel_label``. + + A 'hole' is a 0-component that is not connected to the image border + via 4-connectivity through other 0-pixels. + """ + H, W = labels.shape + bg = labels == 0 + + # Label background components. + bg_labels = _flood_fill_label(bg) + + # Components that touch the border. + border_components = set() + for j in range(W): + if bg_labels[0, j] != 0: + border_components.add(int(bg_labels[0, j])) + if bg_labels[H - 1, j] != 0: + border_components.add(int(bg_labels[H - 1, j])) + for i in range(H): + if bg_labels[i, 0] != 0: + border_components.add(int(bg_labels[i, 0])) + if bg_labels[i, W - 1] != 0: + border_components.add(int(bg_labels[i, W - 1])) + + holes: list[np.ndarray] = [] + n_bg = int(bg_labels.max()) + for k in range(1, n_bg + 1): + if k in border_components: + continue + mask = bg_labels == k + # Sanity: at least one neighbouring pixel must belong to ``panel_label``. + # (Otherwise it's enclosed by a different fg component, which we ignore.) + # Quick neighbour test: + from numpy import roll + nbr = ( + (roll(labels, 1, axis=0) == panel_label) + | (roll(labels, -1, axis=0) == panel_label) + | (roll(labels, 1, axis=1) == panel_label) + | (roll(labels, -1, axis=1) == panel_label) + ) + if np.any(mask & nbr): + holes.append(mask) + return holes + + +def _hole_boundary_pixels(panel_mask: np.ndarray, hole_mask: np.ndarray) -> np.ndarray: + """Pixels in ``panel_mask`` that touch a pixel in ``hole_mask``.""" + from numpy import roll + + nbr = ( + roll(hole_mask, 1, axis=0) + | roll(hole_mask, -1, axis=0) + | roll(hole_mask, 1, axis=1) + | roll(hole_mask, -1, axis=1) + ) + return panel_mask & nbr + + +def find_panel_holes( + grid: OccupancyGrid, + *, + panel_min_area_ratio: float = 0.4, + hole_min_area_mm2: float = 30.0, + hole_max_area_mm2: float = 800.0, + point_uv: np.ndarray | None = None, +) -> tuple[np.ndarray, list[HoleCandidate]]: + """Return (panel_mask, list_of_holes). + + ``point_uv`` is optional but, if provided, lets us hand back the + *original-cloud* point indices that fall inside each hole's boundary + pixels (= proper boundary points for a 3D circle fit). + """ + H, W = grid.occupied.shape + fg_labels = _flood_fill_label(grid.occupied) + panel_label = _largest_component(fg_labels) + if panel_label == 0: + raise RuntimeError("no occupied pixels") + + panel_mask = fg_labels == panel_label + panel_area = panel_mask.sum() * grid.pixel_mm ** 2 + bbox_area = H * W * grid.pixel_mm ** 2 + if panel_area / max(bbox_area, 1e-9) < panel_min_area_ratio: + # The panel CC is suspiciously small. We still proceed; downstream + # code will report a quality flag. + pass + + holes_masks = _holes_inside(fg_labels, panel_label) + out: list[HoleCandidate] = [] + + # Pre-compute (row, col) → list of original-cloud indices, for boundary recovery. + pix_rc = grid.point_pixel_idx if point_uv is not None else None + + for mask in holes_masks: + area = float(mask.sum() * grid.pixel_mm ** 2) + if area < hole_min_area_mm2 or area > hole_max_area_mm2: + continue + + bnd_mask = _hole_boundary_pixels(panel_mask, mask) + if not bnd_mask.any(): + continue + + bnd_rc = np.argwhere(bnd_mask) # (M, 2): (row, col) + bnd_uv = np.column_stack( + [ + grid.u_min + (bnd_rc[:, 1] + 0.5) * grid.pixel_mm, + grid.v_min + (bnd_rc[:, 0] + 0.5) * grid.pixel_mm, + ] + ) + + rows = np.argwhere(mask) + cu = grid.u_min + (rows[:, 1] + 0.5).mean() * grid.pixel_mm + cv = grid.v_min + (rows[:, 0] + 0.5).mean() * grid.pixel_mm + + bbox = ( + grid.u_min + rows[:, 1].min() * grid.pixel_mm, + grid.v_min + rows[:, 0].min() * grid.pixel_mm, + grid.u_min + rows[:, 1].max() * grid.pixel_mm, + grid.v_min + rows[:, 0].max() * grid.pixel_mm, + ) + + if pix_rc is not None: + # Map boundary pixels back to point indices. + row_col_set = set(map(tuple, bnd_rc.tolist())) + point_idx = np.array( + [i for i, (r, c) in enumerate(map(tuple, pix_rc.tolist())) if (r, c) in row_col_set], + dtype=np.int64, + ) + else: + point_idx = np.empty(0, dtype=np.int64) + + out.append( + HoleCandidate( + boundary_uv=bnd_uv, + boundary_point_idx=point_idx, + centroid_uv=(float(cu), float(cv)), + area_mm2=area, + bbox_uv=bbox, + ) + ) + + return panel_mask, out + + +__all__ = [ + "OccupancyGrid", + "HoleCandidate", + "build_grid", + "find_panel_holes", +] diff --git a/alg/delivery_v1.1/coupler_panel_release/coupler_panel/detection/plane_local.py b/alg/delivery_v1.1/coupler_panel_release/coupler_panel/detection/plane_local.py new file mode 100644 index 0000000..1e228d1 --- /dev/null +++ b/alg/delivery_v1.1/coupler_panel_release/coupler_panel/detection/plane_local.py @@ -0,0 +1,127 @@ +"""Build a local 2D frame on the panel and project / lift between it and 3D. + +The panel's normal is roughly along ±x in the CAD frame, but the point +cloud may be misaligned. We accept a ``normal_seed`` (from the GT or +from a robust PCA on the point cloud) and orthonormalise around it. + +Local 2D conventions +-------------------- + +- ``u`` axis: closest to world +y inside the panel plane +- ``v`` axis: closest to world +z inside the panel plane + +So a top-down picture of the panel has u → right, v → up, and matches +how the four locating holes are arranged in the y-z plane. +""" + +from __future__ import annotations + +from dataclasses import dataclass + +import numpy as np + + +@dataclass(frozen=True) +class LocalPanelFrame: + origin: np.ndarray # (3,) — point on plane (typically the centroid) + normal: np.ndarray # (3,) — unit, n_x > 0 + u_axis: np.ndarray # (3,) + v_axis: np.ndarray # (3,) + + def to_2d(self, points: np.ndarray) -> np.ndarray: + """Project ``points`` onto the panel plane in (u, v).""" + rel = points - self.origin + return np.column_stack([rel @ self.u_axis, rel @ self.v_axis]) + + def signed_distance(self, points: np.ndarray) -> np.ndarray: + return (points - self.origin) @ self.normal + + def from_2d(self, uv: np.ndarray, *, signed_distance: float = 0.0) -> np.ndarray: + """Lift ``(u, v)`` coords back into 3D, optionally with a normal offset.""" + uv = np.atleast_2d(uv) + return ( + self.origin + + uv[:, 0:1] * self.u_axis + + uv[:, 1:2] * self.v_axis + + float(signed_distance) * self.normal + ) + + +def _orthonormal_basis(normal: np.ndarray) -> tuple[np.ndarray, np.ndarray]: + """Return (u, v) so that {u, v, normal} is an RHC orthonormal basis, + with ``u`` the in-plane vector closest to world +y. + """ + n = normal / max(np.linalg.norm(normal), 1e-12) + y = np.array([0.0, 1.0, 0.0]) + u = y - np.dot(y, n) * n + if np.linalg.norm(u) < 1e-6: + # n is parallel to y; pick z instead. + z = np.array([0.0, 0.0, 1.0]) + u = z - np.dot(z, n) * n + u /= np.linalg.norm(u) + v = np.cross(n, u) + v /= np.linalg.norm(v) + return u, v + + +def _pca_normal(points: np.ndarray) -> np.ndarray: + centroid = points.mean(axis=0) + Q = points - centroid + cov = (Q.T @ Q) / max(points.shape[0] - 1, 1) + w, V = np.linalg.eigh(cov) + # Smallest eigenvector ≈ panel normal. + n = V[:, 0] + if n[0] < 0: + n = -n + return n + + +def project_to_panel( + points: np.ndarray, + *, + normal_seed: np.ndarray | str = "pca", + origin: np.ndarray | None = None, + pca_kth: int = 0, +) -> tuple[LocalPanelFrame, np.ndarray]: + """Project ``points`` onto a local panel frame. + + Returns + ------- + frame : :class:`LocalPanelFrame` + uv : ``(N, 2)`` projected coordinates + + Parameters + ---------- + normal_seed : str or (3,) + ``"pca"`` → estimate via PCA on the cloud (smallest variance axis). + otherwise a 3-vector. + origin : (3,) or None + If ``None``, use the cloud centroid. + """ + pts = np.asarray(points, dtype=np.float64) + + if isinstance(normal_seed, str): + if normal_seed == "pca": + normal = _pca_normal(pts) + else: + raise ValueError(f"unknown normal_seed string '{normal_seed}'") + else: + normal = np.asarray(normal_seed, dtype=np.float64).reshape(3) + normal = normal / max(np.linalg.norm(normal), 1e-12) + if normal[0] < 0: + normal = -normal + + if origin is None: + origin = pts.mean(axis=0) + origin = np.asarray(origin, dtype=np.float64).reshape(3) + + u, v = _orthonormal_basis(normal) + frame = LocalPanelFrame(origin=origin, normal=normal, u_axis=u, v_axis=v) + return frame, frame.to_2d(pts) + + +def lift_back(uv: np.ndarray, frame: LocalPanelFrame) -> np.ndarray: + return frame.from_2d(uv, signed_distance=0.0) + + +__all__ = ["LocalPanelFrame", "project_to_panel", "lift_back"] diff --git a/alg/delivery_v1.1/coupler_panel_release/coupler_panel/exceptions.py b/alg/delivery_v1.1/coupler_panel_release/coupler_panel/exceptions.py new file mode 100644 index 0000000..734a15a --- /dev/null +++ b/alg/delivery_v1.1/coupler_panel_release/coupler_panel/exceptions.py @@ -0,0 +1,55 @@ +"""Public exception hierarchy for coupler_panel. + +Catch :class:`CouplerError` for any algorithm failure that should be +surfaced to the user. Subclasses indicate what specifically went wrong so +callers can render targeted error messages. + +Non-fatal data-quality issues are **not** raised; they are reported via +``CouplerMeasurement.quality_flags``. +""" + +from __future__ import annotations + + +class CouplerError(Exception): + """Base class for all errors raised by the coupler-panel pipeline.""" + + +class InvalidPointCloudError(CouplerError): + """Raised when the input point cloud is malformed or unreadable.""" + + +class UnsupportedFormatError(InvalidPointCloudError): + """Raised when the input file extension is not supported.""" + + +class InsufficientPointsError(CouplerError): + """Raised when the input has too few points to run the pipeline.""" + + +class CADParseError(CouplerError): + """Raised when the STP file cannot be parsed or has unexpected topology.""" + + +class HoleDetectionError(CouplerError): + """Raised when the four locating holes cannot be reliably detected.""" + + +class RegistrationError(CouplerError): + """Raised when 4-hole Kabsch registration fails (e.g. degenerate pairing).""" + + +class PlaneFitError(CouplerError): + """Raised when the panel plane cannot be fitted (too few inliers, etc.).""" + + +__all__ = [ + "CouplerError", + "InvalidPointCloudError", + "UnsupportedFormatError", + "InsufficientPointsError", + "CADParseError", + "HoleDetectionError", + "RegistrationError", + "PlaneFitError", +] diff --git a/alg/delivery_v1.1/coupler_panel_release/coupler_panel/geometry/__init__.py b/alg/delivery_v1.1/coupler_panel_release/coupler_panel/geometry/__init__.py new file mode 100644 index 0000000..e0dfec1 --- /dev/null +++ b/alg/delivery_v1.1/coupler_panel_release/coupler_panel/geometry/__init__.py @@ -0,0 +1,11 @@ +"""Geometry construction & plane fitting for the coupler panel.""" + +from .reference import build_reference_frame +from .roi import extract_panel_roi +from .plane_fit import fit_panel_plane + +__all__ = [ + "build_reference_frame", + "extract_panel_roi", + "fit_panel_plane", +] diff --git a/alg/delivery_v1.1/coupler_panel_release/coupler_panel/geometry/plane_fit.py b/alg/delivery_v1.1/coupler_panel_release/coupler_panel/geometry/plane_fit.py new file mode 100644 index 0000000..e4b6aa4 --- /dev/null +++ b/alg/delivery_v1.1/coupler_panel_release/coupler_panel/geometry/plane_fit.py @@ -0,0 +1,113 @@ +"""Robust panel-plane fitting. + +Pipeline: + +1. RANSAC: pick 3 random points, build a plane, count inliers within + ``inlier_thresh_mm``. Repeat ``ransac_iterations`` times; keep the + best inlier set. +2. SVD: refit the plane on the inliers (eigenvector with the smallest + eigenvalue of the centred covariance). +3. Sign convention: enforce ``n_x > 0``. + +Returns a :class:`PanelPlane`. +""" + +from __future__ import annotations + +from typing import Any + +import numpy as np + +from ..exceptions import PlaneFitError +from ..structures import PanelPlane + + +def _plane_from_three(p1, p2, p3) -> tuple[np.ndarray, float] | None: + n = np.cross(p2 - p1, p3 - p1) + norm = np.linalg.norm(n) + if norm < 1e-9: + return None + n = n / norm + d = -float(np.dot(n, p1)) + return n, d + + +def _svd_plane(points: np.ndarray) -> tuple[np.ndarray, float]: + centroid = points.mean(axis=0) + Q = points - centroid + cov = (Q.T @ Q) / max(points.shape[0] - 1, 1) + w, V = np.linalg.eigh(cov) + n = V[:, 0] + n = n / max(np.linalg.norm(n), 1e-12) + d = -float(np.dot(n, centroid)) + return n, d + + +def fit_panel_plane( + points: np.ndarray, + *, + plane_fit_cfg: dict[str, Any], + seed: int | None = 7, +) -> PanelPlane: + pts = np.asarray(points, dtype=np.float64) + n_pts = pts.shape[0] + if n_pts < 3: + raise PlaneFitError(f"need ≥ 3 points, got {n_pts}") + + iterations = int(plane_fit_cfg.get("ransac_iterations", 200)) + thresh = plane_fit_cfg.get("ransac_inlier_thresh_mm", 1.5) + if thresh in (None, "auto"): + thresh = 1.5 + thresh = float(thresh) + min_inlier_ratio = float(plane_fit_cfg.get("min_inlier_ratio", 0.6)) + enforce_nx_positive = bool(plane_fit_cfg.get("enforce_nx_positive", True)) + + rng = np.random.default_rng(seed) + + best_inliers: np.ndarray | None = None + best_count = -1 + + for _ in range(iterations): + sample = rng.choice(n_pts, size=3, replace=False) + plane = _plane_from_three(pts[sample[0]], pts[sample[1]], pts[sample[2]]) + if plane is None: + continue + n, d = plane + dist = np.abs(pts @ n + d) + inliers = dist <= thresh + c = int(inliers.sum()) + if c > best_count: + best_count = c + best_inliers = inliers + + if best_inliers is None or best_count < 3: + raise PlaneFitError( + f"RANSAC found {best_count} inliers in {iterations} iters at " + f"thresh={thresh} mm; check noise level / threshold" + ) + + inlier_ratio = best_count / n_pts + inlier_pts = pts[best_inliers] + n, d = _svd_plane(inlier_pts) + if enforce_nx_positive and n[0] < 0: + n = -n + d = -d + + residuals = inlier_pts @ n + d + rms = float(np.sqrt(np.mean(residuals ** 2))) + + if inlier_ratio < min_inlier_ratio: + # Caller will log a quality flag; we still return the result. + pass + + centroid = inlier_pts.mean(axis=0) + return PanelPlane( + normal=tuple(n.tolist()), + point_on_plane=tuple(centroid.tolist()), + d_offset=float(d), + inlier_ratio=float(inlier_ratio), + inlier_residual_rms_mm=rms, + ) + + +__all__ = ["fit_panel_plane"] diff --git a/alg/delivery_v1.1/coupler_panel_release/coupler_panel/geometry/reference.py b/alg/delivery_v1.1/coupler_panel_release/coupler_panel/geometry/reference.py new file mode 100644 index 0000000..17771b2 --- /dev/null +++ b/alg/delivery_v1.1/coupler_panel_release/coupler_panel/geometry/reference.py @@ -0,0 +1,32 @@ +"""Construct the reference frame from four hole centres. + +Given the four locating-hole centres :math:`H_1 .. H_4`: + +- Panel centre :math:`C` = mean of the four centres (= diagonal + intersection for a rectangle). +- Reference line :math:`L = C + \\lambda \\hat e_x`. +- Reference plane: :math:`z = C_z`. +- Longitudinal mid-plane: :math:`y = C_y`. +""" + +from __future__ import annotations + +import numpy as np + +from ..structures import HoleSet, ReferenceFrame + + +def build_reference_frame(holes: HoleSet) -> ReferenceFrame: + centers = holes.locating_centers() + if centers.shape != (4, 3): + raise ValueError(f"need 4 locating-hole centres, got shape {centers.shape}") + C = centers.mean(axis=0) + return ReferenceFrame( + center_C=tuple(C.tolist()), + line_direction=(1.0, 0.0, 0.0), + reference_plane_z=float(C[2]), + longitudinal_mid_y=float(C[1]), + ) + + +__all__ = ["build_reference_frame"] diff --git a/alg/delivery_v1.1/coupler_panel_release/coupler_panel/geometry/roi.py b/alg/delivery_v1.1/coupler_panel_release/coupler_panel/geometry/roi.py new file mode 100644 index 0000000..5647a22 --- /dev/null +++ b/alg/delivery_v1.1/coupler_panel_release/coupler_panel/geometry/roi.py @@ -0,0 +1,159 @@ +"""Extract the *clean* panel ROI from the registered point cloud. + +Strategy: + +1. Take points whose 2D projection falls inside the four-hole rectangle + (shrunk inwards by ``edge_buffer_mm``). +2. Drop points within ``hole_dilate_mm`` of any locating-hole centre + (radius + dilation). +3. Drop points within ``central_hole_dilate_mm`` of the central hole. +4. Drop chamfer points (per-point normal deviates from panel normal by + more than ``acos(chamfer_normal_dot_min)``). Normals are estimated + on the fly via PCA on each point's k-neighbourhood. +5. Drop z-score outliers along the panel normal. +""" + +from __future__ import annotations + +from typing import Any + +import numpy as np + +from ..structures import HoleSet + + +def _kth_normals(points: np.ndarray, k: int = 20) -> np.ndarray: + """Per-point normal via PCA on the k-NN. Returns ``(N, 3)``.""" + n = points.shape[0] + if n < k + 1: + return np.tile(np.array([1.0, 0.0, 0.0]), (n, 1)) + try: + from scipy.spatial import cKDTree + except ImportError: + return np.tile(np.array([1.0, 0.0, 0.0]), (n, 1)) + + tree = cKDTree(points) + _, idx = tree.query(points, k=k + 1) + normals = np.empty((n, 3), dtype=np.float64) + for i in range(n): + nbh = points[idx[i, 1:]] + c = nbh.mean(axis=0) + Q = nbh - c + cov = (Q.T @ Q) / max(k - 1, 1) + try: + w, V = np.linalg.eigh(cov) + normals[i] = V[:, 0] + except np.linalg.LinAlgError: + normals[i] = np.array([1.0, 0.0, 0.0]) + return normals + + +def _signed_to_plane(points: np.ndarray, normal: np.ndarray, offset: float) -> np.ndarray: + """Signed distance from each point to plane n·x + d = 0.""" + return points @ normal + offset + + +def extract_panel_roi( + points: np.ndarray, + *, + holes: HoleSet, + panel_normal_seed: np.ndarray, + roi_cfg: dict[str, Any], +) -> tuple[np.ndarray, dict[str, Any]]: + """Return (ROI points, diagnostics dict). + + ``holes`` and ``panel_normal_seed`` are expected in the *same* frame + as ``points`` — registration must already have happened before this + is called. + """ + pts = np.asarray(points, dtype=np.float64) + if pts.shape[0] == 0: + return pts.copy(), {"n_in": 0, "n_out": 0, "stages": []} + + n = np.asarray(panel_normal_seed, dtype=np.float64) + n = n / max(np.linalg.norm(n), 1e-12) + if n[0] < 0: + n = -n + + centers_3d = holes.locating_centers() # (4, 3) + C = centers_3d.mean(axis=0) + + # Build local 2D frame to test "inside the rectangle". + y_world = np.array([0.0, 1.0, 0.0]) + u = y_world - np.dot(y_world, n) * n + if np.linalg.norm(u) < 1e-6: + u = np.array([0.0, 0.0, 1.0]) - np.dot(np.array([0.0, 0.0, 1.0]), n) * n + u /= np.linalg.norm(u) + v = np.cross(n, u) + v /= np.linalg.norm(v) + + rel = pts - C + uv = np.column_stack([rel @ u, rel @ v]) + centers_uv = np.column_stack([(centers_3d - C) @ u, (centers_3d - C) @ v]) + + edge_buf = float(roi_cfg.get("edge_buffer_mm", 5.0)) + u_lo = centers_uv[:, 0].min() + edge_buf + u_hi = centers_uv[:, 0].max() - edge_buf + v_lo = centers_uv[:, 1].min() + edge_buf + v_hi = centers_uv[:, 1].max() - edge_buf + + stages: list[dict[str, Any]] = [] + n_in = pts.shape[0] + + mask = (uv[:, 0] >= u_lo) & (uv[:, 0] <= u_hi) & (uv[:, 1] >= v_lo) & (uv[:, 1] <= v_hi) + stages.append({"name": "rectangle", "n_kept": int(mask.sum())}) + + # 2. Locating hole exclusion (use 3D distance to each hole's centre, + # with the hole's own radius + dilation). + hole_dilate = float(roi_cfg.get("hole_dilate_mm", 3.0)) + for h in holes.locating: + center = np.asarray(h.center) + r = float(h.radius_mm) + hole_dilate + d2 = np.sum((pts - center) ** 2, axis=1) + mask &= d2 > r * r + stages.append({"name": "locating_holes", "n_kept": int(mask.sum())}) + + # 3. Central hole exclusion. + if holes.central is not None: + center = np.asarray(holes.central.center) + r = float(holes.central.radius_mm) + float(roi_cfg.get("central_hole_dilate_mm", 5.0)) + d2 = np.sum((pts - center) ** 2, axis=1) + mask &= d2 > r * r + stages.append({"name": "central_hole", "n_kept": int(mask.sum())}) + + # 4. Chamfer/normal filter — only run if we still have enough points to PCA. + chamfer_dot_min = float(roi_cfg.get("chamfer_normal_dot_min", 0.85)) + if mask.sum() > 50 and chamfer_dot_min > 0: + kept = pts[mask] + normals = _kth_normals(kept, k=20) + # Align all normals so they point along +n_panel (sign-invariant). + flips = (normals @ n) < 0 + normals[flips] = -normals[flips] + keep_local = (normals @ n) >= chamfer_dot_min + # Map back into the full mask. + idxs = np.where(mask)[0] + new_mask = np.zeros_like(mask) + new_mask[idxs[keep_local]] = True + mask = new_mask + stages.append({"name": "chamfer_normal", "n_kept": int(mask.sum())}) + + # 5. Z-score outlier rejection along panel normal. + z_score = float(roi_cfg.get("outlier_z_score", 3.0)) + if mask.sum() > 50 and z_score > 0: + kept = pts[mask] + d = kept @ n + mu = d.mean() + sigma = d.std() + if sigma > 1e-12: + keep_local = np.abs(d - mu) <= z_score * sigma + idxs = np.where(mask)[0] + new_mask = np.zeros_like(mask) + new_mask[idxs[keep_local]] = True + mask = new_mask + stages.append({"name": "z_score", "n_kept": int(mask.sum())}) + + out = pts[mask] + return out, {"n_in": int(n_in), "n_out": int(out.shape[0]), "stages": stages} + + +__all__ = ["extract_panel_roi"] diff --git a/alg/delivery_v1.1/coupler_panel_release/coupler_panel/io_utils/__init__.py b/alg/delivery_v1.1/coupler_panel_release/coupler_panel/io_utils/__init__.py new file mode 100644 index 0000000..37796e7 --- /dev/null +++ b/alg/delivery_v1.1/coupler_panel_release/coupler_panel/io_utils/__init__.py @@ -0,0 +1,25 @@ +"""Point-cloud / artifact IO utilities for coupler_panel.""" + +from .pointcloud_io import ( + load_point_cloud, + write_point_cloud, + write_metadata_json, +) +from .artifacts import ( + ensure_dir, + write_json, + write_yaml, + write_csv_rows, + Artifacts, +) + +__all__ = [ + "load_point_cloud", + "write_point_cloud", + "write_metadata_json", + "ensure_dir", + "write_json", + "write_yaml", + "write_csv_rows", + "Artifacts", +] diff --git a/alg/delivery_v1.1/coupler_panel_release/coupler_panel/io_utils/artifacts.py b/alg/delivery_v1.1/coupler_panel_release/coupler_panel/io_utils/artifacts.py new file mode 100644 index 0000000..c3f091b --- /dev/null +++ b/alg/delivery_v1.1/coupler_panel_release/coupler_panel/io_utils/artifacts.py @@ -0,0 +1,168 @@ +"""Artifact directory layout for one pipeline run. + +A run directory looks like:: + + runs// + ├── 00_cad_mesh.ply + ├── 01_gt.json + ├── 02_sampled.ply + ├── 03_noisy.ply + ├── 04_transformed.ply + ├── 05_denoised.ply + ├── 06_registered.ply + ├── 07_holes.json + ├── 08_panel_roi.ply + ├── 09_plane.json + ├── 10_metrics.json + ├── images/ + ├── config.resolved.yaml + └── report.json + +Most modules write through the :class:`Artifacts` helper so naming stays +consistent. +""" + +from __future__ import annotations + +import csv +import json +from dataclasses import dataclass +from pathlib import Path +from typing import Any, Iterable, Sequence + +import numpy as np +import yaml + + +# --------------------------------------------------------------------------- +# Generic helpers +# --------------------------------------------------------------------------- + + +def ensure_dir(path: str | Path) -> Path: + p = Path(path) + p.mkdir(parents=True, exist_ok=True) + return p + + +def write_json(data: Any, path: str | Path) -> Path: + path = Path(path) + path.parent.mkdir(parents=True, exist_ok=True) + with path.open("w", encoding="utf-8") as fh: + json.dump(data, fh, ensure_ascii=False, indent=2, default=_json_default) + return path + + +def write_yaml(data: Any, path: str | Path) -> Path: + path = Path(path) + path.parent.mkdir(parents=True, exist_ok=True) + with path.open("w", encoding="utf-8") as fh: + yaml.safe_dump(_yaml_default(data), fh, allow_unicode=True, sort_keys=False) + return path + + +def write_csv_rows( + rows: Iterable[Sequence[Any]], + path: str | Path, + *, + header: Sequence[str] | None = None, +) -> Path: + path = Path(path) + path.parent.mkdir(parents=True, exist_ok=True) + with path.open("w", encoding="utf-8", newline="") as fh: + w = csv.writer(fh) + if header is not None: + w.writerow(header) + for row in rows: + w.writerow(row) + return path + + +def _json_default(obj: Any) -> Any: + if isinstance(obj, np.ndarray): + return obj.tolist() + if isinstance(obj, (np.floating, np.integer)): + return obj.item() + if isinstance(obj, Path): + return str(obj) + raise TypeError(f"not JSON serialisable: {type(obj).__name__}") + + +def _yaml_default(obj: Any) -> Any: + if isinstance(obj, dict): + return {k: _yaml_default(v) for k, v in obj.items()} + if isinstance(obj, (list, tuple)): + return [_yaml_default(v) for v in obj] + if isinstance(obj, np.ndarray): + return obj.tolist() + if isinstance(obj, (np.floating, np.integer)): + return obj.item() + return obj + + +# --------------------------------------------------------------------------- +# Stage-naming helper +# --------------------------------------------------------------------------- + + +@dataclass +class Artifacts: + """Resolves stage filenames inside a run directory. + + Use :meth:`path` to get the canonical name for a given stage: + + >>> art = Artifacts("runs/demo") + >>> art.path("sampled", ext="ply") + PosixPath('runs/demo/02_sampled.ply') + """ + + root: Path + + _STAGE_INDEX: dict[str, int] = None # type: ignore[assignment] + + # Canonical stage ordering. Keys here are referenced from pipeline.py. + STAGE_ORDER = ( + "cad_mesh", # 00 + "gt", # 01 + "sampled", # 02 + "noisy", # 03 + "transformed", # 04 + "denoised", # 05 + "registered", # 06 + "holes", # 07 + "panel_roi", # 08 + "plane", # 09 + "metrics", # 10 + ) + + def __init__(self, root: str | Path) -> None: + self.root = ensure_dir(root) + ensure_dir(self.root / "images") + self._STAGE_INDEX = {name: i for i, name in enumerate(self.STAGE_ORDER)} + + def path(self, stage: str, *, ext: str) -> Path: + idx = self._STAGE_INDEX.get(stage) + if idx is None: + raise KeyError(f"unknown stage '{stage}'; valid: {list(self._STAGE_INDEX)}") + ext = ext.lstrip(".") + return self.root / f"{idx:02d}_{stage}.{ext}" + + def image(self, name: str) -> Path: + return self.root / "images" / name + + @property + def report_json(self) -> Path: + return self.root / "report.json" + + @property + def resolved_config(self) -> Path: + return self.root / "config.resolved.yaml" + + +__all__ = [ + "ensure_dir", + "write_json", + "write_yaml", + "write_csv_rows", + "Artifacts", +] diff --git a/alg/delivery_v1.1/coupler_panel_release/coupler_panel/io_utils/pointcloud_io.py b/alg/delivery_v1.1/coupler_panel_release/coupler_panel/io_utils/pointcloud_io.py new file mode 100644 index 0000000..710352e --- /dev/null +++ b/alg/delivery_v1.1/coupler_panel_release/coupler_panel/io_utils/pointcloud_io.py @@ -0,0 +1,238 @@ +"""Point cloud read / write. + +Supported formats: + - ``.ply`` binary or ASCII (open3d if available, else minimal builtin reader) + - ``.xyz`` whitespace-separated XYZ[ R G B][ NX NY NZ] + - ``.txt`` alias for ``.xyz`` + - ``.csv`` comma-separated, first three columns must be X, Y, Z + - ``.pcd`` open3d only + +The XYZ reader also serves as a fallback for ``.ply`` if open3d is +unavailable and the PLY is ASCII. +""" + +from __future__ import annotations + +import json +from pathlib import Path +from typing import Any + +import numpy as np + +from ..exceptions import InvalidPointCloudError, UnsupportedFormatError + + +_TEXT_SUFFIXES = (".xyz", ".txt", ".csv") +_OPEN3D_SUFFIXES = (".ply", ".pcd") + + +def _try_import_open3d(): + try: + import open3d as o3d # type: ignore + + return o3d + except Exception: + return None + + +# --------------------------------------------------------------------------- +# Loading +# --------------------------------------------------------------------------- + + +def load_point_cloud(path: str | Path) -> np.ndarray: + """Load a point cloud as a float64 ``(N, 3)`` ndarray. + + Raises + ------ + UnsupportedFormatError + If the file extension is not supported. + InvalidPointCloudError + If the file is missing, empty, or fails to parse. + """ + path = Path(path) + if not path.exists(): + raise InvalidPointCloudError(f"point cloud not found: {path}") + + suffix = path.suffix.lower() + if suffix in _TEXT_SUFFIXES: + return _load_text(path, sep="," if suffix == ".csv" else None) + + if suffix in _OPEN3D_SUFFIXES: + o3d = _try_import_open3d() + if o3d is not None: + pcd = o3d.io.read_point_cloud(str(path)) + arr = np.asarray(pcd.points, dtype=np.float64) + if arr.size == 0: + # PLY may also be ASCII XYZ-style; fall back. + if suffix == ".ply": + return _load_ascii_ply_fallback(path) + raise InvalidPointCloudError(f"empty point cloud: {path}") + return arr + if suffix == ".ply": + return _load_ascii_ply_fallback(path) + raise InvalidPointCloudError( + f"open3d is required to read {suffix}; install with `pip install open3d`" + ) + + raise UnsupportedFormatError(f"unsupported extension: {suffix}") + + +def _load_text(path: Path, sep: str | None) -> np.ndarray: + try: + if sep is None: + arr = np.loadtxt(path, comments=("#", "//")) + else: + arr = np.loadtxt(path, comments=("#", "//"), delimiter=sep) + except Exception as exc: + raise InvalidPointCloudError(f"failed to parse {path}: {exc}") from exc + if arr.ndim == 1: + arr = arr.reshape(1, -1) + if arr.shape[1] < 3: + raise InvalidPointCloudError( + f"{path} has fewer than 3 columns (got {arr.shape[1]})" + ) + return arr[:, :3].astype(np.float64, copy=False) + + +def _load_ascii_ply_fallback(path: Path) -> np.ndarray: + """Minimal ASCII-PLY reader for the 'just XYZ' case.""" + with path.open("r", encoding="utf-8", errors="replace") as fh: + header_done = False + n_vertices = 0 + x_idx = y_idx = z_idx = -1 + prop_idx = 0 + format_line: str | None = None + for line in fh: + line = line.strip() + if line == "end_header": + header_done = True + break + if line.startswith("format"): + format_line = line + elif line.startswith("element vertex"): + n_vertices = int(line.split()[-1]) + prop_idx = 0 + elif line.startswith("property"): + name = line.split()[-1] + if name == "x": + x_idx = prop_idx + elif name == "y": + y_idx = prop_idx + elif name == "z": + z_idx = prop_idx + prop_idx += 1 + if not header_done or n_vertices <= 0 or -1 in (x_idx, y_idx, z_idx): + raise InvalidPointCloudError( + f"unsupported PLY header in {path}; install open3d for full PLY support" + ) + if format_line and "binary" in format_line: + raise InvalidPointCloudError( + f"{path} is binary PLY; install open3d to read it" + ) + out = np.empty((n_vertices, 3), dtype=np.float64) + for i in range(n_vertices): + parts = fh.readline().split() + out[i, 0] = float(parts[x_idx]) + out[i, 1] = float(parts[y_idx]) + out[i, 2] = float(parts[z_idx]) + return out + + +# --------------------------------------------------------------------------- +# Writing +# --------------------------------------------------------------------------- + + +def write_point_cloud( + points: np.ndarray, + path: str | Path, + *, + colors: np.ndarray | None = None, + binary: bool = True, +) -> Path: + """Write a point cloud. Format is inferred from the extension. + + For ``.ply``/``.pcd`` open3d is preferred; if open3d is missing and the + target is ``.ply``, we fall back to writing ASCII PLY ourselves. + """ + path = Path(path) + path.parent.mkdir(parents=True, exist_ok=True) + points = np.ascontiguousarray(points, dtype=np.float64) + if points.ndim != 2 or points.shape[1] != 3: + raise InvalidPointCloudError( + f"expected (N,3) points, got shape {points.shape}" + ) + + suffix = path.suffix.lower() + if suffix in (".xyz", ".txt"): + np.savetxt(path, points, fmt="%.6f") + return path + if suffix == ".csv": + np.savetxt(path, points, fmt="%.6f", delimiter=",") + return path + + if suffix in _OPEN3D_SUFFIXES: + o3d = _try_import_open3d() + if o3d is not None: + pcd = o3d.geometry.PointCloud() + pcd.points = o3d.utility.Vector3dVector(points) + if colors is not None: + pcd.colors = o3d.utility.Vector3dVector(np.asarray(colors)[:, :3]) + o3d.io.write_point_cloud(str(path), pcd, write_ascii=not binary) + return path + if suffix == ".ply": + _write_ascii_ply(points, path, colors=colors) + return path + raise InvalidPointCloudError( + f"open3d is required to write {suffix}; install with `pip install open3d`" + ) + + raise UnsupportedFormatError(f"unsupported extension: {suffix}") + + +def _write_ascii_ply( + points: np.ndarray, + path: Path, + colors: np.ndarray | None = None, +) -> None: + have_color = colors is not None + n = points.shape[0] + with path.open("w", encoding="ascii") as fh: + fh.write("ply\nformat ascii 1.0\n") + fh.write(f"element vertex {n}\n") + fh.write("property float x\nproperty float y\nproperty float z\n") + if have_color: + fh.write("property uchar red\nproperty uchar green\nproperty uchar blue\n") + fh.write("end_header\n") + if have_color: + cols = np.clip(np.asarray(colors), 0.0, 1.0) + cols = (cols * 255).astype(np.uint8) + for p, c in zip(points, cols): + fh.write(f"{p[0]:.6f} {p[1]:.6f} {p[2]:.6f} {int(c[0])} {int(c[1])} {int(c[2])}\n") + else: + for p in points: + fh.write(f"{p[0]:.6f} {p[1]:.6f} {p[2]:.6f}\n") + + +def write_metadata_json(meta: dict[str, Any], path: str | Path) -> Path: + path = Path(path) + path.parent.mkdir(parents=True, exist_ok=True) + with path.open("w", encoding="utf-8") as fh: + json.dump(meta, fh, ensure_ascii=False, indent=2, default=_json_default) + return path + + +def _json_default(obj: Any) -> Any: + if isinstance(obj, np.ndarray): + return obj.tolist() + if isinstance(obj, (np.floating, np.integer)): + return obj.item() + raise TypeError(f"not JSON serialisable: {type(obj).__name__}") + + +__all__ = [ + "load_point_cloud", + "write_point_cloud", + "write_metadata_json", +] diff --git a/alg/delivery_v1.1/coupler_panel_release/coupler_panel/metrics/__init__.py b/alg/delivery_v1.1/coupler_panel_release/coupler_panel/metrics/__init__.py new file mode 100644 index 0000000..369a3dc --- /dev/null +++ b/alg/delivery_v1.1/coupler_panel_release/coupler_panel/metrics/__init__.py @@ -0,0 +1,14 @@ +"""Deflection-angle and maximum-deviation computations + GT comparison.""" + +from .deflection import ( + DeflectionMetrics, + compute_deflection_metrics, +) +from .compare import compare_to_gt, GTComparison + +__all__ = [ + "DeflectionMetrics", + "compute_deflection_metrics", + "compare_to_gt", + "GTComparison", +] diff --git a/alg/delivery_v1.1/coupler_panel_release/coupler_panel/metrics/compare.py b/alg/delivery_v1.1/coupler_panel_release/coupler_panel/metrics/compare.py new file mode 100644 index 0000000..47c8eae --- /dev/null +++ b/alg/delivery_v1.1/coupler_panel_release/coupler_panel/metrics/compare.py @@ -0,0 +1,60 @@ +"""Compare measured deflection against the GT (CAD-derived) values.""" + +from __future__ import annotations + +from dataclasses import dataclass + +from .deflection import DeflectionMetrics + + +@dataclass(frozen=True) +class GTComparison: + measured: DeflectionMetrics + gt: DeflectionMetrics + + @property + def d_theta_right_deg(self) -> float: + return self.measured.theta_right_deg - self.gt.theta_right_deg + + @property + def d_theta_ref_deg(self) -> float: + return self.measured.theta_ref_deg - self.gt.theta_ref_deg + + @property + def d_D_y_max_mm(self) -> float: + return self.measured.D_y_max_mm - self.gt.D_y_max_mm + + @property + def d_D_z_max_mm(self) -> float: + return self.measured.D_z_max_mm - self.gt.D_z_max_mm + + @property + def d_dx_y_max_mm(self) -> float: + return self.measured.dx_y_max_mm - self.gt.dx_y_max_mm + + @property + def d_dx_z_max_mm(self) -> float: + return self.measured.dx_z_max_mm - self.gt.dx_z_max_mm + + def to_dict(self) -> dict[str, float | dict]: + return { + "measured": self.measured.to_dict(), + "gt": self.gt.to_dict(), + "diff": { + "theta_right_deg": self.d_theta_right_deg, + "theta_ref_deg": self.d_theta_ref_deg, + "D_y_max_mm": self.d_D_y_max_mm, + "D_z_max_mm": self.d_D_z_max_mm, + "dx_y_max_mm": self.d_dx_y_max_mm, + "dx_z_max_mm": self.d_dx_z_max_mm, + "abs_theta_right_deg": abs(self.d_theta_right_deg), + "abs_theta_ref_deg": abs(self.d_theta_ref_deg), + }, + } + + +def compare_to_gt(measured: DeflectionMetrics, gt: DeflectionMetrics) -> GTComparison: + return GTComparison(measured=measured, gt=gt) + + +__all__ = ["GTComparison", "compare_to_gt"] diff --git a/alg/delivery_v1.1/coupler_panel_release/coupler_panel/metrics/deflection.py b/alg/delivery_v1.1/coupler_panel_release/coupler_panel/metrics/deflection.py new file mode 100644 index 0000000..41a69df --- /dev/null +++ b/alg/delivery_v1.1/coupler_panel_release/coupler_panel/metrics/deflection.py @@ -0,0 +1,103 @@ +"""Deflection-angle and maximum-deviation formulas. + +For a fitted panel-plane normal :math:`(n_x, n_y, n_z)` with +:math:`n_x > 0`, the spec is:: + + theta_right_deg = atan2(n_y, n_x) * 180/pi (right-axis sign applied) + theta_ref_deg = atan2(n_z, n_x) * 180/pi + A = -n_y / n_x + B = -n_z / n_x + D_y_max = max |y_i - C_y| + D_z_max = max |z_i - C_z| + dx_y_max = |A| * D_y_max + dx_z_max = |B| * D_z_max + +``points`` should already be in the *world / CAD* frame, i.e. after the +4-hole Kabsch registration, and clipped to the panel ROI so the maxima +do not pick up edge or chamfer pixels. +""" + +from __future__ import annotations + +from dataclasses import dataclass +from typing import Any + +import numpy as np + + +@dataclass(frozen=True) +class DeflectionMetrics: + theta_right_deg: float + theta_ref_deg: float + D_y_max_mm: float + D_z_max_mm: float + dx_y_max_mm: float + dx_z_max_mm: float + A: float # -n_y / n_x + B: float # -n_z / n_x + + def to_dict(self) -> dict[str, Any]: + return { + "theta_right_deg": self.theta_right_deg, + "theta_ref_deg": self.theta_ref_deg, + "D_y_max_mm": self.D_y_max_mm, + "D_z_max_mm": self.D_z_max_mm, + "dx_y_max_mm": self.dx_y_max_mm, + "dx_z_max_mm": self.dx_z_max_mm, + "A": self.A, + "B": self.B, + } + + +def compute_deflection_metrics( + *, + normal: tuple[float, float, float] | np.ndarray, + center_C: tuple[float, float, float] | np.ndarray, + panel_points: np.ndarray, + right_axis_sign: int = 1, + angle_clamp_deg: float = 0.0, +) -> DeflectionMetrics: + """Compute the six measurands.""" + n = np.asarray(normal, dtype=np.float64).reshape(3) + if n[0] <= 0: + # Caller is expected to enforce n_x > 0 before this; we hard-fail + # because every formula divides by n_x. + raise ValueError( + f"compute_deflection_metrics requires n_x > 0; got n = {tuple(n)}" + ) + C = np.asarray(center_C, dtype=np.float64).reshape(3) + pts = np.asarray(panel_points, dtype=np.float64) + if pts.ndim != 2 or pts.shape[1] != 3: + raise ValueError(f"panel_points must be (N,3); got {pts.shape}") + if pts.shape[0] == 0: + raise ValueError("panel_points is empty") + + sign = 1.0 if int(right_axis_sign) >= 0 else -1.0 + + theta_right = sign * float(np.degrees(np.arctan2(n[1], n[0]))) + theta_ref = float(np.degrees(np.arctan2(n[2], n[0]))) + + if abs(theta_right) < float(angle_clamp_deg): + theta_right = 0.0 + if abs(theta_ref) < float(angle_clamp_deg): + theta_ref = 0.0 + + A = float(-n[1] / n[0]) + B = float(-n[2] / n[0]) + + D_y_max = float(np.max(np.abs(pts[:, 1] - C[1]))) + D_z_max = float(np.max(np.abs(pts[:, 2] - C[2]))) + + return DeflectionMetrics( + theta_right_deg=theta_right, + theta_ref_deg=theta_ref, + D_y_max_mm=D_y_max, + D_z_max_mm=D_z_max, + dx_y_max_mm=abs(A) * D_y_max, + dx_z_max_mm=abs(B) * D_z_max, + A=A, + B=B, + ) + + +__all__ = ["DeflectionMetrics", "compute_deflection_metrics"] diff --git a/alg/delivery_v1.1/coupler_panel_release/coupler_panel/pipeline.py b/alg/delivery_v1.1/coupler_panel_release/coupler_panel/pipeline.py new file mode 100644 index 0000000..a1f0e0a --- /dev/null +++ b/alg/delivery_v1.1/coupler_panel_release/coupler_panel/pipeline.py @@ -0,0 +1,307 @@ +"""End-to-end orchestration for *research / validation* runs. + +Composition:: + + data_prep.prepare_cloud_from_cad / from_scan (Phase A) + api.compute_coupler_measurement (Phase B) + +Phase A is the validation / data-preparation part: STP → mesh → sample +→ noise → transform → denoise → register. Phase B is the public +measurement algorithm. This module saves every intermediate artifact +under a structured run directory so engineers can debug and so the +sweep runner can compare runs. + +Production callers should *not* use this module — they should call +:func:`coupler_panel.api.compute_coupler_measurement` directly with +their own clean point cloud. +""" + +from __future__ import annotations + +from pathlib import Path +from typing import Any + +import numpy as np + +from .api import compute_coupler_measurement, ProgressCallback +from .config import load_config, resolve_auto +from .data_prep import ( + PreparedCloud, + denoise_cloud, + parse_cad, + prepare_cloud_from_cad, + prepare_cloud_from_scan, + register_cloud, + synthesize_cloud, +) +from .io_utils import ( + Artifacts, + load_point_cloud, + write_json, + write_point_cloud, + write_yaml, +) +from .metrics import compute_deflection_metrics +from .roi_select import ROISpec, apply_roi, default_roi_spec, load_roi_json +from .structures import CouplerMeasurement + + +# --------------------------------------------------------------------------- +# Cloud-only research entry point +# --------------------------------------------------------------------------- + + +def run_coupler_measurement_pipeline( + input_cloud: str | Path, + out_dir: str | Path, + *, + config_path: str | Path | None = None, + profile: str = "auto", + cad_stp_path: str | Path | None = None, + roi_spec: ROISpec | None = None, + roi_json: str | Path | None = None, + progress: ProgressCallback | None = None, +) -> CouplerMeasurement: + """Main measurement entry: cloud (in CAD frame) → ROI → denoise → measure. + + **No registration step.** The input cloud is assumed to already live + in the CAD / world frame (scanner extrinsics calibrated upstream). + If it doesn't, 4-hole Kabsch against nominal CAD holes would also + cancel the panel-deflection signal we want to measure — so this + pipeline deliberately does not register. For the research path that + *does* register (using oracle deflected GT), see + :func:`run_from_cad_pipeline`. + + Inputs: + + - ``cad_stp_path`` — optional STEP file. Parsed for the analytical + panel-normal seed (speeds up + stabilises hole detection's + in-plane projection); the parsed GT is written as ``01_gt.json`` + to ``out_dir`` for auditing. + - ``roi_spec`` / ``roi_json`` — ROI selector applied to the cloud + *before* denoising. If both omitted, the default ``mode='all'`` + spec (pass-through) is used. ``roi_spec`` wins over ``roi_json``. + + Writes every intermediate to ``out_dir``. Returns a + :class:`CouplerMeasurement`. + """ + art = Artifacts(out_dir) + cfg = load_config(config_path, profile=profile) + cfg = resolve_auto(cfg) + write_yaml(cfg, art.resolved_config) + + # ---- Load + ROI clip -------------------------------------------------- + raw_pts = load_point_cloud(input_cloud) + if roi_spec is None: + roi_spec = load_roi_json(roi_json) if roi_json is not None else default_roi_spec() + pts, roi_diag = apply_roi(raw_pts, roi_spec) + write_point_cloud(pts, art.path("sampled", ext="ply")) + write_json({"spec": roi_spec.to_dict(), "diagnostics": roi_diag}, art.root / "roi.json") + + # ---- Optional CAD parse (panel_normal_seed + audit GT JSON) ---------- + panel_seed: np.ndarray | None = None + if cad_stp_path is not None: + parsed = parse_cad(cad_stp_path, cfg["cad"]) + write_json(parsed.gt.to_dict(), art.path("gt", ext="json")) + panel_seed = np.asarray(parsed.gt.plane.normal, dtype=np.float64) + + # ---- Denoise (no Kabsch, no prepare_cloud_from_scan) ----------------- + denoised = denoise_cloud(pts, cfg.get("denoise", {})) + write_point_cloud(denoised.points, art.path("denoised", ext="ply")) + + # ---- Measurement ------------------------------------------------------ + result = compute_coupler_measurement( + denoised.points, + config=cfg, + profile="custom", + panel_normal_seed=panel_seed, + progress=progress, + ) + + write_json(result.to_dict(), art.report_json) + write_json(result.to_dict()["report"], art.path("metrics", ext="json")) + return result + + +# --------------------------------------------------------------------------- +# CAD-driven research entry point +# --------------------------------------------------------------------------- + + +def run_from_cad_pipeline( + stp_path: str | Path, + out_dir: str | Path, + *, + config_path: str | Path | None = None, + profile: str = "auto", + progress: ProgressCallback | None = None, +) -> dict[str, Any]: + """End-to-end research run: STP → synthesise → prepare → measure → diff GT. + + Returns a summary dict containing the measurement, GT metrics, and + measured-minus-GT differences. Writes every intermediate cloud and + JSON to ``out_dir``. + """ + art = Artifacts(out_dir) + cfg = load_config(config_path, profile=profile) + cfg = resolve_auto(cfg) + write_yaml(cfg, art.resolved_config) + + # ---- Phase A.1 — parse + GT ------------------------------------------- + parsed = parse_cad(stp_path, cfg["cad"]) + write_point_cloud(parsed.mesh.vertices, art.path("cad_mesh", ext="ply")) + write_json(parsed.gt.to_dict(), art.path("gt", ext="json")) + + # ---- Phase A.2 — synthesise -------------------------------------------- + synth = synthesize_cloud( + parsed, + sampling_cfg=cfg["sampling"], + noise_cfg=cfg.get("noise", {}), + transform_cfg=cfg.get("transform", {}), + scan_cfg=cfg.get("scan_simulator", {}), + deflection_cfg=cfg.get("panel_deflection", {}), + ) + write_point_cloud(synth.sampled, art.path("sampled", ext="ply")) + write_point_cloud(synth.noisy, art.path("noisy", ext="ply")) + write_point_cloud(synth.transformed, art.path("transformed", ext="ply")) + + # ---- Phase A.3 — denoise ---------------------------------------------- + denoised = denoise_cloud(synth.transformed, cfg.get("denoise", {})) + write_point_cloud(denoised.points, art.path("denoised", ext="ply")) + + # ---- Phase A.4 — register --------------------------------------------- + # Use PCA on the cloud itself for the projection seed (the cloud is + # still in the pre-Kabsch frame here; using the CAD normal would + # cause foreshortening bias). Register against the *deflected* GT + # holes so Kabsch only cancels scanner-pose jitter, not the panel + # deflection signal we want to measure. + registered = register_cloud( + denoised.points, + target_holes=synth.gt_holes_after_deflection, + detection_cfg=cfg["detection"], + cad_cfg=cfg["cad"], + registration_cfg=cfg["registration"], + panel_normal_seed=None, + ) + write_point_cloud(registered.points, art.path("registered", ext="ply")) + + # ---- Phase B — measurement algorithm ---------------------------------- + result = compute_coupler_measurement( + registered.points, + config=cfg, + profile="custom", + panel_normal_seed=np.asarray(parsed.gt.plane.normal, dtype=np.float64), + progress=progress, + ) + + # ---- GT metrics -------------------------------------------------------- + # GT for the metrics is the *deflected* panel: angles equal the + # injection, and the maximum-distance / maximum-x-deviation values are + # computed over the deflected 4-hole rectangle (the same primitive the + # algorithm later sees through the noise + jitter chain). + gt_normal = synth.gt_normal_after_deflection + centers = synth.gt_holes_after_deflection.locating_centers() + gt_metrics = compute_deflection_metrics( + normal=gt_normal, + center_C=tuple(centers.mean(axis=0).tolist()), + panel_points=centers, + right_axis_sign=int(cfg["metrics"].get("right_axis_sign", 1)), + angle_clamp_deg=float(cfg["metrics"].get("angle_clamp_deg", 0.0)), + ) + + diff = { + "theta_right_deg": result.theta_right_deg - gt_metrics.theta_right_deg, + "theta_ref_deg": result.theta_ref_deg - gt_metrics.theta_ref_deg, + "D_y_max_mm": result.D_y_max_mm - gt_metrics.D_y_max_mm, + "D_z_max_mm": result.D_z_max_mm - gt_metrics.D_z_max_mm, + "dx_y_max_mm": result.dx_y_max_mm - gt_metrics.dx_y_max_mm, + "dx_z_max_mm": result.dx_z_max_mm - gt_metrics.dx_z_max_mm, + } + + summary = { + "stp": str(stp_path), + "panel_deflection_injected": { + "theta_right_deg": synth.panel_deflection_theta_right_deg, + "theta_ref_deg": synth.panel_deflection_theta_ref_deg, + "R": synth.panel_deflection_R.tolist(), + }, + "scan_pose_perturbation": synth.gt_transform_4x4.tolist(), + "registration": { + "matrix": registered.transform_4x4.tolist(), + "rmse_mm": registered.rmse_mm, + "max_residual_mm": registered.max_residual_mm, + }, + "measured": result.to_dict(), + "gt_cad": parsed.gt.to_dict(), + "gt_after_deflection": { + "normal": gt_normal.tolist(), + "locating_centers": centers.tolist(), + }, + "gt_metrics": gt_metrics.to_dict(), + "diff": diff, + "config": cfg, + } + write_json(summary, art.path("metrics", ext="json")) + write_json(summary, art.report_json) + + return summary + + +# --------------------------------------------------------------------------- +# Helpers +# --------------------------------------------------------------------------- + + +def _try_load_normal_from_gt_json(path: str | Path) -> np.ndarray | None: + """Read ``plane.normal`` from a GT JSON if present. Returns ``None`` otherwise.""" + import json + + try: + with Path(path).open("r", encoding="utf-8") as fh: + d = json.load(fh) + except Exception: + return None + n = d.get("plane", {}).get("normal") + if n is None or len(n) != 3: + return None + arr = np.asarray(n, dtype=np.float64) + norm = float(np.linalg.norm(arr)) + if norm < 1e-9: + return None + return arr / norm + + +def _load_holeset_from_json(path: str | Path): + """Reconstruct a HoleSet from the JSON written by ``GroundTruth.to_dict``.""" + import json + + from .structures import Hole, HoleSet + + with Path(path).open("r", encoding="utf-8") as fh: + d = json.load(fh) + holes = d["holes"] + locating = tuple( + Hole( + center=tuple(h["center"]), + radius_mm=float(h["radius_mm"]), + fit_residual_rms_mm=float(h.get("fit_residual_rms_mm", 0.0)), + role=h.get("role", "locating"), + ) + for h in holes["locating"] + ) + central = None + if holes.get("central") is not None: + c = holes["central"] + central = Hole( + center=tuple(c["center"]), + radius_mm=float(c["radius_mm"]), + fit_residual_rms_mm=float(c.get("fit_residual_rms_mm", 0.0)), + role="central", + ) + return HoleSet(locating=locating, central=central) + + +__all__ = [ + "run_coupler_measurement_pipeline", + "run_from_cad_pipeline", +] diff --git a/alg/delivery_v1.1/coupler_panel_release/coupler_panel/py.typed b/alg/delivery_v1.1/coupler_panel_release/coupler_panel/py.typed new file mode 100644 index 0000000..e69de29 diff --git a/alg/delivery_v1.1/coupler_panel_release/coupler_panel/registration/__init__.py b/alg/delivery_v1.1/coupler_panel_release/coupler_panel/registration/__init__.py new file mode 100644 index 0000000..29afcee --- /dev/null +++ b/alg/delivery_v1.1/coupler_panel_release/coupler_panel/registration/__init__.py @@ -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", +] diff --git a/alg/delivery_v1.1/coupler_panel_release/coupler_panel/registration/kabsch.py b/alg/delivery_v1.1/coupler_panel_release/coupler_panel/registration/kabsch.py new file mode 100644 index 0000000..0f21452 --- /dev/null +++ b/alg/delivery_v1.1/coupler_panel_release/coupler_panel/registration/kabsch.py @@ -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"] diff --git a/alg/delivery_v1.1/coupler_panel_release/coupler_panel/reporting/__init__.py b/alg/delivery_v1.1/coupler_panel_release/coupler_panel/reporting/__init__.py new file mode 100644 index 0000000..ab0b470 --- /dev/null +++ b/alg/delivery_v1.1/coupler_panel_release/coupler_panel/reporting/__init__.py @@ -0,0 +1,5 @@ +"""Batch-evaluation reporting helpers (xlsx output).""" + +from .excel import EvalRow, write_excel_report, build_row_from_summary + +__all__ = ["EvalRow", "write_excel_report", "build_row_from_summary"] diff --git a/alg/delivery_v1.1/coupler_panel_release/coupler_panel/reporting/excel.py b/alg/delivery_v1.1/coupler_panel_release/coupler_panel/reporting/excel.py new file mode 100644 index 0000000..27feb5f --- /dev/null +++ b/alg/delivery_v1.1/coupler_panel_release/coupler_panel/reporting/excel.py @@ -0,0 +1,285 @@ +"""Aggregate batch-eval results into an xlsx report. + +Each row corresponds to one input PLY × one ROI JSON × one run. + +Columns (kept stable across releases unless the field is removed from +the upstream report): + + sample_id, ply_path, roi_kind, + theta_right_gt_deg, theta_ref_gt_deg, + theta_right_pred_deg, theta_ref_pred_deg, + theta_right_err_deg, theta_ref_err_deg, + D_y_max_mm, D_z_max_mm, + dx_y_max_mm, dx_z_max_mm, + center_C_x_mm, center_C_y_mm, center_C_z_mm, + normal_x, normal_y, normal_z, + plane_inlier_ratio, plane_residual_rms_mm, + registration_rmse_mm, registration_max_residual_mm, + n_points_in, n_points_after_roi, + quality_flags, + pipeline_seconds, inner_stage_seconds, + algorithm_version, schema_version + +``pipeline_seconds`` is the **wall-clock** time from the moment the +inputs (cloud PLY + ROI JSON, optionally CAD STP) enter +``run_coupler_measurement_pipeline`` to the moment the +``CouplerMeasurement`` is returned — i.e. the algorithm's total +processing time per sample. + +``inner_stage_seconds`` is the older, narrower number: the sum of +``StageTiming.seconds`` recorded inside ``compute_coupler_measurement`` +itself (validate → detect_holes → … → metrics), excluding file IO, +cloud load, ROI apply, denoise, and any CAD STP parse. It is kept for +per-stage debugging only. +""" + +from __future__ import annotations + +from dataclasses import asdict, dataclass, field +from pathlib import Path +from typing import Any + +try: + from openpyxl import Workbook + from openpyxl.styles import Alignment, Font, PatternFill +except ImportError as exc: # pragma: no cover + raise ImportError( + "openpyxl is required for Excel reporting; install with `pip install openpyxl`" + ) from exc + + +# --------------------------------------------------------------------------- +# Row schema +# --------------------------------------------------------------------------- + + +COLUMNS: tuple[tuple[str, str], ...] = ( + ("sample_id", "Sample ID"), + ("ply_path", "Input PLY"), + ("roi_kind", "ROI kind"), + ("theta_right_gt_deg", "θ_right GT (°)"), + ("theta_ref_gt_deg", "θ_ref GT (°)"), + ("theta_right_pred_deg", "θ_right Pred (°)"), + ("theta_ref_pred_deg", "θ_ref Pred (°)"), + ("theta_right_err_deg", "θ_right Err (°)"), + ("theta_ref_err_deg", "θ_ref Err (°)"), + ("D_y_max_mm", "D_y_max (mm)"), + ("D_z_max_mm", "D_z_max (mm)"), + ("dx_y_max_mm", "dx_y_max (mm)"), + ("dx_z_max_mm", "dx_z_max (mm)"), + ("center_C_x_mm", "C_x (mm)"), + ("center_C_y_mm", "C_y (mm)"), + ("center_C_z_mm", "C_z (mm)"), + ("normal_x", "n_x"), + ("normal_y", "n_y"), + ("normal_z", "n_z"), + ("plane_inlier_ratio", "plane inlier ratio"), + ("plane_residual_rms_mm", "plane res RMS (mm)"), + ("registration_rmse_mm", "Kabsch RMSE (mm)"), + ("registration_max_residual_mm", "Kabsch max res (mm)"), + ("n_points_in", "N in"), + ("n_points_after_roi", "N after ROI"), + ("quality_flags", "quality_flags"), + ("pipeline_seconds", "Algorithm runtime (s)"), + ("inner_stage_seconds", "compute_*_measurement (s)"), + ("algorithm_version", "algo ver"), + ("schema_version", "schema ver"), +) + + +@dataclass +class EvalRow: + """One row in the Excel report (one PLY × one ROI × one config).""" + + sample_id: str + ply_path: str + roi_kind: str + theta_right_gt_deg: float | None + theta_ref_gt_deg: float | None + theta_right_pred_deg: float + theta_ref_pred_deg: float + theta_right_err_deg: float | None + theta_ref_err_deg: float | None + D_y_max_mm: float + D_z_max_mm: float + dx_y_max_mm: float + dx_z_max_mm: float + center_C_x_mm: float + center_C_y_mm: float + center_C_z_mm: float + normal_x: float + normal_y: float + normal_z: float + plane_inlier_ratio: float + plane_residual_rms_mm: float + registration_rmse_mm: float | None + registration_max_residual_mm: float | None + n_points_in: int + n_points_after_roi: int + quality_flags: str + pipeline_seconds: float + inner_stage_seconds: float + algorithm_version: str + schema_version: str + extra: dict[str, Any] = field(default_factory=dict) + + def to_dict(self) -> dict[str, Any]: + d = asdict(self) + d.pop("extra", None) + return d + + +# --------------------------------------------------------------------------- +# Row construction from pipeline outputs +# --------------------------------------------------------------------------- + + +def build_row_from_summary( + *, + sample_id: str, + ply_path: str | Path, + roi_kind: str, + measured: dict[str, Any], + gt_angles: tuple[float, float] | None, + registration: dict[str, Any] | None, + n_points_in: int, + n_points_after_roi: int, + pipeline_seconds: float, +) -> EvalRow: + """Construct an :class:`EvalRow` from the pipeline's report dict. + + ``measured`` is the ``CouplerMeasurement.to_dict()`` payload. It + embeds ``report`` (the inner dict ``compute_coupler_measurement`` + wrote). ``gt_angles`` is the injected ``(theta_right_deg, + theta_ref_deg)`` or ``None`` when not known. + + ``pipeline_seconds`` should be the wall-clock duration of the + ``run_coupler_measurement_pipeline`` call — i.e. the algorithm's + end-to-end runtime per sample. Caller is responsible for timing. + """ + report = measured.get("report", {}) + plane = report.get("plane", {}) + timings = report.get("timings", []) + inner_s = float(sum(t.get("seconds", 0.0) for t in timings)) + + th_r_pred = float(measured["theta_right_deg"]) + th_z_pred = float(measured["theta_ref_deg"]) + if gt_angles is None: + th_r_gt = th_z_gt = None + th_r_err = th_z_err = None + else: + th_r_gt = float(gt_angles[0]) + th_z_gt = float(gt_angles[1]) + th_r_err = th_r_pred - th_r_gt + th_z_err = th_z_pred - th_z_gt + + cx, cy, cz = measured["center_C"] + nx, ny, nz = measured["normal"] + + return EvalRow( + sample_id=sample_id, + ply_path=str(ply_path), + roi_kind=roi_kind, + theta_right_gt_deg=th_r_gt, + theta_ref_gt_deg=th_z_gt, + theta_right_pred_deg=th_r_pred, + theta_ref_pred_deg=th_z_pred, + theta_right_err_deg=th_r_err, + theta_ref_err_deg=th_z_err, + D_y_max_mm=float(measured["D_y_max_mm"]), + D_z_max_mm=float(measured["D_z_max_mm"]), + dx_y_max_mm=float(measured["dx_y_max_mm"]), + dx_z_max_mm=float(measured["dx_z_max_mm"]), + center_C_x_mm=float(cx), + center_C_y_mm=float(cy), + center_C_z_mm=float(cz), + normal_x=float(nx), + normal_y=float(ny), + normal_z=float(nz), + plane_inlier_ratio=float(plane.get("inlier_ratio", float("nan"))), + plane_residual_rms_mm=float(plane.get("inlier_residual_rms_mm", float("nan"))), + registration_rmse_mm=(None if registration is None else float(registration.get("rmse_mm", float("nan")))), + registration_max_residual_mm=( + None + if registration is None + else float(registration.get("max_residual_mm", float("nan"))) + ), + n_points_in=int(n_points_in), + n_points_after_roi=int(n_points_after_roi), + quality_flags=";".join(measured.get("quality_flags", [])) or "", + pipeline_seconds=float(pipeline_seconds), + inner_stage_seconds=inner_s, + algorithm_version=str(measured.get("algorithm_version", "")), + schema_version=str(measured.get("schema_version", "")), + ) + + +# --------------------------------------------------------------------------- +# Excel writer +# --------------------------------------------------------------------------- + + +def write_excel_report(rows: list[EvalRow], path: str | Path) -> Path: + """Write rows to an xlsx file. Returns the path.""" + out = Path(path) + out.parent.mkdir(parents=True, exist_ok=True) + + wb = Workbook() + ws = wb.active + ws.title = "results" + + header_font = Font(bold=True) + header_fill = PatternFill("solid", fgColor="DDDDDD") + center = Alignment(horizontal="center") + + # Header row. + for col_idx, (_, label) in enumerate(COLUMNS, start=1): + cell = ws.cell(row=1, column=col_idx, value=label) + cell.font = header_font + cell.fill = header_fill + cell.alignment = center + + # Data rows. + for r_idx, row in enumerate(rows, start=2): + d = row.to_dict() + for c_idx, (key, _) in enumerate(COLUMNS, start=1): + v = d.get(key) + if isinstance(v, float) and (v != v): # NaN + v = "" + ws.cell(row=r_idx, column=c_idx, value=v) + + # Number formatting for float-typed cells. + float_keys = { + "theta_right_gt_deg", "theta_ref_gt_deg", + "theta_right_pred_deg", "theta_ref_pred_deg", + "theta_right_err_deg", "theta_ref_err_deg", + "D_y_max_mm", "D_z_max_mm", "dx_y_max_mm", "dx_z_max_mm", + "center_C_x_mm", "center_C_y_mm", "center_C_z_mm", + "normal_x", "normal_y", "normal_z", + "plane_inlier_ratio", "plane_residual_rms_mm", + "registration_rmse_mm", "registration_max_residual_mm", + "pipeline_seconds", "inner_stage_seconds", + } + for c_idx, (key, _) in enumerate(COLUMNS, start=1): + if key in float_keys: + for r_idx in range(2, len(rows) + 2): + ws.cell(row=r_idx, column=c_idx).number_format = "0.000000" + + # Auto-width (cap at 32). + for c_idx, (_, label) in enumerate(COLUMNS, start=1): + col_letter = ws.cell(row=1, column=c_idx).column_letter + max_len = len(str(label)) + for r_idx in range(2, len(rows) + 2): + v = ws.cell(row=r_idx, column=c_idx).value + if v is None: + continue + max_len = max(max_len, len(str(v))) + ws.column_dimensions[col_letter].width = min(max_len + 2, 32) + + ws.freeze_panes = "D2" + + wb.save(out) + return out + + +__all__ = ["EvalRow", "COLUMNS", "build_row_from_summary", "write_excel_report"] diff --git a/alg/delivery_v1.1/coupler_panel_release/coupler_panel/roi_select/__init__.py b/alg/delivery_v1.1/coupler_panel_release/coupler_panel/roi_select/__init__.py new file mode 100644 index 0000000..4258ec3 --- /dev/null +++ b/alg/delivery_v1.1/coupler_panel_release/coupler_panel/roi_select/__init__.py @@ -0,0 +1,25 @@ +"""Region-of-interest pre-filter for the input point cloud. + +The main pipeline takes a *raw* point cloud (PLY) plus a ROI JSON +descriptor (an axis-aligned bounding box in the CAD frame) that tells +which subset of the cloud should actually feed the downstream algorithm. + +ROI JSON layout — six flat keys, all in mm:: + + { + "x_min": -12500.0, "x_max": 12500.0, + "y_min": 150.0, "y_max": 4100.0, + "z_min": -1680.0, "z_max": 200.0 + } +""" + +from .schema import ROISpec, default_roi_spec, load_roi_json, validate_roi_spec +from .apply import apply_roi + +__all__ = [ + "ROISpec", + "default_roi_spec", + "load_roi_json", + "validate_roi_spec", + "apply_roi", +] diff --git a/alg/delivery_v1.1/coupler_panel_release/coupler_panel/roi_select/apply.py b/alg/delivery_v1.1/coupler_panel_release/coupler_panel/roi_select/apply.py new file mode 100644 index 0000000..6eaece5 --- /dev/null +++ b/alg/delivery_v1.1/coupler_panel_release/coupler_panel/roi_select/apply.py @@ -0,0 +1,48 @@ +"""Apply an axis-aligned bounding-box :class:`ROISpec` to a point cloud.""" + +from __future__ import annotations + +from typing import Any + +import numpy as np + +from .schema import ROISpec + + +def apply_roi(points: np.ndarray, spec: ROISpec) -> tuple[np.ndarray, dict[str, Any]]: + """Filter ``points`` by the axis-aligned bounding box in ``spec``. + + Returns ``(filtered_points, diagnostics)``. ``diagnostics`` is a small + JSON-friendly dict suitable for embedding in a run report. + """ + pts = np.asarray(points, dtype=np.float64) + n_in = int(pts.shape[0]) + + if n_in == 0: + return pts.copy(), { + "kind": spec.kind, + "n_in": 0, + "n_out": 0, + "kept_ratio": 0.0, + "bounds": spec.to_dict(), + } + + x, y, z = pts[:, 0], pts[:, 1], pts[:, 2] + mask = ( + (x >= spec.x_min) & (x <= spec.x_max) + & (y >= spec.y_min) & (y <= spec.y_max) + & (z >= spec.z_min) & (z <= spec.z_max) + ) + out = pts[mask] + n_out = int(out.shape[0]) + + return out, { + "kind": spec.kind, + "n_in": n_in, + "n_out": n_out, + "kept_ratio": float(n_out / n_in), + "bounds": spec.to_dict(), + } + + +__all__ = ["apply_roi"] diff --git a/alg/delivery_v1.1/coupler_panel_release/coupler_panel/roi_select/schema.py b/alg/delivery_v1.1/coupler_panel_release/coupler_panel/roi_select/schema.py new file mode 100644 index 0000000..91358c7 --- /dev/null +++ b/alg/delivery_v1.1/coupler_panel_release/coupler_panel/roi_select/schema.py @@ -0,0 +1,127 @@ +"""ROI JSON schema definition + loader. + +The ROI descriptor is a **flat axis-aligned bounding box** in the CAD +frame. Six numbers, no nesting: + + { + "x_min": -12500.0, + "x_max": 12500.0, + "y_min": 150.0, + "y_max": 4100.0, + "z_min": -1680.0, + "z_max": 200.0 + } + +Semantics +--------- + +- All units in **mm**, all coordinates in the CAD frame (x = longitudinal, + y = width, z = height). +- A point ``(x, y, z)`` is kept iff:: + + x_min <= x <= x_max and y_min <= y <= y_max and z_min <= z <= z_max + +- To pass the whole cloud through unchanged, supply bounds wider than the + cloud's own bbox (or use :func:`default_roi_spec`, which returns + ``±∞`` on every axis). +""" + +from __future__ import annotations + +import json +import math +from dataclasses import dataclass +from pathlib import Path +from typing import Any + + +@dataclass +class ROISpec: + """Axis-aligned bounding box in CAD-frame mm.""" + + x_min: float + x_max: float + y_min: float + y_max: float + z_min: float + z_max: float + + @property + def kind(self) -> str: + """Stable identifier for logging / reporting.""" + return "aabb" + + def to_dict(self) -> dict[str, float]: + return { + "x_min": float(self.x_min), + "x_max": float(self.x_max), + "y_min": float(self.y_min), + "y_max": float(self.y_max), + "z_min": float(self.z_min), + "z_max": float(self.z_max), + } + + +def default_roi_spec() -> ROISpec: + """Pass-through ROI: infinite bounds on every axis.""" + inf = math.inf + return ROISpec( + x_min=-inf, x_max=inf, + y_min=-inf, y_max=inf, + z_min=-inf, z_max=inf, + ) + + +def validate_roi_spec(spec: ROISpec) -> None: + """Raise ``ValueError`` if any axis has ``min >= max``.""" + pairs = ( + ("x", spec.x_min, spec.x_max), + ("y", spec.y_min, spec.y_max), + ("z", spec.z_min, spec.z_max), + ) + for axis, lo, hi in pairs: + if not (lo < hi): + raise ValueError( + f"ROI {axis}_min must be strictly less than {axis}_max " + f"(got {axis}_min={lo}, {axis}_max={hi})" + ) + + +def load_roi_json(path: str | Path | None) -> ROISpec: + """Read a ROI JSON file. ``None`` → default pass-through spec. + + The file must contain all six keys ``x_min`` / ``x_max`` / ``y_min`` / + ``y_max`` / ``z_min`` / ``z_max``. Extra keys are ignored (so you + can attach a ``comment`` etc. without the loader complaining). + """ + if path is None: + return default_roi_spec() + path = Path(path) + if not path.exists(): + raise FileNotFoundError(f"roi.json not found: {path}") + with path.open("r", encoding="utf-8") as fh: + data: dict[str, Any] = json.load(fh) + + required = ("x_min", "x_max", "y_min", "y_max", "z_min", "z_max") + missing = [k for k in required if k not in data] + if missing: + raise ValueError(f"roi.json {path} is missing required keys: {missing}") + + spec = ROISpec( + x_min=float(data["x_min"]), + x_max=float(data["x_max"]), + y_min=float(data["y_min"]), + y_max=float(data["y_max"]), + z_min=float(data["z_min"]), + z_max=float(data["z_max"]), + ) + validate_roi_spec(spec) + return spec + + +__all__ = [ + "ROISpec", + "default_roi_spec", + "validate_roi_spec", + "load_roi_json", +] diff --git a/alg/delivery_v1.1/coupler_panel_release/coupler_panel/sampling/__init__.py b/alg/delivery_v1.1/coupler_panel_release/coupler_panel/sampling/__init__.py new file mode 100644 index 0000000..a337eef --- /dev/null +++ b/alg/delivery_v1.1/coupler_panel_release/coupler_panel/sampling/__init__.py @@ -0,0 +1,61 @@ +"""Mesh-to-point-cloud sampling strategies. + +Unified entry point:: + + from coupler_panel.sampling import sample_mesh + pts = sample_mesh(mesh, method="poisson_disk", num_points=100_000) + +Each strategy lives in its own module; the dispatcher just routes by name. +``mesh`` must expose ``vertices: (V, 3)`` and ``faces: (F, 3)`` as ndarrays +(such as our :class:`coupler_panel.cad.mesh.TriangleMesh`). +""" + +from __future__ import annotations + +from typing import Any + +import numpy as np + +from .uniform_area import sample_uniform_area +from .poisson_disk import sample_poisson_disk +from .adaptive import sample_adaptive +from .grid_spacing import sample_grid_spacing, num_points_from_spacing + + +_DISPATCH = { + "uniform_area": sample_uniform_area, + "poisson_disk": sample_poisson_disk, + "adaptive": sample_adaptive, + "grid_spacing": sample_grid_spacing, +} + + +def sample_mesh(mesh, method: str, **kwargs) -> np.ndarray: + """Dispatch to the named strategy. ``**kwargs`` are passed through.""" + if method not in _DISPATCH: + raise ValueError( + f"unknown sampling method '{method}'; valid: {sorted(_DISPATCH)}" + ) + return _DISPATCH[method](mesh, **kwargs) + + +def resolve_num_points(mesh, *, num_points: int | None, spacing_mm: float | None) -> int: + """If ``spacing_mm`` is given, convert it to a point count using the + mesh surface area; otherwise return ``num_points``. + """ + if spacing_mm is not None and spacing_mm > 0: + return num_points_from_spacing(mesh, spacing_mm) + if num_points is None: + raise ValueError("either num_points or spacing_mm must be set") + return int(num_points) + + +__all__ = [ + "sample_mesh", + "sample_uniform_area", + "sample_poisson_disk", + "sample_adaptive", + "sample_grid_spacing", + "resolve_num_points", + "num_points_from_spacing", +] diff --git a/alg/delivery_v1.1/coupler_panel_release/coupler_panel/sampling/adaptive.py b/alg/delivery_v1.1/coupler_panel_release/coupler_panel/sampling/adaptive.py new file mode 100644 index 0000000..cea7000 --- /dev/null +++ b/alg/delivery_v1.1/coupler_panel_release/coupler_panel/sampling/adaptive.py @@ -0,0 +1,104 @@ +"""Curvature-adaptive sampling. + +Idea: weight every triangle by ``area * (1 + k * curvature_proxy)``, then +do area-weighted sampling on the *weighted* areas. The curvature proxy is +``1 - mean_dot(face_normal, neighbour_face_normals)``, normalised to +[0, 1] across the mesh. + +This produces denser samples on rounded edges, hole boundaries, and small +features without needing a real curvature operator. +""" + +from __future__ import annotations + +from typing import Any + +import numpy as np + +from .uniform_area import _triangle_areas, sample_uniform_area + + +def _face_normals(vertices: np.ndarray, faces: np.ndarray) -> np.ndarray: + a = vertices[faces[:, 0]] + b = vertices[faces[:, 1]] + c = vertices[faces[:, 2]] + n = np.cross(b - a, c - a) + norms = np.linalg.norm(n, axis=1, keepdims=True) + norms[norms < 1e-12] = 1.0 + return n / norms + + +def _vertex_face_adjacency(faces: np.ndarray, n_vertices: int) -> list[list[int]]: + adj: list[list[int]] = [[] for _ in range(n_vertices)] + for i, f in enumerate(faces): + adj[f[0]].append(i) + adj[f[1]].append(i) + adj[f[2]].append(i) + return adj + + +def _curvature_proxy(vertices: np.ndarray, faces: np.ndarray) -> np.ndarray: + """Per-face roughness in [0, 1].""" + fn = _face_normals(vertices, faces) + adj = _vertex_face_adjacency(faces, vertices.shape[0]) + proxy = np.zeros(faces.shape[0], dtype=np.float64) + for i, f in enumerate(faces): + neighbours: set[int] = set() + for v in f: + neighbours.update(adj[v]) + neighbours.discard(i) + if not neighbours: + continue + nb = fn[list(neighbours)] + d = np.clip(nb @ fn[i], -1.0, 1.0) + proxy[i] = 1.0 - np.mean(d) + if proxy.max() > 0: + proxy = proxy / proxy.max() + return proxy + + +def sample_adaptive( + mesh, + *, + num_points: int, + seed: int | None = 42, + curvature_weight: float = 1.0, + **_: Any, +) -> np.ndarray: + """Curvature-weighted sampling.""" + if num_points <= 0: + return np.empty((0, 3), dtype=np.float64) + + v = np.asarray(mesh.vertices, dtype=np.float64) + f = np.asarray(mesh.faces, dtype=np.int64) + if f.shape[0] == 0: + return np.empty((0, 3), dtype=np.float64) + + # On large meshes the O(F·avg_degree) curvature pass is expensive; if we + # are above ~200k faces, fall back to plain uniform_area, which is the + # natural definition when curvature_weight = 0. + if f.shape[0] > 200_000: + return sample_uniform_area(mesh, num_points=num_points, seed=seed) + + areas = _triangle_areas(v, f) + proxy = _curvature_proxy(v, f) + weights = areas * (1.0 + float(curvature_weight) * proxy) + total = weights.sum() + if total <= 0: + return sample_uniform_area(mesh, num_points=num_points, seed=seed) + + rng = np.random.default_rng(seed) + probs = weights / total + tri_idx = rng.choice(f.shape[0], size=num_points, p=probs) + u = rng.random(num_points) + w = rng.random(num_points) + flip = u + w > 1.0 + u[flip] = 1.0 - u[flip] + w[flip] = 1.0 - w[flip] + a = v[f[tri_idx, 0]] + b = v[f[tri_idx, 1]] + c = v[f[tri_idx, 2]] + return a + (b - a) * u[:, None] + (c - a) * w[:, None] + + +__all__ = ["sample_adaptive"] diff --git a/alg/delivery_v1.1/coupler_panel_release/coupler_panel/sampling/grid_spacing.py b/alg/delivery_v1.1/coupler_panel_release/coupler_panel/sampling/grid_spacing.py new file mode 100644 index 0000000..d32f7b8 --- /dev/null +++ b/alg/delivery_v1.1/coupler_panel_release/coupler_panel/sampling/grid_spacing.py @@ -0,0 +1,56 @@ +"""Spacing-driven sampling. + +Translates a target inter-point spacing in mm into a point count, then +defers to area-weighted sampling. Useful when you want to sweep over +``spacing_mm ∈ {0.25, 0.5, 1, 2, 5}`` rather than over absolute counts. + +For an isotropic surface sample, the mean nearest-neighbour distance is +roughly ``sqrt(area / n)`` (square cells). We invert that: + + n ≈ area / spacing² + +and add a small fudge factor of 1.15 to compensate for sample variance. +""" + +from __future__ import annotations + +from typing import Any + +import numpy as np + +from .uniform_area import _triangle_areas, sample_uniform_area + + +def num_points_from_spacing(mesh, spacing_mm: float, *, fudge: float = 1.15) -> int: + if spacing_mm <= 0: + raise ValueError("spacing_mm must be > 0") + v = np.asarray(mesh.vertices, dtype=np.float64) + f = np.asarray(mesh.faces, dtype=np.int64) + total_area = float(_triangle_areas(v, f).sum()) + n = int(round(fudge * total_area / (spacing_mm ** 2))) + return max(n, 1) + + +def sample_grid_spacing( + mesh, + *, + spacing_mm: float | None = None, + num_points: int | None = None, + seed: int | None = 42, + fudge: float = 1.15, + **_: Any, +) -> np.ndarray: + """If ``spacing_mm`` is given, derive ``num_points`` from it. + + Falls through to area-weighted uniform sampling. + """ + if spacing_mm is not None: + n = num_points_from_spacing(mesh, float(spacing_mm), fudge=fudge) + elif num_points is not None: + n = int(num_points) + else: + raise ValueError("either spacing_mm or num_points must be set") + return sample_uniform_area(mesh, num_points=n, seed=seed) + + +__all__ = ["sample_grid_spacing", "num_points_from_spacing"] diff --git a/alg/delivery_v1.1/coupler_panel_release/coupler_panel/sampling/poisson_disk.py b/alg/delivery_v1.1/coupler_panel_release/coupler_panel/sampling/poisson_disk.py new file mode 100644 index 0000000..e3ed488 --- /dev/null +++ b/alg/delivery_v1.1/coupler_panel_release/coupler_panel/sampling/poisson_disk.py @@ -0,0 +1,95 @@ +"""Poisson-disk surface sampling. + +Uses ``open3d.geometry.TriangleMesh.sample_points_poisson_disk`` when +open3d is available; falls back to area-weighted uniform sampling +followed by an O(N²) Bridson-style culling on small meshes. +""" + +from __future__ import annotations + +from typing import Any + +import numpy as np + +from .uniform_area import sample_uniform_area + + +def _try_open3d(): + try: + import open3d as o3d # type: ignore + + return o3d + except ImportError: + return None + + +def sample_poisson_disk( + mesh, + *, + num_points: int, + seed: int | None = 42, + init_factor: int = 5, + **_: Any, +) -> np.ndarray: + """Poisson-disk samples on a triangle mesh.""" + if num_points <= 0: + return np.empty((0, 3), dtype=np.float64) + + o3d = _try_open3d() + if o3d is not None: + # open3d wants its own mesh class. + m = o3d.geometry.TriangleMesh() + m.vertices = o3d.utility.Vector3dVector(np.asarray(mesh.vertices, dtype=np.float64)) + m.triangles = o3d.utility.Vector3iVector(np.asarray(mesh.faces, dtype=np.int32)) + m.compute_vertex_normals() + # Open3D's API takes seed via 'seed' kwarg (0.18+) — passing it best-effort. + try: + pcd = m.sample_points_poisson_disk( + number_of_points=int(num_points), + init_factor=int(init_factor), + seed=int(seed) if seed is not None else -1, + ) + except TypeError: + pcd = m.sample_points_poisson_disk( + number_of_points=int(num_points), + init_factor=int(init_factor), + ) + return np.asarray(pcd.points, dtype=np.float64) + + # ---- Fallback: oversample + grid cull --------------------------------- + over = sample_uniform_area( + mesh, + num_points=num_points * max(init_factor, 5), + seed=seed, + ) + # Estimate target spacing from mesh bbox. + bbox = over.max(axis=0) - over.min(axis=0) + if num_points <= 1: + return over[:num_points] + target_spacing = float(np.cbrt(np.prod(bbox)) / max(num_points, 1) ** (1 / 3)) + return _grid_cull(over, target_spacing, num_points, seed) + + +def _grid_cull( + points: np.ndarray, + spacing: float, + keep_n: int, + seed: int | None, +) -> np.ndarray: + """Pick at most one point per voxel of size ``spacing``.""" + rng = np.random.default_rng(seed) + perm = rng.permutation(points.shape[0]) + voxel = np.floor(points[perm] / max(spacing, 1e-6)).astype(np.int64) + seen: set[tuple[int, int, int]] = set() + out_idx: list[int] = [] + for k, key in enumerate(map(tuple, voxel)): + if key in seen: + continue + seen.add(key) + out_idx.append(perm[k]) + if len(out_idx) >= keep_n: + break + return points[np.asarray(out_idx)] + + +__all__ = ["sample_poisson_disk"] diff --git a/alg/delivery_v1.1/coupler_panel_release/coupler_panel/sampling/uniform_area.py b/alg/delivery_v1.1/coupler_panel_release/coupler_panel/sampling/uniform_area.py new file mode 100644 index 0000000..9bc759a --- /dev/null +++ b/alg/delivery_v1.1/coupler_panel_release/coupler_panel/sampling/uniform_area.py @@ -0,0 +1,53 @@ +"""Area-weighted uniform triangle sampling.""" + +from __future__ import annotations + +from typing import Any + +import numpy as np + + +def _triangle_areas(vertices: np.ndarray, faces: np.ndarray) -> np.ndarray: + a = vertices[faces[:, 0]] + b = vertices[faces[:, 1]] + c = vertices[faces[:, 2]] + return 0.5 * np.linalg.norm(np.cross(b - a, c - a), axis=1) + + +def sample_uniform_area( + mesh, + *, + num_points: int, + seed: int | None = 42, + **_: Any, +) -> np.ndarray: + """Sample ``num_points`` from a triangle mesh, weighted by area. + + Returns an ``(N, 3) float64`` ndarray. + """ + if num_points <= 0: + return np.empty((0, 3), dtype=np.float64) + + v = np.asarray(mesh.vertices, dtype=np.float64) + f = np.asarray(mesh.faces, dtype=np.int64) + areas = _triangle_areas(v, f) + total = float(areas.sum()) + if total <= 0: + raise ValueError("mesh has zero total area") + + rng = np.random.default_rng(seed) + + probs = areas / total + tri_idx = rng.choice(len(f), size=num_points, p=probs) + u = rng.random(num_points) + w = rng.random(num_points) + flip = u + w > 1.0 + u[flip] = 1.0 - u[flip] + w[flip] = 1.0 - w[flip] + a = v[f[tri_idx, 0]] + b = v[f[tri_idx, 1]] + c = v[f[tri_idx, 2]] + return a + (b - a) * u[:, None] + (c - a) * w[:, None] + + +__all__ = ["sample_uniform_area"] diff --git a/alg/delivery_v1.1/coupler_panel_release/coupler_panel/simulation/__init__.py b/alg/delivery_v1.1/coupler_panel_release/coupler_panel/simulation/__init__.py new file mode 100644 index 0000000..4a90ea6 --- /dev/null +++ b/alg/delivery_v1.1/coupler_panel_release/coupler_panel/simulation/__init__.py @@ -0,0 +1,21 @@ +"""Synthetic-scan utilities: noise, rigid transform, optional view simulator.""" + +from .noise import add_gaussian_noise +from .transform import ( + RandomRigidTransform, + apply_rigid, + compose_rigid, + invert_rigid, + sample_random_rigid, +) +from .scan_simulator import simulate_scan_view + +__all__ = [ + "add_gaussian_noise", + "RandomRigidTransform", + "apply_rigid", + "compose_rigid", + "invert_rigid", + "sample_random_rigid", + "simulate_scan_view", +] diff --git a/alg/delivery_v1.1/coupler_panel_release/coupler_panel/simulation/noise.py b/alg/delivery_v1.1/coupler_panel_release/coupler_panel/simulation/noise.py new file mode 100644 index 0000000..43c5fe9 --- /dev/null +++ b/alg/delivery_v1.1/coupler_panel_release/coupler_panel/simulation/noise.py @@ -0,0 +1,57 @@ +"""Synthetic Gaussian noise generation.""" + +from __future__ import annotations + +import numpy as np + + +def add_gaussian_noise( + points: np.ndarray, + *, + sigma_mm: float, + mode: str = "isotropic", + normals: np.ndarray | None = None, + seed: int | None = 43, +) -> np.ndarray: + """Return a noisy copy of ``points``. + + Parameters + ---------- + points : (N, 3) + sigma_mm : float + Standard deviation in mm. ``0`` is a no-op (returns a copy). + mode : {"isotropic", "along_normal"} + ``isotropic`` — N(0, σ²) noise added independently to x/y/z. + ``along_normal`` — N(0, σ²) along ``normals`` only. Requires + a per-point normal of shape ``(N, 3)``. + normals : (N, 3) or None + Required when ``mode == "along_normal"``. + """ + points = np.asarray(points, dtype=np.float64) + if sigma_mm == 0: + return points.copy() + if sigma_mm < 0: + raise ValueError("sigma_mm must be ≥ 0") + + rng = np.random.default_rng(seed) + if mode == "isotropic": + return points + rng.normal(0.0, sigma_mm, size=points.shape) + + if mode == "along_normal": + if normals is None: + raise ValueError("along_normal mode requires per-point normals") + normals = np.asarray(normals, dtype=np.float64) + if normals.shape != points.shape: + raise ValueError( + f"normals shape {normals.shape} != points shape {points.shape}" + ) + norms = np.linalg.norm(normals, axis=1, keepdims=True) + norms[norms < 1e-12] = 1.0 + unit = normals / norms + offset = rng.normal(0.0, sigma_mm, size=points.shape[0])[:, None] + return points + unit * offset + + raise ValueError(f"unknown noise mode '{mode}'") + + +__all__ = ["add_gaussian_noise"] diff --git a/alg/delivery_v1.1/coupler_panel_release/coupler_panel/simulation/scan_simulator.py b/alg/delivery_v1.1/coupler_panel_release/coupler_panel/simulation/scan_simulator.py new file mode 100644 index 0000000..b6d9f95 --- /dev/null +++ b/alg/delivery_v1.1/coupler_panel_release/coupler_panel/simulation/scan_simulator.py @@ -0,0 +1,80 @@ +"""Single-view scan simulator (optional). + +Models a structured-light / line-laser scanner that only sees points +visible from a single camera origin. Implementation is the *hidden-point +removal* (HPR) of Katz, Tal & Basri (2007), which works directly on a +point cloud — no mesh raycasting needed. + +Open3D ships an HPR primitive; we route to that when available, with a +plain-numpy fallback for the simple convex-hull formulation. +""" + +from __future__ import annotations + +import numpy as np + + +def _try_open3d(): + try: + import open3d as o3d # type: ignore + + return o3d + except ImportError: + return None + + +def simulate_scan_view( + points: np.ndarray, + *, + view_origin, + hpr_radius_mm: float, +) -> np.ndarray: + """Return only the points visible from ``view_origin``. + + Parameters + ---------- + view_origin : (3,) + hpr_radius_mm : float + HPR sphere radius. Larger = more points kept. Pick something + much larger than the cloud diameter (e.g. 100× diameter) to + approximate a "all front-facing surfaces" view. + """ + pts = np.asarray(points, dtype=np.float64) + o = np.asarray(view_origin, dtype=np.float64).reshape(3) + + o3d = _try_open3d() + if o3d is not None: + pcd = o3d.geometry.PointCloud() + pcd.points = o3d.utility.Vector3dVector(pts) + _, idx = pcd.hidden_point_removal(o, float(hpr_radius_mm)) + return pts[np.asarray(idx, dtype=np.int64)] + + # ---- numpy fallback: spherical-flip + convex-hull selection ---------- + # Flip every point through a sphere of radius R centred at the view. + diff = pts - o + norms = np.linalg.norm(diff, axis=1, keepdims=True) + norms[norms < 1e-12] = 1.0 + R = float(hpr_radius_mm) + flipped = pts + 2.0 * (R - norms) * (diff / norms) + # Add the viewpoint itself as a phantom point so its convex hull + # encloses the visible-from-origin set. + augmented = np.vstack([flipped, o[None, :]]) + try: + from scipy.spatial import ConvexHull + + hull = ConvexHull(augmented, qhull_options="QJ") + keep = np.asarray(hull.vertices, dtype=np.int64) + # Drop the phantom point. + keep = keep[keep < pts.shape[0]] + return pts[keep] + except Exception: + # Last-resort: keep points whose dot with (point - origin) along + # the centroid axis is positive. Crude but never crashes. + centroid = pts.mean(axis=0) + view_dir = centroid - o + view_dir /= max(np.linalg.norm(view_dir), 1e-12) + d = (pts - o) @ view_dir + return pts[d > 0] + + +__all__ = ["simulate_scan_view"] diff --git a/alg/delivery_v1.1/coupler_panel_release/coupler_panel/simulation/transform.py b/alg/delivery_v1.1/coupler_panel_release/coupler_panel/simulation/transform.py new file mode 100644 index 0000000..4794e1e --- /dev/null +++ b/alg/delivery_v1.1/coupler_panel_release/coupler_panel/simulation/transform.py @@ -0,0 +1,162 @@ +"""Random rigid transforms used to mimic mounting jitter / scan pose noise. + +The transform is stored as a 4×4 homogeneous matrix; the GT used for +validation is exactly this matrix. The 4-hole Kabsch registration step +should recover its inverse up to noise. +""" + +from __future__ import annotations + +from dataclasses import dataclass +from typing import Sequence + +import numpy as np + + +@dataclass(frozen=True) +class RandomRigidTransform: + """Outcome of one random rigid sample.""" + + matrix: np.ndarray # (4, 4) float64 + rotation_deg: float + rotation_axis: tuple[float, float, float] + translation_mm: float + translation_dir: tuple[float, float, float] + seed: int | None + + def to_dict(self) -> dict: + return { + "matrix": self.matrix.tolist(), + "rotation_deg": self.rotation_deg, + "rotation_axis": list(self.rotation_axis), + "translation_mm": self.translation_mm, + "translation_dir": list(self.translation_dir), + "seed": self.seed, + } + + +# --------------------------------------------------------------------------- +# Construction +# --------------------------------------------------------------------------- + + +def _rodrigues(axis: np.ndarray, angle_rad: float) -> np.ndarray: + """Rodrigues rotation around a unit ``axis``.""" + axis = axis / max(np.linalg.norm(axis), 1e-12) + K = np.array( + [ + [0.0, -axis[2], axis[1]], + [axis[2], 0.0, -axis[0]], + [-axis[1], axis[0], 0.0], + ], + dtype=np.float64, + ) + s = np.sin(angle_rad) + c = np.cos(angle_rad) + return np.eye(3) + s * K + (1.0 - c) * (K @ K) + + +def _resolve_axis(axis_spec, rng) -> np.ndarray: + if isinstance(axis_spec, str): + spec = axis_spec.lower() + if spec == "x": + return np.array([1.0, 0.0, 0.0]) + if spec == "y": + return np.array([0.0, 1.0, 0.0]) + if spec == "z": + return np.array([0.0, 0.0, 1.0]) + if spec == "random": + v = rng.normal(size=3) + return v / max(np.linalg.norm(v), 1e-12) + raise ValueError(f"unknown rotation_axis '{axis_spec}'") + arr = np.asarray(axis_spec, dtype=np.float64) + if arr.shape != (3,): + raise ValueError("rotation_axis must be 'x'/'y'/'z'/'random' or a 3-vector") + n = np.linalg.norm(arr) + if n < 1e-12: + raise ValueError("rotation_axis vector is degenerate") + return arr / n + + +def _resolve_dir(dir_spec, rng) -> np.ndarray: + if isinstance(dir_spec, str): + if dir_spec == "random": + v = rng.normal(size=3) + return v / max(np.linalg.norm(v), 1e-12) + raise ValueError(f"unknown translation_dir '{dir_spec}'") + arr = np.asarray(dir_spec, dtype=np.float64) + if arr.shape != (3,): + raise ValueError("translation_dir must be 'random' or a 3-vector") + n = np.linalg.norm(arr) + if n < 1e-12: + raise ValueError("translation_dir vector is degenerate") + return arr / n + + +def sample_random_rigid( + *, + rotation_deg: float, + translation_mm: float, + rotation_axis: str | Sequence[float] = "random", + translation_dir: str | Sequence[float] = "random", + seed: int | None = 44, +) -> RandomRigidTransform: + rng = np.random.default_rng(seed) + axis = _resolve_axis(rotation_axis, rng) + direction = _resolve_dir(translation_dir, rng) + + R = _rodrigues(axis, np.deg2rad(float(rotation_deg))) + t = direction * float(translation_mm) + + M = np.eye(4) + M[:3, :3] = R + M[:3, 3] = t + + return RandomRigidTransform( + matrix=M, + rotation_deg=float(rotation_deg), + rotation_axis=tuple(axis.tolist()), + translation_mm=float(translation_mm), + translation_dir=tuple(direction.tolist()), + seed=seed, + ) + + +# --------------------------------------------------------------------------- +# Application +# --------------------------------------------------------------------------- + + +def apply_rigid(points: np.ndarray, T: np.ndarray) -> np.ndarray: + """Apply a 4×4 homogeneous transform to an ``(N, 3)`` cloud.""" + P = np.asarray(points, dtype=np.float64) + if P.ndim != 2 or P.shape[1] != 3: + raise ValueError(f"expected (N,3), got {P.shape}") + R = np.asarray(T, dtype=np.float64)[:3, :3] + t = np.asarray(T, dtype=np.float64)[:3, 3] + return P @ R.T + t + + +def compose_rigid(*Ts: np.ndarray) -> np.ndarray: + out = np.eye(4) + for T in Ts: + out = out @ np.asarray(T, dtype=np.float64) + return out + + +def invert_rigid(T: np.ndarray) -> np.ndarray: + R = T[:3, :3] + t = T[:3, 3] + inv = np.eye(4) + inv[:3, :3] = R.T + inv[:3, 3] = -R.T @ t + return inv + + +__all__ = [ + "RandomRigidTransform", + "sample_random_rigid", + "apply_rigid", + "compose_rigid", + "invert_rigid", +] diff --git a/alg/delivery_v1.1/coupler_panel_release/coupler_panel/structures.py b/alg/delivery_v1.1/coupler_panel_release/coupler_panel/structures.py new file mode 100644 index 0000000..07e35ad --- /dev/null +++ b/alg/delivery_v1.1/coupler_panel_release/coupler_panel/structures.py @@ -0,0 +1,148 @@ +"""Result dataclasses for coupler_panel. + +These types form the **stable public schema** of the package; their +field names and semantics are guarded by ``SCHEMA_VERSION`` (see +``_version.py``). + +All numeric units are **mm** for distances and **degree** for angles +unless otherwise noted. +""" + +from __future__ import annotations + +from dataclasses import asdict, dataclass, field +from typing import Any + +import numpy as np + + +# --------------------------------------------------------------------------- +# Building blocks +# --------------------------------------------------------------------------- + + +@dataclass(frozen=True) +class Hole: + """A single detected (or GT) circular hole on the panel.""" + + center: tuple[float, float, float] # (x, y, z), mm + radius_mm: float + fit_residual_rms_mm: float # 0 for GT + role: str # "locating" | "central" | "unknown" + + +@dataclass(frozen=True) +class HoleSet: + """The four locating holes plus an optional central hole. + + ``locating`` is ordered consistently between GT and detection so that + Kabsch pairing is index-aligned. + """ + + locating: tuple[Hole, Hole, Hole, Hole] + central: Hole | None = None + + def locating_centers(self) -> np.ndarray: + """Return a (4, 3) ndarray of locating-hole centres.""" + return np.asarray([h.center for h in self.locating], dtype=np.float64) + + +@dataclass(frozen=True) +class PanelPlane: + """The fitted (or GT) panel plane.""" + + normal: tuple[float, float, float] # unit vector, n_x > 0 enforced + point_on_plane: tuple[float, float, float] + d_offset: float # n . x + d = 0 + inlier_ratio: float # 1.0 for GT + inlier_residual_rms_mm: float # 0 for GT + + +@dataclass(frozen=True) +class ReferenceFrame: + """Reference geometry derived from the four-hole panel centre.""" + + center_C: tuple[float, float, float] + # Reference line L = C + lam * e_x (always parallel to the +x axis). + line_direction: tuple[float, float, float] = (1.0, 0.0, 0.0) + # Reference plane: z = C_z. Stored as plane normal + offset. + reference_plane_z: float = 0.0 + # Longitudinal mid-plane: y = C_y. + longitudinal_mid_y: float = 0.0 + + +@dataclass(frozen=True) +class StageTiming: + """Per-stage wall-clock and peak-memory cost.""" + + stage: str + seconds: float + peak_kib: float + + +# --------------------------------------------------------------------------- +# Top-level result +# --------------------------------------------------------------------------- + + +@dataclass +class CouplerMeasurement: + """Top-level result of the coupler-panel measurement pipeline. + + See ``docs/USAGE.md`` for the stable-API contract. + """ + + schema_version: str + algorithm_version: str + + # --- Primary measurands --- + theta_right_deg: float # atan2(n_y, n_x), with right_axis_sign applied + theta_ref_deg: float # atan2(n_z, n_x) + D_y_max_mm: float # max |y_i - C_y| over panel ROI + D_z_max_mm: float # max |z_i - C_z| over panel ROI + dx_y_max_mm: float # |A| * D_y_max, A = -n_y / n_x + dx_z_max_mm: float # |B| * D_z_max, B = -n_z / n_x + + # --- Geometry --- + center_C: tuple[float, float, float] + normal: tuple[float, float, float] # n_x > 0 + holes: HoleSet + plane: PanelPlane + frame: ReferenceFrame + + # --- Diagnostics --- + quality_flags: list[str] = field(default_factory=list) + timings: list[StageTiming] = field(default_factory=list) + report: dict[str, Any] = field(default_factory=dict) + + def to_dict(self) -> dict[str, Any]: + """Return a fully JSON-serialisable dict.""" + return _to_jsonable(asdict(self)) + + +# --------------------------------------------------------------------------- +# Helpers +# --------------------------------------------------------------------------- + + +def _to_jsonable(obj: Any) -> Any: + """Recursively convert numpy / tuple structures into plain Python types.""" + if isinstance(obj, dict): + return {k: _to_jsonable(v) for k, v in obj.items()} + if isinstance(obj, (list, tuple)): + return [_to_jsonable(v) for v in obj] + if isinstance(obj, np.ndarray): + return obj.tolist() + if isinstance(obj, (np.floating, np.integer)): + return obj.item() + return obj + + +__all__ = [ + "Hole", + "HoleSet", + "PanelPlane", + "ReferenceFrame", + "StageTiming", + "CouplerMeasurement", +] diff --git a/alg/delivery_v1.1/coupler_panel_release/coupler_panel/viz/__init__.py b/alg/delivery_v1.1/coupler_panel_release/coupler_panel/viz/__init__.py new file mode 100644 index 0000000..546582f --- /dev/null +++ b/alg/delivery_v1.1/coupler_panel_release/coupler_panel/viz/__init__.py @@ -0,0 +1,21 @@ +"""Diagnostic visualisation utilities (matplotlib + open3d offscreen). + +All renderers degrade gracefully if matplotlib / open3d are missing — +they raise :class:`ImportError` only when actually called. The pipeline +calls them via :func:`render_all`, which silently skips on missing deps. +""" + +from .cloud_view import render_cloud, render_cloud_compare +from .holes_view import render_holes +from .plane_view import render_plane_with_refs +from .heatmap import render_panel_heatmap +from .render_all import render_all + +__all__ = [ + "render_cloud", + "render_cloud_compare", + "render_holes", + "render_plane_with_refs", + "render_panel_heatmap", + "render_all", +] diff --git a/alg/delivery_v1.1/coupler_panel_release/coupler_panel/viz/_offscreen.py b/alg/delivery_v1.1/coupler_panel_release/coupler_panel/viz/_offscreen.py new file mode 100644 index 0000000..bb8a230 --- /dev/null +++ b/alg/delivery_v1.1/coupler_panel_release/coupler_panel/viz/_offscreen.py @@ -0,0 +1,27 @@ +"""Open3D offscreen renderer factory. + +Wraps the shifting open3d offscreen API so callers don't have to. Returns +``None`` if open3d is not available — callers should fall back to +matplotlib in that case. +""" + +from __future__ import annotations + +from typing import Any + + +def make_offscreen_renderer(width: int = 1024, height: int = 768) -> Any | None: + try: + import open3d as o3d # type: ignore + except ImportError: + return None + + # open3d's offscreen path moved between 0.16 / 0.18. + try: + renderer = o3d.visualization.rendering.OffscreenRenderer(width, height) + return renderer + except Exception: + return None + + +__all__ = ["make_offscreen_renderer"] diff --git a/alg/delivery_v1.1/coupler_panel_release/coupler_panel/viz/cloud_view.py b/alg/delivery_v1.1/coupler_panel_release/coupler_panel/viz/cloud_view.py new file mode 100644 index 0000000..c5ce425 --- /dev/null +++ b/alg/delivery_v1.1/coupler_panel_release/coupler_panel/viz/cloud_view.py @@ -0,0 +1,134 @@ +"""Point cloud rendering using matplotlib. + +By default we render a 2D (y, z) projection — the coupler panel is a thin +sheet whose normal is along x, so a top-down y-z view is more readable +than a 3D scatter. Pass ``mode="3d"`` to force a 3D scatter (slower and +broken on some matplotlib versions, kept for completeness). +""" + +from __future__ import annotations + +from pathlib import Path + +import numpy as np + + +def _import_plt(): + import matplotlib + + matplotlib.use("Agg") + import matplotlib.pyplot as plt # noqa: WPS433 + + return plt + + +def _decimate(points: np.ndarray, max_points: int) -> np.ndarray: + n = points.shape[0] + if n <= max_points: + return points + rng = np.random.default_rng(0) + idx = rng.choice(n, size=max_points, replace=False) + return points[idx] + + +def render_cloud( + points: np.ndarray, + out_path: str | Path, + *, + title: str = "", + color: str | np.ndarray = "tab:blue", + max_points: int = 80_000, + dpi: int = 150, + mode: str = "2d", +) -> Path: + plt = _import_plt() + pts = _decimate(np.asarray(points, dtype=np.float64), max_points) + + if mode == "3d": + return _render_3d(pts, out_path, title=title, color=color, dpi=dpi) + + fig, axes = plt.subplots(1, 2, figsize=(14, 6)) + axes[0].scatter(pts[:, 1], pts[:, 2], s=0.5, c=color) + axes[0].set_xlabel("y (mm)") + axes[0].set_ylabel("z (mm)") + axes[0].set_aspect("equal") + axes[0].set_title("y-z projection (panel face)") + + axes[1].scatter(pts[:, 0], pts[:, 2], s=0.5, c=color) + axes[1].set_xlabel("x (mm)") + axes[1].set_ylabel("z (mm)") + axes[1].set_aspect("equal") + axes[1].set_title("x-z projection (side view)") + + if title: + fig.suptitle(title) + + out_path = Path(out_path) + out_path.parent.mkdir(parents=True, exist_ok=True) + fig.tight_layout() + fig.savefig(out_path, dpi=dpi) + plt.close(fig) + return out_path + + +def render_cloud_compare( + clouds: dict[str, np.ndarray], + out_path: str | Path, + *, + title: str = "", + max_points: int = 50_000, + dpi: int = 150, +) -> Path: + """Side-by-side y-z projections of several clouds.""" + plt = _import_plt() + n = len(clouds) + fig, axes = plt.subplots(1, n, figsize=(6 * n, 6), squeeze=False) + cmap = plt.get_cmap("tab10") + for i, (name, pts) in enumerate(clouds.items()): + ax = axes[0, i] + sub = _decimate(np.asarray(pts, dtype=np.float64), max_points) + ax.scatter(sub[:, 1], sub[:, 2], s=0.5, c=[cmap(i)]) + ax.set_title(name) + ax.set_xlabel("y (mm)") + ax.set_ylabel("z (mm)") + ax.set_aspect("equal") + if title: + fig.suptitle(title) + out_path = Path(out_path) + out_path.parent.mkdir(parents=True, exist_ok=True) + fig.tight_layout() + fig.savefig(out_path, dpi=dpi) + plt.close(fig) + return out_path + + +def _render_3d(pts, out_path, *, title, color, dpi): + plt = _import_plt() + fig = plt.figure(figsize=(8, 6)) + try: + ax = fig.add_subplot(111, projection="3d") + ax.scatter(pts[:, 0], pts[:, 1], pts[:, 2], s=0.5, c=color) + ax.set_xlabel("x (mm)") + ax.set_ylabel("y (mm)") + ax.set_zlabel("z (mm)") + if title: + ax.set_title(title) + except Exception: + # Fallback to 2D when matplotlib's 3D backend is broken (e.g. mpl 3.10 + # has a known incompatibility with mpl_toolkits.mplot3d). + ax = fig.add_subplot(111) + ax.scatter(pts[:, 1], pts[:, 2], s=0.5, c=color) + ax.set_aspect("equal") + ax.set_xlabel("y (mm)") + ax.set_ylabel("z (mm)") + if title: + ax.set_title(f"{title} (3D backend unavailable)") + out_path = Path(out_path) + out_path.parent.mkdir(parents=True, exist_ok=True) + fig.tight_layout() + fig.savefig(out_path, dpi=dpi) + plt.close(fig) + return out_path + + +__all__ = ["render_cloud", "render_cloud_compare"] diff --git a/alg/delivery_v1.1/coupler_panel_release/coupler_panel/viz/heatmap.py b/alg/delivery_v1.1/coupler_panel_release/coupler_panel/viz/heatmap.py new file mode 100644 index 0000000..7da8eb1 --- /dev/null +++ b/alg/delivery_v1.1/coupler_panel_release/coupler_panel/viz/heatmap.py @@ -0,0 +1,73 @@ +"""Panel deviation heatmap. + +Colours each panel-ROI point by its signed perpendicular distance to +the fitted plane, projected onto a 2D image of the panel face. +""" + +from __future__ import annotations + +from pathlib import Path + +import numpy as np + + +def _import_plt(): + import matplotlib + + matplotlib.use("Agg") + import matplotlib.pyplot as plt + + return plt + + +def render_panel_heatmap( + panel_points: np.ndarray, + *, + plane_normal: tuple[float, float, float], + plane_d: float, + center_C: tuple[float, float, float], + out_path: str | Path, + title: str = "", + cmap: str = "viridis", + dpi: int = 150, +) -> Path: + plt = _import_plt() + + pts = np.asarray(panel_points, dtype=np.float64) + n = np.asarray(plane_normal, dtype=np.float64) + n = n / max(np.linalg.norm(n), 1e-12) + C = np.asarray(center_C, dtype=np.float64) + + # Local 2D frame on the panel. + y_world = np.array([0.0, 1.0, 0.0]) + u = y_world - np.dot(y_world, n) * n + if np.linalg.norm(u) < 1e-6: + u = np.array([0.0, 0.0, 1.0]) - np.dot(np.array([0.0, 0.0, 1.0]), n) * n + u /= np.linalg.norm(u) + v = np.cross(n, u) + + rel = pts - C + uu = rel @ u + vv = rel @ v + signed_dist = pts @ n + float(plane_d) + + fig, ax = plt.subplots(figsize=(8, 7)) + sc = ax.scatter(uu, vv, c=signed_dist, cmap=cmap, s=2) + cb = plt.colorbar(sc, ax=ax) + cb.set_label("signed distance to fitted plane (mm)") + + ax.set_aspect("equal") + ax.set_xlabel("panel u (≈ y axis, mm)") + ax.set_ylabel("panel v (≈ z axis, mm)") + if title: + ax.set_title(title) + + out_path = Path(out_path) + out_path.parent.mkdir(parents=True, exist_ok=True) + fig.tight_layout() + fig.savefig(out_path, dpi=dpi) + plt.close(fig) + return out_path + + +__all__ = ["render_panel_heatmap"] diff --git a/alg/delivery_v1.1/coupler_panel_release/coupler_panel/viz/holes_view.py b/alg/delivery_v1.1/coupler_panel_release/coupler_panel/viz/holes_view.py new file mode 100644 index 0000000..219a014 --- /dev/null +++ b/alg/delivery_v1.1/coupler_panel_release/coupler_panel/viz/holes_view.py @@ -0,0 +1,70 @@ +"""4-hole detection diagnostic plot. + +Shows the panel cloud projected to its local 2D frame, with each +detected circle drawn on top and the four selected locating holes +highlighted. +""" + +from __future__ import annotations + +from pathlib import Path + +import numpy as np + + +def _import_plt(): + import matplotlib + + matplotlib.use("Agg") + import matplotlib.pyplot as plt + from matplotlib.patches import Circle # noqa: WPS433 + + return plt, Circle + + +def render_holes( + uv_points: np.ndarray, + candidates: list, + selected_indices: list[int], + out_path: str | Path, + *, + title: str = "", + dpi: int = 150, +) -> Path: + plt, Circle = _import_plt() + fig, ax = plt.subplots(figsize=(8, 8)) + if uv_points.shape[0] > 50_000: + idx = np.random.default_rng(0).choice(uv_points.shape[0], 50_000, replace=False) + sub = uv_points[idx] + else: + sub = uv_points + ax.scatter(sub[:, 0], sub[:, 1], s=0.3, c="lightgray") + + for i, cand in enumerate(candidates): + is_sel = i in set(selected_indices) + col = "tab:red" if is_sel else "tab:blue" + ax.add_patch( + Circle( + (cand.fit.cx, cand.fit.cy), + cand.fit.radius, + fill=False, + edgecolor=col, + lw=2 if is_sel else 1, + ) + ) + ax.plot(cand.fit.cx, cand.fit.cy, "+", color=col) + + ax.set_aspect("equal") + ax.set_xlabel("u (mm)") + ax.set_ylabel("v (mm)") + if title: + ax.set_title(title) + out_path = Path(out_path) + out_path.parent.mkdir(parents=True, exist_ok=True) + fig.tight_layout() + fig.savefig(out_path, dpi=dpi) + plt.close(fig) + return out_path + + +__all__ = ["render_holes"] diff --git a/alg/delivery_v1.1/coupler_panel_release/coupler_panel/viz/plane_view.py b/alg/delivery_v1.1/coupler_panel_release/coupler_panel/viz/plane_view.py new file mode 100644 index 0000000..6ea5b04 --- /dev/null +++ b/alg/delivery_v1.1/coupler_panel_release/coupler_panel/viz/plane_view.py @@ -0,0 +1,105 @@ +"""Plane + reference geometry overlay (2D projection). + +Renders the y-z view of the panel ROI with the fitted plane normal, +reference line L, reference plane z=C_z, and longitudinal mid y=C_y +overlaid as annotations. +""" + +from __future__ import annotations + +from pathlib import Path + +import numpy as np + + +def _import_plt(): + import matplotlib + + matplotlib.use("Agg") + import matplotlib.pyplot as plt + + return plt + + +def render_plane_with_refs( + panel_points: np.ndarray, + *, + plane_normal: tuple[float, float, float], + center_C: tuple[float, float, float], + out_path: str | Path, + title: str = "", + line_length_mm: float = 200.0, + dpi: int = 150, +) -> Path: + plt = _import_plt() + + pts = np.asarray(panel_points, dtype=np.float64) + n = np.asarray(plane_normal, dtype=np.float64) + n = n / max(np.linalg.norm(n), 1e-12) + C = np.asarray(center_C, dtype=np.float64) + + if pts.shape[0] > 80_000: + idx = np.random.default_rng(0).choice(pts.shape[0], 80_000, replace=False) + pts = pts[idx] + + fig, axes = plt.subplots(1, 2, figsize=(14, 6)) + + # Panel face view: y-z, with a small arrow for the normal projected + # into y-z (the in-plane part of n). + ax = axes[0] + ax.scatter(pts[:, 1], pts[:, 2], s=0.5, c="lightgray") + # Reference plane z = C_z + ax.axhline(C[2], color="tab:green", lw=1, alpha=0.7, label=f"ref plane z = {C[2]:.2f}") + # Longitudinal mid y = C_y + ax.axvline(C[1], color="tab:orange", lw=1, alpha=0.7, label=f"long. mid y = {C[1]:.2f}") + ax.scatter([C[1]], [C[2]], color="black", s=40, zorder=5, label=f"C = ({C[1]:.1f}, {C[2]:.1f})") + # Project normal into y-z + n_yz = np.array([n[1], n[2]]) + if np.linalg.norm(n_yz) > 1e-3: + scale = line_length_mm * 0.5 + ax.annotate( + "", + xy=(C[1] + n_yz[0] * scale, C[2] + n_yz[1] * scale), + xytext=(C[1], C[2]), + arrowprops=dict(arrowstyle="->", color="tab:red", lw=2), + ) + ax.set_aspect("equal") + ax.set_xlabel("y (mm)") + ax.set_ylabel("z (mm)") + ax.set_title("y-z view: ROI + reference geometry + n's y-z projection") + ax.legend(loc="upper right", fontsize="x-small") + + # x-y side view: shows the panel tilt out of x. + ax = axes[1] + ax.scatter(pts[:, 0], pts[:, 1], s=0.5, c="lightgray") + # Reference line L (along +x at y=C_y, z=C_z, projected to x-y → horizontal at y=C_y) + L = float(line_length_mm) + ax.plot([C[0] - L, C[0] + L], [C[1], C[1]], color="tab:blue", lw=2, label="ref line L") + # Plane normal (x, y components) + n_xy = np.array([n[0], n[1]]) + scale = line_length_mm * 0.5 + ax.annotate( + "", + xy=(C[0] + n_xy[0] * scale, C[1] + n_xy[1] * scale), + xytext=(C[0], C[1]), + arrowprops=dict(arrowstyle="->", color="tab:red", lw=2), + ) + ax.scatter([C[0]], [C[1]], color="black", s=40, zorder=5) + ax.set_aspect("equal") + ax.set_xlabel("x (mm)") + ax.set_ylabel("y (mm)") + ax.set_title(f"x-y view: n=({n[0]:.4f},{n[1]:.4f},{n[2]:.4f})") + ax.legend(loc="upper right", fontsize="x-small") + + if title: + fig.suptitle(title) + + out_path = Path(out_path) + out_path.parent.mkdir(parents=True, exist_ok=True) + fig.tight_layout() + fig.savefig(out_path, dpi=dpi) + plt.close(fig) + return out_path + + +__all__ = ["render_plane_with_refs"] diff --git a/alg/delivery_v1.1/coupler_panel_release/coupler_panel/viz/render_all.py b/alg/delivery_v1.1/coupler_panel_release/coupler_panel/viz/render_all.py new file mode 100644 index 0000000..d98ab4c --- /dev/null +++ b/alg/delivery_v1.1/coupler_panel_release/coupler_panel/viz/render_all.py @@ -0,0 +1,112 @@ +"""One-shot rendering of every diagnostic image for a single run. + +Reads the artifact directory written by :mod:`coupler_panel.pipeline` and +fills in ``images/`` with PNG plots. Silently skips any stage that +cannot be loaded (e.g. ``denoised.ply`` if denoise was disabled). +""" + +from __future__ import annotations + +from pathlib import Path +from typing import Any + +import numpy as np + +from ..io_utils import load_point_cloud, Artifacts + + +def _safe_load(path: Path) -> np.ndarray | None: + try: + return load_point_cloud(path) if path.exists() else None + except Exception: + return None + + +def render_all(run_dir: str | Path, *, dpi: int = 150) -> dict[str, Path]: + """Render every diagnostic image we know how to make for this run.""" + art = Artifacts(run_dir) + out: dict[str, Path] = {} + + # Lazy imports — matplotlib not strictly required to run pipeline. + try: + from .cloud_view import render_cloud, render_cloud_compare + from .plane_view import render_plane_with_refs + from .heatmap import render_panel_heatmap + except ImportError: + return out + + stages = ("cad_mesh", "sampled", "noisy", "transformed", "denoised", "registered") + clouds: dict[str, np.ndarray] = {} + for st in stages: + pts = _safe_load(art.path(st, ext="ply")) + if pts is not None and pts.shape[0] > 0: + clouds[st] = pts + try: + out[st] = render_cloud( + pts, art.image(f"{st}.png"), title=st, dpi=dpi, + ) + except Exception: + pass + + # Per-stage 2-up comparisons of selected pairs. + if "sampled" in clouds and "noisy" in clouds: + try: + out["sampled_vs_noisy"] = render_cloud_compare( + {"sampled": clouds["sampled"], "noisy": clouds["noisy"]}, + art.image("sampled_vs_noisy.png"), + dpi=dpi, + ) + except Exception: + pass + + if "transformed" in clouds and "registered" in clouds: + try: + out["transformed_vs_registered"] = render_cloud_compare( + {"transformed": clouds["transformed"], "registered": clouds["registered"]}, + art.image("transformed_vs_registered.png"), + dpi=dpi, + ) + except Exception: + pass + + # Plane + reference geometry (read from report.json). + report_path = art.report_json + if report_path.exists(): + try: + import json + + with report_path.open("r", encoding="utf-8") as fh: + report: dict[str, Any] = json.load(fh) + + measured = report.get("measured", report) + plane = measured.get("plane") + frame = measured.get("frame") + roi_pts_path = art.path("panel_roi", ext="ply") + roi_pts = _safe_load(roi_pts_path) + if roi_pts is None and "registered" in clouds: + roi_pts = clouds["registered"] + if plane and frame and roi_pts is not None and roi_pts.shape[0] > 0: + out["plane_with_refs"] = render_plane_with_refs( + roi_pts, + plane_normal=tuple(plane["normal"]), + center_C=tuple(frame["center_C"]), + out_path=art.image("plane_with_refs.png"), + title="fitted plane + reference geometry", + dpi=dpi, + ) + out["heatmap"] = render_panel_heatmap( + roi_pts, + plane_normal=tuple(plane["normal"]), + plane_d=float(plane["d_offset"]), + center_C=tuple(frame["center_C"]), + out_path=art.image("deviation_heatmap.png"), + title="panel deviation heatmap", + dpi=dpi, + ) + except Exception: + pass + + return out + + +__all__ = ["render_all"] diff --git a/alg/delivery_v1.1/coupler_panel_release/examples/generate_test_clouds.py b/alg/delivery_v1.1/coupler_panel_release/examples/generate_test_clouds.py new file mode 100644 index 0000000..0fdd870 --- /dev/null +++ b/alg/delivery_v1.1/coupler_panel_release/examples/generate_test_clouds.py @@ -0,0 +1,128 @@ +"""Generate synthetic test point clouds at varying injected panel-deflection +angles. Each cloud is produced by the *existing* CAD synthesis path +(parse → sample → deflect → noise) **with the scanner-pose jitter step +turned off**, so each PLY already lives in the CAD frame — which is the +contract the main measurement pipeline expects (no registration step). + +Each PLY is paired with one row in ``manifest.json`` so the batch +evaluator knows the GT angle to compare against. + +Run:: + + python examples/generate_test_clouds.py \ + --stp test_data/lu1.stp \ + --out test_data/synth \ + --profile lu1_af4 +""" + +from __future__ import annotations + +import argparse +import logging +from pathlib import Path + +from coupler_panel.config import load_config, resolve_auto +from coupler_panel.data_prep import parse_cad, synthesize_cloud +from coupler_panel.io_utils import write_point_cloud, write_json + + +# (theta_right_deg, theta_ref_deg) pairs — mirrors the spirit of the +# existing lu1_deflection_sweep matrix. +DEFAULT_INJECTIONS = [ + (0.0, 0.0), + (0.1, 0.0), + (0.3, 0.0), + (0.5, 0.0), + (1.0, 0.0), + (2.0, 0.0), + (0.0, 0.5), + (0.5, 0.5), + (1.0, 1.0), +] + + +def _build_parser() -> argparse.ArgumentParser: + p = argparse.ArgumentParser(description=__doc__) + p.add_argument("--stp", type=Path, required=True, help="Path to the panel STEP file.") + p.add_argument("--out", type=Path, required=True, help="Output directory for PLYs + manifest.") + p.add_argument("--profile", type=str, default="lu1_af4", help="Config profile.") + p.add_argument( + "--config", + type=Path, + default=None, + help="Optional YAML override; takes precedence over --profile.", + ) + return p + + +def main(argv: list[str] | None = None) -> int: + logging.basicConfig(level=logging.INFO, format="%(levelname)s %(name)s: %(message)s") + log = logging.getLogger("generate_test_clouds") + args = _build_parser().parse_args(argv) + + cfg = load_config(args.config, profile=args.profile) + cfg = resolve_auto(cfg) + + log.info("parsing CAD: %s", args.stp) + parsed = parse_cad(args.stp, cfg["cad"]) + + out_dir = args.out + out_dir.mkdir(parents=True, exist_ok=True) + + # CAD nominal GT — kept as an audit reference (not consumed by the + # main pipeline, which only needs the STP). + write_json(parsed.gt.to_dict(), out_dir / "gt.json") + + manifest: list[dict] = [] + for i, (th_r, th_z) in enumerate(DEFAULT_INJECTIONS): + cfg_i = dict(cfg) + cfg_i["panel_deflection"] = { + "enabled": True, + "theta_right_deg": float(th_r), + "theta_ref_deg": float(th_z), + } + # Scanner-pose jitter is OFF: the cloud must already be in the + # CAD frame because the main pipeline does not register. + cfg_i["transform"] = {"enabled": False} + + log.info("[%d/%d] synthesising θ_right=%+.2f° θ_ref=%+.2f°", + i + 1, len(DEFAULT_INJECTIONS), th_r, th_z) + synth = synthesize_cloud( + parsed, + sampling_cfg=cfg_i["sampling"], + noise_cfg=cfg_i.get("noise", {}), + transform_cfg=cfg_i["transform"], + scan_cfg=cfg_i.get("scan_simulator", {}), + deflection_cfg=cfg_i["panel_deflection"], + ) + sample_id = ( + f"r{th_r:+.2f}_z{th_z:+.2f}" + .replace("+", "p").replace("-", "n").replace(".", "") + ) + ply_path = out_dir / f"cloud_{sample_id}.ply" + # synth.transformed == synth.noisy when transform is disabled. + write_point_cloud(synth.transformed, ply_path) + + manifest.append({ + "sample_id": sample_id, + "ply": ply_path.name, + "theta_right_deg": float(th_r), + "theta_ref_deg": float(th_z), + "n_points": int(synth.transformed.shape[0]), + }) + + write_json( + { + "stp": str(args.stp), + "profile": args.profile, + "gt": "gt.json", + "samples": manifest, + }, + out_dir / "manifest.json", + ) + log.info("done: %d clouds + gt.json + manifest.json under %s", len(manifest), out_dir) + return 0 + + +if __name__ == "__main__": + raise SystemExit(main()) diff --git a/alg/delivery_v1.1/coupler_panel_release/examples/integrate_minimal.py b/alg/delivery_v1.1/coupler_panel_release/examples/integrate_minimal.py new file mode 100644 index 0000000..b7b8ce6 --- /dev/null +++ b/alg/delivery_v1.1/coupler_panel_release/examples/integrate_minimal.py @@ -0,0 +1,58 @@ +"""Minimal integration example for software developers. + +This script shows the *only* code path a production caller needs: + + load a clean, registered point cloud → call the API → use the result + +Run:: + + python examples/integrate_minimal.py path/to/scan.ply +""" + +from __future__ import annotations + +import logging +import sys +from pathlib import Path + +from coupler_panel import ( + CouplerError, + compute_coupler_measurement, + load_point_cloud, +) + + +def main(input_path: Path) -> int: + logging.basicConfig(level=logging.INFO, format="%(levelname)s %(name)s: %(message)s") + log = logging.getLogger("integrate_minimal") + + points = load_point_cloud(input_path) + log.info("loaded %d points from %s", points.shape[0], input_path) + + try: + result = compute_coupler_measurement( + points, + progress=lambda s, f: log.info("[%5.1f%%] %s", f * 100, s), + ) + except CouplerError as exc: + log.error("measurement failed: %s", exc) + return 2 + + print(f"theta_right_deg = {result.theta_right_deg:+.4f}") + print(f"theta_ref_deg = {result.theta_ref_deg:+.4f}") + print(f"D_y_max_mm = {result.D_y_max_mm:.3f}") + print(f"D_z_max_mm = {result.D_z_max_mm:.3f}") + print(f"dx_y_max_mm = {result.dx_y_max_mm:.3f}") + print(f"dx_z_max_mm = {result.dx_z_max_mm:.3f}") + print(f"center_C = {tuple(round(c, 3) for c in result.center_C)}") + print(f"normal = {tuple(round(c, 6) for c in result.normal)}") + print(f"quality_flags = {result.quality_flags}") + + return 0 + + +if __name__ == "__main__": + if len(sys.argv) != 2: + print(__doc__.strip()) + sys.exit(1) + sys.exit(main(Path(sys.argv[1]))) diff --git a/alg/delivery_v1.1/coupler_panel_release/examples/roi_all.json b/alg/delivery_v1.1/coupler_panel_release/examples/roi_all.json new file mode 100644 index 0000000..70381ba --- /dev/null +++ b/alg/delivery_v1.1/coupler_panel_release/examples/roi_all.json @@ -0,0 +1,8 @@ +{ + "x_min": -12000.0, + "x_max": 100.0, + "y_min": -300.0, + "y_max": 1200.0, + "z_min": -1000.0, + "z_max": 800.0 +} diff --git a/alg/delivery_v1.1/coupler_panel_release/examples/run_batch_eval.py b/alg/delivery_v1.1/coupler_panel_release/examples/run_batch_eval.py new file mode 100644 index 0000000..febd62a --- /dev/null +++ b/alg/delivery_v1.1/coupler_panel_release/examples/run_batch_eval.py @@ -0,0 +1,147 @@ +"""Batch evaluator: main measurement program with CAD + cloud + ROI inputs. + +For each PLY listed in the manifest: + +1. Load the cloud (PLY). +2. Apply the ROI JSON (currently ``mode='all'`` placeholder). +3. Feed (CAD STP, ROI'd cloud, GT JSON) into the existing measurement + pipeline — internal stages (denoise / Kabsch / detection / ROI / + plane fit / metrics) are unchanged. +4. Compare θ_right / θ_ref against the injected GT recorded in the + manifest. +5. Append a row to the Excel report. + +Run:: + + python examples/run_batch_eval.py \ + --cad test_data/lu1.stp \ + --data test_data/synth \ + --roi examples/roi_all.json \ + --out runs/batch_eval \ + --profile lu1_af4 +""" + +from __future__ import annotations + +import argparse +import json +import logging +import time +from pathlib import Path + +from coupler_panel.exceptions import CouplerError +from coupler_panel.io_utils import load_point_cloud +from coupler_panel.pipeline import run_coupler_measurement_pipeline +from coupler_panel.reporting import build_row_from_summary, write_excel_report +from coupler_panel.roi_select import load_roi_json + + +def _build_parser() -> argparse.ArgumentParser: + p = argparse.ArgumentParser(description=__doc__) + p.add_argument("--cad", type=Path, required=True, help="STEP file (panel CAD).") + p.add_argument("--data", type=Path, required=True, help="Directory with PLYs + manifest.json.") + p.add_argument("--roi", type=Path, required=True, help="ROI JSON file.") + p.add_argument("--out", type=Path, required=True, help="Output directory for runs + xlsx.") + p.add_argument("--profile", type=str, default="lu1_af4", help="Config profile.") + p.add_argument("--config", type=Path, default=None, help="Optional YAML override.") + return p + + +def main(argv: list[str] | None = None) -> int: + logging.basicConfig(level=logging.INFO, format="%(levelname)s %(name)s: %(message)s") + log = logging.getLogger("run_batch_eval") + args = _build_parser().parse_args(argv) + + args.out.mkdir(parents=True, exist_ok=True) + + manifest_path = args.data / "manifest.json" + with manifest_path.open("r", encoding="utf-8") as fh: + manifest = json.load(fh) + samples = manifest["samples"] + roi_spec = load_roi_json(args.roi) + log.info( + "loaded ROI spec (%s): x∈[%g,%g] y∈[%g,%g] z∈[%g,%g]", + roi_spec.kind, + roi_spec.x_min, roi_spec.x_max, + roi_spec.y_min, roi_spec.y_max, + roi_spec.z_min, roi_spec.z_max, + ) + log.info("samples in manifest: %d", len(samples)) + + rows = [] + for i, sample in enumerate(samples, start=1): + sid = sample["sample_id"] + ply = args.data / sample["ply"] + log.info("[%d/%d] %s — %s", i, len(samples), sid, ply.name) + + run_dir = args.out / sid + try: + # Pre-count points to record n_points_in (before ROI). + n_in = int(load_point_cloud(ply).shape[0]) + + # Main pipeline: CAD STP is parsed for the panel-normal seed + # only; the cloud is assumed to be in the CAD frame already + # (scanner extrinsics calibrated upstream). No Kabsch. + # The wall-clock duration from "inputs enter the algorithm" + # to "result returned" goes straight into the Excel report. + t_start = time.perf_counter() + result = run_coupler_measurement_pipeline( + input_cloud=ply, + out_dir=run_dir, + config_path=args.config, + profile=args.profile, + cad_stp_path=args.cad, + roi_spec=roi_spec, + ) + pipeline_seconds = time.perf_counter() - t_start + except CouplerError as exc: + log.error(" -> FAILED: %s", exc) + continue + except Exception as exc: + log.exception(" -> CRASH: %s", exc) + continue + + # n_after_roi is recorded in run_dir/roi.json.diagnostics.n_out + roi_diag_path = run_dir / "roi.json" + n_after = n_in + if roi_diag_path.exists(): + with roi_diag_path.open("r", encoding="utf-8") as fh: + n_after = int(json.load(fh).get("diagnostics", {}).get("n_out", n_in)) + + # Kabsch RMSE: pull from the prepared/registration log if present + # (the persisted pipeline does not currently expose this in the + # report dict; it's available via report.json side-files but for a + # minimum-touch wiring we leave it None when not surfaced). + registration = None + + row = build_row_from_summary( + sample_id=sid, + ply_path=str(ply), + roi_kind=roi_spec.kind, + measured=result.to_dict(), + gt_angles=(float(sample["theta_right_deg"]), float(sample["theta_ref_deg"])), + registration=registration, + n_points_in=n_in, + n_points_after_roi=n_after, + pipeline_seconds=pipeline_seconds, + ) + rows.append(row) + log.info(" θ_right pred=%+.4f° GT=%+.2f° err=%+.4f°", + row.theta_right_pred_deg, row.theta_right_gt_deg, row.theta_right_err_deg) + log.info(" θ_ref pred=%+.4f° GT=%+.2f° err=%+.4f°", + row.theta_ref_pred_deg, row.theta_ref_gt_deg, row.theta_ref_err_deg) + log.info(" runtime: %.3f s (inner stages: %.3f s)", + row.pipeline_seconds, row.inner_stage_seconds) + + if not rows: + log.error("no successful runs; nothing to write") + return 2 + + xlsx_path = args.out / "eval_report.xlsx" + write_excel_report(rows, xlsx_path) + log.info("wrote %s (%d rows)", xlsx_path, len(rows)) + return 0 + + +if __name__ == "__main__": + raise SystemExit(main()) diff --git a/alg/delivery_v1.1/coupler_panel_release/examples/run_full_validation.py b/alg/delivery_v1.1/coupler_panel_release/examples/run_full_validation.py new file mode 100644 index 0000000..809efb7 --- /dev/null +++ b/alg/delivery_v1.1/coupler_panel_release/examples/run_full_validation.py @@ -0,0 +1,43 @@ +"""Full research demo: STP → synthetic cloud → measure → diff GT. + +Run:: + + python examples/run_full_validation.py path/to/panel.stp runs/demo +""" + +from __future__ import annotations + +import json +import logging +import sys +from pathlib import Path + +from coupler_panel.pipeline import run_from_cad_pipeline + + +def main(stp: Path, out: Path) -> int: + logging.basicConfig(level=logging.INFO, format="%(levelname)s %(name)s: %(message)s") + summary = run_from_cad_pipeline( + stp_path=stp, + out_dir=out, + progress=lambda s, f: print(f"[{f*100:5.1f}%] {s}", flush=True), + ) + print("\n=== diff (measured - GT) ===") + print(json.dumps(summary["diff"], indent=2)) + + # Render diagnostic PNGs. + try: + from coupler_panel.viz import render_all + + render_all(out) + except ImportError: + print("(matplotlib not installed; skip rendering)") + + return 0 + + +if __name__ == "__main__": + if len(sys.argv) != 3: + print(__doc__.strip()) + sys.exit(1) + sys.exit(main(Path(sys.argv[1]), Path(sys.argv[2]))) diff --git a/alg/delivery_v1.1/coupler_panel_release/pyproject.toml b/alg/delivery_v1.1/coupler_panel_release/pyproject.toml new file mode 100644 index 0000000..e62b498 --- /dev/null +++ b/alg/delivery_v1.1/coupler_panel_release/pyproject.toml @@ -0,0 +1,89 @@ +[build-system] +requires = ["setuptools>=68", "wheel"] +build-backend = "setuptools.build_meta" + +[project] +name = "coupler-panel-validation" +version = "0.1.0" +description = "CAD-STP to point-cloud measurement validation system for coupler panel deflection." +readme = "README.md" +requires-python = ">=3.10" +license = { text = "Proprietary" } +authors = [{ name = "PKU High-Speed Train Project" }] +keywords = ["point-cloud", "cad", "step", "metrology", "rail", "coupler"] +classifiers = [ + "Development Status :: 3 - Alpha", + "Intended Audience :: Manufacturing", + "Programming Language :: Python :: 3", + "Programming Language :: Python :: 3.10", + "Programming Language :: Python :: 3.11", + "Programming Language :: Python :: 3.12", + "Topic :: Scientific/Engineering", +] + +# Core: pure compute (no plotting, no CAD). +dependencies = [ + "numpy>=1.24", + "scipy>=1.10", + "pyyaml>=6.0", + "tqdm>=4.65", +] + +[project.optional-dependencies] +# Required to run the full simulation / measurement pipeline: +# point-cloud IO, sampling, denoising, registration helpers. +cloud = [ + "open3d>=0.18", + "trimesh>=4.0", +] +# Required to parse STP files into a tessellated mesh and to extract +# topological GT (hole circles, panel plane). +cad = [ + "cadquery-ocp>=7.7", + "trimesh>=4.0", +] +# Visualisation (PNG diagnostics + offscreen 3D renders). +viz = [ + "matplotlib>=3.7", + "open3d>=0.18", +] +# Excel batch-evaluation reporting (reporting.excel). +report = [ + "openpyxl>=3.1", +] +# Convenience: install everything (compute + cad + cloud + plotting + report). +all = [ + "open3d>=0.18", + "trimesh>=4.0", + "cadquery-ocp>=7.7", + "matplotlib>=3.7", + "openpyxl>=3.1", +] +dev = [ + "pytest>=7", + "pytest-cov", +] + +[project.scripts] +coupler-measure = "coupler_panel.cli:main" + +[tool.setuptools] +packages = [ + "coupler_panel", + "coupler_panel.cad", + "coupler_panel.sampling", + "coupler_panel.simulation", + "coupler_panel.denoise", + "coupler_panel.registration", + "coupler_panel.detection", + "coupler_panel.geometry", + "coupler_panel.metrics", + "coupler_panel.viz", + "coupler_panel.io_utils", + "coupler_panel.roi_select", + "coupler_panel.reporting", +] +include-package-data = true + +[tool.setuptools.package-data] +coupler_panel = ["py.typed"] diff --git a/alg/delivery_v1.1/coupler_panel_release/requirements.txt b/alg/delivery_v1.1/coupler_panel_release/requirements.txt new file mode 100644 index 0000000..6da3bcf --- /dev/null +++ b/alg/delivery_v1.1/coupler_panel_release/requirements.txt @@ -0,0 +1,52 @@ +# coupler-panel-validation — locked dependency list. +# +# Versions in this file are the ones the package has been *end-to-end +# verified* against on Python 3.10 (Windows 11). For a clean install use: +# +# python -m pip install -r requirements.txt +# pip install -e . +# +# If you only need a subset of the functionality, use the optional +# extras declared in pyproject.toml instead: +# +# pip install -e ".[cloud]" # core + point-cloud IO + denoise +# pip install -e ".[cad]" # core + STP parsing + GT extraction +# pip install -e ".[viz]" # core + matplotlib diagnostics +# pip install -e ".[report]" # core + Excel batch reporting +# pip install -e ".[all]" # everything above +# +# --------------------------------------------------------------------------- +# Core compute — required even for the thinnest integration deployment. +# --------------------------------------------------------------------------- +numpy==1.26.4 +scipy==1.15.3 +pyyaml==6.0.2 +tqdm==4.65.0 + +# --------------------------------------------------------------------------- +# Point-cloud IO + denoise helpers (PLY/PCD read & write, voxel/SOR/ROR, +# normal-via-PCA). Required for the main pipeline; the package will fall +# back to ASCII-only PLY reading without open3d, which is not enough for +# the binary PLYs produced by the data generator. +# --------------------------------------------------------------------------- +open3d==0.19.0 +trimesh==4.12.2 + +# --------------------------------------------------------------------------- +# STP / STEP parsing — required if you pass cad_stp_path. (Skip this group +# if you don't have CAD STP at integration time and instead pre-compute +# the panel-normal seed yourself.) +# --------------------------------------------------------------------------- +cadquery-ocp==7.8.1.1.post1 + +# --------------------------------------------------------------------------- +# Diagnostic PNG renders + offscreen 3D screenshots — only used by +# coupler_panel.viz.render_all (optional). +# --------------------------------------------------------------------------- +matplotlib==3.10.8 + +# --------------------------------------------------------------------------- +# Excel batch-eval reporting — required if you use +# coupler_panel.reporting.write_excel_report or examples/run_batch_eval.py. +# --------------------------------------------------------------------------- +openpyxl==3.1.2 diff --git a/alg/delivery_v1.1/coupler_panel_release/test_data/01_gt.json b/alg/delivery_v1.1/coupler_panel_release/test_data/01_gt.json new file mode 100644 index 0000000..5f2ef58 --- /dev/null +++ b/alg/delivery_v1.1/coupler_panel_release/test_data/01_gt.json @@ -0,0 +1,1907 @@ +{ + "holes": { + "locating": [ + { + "center": [ + 0.0, + -99.00000120767, + -179.9999990121 + ], + "radius_mm": 16.0, + "fit_residual_rms_mm": 0.0, + "role": "locating" + }, + { + "center": [ + 0.0, + 100.9999987923, + -179.9999990121 + ], + "radius_mm": 16.0, + "fit_residual_rms_mm": 0.0, + "role": "locating" + }, + { + "center": [ + 0.0, + 100.9999987923, + 180.0000009879 + ], + "radius_mm": 16.0, + "fit_residual_rms_mm": 0.0, + "role": "locating" + }, + { + "center": [ + 0.0, + -99.00000120767, + 180.0000009879 + ], + "radius_mm": 16.0, + "fit_residual_rms_mm": 0.0, + "role": "locating" + } + ], + "central": { + "center": [ + 0.0, + 0.999998792332, + 0.0 + ], + "radius_mm": 100.0, + "fit_residual_rms_mm": 0.0, + "role": "central" + } + }, + "plane": { + "normal": [ + 1.0, + -0.0, + -0.0 + ], + "point_on_plane": [ + 0.0, + 0.9999987923150044, + 9.878999946977274e-07 + ], + "d_offset": -0.0, + "inlier_ratio": 1.0, + "inlier_residual_rms_mm": 0.0 + }, + "frame": { + "center_C": [ + 0.0, + 0.9999987923150044, + 9.878999946977274e-07 + ], + "line_direction": [ + 1.0, + 0.0, + 0.0 + ], + "reference_plane_z": 9.878999946977274e-07, + "longitudinal_mid_y": 0.9999987923150044 + }, + "raw_circles": [ + { + "center": [ + 0.0, + 106.0, + 669.0000154664 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 20.0 + }, + { + "center": [ + 0.0, + 136.0, + 699.0000154664 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 10.0 + }, + { + "center": [ + 0.0, + 169.0, + 689.9995359775 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 3.0 + }, + { + "center": [ + 0.0, + 169.0, + -689.9995359775 + ], + "axis": [ + 1.0, + 0.0, + -0.0 + ], + "radius": 3.0 + }, + { + "center": [ + 0.0, + 136.0, + -699.0000154664 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 10.0 + }, + { + "center": [ + 0.0, + 106.0, + -669.0000154664 + ], + "axis": [ + 1.0, + 0.0, + 0.0 + ], + "radius": 20.0 + }, + { + "center": [ + 0.0, + 0.999998792332, + 0.0 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 100.0 + }, + { + "center": [ + 0.0, + 0.999998792332, + 0.0 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 100.0 + }, + { + "center": [ + 0.0, + 40.0, + 180.0 + ], + "axis": [ + 1.0, + 0.0, + 0.0 + ], + "radius": 7.125 + }, + { + "center": [ + 0.0, + 40.0, + 180.0 + ], + "axis": [ + 1.0, + 0.0, + 0.0 + ], + "radius": 7.125 + }, + { + "center": [ + 0.0, + -40.0, + 180.0 + ], + "axis": [ + 1.0, + 0.0, + 0.0 + ], + "radius": 7.125 + }, + { + "center": [ + 0.0, + -40.0, + 180.0 + ], + "axis": [ + 1.0, + 0.0, + 0.0 + ], + "radius": 7.125 + }, + { + "center": [ + 0.0, + -40.0, + -180.0 + ], + "axis": [ + 1.0, + 0.0, + 0.0 + ], + "radius": 7.125 + }, + { + "center": [ + 0.0, + -40.0, + -180.0 + ], + "axis": [ + 1.0, + 0.0, + 0.0 + ], + "radius": 7.125 + }, + { + "center": [ + 0.0, + 40.0, + -180.0 + ], + "axis": [ + 1.0, + 0.0, + 0.0 + ], + "radius": 7.125 + }, + { + "center": [ + 0.0, + 40.0, + -180.0 + ], + "axis": [ + 1.0, + 0.0, + 0.0 + ], + "radius": 7.125 + }, + { + "center": [ + 0.0, + 100.9999987923, + -179.9999990121 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 16.0 + }, + { + "center": [ + 0.0, + 100.9999987923, + -179.9999990121 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 16.0 + }, + { + "center": [ + 0.0, + 100.9999987923, + 180.0000009879 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 16.0 + }, + { + "center": [ + 0.0, + 100.9999987923, + 180.0000009879 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 16.0 + }, + { + "center": [ + 0.0, + -99.00000120767, + -179.9999990121 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 16.0 + }, + { + "center": [ + 0.0, + -99.00000120767, + -179.9999990121 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 16.0 + }, + { + "center": [ + 0.0, + -99.00000120767, + 180.0000009879 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 16.0 + }, + { + "center": [ + 0.0, + -99.00000120767, + 180.0000009879 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 16.0 + }, + { + "center": [ + -80.0, + -99.00000120767, + -189.9999990121 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 40.0 + }, + { + "center": [ + -80.0, + -99.00000120767, + 190.0000009879 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 40.0 + }, + { + "center": [ + -79.0, + -99.00000120767, + 227.0000009879 + ], + "axis": [ + 0.0, + -1.0, + 0.0 + ], + "radius": 3.0 + }, + { + "center": [ + -80.0, + -99.00000120767, + 190.0000009879 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 40.0 + }, + { + "center": [ + -79.0, + -99.00000120767, + 190.0000009879 + ], + "axis": [ + 1.0, + 0.0, + -0.0 + ], + "radius": 40.0 + }, + { + "center": [ + -80.0, + -99.00000120767, + -189.9999990121 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 40.0 + }, + { + "center": [ + -79.0, + -99.00000120767, + -189.9999990121 + ], + "axis": [ + 1.0, + 0.0, + 0.0 + ], + "radius": 40.0 + }, + { + "center": [ + -79.0, + -99.00000120767, + -226.9999990121 + ], + "axis": [ + -0.0, + 1.0, + 0.0 + ], + "radius": 3.0 + }, + { + "center": [ + -76.0, + -99.00000120767, + 190.0000009879 + ], + "axis": [ + 1.0, + 0.0, + -0.0 + ], + "radius": 37.0 + }, + { + "center": [ + -76.0, + -99.00000120767, + -189.9999990121 + ], + "axis": [ + 1.0, + 0.0, + 0.0 + ], + "radius": 37.0 + }, + { + "center": [ + -76.0, + 106.0, + -669.0000154664 + ], + "axis": [ + 1.0, + 0.0, + 0.0 + ], + "radius": 20.0 + }, + { + "center": [ + -76.0, + 136.0, + -699.0000154664 + ], + "axis": [ + 1.0, + 0.0, + 0.0 + ], + "radius": 10.0 + }, + { + "center": [ + -76.0, + 169.0, + -689.9995359775 + ], + "axis": [ + 1.0, + 0.0, + 0.0 + ], + "radius": 3.0 + }, + { + "center": [ + -76.0, + 169.0, + 689.9995359775 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 3.0 + }, + { + "center": [ + -76.0, + 136.0, + 699.0000154664 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 10.0 + }, + { + "center": [ + -76.0, + 106.0, + 669.0000154664 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 20.0 + }, + { + "center": [ + -76.0, + -99.00000120767, + -179.9999990121 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 16.0 + }, + { + "center": [ + -76.0, + -99.00000120767, + -179.9999990121 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 16.0 + }, + { + "center": [ + -76.0, + 100.9999987923, + 180.0000009879 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 16.0 + }, + { + "center": [ + -76.0, + 100.9999987923, + 180.0000009879 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 16.0 + }, + { + "center": [ + -76.0, + 100.9999987923, + -179.9999990121 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 16.0 + }, + { + "center": [ + -76.0, + 100.9999987923, + -179.9999990121 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 16.0 + }, + { + "center": [ + -76.0, + 0.999998792332, + 0.0 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 100.0 + }, + { + "center": [ + -76.0, + 0.999998792332, + 0.0 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 100.0 + }, + { + "center": [ + -76.0, + -99.00000120767, + 180.0000009879 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 16.0 + }, + { + "center": [ + -76.0, + -99.00000120767, + 180.0000009879 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 16.0 + }, + { + "center": [ + -76.0, + -99.00000120767, + 180.0000009879 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 16.0 + }, + { + "center": [ + 0.0, + -99.00000120767, + 180.0000009879 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 16.0 + }, + { + "center": [ + -76.0, + -99.00000120767, + 180.0000009879 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 16.0 + }, + { + "center": [ + 0.0, + -99.00000120767, + 180.0000009879 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 16.0 + }, + { + "center": [ + -76.0, + -99.00000120767, + -179.9999990121 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 16.0 + }, + { + "center": [ + 0.0, + -99.00000120767, + -179.9999990121 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 16.0 + }, + { + "center": [ + -76.0, + -99.00000120767, + -179.9999990121 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 16.0 + }, + { + "center": [ + 0.0, + -99.00000120767, + -179.9999990121 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 16.0 + }, + { + "center": [ + -76.0, + 100.9999987923, + 180.0000009879 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 16.0 + }, + { + "center": [ + 0.0, + 100.9999987923, + 180.0000009879 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 16.0 + }, + { + "center": [ + -76.0, + 100.9999987923, + 180.0000009879 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 16.0 + }, + { + "center": [ + 0.0, + 100.9999987923, + 180.0000009879 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 16.0 + }, + { + "center": [ + -76.0, + 100.9999987923, + -179.9999990121 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 16.0 + }, + { + "center": [ + 0.0, + 100.9999987923, + -179.9999990121 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 16.0 + }, + { + "center": [ + -76.0, + 100.9999987923, + -179.9999990121 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 16.0 + }, + { + "center": [ + 0.0, + 100.9999987923, + -179.9999990121 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 16.0 + }, + { + "center": [ + -76.0, + 0.999998792332, + 0.0 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 100.0 + }, + { + "center": [ + 0.0, + 0.999998792332, + 0.0 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 100.0 + }, + { + "center": [ + -76.0, + 0.999998792332, + 0.0 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 100.0 + }, + { + "center": [ + 0.0, + 0.999998792332, + 0.0 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 100.0 + }, + { + "center": [ + -76.0, + -99.00000120767, + 190.0000009879 + ], + "axis": [ + 1.0, + 0.0, + -0.0 + ], + "radius": 37.0 + }, + { + "center": [ + -79.0, + -99.00000120767, + 227.0000009879 + ], + "axis": [ + 0.0, + -1.0, + 0.0 + ], + "radius": 3.0 + }, + { + "center": [ + -79.0, + -99.00000120767, + 190.0000009879 + ], + "axis": [ + 1.0, + 0.0, + -0.0 + ], + "radius": 40.0 + }, + { + "center": [ + -79.0, + -136.0000012077, + 190.0000009879 + ], + "axis": [ + 0.0, + 0.0, + -1.0 + ], + "radius": 3.0 + }, + { + "center": [ + -79.0, + -136.0000012077, + 190.0000009879 + ], + "axis": [ + 0.0, + 0.0, + -1.0 + ], + "radius": 3.0 + }, + { + "center": [ + -79.0, + -136.0000012077, + -189.9999990121 + ], + "axis": [ + 0.0, + 0.0, + -1.0 + ], + "radius": 3.0 + }, + { + "center": [ + -76.0, + -99.00000120767, + -189.9999990121 + ], + "axis": [ + 1.0, + 0.0, + 0.0 + ], + "radius": 37.0 + }, + { + "center": [ + -79.0, + -136.0000012077, + -189.9999990121 + ], + "axis": [ + 0.0, + 0.0, + -1.0 + ], + "radius": 3.0 + }, + { + "center": [ + -79.0, + -99.00000120767, + -189.9999990121 + ], + "axis": [ + 1.0, + 0.0, + 0.0 + ], + "radius": 40.0 + }, + { + "center": [ + -79.0, + -99.00000120767, + -226.9999990121 + ], + "axis": [ + -0.0, + 1.0, + 0.0 + ], + "radius": 3.0 + }, + { + "center": [ + -76.0, + 106.0, + -669.0000154664 + ], + "axis": [ + 1.0, + 0.0, + 0.0 + ], + "radius": 20.0 + }, + { + "center": [ + 0.0, + 106.0, + -669.0000154664 + ], + "axis": [ + 1.0, + 0.0, + 0.0 + ], + "radius": 20.0 + }, + { + "center": [ + 0.0, + 136.0, + -699.0000154664 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 10.0 + }, + { + "center": [ + -76.0, + 136.0, + -699.0000154664 + ], + "axis": [ + 1.0, + 0.0, + 0.0 + ], + "radius": 10.0 + }, + { + "center": [ + -76.0, + 169.0, + -689.9995359775 + ], + "axis": [ + 1.0, + 0.0, + 0.0 + ], + "radius": 3.0 + }, + { + "center": [ + 0.0, + 169.0, + -689.9995359775 + ], + "axis": [ + 1.0, + 0.0, + -0.0 + ], + "radius": 3.0 + }, + { + "center": [ + -76.0, + 169.0, + 689.9995359775 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 3.0 + }, + { + "center": [ + 0.0, + 169.0, + 689.9995359775 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 3.0 + }, + { + "center": [ + 0.0, + 136.0, + 699.0000154664 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 10.0 + }, + { + "center": [ + -76.0, + 136.0, + 699.0000154664 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 10.0 + }, + { + "center": [ + -76.0, + 106.0, + 669.0000154664 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 20.0 + }, + { + "center": [ + 0.0, + 106.0, + 669.0000154664 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 20.0 + }, + { + "center": [ + 0.0, + 40.0, + 180.0 + ], + "axis": [ + 1.0, + 0.0, + 0.0 + ], + "radius": 7.125 + }, + { + "center": [ + -0.725, + 40.0, + 180.0 + ], + "axis": [ + 1.0, + 0.0, + 0.0 + ], + "radius": 6.4 + }, + { + "center": [ + 0.0, + 40.0, + 180.0 + ], + "axis": [ + 1.0, + 0.0, + 0.0 + ], + "radius": 7.125 + }, + { + "center": [ + -0.725, + 40.0, + 180.0 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 6.4 + }, + { + "center": [ + -0.725, + 40.0, + 180.0 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 6.4 + }, + { + "center": [ + -40.0, + 40.0, + 180.0 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 6.4 + }, + { + "center": [ + -40.0, + 40.0, + 180.0 + ], + "axis": [ + 1.0, + 0.0, + 0.0 + ], + "radius": 6.4 + }, + { + "center": [ + -0.725, + 40.0, + 180.0 + ], + "axis": [ + 1.0, + 0.0, + 0.0 + ], + "radius": 6.4 + }, + { + "center": [ + -45.37023763953209, + 40.0, + 180.0 + ], + "axis": [ + 1.0, + 0.0, + 0.0 + ], + "radius": 1.7505996652289468e-12 + }, + { + "center": [ + -40.0, + 40.0, + 180.0 + ], + "axis": [ + 1.0, + 0.0, + 0.0 + ], + "radius": 6.4 + }, + { + "center": [ + -45.37023763953209, + 40.00000000000001, + 180.0 + ], + "axis": [ + 1.0, + -0.0, + 0.0 + ], + "radius": 1.7483792191796965e-12 + }, + { + "center": [ + -40.0, + 40.0, + 180.0 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 6.4 + }, + { + "center": [ + 0.0, + -40.0, + 180.0 + ], + "axis": [ + 1.0, + 0.0, + 0.0 + ], + "radius": 7.125 + }, + { + "center": [ + -0.725, + -40.0, + 180.0 + ], + "axis": [ + 1.0, + 0.0, + 0.0 + ], + "radius": 6.4 + }, + { + "center": [ + 0.0, + -40.0, + 180.0 + ], + "axis": [ + 1.0, + 0.0, + 0.0 + ], + "radius": 7.125 + }, + { + "center": [ + -0.725, + -40.0, + 180.0 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 6.4 + }, + { + "center": [ + -0.725, + -40.0, + 180.0 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 6.4 + }, + { + "center": [ + -40.0, + -40.0, + 180.0 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 6.4 + }, + { + "center": [ + -40.0, + -40.0, + 180.0 + ], + "axis": [ + 1.0, + 0.0, + 0.0 + ], + "radius": 6.4 + }, + { + "center": [ + -0.725, + -40.0, + 180.0 + ], + "axis": [ + 1.0, + 0.0, + 0.0 + ], + "radius": 6.4 + }, + { + "center": [ + -45.37023763953209, + -40.0, + 180.0 + ], + "axis": [ + 1.0, + 0.0, + 0.0 + ], + "radius": 1.7505996652289468e-12 + }, + { + "center": [ + -40.0, + -40.0, + 180.0 + ], + "axis": [ + 1.0, + 0.0, + 0.0 + ], + "radius": 6.4 + }, + { + "center": [ + -45.37023763953209, + -39.99999999999999, + 180.0 + ], + "axis": [ + 1.0, + -0.0, + 0.0 + ], + "radius": 1.7483792191796965e-12 + }, + { + "center": [ + -40.0, + -40.0, + 180.0 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 6.4 + }, + { + "center": [ + 0.0, + -40.0, + -180.0 + ], + "axis": [ + 1.0, + 0.0, + 0.0 + ], + "radius": 7.125 + }, + { + "center": [ + -0.725, + -40.0, + -180.0 + ], + "axis": [ + 1.0, + 0.0, + 0.0 + ], + "radius": 6.4 + }, + { + "center": [ + 0.0, + -40.0, + -180.0 + ], + "axis": [ + 1.0, + 0.0, + 0.0 + ], + "radius": 7.125 + }, + { + "center": [ + -0.725, + -40.0, + -180.0 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 6.4 + }, + { + "center": [ + -0.725, + -40.0, + -180.0 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 6.4 + }, + { + "center": [ + -40.0, + -40.0, + -180.0 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 6.4 + }, + { + "center": [ + -40.0, + -40.0, + -180.0 + ], + "axis": [ + 1.0, + 0.0, + 0.0 + ], + "radius": 6.4 + }, + { + "center": [ + -0.725, + -40.0, + -180.0 + ], + "axis": [ + 1.0, + 0.0, + 0.0 + ], + "radius": 6.4 + }, + { + "center": [ + -45.37023763953209, + -40.0, + -180.0 + ], + "axis": [ + 1.0, + 0.0, + 0.0 + ], + "radius": 1.7505996652289468e-12 + }, + { + "center": [ + -40.0, + -40.0, + -180.0 + ], + "axis": [ + 1.0, + 0.0, + 0.0 + ], + "radius": 6.4 + }, + { + "center": [ + -45.37023763953209, + -40.00000000000001, + -180.0 + ], + "axis": [ + 1.0, + -0.0, + 0.0 + ], + "radius": 1.7483792191796965e-12 + }, + { + "center": [ + -40.0, + -40.0, + -180.0 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 6.4 + }, + { + "center": [ + 0.0, + 40.0, + -180.0 + ], + "axis": [ + 1.0, + 0.0, + 0.0 + ], + "radius": 7.125 + }, + { + "center": [ + -0.725, + 40.0, + -180.0 + ], + "axis": [ + 1.0, + 0.0, + 0.0 + ], + "radius": 6.4 + }, + { + "center": [ + 0.0, + 40.0, + -180.0 + ], + "axis": [ + 1.0, + 0.0, + 0.0 + ], + "radius": 7.125 + }, + { + "center": [ + -0.725, + 40.0, + -180.0 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 6.4 + }, + { + "center": [ + -0.725, + 40.0, + -180.0 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 6.4 + }, + { + "center": [ + -40.0, + 40.0, + -180.0 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 6.4 + }, + { + "center": [ + -40.0, + 40.0, + -180.0 + ], + "axis": [ + 1.0, + 0.0, + 0.0 + ], + "radius": 6.4 + }, + { + "center": [ + -0.725, + 40.0, + -180.0 + ], + "axis": [ + 1.0, + 0.0, + 0.0 + ], + "radius": 6.4 + }, + { + "center": [ + -45.37023763953209, + 40.0, + -180.0 + ], + "axis": [ + 1.0, + 0.0, + 0.0 + ], + "radius": 1.7505996652289468e-12 + }, + { + "center": [ + -40.0, + 40.0, + -180.0 + ], + "axis": [ + 1.0, + 0.0, + 0.0 + ], + "radius": 6.4 + }, + { + "center": [ + -45.37023763953209, + 39.99999999999999, + -180.0 + ], + "axis": [ + 1.0, + -0.0, + 0.0 + ], + "radius": 1.7483792191796965e-12 + }, + { + "center": [ + -40.0, + 40.0, + -180.0 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 6.4 + } + ] +} \ No newline at end of file diff --git a/alg/delivery_v1.1/coupler_panel_release/test_data/10_metrics.json b/alg/delivery_v1.1/coupler_panel_release/test_data/10_metrics.json new file mode 100644 index 0000000..f73d814 --- /dev/null +++ b/alg/delivery_v1.1/coupler_panel_release/test_data/10_metrics.json @@ -0,0 +1,2512 @@ +{ + "stp": "D:\\myDocuments\\PKU\\high_speed_train_proj\\lu1-M000004835653_AF4_ASMP072708187_N_AF4.stp", + "panel_deflection_injected": { + "theta_right_deg": 0.5, + "theta_ref_deg": 0.3, + "R": [ + [ + 0.9999482158335473, + -0.008726535498373935, + -0.0052357644619607695 + ], + [ + 0.008726415877184502, + 0.9999619230641713, + -4.569182424308496e-05 + ], + [ + 0.00523596383141958, + 0.0, + 0.9999862922474267 + ] + ] + }, + "scan_pose_perturbation": [ + [ + 0.9999979783441972, + -0.0019163717426360569, + 0.0006089555505671704, + 1.4967337082610368 + ], + [ + 0.0019214737281491043, + 0.9999621035526889, + -0.008491136389996271, + 1.0855384993902797 + ], + [ + -0.000592660299474585, + 0.00849228931593314, + 0.9999637642314565, + 0.7624922116957428 + ], + [ + 0.0, + 0.0, + 0.0, + 1.0 + ] + ], + "registration": { + "matrix": [ + [ + 0.9999991698935765, + 0.0011435168147077325, + -0.0005937856954023229, + -1.42312474833891 + ], + [ + -0.001138363345940219, + 0.999962302539521, + 0.008607997952637694, + -1.3922014879928315 + ], + [ + 0.0006036067015895135, + -0.008607314863212156, + 0.9999627742020176, + -1.1453314127968168 + ], + [ + 0.0, + 0.0, + 0.0, + 1.0 + ] + ], + "rmse_mm": 1.279801346259311, + "max_residual_mm": 1.7348149080058168 + }, + "measured": { + "schema_version": "1.0", + "algorithm_version": "0.1.0", + "theta_right_deg": 0.544354519421902, + "theta_ref_deg": 0.2997466331976193, + "D_y_max_mm": 94.08063004084244, + "D_z_max_mm": 172.21417148158895, + "dx_y_max_mm": 0.893866137444892, + "dx_z_max_mm": 0.9009579671326199, + "center_C": [ + -0.007647024901048821, + 0.9158090934040075, + -0.06590994857895538 + ], + "normal": [ + 0.9999411851861564, + 0.00950050573095007, + 0.005231305703281311 + ], + "holes": { + "locating": [ + { + "center": [ + 1.8237358542499582, + -101.1715281534813, + -179.67591341367824 + ], + "radius_mm": 19.34475104619178, + "fit_residual_rms_mm": 2.2818699650684904, + "role": "locating" + }, + { + "center": [ + 0.05005657933939589, + 102.44798627110123, + -180.30256731680095 + ], + "radius_mm": 19.71317672500548, + "fit_residual_rms_mm": 3.220214937115534, + "role": "locating" + }, + { + "center": [ + -1.820222447435446, + 100.64818807060959, + 179.87714825154933 + ], + "radius_mm": 18.68294341110619, + "fit_residual_rms_mm": 2.0067435575879573, + "role": "locating" + }, + { + "center": [ + -0.08415808575810335, + -98.26140981461349, + 179.83769268461404 + ], + "radius_mm": 20.21695365907737, + "fit_residual_rms_mm": 2.5642764666909246, + "role": "locating" + } + ], + "central": null + }, + "plane": { + "normal": [ + 0.9999411851861564, + 0.00950050573095007, + 0.005231305703281311 + ], + "point_on_plane": [ + 0.07084859134750632, + -0.6075903692234997, + 0.7670896792673196 + ], + "d_offset": -0.06908488922999657, + "inlier_ratio": 1.0, + "inlier_residual_rms_mm": 0.04977002501374635 + }, + "frame": { + "center_C": [ + -0.007647024901048821, + 0.9158090934040075, + -0.06590994857895538 + ], + "line_direction": [ + 1.0, + 0.0, + 0.0 + ], + "reference_plane_z": -0.06590994857895538, + "longitudinal_mid_y": 0.9158090934040075 + }, + "quality_flags": [], + "timings": [ + { + "stage": "detect_holes", + "seconds": 2.090514300041832, + "peak_kib": 32527.3603515625 + }, + { + "stage": "build_frame", + "seconds": 4.1000021155923605e-05, + "peak_kib": 0.0 + }, + { + "stage": "extract_roi", + "seconds": 0.33327039994765073, + "peak_kib": 17444.6552734375 + }, + { + "stage": "fit_plane", + "seconds": 0.07231109996791929, + "peak_kib": 626.2568359375 + }, + { + "stage": "metrics", + "seconds": 4.96999709866941e-05, + "peak_kib": 0.0 + } + ], + "report": { + "schema_version": "1.0", + "algorithm_version": "0.1.0", + "config": { + "schema_version": "1.0", + "cad": { + "linear_deflection_mm": 0.5, + "angular_deflection_rad": 0.1, + "locating_hole_radius_mm": [ + 14.0, + 18.0 + ], + "central_hole_radius_min_mm": 80.0, + "rectangle_aspect_tol": 0.15 + }, + "sampling": { + "method": "grid_spacing", + "spacing_mm": null, + "num_points": 500000, + "seed": 42, + "adaptive": { + "curvature_weight": 1.0, + "min_spacing_factor": 0.25, + "max_spacing_factor": 4.0 + } + }, + "scan_simulator": { + "enabled": true, + "view_origin": [ + 10000.0, + 0.0, + 0.0 + ], + "hpr_radius_mm": 200000.0 + }, + "panel_deflection": { + "enabled": true, + "theta_right_deg": 0.5, + "theta_ref_deg": 0.3 + }, + "noise": { + "enabled": true, + "sigma_mm": 0.05, + "mode": "isotropic", + "seed": 43 + }, + "transform": { + "enabled": true, + "rotation_deg": 0.5, + "rotation_axis": "random", + "translation_mm": 2.0, + "translation_dir": "random", + "seed": 44 + }, + "denoise": { + "steps": [ + { + "type": "passthrough", + "bbox_padding_mm": 50.0 + }, + { + "type": "voxel", + "voxel_mm": 0.5 + }, + { + "type": "statistical", + "nb_neighbors": 20, + "std_ratio": 2.5 + } + ], + "smoothing": { + "enabled": false, + "search_radius_mm": 2.0, + "polynomial_order": 2 + } + }, + "registration": { + "method": "kabsch_four_holes", + "pre_align": "pca_centroid", + "refuse_if_residual_above_mm": 5.0 + }, + "detection": { + "panel_normal_seed": "pca", + "panel_normal_pca_kth": 1, + "grid_pixel_mm": 2.0, + "panel_min_area_ratio": 0.15, + "hole_min_area_mm2": 600.0, + "hole_max_area_mm2": 5000.0, + "circle_fit": { + "method": "algebraic_then_lsq", + "inlier_residual_mm": 1.5 + }, + "select_top_k_by_radius_match": 4, + "rectangle_aspect_tol": 0.4, + "fill_bg_below_mm2": 100.0, + "locating_hole_radius_mm": [ + 14.0, + 22.0 + ] + }, + "roi": { + "hole_dilate_mm": 5.0, + "central_hole_dilate_mm": 10.0, + "edge_buffer_mm": 8.0, + "chamfer_normal_dot_min": 0.8, + "outlier_z_score": 3.0 + }, + "plane_fit": { + "method": "ransac_svd", + "ransac_iterations": 300, + "ransac_inlier_thresh_mm": 0.5, + "min_inlier_ratio": 0.5, + "enforce_nx_positive": true + }, + "metrics": { + "right_axis_sign": 1, + "angle_clamp_deg": 0.0 + }, + "artifacts": { + "save_intermediate": true, + "ply_format": "binary", + "image_dpi": 150, + "heatmap_colormap": "viridis" + } + }, + "n_points_in": 184135, + "detection": { + "n_cavities": 4, + "n_circle_fits": 4, + "selection": { + "radius_score": 0.02869114494902652, + "rectangle_score": 0.025074534730867147, + "score": 0.053765679679893666, + "radii_mean": 19.489456210345203, + "radii_std": 0.5591748131087193, + "selected_indices": [ + 0, + 1, + 3, + 2 + ], + "selected_radii": [ + 19.34475104619178, + 19.71317672500548, + 18.68294341110619, + 20.21695365907737 + ] + } + }, + "roi": { + "n_in": 184135, + "n_out": 9855, + "stages": [ + { + "name": "rectangle", + "n_kept": 10009 + }, + { + "name": "locating_holes", + "n_kept": 9855 + }, + { + "name": "chamfer_normal", + "n_kept": 9855 + }, + { + "name": "z_score", + "n_kept": 9855 + } + ] + }, + "plane": { + "normal": [ + 0.9999411851861564, + 0.00950050573095007, + 0.005231305703281311 + ], + "point_on_plane": [ + 0.07084859134750632, + -0.6075903692234997, + 0.7670896792673196 + ], + "d_offset": -0.06908488922999657, + "inlier_ratio": 1.0, + "inlier_residual_rms_mm": 0.04977002501374635 + }, + "frame": { + "center_C": [ + -0.007647024901048821, + 0.9158090934040075, + -0.06590994857895538 + ], + "reference_plane_z": -0.06590994857895538, + "longitudinal_mid_y": 0.9158090934040075 + }, + "metrics": { + "theta_right_deg": 0.544354519421902, + "theta_ref_deg": 0.2997466331976193, + "D_y_max_mm": 94.08063004084244, + "D_z_max_mm": 172.21417148158895, + "dx_y_max_mm": 0.893866137444892, + "dx_z_max_mm": 0.9009579671326199, + "A": -0.009501064534291972, + "B": -0.0052316133996495135 + }, + "quality_flags": [], + "timings": [ + { + "stage": "detect_holes", + "seconds": 2.090514300041832, + "peak_kib": 32527.3603515625 + }, + { + "stage": "build_frame", + "seconds": 4.1000021155923605e-05, + "peak_kib": 0.0 + }, + { + "stage": "extract_roi", + "seconds": 0.33327039994765073, + "peak_kib": 17444.6552734375 + }, + { + "stage": "fit_plane", + "seconds": 0.07231109996791929, + "peak_kib": 626.2568359375 + }, + { + "stage": "metrics", + "seconds": 4.96999709866941e-05, + "peak_kib": 0.0 + } + ] + } + }, + "gt_cad": { + "holes": { + "locating": [ + { + "center": [ + 0.0, + -99.00000120767, + -179.9999990121 + ], + "radius_mm": 16.0, + "fit_residual_rms_mm": 0.0, + "role": "locating" + }, + { + "center": [ + 0.0, + 100.9999987923, + -179.9999990121 + ], + "radius_mm": 16.0, + "fit_residual_rms_mm": 0.0, + "role": "locating" + }, + { + "center": [ + 0.0, + 100.9999987923, + 180.0000009879 + ], + "radius_mm": 16.0, + "fit_residual_rms_mm": 0.0, + "role": "locating" + }, + { + "center": [ + 0.0, + -99.00000120767, + 180.0000009879 + ], + "radius_mm": 16.0, + "fit_residual_rms_mm": 0.0, + "role": "locating" + } + ], + "central": { + "center": [ + 0.0, + 0.999998792332, + 0.0 + ], + "radius_mm": 100.0, + "fit_residual_rms_mm": 0.0, + "role": "central" + } + }, + "plane": { + "normal": [ + 1.0, + -0.0, + -0.0 + ], + "point_on_plane": [ + 0.0, + 0.9999987923150044, + 9.878999946977274e-07 + ], + "d_offset": -0.0, + "inlier_ratio": 1.0, + "inlier_residual_rms_mm": 0.0 + }, + "frame": { + "center_C": [ + 0.0, + 0.9999987923150044, + 9.878999946977274e-07 + ], + "line_direction": [ + 1.0, + 0.0, + 0.0 + ], + "reference_plane_z": 9.878999946977274e-07, + "longitudinal_mid_y": 0.9999987923150044 + }, + "raw_circles": [ + { + "center": [ + 0.0, + 106.0, + 669.0000154664 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 20.0 + }, + { + "center": [ + 0.0, + 136.0, + 699.0000154664 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 10.0 + }, + { + "center": [ + 0.0, + 169.0, + 689.9995359775 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 3.0 + }, + { + "center": [ + 0.0, + 169.0, + -689.9995359775 + ], + "axis": [ + 1.0, + 0.0, + -0.0 + ], + "radius": 3.0 + }, + { + "center": [ + 0.0, + 136.0, + -699.0000154664 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 10.0 + }, + { + "center": [ + 0.0, + 106.0, + -669.0000154664 + ], + "axis": [ + 1.0, + 0.0, + 0.0 + ], + "radius": 20.0 + }, + { + "center": [ + 0.0, + 0.999998792332, + 0.0 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 100.0 + }, + { + "center": [ + 0.0, + 0.999998792332, + 0.0 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 100.0 + }, + { + "center": [ + 0.0, + 40.0, + 180.0 + ], + "axis": [ + 1.0, + 0.0, + 0.0 + ], + "radius": 7.125 + }, + { + "center": [ + 0.0, + 40.0, + 180.0 + ], + "axis": [ + 1.0, + 0.0, + 0.0 + ], + "radius": 7.125 + }, + { + "center": [ + 0.0, + -40.0, + 180.0 + ], + "axis": [ + 1.0, + 0.0, + 0.0 + ], + "radius": 7.125 + }, + { + "center": [ + 0.0, + -40.0, + 180.0 + ], + "axis": [ + 1.0, + 0.0, + 0.0 + ], + "radius": 7.125 + }, + { + "center": [ + 0.0, + -40.0, + -180.0 + ], + "axis": [ + 1.0, + 0.0, + 0.0 + ], + "radius": 7.125 + }, + { + "center": [ + 0.0, + -40.0, + -180.0 + ], + "axis": [ + 1.0, + 0.0, + 0.0 + ], + "radius": 7.125 + }, + { + "center": [ + 0.0, + 40.0, + -180.0 + ], + "axis": [ + 1.0, + 0.0, + 0.0 + ], + "radius": 7.125 + }, + { + "center": [ + 0.0, + 40.0, + -180.0 + ], + "axis": [ + 1.0, + 0.0, + 0.0 + ], + "radius": 7.125 + }, + { + "center": [ + 0.0, + 100.9999987923, + -179.9999990121 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 16.0 + }, + { + "center": [ + 0.0, + 100.9999987923, + -179.9999990121 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 16.0 + }, + { + "center": [ + 0.0, + 100.9999987923, + 180.0000009879 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 16.0 + }, + { + "center": [ + 0.0, + 100.9999987923, + 180.0000009879 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 16.0 + }, + { + "center": [ + 0.0, + -99.00000120767, + -179.9999990121 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 16.0 + }, + { + "center": [ + 0.0, + -99.00000120767, + -179.9999990121 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 16.0 + }, + { + "center": [ + 0.0, + -99.00000120767, + 180.0000009879 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 16.0 + }, + { + "center": [ + 0.0, + -99.00000120767, + 180.0000009879 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 16.0 + }, + { + "center": [ + -80.0, + -99.00000120767, + -189.9999990121 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 40.0 + }, + { + "center": [ + -80.0, + -99.00000120767, + 190.0000009879 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 40.0 + }, + { + "center": [ + -79.0, + -99.00000120767, + 227.0000009879 + ], + "axis": [ + 0.0, + -1.0, + 0.0 + ], + "radius": 3.0 + }, + { + "center": [ + -80.0, + -99.00000120767, + 190.0000009879 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 40.0 + }, + { + "center": [ + -79.0, + -99.00000120767, + 190.0000009879 + ], + "axis": [ + 1.0, + 0.0, + -0.0 + ], + "radius": 40.0 + }, + { + "center": [ + -80.0, + -99.00000120767, + -189.9999990121 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 40.0 + }, + { + "center": [ + -79.0, + -99.00000120767, + -189.9999990121 + ], + "axis": [ + 1.0, + 0.0, + 0.0 + ], + "radius": 40.0 + }, + { + "center": [ + -79.0, + -99.00000120767, + -226.9999990121 + ], + "axis": [ + -0.0, + 1.0, + 0.0 + ], + "radius": 3.0 + }, + { + "center": [ + -76.0, + -99.00000120767, + 190.0000009879 + ], + "axis": [ + 1.0, + 0.0, + -0.0 + ], + "radius": 37.0 + }, + { + "center": [ + -76.0, + -99.00000120767, + -189.9999990121 + ], + "axis": [ + 1.0, + 0.0, + 0.0 + ], + "radius": 37.0 + }, + { + "center": [ + -76.0, + 106.0, + -669.0000154664 + ], + "axis": [ + 1.0, + 0.0, + 0.0 + ], + "radius": 20.0 + }, + { + "center": [ + -76.0, + 136.0, + -699.0000154664 + ], + "axis": [ + 1.0, + 0.0, + 0.0 + ], + "radius": 10.0 + }, + { + "center": [ + -76.0, + 169.0, + -689.9995359775 + ], + "axis": [ + 1.0, + 0.0, + 0.0 + ], + "radius": 3.0 + }, + { + "center": [ + -76.0, + 169.0, + 689.9995359775 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 3.0 + }, + { + "center": [ + -76.0, + 136.0, + 699.0000154664 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 10.0 + }, + { + "center": [ + -76.0, + 106.0, + 669.0000154664 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 20.0 + }, + { + "center": [ + -76.0, + -99.00000120767, + -179.9999990121 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 16.0 + }, + { + "center": [ + -76.0, + -99.00000120767, + -179.9999990121 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 16.0 + }, + { + "center": [ + -76.0, + 100.9999987923, + 180.0000009879 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 16.0 + }, + { + "center": [ + -76.0, + 100.9999987923, + 180.0000009879 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 16.0 + }, + { + "center": [ + -76.0, + 100.9999987923, + -179.9999990121 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 16.0 + }, + { + "center": [ + -76.0, + 100.9999987923, + -179.9999990121 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 16.0 + }, + { + "center": [ + -76.0, + 0.999998792332, + 0.0 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 100.0 + }, + { + "center": [ + -76.0, + 0.999998792332, + 0.0 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 100.0 + }, + { + "center": [ + -76.0, + -99.00000120767, + 180.0000009879 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 16.0 + }, + { + "center": [ + -76.0, + -99.00000120767, + 180.0000009879 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 16.0 + }, + { + "center": [ + -76.0, + -99.00000120767, + 180.0000009879 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 16.0 + }, + { + "center": [ + 0.0, + -99.00000120767, + 180.0000009879 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 16.0 + }, + { + "center": [ + -76.0, + -99.00000120767, + 180.0000009879 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 16.0 + }, + { + "center": [ + 0.0, + -99.00000120767, + 180.0000009879 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 16.0 + }, + { + "center": [ + -76.0, + -99.00000120767, + -179.9999990121 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 16.0 + }, + { + "center": [ + 0.0, + -99.00000120767, + -179.9999990121 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 16.0 + }, + { + "center": [ + -76.0, + -99.00000120767, + -179.9999990121 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 16.0 + }, + { + "center": [ + 0.0, + -99.00000120767, + -179.9999990121 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 16.0 + }, + { + "center": [ + -76.0, + 100.9999987923, + 180.0000009879 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 16.0 + }, + { + "center": [ + 0.0, + 100.9999987923, + 180.0000009879 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 16.0 + }, + { + "center": [ + -76.0, + 100.9999987923, + 180.0000009879 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 16.0 + }, + { + "center": [ + 0.0, + 100.9999987923, + 180.0000009879 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 16.0 + }, + { + "center": [ + -76.0, + 100.9999987923, + -179.9999990121 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 16.0 + }, + { + "center": [ + 0.0, + 100.9999987923, + -179.9999990121 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 16.0 + }, + { + "center": [ + -76.0, + 100.9999987923, + -179.9999990121 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 16.0 + }, + { + "center": [ + 0.0, + 100.9999987923, + -179.9999990121 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 16.0 + }, + { + "center": [ + -76.0, + 0.999998792332, + 0.0 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 100.0 + }, + { + "center": [ + 0.0, + 0.999998792332, + 0.0 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 100.0 + }, + { + "center": [ + -76.0, + 0.999998792332, + 0.0 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 100.0 + }, + { + "center": [ + 0.0, + 0.999998792332, + 0.0 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 100.0 + }, + { + "center": [ + -76.0, + -99.00000120767, + 190.0000009879 + ], + "axis": [ + 1.0, + 0.0, + -0.0 + ], + "radius": 37.0 + }, + { + "center": [ + -79.0, + -99.00000120767, + 227.0000009879 + ], + "axis": [ + 0.0, + -1.0, + 0.0 + ], + "radius": 3.0 + }, + { + "center": [ + -79.0, + -99.00000120767, + 190.0000009879 + ], + "axis": [ + 1.0, + 0.0, + -0.0 + ], + "radius": 40.0 + }, + { + "center": [ + -79.0, + -136.0000012077, + 190.0000009879 + ], + "axis": [ + 0.0, + 0.0, + -1.0 + ], + "radius": 3.0 + }, + { + "center": [ + -79.0, + -136.0000012077, + 190.0000009879 + ], + "axis": [ + 0.0, + 0.0, + -1.0 + ], + "radius": 3.0 + }, + { + "center": [ + -79.0, + -136.0000012077, + -189.9999990121 + ], + "axis": [ + 0.0, + 0.0, + -1.0 + ], + "radius": 3.0 + }, + { + "center": [ + -76.0, + -99.00000120767, + -189.9999990121 + ], + "axis": [ + 1.0, + 0.0, + 0.0 + ], + "radius": 37.0 + }, + { + "center": [ + -79.0, + -136.0000012077, + -189.9999990121 + ], + "axis": [ + 0.0, + 0.0, + -1.0 + ], + "radius": 3.0 + }, + { + "center": [ + -79.0, + -99.00000120767, + -189.9999990121 + ], + "axis": [ + 1.0, + 0.0, + 0.0 + ], + "radius": 40.0 + }, + { + "center": [ + -79.0, + -99.00000120767, + -226.9999990121 + ], + "axis": [ + -0.0, + 1.0, + 0.0 + ], + "radius": 3.0 + }, + { + "center": [ + -76.0, + 106.0, + -669.0000154664 + ], + "axis": [ + 1.0, + 0.0, + 0.0 + ], + "radius": 20.0 + }, + { + "center": [ + 0.0, + 106.0, + -669.0000154664 + ], + "axis": [ + 1.0, + 0.0, + 0.0 + ], + "radius": 20.0 + }, + { + "center": [ + 0.0, + 136.0, + -699.0000154664 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 10.0 + }, + { + "center": [ + -76.0, + 136.0, + -699.0000154664 + ], + "axis": [ + 1.0, + 0.0, + 0.0 + ], + "radius": 10.0 + }, + { + "center": [ + -76.0, + 169.0, + -689.9995359775 + ], + "axis": [ + 1.0, + 0.0, + 0.0 + ], + "radius": 3.0 + }, + { + "center": [ + 0.0, + 169.0, + -689.9995359775 + ], + "axis": [ + 1.0, + 0.0, + -0.0 + ], + "radius": 3.0 + }, + { + "center": [ + -76.0, + 169.0, + 689.9995359775 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 3.0 + }, + { + "center": [ + 0.0, + 169.0, + 689.9995359775 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 3.0 + }, + { + "center": [ + 0.0, + 136.0, + 699.0000154664 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 10.0 + }, + { + "center": [ + -76.0, + 136.0, + 699.0000154664 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 10.0 + }, + { + "center": [ + -76.0, + 106.0, + 669.0000154664 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 20.0 + }, + { + "center": [ + 0.0, + 106.0, + 669.0000154664 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 20.0 + }, + { + "center": [ + 0.0, + 40.0, + 180.0 + ], + "axis": [ + 1.0, + 0.0, + 0.0 + ], + "radius": 7.125 + }, + { + "center": [ + -0.725, + 40.0, + 180.0 + ], + "axis": [ + 1.0, + 0.0, + 0.0 + ], + "radius": 6.4 + }, + { + "center": [ + 0.0, + 40.0, + 180.0 + ], + "axis": [ + 1.0, + 0.0, + 0.0 + ], + "radius": 7.125 + }, + { + "center": [ + -0.725, + 40.0, + 180.0 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 6.4 + }, + { + "center": [ + -0.725, + 40.0, + 180.0 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 6.4 + }, + { + "center": [ + -40.0, + 40.0, + 180.0 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 6.4 + }, + { + "center": [ + -40.0, + 40.0, + 180.0 + ], + "axis": [ + 1.0, + 0.0, + 0.0 + ], + "radius": 6.4 + }, + { + "center": [ + -0.725, + 40.0, + 180.0 + ], + "axis": [ + 1.0, + 0.0, + 0.0 + ], + "radius": 6.4 + }, + { + "center": [ + -45.37023763953209, + 40.0, + 180.0 + ], + "axis": [ + 1.0, + 0.0, + 0.0 + ], + "radius": 1.7505996652289468e-12 + }, + { + "center": [ + -40.0, + 40.0, + 180.0 + ], + "axis": [ + 1.0, + 0.0, + 0.0 + ], + "radius": 6.4 + }, + { + "center": [ + -45.37023763953209, + 40.00000000000001, + 180.0 + ], + "axis": [ + 1.0, + -0.0, + 0.0 + ], + "radius": 1.7483792191796965e-12 + }, + { + "center": [ + -40.0, + 40.0, + 180.0 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 6.4 + }, + { + "center": [ + 0.0, + -40.0, + 180.0 + ], + "axis": [ + 1.0, + 0.0, + 0.0 + ], + "radius": 7.125 + }, + { + "center": [ + -0.725, + -40.0, + 180.0 + ], + "axis": [ + 1.0, + 0.0, + 0.0 + ], + "radius": 6.4 + }, + { + "center": [ + 0.0, + -40.0, + 180.0 + ], + "axis": [ + 1.0, + 0.0, + 0.0 + ], + "radius": 7.125 + }, + { + "center": [ + -0.725, + -40.0, + 180.0 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 6.4 + }, + { + "center": [ + -0.725, + -40.0, + 180.0 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 6.4 + }, + { + "center": [ + -40.0, + -40.0, + 180.0 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 6.4 + }, + { + "center": [ + -40.0, + -40.0, + 180.0 + ], + "axis": [ + 1.0, + 0.0, + 0.0 + ], + "radius": 6.4 + }, + { + "center": [ + -0.725, + -40.0, + 180.0 + ], + "axis": [ + 1.0, + 0.0, + 0.0 + ], + "radius": 6.4 + }, + { + "center": [ + -45.37023763953209, + -40.0, + 180.0 + ], + "axis": [ + 1.0, + 0.0, + 0.0 + ], + "radius": 1.7505996652289468e-12 + }, + { + "center": [ + -40.0, + -40.0, + 180.0 + ], + "axis": [ + 1.0, + 0.0, + 0.0 + ], + "radius": 6.4 + }, + { + "center": [ + -45.37023763953209, + -39.99999999999999, + 180.0 + ], + "axis": [ + 1.0, + -0.0, + 0.0 + ], + "radius": 1.7483792191796965e-12 + }, + { + "center": [ + -40.0, + -40.0, + 180.0 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 6.4 + }, + { + "center": [ + 0.0, + -40.0, + -180.0 + ], + "axis": [ + 1.0, + 0.0, + 0.0 + ], + "radius": 7.125 + }, + { + "center": [ + -0.725, + -40.0, + -180.0 + ], + "axis": [ + 1.0, + 0.0, + 0.0 + ], + "radius": 6.4 + }, + { + "center": [ + 0.0, + -40.0, + -180.0 + ], + "axis": [ + 1.0, + 0.0, + 0.0 + ], + "radius": 7.125 + }, + { + "center": [ + -0.725, + -40.0, + -180.0 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 6.4 + }, + { + "center": [ + -0.725, + -40.0, + -180.0 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 6.4 + }, + { + "center": [ + -40.0, + -40.0, + -180.0 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 6.4 + }, + { + "center": [ + -40.0, + -40.0, + -180.0 + ], + "axis": [ + 1.0, + 0.0, + 0.0 + ], + "radius": 6.4 + }, + { + "center": [ + -0.725, + -40.0, + -180.0 + ], + "axis": [ + 1.0, + 0.0, + 0.0 + ], + "radius": 6.4 + }, + { + "center": [ + -45.37023763953209, + -40.0, + -180.0 + ], + "axis": [ + 1.0, + 0.0, + 0.0 + ], + "radius": 1.7505996652289468e-12 + }, + { + "center": [ + -40.0, + -40.0, + -180.0 + ], + "axis": [ + 1.0, + 0.0, + 0.0 + ], + "radius": 6.4 + }, + { + "center": [ + -45.37023763953209, + -40.00000000000001, + -180.0 + ], + "axis": [ + 1.0, + -0.0, + 0.0 + ], + "radius": 1.7483792191796965e-12 + }, + { + "center": [ + -40.0, + -40.0, + -180.0 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 6.4 + }, + { + "center": [ + 0.0, + 40.0, + -180.0 + ], + "axis": [ + 1.0, + 0.0, + 0.0 + ], + "radius": 7.125 + }, + { + "center": [ + -0.725, + 40.0, + -180.0 + ], + "axis": [ + 1.0, + 0.0, + 0.0 + ], + "radius": 6.4 + }, + { + "center": [ + 0.0, + 40.0, + -180.0 + ], + "axis": [ + 1.0, + 0.0, + 0.0 + ], + "radius": 7.125 + }, + { + "center": [ + -0.725, + 40.0, + -180.0 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 6.4 + }, + { + "center": [ + -0.725, + 40.0, + -180.0 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 6.4 + }, + { + "center": [ + -40.0, + 40.0, + -180.0 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 6.4 + }, + { + "center": [ + -40.0, + 40.0, + -180.0 + ], + "axis": [ + 1.0, + 0.0, + 0.0 + ], + "radius": 6.4 + }, + { + "center": [ + -0.725, + 40.0, + -180.0 + ], + "axis": [ + 1.0, + 0.0, + 0.0 + ], + "radius": 6.4 + }, + { + "center": [ + -45.37023763953209, + 40.0, + -180.0 + ], + "axis": [ + 1.0, + 0.0, + 0.0 + ], + "radius": 1.7505996652289468e-12 + }, + { + "center": [ + -40.0, + 40.0, + -180.0 + ], + "axis": [ + 1.0, + 0.0, + 0.0 + ], + "radius": 6.4 + }, + { + "center": [ + -45.37023763953209, + 39.99999999999999, + -180.0 + ], + "axis": [ + 1.0, + -0.0, + 0.0 + ], + "radius": 1.7483792191796965e-12 + }, + { + "center": [ + -40.0, + 40.0, + -180.0 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 6.4 + } + ] + }, + "gt_after_deflection": { + "normal": [ + 0.9999482158335473, + 0.008726415877184502, + 0.00523596383141958 + ], + "locating_centers": [ + [ + 1.8063646228583214, + -98.98800706265835, + -179.99753161665035 + ], + [ + 0.061057523183796314, + 101.00437755014592, + -179.99753161665035 + ], + [ + -1.8238176831220807, + 100.98792849341841, + 179.99753359242328 + ], + [ + -0.07851058344755557, + -99.00445611938586, + 179.99753359242328 + ] + ] + }, + "gt_metrics": { + "theta_right_deg": 0.5, + "theta_ref_deg": 0.30001142330692215, + "D_y_max_mm": 100.00441683476589, + "D_z_max_mm": 179.99753260453681, + "dx_y_max_mm": 0.8727253242089344, + "dx_z_max_mm": 0.9425093775246102, + "A": -0.00872686779075879, + "B": -0.005236234985483653 + }, + "diff": { + "theta_right_deg": 0.04435451942190205, + "theta_ref_deg": -0.0002647901093028593, + "D_y_max_mm": -5.923786793923455, + "D_z_max_mm": -7.783361122947866, + "dx_y_max_mm": 0.021140813235957512, + "dx_z_max_mm": -0.041551410391990284 + }, + "config": { + "schema_version": "1.0", + "cad": { + "linear_deflection_mm": 0.5, + "angular_deflection_rad": 0.1, + "locating_hole_radius_mm": [ + 14.0, + 18.0 + ], + "central_hole_radius_min_mm": 80.0, + "rectangle_aspect_tol": 0.15 + }, + "sampling": { + "method": "grid_spacing", + "spacing_mm": null, + "num_points": 500000, + "seed": 42, + "adaptive": { + "curvature_weight": 1.0, + "min_spacing_factor": 0.25, + "max_spacing_factor": 4.0 + } + }, + "scan_simulator": { + "enabled": true, + "view_origin": [ + 10000.0, + 0.0, + 0.0 + ], + "hpr_radius_mm": 200000.0 + }, + "panel_deflection": { + "enabled": true, + "theta_right_deg": 0.5, + "theta_ref_deg": 0.3 + }, + "noise": { + "enabled": true, + "sigma_mm": 0.05, + "mode": "isotropic", + "seed": 43 + }, + "transform": { + "enabled": true, + "rotation_deg": 0.5, + "rotation_axis": "random", + "translation_mm": 2.0, + "translation_dir": "random", + "seed": 44 + }, + "denoise": { + "steps": [ + { + "type": "passthrough", + "bbox_padding_mm": 50.0 + }, + { + "type": "voxel", + "voxel_mm": 0.5 + }, + { + "type": "statistical", + "nb_neighbors": 20, + "std_ratio": 2.5 + } + ], + "smoothing": { + "enabled": false, + "search_radius_mm": 2.0, + "polynomial_order": 2 + } + }, + "registration": { + "method": "kabsch_four_holes", + "pre_align": "pca_centroid", + "refuse_if_residual_above_mm": 5.0 + }, + "detection": { + "panel_normal_seed": "pca", + "panel_normal_pca_kth": 1, + "grid_pixel_mm": 2.0, + "panel_min_area_ratio": 0.15, + "hole_min_area_mm2": 600.0, + "hole_max_area_mm2": 5000.0, + "circle_fit": { + "method": "algebraic_then_lsq", + "inlier_residual_mm": 1.5 + }, + "select_top_k_by_radius_match": 4, + "rectangle_aspect_tol": 0.4, + "fill_bg_below_mm2": 100.0, + "locating_hole_radius_mm": [ + 14.0, + 22.0 + ] + }, + "roi": { + "hole_dilate_mm": 5.0, + "central_hole_dilate_mm": 10.0, + "edge_buffer_mm": 8.0, + "chamfer_normal_dot_min": 0.8, + "outlier_z_score": 3.0 + }, + "plane_fit": { + "method": "ransac_svd", + "ransac_iterations": 300, + "ransac_inlier_thresh_mm": 0.5, + "min_inlier_ratio": 0.5, + "enforce_nx_positive": true + }, + "metrics": { + "right_axis_sign": 1, + "angle_clamp_deg": 0.0 + }, + "artifacts": { + "save_intermediate": true, + "ply_format": "binary", + "image_dpi": 150, + "heatmap_colormap": "viridis" + } + } +} \ No newline at end of file diff --git a/alg/delivery_v1.1/coupler_panel_release/test_data/config.resolved.yaml b/alg/delivery_v1.1/coupler_panel_release/test_data/config.resolved.yaml new file mode 100644 index 0000000..346e75c --- /dev/null +++ b/alg/delivery_v1.1/coupler_panel_release/test_data/config.resolved.yaml @@ -0,0 +1,94 @@ +schema_version: '1.0' +cad: + linear_deflection_mm: 0.5 + angular_deflection_rad: 0.1 + locating_hole_radius_mm: + - 14.0 + - 18.0 + central_hole_radius_min_mm: 80.0 + rectangle_aspect_tol: 0.15 +sampling: + method: grid_spacing + spacing_mm: null + num_points: 500000 + seed: 42 + adaptive: + curvature_weight: 1.0 + min_spacing_factor: 0.25 + max_spacing_factor: 4.0 +scan_simulator: + enabled: true + view_origin: + - 10000.0 + - 0.0 + - 0.0 + hpr_radius_mm: 200000.0 +panel_deflection: + enabled: true + theta_right_deg: 0.5 + theta_ref_deg: 0.3 +noise: + enabled: true + sigma_mm: 0.05 + mode: isotropic + seed: 43 +transform: + enabled: true + rotation_deg: 0.5 + rotation_axis: random + translation_mm: 2.0 + translation_dir: random + seed: 44 +denoise: + steps: + - type: passthrough + bbox_padding_mm: 50.0 + - type: voxel + voxel_mm: 0.5 + - type: statistical + nb_neighbors: 20 + std_ratio: 2.5 + smoothing: + enabled: false + search_radius_mm: 2.0 + polynomial_order: 2 +registration: + method: kabsch_four_holes + pre_align: pca_centroid + refuse_if_residual_above_mm: 5.0 +detection: + panel_normal_seed: pca + panel_normal_pca_kth: 1 + grid_pixel_mm: 2.0 + panel_min_area_ratio: 0.15 + hole_min_area_mm2: 600.0 + hole_max_area_mm2: 5000.0 + circle_fit: + method: algebraic_then_lsq + inlier_residual_mm: 1.5 + select_top_k_by_radius_match: 4 + rectangle_aspect_tol: 0.4 + fill_bg_below_mm2: 100.0 + locating_hole_radius_mm: + - 14.0 + - 22.0 +roi: + hole_dilate_mm: 5.0 + central_hole_dilate_mm: 10.0 + edge_buffer_mm: 8.0 + chamfer_normal_dot_min: 0.8 + outlier_z_score: 3.0 +plane_fit: + method: ransac_svd + ransac_iterations: 300 + ransac_inlier_thresh_mm: 0.5 + min_inlier_ratio: 0.5 + enforce_nx_positive: true +metrics: + right_axis_sign: 1 + angle_clamp_deg: 0.0 +artifacts: + save_intermediate: true + ply_format: binary + image_dpi: 150 + heatmap_colormap: viridis diff --git a/alg/delivery_v1.1/coupler_panel_release/test_data/report.json b/alg/delivery_v1.1/coupler_panel_release/test_data/report.json new file mode 100644 index 0000000..f73d814 --- /dev/null +++ b/alg/delivery_v1.1/coupler_panel_release/test_data/report.json @@ -0,0 +1,2512 @@ +{ + "stp": "D:\\myDocuments\\PKU\\high_speed_train_proj\\lu1-M000004835653_AF4_ASMP072708187_N_AF4.stp", + "panel_deflection_injected": { + "theta_right_deg": 0.5, + "theta_ref_deg": 0.3, + "R": [ + [ + 0.9999482158335473, + -0.008726535498373935, + -0.0052357644619607695 + ], + [ + 0.008726415877184502, + 0.9999619230641713, + -4.569182424308496e-05 + ], + [ + 0.00523596383141958, + 0.0, + 0.9999862922474267 + ] + ] + }, + "scan_pose_perturbation": [ + [ + 0.9999979783441972, + -0.0019163717426360569, + 0.0006089555505671704, + 1.4967337082610368 + ], + [ + 0.0019214737281491043, + 0.9999621035526889, + -0.008491136389996271, + 1.0855384993902797 + ], + [ + -0.000592660299474585, + 0.00849228931593314, + 0.9999637642314565, + 0.7624922116957428 + ], + [ + 0.0, + 0.0, + 0.0, + 1.0 + ] + ], + "registration": { + "matrix": [ + [ + 0.9999991698935765, + 0.0011435168147077325, + -0.0005937856954023229, + -1.42312474833891 + ], + [ + -0.001138363345940219, + 0.999962302539521, + 0.008607997952637694, + -1.3922014879928315 + ], + [ + 0.0006036067015895135, + -0.008607314863212156, + 0.9999627742020176, + -1.1453314127968168 + ], + [ + 0.0, + 0.0, + 0.0, + 1.0 + ] + ], + "rmse_mm": 1.279801346259311, + "max_residual_mm": 1.7348149080058168 + }, + "measured": { + "schema_version": "1.0", + "algorithm_version": "0.1.0", + "theta_right_deg": 0.544354519421902, + "theta_ref_deg": 0.2997466331976193, + "D_y_max_mm": 94.08063004084244, + "D_z_max_mm": 172.21417148158895, + "dx_y_max_mm": 0.893866137444892, + "dx_z_max_mm": 0.9009579671326199, + "center_C": [ + -0.007647024901048821, + 0.9158090934040075, + -0.06590994857895538 + ], + "normal": [ + 0.9999411851861564, + 0.00950050573095007, + 0.005231305703281311 + ], + "holes": { + "locating": [ + { + "center": [ + 1.8237358542499582, + -101.1715281534813, + -179.67591341367824 + ], + "radius_mm": 19.34475104619178, + "fit_residual_rms_mm": 2.2818699650684904, + "role": "locating" + }, + { + "center": [ + 0.05005657933939589, + 102.44798627110123, + -180.30256731680095 + ], + "radius_mm": 19.71317672500548, + "fit_residual_rms_mm": 3.220214937115534, + "role": "locating" + }, + { + "center": [ + -1.820222447435446, + 100.64818807060959, + 179.87714825154933 + ], + "radius_mm": 18.68294341110619, + "fit_residual_rms_mm": 2.0067435575879573, + "role": "locating" + }, + { + "center": [ + -0.08415808575810335, + -98.26140981461349, + 179.83769268461404 + ], + "radius_mm": 20.21695365907737, + "fit_residual_rms_mm": 2.5642764666909246, + "role": "locating" + } + ], + "central": null + }, + "plane": { + "normal": [ + 0.9999411851861564, + 0.00950050573095007, + 0.005231305703281311 + ], + "point_on_plane": [ + 0.07084859134750632, + -0.6075903692234997, + 0.7670896792673196 + ], + "d_offset": -0.06908488922999657, + "inlier_ratio": 1.0, + "inlier_residual_rms_mm": 0.04977002501374635 + }, + "frame": { + "center_C": [ + -0.007647024901048821, + 0.9158090934040075, + -0.06590994857895538 + ], + "line_direction": [ + 1.0, + 0.0, + 0.0 + ], + "reference_plane_z": -0.06590994857895538, + "longitudinal_mid_y": 0.9158090934040075 + }, + "quality_flags": [], + "timings": [ + { + "stage": "detect_holes", + "seconds": 2.090514300041832, + "peak_kib": 32527.3603515625 + }, + { + "stage": "build_frame", + "seconds": 4.1000021155923605e-05, + "peak_kib": 0.0 + }, + { + "stage": "extract_roi", + "seconds": 0.33327039994765073, + "peak_kib": 17444.6552734375 + }, + { + "stage": "fit_plane", + "seconds": 0.07231109996791929, + "peak_kib": 626.2568359375 + }, + { + "stage": "metrics", + "seconds": 4.96999709866941e-05, + "peak_kib": 0.0 + } + ], + "report": { + "schema_version": "1.0", + "algorithm_version": "0.1.0", + "config": { + "schema_version": "1.0", + "cad": { + "linear_deflection_mm": 0.5, + "angular_deflection_rad": 0.1, + "locating_hole_radius_mm": [ + 14.0, + 18.0 + ], + "central_hole_radius_min_mm": 80.0, + "rectangle_aspect_tol": 0.15 + }, + "sampling": { + "method": "grid_spacing", + "spacing_mm": null, + "num_points": 500000, + "seed": 42, + "adaptive": { + "curvature_weight": 1.0, + "min_spacing_factor": 0.25, + "max_spacing_factor": 4.0 + } + }, + "scan_simulator": { + "enabled": true, + "view_origin": [ + 10000.0, + 0.0, + 0.0 + ], + "hpr_radius_mm": 200000.0 + }, + "panel_deflection": { + "enabled": true, + "theta_right_deg": 0.5, + "theta_ref_deg": 0.3 + }, + "noise": { + "enabled": true, + "sigma_mm": 0.05, + "mode": "isotropic", + "seed": 43 + }, + "transform": { + "enabled": true, + "rotation_deg": 0.5, + "rotation_axis": "random", + "translation_mm": 2.0, + "translation_dir": "random", + "seed": 44 + }, + "denoise": { + "steps": [ + { + "type": "passthrough", + "bbox_padding_mm": 50.0 + }, + { + "type": "voxel", + "voxel_mm": 0.5 + }, + { + "type": "statistical", + "nb_neighbors": 20, + "std_ratio": 2.5 + } + ], + "smoothing": { + "enabled": false, + "search_radius_mm": 2.0, + "polynomial_order": 2 + } + }, + "registration": { + "method": "kabsch_four_holes", + "pre_align": "pca_centroid", + "refuse_if_residual_above_mm": 5.0 + }, + "detection": { + "panel_normal_seed": "pca", + "panel_normal_pca_kth": 1, + "grid_pixel_mm": 2.0, + "panel_min_area_ratio": 0.15, + "hole_min_area_mm2": 600.0, + "hole_max_area_mm2": 5000.0, + "circle_fit": { + "method": "algebraic_then_lsq", + "inlier_residual_mm": 1.5 + }, + "select_top_k_by_radius_match": 4, + "rectangle_aspect_tol": 0.4, + "fill_bg_below_mm2": 100.0, + "locating_hole_radius_mm": [ + 14.0, + 22.0 + ] + }, + "roi": { + "hole_dilate_mm": 5.0, + "central_hole_dilate_mm": 10.0, + "edge_buffer_mm": 8.0, + "chamfer_normal_dot_min": 0.8, + "outlier_z_score": 3.0 + }, + "plane_fit": { + "method": "ransac_svd", + "ransac_iterations": 300, + "ransac_inlier_thresh_mm": 0.5, + "min_inlier_ratio": 0.5, + "enforce_nx_positive": true + }, + "metrics": { + "right_axis_sign": 1, + "angle_clamp_deg": 0.0 + }, + "artifacts": { + "save_intermediate": true, + "ply_format": "binary", + "image_dpi": 150, + "heatmap_colormap": "viridis" + } + }, + "n_points_in": 184135, + "detection": { + "n_cavities": 4, + "n_circle_fits": 4, + "selection": { + "radius_score": 0.02869114494902652, + "rectangle_score": 0.025074534730867147, + "score": 0.053765679679893666, + "radii_mean": 19.489456210345203, + "radii_std": 0.5591748131087193, + "selected_indices": [ + 0, + 1, + 3, + 2 + ], + "selected_radii": [ + 19.34475104619178, + 19.71317672500548, + 18.68294341110619, + 20.21695365907737 + ] + } + }, + "roi": { + "n_in": 184135, + "n_out": 9855, + "stages": [ + { + "name": "rectangle", + "n_kept": 10009 + }, + { + "name": "locating_holes", + "n_kept": 9855 + }, + { + "name": "chamfer_normal", + "n_kept": 9855 + }, + { + "name": "z_score", + "n_kept": 9855 + } + ] + }, + "plane": { + "normal": [ + 0.9999411851861564, + 0.00950050573095007, + 0.005231305703281311 + ], + "point_on_plane": [ + 0.07084859134750632, + -0.6075903692234997, + 0.7670896792673196 + ], + "d_offset": -0.06908488922999657, + "inlier_ratio": 1.0, + "inlier_residual_rms_mm": 0.04977002501374635 + }, + "frame": { + "center_C": [ + -0.007647024901048821, + 0.9158090934040075, + -0.06590994857895538 + ], + "reference_plane_z": -0.06590994857895538, + "longitudinal_mid_y": 0.9158090934040075 + }, + "metrics": { + "theta_right_deg": 0.544354519421902, + "theta_ref_deg": 0.2997466331976193, + "D_y_max_mm": 94.08063004084244, + "D_z_max_mm": 172.21417148158895, + "dx_y_max_mm": 0.893866137444892, + "dx_z_max_mm": 0.9009579671326199, + "A": -0.009501064534291972, + "B": -0.0052316133996495135 + }, + "quality_flags": [], + "timings": [ + { + "stage": "detect_holes", + "seconds": 2.090514300041832, + "peak_kib": 32527.3603515625 + }, + { + "stage": "build_frame", + "seconds": 4.1000021155923605e-05, + "peak_kib": 0.0 + }, + { + "stage": "extract_roi", + "seconds": 0.33327039994765073, + "peak_kib": 17444.6552734375 + }, + { + "stage": "fit_plane", + "seconds": 0.07231109996791929, + "peak_kib": 626.2568359375 + }, + { + "stage": "metrics", + "seconds": 4.96999709866941e-05, + "peak_kib": 0.0 + } + ] + } + }, + "gt_cad": { + "holes": { + "locating": [ + { + "center": [ + 0.0, + -99.00000120767, + -179.9999990121 + ], + "radius_mm": 16.0, + "fit_residual_rms_mm": 0.0, + "role": "locating" + }, + { + "center": [ + 0.0, + 100.9999987923, + -179.9999990121 + ], + "radius_mm": 16.0, + "fit_residual_rms_mm": 0.0, + "role": "locating" + }, + { + "center": [ + 0.0, + 100.9999987923, + 180.0000009879 + ], + "radius_mm": 16.0, + "fit_residual_rms_mm": 0.0, + "role": "locating" + }, + { + "center": [ + 0.0, + -99.00000120767, + 180.0000009879 + ], + "radius_mm": 16.0, + "fit_residual_rms_mm": 0.0, + "role": "locating" + } + ], + "central": { + "center": [ + 0.0, + 0.999998792332, + 0.0 + ], + "radius_mm": 100.0, + "fit_residual_rms_mm": 0.0, + "role": "central" + } + }, + "plane": { + "normal": [ + 1.0, + -0.0, + -0.0 + ], + "point_on_plane": [ + 0.0, + 0.9999987923150044, + 9.878999946977274e-07 + ], + "d_offset": -0.0, + "inlier_ratio": 1.0, + "inlier_residual_rms_mm": 0.0 + }, + "frame": { + "center_C": [ + 0.0, + 0.9999987923150044, + 9.878999946977274e-07 + ], + "line_direction": [ + 1.0, + 0.0, + 0.0 + ], + "reference_plane_z": 9.878999946977274e-07, + "longitudinal_mid_y": 0.9999987923150044 + }, + "raw_circles": [ + { + "center": [ + 0.0, + 106.0, + 669.0000154664 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 20.0 + }, + { + "center": [ + 0.0, + 136.0, + 699.0000154664 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 10.0 + }, + { + "center": [ + 0.0, + 169.0, + 689.9995359775 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 3.0 + }, + { + "center": [ + 0.0, + 169.0, + -689.9995359775 + ], + "axis": [ + 1.0, + 0.0, + -0.0 + ], + "radius": 3.0 + }, + { + "center": [ + 0.0, + 136.0, + -699.0000154664 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 10.0 + }, + { + "center": [ + 0.0, + 106.0, + -669.0000154664 + ], + "axis": [ + 1.0, + 0.0, + 0.0 + ], + "radius": 20.0 + }, + { + "center": [ + 0.0, + 0.999998792332, + 0.0 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 100.0 + }, + { + "center": [ + 0.0, + 0.999998792332, + 0.0 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 100.0 + }, + { + "center": [ + 0.0, + 40.0, + 180.0 + ], + "axis": [ + 1.0, + 0.0, + 0.0 + ], + "radius": 7.125 + }, + { + "center": [ + 0.0, + 40.0, + 180.0 + ], + "axis": [ + 1.0, + 0.0, + 0.0 + ], + "radius": 7.125 + }, + { + "center": [ + 0.0, + -40.0, + 180.0 + ], + "axis": [ + 1.0, + 0.0, + 0.0 + ], + "radius": 7.125 + }, + { + "center": [ + 0.0, + -40.0, + 180.0 + ], + "axis": [ + 1.0, + 0.0, + 0.0 + ], + "radius": 7.125 + }, + { + "center": [ + 0.0, + -40.0, + -180.0 + ], + "axis": [ + 1.0, + 0.0, + 0.0 + ], + "radius": 7.125 + }, + { + "center": [ + 0.0, + -40.0, + -180.0 + ], + "axis": [ + 1.0, + 0.0, + 0.0 + ], + "radius": 7.125 + }, + { + "center": [ + 0.0, + 40.0, + -180.0 + ], + "axis": [ + 1.0, + 0.0, + 0.0 + ], + "radius": 7.125 + }, + { + "center": [ + 0.0, + 40.0, + -180.0 + ], + "axis": [ + 1.0, + 0.0, + 0.0 + ], + "radius": 7.125 + }, + { + "center": [ + 0.0, + 100.9999987923, + -179.9999990121 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 16.0 + }, + { + "center": [ + 0.0, + 100.9999987923, + -179.9999990121 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 16.0 + }, + { + "center": [ + 0.0, + 100.9999987923, + 180.0000009879 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 16.0 + }, + { + "center": [ + 0.0, + 100.9999987923, + 180.0000009879 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 16.0 + }, + { + "center": [ + 0.0, + -99.00000120767, + -179.9999990121 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 16.0 + }, + { + "center": [ + 0.0, + -99.00000120767, + -179.9999990121 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 16.0 + }, + { + "center": [ + 0.0, + -99.00000120767, + 180.0000009879 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 16.0 + }, + { + "center": [ + 0.0, + -99.00000120767, + 180.0000009879 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 16.0 + }, + { + "center": [ + -80.0, + -99.00000120767, + -189.9999990121 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 40.0 + }, + { + "center": [ + -80.0, + -99.00000120767, + 190.0000009879 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 40.0 + }, + { + "center": [ + -79.0, + -99.00000120767, + 227.0000009879 + ], + "axis": [ + 0.0, + -1.0, + 0.0 + ], + "radius": 3.0 + }, + { + "center": [ + -80.0, + -99.00000120767, + 190.0000009879 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 40.0 + }, + { + "center": [ + -79.0, + -99.00000120767, + 190.0000009879 + ], + "axis": [ + 1.0, + 0.0, + -0.0 + ], + "radius": 40.0 + }, + { + "center": [ + -80.0, + -99.00000120767, + -189.9999990121 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 40.0 + }, + { + "center": [ + -79.0, + -99.00000120767, + -189.9999990121 + ], + "axis": [ + 1.0, + 0.0, + 0.0 + ], + "radius": 40.0 + }, + { + "center": [ + -79.0, + -99.00000120767, + -226.9999990121 + ], + "axis": [ + -0.0, + 1.0, + 0.0 + ], + "radius": 3.0 + }, + { + "center": [ + -76.0, + -99.00000120767, + 190.0000009879 + ], + "axis": [ + 1.0, + 0.0, + -0.0 + ], + "radius": 37.0 + }, + { + "center": [ + -76.0, + -99.00000120767, + -189.9999990121 + ], + "axis": [ + 1.0, + 0.0, + 0.0 + ], + "radius": 37.0 + }, + { + "center": [ + -76.0, + 106.0, + -669.0000154664 + ], + "axis": [ + 1.0, + 0.0, + 0.0 + ], + "radius": 20.0 + }, + { + "center": [ + -76.0, + 136.0, + -699.0000154664 + ], + "axis": [ + 1.0, + 0.0, + 0.0 + ], + "radius": 10.0 + }, + { + "center": [ + -76.0, + 169.0, + -689.9995359775 + ], + "axis": [ + 1.0, + 0.0, + 0.0 + ], + "radius": 3.0 + }, + { + "center": [ + -76.0, + 169.0, + 689.9995359775 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 3.0 + }, + { + "center": [ + -76.0, + 136.0, + 699.0000154664 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 10.0 + }, + { + "center": [ + -76.0, + 106.0, + 669.0000154664 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 20.0 + }, + { + "center": [ + -76.0, + -99.00000120767, + -179.9999990121 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 16.0 + }, + { + "center": [ + -76.0, + -99.00000120767, + -179.9999990121 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 16.0 + }, + { + "center": [ + -76.0, + 100.9999987923, + 180.0000009879 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 16.0 + }, + { + "center": [ + -76.0, + 100.9999987923, + 180.0000009879 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 16.0 + }, + { + "center": [ + -76.0, + 100.9999987923, + -179.9999990121 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 16.0 + }, + { + "center": [ + -76.0, + 100.9999987923, + -179.9999990121 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 16.0 + }, + { + "center": [ + -76.0, + 0.999998792332, + 0.0 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 100.0 + }, + { + "center": [ + -76.0, + 0.999998792332, + 0.0 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 100.0 + }, + { + "center": [ + -76.0, + -99.00000120767, + 180.0000009879 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 16.0 + }, + { + "center": [ + -76.0, + -99.00000120767, + 180.0000009879 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 16.0 + }, + { + "center": [ + -76.0, + -99.00000120767, + 180.0000009879 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 16.0 + }, + { + "center": [ + 0.0, + -99.00000120767, + 180.0000009879 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 16.0 + }, + { + "center": [ + -76.0, + -99.00000120767, + 180.0000009879 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 16.0 + }, + { + "center": [ + 0.0, + -99.00000120767, + 180.0000009879 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 16.0 + }, + { + "center": [ + -76.0, + -99.00000120767, + -179.9999990121 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 16.0 + }, + { + "center": [ + 0.0, + -99.00000120767, + -179.9999990121 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 16.0 + }, + { + "center": [ + -76.0, + -99.00000120767, + -179.9999990121 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 16.0 + }, + { + "center": [ + 0.0, + -99.00000120767, + -179.9999990121 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 16.0 + }, + { + "center": [ + -76.0, + 100.9999987923, + 180.0000009879 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 16.0 + }, + { + "center": [ + 0.0, + 100.9999987923, + 180.0000009879 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 16.0 + }, + { + "center": [ + -76.0, + 100.9999987923, + 180.0000009879 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 16.0 + }, + { + "center": [ + 0.0, + 100.9999987923, + 180.0000009879 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 16.0 + }, + { + "center": [ + -76.0, + 100.9999987923, + -179.9999990121 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 16.0 + }, + { + "center": [ + 0.0, + 100.9999987923, + -179.9999990121 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 16.0 + }, + { + "center": [ + -76.0, + 100.9999987923, + -179.9999990121 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 16.0 + }, + { + "center": [ + 0.0, + 100.9999987923, + -179.9999990121 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 16.0 + }, + { + "center": [ + -76.0, + 0.999998792332, + 0.0 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 100.0 + }, + { + "center": [ + 0.0, + 0.999998792332, + 0.0 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 100.0 + }, + { + "center": [ + -76.0, + 0.999998792332, + 0.0 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 100.0 + }, + { + "center": [ + 0.0, + 0.999998792332, + 0.0 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 100.0 + }, + { + "center": [ + -76.0, + -99.00000120767, + 190.0000009879 + ], + "axis": [ + 1.0, + 0.0, + -0.0 + ], + "radius": 37.0 + }, + { + "center": [ + -79.0, + -99.00000120767, + 227.0000009879 + ], + "axis": [ + 0.0, + -1.0, + 0.0 + ], + "radius": 3.0 + }, + { + "center": [ + -79.0, + -99.00000120767, + 190.0000009879 + ], + "axis": [ + 1.0, + 0.0, + -0.0 + ], + "radius": 40.0 + }, + { + "center": [ + -79.0, + -136.0000012077, + 190.0000009879 + ], + "axis": [ + 0.0, + 0.0, + -1.0 + ], + "radius": 3.0 + }, + { + "center": [ + -79.0, + -136.0000012077, + 190.0000009879 + ], + "axis": [ + 0.0, + 0.0, + -1.0 + ], + "radius": 3.0 + }, + { + "center": [ + -79.0, + -136.0000012077, + -189.9999990121 + ], + "axis": [ + 0.0, + 0.0, + -1.0 + ], + "radius": 3.0 + }, + { + "center": [ + -76.0, + -99.00000120767, + -189.9999990121 + ], + "axis": [ + 1.0, + 0.0, + 0.0 + ], + "radius": 37.0 + }, + { + "center": [ + -79.0, + -136.0000012077, + -189.9999990121 + ], + "axis": [ + 0.0, + 0.0, + -1.0 + ], + "radius": 3.0 + }, + { + "center": [ + -79.0, + -99.00000120767, + -189.9999990121 + ], + "axis": [ + 1.0, + 0.0, + 0.0 + ], + "radius": 40.0 + }, + { + "center": [ + -79.0, + -99.00000120767, + -226.9999990121 + ], + "axis": [ + -0.0, + 1.0, + 0.0 + ], + "radius": 3.0 + }, + { + "center": [ + -76.0, + 106.0, + -669.0000154664 + ], + "axis": [ + 1.0, + 0.0, + 0.0 + ], + "radius": 20.0 + }, + { + "center": [ + 0.0, + 106.0, + -669.0000154664 + ], + "axis": [ + 1.0, + 0.0, + 0.0 + ], + "radius": 20.0 + }, + { + "center": [ + 0.0, + 136.0, + -699.0000154664 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 10.0 + }, + { + "center": [ + -76.0, + 136.0, + -699.0000154664 + ], + "axis": [ + 1.0, + 0.0, + 0.0 + ], + "radius": 10.0 + }, + { + "center": [ + -76.0, + 169.0, + -689.9995359775 + ], + "axis": [ + 1.0, + 0.0, + 0.0 + ], + "radius": 3.0 + }, + { + "center": [ + 0.0, + 169.0, + -689.9995359775 + ], + "axis": [ + 1.0, + 0.0, + -0.0 + ], + "radius": 3.0 + }, + { + "center": [ + -76.0, + 169.0, + 689.9995359775 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 3.0 + }, + { + "center": [ + 0.0, + 169.0, + 689.9995359775 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 3.0 + }, + { + "center": [ + 0.0, + 136.0, + 699.0000154664 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 10.0 + }, + { + "center": [ + -76.0, + 136.0, + 699.0000154664 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 10.0 + }, + { + "center": [ + -76.0, + 106.0, + 669.0000154664 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 20.0 + }, + { + "center": [ + 0.0, + 106.0, + 669.0000154664 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 20.0 + }, + { + "center": [ + 0.0, + 40.0, + 180.0 + ], + "axis": [ + 1.0, + 0.0, + 0.0 + ], + "radius": 7.125 + }, + { + "center": [ + -0.725, + 40.0, + 180.0 + ], + "axis": [ + 1.0, + 0.0, + 0.0 + ], + "radius": 6.4 + }, + { + "center": [ + 0.0, + 40.0, + 180.0 + ], + "axis": [ + 1.0, + 0.0, + 0.0 + ], + "radius": 7.125 + }, + { + "center": [ + -0.725, + 40.0, + 180.0 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 6.4 + }, + { + "center": [ + -0.725, + 40.0, + 180.0 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 6.4 + }, + { + "center": [ + -40.0, + 40.0, + 180.0 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 6.4 + }, + { + "center": [ + -40.0, + 40.0, + 180.0 + ], + "axis": [ + 1.0, + 0.0, + 0.0 + ], + "radius": 6.4 + }, + { + "center": [ + -0.725, + 40.0, + 180.0 + ], + "axis": [ + 1.0, + 0.0, + 0.0 + ], + "radius": 6.4 + }, + { + "center": [ + -45.37023763953209, + 40.0, + 180.0 + ], + "axis": [ + 1.0, + 0.0, + 0.0 + ], + "radius": 1.7505996652289468e-12 + }, + { + "center": [ + -40.0, + 40.0, + 180.0 + ], + "axis": [ + 1.0, + 0.0, + 0.0 + ], + "radius": 6.4 + }, + { + "center": [ + -45.37023763953209, + 40.00000000000001, + 180.0 + ], + "axis": [ + 1.0, + -0.0, + 0.0 + ], + "radius": 1.7483792191796965e-12 + }, + { + "center": [ + -40.0, + 40.0, + 180.0 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 6.4 + }, + { + "center": [ + 0.0, + -40.0, + 180.0 + ], + "axis": [ + 1.0, + 0.0, + 0.0 + ], + "radius": 7.125 + }, + { + "center": [ + -0.725, + -40.0, + 180.0 + ], + "axis": [ + 1.0, + 0.0, + 0.0 + ], + "radius": 6.4 + }, + { + "center": [ + 0.0, + -40.0, + 180.0 + ], + "axis": [ + 1.0, + 0.0, + 0.0 + ], + "radius": 7.125 + }, + { + "center": [ + -0.725, + -40.0, + 180.0 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 6.4 + }, + { + "center": [ + -0.725, + -40.0, + 180.0 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 6.4 + }, + { + "center": [ + -40.0, + -40.0, + 180.0 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 6.4 + }, + { + "center": [ + -40.0, + -40.0, + 180.0 + ], + "axis": [ + 1.0, + 0.0, + 0.0 + ], + "radius": 6.4 + }, + { + "center": [ + -0.725, + -40.0, + 180.0 + ], + "axis": [ + 1.0, + 0.0, + 0.0 + ], + "radius": 6.4 + }, + { + "center": [ + -45.37023763953209, + -40.0, + 180.0 + ], + "axis": [ + 1.0, + 0.0, + 0.0 + ], + "radius": 1.7505996652289468e-12 + }, + { + "center": [ + -40.0, + -40.0, + 180.0 + ], + "axis": [ + 1.0, + 0.0, + 0.0 + ], + "radius": 6.4 + }, + { + "center": [ + -45.37023763953209, + -39.99999999999999, + 180.0 + ], + "axis": [ + 1.0, + -0.0, + 0.0 + ], + "radius": 1.7483792191796965e-12 + }, + { + "center": [ + -40.0, + -40.0, + 180.0 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 6.4 + }, + { + "center": [ + 0.0, + -40.0, + -180.0 + ], + "axis": [ + 1.0, + 0.0, + 0.0 + ], + "radius": 7.125 + }, + { + "center": [ + -0.725, + -40.0, + -180.0 + ], + "axis": [ + 1.0, + 0.0, + 0.0 + ], + "radius": 6.4 + }, + { + "center": [ + 0.0, + -40.0, + -180.0 + ], + "axis": [ + 1.0, + 0.0, + 0.0 + ], + "radius": 7.125 + }, + { + "center": [ + -0.725, + -40.0, + -180.0 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 6.4 + }, + { + "center": [ + -0.725, + -40.0, + -180.0 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 6.4 + }, + { + "center": [ + -40.0, + -40.0, + -180.0 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 6.4 + }, + { + "center": [ + -40.0, + -40.0, + -180.0 + ], + "axis": [ + 1.0, + 0.0, + 0.0 + ], + "radius": 6.4 + }, + { + "center": [ + -0.725, + -40.0, + -180.0 + ], + "axis": [ + 1.0, + 0.0, + 0.0 + ], + "radius": 6.4 + }, + { + "center": [ + -45.37023763953209, + -40.0, + -180.0 + ], + "axis": [ + 1.0, + 0.0, + 0.0 + ], + "radius": 1.7505996652289468e-12 + }, + { + "center": [ + -40.0, + -40.0, + -180.0 + ], + "axis": [ + 1.0, + 0.0, + 0.0 + ], + "radius": 6.4 + }, + { + "center": [ + -45.37023763953209, + -40.00000000000001, + -180.0 + ], + "axis": [ + 1.0, + -0.0, + 0.0 + ], + "radius": 1.7483792191796965e-12 + }, + { + "center": [ + -40.0, + -40.0, + -180.0 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 6.4 + }, + { + "center": [ + 0.0, + 40.0, + -180.0 + ], + "axis": [ + 1.0, + 0.0, + 0.0 + ], + "radius": 7.125 + }, + { + "center": [ + -0.725, + 40.0, + -180.0 + ], + "axis": [ + 1.0, + 0.0, + 0.0 + ], + "radius": 6.4 + }, + { + "center": [ + 0.0, + 40.0, + -180.0 + ], + "axis": [ + 1.0, + 0.0, + 0.0 + ], + "radius": 7.125 + }, + { + "center": [ + -0.725, + 40.0, + -180.0 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 6.4 + }, + { + "center": [ + -0.725, + 40.0, + -180.0 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 6.4 + }, + { + "center": [ + -40.0, + 40.0, + -180.0 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 6.4 + }, + { + "center": [ + -40.0, + 40.0, + -180.0 + ], + "axis": [ + 1.0, + 0.0, + 0.0 + ], + "radius": 6.4 + }, + { + "center": [ + -0.725, + 40.0, + -180.0 + ], + "axis": [ + 1.0, + 0.0, + 0.0 + ], + "radius": 6.4 + }, + { + "center": [ + -45.37023763953209, + 40.0, + -180.0 + ], + "axis": [ + 1.0, + 0.0, + 0.0 + ], + "radius": 1.7505996652289468e-12 + }, + { + "center": [ + -40.0, + 40.0, + -180.0 + ], + "axis": [ + 1.0, + 0.0, + 0.0 + ], + "radius": 6.4 + }, + { + "center": [ + -45.37023763953209, + 39.99999999999999, + -180.0 + ], + "axis": [ + 1.0, + -0.0, + 0.0 + ], + "radius": 1.7483792191796965e-12 + }, + { + "center": [ + -40.0, + 40.0, + -180.0 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 6.4 + } + ] + }, + "gt_after_deflection": { + "normal": [ + 0.9999482158335473, + 0.008726415877184502, + 0.00523596383141958 + ], + "locating_centers": [ + [ + 1.8063646228583214, + -98.98800706265835, + -179.99753161665035 + ], + [ + 0.061057523183796314, + 101.00437755014592, + -179.99753161665035 + ], + [ + -1.8238176831220807, + 100.98792849341841, + 179.99753359242328 + ], + [ + -0.07851058344755557, + -99.00445611938586, + 179.99753359242328 + ] + ] + }, + "gt_metrics": { + "theta_right_deg": 0.5, + "theta_ref_deg": 0.30001142330692215, + "D_y_max_mm": 100.00441683476589, + "D_z_max_mm": 179.99753260453681, + "dx_y_max_mm": 0.8727253242089344, + "dx_z_max_mm": 0.9425093775246102, + "A": -0.00872686779075879, + "B": -0.005236234985483653 + }, + "diff": { + "theta_right_deg": 0.04435451942190205, + "theta_ref_deg": -0.0002647901093028593, + "D_y_max_mm": -5.923786793923455, + "D_z_max_mm": -7.783361122947866, + "dx_y_max_mm": 0.021140813235957512, + "dx_z_max_mm": -0.041551410391990284 + }, + "config": { + "schema_version": "1.0", + "cad": { + "linear_deflection_mm": 0.5, + "angular_deflection_rad": 0.1, + "locating_hole_radius_mm": [ + 14.0, + 18.0 + ], + "central_hole_radius_min_mm": 80.0, + "rectangle_aspect_tol": 0.15 + }, + "sampling": { + "method": "grid_spacing", + "spacing_mm": null, + "num_points": 500000, + "seed": 42, + "adaptive": { + "curvature_weight": 1.0, + "min_spacing_factor": 0.25, + "max_spacing_factor": 4.0 + } + }, + "scan_simulator": { + "enabled": true, + "view_origin": [ + 10000.0, + 0.0, + 0.0 + ], + "hpr_radius_mm": 200000.0 + }, + "panel_deflection": { + "enabled": true, + "theta_right_deg": 0.5, + "theta_ref_deg": 0.3 + }, + "noise": { + "enabled": true, + "sigma_mm": 0.05, + "mode": "isotropic", + "seed": 43 + }, + "transform": { + "enabled": true, + "rotation_deg": 0.5, + "rotation_axis": "random", + "translation_mm": 2.0, + "translation_dir": "random", + "seed": 44 + }, + "denoise": { + "steps": [ + { + "type": "passthrough", + "bbox_padding_mm": 50.0 + }, + { + "type": "voxel", + "voxel_mm": 0.5 + }, + { + "type": "statistical", + "nb_neighbors": 20, + "std_ratio": 2.5 + } + ], + "smoothing": { + "enabled": false, + "search_radius_mm": 2.0, + "polynomial_order": 2 + } + }, + "registration": { + "method": "kabsch_four_holes", + "pre_align": "pca_centroid", + "refuse_if_residual_above_mm": 5.0 + }, + "detection": { + "panel_normal_seed": "pca", + "panel_normal_pca_kth": 1, + "grid_pixel_mm": 2.0, + "panel_min_area_ratio": 0.15, + "hole_min_area_mm2": 600.0, + "hole_max_area_mm2": 5000.0, + "circle_fit": { + "method": "algebraic_then_lsq", + "inlier_residual_mm": 1.5 + }, + "select_top_k_by_radius_match": 4, + "rectangle_aspect_tol": 0.4, + "fill_bg_below_mm2": 100.0, + "locating_hole_radius_mm": [ + 14.0, + 22.0 + ] + }, + "roi": { + "hole_dilate_mm": 5.0, + "central_hole_dilate_mm": 10.0, + "edge_buffer_mm": 8.0, + "chamfer_normal_dot_min": 0.8, + "outlier_z_score": 3.0 + }, + "plane_fit": { + "method": "ransac_svd", + "ransac_iterations": 300, + "ransac_inlier_thresh_mm": 0.5, + "min_inlier_ratio": 0.5, + "enforce_nx_positive": true + }, + "metrics": { + "right_axis_sign": 1, + "angle_clamp_deg": 0.0 + }, + "artifacts": { + "save_intermediate": true, + "ply_format": "binary", + "image_dpi": 150, + "heatmap_colormap": "viridis" + } + } +} \ No newline at end of file diff --git a/alg/delivery_v1.1/coupler_panel_release/test_data/synth/gt.json b/alg/delivery_v1.1/coupler_panel_release/test_data/synth/gt.json new file mode 100644 index 0000000..742e4b2 --- /dev/null +++ b/alg/delivery_v1.1/coupler_panel_release/test_data/synth/gt.json @@ -0,0 +1,3727 @@ +{ + "holes": { + "locating": [ + { + "center": [ + -11598.00007887, + 835.00000000003, + -180.0 + ], + "radius_mm": 16.0, + "fit_residual_rms_mm": 0.0, + "role": "locating" + }, + { + "center": [ + -11598.00007887, + 1035.0, + -180.0 + ], + "radius_mm": 16.0, + "fit_residual_rms_mm": 0.0, + "role": "locating" + }, + { + "center": [ + -11598.00007887, + 1035.0, + 179.99999999996 + ], + "radius_mm": 16.0, + "fit_residual_rms_mm": 0.0, + "role": "locating" + }, + { + "center": [ + -11598.00007887, + 835.00000000003, + 179.99999999996 + ], + "radius_mm": 16.0, + "fit_residual_rms_mm": 0.0, + "role": "locating" + } + ], + "central": { + "center": [ + -11598.00007887, + 935.00000000003, + -9.879379026e-07 + ], + "radius_mm": 100.0, + "fit_residual_rms_mm": 0.0, + "role": "central" + } + }, + "plane": { + "normal": [ + 1.0, + -0.0, + -0.0 + ], + "point_on_plane": [ + -11598.00007887, + 935.000000000015, + -1.999467258428922e-11 + ], + "d_offset": 11598.00007887, + "inlier_ratio": 1.0, + "inlier_residual_rms_mm": 0.0 + }, + "frame": { + "center_C": [ + -11598.00007887, + 935.000000000015, + -1.999467258428922e-11 + ], + "line_direction": [ + 1.0, + 0.0, + 0.0 + ], + "reference_plane_z": -1.999467258428922e-11, + "longitudinal_mid_y": 935.000000000015 + }, + "raw_circles": [ + { + "center": [ + -11598.00007887, + 1040.000001208, + 669.00001447846 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 20.0 + }, + { + "center": [ + -11598.00007887, + 1070.000001208, + 699.00001447846 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 10.0 + }, + { + "center": [ + -11598.00007887, + 1103.000001208, + 689.99953498956 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 3.0 + }, + { + "center": [ + -11598.00007887, + 1103.000001208, + -689.9995369654 + ], + "axis": [ + 1.0, + 0.0, + -0.0 + ], + "radius": 3.0 + }, + { + "center": [ + -11598.00007887, + 1070.000001208, + -699.0000164543 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 10.0 + }, + { + "center": [ + -11598.00007887, + 1040.000001208, + -669.0000164543 + ], + "axis": [ + 1.0, + 0.0, + -0.0 + ], + "radius": 20.0 + }, + { + "center": [ + -11598.00007887, + 935.00000000003, + -9.879379026e-07 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 100.0 + }, + { + "center": [ + -11598.00007887, + 935.00000000003, + -9.879379026e-07 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 100.0 + }, + { + "center": [ + -11598.00007887, + 974.0000012077, + 179.99999901206 + ], + "axis": [ + 1.0, + 0.0, + 0.0 + ], + "radius": 7.125 + }, + { + "center": [ + -11598.00007887, + 974.0000012077, + 179.99999901206 + ], + "axis": [ + 1.0, + 0.0, + 0.0 + ], + "radius": 7.125 + }, + { + "center": [ + -11598.00007887, + 894.0000012077, + 179.99999901206 + ], + "axis": [ + 1.0, + 0.0, + 0.0 + ], + "radius": 7.125 + }, + { + "center": [ + -11598.00007887, + 894.0000012077, + 179.99999901206 + ], + "axis": [ + 1.0, + 0.0, + 0.0 + ], + "radius": 7.125 + }, + { + "center": [ + -11598.00007887, + 894.0000012077, + -180.0000009879 + ], + "axis": [ + 1.0, + 0.0, + 0.0 + ], + "radius": 7.125 + }, + { + "center": [ + -11598.00007887, + 894.0000012077, + -180.0000009879 + ], + "axis": [ + 1.0, + 0.0, + 0.0 + ], + "radius": 7.125 + }, + { + "center": [ + -11598.00007887, + 974.0000012077, + -180.0000009879 + ], + "axis": [ + 1.0, + 0.0, + 0.0 + ], + "radius": 7.125 + }, + { + "center": [ + -11598.00007887, + 974.0000012077, + -180.0000009879 + ], + "axis": [ + 1.0, + 0.0, + 0.0 + ], + "radius": 7.125 + }, + { + "center": [ + -11598.00007887, + 1035.0, + -180.0 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 16.0 + }, + { + "center": [ + -11598.00007887, + 1035.0, + -180.0 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 16.0 + }, + { + "center": [ + -11598.00007887, + 1035.0, + 179.99999999996 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 16.0 + }, + { + "center": [ + -11598.00007887, + 1035.0, + 179.99999999996 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 16.0 + }, + { + "center": [ + -11598.00007887, + 835.00000000003, + -180.0 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 16.0 + }, + { + "center": [ + -11598.00007887, + 835.00000000003, + -180.0 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 16.0 + }, + { + "center": [ + -11598.00007887, + 835.00000000003, + 179.99999999996 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 16.0 + }, + { + "center": [ + -11598.00007887, + 835.00000000003, + 179.99999999996 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 16.0 + }, + { + "center": [ + -11678.00007887, + 835.00000000003, + -190.0 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 40.0 + }, + { + "center": [ + -11678.00007887, + 835.00000000003, + 189.99999999996 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 40.0 + }, + { + "center": [ + -11677.00007887, + 835.00000000003, + 226.99999999996 + ], + "axis": [ + 0.0, + -1.0, + 0.0 + ], + "radius": 3.0 + }, + { + "center": [ + -11678.00007887, + 835.00000000003, + 189.99999999996 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 40.0 + }, + { + "center": [ + -11677.00007887, + 835.00000000003, + 189.99999999996 + ], + "axis": [ + 1.0, + 0.0, + -0.0 + ], + "radius": 40.0 + }, + { + "center": [ + -11678.00007887, + 835.00000000003, + -190.0 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 40.0 + }, + { + "center": [ + -11677.00007887, + 835.00000000003, + -190.0 + ], + "axis": [ + 1.0, + 0.0, + 0.0 + ], + "radius": 40.0 + }, + { + "center": [ + -11677.00007887, + 835.00000000003, + -227.0 + ], + "axis": [ + -0.0, + 1.0, + 0.0 + ], + "radius": 3.0 + }, + { + "center": [ + -11674.00007887, + 835.00000000003, + 189.99999999996 + ], + "axis": [ + 1.0, + 0.0, + -0.0 + ], + "radius": 37.0 + }, + { + "center": [ + -11674.00007887, + 835.00000000003, + -190.0 + ], + "axis": [ + 1.0, + 0.0, + 0.0 + ], + "radius": 37.0 + }, + { + "center": [ + -11674.00007887, + 1040.000001208, + -669.0000164543 + ], + "axis": [ + 1.0, + 0.0, + 0.0 + ], + "radius": 20.0 + }, + { + "center": [ + -11674.00007887, + 1070.000001208, + -699.0000164543 + ], + "axis": [ + 1.0, + 0.0, + 0.0 + ], + "radius": 10.0 + }, + { + "center": [ + -11674.00007887, + 1103.000001208, + -689.9995369654 + ], + "axis": [ + 1.0, + 0.0, + 0.0 + ], + "radius": 3.0 + }, + { + "center": [ + -11674.00007887, + 1103.000001208, + 689.99953498956 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 3.0 + }, + { + "center": [ + -11674.00007887, + 1070.000001208, + 699.00001447846 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 10.0 + }, + { + "center": [ + -11674.00007887, + 1040.000001208, + 669.00001447846 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 20.0 + }, + { + "center": [ + -11674.00007887, + 835.00000000003, + -180.0 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 16.0 + }, + { + "center": [ + -11674.00007887, + 835.00000000003, + -180.0 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 16.0 + }, + { + "center": [ + -11674.00007887, + 1035.0, + 179.99999999996 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 16.0 + }, + { + "center": [ + -11674.00007887, + 1035.0, + 179.99999999996 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 16.0 + }, + { + "center": [ + -11674.00007887, + 1035.0, + -180.0 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 16.0 + }, + { + "center": [ + -11674.00007887, + 1035.0, + -180.0 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 16.0 + }, + { + "center": [ + -11674.00007887, + 935.00000000003, + -9.879379026e-07 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 100.0 + }, + { + "center": [ + -11674.00007887, + 935.00000000003, + -9.879379026e-07 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 100.0 + }, + { + "center": [ + -11674.00007887, + 835.00000000003, + 179.99999999996 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 16.0 + }, + { + "center": [ + -11674.00007887, + 835.00000000003, + 179.99999999996 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 16.0 + }, + { + "center": [ + -11674.00007887, + 835.00000000003, + 179.99999999996 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 16.0 + }, + { + "center": [ + -11598.00007887, + 835.00000000003, + 179.99999999996 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 16.0 + }, + { + "center": [ + -11674.00007887, + 835.00000000003, + 179.99999999996 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 16.0 + }, + { + "center": [ + -11598.00007887, + 835.00000000003, + 179.99999999996 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 16.0 + }, + { + "center": [ + -11674.00007887, + 835.00000000003, + -180.0 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 16.0 + }, + { + "center": [ + -11598.00007887, + 835.00000000003, + -180.0 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 16.0 + }, + { + "center": [ + -11674.00007887, + 835.00000000003, + -180.0 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 16.0 + }, + { + "center": [ + -11598.00007887, + 835.00000000003, + -180.0 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 16.0 + }, + { + "center": [ + -11674.00007887, + 1035.0, + 179.99999999996 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 16.0 + }, + { + "center": [ + -11598.00007887, + 1035.0, + 179.99999999996 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 16.0 + }, + { + "center": [ + -11674.00007887, + 1035.0, + 179.99999999996 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 16.0 + }, + { + "center": [ + -11598.00007887, + 1035.0, + 179.99999999996 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 16.0 + }, + { + "center": [ + -11674.00007887, + 1035.0, + -180.0 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 16.0 + }, + { + "center": [ + -11598.00007887, + 1035.0, + -180.0 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 16.0 + }, + { + "center": [ + -11674.00007887, + 1035.0, + -180.0 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 16.0 + }, + { + "center": [ + -11598.00007887, + 1035.0, + -180.0 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 16.0 + }, + { + "center": [ + -11674.00007887, + 935.00000000003, + -9.879379026e-07 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 100.0 + }, + { + "center": [ + -11598.00007887, + 935.00000000003, + -9.879379026e-07 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 100.0 + }, + { + "center": [ + -11674.00007887, + 935.00000000003, + -9.879379026e-07 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 100.0 + }, + { + "center": [ + -11598.00007887, + 935.00000000003, + -9.879379026e-07 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 100.0 + }, + { + "center": [ + -11674.00007887, + 835.00000000003, + 189.99999999996 + ], + "axis": [ + 1.0, + 0.0, + -0.0 + ], + "radius": 37.0 + }, + { + "center": [ + -11677.00007887, + 835.00000000003, + 226.99999999996 + ], + "axis": [ + 0.0, + -1.0, + 0.0 + ], + "radius": 3.0 + }, + { + "center": [ + -11677.00007887, + 835.00000000003, + 189.99999999996 + ], + "axis": [ + 1.0, + 0.0, + -0.0 + ], + "radius": 40.0 + }, + { + "center": [ + -11677.00007887, + 798.0, + 189.99999999996 + ], + "axis": [ + 0.0, + 0.0, + -1.0 + ], + "radius": 3.0 + }, + { + "center": [ + -11677.00007887, + 798.0, + 189.99999999996 + ], + "axis": [ + 0.0, + 0.0, + -1.0 + ], + "radius": 3.0 + }, + { + "center": [ + -11677.00007887, + 798.0, + -190.0 + ], + "axis": [ + 0.0, + 0.0, + -1.0 + ], + "radius": 3.0 + }, + { + "center": [ + -11674.00007887, + 835.00000000003, + -190.0 + ], + "axis": [ + 1.0, + 0.0, + 0.0 + ], + "radius": 37.0 + }, + { + "center": [ + -11677.00007887, + 798.0, + -190.0 + ], + "axis": [ + 0.0, + 0.0, + -1.0 + ], + "radius": 3.0 + }, + { + "center": [ + -11677.00007887, + 835.00000000003, + -190.0 + ], + "axis": [ + 1.0, + 0.0, + 0.0 + ], + "radius": 40.0 + }, + { + "center": [ + -11677.00007887, + 835.00000000003, + -227.0 + ], + "axis": [ + -0.0, + 1.0, + 0.0 + ], + "radius": 3.0 + }, + { + "center": [ + -11674.00007887, + 1040.000001208, + -669.0000164543 + ], + "axis": [ + 1.0, + 0.0, + 0.0 + ], + "radius": 20.0 + }, + { + "center": [ + -11598.00007887, + 1040.000001208, + -669.0000164543 + ], + "axis": [ + 1.0, + 0.0, + -0.0 + ], + "radius": 20.0 + }, + { + "center": [ + -11598.00007887, + 1070.000001208, + -699.0000164543 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 10.0 + }, + { + "center": [ + -11674.00007887, + 1070.000001208, + -699.0000164543 + ], + "axis": [ + 1.0, + 0.0, + 0.0 + ], + "radius": 10.0 + }, + { + "center": [ + -11674.00007887, + 1103.000001208, + -689.9995369654 + ], + "axis": [ + 1.0, + 0.0, + 0.0 + ], + "radius": 3.0 + }, + { + "center": [ + -11598.00007887, + 1103.000001208, + -689.9995369654 + ], + "axis": [ + 1.0, + 0.0, + -0.0 + ], + "radius": 3.0 + }, + { + "center": [ + -11674.00007887, + 1103.000001208, + 689.99953498956 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 3.0 + }, + { + "center": [ + -11598.00007887, + 1103.000001208, + 689.99953498956 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 3.0 + }, + { + "center": [ + -11598.00007887, + 1070.000001208, + 699.00001447846 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 10.0 + }, + { + "center": [ + -11674.00007887, + 1070.000001208, + 699.00001447846 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 10.0 + }, + { + "center": [ + -11674.00007887, + 1040.000001208, + 669.00001447846 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 20.0 + }, + { + "center": [ + -11598.00007887, + 1040.000001208, + 669.00001447846 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 20.0 + }, + { + "center": [ + -11598.00007887, + 974.0000012077, + 179.99999901206 + ], + "axis": [ + 1.0, + 0.0, + 0.0 + ], + "radius": 7.125 + }, + { + "center": [ + -11598.72507887, + 974.0000012077, + 179.99999901206 + ], + "axis": [ + 1.0, + 0.0, + 0.0 + ], + "radius": 6.4 + }, + { + "center": [ + -11598.00007887, + 974.0000012077, + 179.99999901206 + ], + "axis": [ + 1.0, + 0.0, + 0.0 + ], + "radius": 7.125 + }, + { + "center": [ + -11598.72507887, + 974.0000012077, + 179.99999901206 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 6.4 + }, + { + "center": [ + -11598.72507887, + 974.0000012077, + 179.99999901206 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 6.4 + }, + { + "center": [ + -11638.00007887, + 974.0000012077, + 179.99999901206 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 6.4 + }, + { + "center": [ + -11638.00007887, + 974.0000012077, + 179.99999901206 + ], + "axis": [ + 1.0, + 0.0, + 0.0 + ], + "radius": 6.4 + }, + { + "center": [ + -11598.72507887, + 974.0000012077, + 179.99999901206 + ], + "axis": [ + 1.0, + 0.0, + 0.0 + ], + "radius": 6.4 + }, + { + "center": [ + -11643.37031650775, + 974.0000012077, + 179.99999901206 + ], + "axis": [ + 1.0, + 0.0, + 0.0 + ], + "radius": 1.8884436236987767e-09 + }, + { + "center": [ + -11638.00007887, + 974.0000012077, + 179.99999901206 + ], + "axis": [ + 1.0, + 0.0, + 0.0 + ], + "radius": 6.4 + }, + { + "center": [ + -11643.37031650775, + 974.0000012077, + 179.99999901206 + ], + "axis": [ + 1.0, + 0.0, + 0.0 + ], + "radius": 1.8884498409477146e-09 + }, + { + "center": [ + -11638.00007887, + 974.0000012077, + 179.99999901206 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 6.4 + }, + { + "center": [ + -11598.00007887, + 894.0000012077, + 179.99999901206 + ], + "axis": [ + 1.0, + 0.0, + 0.0 + ], + "radius": 7.125 + }, + { + "center": [ + -11598.72507887, + 894.0000012077, + 179.99999901206 + ], + "axis": [ + 1.0, + 0.0, + 0.0 + ], + "radius": 6.4 + }, + { + "center": [ + -11598.00007887, + 894.0000012077, + 179.99999901206 + ], + "axis": [ + 1.0, + 0.0, + 0.0 + ], + "radius": 7.125 + }, + { + "center": [ + -11598.72507887, + 894.0000012077, + 179.99999901206 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 6.4 + }, + { + "center": [ + -11598.72507887, + 894.0000012077, + 179.99999901206 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 6.4 + }, + { + "center": [ + -11638.00007887, + 894.0000012077, + 179.99999901206 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 6.4 + }, + { + "center": [ + -11638.00007887, + 894.0000012077, + 179.99999901206 + ], + "axis": [ + 1.0, + 0.0, + 0.0 + ], + "radius": 6.4 + }, + { + "center": [ + -11598.72507887, + 894.0000012077, + 179.99999901206 + ], + "axis": [ + 1.0, + 0.0, + 0.0 + ], + "radius": 6.4 + }, + { + "center": [ + -11643.37031650775, + 894.0000012077, + 179.99999901206 + ], + "axis": [ + 1.0, + 0.0, + 0.0 + ], + "radius": 1.8884436236987767e-09 + }, + { + "center": [ + -11638.00007887, + 894.0000012077, + 179.99999901206 + ], + "axis": [ + 1.0, + 0.0, + 0.0 + ], + "radius": 6.4 + }, + { + "center": [ + -11643.37031650775, + 894.0000012077, + 179.99999901206 + ], + "axis": [ + 1.0, + 0.0, + 0.0 + ], + "radius": 1.8884498409477146e-09 + }, + { + "center": [ + -11638.00007887, + 894.0000012077, + 179.99999901206 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 6.4 + }, + { + "center": [ + -11598.00007887, + 894.0000012077, + -180.0000009879 + ], + "axis": [ + 1.0, + 0.0, + 0.0 + ], + "radius": 7.125 + }, + { + "center": [ + -11598.72507887, + 894.0000012077, + -180.0000009879 + ], + "axis": [ + 1.0, + 0.0, + 0.0 + ], + "radius": 6.4 + }, + { + "center": [ + -11598.00007887, + 894.0000012077, + -180.0000009879 + ], + "axis": [ + 1.0, + 0.0, + 0.0 + ], + "radius": 7.125 + }, + { + "center": [ + -11598.72507887, + 894.0000012077, + -180.0000009879 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 6.4 + }, + { + "center": [ + -11598.72507887, + 894.0000012077, + -180.0000009879 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 6.4 + }, + { + "center": [ + -11638.00007887, + 894.0000012077, + -180.0000009879 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 6.4 + }, + { + "center": [ + -11638.00007887, + 894.0000012077, + -180.0000009879 + ], + "axis": [ + 1.0, + 0.0, + 0.0 + ], + "radius": 6.4 + }, + { + "center": [ + -11598.72507887, + 894.0000012077, + -180.0000009879 + ], + "axis": [ + 1.0, + 0.0, + 0.0 + ], + "radius": 6.4 + }, + { + "center": [ + -11643.37031650775, + 894.0000012077, + -180.0000009879 + ], + "axis": [ + 1.0, + 0.0, + 0.0 + ], + "radius": 1.8884436236987767e-09 + }, + { + "center": [ + -11638.00007887, + 894.0000012077, + -180.0000009879 + ], + "axis": [ + 1.0, + 0.0, + 0.0 + ], + "radius": 6.4 + }, + { + "center": [ + -11643.37031650775, + 894.0000012077, + -180.0000009879 + ], + "axis": [ + 1.0, + 0.0, + 0.0 + ], + "radius": 1.8884498409477146e-09 + }, + { + "center": [ + -11638.00007887, + 894.0000012077, + -180.0000009879 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 6.4 + }, + { + "center": [ + -11598.00007887, + 974.0000012077, + -180.0000009879 + ], + "axis": [ + 1.0, + 0.0, + 0.0 + ], + "radius": 7.125 + }, + { + "center": [ + -11598.72507887, + 974.0000012077, + -180.0000009879 + ], + "axis": [ + 1.0, + 0.0, + 0.0 + ], + "radius": 6.4 + }, + { + "center": [ + -11598.00007887, + 974.0000012077, + -180.0000009879 + ], + "axis": [ + 1.0, + 0.0, + 0.0 + ], + "radius": 7.125 + }, + { + "center": [ + -11598.72507887, + 974.0000012077, + -180.0000009879 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 6.4 + }, + { + "center": [ + -11598.72507887, + 974.0000012077, + -180.0000009879 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 6.4 + }, + { + "center": [ + -11638.00007887, + 974.0000012077, + -180.0000009879 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 6.4 + }, + { + "center": [ + -11638.00007887, + 974.0000012077, + -180.0000009879 + ], + "axis": [ + 1.0, + 0.0, + 0.0 + ], + "radius": 6.4 + }, + { + "center": [ + -11598.72507887, + 974.0000012077, + -180.0000009879 + ], + "axis": [ + 1.0, + 0.0, + 0.0 + ], + "radius": 6.4 + }, + { + "center": [ + -11643.37031650775, + 974.0000012077, + -180.0000009879 + ], + "axis": [ + 1.0, + 0.0, + 0.0 + ], + "radius": 1.8884436236987767e-09 + }, + { + "center": [ + -11638.00007887, + 974.0000012077, + -180.0000009879 + ], + "axis": [ + 1.0, + 0.0, + 0.0 + ], + "radius": 6.4 + }, + { + "center": [ + -11643.37031650775, + 974.0000012077, + -180.0000009879 + ], + "axis": [ + 1.0, + 0.0, + 0.0 + ], + "radius": 1.8884498409477146e-09 + }, + { + "center": [ + -11638.00007887, + 974.0000012077, + -180.0000009879 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 6.4 + }, + { + "center": [ + 0.0, + 106.0, + 669.0000154664 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 20.0 + }, + { + "center": [ + 0.0, + 136.0, + 699.0000154664 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 10.0 + }, + { + "center": [ + 0.0, + 169.0, + 689.9995359775 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 3.0 + }, + { + "center": [ + 0.0, + 169.0, + -689.9995359775 + ], + "axis": [ + 1.0, + 0.0, + -0.0 + ], + "radius": 3.0 + }, + { + "center": [ + 0.0, + 136.0, + -699.0000154664 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 10.0 + }, + { + "center": [ + 0.0, + 106.0, + -669.0000154664 + ], + "axis": [ + 1.0, + 0.0, + -0.0 + ], + "radius": 20.0 + }, + { + "center": [ + 0.0, + 0.999998792332, + 0.0 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 100.0 + }, + { + "center": [ + 0.0, + 0.999998792332, + 0.0 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 100.0 + }, + { + "center": [ + 0.0, + 40.0, + 180.0 + ], + "axis": [ + 1.0, + 0.0, + 0.0 + ], + "radius": 7.125 + }, + { + "center": [ + 0.0, + 40.0, + 180.0 + ], + "axis": [ + 1.0, + 0.0, + 0.0 + ], + "radius": 7.125 + }, + { + "center": [ + 0.0, + -40.0, + 180.0 + ], + "axis": [ + 1.0, + 0.0, + 0.0 + ], + "radius": 7.125 + }, + { + "center": [ + 0.0, + -40.0, + 180.0 + ], + "axis": [ + 1.0, + 0.0, + 0.0 + ], + "radius": 7.125 + }, + { + "center": [ + 0.0, + -40.0, + -180.0 + ], + "axis": [ + 1.0, + 0.0, + 0.0 + ], + "radius": 7.125 + }, + { + "center": [ + 0.0, + -40.0, + -180.0 + ], + "axis": [ + 1.0, + 0.0, + 0.0 + ], + "radius": 7.125 + }, + { + "center": [ + 0.0, + 40.0, + -180.0 + ], + "axis": [ + 1.0, + 0.0, + 0.0 + ], + "radius": 7.125 + }, + { + "center": [ + 0.0, + 40.0, + -180.0 + ], + "axis": [ + 1.0, + 0.0, + 0.0 + ], + "radius": 7.125 + }, + { + "center": [ + 0.0, + 100.9999987923, + -179.9999990121 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 16.0 + }, + { + "center": [ + 0.0, + 100.9999987923, + -179.9999990121 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 16.0 + }, + { + "center": [ + 0.0, + 100.9999987923, + 180.0000009879 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 16.0 + }, + { + "center": [ + 0.0, + 100.9999987923, + 180.0000009879 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 16.0 + }, + { + "center": [ + 0.0, + -99.00000120767, + -179.9999990121 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 16.0 + }, + { + "center": [ + 0.0, + -99.00000120767, + -179.9999990121 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 16.0 + }, + { + "center": [ + 0.0, + -99.00000120767, + 180.0000009879 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 16.0 + }, + { + "center": [ + 0.0, + -99.00000120767, + 180.0000009879 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 16.0 + }, + { + "center": [ + -80.0, + -99.00000120767, + -189.9999990121 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 40.0 + }, + { + "center": [ + -80.0, + -99.00000120767, + 190.0000009879 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 40.0 + }, + { + "center": [ + -79.0, + -99.00000120767, + 227.0000009879 + ], + "axis": [ + 0.0, + -1.0, + 0.0 + ], + "radius": 3.0 + }, + { + "center": [ + -80.0, + -99.00000120767, + 190.0000009879 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 40.0 + }, + { + "center": [ + -79.0, + -99.00000120767, + 190.0000009879 + ], + "axis": [ + 1.0, + 0.0, + -0.0 + ], + "radius": 40.0 + }, + { + "center": [ + -80.0, + -99.00000120767, + -189.9999990121 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 40.0 + }, + { + "center": [ + -79.0, + -99.00000120767, + -189.9999990121 + ], + "axis": [ + 1.0, + 0.0, + 0.0 + ], + "radius": 40.0 + }, + { + "center": [ + -79.0, + -99.00000120767, + -226.9999990121 + ], + "axis": [ + -0.0, + 1.0, + 0.0 + ], + "radius": 3.0 + }, + { + "center": [ + -76.0, + -99.00000120767, + 190.0000009879 + ], + "axis": [ + 1.0, + 0.0, + -0.0 + ], + "radius": 37.0 + }, + { + "center": [ + -76.0, + -99.00000120767, + -189.9999990121 + ], + "axis": [ + 1.0, + 0.0, + 0.0 + ], + "radius": 37.0 + }, + { + "center": [ + -76.0, + 106.0, + -669.0000154664 + ], + "axis": [ + 1.0, + 0.0, + 0.0 + ], + "radius": 20.0 + }, + { + "center": [ + -76.0, + 136.0, + -699.0000154664 + ], + "axis": [ + 1.0, + 0.0, + 0.0 + ], + "radius": 10.0 + }, + { + "center": [ + -76.0, + 169.0, + -689.9995359775 + ], + "axis": [ + 1.0, + 0.0, + 0.0 + ], + "radius": 3.0 + }, + { + "center": [ + -76.0, + 169.0, + 689.9995359775 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 3.0 + }, + { + "center": [ + -76.0, + 136.0, + 699.0000154664 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 10.0 + }, + { + "center": [ + -76.0, + 106.0, + 669.0000154664 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 20.0 + }, + { + "center": [ + -76.0, + -99.00000120767, + -179.9999990121 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 16.0 + }, + { + "center": [ + -76.0, + -99.00000120767, + -179.9999990121 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 16.0 + }, + { + "center": [ + -76.0, + 100.9999987923, + 180.0000009879 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 16.0 + }, + { + "center": [ + -76.0, + 100.9999987923, + 180.0000009879 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 16.0 + }, + { + "center": [ + -76.0, + 100.9999987923, + -179.9999990121 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 16.0 + }, + { + "center": [ + -76.0, + 100.9999987923, + -179.9999990121 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 16.0 + }, + { + "center": [ + -76.0, + 0.999998792332, + 0.0 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 100.0 + }, + { + "center": [ + -76.0, + 0.999998792332, + 0.0 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 100.0 + }, + { + "center": [ + -76.0, + -99.00000120767, + 180.0000009879 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 16.0 + }, + { + "center": [ + -76.0, + -99.00000120767, + 180.0000009879 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 16.0 + }, + { + "center": [ + -76.0, + -99.00000120767, + 180.0000009879 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 16.0 + }, + { + "center": [ + 0.0, + -99.00000120767, + 180.0000009879 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 16.0 + }, + { + "center": [ + -76.0, + -99.00000120767, + 180.0000009879 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 16.0 + }, + { + "center": [ + 0.0, + -99.00000120767, + 180.0000009879 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 16.0 + }, + { + "center": [ + -76.0, + -99.00000120767, + -179.9999990121 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 16.0 + }, + { + "center": [ + 0.0, + -99.00000120767, + -179.9999990121 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 16.0 + }, + { + "center": [ + -76.0, + -99.00000120767, + -179.9999990121 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 16.0 + }, + { + "center": [ + 0.0, + -99.00000120767, + -179.9999990121 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 16.0 + }, + { + "center": [ + -76.0, + 100.9999987923, + 180.0000009879 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 16.0 + }, + { + "center": [ + 0.0, + 100.9999987923, + 180.0000009879 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 16.0 + }, + { + "center": [ + -76.0, + 100.9999987923, + 180.0000009879 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 16.0 + }, + { + "center": [ + 0.0, + 100.9999987923, + 180.0000009879 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 16.0 + }, + { + "center": [ + -76.0, + 100.9999987923, + -179.9999990121 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 16.0 + }, + { + "center": [ + 0.0, + 100.9999987923, + -179.9999990121 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 16.0 + }, + { + "center": [ + -76.0, + 100.9999987923, + -179.9999990121 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 16.0 + }, + { + "center": [ + 0.0, + 100.9999987923, + -179.9999990121 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 16.0 + }, + { + "center": [ + -76.0, + 0.999998792332, + 0.0 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 100.0 + }, + { + "center": [ + 0.0, + 0.999998792332, + 0.0 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 100.0 + }, + { + "center": [ + -76.0, + 0.999998792332, + 0.0 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 100.0 + }, + { + "center": [ + 0.0, + 0.999998792332, + 0.0 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 100.0 + }, + { + "center": [ + -76.0, + -99.00000120767, + 190.0000009879 + ], + "axis": [ + 1.0, + 0.0, + -0.0 + ], + "radius": 37.0 + }, + { + "center": [ + -79.0, + -99.00000120767, + 227.0000009879 + ], + "axis": [ + 0.0, + -1.0, + 0.0 + ], + "radius": 3.0 + }, + { + "center": [ + -79.0, + -99.00000120767, + 190.0000009879 + ], + "axis": [ + 1.0, + 0.0, + -0.0 + ], + "radius": 40.0 + }, + { + "center": [ + -79.0, + -136.0000012077, + 190.0000009879 + ], + "axis": [ + 0.0, + 0.0, + -1.0 + ], + "radius": 3.0 + }, + { + "center": [ + -79.0, + -136.0000012077, + 190.0000009879 + ], + "axis": [ + 0.0, + 0.0, + -1.0 + ], + "radius": 3.0 + }, + { + "center": [ + -79.0, + -136.0000012077, + -189.9999990121 + ], + "axis": [ + 0.0, + 0.0, + -1.0 + ], + "radius": 3.0 + }, + { + "center": [ + -76.0, + -99.00000120767, + -189.9999990121 + ], + "axis": [ + 1.0, + 0.0, + 0.0 + ], + "radius": 37.0 + }, + { + "center": [ + -79.0, + -136.0000012077, + -189.9999990121 + ], + "axis": [ + 0.0, + 0.0, + -1.0 + ], + "radius": 3.0 + }, + { + "center": [ + -79.0, + -99.00000120767, + -189.9999990121 + ], + "axis": [ + 1.0, + 0.0, + 0.0 + ], + "radius": 40.0 + }, + { + "center": [ + -79.0, + -99.00000120767, + -226.9999990121 + ], + "axis": [ + -0.0, + 1.0, + 0.0 + ], + "radius": 3.0 + }, + { + "center": [ + -76.0, + 106.0, + -669.0000154664 + ], + "axis": [ + 1.0, + 0.0, + 0.0 + ], + "radius": 20.0 + }, + { + "center": [ + 0.0, + 106.0, + -669.0000154664 + ], + "axis": [ + 1.0, + 0.0, + -0.0 + ], + "radius": 20.0 + }, + { + "center": [ + 0.0, + 136.0, + -699.0000154664 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 10.0 + }, + { + "center": [ + -76.0, + 136.0, + -699.0000154664 + ], + "axis": [ + 1.0, + 0.0, + 0.0 + ], + "radius": 10.0 + }, + { + "center": [ + -76.0, + 169.0, + -689.9995359775 + ], + "axis": [ + 1.0, + 0.0, + 0.0 + ], + "radius": 3.0 + }, + { + "center": [ + 0.0, + 169.0, + -689.9995359775 + ], + "axis": [ + 1.0, + 0.0, + -0.0 + ], + "radius": 3.0 + }, + { + "center": [ + -76.0, + 169.0, + 689.9995359775 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 3.0 + }, + { + "center": [ + 0.0, + 169.0, + 689.9995359775 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 3.0 + }, + { + "center": [ + 0.0, + 136.0, + 699.0000154664 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 10.0 + }, + { + "center": [ + -76.0, + 136.0, + 699.0000154664 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 10.0 + }, + { + "center": [ + -76.0, + 106.0, + 669.0000154664 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 20.0 + }, + { + "center": [ + 0.0, + 106.0, + 669.0000154664 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 20.0 + }, + { + "center": [ + 0.0, + 40.0, + 180.0 + ], + "axis": [ + 1.0, + 0.0, + 0.0 + ], + "radius": 7.125 + }, + { + "center": [ + -0.725, + 40.0, + 180.0 + ], + "axis": [ + 1.0, + 0.0, + 0.0 + ], + "radius": 6.4 + }, + { + "center": [ + 0.0, + 40.0, + 180.0 + ], + "axis": [ + 1.0, + 0.0, + 0.0 + ], + "radius": 7.125 + }, + { + "center": [ + -0.725, + 40.0, + 180.0 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 6.4 + }, + { + "center": [ + -0.725, + 40.0, + 180.0 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 6.4 + }, + { + "center": [ + -40.0, + 40.0, + 180.0 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 6.4 + }, + { + "center": [ + -40.0, + 40.0, + 180.0 + ], + "axis": [ + 1.0, + 0.0, + 0.0 + ], + "radius": 6.4 + }, + { + "center": [ + -0.725, + 40.0, + 180.0 + ], + "axis": [ + 1.0, + 0.0, + 0.0 + ], + "radius": 6.4 + }, + { + "center": [ + -45.37023763953209, + 40.0, + 180.0 + ], + "axis": [ + 1.0, + 0.0, + 0.0 + ], + "radius": 1.7505996652289468e-12 + }, + { + "center": [ + -40.0, + 40.0, + 180.0 + ], + "axis": [ + 1.0, + 0.0, + 0.0 + ], + "radius": 6.4 + }, + { + "center": [ + -45.37023763953209, + 40.00000000000001, + 180.0 + ], + "axis": [ + 1.0, + -0.0, + 0.0 + ], + "radius": 1.7483792191796965e-12 + }, + { + "center": [ + -40.0, + 40.0, + 180.0 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 6.4 + }, + { + "center": [ + 0.0, + -40.0, + 180.0 + ], + "axis": [ + 1.0, + 0.0, + 0.0 + ], + "radius": 7.125 + }, + { + "center": [ + -0.725, + -40.0, + 180.0 + ], + "axis": [ + 1.0, + 0.0, + 0.0 + ], + "radius": 6.4 + }, + { + "center": [ + 0.0, + -40.0, + 180.0 + ], + "axis": [ + 1.0, + 0.0, + 0.0 + ], + "radius": 7.125 + }, + { + "center": [ + -0.725, + -40.0, + 180.0 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 6.4 + }, + { + "center": [ + -0.725, + -40.0, + 180.0 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 6.4 + }, + { + "center": [ + -40.0, + -40.0, + 180.0 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 6.4 + }, + { + "center": [ + -40.0, + -40.0, + 180.0 + ], + "axis": [ + 1.0, + 0.0, + 0.0 + ], + "radius": 6.4 + }, + { + "center": [ + -0.725, + -40.0, + 180.0 + ], + "axis": [ + 1.0, + 0.0, + 0.0 + ], + "radius": 6.4 + }, + { + "center": [ + -45.37023763953209, + -40.0, + 180.0 + ], + "axis": [ + 1.0, + 0.0, + 0.0 + ], + "radius": 1.7505996652289468e-12 + }, + { + "center": [ + -40.0, + -40.0, + 180.0 + ], + "axis": [ + 1.0, + 0.0, + 0.0 + ], + "radius": 6.4 + }, + { + "center": [ + -45.37023763953209, + -39.99999999999999, + 180.0 + ], + "axis": [ + 1.0, + -0.0, + 0.0 + ], + "radius": 1.7483792191796965e-12 + }, + { + "center": [ + -40.0, + -40.0, + 180.0 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 6.4 + }, + { + "center": [ + 0.0, + -40.0, + -180.0 + ], + "axis": [ + 1.0, + 0.0, + 0.0 + ], + "radius": 7.125 + }, + { + "center": [ + -0.725, + -40.0, + -180.0 + ], + "axis": [ + 1.0, + 0.0, + 0.0 + ], + "radius": 6.4 + }, + { + "center": [ + 0.0, + -40.0, + -180.0 + ], + "axis": [ + 1.0, + 0.0, + 0.0 + ], + "radius": 7.125 + }, + { + "center": [ + -0.725, + -40.0, + -180.0 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 6.4 + }, + { + "center": [ + -0.725, + -40.0, + -180.0 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 6.4 + }, + { + "center": [ + -40.0, + -40.0, + -180.0 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 6.4 + }, + { + "center": [ + -40.0, + -40.0, + -180.0 + ], + "axis": [ + 1.0, + 0.0, + 0.0 + ], + "radius": 6.4 + }, + { + "center": [ + -0.725, + -40.0, + -180.0 + ], + "axis": [ + 1.0, + 0.0, + 0.0 + ], + "radius": 6.4 + }, + { + "center": [ + -45.37023763953209, + -40.0, + -180.0 + ], + "axis": [ + 1.0, + 0.0, + 0.0 + ], + "radius": 1.7505996652289468e-12 + }, + { + "center": [ + -40.0, + -40.0, + -180.0 + ], + "axis": [ + 1.0, + 0.0, + 0.0 + ], + "radius": 6.4 + }, + { + "center": [ + -45.37023763953209, + -40.00000000000001, + -180.0 + ], + "axis": [ + 1.0, + -0.0, + 0.0 + ], + "radius": 1.7483792191796965e-12 + }, + { + "center": [ + -40.0, + -40.0, + -180.0 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 6.4 + }, + { + "center": [ + 0.0, + 40.0, + -180.0 + ], + "axis": [ + 1.0, + 0.0, + 0.0 + ], + "radius": 7.125 + }, + { + "center": [ + -0.725, + 40.0, + -180.0 + ], + "axis": [ + 1.0, + 0.0, + 0.0 + ], + "radius": 6.4 + }, + { + "center": [ + 0.0, + 40.0, + -180.0 + ], + "axis": [ + 1.0, + 0.0, + 0.0 + ], + "radius": 7.125 + }, + { + "center": [ + -0.725, + 40.0, + -180.0 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 6.4 + }, + { + "center": [ + -0.725, + 40.0, + -180.0 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 6.4 + }, + { + "center": [ + -40.0, + 40.0, + -180.0 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 6.4 + }, + { + "center": [ + -40.0, + 40.0, + -180.0 + ], + "axis": [ + 1.0, + 0.0, + 0.0 + ], + "radius": 6.4 + }, + { + "center": [ + -0.725, + 40.0, + -180.0 + ], + "axis": [ + 1.0, + 0.0, + 0.0 + ], + "radius": 6.4 + }, + { + "center": [ + -45.37023763953209, + 40.0, + -180.0 + ], + "axis": [ + 1.0, + 0.0, + 0.0 + ], + "radius": 1.7505996652289468e-12 + }, + { + "center": [ + -40.0, + 40.0, + -180.0 + ], + "axis": [ + 1.0, + 0.0, + 0.0 + ], + "radius": 6.4 + }, + { + "center": [ + -45.37023763953209, + 39.99999999999999, + -180.0 + ], + "axis": [ + 1.0, + -0.0, + 0.0 + ], + "radius": 1.7483792191796965e-12 + }, + { + "center": [ + -40.0, + 40.0, + -180.0 + ], + "axis": [ + -1.0, + 0.0, + 0.0 + ], + "radius": 6.4 + } + ] +} \ No newline at end of file diff --git a/alg/delivery_v1.1/coupler_panel_release/test_data/synth/manifest.json b/alg/delivery_v1.1/coupler_panel_release/test_data/synth/manifest.json new file mode 100644 index 0000000..0c207d0 --- /dev/null +++ b/alg/delivery_v1.1/coupler_panel_release/test_data/synth/manifest.json @@ -0,0 +1,70 @@ +{ + "stp": "test_data\\lu1.stp", + "profile": "lu1_af4", + "gt": "gt.json", + "samples": [ + { + "sample_id": "rp000_zp000", + "ply": "cloud_rp000_zp000.ply", + "theta_right_deg": 0.0, + "theta_ref_deg": 0.0, + "n_points": 96662 + }, + { + "sample_id": "rp010_zp000", + "ply": "cloud_rp010_zp000.ply", + "theta_right_deg": 0.1, + "theta_ref_deg": 0.0, + "n_points": 96459 + }, + { + "sample_id": "rp030_zp000", + "ply": "cloud_rp030_zp000.ply", + "theta_right_deg": 0.3, + "theta_ref_deg": 0.0, + "n_points": 96605 + }, + { + "sample_id": "rp050_zp000", + "ply": "cloud_rp050_zp000.ply", + "theta_right_deg": 0.5, + "theta_ref_deg": 0.0, + "n_points": 96706 + }, + { + "sample_id": "rp100_zp000", + "ply": "cloud_rp100_zp000.ply", + "theta_right_deg": 1.0, + "theta_ref_deg": 0.0, + "n_points": 96664 + }, + { + "sample_id": "rp200_zp000", + "ply": "cloud_rp200_zp000.ply", + "theta_right_deg": 2.0, + "theta_ref_deg": 0.0, + "n_points": 96713 + }, + { + "sample_id": "rp000_zp050", + "ply": "cloud_rp000_zp050.ply", + "theta_right_deg": 0.0, + "theta_ref_deg": 0.5, + "n_points": 96666 + }, + { + "sample_id": "rp050_zp050", + "ply": "cloud_rp050_zp050.ply", + "theta_right_deg": 0.5, + "theta_ref_deg": 0.5, + "n_points": 96614 + }, + { + "sample_id": "rp100_zp100", + "ply": "cloud_rp100_zp100.ply", + "theta_right_deg": 1.0, + "theta_ref_deg": 1.0, + "n_points": 96601 + } + ] +} \ No newline at end of file diff --git a/alg/delivery_v1.1/underframe_deflection/README.md b/alg/delivery_v1.1/underframe_deflection/README.md new file mode 100644 index 0000000..2cdfd9b --- /dev/null +++ b/alg/delivery_v1.1/underframe_deflection/README.md @@ -0,0 +1,200 @@ +# underframe-deflection + +**从扫描点云测量高铁底架纵梁挠度的 Python 算法包：接受"CAD 基准 + 扫描点云 + ROI JSON"三件套输入，自动配准、去噪、识别梁、出 Excel 误差报表。** + +[![python](https://img.shields.io/badge/python-3.10%2B-blue)]() [![version](https://img.shields.io/badge/version-1.1.0-green)]() [![status](https://img.shields.io/badge/status-beta-yellow)]() + +--- + +## 这是什么 + +输入一份现场扫描点云（`.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. diff --git a/alg/delivery_v1.1/underframe_deflection/USAGE.md b/alg/delivery_v1.1/underframe_deflection/USAGE.md new file mode 100644 index 0000000..80d2b92 --- /dev/null +++ b/alg/delivery_v1.1/underframe_deflection/USAGE.md @@ -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, + } +``` diff --git a/alg/delivery_v1.1/underframe_deflection/__init__.py b/alg/delivery_v1.1/underframe_deflection/__init__.py new file mode 100644 index 0000000..806717a --- /dev/null +++ b/alg/delivery_v1.1/underframe_deflection/__init__.py @@ -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", +] diff --git a/alg/delivery_v1.1/underframe_deflection/_version.py b/alg/delivery_v1.1/underframe_deflection/_version.py new file mode 100644 index 0000000..9daa655 --- /dev/null +++ b/alg/delivery_v1.1/underframe_deflection/_version.py @@ -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"] diff --git a/alg/delivery_v1.1/underframe_deflection/api.py b/alg/delivery_v1.1/underframe_deflection/api.py new file mode 100644 index 0000000..7c9e543 --- /dev/null +++ b/alg/delivery_v1.1/underframe_deflection/api.py @@ -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", +] diff --git a/alg/delivery_v1.1/underframe_deflection/artifacts.py b/alg/delivery_v1.1/underframe_deflection/artifacts.py new file mode 100644 index 0000000..6bdbc48 --- /dev/null +++ b/alg/delivery_v1.1/underframe_deflection/artifacts.py @@ -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", +] diff --git a/alg/delivery_v1.1/underframe_deflection/beams.py b/alg/delivery_v1.1/underframe_deflection/beams.py new file mode 100644 index 0000000..186d2de --- /dev/null +++ b/alg/delivery_v1.1/underframe_deflection/beams.py @@ -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 diff --git a/alg/delivery_v1.1/underframe_deflection/cad_io.py b/alg/delivery_v1.1/underframe_deflection/cad_io.py new file mode 100644 index 0000000..38da878 --- /dev/null +++ b/alg/delivery_v1.1/underframe_deflection/cad_io.py @@ -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(" 36 bytes -> 9 floats. + floats = np.frombuffer(arr[:, 12:48].tobytes(), dtype=" 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"] diff --git a/alg/delivery_v1.1/underframe_deflection/cli.py b/alg/delivery_v1.1/underframe_deflection/cli.py new file mode 100644 index 0000000..546bac8 --- /dev/null +++ b/alg/delivery_v1.1/underframe_deflection/cli.py @@ -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() diff --git a/alg/delivery_v1.1/underframe_deflection/config.py b/alg/delivery_v1.1/underframe_deflection/config.py new file mode 100644 index 0000000..43f30a1 --- /dev/null +++ b/alg/delivery_v1.1/underframe_deflection/config.py @@ -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 diff --git a/alg/delivery_v1.1/underframe_deflection/excel_report.py b/alg/delivery_v1.1/underframe_deflection/excel_report.py new file mode 100644 index 0000000..9e2223c --- /dev/null +++ b/alg/delivery_v1.1/underframe_deflection/excel_report.py @@ -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"] diff --git a/alg/delivery_v1.1/underframe_deflection/exceptions.py b/alg/delivery_v1.1/underframe_deflection/exceptions.py new file mode 100644 index 0000000..a1c11c4 --- /dev/null +++ b/alg/delivery_v1.1/underframe_deflection/exceptions.py @@ -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", +] diff --git a/alg/delivery_v1.1/underframe_deflection/fitting.py b/alg/delivery_v1.1/underframe_deflection/fitting.py new file mode 100644 index 0000000..595babe --- /dev/null +++ b/alg/delivery_v1.1/underframe_deflection/fitting.py @@ -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] diff --git a/alg/delivery_v1.1/underframe_deflection/frame.py b/alg/delivery_v1.1/underframe_deflection/frame.py new file mode 100644 index 0000000..ead6d8d --- /dev/null +++ b/alg/delivery_v1.1/underframe_deflection/frame.py @@ -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 diff --git a/alg/delivery_v1.1/underframe_deflection/io.py b/alg/delivery_v1.1/underframe_deflection/io.py new file mode 100644 index 0000000..60e49e9 --- /dev/null +++ b/alg/delivery_v1.1/underframe_deflection/io.py @@ -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") diff --git a/alg/delivery_v1.1/underframe_deflection/pipeline.py b/alg/delivery_v1.1/underframe_deflection/pipeline.py new file mode 100644 index 0000000..b7da5a9 --- /dev/null +++ b/alg/delivery_v1.1/underframe_deflection/pipeline.py @@ -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 ``/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"] diff --git a/alg/delivery_v1.1/underframe_deflection/ply_overlay.py b/alg/delivery_v1.1/underframe_deflection/ply_overlay.py new file mode 100644 index 0000000..6ed1e7d --- /dev/null +++ b/alg/delivery_v1.1/underframe_deflection/ply_overlay.py @@ -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), + } diff --git a/alg/delivery_v1.1/underframe_deflection/preprocess.py b/alg/delivery_v1.1/underframe_deflection/preprocess.py new file mode 100644 index 0000000..175236f --- /dev/null +++ b/alg/delivery_v1.1/underframe_deflection/preprocess.py @@ -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, + ) diff --git a/alg/delivery_v1.1/underframe_deflection/py.typed b/alg/delivery_v1.1/underframe_deflection/py.typed new file mode 100644 index 0000000..e69de29 diff --git a/alg/delivery_v1.1/underframe_deflection/pyproject.toml b/alg/delivery_v1.1/underframe_deflection/pyproject.toml new file mode 100644 index 0000000..7152f89 --- /dev/null +++ b/alg/delivery_v1.1/underframe_deflection/pyproject.toml @@ -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"] diff --git a/alg/delivery_v1.1/underframe_deflection/reference.py b/alg/delivery_v1.1/underframe_deflection/reference.py new file mode 100644 index 0000000..abb80ba --- /dev/null +++ b/alg/delivery_v1.1/underframe_deflection/reference.py @@ -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 diff --git a/alg/delivery_v1.1/underframe_deflection/registration.py b/alg/delivery_v1.1/underframe_deflection/registration.py new file mode 100644 index 0000000..c4a3a11 --- /dev/null +++ b/alg/delivery_v1.1/underframe_deflection/registration.py @@ -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"] diff --git a/alg/delivery_v1.1/underframe_deflection/requirements.txt b/alg/delivery_v1.1/underframe_deflection/requirements.txt new file mode 100644 index 0000000..2e89148 --- /dev/null +++ b/alg/delivery_v1.1/underframe_deflection/requirements.txt @@ -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) diff --git a/alg/delivery_v1.1/underframe_deflection/roi.py b/alg/delivery_v1.1/underframe_deflection/roi.py new file mode 100644 index 0000000..b87493c --- /dev/null +++ b/alg/delivery_v1.1/underframe_deflection/roi.py @@ -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="") + 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"] diff --git a/alg/delivery_v1.1/underframe_deflection/structures.py b/alg/delivery_v1.1/underframe_deflection/structures.py new file mode 100644 index 0000000..c4011ad --- /dev/null +++ b/alg/delivery_v1.1/underframe_deflection/structures.py @@ -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) diff --git a/alg/delivery_v1.1/underframe_deflection/test_data/sample_roi.json b/alg/delivery_v1.1/underframe_deflection/test_data/sample_roi.json new file mode 100644 index 0000000..595aa12 --- /dev/null +++ b/alg/delivery_v1.1/underframe_deflection/test_data/sample_roi.json @@ -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 +} diff --git a/alg/delivery_v1.1/underframe_deflection/visualization.py b/alg/delivery_v1.1/underframe_deflection/visualization.py new file mode 100644 index 0000000..18106ab --- /dev/null +++ b/alg/delivery_v1.1/underframe_deflection/visualization.py @@ -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) diff --git a/alg/point_porj/Readme.md b/alg/point_porj/Readme.md new file mode 100644 index 0000000..aa00618 --- /dev/null +++ b/alg/point_porj/Readme.md @@ -0,0 +1,454 @@ +# 测量工具 + +这是一个面向工业零件点云、网格和 CAD STEP 模型的几何测量工具。项目支持 CAD 转 STL、点云加噪/滤波、点云配准、点云到 Mesh/CAD 的距离评估、点云到 CAD 的仅平移对齐、曲率估计和平面拟合。 + +正式代码位于项目根目录，辅助脚本放在 `archive/`，示例数据放在 `datasets/`，打包工具放在 `packaging/`。推荐在项目根目录执行命令。 + +面向开发人员的完整使用说明见 [USAGE.md](USAGE.md)。 + +## 1. 项目结构 + +```text +. + main.py # 命令行入口 + pipeline.py # MeasurementPipeline，总流程入口类 + io_handler.py # 输入检查、点云/Mesh/CAD 读取 + config_loader.py # YAML 配置加载 + config.yaml # 默认配置 + cad_converter.py # STEP -> STL + registration.py # RANSAC + ICP 配准 + evaluation.py # Chamfer、点云到 Mesh 距离、统计指标 + visualization.py # Open3D 可视化 + preprocessing/ # 加噪、滤波、曲率、平面拟合 + datasets/ # 示例 CAD、Mesh、PointCloud、Noise_PointCloud + vis/ # 建议保存命令日志、可视化点云和中间结果 + results/ # translate-align 默认输出目录 + archive/ # 辅助脚本 + packaging/ # pip 打包文件夹 +``` + +## 2. 环境安装 + +建议使用独立的 Python 3.10 环境，不要直接安装到 conda 的 `base` 环境。`pythonocc-core` 需要通过 conda 安装，`requirements.txt` 只管理 pip 依赖。 + +```bash +conda env create -f environment.yml +conda activate point-porj +pip install -r requirements.txt +pip install git+https://github.com/AutodeskAILab/occwl +``` + +如果已经创建过环境，需要补装或更新 CAD 转 Mesh 依赖： + +```bash +conda env update -n point-porj -f environment.yml +pip install git+https://github.com/AutodeskAILab/occwl +``` + +如果当前终端显示 `(base)`，并且 base 环境是 Python 3.12，直接安装 `pythonocc-core=7.5.1` 会失败；该版本只支持 Python 3.7 到 3.10。 + +验证环境： + +```bash +python -c "import open3d, trimesh, torch, yaml, OCC.Core.STEPControl; print('env ok')" +``` + +如果日志里出现 CUDA driver 版本过旧的 warning，程序会退回 CPU 计算。当前配准流程本身主要使用 Open3D CPU 管线；点云到 Mesh/CAD 的距离计算使用 torch，理论上可用 GPU，但取决于 `torch.cuda.is_available()`。 + +## 3. 数据要求 + +默认数据目录结构如下： + +```text +datasets/ + CAD/ # STEP CAD 文件，示例：11.step、40.step + Mesh/ # STL/OBJ/PLY Mesh 文件，示例：11.stl、40.stl + PointCloud/ # 原始/干净点云 PLY，示例：11.ply、40.ply + Noise_PointCloud/ # 加噪或待配准点云 PLY，示例：11_noisy.ply、40_noisy.ply + shift_test_output/ # translate-align 测试点云目录，默认读取 shifted_whole_pointcloud.ply +``` + +当前批量评估示例数据包含编号 `11`、`13`、`17`、`20`、`25`、`26`、`27`、`31`、`34`、`35`、`36`、`37`、`38`、`39`、`40`、`41`、`42`、`45`，同一编号通常应同时存在： + +```text +datasets/CAD/11.step +datasets/Mesh/11.stl +datasets/PointCloud/11.ply +datasets/Noise_PointCloud/11_noisy.ply +``` + +`translate-align` 默认使用： + +```text +datasets/CAD/M02.stp #标准CAD +datasets/shift_test_output/shifted_whole_pointcloud.ply #实际采集点云 +``` + +输入格式要求： + +- CAD：推荐 STEP，扩展名通常为 `.step` 或 `.stp`。用于 `cad2mesh` 和 `contour`。 +- Mesh：支持 `trimesh` 能读取的格式，常用 `.stl`、`.obj`、`.ply`。Mesh 必须包含顶点和三角面。 +- PointCloud：推荐 ASCII 或 binary `.ply`，必须包含三维点坐标。曲率、配准、距离评估都需要非空点云。 +- 路径：可以使用相对路径或绝对路径；相对路径按当前运行目录解析，建议在项目根目录执行。 +- 单位：示例数据按毫米量级处理。CAD、Mesh 和点云必须使用同一坐标系和同一单位，否则距离、配准结果没有可比性。 +- 对齐关系：`contour` 假设点云已经与 Mesh/CAD 在同一坐标系下；`registration` 会模拟旋转后做配准。 + +数据规模注意： + +- 百万级点云可以运行，但 `registration` 和 `contour` 会比较慢。 +- `evaluation.chunk_size` 控制点云分块大小。 +- `evaluation.triangle_chunk_size` 控制 Mesh/CAD 三角面分块大小。内存或显存不足时调小，例如 `1024` 或 `2048`。 +- `evaluation.cad_mesh_deflection` 控制 STEP 三角化精度。越小越精细，但三角面更多、计算更慢。 + +## 4. 输出说明 + +建议将通用实验结果放到 `vis/`，Excel 结果放到 `excel/`，`translate-align` 默认写入 `results/`： + +```bash +mkdir -p vis excel results +``` + +常见输出包括： + +- `.log`：命令行指标输出。 +- `.time`：命令耗时记录。 +- `.stl`：CAD 转 Mesh 结果。 +- `.ply`：带颜色的点云结果，例如曲率点云或 CAD 距离热力图。 +- `.xlsx`：`contour --excel-output` 生成的 Excel 汇总，默认保存到 `excel/contour.xlsx`。 +- `results/aligned_pointcloud_translation_only.ply`：`translate-align` 默认输出的对齐后点云。 +- `results/T_translation_only.txt`：`translate-align` 默认输出的 4x4 平移矩阵。 + +本项目不会自动把所有命令结果写入 `vis/`；如果需要保留日志，可以用 shell 重定向： + +```bash +python main.py contour --pointcloud datasets/PointCloud/11.ply --cad datasets/CAD/11.step --no-visualize > vis/contour.log 2>&1 +``` + +## 5. 方法输入输出总览 + +| 方法 | 功能 | 输入 | 输出 | +|------|------|------|------| +| `translate-align` | 按车头/车尾质心将点云与标准CAD进行配准 | 默认 `--cad datasets/CAD/M02.stp --pointcloud datasets/shift_test_output/`；可调 `--axis`、`--crop-ratio`、`--cad-sample-points`、`--local-sample-points` | 默认写入 `results/`，输出对齐后点云 `aligned_pointcloud_translation_only.ply`、变换矩阵 `T_translation_only.txt` 和局部采样检查文件 | +| `cad2mesh` | 将 STEP CAD 转成 STL 三角网格 | `--cad`：STEP 文件 | `--mesh`：STL 文件 | +| `denoise` | 给干净点云加噪，并运行配置中的滤波方法 | `--pointcloud`：干净 PLY 点云；`--noise-std`：噪声标准差 | 终端输出点数、平均噪声偏移、滤波保留比例、Chamfer Distance；当前不自动保存滤波点云 | +| `registration` | 模拟旋转后进行点云到 Mesh 的配准，并评估配准后距离 | `--pointcloud`：待配准 PLY 点云；`--mesh`：参考 STL/OBJ/PLY Mesh | 终端输出 fitness、RMSE、变换矩阵、配准后点云到 Mesh 的距离统计 | +| `contour` | 轮廓/平整度/平面度评估，计算点云到 STEP CAD 三角面的距离 | 单任务：`--pointcloud` 和 `--cad`；批量：`--tasks` 或 `--all`，并指定 `--data-root`、`--pointcloud-kind` | 终端输出 Mean、Max、RMSE、Min、Std；可用 `--save` 保存热力图 PLY；可用 `--excel-output` 保存 Excel | +| `curvature` | 估计点云局部曲率并着色 | `--pointcloud`：PLY 点云；`--radius`：邻域半径；`--max-nn`：最大邻居数 | 终端输出曲率 mean/max；可用 `--save` 保存彩色曲率 PLY | +| `plane` | 使用 RANSAC 拟合平面 | 可选 `--pointcloud`：PLY 点云；不传则生成模拟带噪平面 | 终端输出平面模型、内点比例、法向角度误差、RMSE；可视化拟合结果 | +| `full` | 按配置串联运行加噪/滤波、配准和距离评估 | `--config`：YAML 配置文件；默认使用 `config.yaml` | 返回完整流程结果；终端输出各阶段指标；默认不自动执行 CAD 转 Mesh | + +`contour --excel-output` 生成的 Excel 只包含四列：任务号、最大值、平均值、状态。`--excel-output` 不带值时保存为 `excel/contour.xlsx`；只传文件名时会自动保存到 `excel/` 文件夹。 + +## 6. 快速使用 + +查看所有命令： + +```bash +python main.py --help +``` + +查看某个命令参数： + +```bash +python main.py registration --help +``` + +## 7. 常用命令 + +点云到 CAD 的配准： + +```bash +python main.py translate-align --cad datasets/CAD/M02.stp --pointcloud datasets/shift_test_output/ --output-dir results --no-visualize +``` + +CAD 转 Mesh： + +```bash +python main.py cad2mesh --cad datasets/CAD/11.step --mesh vis/11_cad2mesh.stl --precision 1.0 +``` + +加噪与去噪： + +```bash +python main.py denoise --pointcloud datasets/PointCloud/11.ply --noise-std 4 --no-visualize +``` + +点云配准与 Mesh 距离评估： + +```bash +python main.py registration --pointcloud datasets/Noise_PointCloud/11_noisy.ply --mesh datasets/Mesh/11.stl --no-visualize +``` + +轮廓/平整度/平面度评估（快速查看统计结果）： + +```bash +python main.py contour --pointcloud datasets/PointCloud/11.ply --cad datasets/CAD/11.step --no-visualize +``` + +轮廓/平整度/平面度评估（保存热力图和 Excel）： + +```bash +python main.py contour --cad datasets/CAD/11.step --pointcloud datasets/Noise_PointCloud/11_noisy.ply --save vis/11_contour.ply --excel-output --no-visualize +``` + +`contour` 是轮廓/平整度/平面度评估的统一入口：不加导出参数时适合快速在命令行查看统计结果；添加 `--save` 时会保存距离热力图 PLY；添加 `--excel-output` 时会把统计结果保存到 `excel/contour.xlsx`。也可以写成 `--excel-output 11_contour.xlsx`，结果会保存到 `excel/11_contour.xlsx`。Excel 只包含四列：任务号、最大值、平均值、状态。 + +轮廓/平整度/平面度评估（指定多个数据）： + +```bash +python main.py contour --tasks 11,31,40 --data-root datasets --pointcloud-kind noise --save vis/contour --excel-output selected_contour.xlsx --no-visualize +``` + +轮廓/平整度/平面度评估（全部数据）： + +```bash +python main.py contour --all --data-root datasets --pointcloud-kind noise --save vis/contour --excel-output all_contour.xlsx --no-visualize +``` + +批量模式会从 `datasets/CAD/{任务号}.step` 读取 CAD。`--pointcloud-kind noise` 会读取 `datasets/Noise_PointCloud/{任务号}_noisy.ply`；`--pointcloud-kind clean` 会读取 `datasets/PointCloud/{任务号}.ply`。批量模式下 `--save vis/contour` 会按任务号保存为 `vis/contour/{任务号}_contour.ply`，Excel 每个任务一行。 + +曲率估计： + +```bash +python main.py curvature --pointcloud datasets/PointCloud/11.ply --radius 2 --max-nn 30 --save vis/11_curvature.ply --no-visualize +``` + +平面拟合： + +```bash +python main.py plane --no-visualize +``` + +完整流程： + +```bash +python main.py --config config.yaml full --no-visualize +``` + +`full` 会按配置串联加噪/滤波、配准和距离评估。默认 `cad.enabled: false`，不会自动执行 CAD 转 Mesh。 + +## 8. 打包使用 + +生成独立 Python 包源码： + +```bash +python packaging/build_package.py +``` + +本地开发安装： + +```bash +pip install -e packaging/build +geometry-measure --help +``` + +构建 wheel： + +```bash +pip install build +python packaging/build_package.py +python -m build packaging/build +``` + +打包说明见 [packaging/README.md](packaging/README.md)。 + +## 9. 输入不同数据 + +方式一：命令行覆盖路径。 + +点云配准： + +```bash +python main.py registration \ + --pointcloud datasets/Noise_PointCloud/40_noisy.ply \ + --mesh datasets/Mesh/40.stl \ + --no-visualize +``` + +单个数据快速评估： + +```bash +python main.py contour \ + --pointcloud datasets/PointCloud/40.ply \ + --cad datasets/CAD/40.step \ + --no-visualize +``` + +单个数据保存热力图和 Excel： + +```bash +python main.py contour \ + --cad datasets/CAD/40.step \ + --pointcloud datasets/Noise_PointCloud/40_noisy.ply \ + --save vis/40_contour.ply \ + --excel-output 40_contour.xlsx \ + --no-visualize +``` + +指定多个数据批量评估： + +```bash +python main.py contour \ + --tasks 11,31,40 \ + --data-root datasets \ + --pointcloud-kind noise \ + --save vis/contour \ + --excel-output selected_contour.xlsx \ + --no-visualize +``` + +全部数据批量评估： + +```bash +python main.py contour \ + --all \ + --data-root datasets \ + --pointcloud-kind noise \ + --save vis/contour \ + --excel-output all_contour.xlsx \ + --no-visualize +``` + +方式二：修改 `config.yaml`。 + +```yaml +paths: + cad: datasets/CAD/40.step + mesh: datasets/Mesh/40.stl + pointcloud: datasets/PointCloud/40.ply + noisy_pointcloud: datasets/Noise_PointCloud/40_noisy.ply + output_mesh: vis/40_cad2mesh.stl + curvature_output: vis/40_curvature.ply +``` + +然后运行： + +```bash +python main.py --config config.yaml full --no-visualize +``` + +## 10. 参数配置 + +常用参数都在 `config.yaml`： + +- `noise.std`：高斯噪声标准差。 +- `filters.methods`：启用哪些滤波器。 +- `registration.voxel_size`：配准下采样体素大小。 +- `registration.roll/pitch/yaw`：模拟旋转角度。 +- `registration.trans_range`：模拟平移范围。 +- `evaluation.chunk_size`：点云到 Mesh 距离分块大小。 +- `evaluation.triangle_chunk_size`：距离计算时三角面分块大小，显存/内存不足时调小。 +- `evaluation.cad_mesh_deflection`：STEP CAD 三角化精度，越小越精细但越慢。 +- `visualization.enabled`：是否默认开启可视化。 + +命令行参数优先级高于配置文件。 + +## 11. 命令说明 + +### 11.1 `translate-align` + +- 用途：将 `archive/translate_align_pointcloud_to_cad.py` 接入统一入口，按车头/车尾局部点的质心差估计整体平移，只修正平移，不修正旋转。 +- 默认输入：`--cad datasets/CAD/M02.stp --pointcloud datasets/shift_test_output/`。当 `--pointcloud` 传目录时，会优先读取目录下的 `shifted_whole_pointcloud.ply`。 +- 默认输出目录：`results/`。 +- 常用调参：`--axis` 指定车辆长度方向；`--crop-ratio` 控制头尾截取比例；`--cad-sample-points` 控制从 CAD mesh 采样的点数；`--local-sample-points` 控制头尾局部用于算质心的采样点数；`--scale-pointcloud` 用于 CAD 与点云单位不一致时缩放点云。 +- 输出文件： + +```text +results/aligned_pointcloud_translation_only.ply # 最终配准后的完整点云 +results/T_translation_only.txt # 4x4 平移矩阵，可复用到其它点云或评估流程 +results/cad_sampled.ply # 从 CAD mesh 采样得到的 CAD 点云 +results/cad_front_sample.ply # CAD 车头区域采样点，用于检查质心计算 +results/cad_tail_sample.ply # CAD 车尾区域采样点，用于检查质心计算 +results/source_front_sample.ply # 输入点云车头区域采样点，用于检查质心计算 +results/source_tail_sample.ply # 输入点云车尾区域采样点，用于检查质心计算 +``` + +- 终端输出：前端/后端质心、前端平移量 `t_front`、后端平移量 `t_tail`、二者差值、最终融合平移 `t_final` 和最终矩阵 `T_final`。如果前后端平移差值较大，通常说明点云和 CAD 存在旋转误差、裁剪区域不合适或单位不一致。 +- 示例： + +```bash +python main.py translate-align --cad datasets/CAD/M02.stp --pointcloud datasets/shift_test_output/ --output-dir results --axis x --crop-ratio 0.01 --no-visualize +``` + +也可以直接运行辅助脚本： + +```bash +python archive/translate_align_pointcloud_to_cad.py -c datasets/CAD/M02.stp -p datasets/shift_test_output/ -o results --no_visualize +``` + +### 11.2 `cad2mesh` + +- 输入：STEP 文件。 +- 输出：STL 文件。 +- 依赖：`pythonocc-core`。 +- 典型用途：将 CAD 转成后续可用于 Mesh 距离评估的三角网格。 + +### 11.3 `denoise` + +- 输入：干净点云。 +- 输出：终端指标；当前不会自动保存滤波后的点云。 +- 指标：点数、平均噪声偏移、滤波保留比例、Chamfer Distance。 + +### 11.4 `registration` + +- 输入：点云和参考 Mesh。 +- 输出：配准 fitness、RMSE、变换矩阵、配准后点云到 Mesh 距离统计。 +- 注意：百万级点云可能耗时较长；当前 Open3D 配准部分主要走 CPU。 + +### 11.5 `contour` + +- 用途：轮廓/平整度/平面度评估的统一入口。 +- 输入：单任务使用点云和 STEP CAD；批量任务使用 `--tasks` 或 `--all` 从 `--data-root` 中读取多组 CAD/点云；如果显式传 `--mesh`，会走点云到 Mesh 距离计算。 +- 输出：命令行打印点云到 CAD 三角面的 Mean、Max、RMSE、Min、Std；可通过 `--save` 保存距离热力图 PLY，可通过 `--excel-output` 保存 Excel。 +- 注意：这是全量点到三角面的距离计算，数据越大越慢；STEP 三角化精度由 `--mesh-deflection` 控制。 +- 常用调参：`--mesh-deflection`、`--point-chunk-size`、`--triangle-chunk-size`。 +- 批量参数：`--tasks 11,31,40` 执行指定任务；`--all` 执行 `--data-root` 下全部 CAD；`--pointcloud-kind noise|clean` 选择加噪点云或干净点云。 +- 批量热力图：`--save vis/contour` 会为每个任务生成一个 PLY；也可以用 `--save vis/{task_id}_contour.ply` 自定义文件名模板。 +- Excel 输出：`--excel-output` 不带值时保存为 `excel/contour.xlsx`；只传文件名时自动保存到 `excel/` 文件夹。表格列为任务号、最大值、平均值、状态。 + +### 11.6 `archive/Contour_Flatness.py` + +- 默认批量模式：从 `High_Speed_Data/CAD/{id}.step` 和 `High_Speed_Data/Noise_PointCloud/{id}_noisy.ply` 读取任务，输出 Excel 汇总。 +- 单任务模式：加 `--single` 后使用 `--CAD/-c` 和 `--pointcloud/-p`，可加 `--visualize` 显示距离热力图。 +- 示例： + +```bash +python archive/Contour_Flatness.py --tasks "11,31,40" --data_root High_Speed_Data --excel_output vis/flatness.xlsx +python archive/Contour_Flatness.py --single -c High_Speed_Data/CAD/11.step -p High_Speed_Data/Noise_PointCloud/11_noisy.ply --visualize +``` + +### 11.7 `curvature` + +- 输入：点云。 +- 输出：曲率 mean/max；可通过 `--save` 保存彩色曲率点云。 + +### 11.8 `plane` + +- 输入：可选点云；不传点云时会生成一块带噪平面用于测试。 +- 输出：平面模型、内点比例、法向角度误差、RMSE。 + +## 12. 常见问题 + +`No module named OCC`： + +说明 CAD 依赖没有装好。执行： + +```bash +conda env update -n point-porj -f environment.yml +``` + +`pythonocc-core` 无法通过 pip 安装： + +这是正常的，`pythonocc-core` 应通过 conda 安装。不要把它放进 `requirements.txt`。 + +`pythonocc-core=7.5.1` 和 Python 3.12 冲突： + +新建 Python 3.10 环境，不要在 base 里安装。 + +`registration` 或 `contour` 很慢： + +这是因为示例点云超过百万点，且距离计算要比较大量三角面。可以先换小点云测试，或者调小点数、增大 `registration.voxel_size`、调小 `evaluation.chunk_size`/`triangle_chunk_size` 以降低内存压力。 + +CUDA warning： + +如果 torch 提示 NVIDIA driver 太旧，距离计算会退回 CPU。功能不受影响，但速度会慢。 diff --git a/alg/point_porj/USAGE.md b/alg/point_porj/USAGE.md new file mode 100644 index 0000000..3b002be --- /dev/null +++ b/alg/point_porj/USAGE.md @@ -0,0 +1,737 @@ +# USAGE + +本文档面向后续开发人员，说明本项目的运行环境、数据约定、命令输入输出、常用工作流和排错方法。 + +项目根目录下的 `main.py` 是统一命令行入口。除非特别说明，所有命令都建议在项目根目录执行。 + +```bash +python main.py --help +``` + +当前公开命令： + +```text +translate-align +cad2mesh +denoise +registration +contour +curvature +plane +full +``` + +## 1. 环境准备 + +推荐使用独立 conda 环境。不要直接在 `base` 环境中安装，尤其不要在 Python 3.12 的 base 环境里安装 `pythonocc-core`。 + +```bash +conda env create -f environment.yml +conda activate point-porj +pip install -r requirements.txt +pip install git+https://github.com/AutodeskAILab/occwl +``` + +如果环境已经存在： + +```bash +conda env update -n point-porj -f environment.yml +conda activate point-porj +pip install -r requirements.txt +pip install git+https://github.com/AutodeskAILab/occwl +``` + +验证依赖： + +```bash +python -c "import open3d, trimesh, torch, yaml, OCC.Core.STEPControl; print('env ok')" +``` + +如果 `torch` 提示 CUDA driver 版本过旧，程序会退回 CPU。功能不受影响，但点云到三角面的距离计算会慢。 + +## 2. 数据目录约定 + +默认示例数据放在 `datasets/`： + +```text +datasets/ + CAD/ # STEP CAD，例如 11.step + Mesh/ # STL/OBJ/PLY Mesh，例如 11.stl + PointCloud/ # 原始/干净点云 PLY，例如 11.ply + Noise_PointCloud/ # 加噪或待配准点云 PLY，例如 11_noisy.ply + shift_test_output/ # translate-align 测试输出，默认读取 shifted_whole_pointcloud.ply +``` + +同一编号的数据通常应成组存在： + +```text +datasets/CAD/11.step +datasets/Mesh/11.stl +datasets/PointCloud/11.ply +datasets/Noise_PointCloud/11_noisy.ply +``` + +`translate-align` 默认使用下面这组输入： + +```text +datasets/CAD/M02.stp +datasets/shift_test_output/shifted_whole_pointcloud.ply +``` + +数据要求： + +- CAD 使用 STEP，扩展名通常是 `.step` 或 `.stp`。 +- Mesh 使用 `trimesh` 可读取的格式，例如 `.stl`、`.obj`、`.ply`。 +- PointCloud 推荐 PLY，必须包含三维点坐标。 +- CAD、Mesh、PointCloud 必须处于同一坐标系和同一单位。 +- 示例数据按毫米量级处理。 +- `contour` 假设点云已与 CAD/Mesh 对齐。 +- `registration` 会模拟旋转后做配准。 + +## 3. 输出目录约定 + +建议保留以下输出目录： + +```text +vis/ # 日志、热力图点云、曲率点云、中间结果 +excel/ # contour 输出的 Excel 表 +results/ # translate-align 默认输出目录 +``` + +可以手动创建： + +```bash +mkdir -p vis excel results +``` + +常见输出： + +```text +*.stl # CAD 转 Mesh 结果 +*.ply # 彩色点云、热力图点云 +*.xlsx # 轮廓/平整度/平面度 Excel 汇总 +*.log # shell 重定向保存的终端输出 +results/T_translation_only.txt +results/aligned_pointcloud_translation_only.ply +``` + +## 4. 方法输入输出总览 + +| 方法 | 用途 | 输入 | 输出 | +|------|------|------|------| +| `translate-align` | 点云到标准CAD的配准 | 默认 `--cad datasets/CAD/M02.stp --pointcloud datasets/shift_test_output/`；可调头尾截取和采样参数 | 默认写入 `results/`，输出对齐后点云、4x4 平移矩阵和头尾采样检查文件 | +| `cad2mesh` | STEP CAD 转 STL Mesh | `--cad` STEP 文件 | `--mesh` STL 文件 | +| `denoise` | 给干净点云加噪，并运行滤波方法 | `--pointcloud` 干净 PLY；`--noise-std` 噪声标准差 | 终端输出点数、平均噪声偏移、滤波保留比例、Chamfer Distance | +| `registration` | 点云到 Mesh 配准与距离评估 | `--pointcloud` 待配准 PLY；`--mesh` 参考 Mesh | 终端输出 fitness、RMSE、变换矩阵、配准后距离统计 | +| `contour` | 轮廓/平整度/平面度评估 | 单任务：`--pointcloud` 和 `--cad`；批量：`--tasks` 或 `--all`，并指定 `--data-root`、`--pointcloud-kind` | 终端输出 Mean、Max、RMSE、Min、Std；可保存热力图 PLY 和 Excel | +| `curvature` | 点云曲率估计 | `--pointcloud` PLY；`--radius` 邻域半径；`--max-nn` 最大邻居数 | 终端输出曲率 mean/max；可保存彩色曲率 PLY | +| `plane` | RANSAC 平面拟合 | 可选 `--pointcloud` PLY；不传则生成模拟带噪平面 | 终端输出平面模型、内点比例、法向角度误差、RMSE | +| `full` | 配置驱动的完整流程 | `--config` YAML 配置；默认 `config.yaml` | 串联输出加噪/滤波、配准和距离评估结果 | + +## 5. 命令详解 + +### 5.1 点云和标准CAD配准：`translate-align` + +功能：调用 `archive/translate_align_pointcloud_to_cad.py`，从 CAD 和待对齐点云的车头/车尾区域各采样少量点，计算前后端质心差，并融合成一个整体平移矩阵。该方法只修正平移，不修正旋转。 + +输入： + +- `--cad`：参考 STEP CAD，默认 `datasets/CAD/M02.stp`。 +- `--pointcloud`：待对齐 PLY 点云或点云目录，默认 `datasets/shift_test_output/`。传目录时会优先读取 `shifted_whole_pointcloud.ply`。 +- `--output-dir`：输出目录，默认 `results`。 +- `--axis`：车辆长度方向，默认 `x`。 +- `--crop-ratio`：车头/车尾各截取整体长度的比例，默认 `0.01`。 +- `--mesh-deflection`：CAD 三角化精度。 +- `--cad-sample-points`：从 CAD mesh 采样的点数，默认 `30000`。 +- `--local-sample-points`：车头/车尾局部用于计算质心的采样点数，默认 `1000`。 +- `--scale-pointcloud`：点云缩放系数。例如点云是米、CAD 是毫米时可设为 `1000`。 +- `--no-visualize`：禁用 Open3D 窗口。 + +输出： + +```text +results/aligned_pointcloud_translation_only.ply # 最终配准后的完整点云 +results/T_translation_only.txt # 4x4 平移矩阵，可复用到其它点云或评估流程 +results/cad_sampled.ply # 从 CAD mesh 采样得到的 CAD 点云 +results/cad_front_sample.ply # CAD 车头区域采样点，用于检查质心计算 +results/cad_tail_sample.ply # CAD 车尾区域采样点，用于检查质心计算 +results/source_front_sample.ply # 输入点云车头区域采样点，用于检查质心计算 +results/source_tail_sample.ply # 输入点云车尾区域采样点，用于检查质心计算 +``` + +示例： + +```bash +python main.py translate-align \ + --cad datasets/CAD/M02.stp \ + --pointcloud datasets/shift_test_output/ \ + --output-dir results \ + --axis x \ + --crop-ratio 0.01 \ + --cad-sample-points 30000 \ + --local-sample-points 1000 \ + --no-visualize +``` + +### 5.2 CAD 转 Mesh：`cad2mesh` + +功能：读取 STEP CAD，离散成 STL 三角网格。 + +输入： + +- `--cad`：输入 STEP 文件。 +- `--mesh`：输出 STL 文件。 +- `--precision`：离散精度，单位为 mm。值越小通常越精细，生成文件也可能越大。 + +输出： + +- 一个 STL 文件。 +- 终端输出 STEP 读取、网格化和 STL 写入进度。 + +示例： + +```bash +python main.py cad2mesh \ + --cad datasets/CAD/11.step \ + --mesh vis/11_cad2mesh.stl \ + --precision 1.0 +``` + +### 5.3 加噪与滤波：`denoise` + +功能：读取干净点云，加高斯噪声，并按配置运行滤波方法。 + +输入： + +- `--pointcloud`：干净 PLY 点云。 +- `--noise-std`：高斯噪声标准差。 +- `--no-visualize`：禁用 Open3D 窗口。 + +输出： + +- 终端输出干净点数、加噪点数、平均噪声偏移。 +- 对每个滤波方法输出保留比例和 Chamfer Distance。 +- 当前主流程不会自动保存滤波后的点云文件。 + +示例： + +```bash +python main.py denoise \ + --pointcloud datasets/PointCloud/11.ply \ + --noise-std 4 \ + --no-visualize +``` + +### 5.4 配准：`registration` + +功能：对输入点云模拟旋转，然后通过 RANSAC + ICP 配准到 Mesh，并计算配准后距离。 + +输入： + +- `--pointcloud`：待配准 PLY 点云，通常使用 `datasets/Noise_PointCloud/*_noisy.ply`。 +- `--mesh`：参考 STL/OBJ/PLY Mesh。 +- `--voxel-size`：配准下采样体素大小。 +- `--roll`、`--pitch`、`--yaw`：模拟旋转角度，单位为 degree。 +- `--trans-range`：随机平移范围。 +- `--no-visualize`：禁用 Open3D 窗口。 + +输出： + +- `Fitness` +- `RMSE` +- 4x4 transformation matrix +- 配准后点云到 Mesh 的距离统计：Mean、Max、RMSE、Min、Std + +示例： + +```bash +python main.py registration \ + --pointcloud datasets/Noise_PointCloud/11_noisy.ply \ + --mesh datasets/Mesh/11.stl \ + --voxel-size 10 \ + --roll 10 \ + --pitch 10 \ + --yaw 10 \ + --no-visualize +``` + +### 5.5 轮廓/平整度/平面度评估：`contour` + +功能：统一的轮廓/平整度/平面度评估入口。默认读取 STEP CAD，三角化后计算点云到 CAD 三角面的最近距离。 + +输入： + +- `--pointcloud`：待评估 PLY 点云。 +- `--cad`：参考 STEP CAD。 +- `--mesh`：可传参考 STL/OBJ/PLY Mesh。传了 `--mesh` 时会走点云到 Mesh 距离计算。 +- `--tasks`：批量执行指定任务编号，用英文逗号分隔，例如 `11,31,40`。 +- `--all`：批量执行 `--data-root` 下 `CAD/` 中的全部 STEP 数据。 +- `--data-root`：批量数据根目录，默认 `datasets`。 +- `--pointcloud-kind`：批量点云来源，`noise` 读取 `Noise_PointCloud/{id}_noisy.ply`，`clean` 读取 `PointCloud/{id}.ply`。 +- `--mesh-deflection`：STEP 三角化精度，越小越精细，但三角面更多、计算更慢。 +- `--point-chunk-size`：点云分块大小。 +- `--triangle-chunk-size`：三角面分块大小。 +- `--save`：保存距离热力图 PLY。 +- `--excel-output`：保存 Excel。 +- `--no-visualize`：禁用 Open3D 窗口。 + +输出： + +- 终端距离统计：Mean、Max、RMSE、Min、Std。 +- 如果传 `--save`：保存彩色热力图 PLY。 +- 如果传 `--excel-output`：保存 Excel。 + +Excel 格式： + +```text +任务号 | 最大值 | 平均值 | 状态 +``` + +任务号从 CAD 文件名提取。例如： + +```text +datasets/CAD/11.step -> 11 +``` + +Excel 路径规则： + +- `--excel-output` 不带值：保存到 `excel/contour.xlsx` +- `--excel-output 11_contour.xlsx`：保存到 `excel/11_contour.xlsx` +- `--excel-output reports/11.xlsx`：保存到 `reports/11.xlsx` + +批量模式路径规则： + +- CAD：`{data_root}/CAD/{任务号}.step` +- 加噪点云：`{data_root}/Noise_PointCloud/{任务号}_noisy.ply` +- 干净点云：`{data_root}/PointCloud/{任务号}.ply` +- 如果 `--save` 是目录，例如 `--save vis/contour`，每个任务会保存为 `vis/contour/{任务号}_contour.ply` +- 如果 `--save` 是带 `{task_id}` 的文件模板，例如 `--save vis/{task_id}_contour.ply`，会替换任务号 +- 如果 `--save` 是普通 PLY 文件，例如 `--save vis/contour.ply`，批量模式会保存为 `vis/contour_{任务号}.ply` + +快速查看统计结果： + +```bash +python main.py contour \ + --pointcloud datasets/PointCloud/11.ply \ + --cad datasets/CAD/11.step \ + --no-visualize +``` + +保存热力图和 Excel： + +```bash +python main.py contour \ + --cad datasets/CAD/11.step \ + --pointcloud datasets/Noise_PointCloud/11_noisy.ply \ + --save vis/11_contour.ply \ + --excel-output 11_contour.xlsx \ + --no-visualize +``` + +指定多个数据： + +```bash +python main.py contour \ + --tasks 11,31,40 \ + --data-root datasets \ + --pointcloud-kind noise \ + --save vis/contour \ + --excel-output selected_contour.xlsx \ + --no-visualize +``` + +执行全部数据： + +```bash +python main.py contour \ + --all \ + --data-root datasets \ + --pointcloud-kind noise \ + --save vis/contour \ + --excel-output all_contour.xlsx \ + --no-visualize +``` + +显存或内存不足时调小分块： + +```bash +python main.py contour \ + --cad datasets/CAD/11.step \ + --pointcloud datasets/Noise_PointCloud/11_noisy.ply \ + --mesh-deflection 0.5 \ + --point-chunk-size 512 \ + --triangle-chunk-size 1024 \ + --excel-output \ + --no-visualize +``` + +### 5.6 曲率估计：`curvature` + +功能：计算点云局部曲率，并按曲率着色。 + +输入： + +- `--pointcloud`：PLY 点云。 +- `--radius`：邻域搜索半径。 +- `--max-nn`：最大邻居数。 +- `--color-scale`：颜色缩放系数。 +- `--save`：保存彩色曲率点云。 +- `--no-visualize`：禁用 Open3D 窗口。 + +输出： + +- 终端输出曲率 mean 和 max。 +- 如果传 `--save`：保存彩色曲率 PLY。 + +示例： + +```bash +python main.py curvature \ + --pointcloud datasets/PointCloud/11.ply \ + --radius 2 \ + --max-nn 30 \ + --save vis/11_curvature.ply \ + --no-visualize +``` + +### 5.7 平面拟合：`plane` + +功能：使用 RANSAC 拟合平面。 + +输入： + +- `--pointcloud`：可选 PLY 点云。不传时会根据 `config.yaml` 生成模拟带噪平面。 +- `--threshold-mm`：RANSAC 距离阈值，单位 mm。 +- `--max-iterations`：RANSAC 最大迭代次数。 +- `--no-visualize`：禁用 Open3D 窗口。 + +输出： + +- 平面模型参数。 +- 内点比例。 +- 法向角度误差。 +- 平面 RMSE。 + +示例： + +```bash +python main.py plane --no-visualize +``` + +带输入点云： + +```bash +python main.py plane \ + --pointcloud datasets/PointCloud/11.ply \ + --threshold-mm 0.5 \ + --max-iterations 2000 \ + --no-visualize +``` + +### 5.8 完整流程：`full` + +功能：按 `config.yaml` 串联运行流程。 + +当前默认行为： + +- 如果 `cad.enabled: false`，不会自动执行 CAD 转 Mesh。 +- 如果 `noise.enabled: true`，会先运行 `denoise`，再对加噪点云做配准。 +- 最后运行 `contour`。 + +输入： + +- `--config`：配置文件路径。不传时使用内置默认配置。 +- `--no-visualize`：禁用 Open3D 窗口。 + +输出： + +- 每个阶段的终端指标。 +- Python 调用时会返回一个结果字典。 + +示例： + +```bash +python main.py --config config.yaml full --no-visualize +``` + +## 6. 配置文件 + +默认配置在 `config.yaml`。 + +常用字段： + +```yaml +paths: + cad: datasets/CAD/11.step + mesh: datasets/Mesh/11.stl + pointcloud: datasets/PointCloud/11.ply + noisy_pointcloud: datasets/Noise_PointCloud/11_noisy.ply + output_mesh: datasets/Mesh/11.stl + curvature_output: + +cad: + enabled: false + precision: 1.0 + +noise: + enabled: true + std: 4.0 + +registration: + voxel_size: 10.0 + roll: 10.0 + pitch: 10.0 + yaw: 10.0 + trans_range: 0.0 + +evaluation: + chunk_size: 2048 + triangle_chunk_size: 4096 + cad_mesh_deflection: 0.1 + +visualization: + enabled: true +``` + +命令行参数优先级高于配置文件。例如： + +```bash +python main.py --config config.yaml contour \ + --cad datasets/CAD/40.step \ + --pointcloud datasets/Noise_PointCloud/40_noisy.ply \ + --excel-output 40_contour.xlsx \ + --no-visualize +``` + +## 7. Python API 用法 + +除了命令行，也可以在 Python 中调用： + +```python +from pipeline import MeasurementPipeline + +pipeline = MeasurementPipeline(config_path="config.yaml") + +result = pipeline.run_contour( + cad_path="datasets/CAD/11.step", + pointcloud_path="datasets/Noise_PointCloud/11_noisy.ply", + save_path="vis/11_contour.ply", + excel_output_path="11_contour.xlsx", + no_visualize=True, +) + +print(result["distance_stats"]) +``` + +常用返回字段： + +- `run_contour`：`pointcloud`、`distances`、`distance_stats`、`cad_vertices`、`cad_faces`、`excel_output` +- `run_translate_align_pointcloud_to_cad`：`translation`、`T_final`、`aligned_path`、`matrix_path` +- `run_registration`：`source`、`registration`、`distance_stats` +- `run_curvature`：`pointcloud`、`curvatures` +- `run_plane`：`plane_result` + +## 8. 打包使用 + +生成可安装包源码： + +```bash +python packaging/build_package.py +``` + +本地开发安装： + +```bash +pip install -e packaging/build +geometry-measure --help +``` + +安装后命令与 `main.py` 对应： + +```bash +geometry-measure contour \ + --cad datasets/CAD/11.step \ + --pointcloud datasets/Noise_PointCloud/11_noisy.ply \ + --save vis/11_contour.ply \ + --excel-output 11_contour.xlsx \ + --no-visualize +``` + +每次修改根目录源码后，如果需要同步到打包目录，重新运行： + +```bash +python packaging/build_package.py +``` + +## 9. 辅助脚本说明 + +辅助脚本放在 `archive/`。常用流程推荐优先使用 `main.py` 统一入口。 + +`archive/Contour_Flatness.py` 可用于批量轮廓/平整度/平面度评估： + +```bash +python archive/Contour_Flatness.py \ + --tasks "11,31,40" \ + --data_root High_Speed_Data \ + --pointcloud_kind noise \ + --excel_output vis/flatness.xlsx +``` + +单任务模式： + +```bash +python archive/Contour_Flatness.py \ + --single \ + -c High_Speed_Data/CAD/11.step \ + -p High_Speed_Data/Noise_PointCloud/11_noisy.ply \ + --visualize +``` + +`archive/translate_align_pointcloud_to_cad.py` 已接入统一入口，推荐用 `main.py translate-align` 调用；需要单独运行脚本时也可以直接执行： + +```bash +python archive/translate_align_pointcloud_to_cad.py \ + -c datasets/CAD/M02.stp \ + -p datasets/shift_test_output/ \ + -o results \ + --axis x \ + --crop_ratio 0.01 \ + --no_visualize +``` + +## 10. 常见开发任务 + +### 添加一个新数据编号 + +例如添加编号 `45`： + +```text +datasets/CAD/45.step +datasets/Mesh/45.stl +datasets/PointCloud/45.ply +datasets/Noise_PointCloud/45_noisy.ply +``` + +运行评估： + +```bash +python main.py contour \ + --tasks 45 \ + --data-root datasets \ + --pointcloud-kind noise \ + --save vis/contour \ + --excel-output 45_contour.xlsx \ + --no-visualize +``` + +### 保存命令日志 + +```bash +python main.py contour \ + --cad datasets/CAD/11.step \ + --pointcloud datasets/Noise_PointCloud/11_noisy.ply \ + --excel-output 11_contour.xlsx \ + --no-visualize > vis/11_contour.log 2>&1 +``` + +### 内存不足或显存不足 + +优先调小： + +```bash +--point-chunk-size 512 +--triangle-chunk-size 1024 +``` + +如果 CAD 三角面过多，也可以增大： + +```bash +--mesh-deflection 0.5 +``` + +注意：`mesh_deflection` 越大，CAD 三角化越粗，速度更快但几何近似更粗。 + +## 11. 排错 + +### `No module named OCC` + +说明 CAD 依赖未安装或当前 Python 环境不对。 + +```bash +conda activate point-porj +conda env update -n point-porj -f environment.yml +``` + +### `pythonocc-core` 无法通过 pip 安装 + +这是正常情况。`pythonocc-core` 应通过 conda 安装，不要放进纯 pip 安装流程里。 + +### 找不到文件 + +检查当前工作目录是否为项目根目录。相对路径都按当前运行目录解析。 + +推荐： + +```bash +pwd +python main.py --help +``` + +Windows PowerShell 下可以使用： + +```powershell +Get-Location +python main.py --help +``` + +### Open3D 可视化窗口影响自动化运行 + +加 `--no-visualize`。 + +```bash +python main.py contour --cad datasets/CAD/11.step --pointcloud datasets/PointCloud/11.ply --no-visualize +``` + +### 结果距离明显不合理 + +重点检查： + +- CAD 和点云是否同单位。 +- CAD 和点云是否已经对齐。 +- 是否把干净点云和加噪点云用错目录。 +- `mesh_deflection` 是否过大导致 CAD 近似过粗。 + +### Excel 没有生成 + +`contour` 只有传了 `--excel-output` 才会生成 Excel： + +```bash +python main.py contour \ + --cad datasets/CAD/11.step \ + --pointcloud datasets/Noise_PointCloud/11_noisy.ply \ + --excel-output \ + --no-visualize +``` + +默认输出： + +```text +excel/contour.xlsx +``` + +### `registration` 或 `contour` 很慢 + +原因通常是点云超过百万点，且距离计算要遍历大量三角面。可以： + +- 调小点云规模。 +- 调小 `--point-chunk-size` 和 `--triangle-chunk-size` 以降低内存压力。 +- 增大 `--mesh-deflection` 减少 CAD 三角面数量。 +- 对配准增加 `--voxel-size` 做更强下采样。 + +## 12. 开发约定 + +- 面向用户的新功能优先接入 `main.py` 和 `MeasurementPipeline`。 +- `contour` 是轮廓/平整度/平面度评估的唯一公开入口，不再新增其它容易混淆的同类命令。 +- 新增输出文件时优先放入已有输出目录：通用可视化和日志放 `vis/`，表格放 `excel/`，对齐结果放 `results/`。 +- 修改根目录源码后，如果需要打包版本同步，运行 `python packaging/build_package.py`。 +- 不要在文档中引入其它同类距离评估命令名，避免和 `contour` 产生歧义。 diff --git a/alg/point_porj/archive/CAD2Mesh.py b/alg/point_porj/archive/CAD2Mesh.py new file mode 100644 index 0000000..b1740cc --- /dev/null +++ b/alg/point_porj/archive/CAD2Mesh.py @@ -0,0 +1,55 @@ +from OCC.Core.STEPControl import STEPControl_Reader +from OCC.Core.IFSelect import IFSelect_RetDone +from OCC.Core.BRepMesh import BRepMesh_IncrementalMesh +from OCC.Core.StlAPI import StlAPI_Writer +import time +import argparse # 命令行参数解析 + +def step2lightweight_stl(mesh_precision, input_step, output_stl): + """STEP转轻量化STL，纯后台处理，无显示，体积暴减""" + start_total = time.time() + # 1. 读取STEP文件 + print(f"【1/3】读取STEP文件：{input_step}") + step_reader = STEPControl_Reader() + if step_reader.ReadFile(input_step) != IFSelect_RetDone: + raise Exception("STEP文件读取失败！检查路径/文件完整性") + step_reader.TransferRoots() + shape = step_reader.OneShape() + print(f"【1/3】读取完成，耗时：{time.time()-start_total:.2f}秒") + + # 2. 核心：网格离散化 + print(f"【2/3】网格离散化（精度{mesh_precision}mm），开始轻量化...") + mesh = BRepMesh_IncrementalMesh(shape, mesh_precision, False) + mesh.Perform() + if not mesh.IsDone(): + raise Exception("网格离散化失败！") + print(f"【2/3】轻量化完成，耗时：{time.time()-start_total:.2f}秒") + + # 3. 保存为STL + print(f"【3/3】保存轻量化STL：{output_stl}") + stl_writer = StlAPI_Writer() + stl_writer.Write(shape, output_stl) + + total_time = time.time() - start_total + print(f"✅ 转换完成！总耗时：{total_time:.2f}秒") + print(f"💡 提示：STL文件可直接用FreeCAD/MeshLab/3D Viewer打开") + +if __name__ == "__main__": + # ========== 命令行参数定义 ========== + parser = argparse.ArgumentParser(description="STEP 转 轻量化 STL (命令行args版)") + parser.add_argument("--precision", "-p", type=float, default=1.0, help="网格精度 MESH_PRECISION (mm)，越大文件越小") + parser.add_argument("--input", "-i", type=str, default="High_Speed_Data/CAD/11.step", help="输入STEP文件路径 INPUT_STEP_PATH") + parser.add_argument("--output", "-o", type=str, default="High_Speed_Data/Mesh/11.stl", help="输出STL文件路径 OUTPUT_STL_PATH") + + # 解析参数 + args = parser.parse_args() + + # 调用转换函数 + try: + step2lightweight_stl( + mesh_precision=args.precision, + input_step=args.input, + output_stl=args.output + ) + except Exception as e: + print(f"❌ 处理失败：{str(e)}") \ No newline at end of file diff --git a/alg/point_porj/archive/Contour_Flatness.py b/alg/point_porj/archive/Contour_Flatness.py new file mode 100644 index 0000000..e24dba4 --- /dev/null +++ b/alg/point_porj/archive/Contour_Flatness.py @@ -0,0 +1,799 @@ +import argparse +from pathlib import Path +from zipfile import ZipFile, ZIP_DEFLATED +from xml.sax.saxutils import escape +import numpy as np +import torch +import trimesh +import matplotlib.pyplot as plt + +from OCC.Core.STEPControl import STEPControl_Reader +from OCC.Core.IFSelect import IFSelect_RetDone +from OCC.Core.BRepMesh import BRepMesh_IncrementalMesh + +from OCC.Core.TopExp import TopExp_Explorer +from OCC.Core.TopAbs import TopAbs_FACE +from OCC.Core.BRep import BRep_Tool + +# 兼容不同 pythonocc 版本 +try: + from OCC.Core.TopoDS import topods + USE_TOPODS = "topods" +except ImportError: + from OCC.Core.TopoDS import TopoDS + USE_TOPODS = "TopoDS" + +try: + import open3d as o3d +except ModuleNotFoundError: + o3d = None + + +# ============================================================ +# 设备与精度 +# ============================================================ + +device = torch.device("cuda" if torch.cuda.is_available() else "cpu") +dtype = torch.float64 + +DEFAULT_TASK_IDS = [ + 11, 13, 17, 20, 25, 26, 27, 31, 34, + 35, 36, 37, 38, 39, 40, 41, 42, 45 +] + +task_distance_data = {} + + +# ============================================================ +# STEP 读取 +# ============================================================ + +def read_step_file(step_path): + reader = STEPControl_Reader() + status = reader.ReadFile(step_path) + + if status != IFSelect_RetDone: + raise RuntimeError(f"STEP 文件读取失败: {step_path}") + + reader.TransferRoots() + shape = reader.OneShape() + + print("STEP 文件读取完成") + + return shape + + +# ============================================================ +# OCC Shape -> numpy mesh +# ============================================================ + +def occ_shape_to_numpy_mesh(shape): + """ + 从 OCC Shape 中提取三角网格。 + + 返回: + vertices_np: [N, 3] + faces_np: [M, 3] + """ + + vertices = [] + faces = [] + + explorer = TopExp_Explorer(shape, TopAbs_FACE) + + face_count = 0 + triangle_count = 0 + + while explorer.More(): + current_shape = explorer.Current() + + if USE_TOPODS == "topods": + face = topods.Face(current_shape) + else: + face = TopoDS.Face(current_shape) + + location = face.Location() + + triangulation = BRep_Tool.Triangulation(face, location) + + if triangulation is None: + explorer.Next() + continue + + trsf = location.Transformation() + + base_index = len(vertices) + + # 读取当前 Face 的顶点 + for i in range(1, triangulation.NbNodes() + 1): + pnt = triangulation.Node(i) + + # 应用局部变换 + pnt.Transform(trsf) + + vertices.append([ + pnt.X(), + pnt.Y(), + pnt.Z() + ]) + + # 读取当前 Face 的三角形 + for i in range(1, triangulation.NbTriangles() + 1): + tri = triangulation.Triangle(i) + + n1, n2, n3 = tri.Get() + + # OCC 索引从 1 开始，numpy / torch 索引从 0 开始 + faces.append([ + base_index + n1 - 1, + base_index + n2 - 1, + base_index + n3 - 1 + ]) + + triangle_count += 1 + + face_count += 1 + explorer.Next() + + vertices_np = np.asarray(vertices, dtype=np.float64) + faces_np = np.asarray(faces, dtype=np.int64) + + print("CAD 网格提取完成") + print(f"CAD 面数量: {face_count}") + print(f"网格顶点: {vertices_np.shape}") + print(f"网格三角面: {faces_np.shape}") + + if vertices_np.shape[0] == 0 or faces_np.shape[0] == 0: + raise RuntimeError( + "CAD 三角网格提取失败。可以尝试减小 MESH_DEFLECTION，或者检查 STEP 文件是否正常。" + ) + + return vertices_np, faces_np + + +# ============================================================ +# 读取点云 +# ============================================================ + +def read_point_cloud(pointcloud_path): + if o3d is not None: + pcd = o3d.io.read_point_cloud(pointcloud_path) + points_np = np.asarray(pcd.points, dtype=np.float64) + else: + pcd = trimesh.load(pointcloud_path) + points_np = np.asarray(pcd.vertices, dtype=np.float64) + + if points_np.shape[0] == 0: + raise RuntimeError(f"点云读取失败或点数为 0: {pointcloud_path}") + + print("点云读取完成") + print(f"点云点数: {points_np.shape}") + + return points_np + + +# ============================================================ +# 点到三角形距离 +# ============================================================ + +def point_to_triangle_distance_squared(points, triangles): + """ + points: + [P, 3] + triangles: + [T, 3, 3] + + 返回: + dist2: [P, T] + """ + + p = points[:, None, :] + + a = triangles[None, :, 0, :] + b = triangles[None, :, 1, :] + c = triangles[None, :, 2, :] + + ab = b - a + ac = c - a + ap = p - a + + d1 = (ab * ap).sum(dim=-1) + d2 = (ac * ap).sum(dim=-1) + + inf = torch.full_like(d1, torch.inf) + + # 区域 A + mask = (d1 <= 0.0) & (d2 <= 0.0) + dist2 = torch.where(mask, (ap * ap).sum(dim=-1), inf) + + # 区域 B + bp = p - b + d3 = (ab * bp).sum(dim=-1) + d4 = (ac * bp).sum(dim=-1) + + mask = (d3 >= 0.0) & (d4 <= d3) + dist2 = torch.minimum( + dist2, + torch.where(mask, (bp * bp).sum(dim=-1), inf) + ) + + # 边 AB + vc = d1 * d4 - d3 * d2 + mask = (vc <= 0.0) & (d1 >= 0.0) & (d3 <= 0.0) + + v = d1 / (d1 - d3).clamp_min(torch.finfo(points.dtype).eps) + closest = a + v[..., None] * ab + edge_dist2 = ((p - closest) ** 2).sum(dim=-1) + + dist2 = torch.minimum( + dist2, + torch.where(mask, edge_dist2, inf) + ) + + # 区域 C + cp = p - c + d5 = (ab * cp).sum(dim=-1) + d6 = (ac * cp).sum(dim=-1) + + mask = (d6 >= 0.0) & (d5 <= d6) + dist2 = torch.minimum( + dist2, + torch.where(mask, (cp * cp).sum(dim=-1), inf) + ) + + # 边 AC + vb = d5 * d2 - d1 * d6 + mask = (vb <= 0.0) & (d2 >= 0.0) & (d6 <= 0.0) + + w = d2 / (d2 - d6).clamp_min(torch.finfo(points.dtype).eps) + closest = a + w[..., None] * ac + edge_dist2 = ((p - closest) ** 2).sum(dim=-1) + + dist2 = torch.minimum( + dist2, + torch.where(mask, edge_dist2, inf) + ) + + # 边 BC + va = d3 * d6 - d5 * d4 + mask = (va <= 0.0) & ((d4 - d3) >= 0.0) & ((d5 - d6) >= 0.0) + + w = (d4 - d3) / ((d4 - d3) + (d5 - d6)).clamp_min( + torch.finfo(points.dtype).eps + ) + closest = b + w[..., None] * (c - b) + edge_dist2 = ((p - closest) ** 2).sum(dim=-1) + + dist2 = torch.minimum( + dist2, + torch.where(mask, edge_dist2, inf) + ) + + # 三角形内部区域 + denom = (va + vb + vc).clamp_min(torch.finfo(points.dtype).eps) + + v = vb / denom + w = vc / denom + + closest = a + ab * v[..., None] + ac * w[..., None] + face_dist2 = ((p - closest) ** 2).sum(dim=-1) + + dist2 = torch.minimum(dist2, face_dist2) + + return dist2 + + +# ============================================================ +# 分块计算最近距离 +# ============================================================ + +def nearest_mesh_distances( + points, + triangles, + point_chunk_size=2048, + triangle_chunk_size=4096 +): + """ + 为了避免显存爆炸，点和三角面都分块计算。 + + points: + [N, 3] + triangles: + [M, 3, 3] + + 返回: + distances: [N] + """ + + all_distances = [] + + num_points = points.shape[0] + num_triangles = triangles.shape[0] + + for p_start in range(0, num_points, point_chunk_size): + p_end = min(p_start + point_chunk_size, num_points) + + point_chunk = points[p_start:p_end] + + min_dist2 = torch.full( + (point_chunk.shape[0],), + torch.inf, + dtype=points.dtype, + device=points.device + ) + + for t_start in range(0, num_triangles, triangle_chunk_size): + t_end = min(t_start + triangle_chunk_size, num_triangles) + + triangle_chunk = triangles[t_start:t_end] + + dist2 = point_to_triangle_distance_squared( + point_chunk, + triangle_chunk + ) + + min_dist2 = torch.minimum( + min_dist2, + dist2.min(dim=1).values + ) + + all_distances.append(torch.sqrt(min_dist2)) + + # print(f"processed points: {p_end}/{num_points}") + + distances = torch.cat(all_distances, dim=0) + + return distances + + +# ============================================================ +# 热力图显示 +# ============================================================ + +def visualize_point_cloud_heatmap(points_np, distances_np): + if o3d is None: + print("未安装 open3d，跳过可视化。") + return + + d_min = distances_np.min() + d_max = distances_np.max() + + dist_norm = (distances_np - d_min) / (d_max - d_min + 1e-12) + + colors = plt.cm.turbo(dist_norm)[:, :3] + + pcd_show = o3d.geometry.PointCloud() + pcd_show.points = o3d.utility.Vector3dVector(points_np) + pcd_show.colors = o3d.utility.Vector3dVector(colors) + + print("色阶规则：深蓝/紫色 = 最近，红色 = 最远") + + o3d.visualization.draw_geometries( + [pcd_show], + window_name="点云到 CAD 距离热力图" + ) + + +def visualize_task_result(task_results, task_id): + """ + task_results: run_batch_tasks 返回的字典，key 为任务号。 + task_id: 需要可视化的任务号。 + """ + + result = task_results.get(int(task_id)) + + if result is None: + raise KeyError(f"未找到任务 {task_id} 的计算结果。") + + visualize_point_cloud_heatmap( + result["points_np"], + result["distances_np"] + ) + + +def _xlsx_cell_ref(row_index, col_index): + letters = "" + col = col_index + + while col: + col, remainder = divmod(col - 1, 26) + letters = chr(65 + remainder) + letters + + return f"{letters}{row_index}" + + +def _xlsx_cell_xml(row_index, col_index, value): + cell_ref = _xlsx_cell_ref(row_index, col_index) + + if isinstance(value, (int, float, np.integer, np.floating)) and not isinstance(value, bool): + return f'{value}' + + text = escape("" if value is None else str(value)) + return f'{text}' + + +def write_flatness_excel(rows, output_path, sheet_name="平整度"): + """ + 使用 Python 标准库生成 .xlsx，避免额外依赖。 + """ + + output_path = Path(output_path) + output_path.parent.mkdir(parents=True, exist_ok=True) + + headers = ["任务号", "平均距离(mm)", "最大距离(mm)", "状态", "说明"] + table = [headers] + + for row in rows: + table.append([ + row.get("task_id"), + row.get("mean_dist"), + row.get("max_dist"), + row.get("status", ""), + row.get("message", "") + ]) + + sheet_rows = [] + for row_index, row_values in enumerate(table, start=1): + cells = [ + _xlsx_cell_xml(row_index, col_index, value) + for col_index, value in enumerate(row_values, start=1) + ] + sheet_rows.append(f'{"".join(cells)}') + + safe_sheet_name = escape(sheet_name[:31]) + + workbook_xml = ( + '' + '' + f'' + '' + ) + + worksheet_xml = ( + '' + '' + '' + f'{"".join(sheet_rows)}' + '' + '' + ) + + content_types_xml = ( + '' + '' + '' + '' + '' + '' + '' + ) + + root_rels_xml = ( + '' + '' + '' + '' + ) + + workbook_rels_xml = ( + '' + '' + '' + '' + ) + + with ZipFile(output_path, "w", ZIP_DEFLATED) as xlsx: + xlsx.writestr("[Content_Types].xml", content_types_xml) + xlsx.writestr("_rels/.rels", root_rels_xml) + xlsx.writestr("xl/workbook.xml", workbook_xml) + xlsx.writestr("xl/_rels/workbook.xml.rels", workbook_rels_xml) + xlsx.writestr("xl/worksheets/sheet1.xml", worksheet_xml) + + return output_path + + +# ============================================================ +# 主函数 +# ============================================================ + +def compute_point_CAD_distance( + CAD_path, + pointcloud_path, + mesh_deflection=0.1, + point_chunk_size=2048, + triangle_chunk_size=4096, + visualize=False +): + print("计算设备:", device) + print("数据精度:", dtype) + + # STEP -> OCC Shape + shape = read_step_file(CAD_path) + + # OCC Shape 三角化 + print("正在对 CAD 模型进行三角化...") + mesh = BRepMesh_IncrementalMesh(shape, mesh_deflection) + mesh.Perform() + + # 提取 numpy 网格 + vertices_np, faces_np = occ_shape_to_numpy_mesh(shape) + + # 读取点云 + points_np = read_point_cloud(pointcloud_path) + + # numpy -> torch + points = torch.as_tensor(points_np, dtype=dtype, device=device) + vertices = torch.as_tensor(vertices_np, dtype=dtype, device=device) + faces = torch.as_tensor(faces_np, dtype=torch.long, device=device) + + triangles = vertices[faces] + + print("开始计算点云到 CAD 的最近距离...") + print("点云张量:", points.shape) + print("三角面张量:", triangles.shape) + + with torch.no_grad(): + distances_torch = nearest_mesh_distances( + points, + triangles, + point_chunk_size=point_chunk_size, + triangle_chunk_size=triangle_chunk_size + ) + + distances_np = distances_torch.detach().cpu().numpy() + + mean_dist = float(distances_np.mean()) + max_dist = float(distances_np.max()) + min_dist = float(distances_np.min()) + + # print("Distance computation finished") + print(f"最小距离: {min_dist:.8f}") + print(f"平均距离: {mean_dist:.8f}") + print(f"最大距离: {max_dist:.8f}") + + if visualize: + visualize_point_cloud_heatmap(points_np, distances_np) + + return { + "mean_dist": mean_dist, + "max_dist": max_dist, + "min_dist": min_dist, + "points_np": points_np, + "distances_np": distances_np + } + + +def get_task_paths(task_id, data_root="High_Speed_Data", pointcloud_kind="noise"): + data_root = Path(data_root) + task_id = int(task_id) + + cad_path = data_root / "CAD" / f"{task_id}.step" + + if pointcloud_kind == "clean": + pointcloud_path = data_root / "PointCloud" / f"{task_id}.ply" + else: + pointcloud_path = data_root / "Noise_PointCloud" / f"{task_id}_noisy.ply" + + return cad_path, pointcloud_path + + +def run_batch_tasks( + task_ids=DEFAULT_TASK_IDS, + data_root="High_Speed_Data", + pointcloud_kind="noise", + excel_output="平整度.xlsx", + mesh_deflection=0.1, + point_chunk_size=2048, + triangle_chunk_size=4096 +): + global task_distance_data + + result_rows = [] + task_distance_data = {} + + for task_id in task_ids: + task_id = int(task_id) + cad_path, pointcloud_path = get_task_paths( + task_id, + data_root=data_root, + pointcloud_kind=pointcloud_kind + ) + + print(f"===== 任务 {task_id} =====") + + missing_paths = [ + str(path) + for path in [cad_path, pointcloud_path] + if not path.exists() + ] + + if missing_paths: + message = "缺少文件: " + "; ".join(missing_paths) + print(message) + result_rows.append({ + "task_id": task_id, + "mean_dist": None, + "max_dist": None, + "status": "失败", + "message": message + }) + continue + + try: + result = compute_point_CAD_distance( + CAD_path=str(cad_path), + pointcloud_path=str(pointcloud_path), + mesh_deflection=mesh_deflection, + point_chunk_size=point_chunk_size, + triangle_chunk_size=triangle_chunk_size, + visualize=False + ) + except Exception as exc: + message = str(exc) + print(f"任务 {task_id} 失败: {message}") + result_rows.append({ + "task_id": task_id, + "mean_dist": None, + "max_dist": None, + "status": "失败", + "message": message + }) + continue + + task_distance_data[task_id] = { + "points_np": result["points_np"], + "distances_np": result["distances_np"] + } + + result_rows.append({ + "task_id": task_id, + "mean_dist": result["mean_dist"], + "max_dist": result["max_dist"], + "status": "完成", + "message": "" + }) + + excel_path = write_flatness_excel( + result_rows, + excel_output, + sheet_name="平整度" + ) + + print(f"Excel 已保存: {excel_path}") + + return result_rows, task_distance_data + + +# ============================================================ +# 命令行入口 +# ============================================================ + +if __name__ == "__main__": + + parser = argparse.ArgumentParser( + description="计算点云到 CAD STEP 模型的最近距离，支持单任务或批量任务。" + ) + + parser.add_argument( + "--CAD", + "-c", + default="High_Speed_Data/CAD/11.step", + help="CAD STEP 文件路径，仅在 --single 单任务模式下使用。" + ) + + parser.add_argument( + "--pointcloud", + "-p", + default="High_Speed_Data/Noise_PointCloud/11_noisy.ply", + help="点云 PLY 文件路径，仅在 --single 单任务模式下使用。" + ) + + parser.add_argument( + "--tasks", + default=",".join(str(task_id) for task_id in DEFAULT_TASK_IDS), + help="批量任务号，多个任务用英文逗号分隔。" + ) + + parser.add_argument( + "--data_root", + default="High_Speed_Data", + help="批量任务的数据根目录。" + ) + + parser.add_argument( + "--pointcloud_kind", + choices=["noise", "clean"], + default="noise", + help="批量任务点云来源：noise=Noise_PointCloud，clean=PointCloud。" + ) + + parser.add_argument( + "--excel_output", + default="平整度.xlsx", + help="批量结果 Excel 输出路径。" + ) + + parser.add_argument( + "--single", + action="store_true", + help="使用 --CAD 和 --pointcloud 执行单任务，而不是默认批量任务。" + ) + + parser.add_argument( + "--visualize", + action="store_true", + help="显示单任务热力图。批量模式不会自动可视化。" + ) + + parser.add_argument( + "--mesh_deflection", + default=0.1, + type=float, + help="CAD 三角化精度，越小越精细，但计算越慢。" + ) + + parser.add_argument( + "--point_chunk_size", + default=2048, + type=int, + help="点云分块大小，显存不够时可调小。" + ) + + parser.add_argument( + "--triangle_chunk_size", + default=4096, + type=int, + help="三角面分块大小，显存不够时可调小。" + ) + + args = parser.parse_args() + + if args.single: + result = compute_point_CAD_distance( + CAD_path=args.CAD, + pointcloud_path=args.pointcloud, + mesh_deflection=args.mesh_deflection, + point_chunk_size=args.point_chunk_size, + triangle_chunk_size=args.triangle_chunk_size, + visualize=args.visualize + ) + + print("===== 计算结果 =====") + print(f"平均距离: {result['mean_dist']:.8f} mm") + print(f"最大距离: {result['max_dist']:.8f} mm") + else: + task_ids = [ + int(task_id.strip()) + for task_id in args.tasks.split(",") + if task_id.strip() + ] + + result_rows, task_distance_data = run_batch_tasks( + task_ids=task_ids, + data_root=args.data_root, + pointcloud_kind=args.pointcloud_kind, + excel_output=args.excel_output, + mesh_deflection=args.mesh_deflection, + point_chunk_size=args.point_chunk_size, + triangle_chunk_size=args.triangle_chunk_size + ) + + print("===== 批量计算结果 =====") + for row in result_rows: + if row["status"] == "完成": + print( + f"{row['task_id']}: " + f"平均距离={row['mean_dist']:.8f} mm, " + f"最大距离={row['max_dist']:.8f} mm" + ) + else: + print(f"{row['task_id']}: {row['status']} - {row['message']}") + + print("task_distance_data 已按任务号保存 points_np 和 distances_np。") diff --git a/alg/point_porj/archive/Curvature_Estimation.py b/alg/point_porj/archive/Curvature_Estimation.py new file mode 100644 index 0000000..156317d --- /dev/null +++ b/alg/point_porj/archive/Curvature_Estimation.py @@ -0,0 +1,127 @@ +import open3d as o3d +import numpy as np +import argparse +import os + + +def compute_curvature_open3d(pcd, radius=0.05, max_nn=30): + """ + 使用 Open3D 计算点云曲率 + + Args: + pcd: Open3D 点云对象 + radius: 搜索半径 + max_nn: 最大邻居数 + + Returns: + curvatures: (N,) 曲率数组 + """ + # 估计法线（同时获取协方差矩阵信息） + pcd.estimate_normals( + search_param=o3d.geometry.KDTreeSearchParamHybrid(radius=radius, max_nn=max_nn) + ) + + # 获取点云数据 + points = np.asarray(pcd.points) + n_points = len(points) + curvatures = np.zeros(n_points) + + # 构建 KD-Tree + tree = o3d.geometry.KDTreeFlann(pcd) + + for i in range(n_points): + # 搜索邻域点 + [k, idx, _] = tree.search_radius_vector_3d(pcd.points[i], radius) + + if k < 3: + curvatures[i] = 1.0 + continue + + # 获取邻域点坐标 + neighbors = points[idx] + + # 计算协方差矩阵 + centroid = np.mean(neighbors, axis=0) + centered = neighbors - centroid + cov = np.cov(centered.T) + + # 特征值分解 + eigenvalues = np.linalg.eigvalsh(cov) + eigenvalues = np.sort(eigenvalues) + + # 计算曲率 + curvatures[i] = eigenvalues[0] / (eigenvalues.sum() + 1e-8) + + return curvatures + + +def visualize_curvature(pcd, curvatures, color_scale=3.0): + """ + 根据曲率可视化点云 + + Args: + pcd: Open3D 点云对象 + curvatures: 曲率数组 + color_scale: 颜色缩放因子 + """ + colors = np.zeros((len(pcd.points), 3)) + # 红色通道表示曲率（高曲率区域为红色） + colors[:, 0] = np.clip(curvatures * color_scale, 0, 1) + # 绿色通道为互补色（低曲率区域为绿色） + colors[:, 1] = np.clip(1 - curvatures * color_scale, 0, 1) + # 蓝色通道设为0 + colors[:, 2] = 0 + + pcd.colors = o3d.utility.Vector3dVector(colors) + + +def main(): + parser = argparse.ArgumentParser(description='计算点云曲率并可视化') + parser.add_argument('--ply_path', type=str,default="./High_Speed_Data/PointCloud/11.ply", help='输入点云文件路径 (.ply)') + parser.add_argument('--radius', type=float, default=2, help='搜索半径 (默认: 50)') + parser.add_argument('--max_nn', type=int, default=30, help='最大邻居数 (默认: 30)') + parser.add_argument('--color_scale', type=float, default=3.0, help='颜色缩放因子 (默认: 3.0)') + parser.add_argument('--save', type=str, default=None, help='保存可视化图像路径 (可选)') + + args = parser.parse_args() + + # 检查文件是否存在 + if not os.path.exists(args.ply_path): + print(f"错误: 文件不存在 - {args.ply_path}") + return + + print(f"读取点云: {args.ply_path}") + pcd = o3d.io.read_point_cloud(args.ply_path) + # points=np.array(pcd.points) + if len(pcd.points) == 0: + print("错误: 读取点云失败，文件可能为空或格式不正确") + return + + print(f"点云点数: {len(pcd.points)}") + + # 计算曲率 + print("计算曲率中...") + curvatures = compute_curvature_open3d(pcd, radius=args.radius, max_nn=args.max_nn) + + # 统计曲率信息 + print(f"曲率统计:") + print(f" 最小值: {curvatures.min():.6f}") + print(f" 最大值: {curvatures.max():.6f}") + print(f" 平均值: {curvatures.mean():.6f}") + print(f" 标准差: {curvatures.std():.6f}") + + # 可视化 + print("可视化曲率...") + visualize_curvature(pcd, curvatures, color_scale=args.color_scale) + + # 保存截图（如果指定） + if args.save: + o3d.io.write_point_cloud(args.save, pcd) + print(f"点云已保存: {args.save}") + + # 显示窗口 + o3d.visualization.draw_geometries([pcd], window_name=f"Curvature Visualization - {os.path.basename(args.ply_path)}") + + +if __name__ == "__main__": + main() \ No newline at end of file diff --git a/alg/point_porj/archive/Denoise.py b/alg/point_porj/archive/Denoise.py new file mode 100644 index 0000000..6f68fa4 --- /dev/null +++ b/alg/point_porj/archive/Denoise.py @@ -0,0 +1,431 @@ +import open3d as o3d +import numpy as np +from scipy.spatial import KDTree +import copy +import argparse +import os + + +# ========== 1. Chamfer Distance 计算 ========== +def chamfer_distance(pcd1, pcd2, sqrt=True): + """ + 计算两个点云之间的 CD 距离（Chamfer Distance） + + Args: + pcd1: Open3D 点云对象1 + pcd2: Open3D 点云对象2 + sqrt: 是否对最终结果开平方根 + + Returns: + cd: Chamfer Distance 值 + cd1: 点云1 到 点云2 的平均距离 + cd2: 点云2 到 点云1 的平均距离 + """ + points1 = np.asarray(pcd1.points) + points2 = np.asarray(pcd2.points) + + if len(points1) == 0 or len(points2) == 0: + return float('inf'), 0, 0 + + tree1 = KDTree(points1) + tree2 = KDTree(points2) + + dist1, _ = tree2.query(points1, k=1) + cd1 = np.mean(dist1 ** 2) + + dist2, _ = tree1.query(points2, k=1) + cd2 = np.mean(dist2 ** 2) + + cd = cd1 + cd2 + + if sqrt: + return np.sqrt(cd), cd1, cd2 + return cd, cd1, cd2 + + +# ========== 添加高斯噪声 ========== +def add_gaussian_noise(pcd, noise_std=4.0): + """ + 给点云添加高斯噪声 + + Args: + pcd: Open3D 点云对象 + noise_std: 噪声标准差 + + Returns: + noisy_pcd: 添加噪声后的点云 + """ + noisy_pcd = copy.deepcopy(pcd) + points = np.asarray(noisy_pcd.points) + + noise = np.random.normal(0, noise_std, points.shape) + noisy_points = points + noise + + noisy_pcd.points = o3d.utility.Vector3dVector(noisy_points) + + return noisy_pcd + + +# ========== 2. 统计离群点移除 (SOR) ========== +def statistical_outlier_removal(pcd, nb_neighbors=20, std_ratio=2.0): + """ + 统计离群点移除 + """ + cl, ind = pcd.remove_statistical_outlier(nb_neighbors=nb_neighbors, std_ratio=std_ratio) + + inlier_cloud = pcd.select_by_index(ind) + outlier_cloud = pcd.select_by_index(ind, invert=True) + + metrics = { + 'original_points': len(pcd.points), + 'filtered_points': len(inlier_cloud.points), + 'removed_points': len(outlier_cloud.points), + 'keep_ratio': len(inlier_cloud.points) / len(pcd.points) * 100 if len(pcd.points) > 0 else 0, + 'method': 'SOR', + 'params': {'nb_neighbors': nb_neighbors, 'std_ratio': std_ratio} + } + + return inlier_cloud, outlier_cloud, metrics + + +# ========== 3. 半径离群点移除 (ROR) ========== +def radius_outlier_removal(pcd, nb_points=5, radius=20): + """ + 半径离群点移除 + """ + cl, ind = pcd.remove_radius_outlier(nb_points=nb_points, radius=radius) + + inlier_cloud = pcd.select_by_index(ind) + outlier_cloud = pcd.select_by_index(ind, invert=True) + + metrics = { + 'original_points': len(pcd.points), + 'filtered_points': len(inlier_cloud.points), + 'removed_points': len(outlier_cloud.points), + 'keep_ratio': len(inlier_cloud.points) / len(pcd.points) * 100 if len(pcd.points) > 0 else 0, + 'method': 'ROR', + 'params': {'nb_points': nb_points, 'radius': radius} + } + + return inlier_cloud, outlier_cloud, metrics + + +# ========== 4. 直通滤波 (PF) ========== +def pass_through_filter(pcd, axis='z', min_val=-6000.0, max_val=6000.0): + """ + 直通滤波 + """ + points = np.asarray(pcd.points) + axis_map = {'x': 0, 'y': 1, 'z': 2} + idx = axis_map[axis] + + mask = (points[:, idx] >= min_val) & (points[:, idx] <= max_val) + filtered_points = points[mask] + + filtered_pcd = o3d.geometry.PointCloud() + filtered_pcd.points = o3d.utility.Vector3dVector(filtered_points) + + metrics = { + 'original_points': len(pcd.points), + 'filtered_points': len(filtered_points), + 'removed_points': len(pcd.points) - len(filtered_points), + 'keep_ratio': len(filtered_points) / len(pcd.points) * 100 if len(pcd.points) > 0 else 0, + 'method': 'PF', + 'params': {'axis': axis, 'min_val': min_val, 'max_val': max_val} + } + + return filtered_pcd, metrics + + +def compute_keep_ratio_and_density(pcd, filtered_pcd, sample_size=1000): + """计算直通滤波的保留率和密度变化""" + n_orig = len(pcd.points) + n_filt = len(filtered_pcd.points) + + if n_orig == 0 or n_filt == 0: + return 0, 0 + + keep_ratio = n_filt / n_orig * 100 + + points_orig = np.asarray(pcd.points) + points_filt = np.asarray(filtered_pcd.points) + + if len(points_orig) > sample_size: + idx_orig = np.random.choice(len(points_orig), sample_size, replace=False) + points_orig_sample = points_orig[idx_orig] + else: + points_orig_sample = points_orig + + if len(points_filt) > sample_size: + idx_filt = np.random.choice(len(points_filt), sample_size, replace=False) + points_filt_sample = points_filt[idx_filt] + else: + points_filt_sample = points_filt + + if len(points_orig_sample) > 5: + tree_orig = KDTree(points_orig_sample) + dist_orig, _ = tree_orig.query(points_orig_sample, k=2) + avg_dist_orig = np.mean(dist_orig[:, 1]) + else: + avg_dist_orig = 0 + + if len(points_filt_sample) > 5: + tree_filt = KDTree(points_filt_sample) + dist_filt, _ = tree_filt.query(points_filt_sample, k=2) + avg_dist_filt = np.mean(dist_filt[:, 1]) + else: + avg_dist_filt = 0 + + if avg_dist_orig > 0: + density_change = (avg_dist_filt - avg_dist_orig) / avg_dist_orig * 100 + else: + density_change = 0 + + return keep_ratio, density_change + + +# ========== 5. 体素滤波 (VGF) ========== +def voxel_grid_filter(pcd, leaf_size=0.05): + """ + 体素滤波 + """ + filtered_pcd = pcd.voxel_down_sample(leaf_size) + + n_orig = len(pcd.points) + n_filt = len(filtered_pcd.points) + + metrics = { + 'original_points': n_orig, + 'filtered_points': n_filt, + 'removed_points': n_orig - n_filt, + 'keep_ratio': n_filt / n_orig * 100 if n_orig > 0 else 0, + 'downsample_rate': (n_orig - n_filt) / n_orig * 100 if n_orig > 0 else 0, + 'compression_ratio': n_orig / n_filt if n_filt > 0 else 0, + 'method': 'VGF', + 'params': {'leaf_size': leaf_size} + } + + if n_filt > 5: + points_sample = np.asarray(filtered_pcd.points) + sample_size = min(1000, n_filt) + if n_filt > sample_size: + idx = np.random.choice(n_filt, sample_size, replace=False) + points_sample = points_sample[idx] + tree = KDTree(points_sample) + dist, _ = tree.query(points_sample, k=2) + metrics['avg_point_distance'] = np.mean(dist[:, 1]) + + return filtered_pcd, metrics + + +# ========== 6. 双边滤波 (BF) ========== +def bilateral_filter(pcd, sigma_s=0.01, sigma_r=0.1): + """ + 双边滤波 + """ + if len(pcd.points) == 0: + return pcd, {'original_points': 0, 'filtered_points': 0, 'removed_points': 0, 'keep_ratio': 0, 'method': 'BF', 'params': {}} + + # 确保有法线 + if not pcd.has_normals(): + pcd.estimate_normals() + + points = np.asarray(pcd.points) + normals = np.asarray(pcd.normals) + + tree = KDTree(points) + filtered_points = np.zeros_like(points) + + for i, p in enumerate(points): + indices = tree.query_ball_point(p, r=2 * sigma_s) + if len(indices) <= 1: + filtered_points[i] = p + continue + + neigh_points = points[indices] + neigh_normals = normals[indices] + + spatial_dist = np.linalg.norm(neigh_points - p, axis=1) + w_s = np.exp(-spatial_dist ** 2 / (2 * sigma_s ** 2)) + + normal_diff = np.linalg.norm(neigh_normals - normals[i], axis=1) + w_r = np.exp(-normal_diff ** 2 / (2 * sigma_r ** 2)) + + w = w_s * w_r + if w.sum() > 0: + w = w / w.sum() + filtered_points[i] = np.sum(neigh_points.T * w, axis=1) + + filtered_pcd = o3d.geometry.PointCloud() + filtered_pcd.points = o3d.utility.Vector3dVector(filtered_points) + filtered_pcd.normals = o3d.utility.Vector3dVector(normals) + + metrics = { + 'original_points': len(pcd.points), + 'filtered_points': len(filtered_points), + 'removed_points': 0, + 'keep_ratio': 100, + 'method': 'BF', + 'params': {'sigma_s': sigma_s, 'sigma_r': sigma_r} + } + + return filtered_pcd, metrics + + +# ========== 主函数 ========== +def main(): + parser = argparse.ArgumentParser(description='点云去噪算法对比 (SOR, ROR, PF, VGF, BF)') + parser.add_argument('--ply_path', '-p', type=str, default="./High_Speed_Data/PointCloud/11.ply", + help='输入点云文件路径 (.ply)') + parser.add_argument('--noise_std', '-n', type=float, default=4.0, + help='高斯噪声标准差 (默认: 4.0)') + parser.add_argument('--voxel_size', '-v', type=float, default=0.05, + help='体素滤波大小 (默认: 0.05)') + parser.add_argument('--sor_neighbors', type=int, default=20, + help='SOR 邻域点数 (默认: 20)') + parser.add_argument('--sor_std_ratio', type=float, default=2.0, + help='SOR 标准差倍数 (默认: 2.0)') + parser.add_argument('--ror_points', type=int, default=5, + help='ROR 半径内最小点数 (默认: 5)') + parser.add_argument('--ror_radius', type=float, default=20, + help='ROR 搜索半径 (默认: 20)') + parser.add_argument('--bf_sigma_s', type=float, default=0.01, + help='双边滤波空间标准差 (默认: 0.01)') + parser.add_argument('--bf_sigma_r', type=float, default=0.1, + help='双边滤波特征标准差 (默认: 0.1)') + parser.add_argument('--no_visualize', action='store_true', + help='不显示可视化窗口') + + args = parser.parse_args() + + # 检查文件是否存在 + if not os.path.exists(args.ply_path): + print(f"错误: 点云文件不存在 - {args.ply_path}") + return + + # 1. 读取原始点云 + print(f"读取点云: {args.ply_path}") + pcd_clean = o3d.io.read_point_cloud(args.ply_path) + + if len(pcd_clean.points) == 0: + print("读取失败，请检查文件路径") + return + + print(f"原始无噪点云点数: {len(pcd_clean.points)}") + + # ========== 添加高斯噪声 ========== + print("\n" + "=" * 50) + print(f"添加高斯噪声 (std={args.noise_std})") + print("=" * 50) + pcd_noisy = add_gaussian_noise(pcd_clean, args.noise_std) + print(f"加噪后点云点数: {len(pcd_noisy.points)}") + + # 计算噪声水平 + points_clean = np.asarray(pcd_clean.points) + points_noisy = np.asarray(pcd_noisy.points) + noise_level = np.mean(np.linalg.norm(points_noisy - points_clean, axis=1)) + print(f"平均噪声偏移量: {noise_level:.4f}") + + # 加噪点云着色 + pcd_noisy.paint_uniform_color([1.0, 0.7, 0.5]) + + # ========== 运行各去噪算法 ========== + results = {} + + # 1. SOR + print("\n" + "=" * 50) + print("统计离群点移除 (SOR)") + print("=" * 50) + sor_filtered, sor_outlier, sor_metrics = statistical_outlier_removal( + pcd_noisy, nb_neighbors=args.sor_neighbors, std_ratio=args.sor_std_ratio) + results['SOR'] = sor_metrics + print(f"保留率: {sor_metrics['keep_ratio']:.2f}%") + print(f"移除: {sor_metrics['removed_points']} 点") + + # 2. ROR + print("\n" + "=" * 50) + print("半径离群点移除 (ROR)") + print("=" * 50) + ror_filtered, ror_outlier, ror_metrics = radius_outlier_removal( + pcd_noisy, nb_points=args.ror_points, radius=args.ror_radius) + results['ROR'] = ror_metrics + print(f"保留率: {ror_metrics['keep_ratio']:.2f}%") + print(f"移除: {ror_metrics['removed_points']} 点") + + # 3. 直通滤波 (PF) + print("\n" + "=" * 50) + print("直通滤波 (PF)") + print("=" * 50) + points = np.asarray(pcd_noisy.points) + z_min, z_max = points[:, 2].min(), points[:, 2].max() + pf_filtered, pf_metrics = pass_through_filter(pcd_noisy, axis='z', min_val=z_min, max_val=z_max) + results['PF'] = pf_metrics + print(f"保留率: {pf_metrics['keep_ratio']:.2f}%") + keep_ratio, density_change = compute_keep_ratio_and_density(pcd_noisy, pf_filtered) + print(f"密度变化: {density_change:+.2f}%") + + # 4. 体素滤波 (VGF) + print("\n" + "=" * 50) + print("体素滤波 (VGF)") + print("=" * 50) + vgf_filtered, vgf_metrics = voxel_grid_filter(pcd_noisy, leaf_size=args.voxel_size) + results['VGF'] = vgf_metrics + print(f"保留率: {vgf_metrics['keep_ratio']:.2f}%") + print(f"压缩比: {vgf_metrics['compression_ratio']:.1f}x") + + # 5. 双边滤波 (BF) + print("\n" + "=" * 50) + print("双边滤波 (BF)") + print("=" * 50) + bf_filtered, bf_metrics = bilateral_filter(pcd_noisy, sigma_s=args.bf_sigma_s, sigma_r=args.bf_sigma_r) + results['BF'] = bf_metrics + print(f"保留率: {bf_metrics['keep_ratio']:.2f}%") + + # Chamfer Distance 计算 + print("\n" + "=" * 50) + print("Chamfer Distance 评估 (与干净点云对比)") + print("=" * 50) + + for name, metrics in results.items(): + if name == 'BF': + filtered = bf_filtered + elif name == 'SOR': + filtered = sor_filtered + elif name == 'ROR': + filtered = ror_filtered + elif name == 'PF': + filtered = pf_filtered + elif name == 'VGF': + filtered = vgf_filtered + else: + continue + + if len(filtered.points) > 0: + cd, _, _ = chamfer_distance(pcd_clean, filtered) + print(f"{name}: CD = {cd:.6f} m") + + print("-" * 40) + cd_noisy, _, _ = chamfer_distance(pcd_clean, pcd_noisy) + print(f"加噪点云 (未经滤波): CD = {cd_noisy:.6f} m") + + # 可视化 + if not args.no_visualize: + print("\n可视化对比...") + pcd_clean.paint_uniform_color([0, 0, 1]) # 蓝色 - 原始干净点云 + pcd_noisy.paint_uniform_color([1.0, 0.7, 0.5]) # 浅橙色 - 加噪点云 + + sor_filtered.paint_uniform_color([0, 1, 0]) # 绿色 - SOR 结果 + + o3d.visualization.draw_geometries( + [pcd_clean, pcd_noisy], + window_name=f"Clean (Blue, {len(pcd_clean.points)}) vs Noisy (Orange, {len(pcd_noisy.points)})" + ) + + o3d.visualization.draw_geometries( + [pcd_clean, sor_filtered], + window_name=f"SOR: Clean (Blue, {len(pcd_clean.points)}) vs Filtered (Green, {len(sor_filtered.points)})" + ) + + +if __name__ == "__main__": + main() \ No newline at end of file diff --git a/alg/point_porj/archive/Plane_Estimation.py b/alg/point_porj/archive/Plane_Estimation.py new file mode 100644 index 0000000..c90ee41 --- /dev/null +++ b/alg/point_porj/archive/Plane_Estimation.py @@ -0,0 +1,380 @@ +# import numpy as np +# import open3d as o3d +# import random +# +# +# def ransac_plane(points, threshold=0.01, max_iterations=1000, min_inliers=100): +# """ +# RANSAC 平面拟合（单个） +# """ +# best_inliers = [] +# best_plane = None +# n_points = len(points) +# +# if n_points < 3: +# return None, [] +# +# for _ in range(max_iterations): +# sample_idx = random.sample(range(n_points), 3) +# p1, p2, p3 = points[sample_idx] +# +# v1 = p2 - p1 +# v2 = p3 - p1 +# normal = np.cross(v1, v2) +# norm = np.linalg.norm(normal) +# if norm < 1e-8: +# continue +# normal = normal / norm +# a, b, c = normal +# d = -np.dot(normal, p1) +# +# distances = np.abs(np.dot(points, normal) + d) +# inliers = np.where(distances < threshold)[0] +# +# if len(inliers) > len(best_inliers) and len(inliers) >= min_inliers: +# best_inliers = inliers +# best_plane = (a, b, c, d) +# +# if len(best_inliers) > 0.9 * n_points: +# break +# +# # 用内点重新拟合 +# if best_inliers is not None and len(best_inliers) > 3: +# inlier_points = points[best_inliers] +# centroid = np.mean(inlier_points, axis=0) +# _, _, vh = np.linalg.svd(inlier_points - centroid) +# normal = vh[2] +# normal = normal / np.linalg.norm(normal) +# a, b, c = normal +# d = -np.dot(normal, centroid) +# best_plane = (a, b, c, d) +# +# return best_plane, best_inliers +# +# +# def sequential_ransac_planes(points, threshold=0.01, max_iterations=1000, min_inliers=100, max_planes=5): +# """ +# 迭代 RANSAC 拟合多个平面 +# """ +# remaining_points = points.copy() +# remaining_indices = np.arange(len(points)) +# +# planes = [] +# inliers_list = [] +# +# for i in range(max_planes): +# if len(remaining_points) < min_inliers: +# break +# +# plane, inliers = ransac_plane(remaining_points, threshold, max_iterations, min_inliers) +# +# if plane is None or len(inliers) < min_inliers: +# break +# +# planes.append(plane) +# original_indices = remaining_indices[inliers] +# inliers_list.append(original_indices) +# +# # 移除内点 +# mask = np.ones(len(remaining_points), dtype=bool) +# mask[inliers] = False +# remaining_points = remaining_points[mask] +# remaining_indices = remaining_indices[mask] +# +# print(f"平面 {len(planes)}: {len(inliers)} 个内点, 剩余 {len(remaining_points)} 个点") +# +# return planes, inliers_list +# +# +# def create_plane_mesh(plane, bounds): +# """ +# 创建平面网格用于可视化 +# """ +# a, b, c, d = plane +# x_min, x_max = bounds[0] +# y_min, y_max = bounds[1] +# +# x = np.linspace(x_min, x_max, 50) +# y = np.linspace(y_min, y_max, 50) +# X, Y = np.meshgrid(x, y) +# +# if abs(c) > 1e-8: +# Z = -(a * X + b * Y + d) / c +# else: +# Z = np.zeros_like(X) +# +# vertices = [] +# triangles = [] +# rows, cols = X.shape +# +# for i in range(rows): +# for j in range(cols): +# vertices.append([X[i, j], Y[i, j], Z[i, j]]) +# +# for i in range(rows - 1): +# for j in range(cols - 1): +# idx = i * cols + j +# triangles.append([idx, idx + 1, idx + cols]) +# triangles.append([idx + 1, idx + cols + 1, idx + cols]) +# +# mesh = o3d.geometry.TriangleMesh() +# mesh.vertices = o3d.utility.Vector3dVector(np.array(vertices)) +# mesh.triangles = o3d.utility.Vector3iVector(np.array(triangles)) +# mesh.compute_vertex_normals() +# +# return mesh +# +# +# def visualize_multiple_planes(points, planes, inliers_list): +# """ +# 可视化多个平面（点云 + 平面网格） +# +# Args: +# points: (N, 3) 点云坐标 +# planes: 平面方程列表 [(a,b,c,d), ...] +# inliers_list: 每个平面对应的内点索引列表 +# """ +# # 颜色列表（每个平面不同颜色） +# colors = [ +# [1, 0, 0], # 红色 +# [0, 1, 0], # 绿色 +# [0, 0, 1], # 蓝色 +# [1, 1, 0], # 黄色 +# [1, 0, 1], # 品红 +# [0, 1, 1], # 青色 +# [1, 0.5, 0], # 橙色 +# [0.5, 0, 1], # 紫色 +# ] +# +# geometries = [] +# +# # 1. 为每个平面的内点着色 +# point_colors = np.zeros((len(points), 3)) +# for i, inliers in enumerate(inliers_list): +# color = colors[i % len(colors)] +# point_colors[inliers] = color +# +# # 未分配的点（剩余点）设为灰色 +# unassigned = np.where(np.sum(point_colors, axis=1) == 0)[0] +# point_colors[unassigned] = [0.5, 0.5, 0.5] +# +# colored_pcd = o3d.geometry.PointCloud() +# colored_pcd.points = o3d.utility.Vector3dVector(points) +# colored_pcd.colors = o3d.utility.Vector3dVector(point_colors) +# geometries.append(colored_pcd) +# +# # 2. 为每个平面创建网格 +# for i, (plane, inliers) in enumerate(zip(planes, inliers_list)): +# # 计算该平面对应点的范围 +# inlier_points = points[inliers] +# if len(inlier_points) > 0: +# x_min, x_max = inlier_points[:, 0].min(), inlier_points[:, 0].max() +# y_min, y_max = inlier_points[:, 1].min(), inlier_points[:, 1].max() +# +# # 稍微扩展边界 +# x_range = x_max - x_min +# y_range = y_max - y_min +# x_min -= x_range * 0.1 +# x_max += x_range * 0.1 +# y_min -= y_range * 0.1 +# y_max += y_range * 0.1 +# +# mesh = create_plane_mesh(plane, [[x_min, x_max], [y_min, y_max]]) +# mesh.paint_uniform_color(colors[i % len(colors)]) +# # 设置半透明 +# mesh.paint_uniform_color([colors[i % len(colors)][0], +# colors[i % len(colors)][1], +# colors[i % len(colors)][2]]) +# geometries.append(mesh) +# +# # 可视化 +# o3d.visualization.draw_geometries( +# geometries, +# window_name="Multiple Planes - RANSAC", +# width=1024, +# height=768 +# ) +# +# +# def visualize_single_plane(points, plane, inliers): +# """ +# 可视化单个平面（兼容单平面情况） +# """ +# visualize_multiple_planes(points, [plane], [inliers]) +# +# +# # ========== 主程序 ========== +# if __name__ == "__main__": +# # 读取点云 +# file_path = r"E:\point_distance\data\part_car_head.ply" +# pcd = o3d.io.read_point_cloud(file_path) +# +# if len(pcd.points) == 0: +# print("读取失败，请检查文件路径") +# exit() +# +# points = np.asarray(pcd.points) +# print(f"原始点云点数: {len(points)}") +# +# # 计算点云范围 +# points_range = points.max(axis=0) - points.min(axis=0) +# print(f"点云范围: X[{points_range[0]:.3f}], Y[{points_range[1]:.3f}], Z[{points_range[2]:.3f}]") +# +# # 设置阈值 +# threshold = points_range[2] * 0.02 +# print(f"使用阈值: {threshold:.4f}") +# +# # ========== 单平面拟合 ========== +# # print("\n" + "=" * 50) +# # print("单平面拟合") +# # print("=" * 50) +# # +# # plane, inliers = ransac_plane(points, threshold=threshold, max_iterations=1, min_inliers=100) +# # +# # if plane is not None: +# # a, b, c, d = plane +# # print(f"拟合平面: {a:.4f}x + {b:.4f}y + {c:.4f}z + {d:.4f} = 0") +# # print(f"内点数量: {len(inliers)} / {len(points)} ({len(inliers) / len(points) * 100:.1f}%)") +# # visualize_single_plane(points, plane, inliers) +# +# # ========== 多平面拟合 ========== +# print("\n" + "=" * 50) +# print("多平面拟合") +# print("=" * 50) +# +# planes, inliers_list = sequential_ransac_planes( +# points, +# threshold=threshold, +# max_iterations=1, +# min_inliers=100, +# max_planes=1 +# ) +# +# print(f"共提取 {len(planes)} 个平面") +# for i, (plane, inliers) in enumerate(zip(planes, inliers_list)): +# a, b, c, d = plane +# print(f"平面 {i + 1}: {a:.4f}x + {b:.4f}y + {c:.4f}z + {d:.4f} = 0, 内点数: {len(inliers)}") +# +# if len(planes) > 0: +# visualize_multiple_planes(points, planes, inliers_list) + +import numpy as np +import open3d as o3d +import random + + +def create_square_plane_with_noise(size_mm=1000, noise_std_mm=0.5, num_points=10000): + """创建正方形平面点云并添加高斯噪声""" + size_m = size_mm / 1000.0 + noise_std_m = noise_std_mm / 1000.0 + + half_size = size_m / 2 + x = np.random.uniform(-half_size, half_size, num_points) + y = np.random.uniform(-half_size, half_size, num_points) + z = np.zeros(num_points) + + points = np.column_stack([x, y, z]) + noise_z = np.random.normal(0, noise_std_m, num_points) + points[:, 2] += noise_z + + return points + + +def estimate_plane_ransac_open3d(points, threshold_mm=0.5, max_iterations=2000): + """使用 Open3D 内置 RANSAC 平面估计""" + pcd = o3d.geometry.PointCloud() + pcd.points = o3d.utility.Vector3dVector(points) + + threshold_m = threshold_mm / 1000.0 + + plane_model, inliers = pcd.segment_plane( + distance_threshold=threshold_m, + ransac_n=3, + num_iterations=max_iterations + ) + + a, b, c, d = plane_model + + # 计算误差 + ideal_normal = np.array([0, 0, 1]) + estimated_normal = np.array([a, b, c]) + cos_angle = np.dot(ideal_normal, estimated_normal) + cos_angle = np.clip(cos_angle, -1, 1) + angle_error = np.degrees(np.arccos(cos_angle)) + offset_error = abs(d) + + inlier_points = points[inliers] + distances = np.abs(np.dot(inlier_points, estimated_normal) + d) + rmse = np.sqrt(np.mean(distances ** 2)) + + print(f"\nRANSAC 平面拟合结果 (Open3D):") + print(f" 估计平面: {a:.6f}x + {b:.6f}y + {c:.6f}z + {d:.6f} = 0") + print(f" 内点数量: {len(inliers)} / {len(points)} ({len(inliers) / len(points) * 100:.1f}%)") + print(f" 法线角度误差: {angle_error:.4f} 度") + print(f" 偏移误差: {offset_error:.6f} m ({offset_error * 1000:.3f} mm)") + print(f" 平面拟合 RMSE: {rmse:.6f} m ({rmse * 1000:.3f} mm)") + + return plane_model, inliers + + +def visualize_plane_fitting(points, plane): + """可视化原始点云和拟合平面""" + # 原始点云 + pcd = o3d.geometry.PointCloud() + pcd.points = o3d.utility.Vector3dVector(points) + pcd.paint_uniform_color([0.6, 0.8, 1.0]) + + # 创建平面点云 + a, b, c, d = plane + x_min, x_max = points[:, 0].min(), points[:, 0].max() + y_min, y_max = points[:, 1].min(), points[:, 1].max() + + x = np.linspace(x_min, x_max, 50) + y = np.linspace(y_min, y_max, 50) + X, Y = np.meshgrid(x, y) + + if abs(c) > 1e-8: + Z = -(a * X + b * Y + d) / c + else: + Z = np.zeros_like(X) + + plane_points = np.column_stack([X.ravel(), Y.ravel(), Z.ravel()]) + + pcd_plane = o3d.geometry.PointCloud() + pcd_plane.points = o3d.utility.Vector3dVector(plane_points) + pcd_plane.paint_uniform_color([1, 0, 0]) # 红色 + + o3d.visualization.draw_geometries( + [pcd, pcd_plane], + window_name="Plane Fitting", + width=1024, + height=768 + ) + + +# ========== 主程序 ========== +if __name__ == "__main__": + # 参数设置 + size_mm = 1000 + noise_std_mm = 0.5 + num_points = 100000 + + print(f"参数:") + print(f" 正方形边长: {size_mm} mm") + print(f" 噪声标准差: {noise_std_mm} mm") + print(f" 点数: {num_points}") + + # 生成点云 + points = create_square_plane_with_noise(size_mm, noise_std_mm, num_points) + + # 检查数据范围 + print(f"\n数据范围:") + print(f" X: [{points[:, 0].min():.3f}, {points[:, 0].max():.3f}]") + print(f" Y: [{points[:, 1].min():.3f}, {points[:, 1].max():.3f}]") + print(f" Z: [{points[:, 2].min():.6f}, {points[:, 2].max():.6f}]") + + # 使用 Open3D RANSAC + plane, inliers = estimate_plane_ransac_open3d(points, threshold_mm=0.5, max_iterations=2000) + + # 可视化 + visualize_plane_fitting(points, plane) \ No newline at end of file diff --git a/alg/point_porj/archive/Point_Fitness.py b/alg/point_porj/archive/Point_Fitness.py new file mode 100644 index 0000000..2543e7c --- /dev/null +++ b/alg/point_porj/archive/Point_Fitness.py @@ -0,0 +1,246 @@ +import copy +import argparse +import os +from scipy.spatial.transform import Rotation as R +import trimesh +import numpy as np +import open3d as o3d +import time + + +def rotation_matrix_scipy(roll_deg=10, pitch_deg=10, yaw_deg=10): + """ + 使用 scipy 生成绕 XYZ 轴旋转的矩阵 + """ + r = R.from_euler('xyz', [roll_deg, pitch_deg, yaw_deg], degrees=True) + return r.as_matrix() + + +def random_rotation_matrix(): + """生成随机旋转矩阵""" + angle = np.random.uniform(0, 2 * np.pi) + axis = np.random.randn(3) + axis = axis / np.linalg.norm(axis) + + K = np.array([ + [0, -axis[2], axis[1]], + [axis[2], 0, -axis[0]], + [-axis[1], axis[0], 0] + ]) + R = np.eye(3) + np.sin(angle) * K + (1 - np.cos(angle)) * K @ K + return R + + +def random_translation_vector(range_xyz=0.5): + """生成随机平移向量""" + return np.random.uniform(-range_xyz, range_xyz, 3) + + +def transform_point_cloud(pcd, R, t): + """对点云应用变换""" + transformed_pcd = copy.deepcopy(pcd) + points = np.asarray(transformed_pcd.points) + points_transformed = (R @ points.T).T + t + transformed_pcd.points = o3d.utility.Vector3dVector(points_transformed) + return transformed_pcd + + +def registration_open3d(source_pcd, target_pcd, voxel_size=0.005): + """ + 点云配准（RANSAC + ICP） + + Returns: + transformation: 4x4 变换矩阵 + fitness: 配准 fitness 分数 + rmse: 配准 RMSE + """ + # 1. 下采样 + source_down = source_pcd.voxel_down_sample(voxel_size) + target_down = target_pcd.voxel_down_sample(voxel_size) + + # 2. 估计法线 + radius = voxel_size * 2 + source_down.estimate_normals(o3d.geometry.KDTreeSearchParamHybrid(radius=radius, max_nn=30)) + target_down.estimate_normals(o3d.geometry.KDTreeSearchParamHybrid(radius=radius, max_nn=30)) + + # 3. 计算 FPFH 特征 + radius_feature = voxel_size * 5 + source_fpfh = o3d.pipelines.registration.compute_fpfh_feature( + source_down, o3d.geometry.KDTreeSearchParamHybrid(radius=radius_feature, max_nn=100)) + target_fpfh = o3d.pipelines.registration.compute_fpfh_feature( + target_down, o3d.geometry.KDTreeSearchParamHybrid(radius=radius_feature, max_nn=100)) + + # 4. RANSAC 粗配准 + distance_threshold = voxel_size * 1.5 + result_ransac = o3d.pipelines.registration.registration_ransac_based_on_feature_matching( + source_down, target_down, source_fpfh, target_fpfh, + mutual_filter=True, + max_correspondence_distance=distance_threshold, + estimation_method=o3d.pipelines.registration.TransformationEstimationPointToPoint(), + ransac_n=3, + checkers=[ + o3d.pipelines.registration.CorrespondenceCheckerBasedOnEdgeLength(0.9), + o3d.pipelines.registration.CorrespondenceCheckerBasedOnDistance(distance_threshold) + ], + criteria=o3d.pipelines.registration.RANSACConvergenceCriteria(100000, 0.999)) + + print(f"RANSAC 配准完成") + source_pcd.estimate_normals(o3d.geometry.KDTreeSearchParamHybrid(radius=0.1, max_nn=30)) + target_pcd.estimate_normals(o3d.geometry.KDTreeSearchParamHybrid(radius=0.1, max_nn=30)) + + # 5. ICP 精配准 + result_icp = o3d.pipelines.registration.registration_icp( + source_pcd, target_pcd, distance_threshold / 2, result_ransac.transformation, + o3d.pipelines.registration.TransformationEstimationPointToPlane(), + o3d.pipelines.registration.ICPConvergenceCriteria(max_iteration=200)) + + print(f"ICP 精配准完成") + print(f" Fitness: {result_icp.fitness:.4f}") + print(f" RMSE: {result_icp.inlier_rmse:.6f} m") + + return result_icp.transformation, result_icp.fitness, result_icp.inlier_rmse + + +def visualize_registration_result(original_pcd, transformed_pcd, registered_pcd): + """可视化原始点云、变换后点云和配准后点云""" + source_colored = copy.deepcopy(original_pcd) + target_colored = copy.deepcopy(transformed_pcd) + registered_colored = copy.deepcopy(registered_pcd) + + source_colored.paint_uniform_color([0, 0, 1]) # 蓝色 - 原始点云 + target_colored.paint_uniform_color([0, 1, 0]) # 绿色 - 目标点云 + registered_colored.paint_uniform_color([1, 0, 0]) # 红色 - 配准后点云 + + o3d.visualization.draw_geometries( + [source_colored, target_colored], + window_name="Before Registration (Blue: Source, Green: Target)", + width=1024, height=768 + ) + + o3d.visualization.draw_geometries( + [registered_colored, target_colored], + window_name="After Registration (Red: Registered, Green: Target)", + width=1024, height=768 + ) + + +def visualize_registration_comparison(original_pcd, transformed_pcd, registered_pcd): + """并排可视化配准前后的对比""" + source_before = copy.deepcopy(original_pcd) + source_before.paint_uniform_color([0, 0, 1]) # 蓝色 + + target_before = copy.deepcopy(transformed_pcd) + target_before.paint_uniform_color([0, 1, 0]) # 绿色 + + registered_after = copy.deepcopy(registered_pcd) + registered_after.paint_uniform_color([1, 0, 0]) # 红色 + + o3d.visualization.draw_geometries( + [source_before, target_before], + window_name="Before Registration (Blue: Source, Green: Target)", + width=1024, height=768 + ) + + o3d.visualization.draw_geometries( + [registered_after, target_before], + window_name="After Registration (Red: Registered, Green: Target)", + width=1024, height=768 + ) + + +def compute_point_to_mesh_distance_trimesh(points, mesh_path): + """使用 trimesh 计算点云到 Mesh 的距离""" + mesh_trimesh = trimesh.load(mesh_path) + closest_points, distances, triangle_ids = mesh_trimesh.nearest.on_surface(points) + return distances + + +# ========== 主程序 ========== +def main(): + parser = argparse.ArgumentParser(description='点云配准与 Mesh 距离计算') + parser.add_argument('--ply_path', '-p', type=str, default="./High_Speed_Data/Noise_PointCloud/11_noisy.ply", + help='输入点云文件路径 (.ply)') + parser.add_argument('--mesh_path', '-m', type=str, default="./High_Speed_Data/Mesh/11.stl", + help='输入 Mesh 文件路径 (.stl, .ply, .obj 等)') + parser.add_argument('--voxel_size', '-v', type=float, default=10, + help='体素滤波大小 (默认: 10)') + parser.add_argument('--roll', type=float, default=10, + help='绕 X 轴旋转角度 (度, 默认: 10)') + parser.add_argument('--pitch', type=float, default=10, + help='绕 Y 轴旋转角度 (度, 默认: 10)') + parser.add_argument('--yaw', type=float, default=10, + help='绕 Z 轴旋转角度 (度, 默认: 10)') + parser.add_argument('--trans_range', type=float, default=0, + help='平移范围 (米, 默认: 0)') + parser.add_argument('--no_visualize', action='store_true', + help='不显示可视化窗口') + + args = parser.parse_args() + + # 检查文件是否存在 + if not os.path.exists(args.ply_path): + print(f"错误: 点云文件不存在 - {args.ply_path}") + return + + if not os.path.exists(args.mesh_path): + print(f"错误: Mesh 文件不存在 - {args.mesh_path}") + return + + # 1. 读取原始点云 + print(f"读取点云: {args.ply_path}") + source_pcd = o3d.io.read_point_cloud(args.ply_path) + + if len(source_pcd.points) == 0: + print("错误: 点云读取失败") + return + + print(f"原始点云点数: {len(source_pcd.points)}") + + # 2. 生成变换 + R = rotation_matrix_scipy(args.roll, args.pitch, args.yaw) + t = random_translation_vector(range_xyz=args.trans_range) + + print(f"旋转矩阵:\n{R}") + print(f"平移向量: {t}") + + # 3. 创建目标点云（变换后的源点云） + target_pcd = transform_point_cloud(source_pcd, R, t) + + # 4. 点云配准 + print("\n开始点云配准...") + t0 = time.time() + transformation, fitness, rmse = registration_open3d(source_pcd, target_pcd, voxel_size=args.voxel_size) + + # 5. 将目标点云变换回源点云坐标系 + inv_transformation = np.linalg.inv(transformation) + target_recovered = copy.deepcopy(target_pcd) + target_recovered.transform(inv_transformation) + + t1 = time.time() + print(f"\n配准结果:") + print(f" Fitness: {fitness:.4f}") + print(f" RMSE: {rmse:.6f} m") + print(f" 估计的变换矩阵:\n{transformation}") + print(f"配准耗时: {t1 - t0:.4f} 秒") + + # 6. 计算点到 Mesh 的距离 + print("\n计算点到 Mesh 距离...") + t2 = time.time() + points = np.asarray(target_recovered.points) + distances = compute_point_to_mesh_distance_trimesh(points, args.mesh_path) + t3 = time.time() + + print(f"距离统计:") + print(f" 平均距离: {np.mean(distances):.6f} mm ({np.mean(distances) * 1000:.3f} mm)") + print(f" 最大距离: {np.max(distances):.6f} mm ({np.max(distances) * 1000:.3f} mm)") + print(f" RMSE: {np.sqrt(np.mean(distances ** 2)):.6f} m") + print(f"距离计算耗时: {t3 - t2:.4f} 秒") + + # 7. 可视化 + if not args.no_visualize: + print("\n开始可视化...") + visualize_registration_comparison(source_pcd, target_pcd, target_recovered) + + +if __name__ == "__main__": + main() \ No newline at end of file diff --git a/alg/point_porj/archive/Readme.md b/alg/point_porj/archive/Readme.md new file mode 100644 index 0000000..288e4a7 --- /dev/null +++ b/alg/point_porj/archive/Readme.md @@ -0,0 +1,270 @@ +# 工业零件几何测量工具使用指南 +> 支持测量种类：平整度、平面度、轮廓测量、曲率分析、点云去噪、平面拟合、点云配准评估 +> 支持测量任务编号：11、13、17、20、25、26、27、31、34、35、36、37、38、39、40、41、42、45 + +## 一、环境配置 +### 基础环境要求 +- Python 3.10.8 +- CUDA 11.8（建议，无CUDA时自动降级为CPU计算） +- PyTorch 2.2 + +### 依赖安装 +```bash +conda install -f requirements.txt +``` +> 注：需确保环境中已安装OCC Core（STEP/STL处理）、trimesh、open3d、numpy、scipy等依赖 + +--- + +## 二、CAD转轻量化Mesh（CAD2Mesh.py） +### 功能说明 +将STEP格式的CAD模型转换为轻量化STL网格文件，通过调整网格精度控制文件体积（精度值越大，文件越小，精度越低），纯后台处理无界面显示，转换效率高。 + +### 输入输出 +- **输入**：STEP格式CAD文件（路径示例：`High_Speed_Data/CAD/11.step`） +- **输出**：STL格式轻量化网格文件（路径示例：`High_Speed_Data/Mesh/11.stl`） + +### 命令行参数说明 +| 参数 | 简写 | 类型 | 默认值 | 说明 | +|--------------|------|--------|---------------------------------|----------------------------------------| +| --precision | -p | float | 1.0 | 网格精度（单位：mm），值越大文件体积越小 | +| --input | -i | string | High_Speed_Data/CAD/11.step | 输入STEP文件路径 | +| --output | -o | string | High_Speed_Data/Mesh/11.stl | 输出STL文件路径 | + +### 运行示例 +```bash +# 完整参数形式 +python CAD2Mesh.py --precision 1 --input "High_Speed_Data/CAD/11.step" --output "High_Speed_Data/Mesh/11.stl" + +# 简写参数形式 +python CAD2Mesh.py -p 1 -i "High_Speed_Data/CAD/11.step" -o "High_Speed_Data/Mesh/11.stl" +``` + +### 运行日志说明 +程序会输出三步处理日志及耗时： +1. STEP文件读取阶段 +2. 网格离散化（轻量化）阶段 +3. STL文件保存阶段 +转换完成后会提示总耗时及STL文件打开工具建议（FreeCAD/MeshLab/3D Viewer）。 + +--- + +## 三、轮廓-平整度测量（Contour_Flatness.py） +### 功能说明 +高精度计算点云到 CAD STEP 模型三角化表面的最近距离，输出平均距离（mean_dist）、最大距离（max_dist）和最小距离（min_dist），用于评估零件的平整度、平面度、轮廓偏差等几何精度指标；支持 CUDA 加速（自动检测），采用 torch.float64 高精度计算。 + +脚本现在支持两种模式： +- 默认批量模式：按任务编号从 `High_Speed_Data/CAD/` 和点云目录中读取多组数据，并输出 Excel 汇总。 +- 单任务模式：添加 `--single` 后，使用 `--CAD` 和 `--pointcloud` 计算一组文件，可选 `--visualize` 显示热力图。 + +### 输入输出 +- **输入**： + - CAD STEP 文件：路径示例 `High_Speed_Data/CAD/11.step` + - 点云 PLY 文件：批量模式默认读取 `High_Speed_Data/Noise_PointCloud/{id}_noisy.ply`，也可用 `--pointcloud_kind clean` 读取 `High_Speed_Data/PointCloud/{id}.ply` +- **输出**： + - 平均距离（mean_dist）：点云中所有点到 CAD 三角面的平均最近距离（单位：mm） + - 最大距离（max_dist）：点云中所有点到 CAD 三角面的最大最近距离（单位：mm） + - 最小距离（min_dist）：点云中所有点到 CAD 三角面的最小最近距离（单位：mm） + - 批量模式会生成 Excel 汇总文件，默认 `平整度.xlsx` + +### 命令行参数说明 +| 参数 | 简写 | 类型 | 默认值 | 说明 | +|------|------|------|--------|------| +| --CAD | -c | string | High_Speed_Data/CAD/11.step | 单任务模式 CAD STEP 文件路径，仅 `--single` 时使用 | +| --pointcloud | -p | string | High_Speed_Data/Noise_PointCloud/11_noisy.ply | 单任务模式点云路径，仅 `--single` 时使用 | +| --tasks | - | string | 11,13,17,20,25,26,27,31,34,35,36,37,38,39,40,41,42,45 | 批量任务编号，多个编号用英文逗号分隔 | +| --data_root | - | string | High_Speed_Data | 批量任务数据根目录 | +| --pointcloud_kind | - | string | noise | 批量模式点云来源：`noise` 读取 Noise_PointCloud，`clean` 读取 PointCloud | +| --excel_output | - | string | 平整度.xlsx | 批量结果 Excel 输出路径 | +| --single | - | flag | false | 启用单任务模式 | +| --visualize | - | flag | false | 单任务模式下显示距离热力图 | +| --mesh_deflection | - | float | 0.1 | CAD 三角化精度，越小越精细但越慢 | +| --point_chunk_size | - | int | 2048 | 点云分块大小，显存/内存不足时可调小 | +| --triangle_chunk_size | - | int | 4096 | 三角面分块大小，显存/内存不足时可调小 | + +### 运行示例 +```bash +# 默认批量模式，使用 DEFAULT_TASK_IDS 并输出 平整度.xlsx +python Contour_Flatness.py + +# 指定批量任务、点云来源和 Excel 输出 +python Contour_Flatness.py --tasks "11,31,40" --data_root "High_Speed_Data" --pointcloud_kind noise --excel_output "vis/flatness.xlsx" + +# 单任务模式 +python Contour_Flatness.py --single --CAD "High_Speed_Data/CAD/11.step" --pointcloud "High_Speed_Data/Noise_PointCloud/11_noisy.ply" + +# 单任务模式并显示热力图 +python Contour_Flatness.py --single -c "High_Speed_Data/CAD/11.step" -p "High_Speed_Data/PointCloud/11.ply" --visualize +``` + +### 计算说明 +1. CAD 读取：通过 pythonocc-core 读取 STEP，并使用 `mesh_deflection` 三角化。 +2. 点云读取：优先使用 open3d（若安装），否则使用 trimesh 兼容读取。 +3. 距离计算：点云和 CAD 三角面都分块处理，避免内存/显存溢出，支持大规模点云计算。 +4. 批量结果：每个任务会记录成功/失败状态，成功任务会把 `points_np` 和 `distances_np` 缓存在 `task_distance_data` 中，便于后续可视化。 +5. 精度保障：采用 torch.float64 高精度计算，支持 GPU/CPU 自动切换。 + +--- + +## 四、点云曲率估计（curvature_estimation.py） +### 功能说明 +基于Open3D对工业零件点云进行曲率计算，通过邻域协方差矩阵分解获取曲率值，支持曲率彩色可视化，用于识别零件边缘、棱角、曲面等特征区域，为表面缺陷检测提供依据。 + +### 输入输出 +- **输入**：PLY格式点云文件（路径示例：`High_Speed_Data/PointCloud/11.ply`） +- **输出**：曲率统计值、彩色可视化点云、可保存带曲率信息的PLY文件 + +### 命令行参数说明 +| 参数 | 简写 | 类型 | 默认值 | 说明 | +|--------------|------|--------|---------------------------------|----------------------------------------| +| --ply_path | — | string | ./High_Speed_Data/PointCloud/11.ply | 输入点云路径 | +| --radius | — | float | 2.0 | 邻域搜索半径 | +| --max_nn | — | int | 30 | 最大邻域点数 | +| --color_scale| — | float | 3.0 | 曲率颜色缩放因子 | +| --save | — | string | None | 保存着色点云路径（可选）| + +### 运行示例 +```bash +# 基础运行 +python curvature_estimation.py --ply_path "High_Speed_Data/PointCloud/11.ply" + +# 保存结果 +python curvature_estimation.py --ply_path "High_Speed_Data/PointCloud/11.ply" --save "High_Speed_Data/Result/11_curvature.ply" +``` + +### 计算说明 +1. 自动估计法线并构建KD-Tree加速邻域搜索 +2. 邻域点不足3个时自动标记为高曲率 +3. 颜色映射：红色=高曲率，绿色=低曲率 + +--- + +## 五、点云去噪处理（denoise.py） +### 功能说明 +集成SOR、ROR、PF、VGF、BF五种去噪与滤波算法，自动添加高斯噪声并对比效果，通过Chamfer Distance量化评估去噪质量，用于扫描点云预处理，提升测量精度。 + +### 输入输出 +- **输入**：PLY格式干净点云（路径示例：`High_Speed_Data/PointCloud/11.ply`） +- **输出**：各算法去噪指标、保留率、移除点数、CD距离、可视化对比 + +### 命令行参数说明 +| 参数 | 简写 | 类型 | 默认值 | 说明 | +|--------------|------|--------|---------------------------------|----------------------------------------| +| --ply_path | -p | string | ./High_Speed_Data/PointCloud/11.ply | 输入点云路径 | +| --noise_std | -n | float | 4.0 | 高斯噪声标准差 | +| --voxel_size | -v | float | 0.05 | 体素滤波大小 | +| --no_visualize | — | bool | False | 关闭可视化窗口 | + +### 运行示例 +```bash +# 基础去噪对比 +python denoise.py -p "High_Speed_Data/PointCloud/11.ply" --noise_std 2.0 + +# 无可视化运行 +python denoise.py -p "High_Speed_Data/PointCloud/11.ply" --no_visualize +``` + +### 计算说明 +1. 自动给点云加噪，模拟真实扫描噪声 +2. 依次运行SOR/ROR/PF/VGF/BF算法 +3. 输出保留率、移除点数、Chamfer Distance评估结果 + +--- + +## 六、平面拟合估计（plane_estimation.py） +### 功能说明 +基于Open3D内置RANSAC算法自动拟合点云中平面结构，输出平面方程、内点占比、法线误差、拟合RMSE，用于提取零件基准平面。 + +### 输入输出 +- **输入**：可自定义生成带噪平面点云 +- **输出**：平面方程、误差统计、拟合可视化 + +### 命令行参数说明 +| 参数 | 简写 | 类型 | 默认值 | 说明 | +|----------------|------|--------|--------|--------------------------| +| --threshold_mm | -t | float | 0.5 | 平面距离阈值（mm）| +| --max_iterations | -i | int | 2000 | RANSAC迭代次数 | + +### 运行示例 +```bash +# 默认运行 +python plane_estimation.py + +# 自定义参数 +python plane_estimation.py --threshold_mm 0.3 --max_iterations 1500 +``` + +### 计算说明 +1. 支持生成带噪声标准平面用于测试 +2. 输出平面法向量与理想Z轴夹角误差、偏移误差、拟合RMSE + +--- + +## 七、点云配准与适应度评估（point-fitness.py） +### 功能说明 +对点云执行旋转/平移变换，通过RANSAC+ICP完成高精度配准，输出配准适应度Fitness、RMSE，并计算配准后点云到参考网格的距离，用于评估配准精度与零件偏差。 + +### 输入输出 +- **输入**：待配准PLY点云、参考STL网格 +- **输出**：变换矩阵、配准分数、误差、点云到网格距离统计、配准可视化 + +### 命令行参数说明 +| 参数 | 简写 | 类型 | 默认值 | 说明 | +|--------------|------|--------|---------------------------------------------|--------------------| +| --ply_path | -p | string | ./High_Speed_Data/Noise_PointCloud/11_noisy.ply | 点云路径 | +| --mesh_path | -m | string | ./High_Speed_Data/Mesh/11.stl | 网格路径 | +| --voxel_size | -v | float | 10 | 配准下采样大小 | +| --no_visualize | — | bool | False | 关闭可视化 | + +### 运行示例 +```bash +# 完整配准+距离计算 +python point-fitness.py -p "High_Speed_Data/Noise_PointCloud/11_noisy.ply" -m "High_Speed_Data/Mesh/11.stl" -v 5 + +# 无可视化 +python point-fitness.py -p "High_Speed_Data/Noise_PointCloud/11_noisy.ply" -m "High_Speed_Data/Mesh/11.stl" --no_visualize +``` + +### 计算说明 +1. 支持指定XYZ三轴旋转与随机平移 +2. RANSAC粗配准 → ICP精配准 +3. 输出Fitness（越高越好）、RMSE（越低越好） + +--- + +## 八、完整工作流程示例（任务11全流程） +1. CAD转轻量化网格 +```bash +python CAD2Mesh.py -p 0.5 -i "High_Speed_Data/CAD/11.step" -o "High_Speed_Data/Mesh/11.stl" +``` +2. 点云去噪预处理 +```bash +python denoise.py -p "High_Speed_Data/PointCloud/11.ply" --noise_std 2.0 +``` +3. 平面基准拟合 +```bash +python plane_estimation.py +``` +4. 曲率特征分析 +```bash +python curvature_estimation.py --ply_path "High_Speed_Data/PointCloud/11.ply" +``` +5. 轮廓-平整度测量 +```bash +python Contour_Flatness.py --tasks "11,31,40" --data_root "High_Speed_Data" --excel_output "vis/flatness.xlsx" +``` +6. 点云配准评估 +```bash +python point-fitness.py -p "High_Speed_Data/Noise_PointCloud/11_noisy.ply" -m "High_Speed_Data/Mesh/11.stl" +``` + +--- + +## 九、常见问题 +1. STEP文件读取失败：检查路径、文件完整性、OCC Core安装 +2. 点云读取报错：安装open3d（`pip install open3d`） +3. CUDA Out of Memory：减小分块大小或降低点云密度 +4. 去噪效果差：调小SOR标准差、增大ROR半径 +5. 平面拟合失败：提高RANSAC迭代次数、降低距离阈值 +6. 配准Fitness低：减小voxel_size，降低旋转/平移幅度 +7. 可视化无窗口：服务器环境加`--no_visualize`参数 diff --git a/alg/point_porj/archive/generate_shifted_train_pointcloud.py b/alg/point_porj/archive/generate_shifted_train_pointcloud.py new file mode 100644 index 0000000..13abdde --- /dev/null +++ b/alg/point_porj/archive/generate_shifted_train_pointcloud.py @@ -0,0 +1,447 @@ +import os +import argparse +import numpy as np +import open3d as o3d + +from OCC.Core.STEPControl import STEPControl_Reader +from OCC.Core.IFSelect import IFSelect_RetDone +from OCC.Core.BRepMesh import BRepMesh_IncrementalMesh +from OCC.Core.TopExp import TopExp_Explorer +from OCC.Core.TopAbs import TopAbs_FACE +from OCC.Core.BRep import BRep_Tool + +try: + from OCC.Core.TopoDS import topods + USE_TOPODS = "topods" +except ImportError: + from OCC.Core.TopoDS import TopoDS + USE_TOPODS = "TopoDS" + + +# ============================================================ +# STEP -> Open3D Mesh +# ============================================================ + +def read_step_file(step_path): + reader = STEPControl_Reader() + status = reader.ReadFile(step_path) + + if status != IFSelect_RetDone: + raise RuntimeError(f"STEP 文件读取失败: {step_path}") + + reader.TransferRoots() + shape = reader.OneShape() + + print("[OK] STEP loaded:", step_path) + return shape + + +def occ_shape_to_numpy_mesh(shape): + vertices = [] + triangles = [] + + explorer = TopExp_Explorer(shape, TopAbs_FACE) + + face_count = 0 + tri_count = 0 + + while explorer.More(): + current_shape = explorer.Current() + + if USE_TOPODS == "topods": + face = topods.Face(current_shape) + else: + face = TopoDS.Face(current_shape) + + location = face.Location() + triangulation = BRep_Tool.Triangulation(face, location) + + if triangulation is None: + explorer.Next() + continue + + trsf = location.Transformation() + base_index = len(vertices) + + for i in range(1, triangulation.NbNodes() + 1): + pnt = triangulation.Node(i) + pnt.Transform(trsf) + + vertices.append([ + pnt.X(), + pnt.Y(), + pnt.Z() + ]) + + for i in range(1, triangulation.NbTriangles() + 1): + tri = triangulation.Triangle(i) + n1, n2, n3 = tri.Get() + + triangles.append([ + base_index + n1 - 1, + base_index + n2 - 1, + base_index + n3 - 1 + ]) + + tri_count += 1 + + face_count += 1 + explorer.Next() + + vertices = np.asarray(vertices, dtype=np.float64) + triangles = np.asarray(triangles, dtype=np.int32) + + print("[OK] CAD mesh extracted") + print(" CAD faces:", face_count) + print(" mesh vertices:", vertices.shape) + print(" mesh triangles:", triangles.shape) + + if vertices.shape[0] == 0 or triangles.shape[0] == 0: + raise RuntimeError("CAD 三角网格提取失败") + + return vertices, triangles + + +def step_to_open3d_mesh(step_path, mesh_deflection=0.1): + shape = read_step_file(step_path) + + print("[INFO] Meshing CAD...") + mesh_algo = BRepMesh_IncrementalMesh(shape, mesh_deflection) + mesh_algo.Perform() + + vertices, triangles = occ_shape_to_numpy_mesh(shape) + + mesh = o3d.geometry.TriangleMesh() + mesh.vertices = o3d.utility.Vector3dVector(vertices) + mesh.triangles = o3d.utility.Vector3iVector(triangles) + + mesh.remove_duplicated_vertices() + mesh.remove_duplicated_triangles() + mesh.remove_degenerate_triangles() + mesh.compute_vertex_normals() + + return mesh + + +# ============================================================ +# 点云处理 +# ============================================================ + +def make_translation_matrix(dx, dy, dz): + T = np.eye(4, dtype=np.float64) + T[0, 3] = dx + T[1, 3] = dy + T[2, 3] = dz + return T + + +def crop_front_tail_by_x(pcd, ratio=0.08): + """ + 按 X 轴裁剪车头和车尾。 + + front: X 最小端 + tail: X 最大端 + """ + + points = np.asarray(pcd.points) + + min_bound = points.min(axis=0) + max_bound = points.max(axis=0) + + x_min = min_bound[0] + x_max = max_bound[0] + x_len = x_max - x_min + + front_min = min_bound.copy() + front_max = max_bound.copy() + front_min[0] = x_min + front_max[0] = x_min + ratio * x_len + + tail_min = min_bound.copy() + tail_max = max_bound.copy() + tail_min[0] = x_max - ratio * x_len + tail_max[0] = x_max + + front_box = o3d.geometry.AxisAlignedBoundingBox( + min_bound=front_min, + max_bound=front_max + ) + + tail_box = o3d.geometry.AxisAlignedBoundingBox( + min_bound=tail_min, + max_bound=tail_max + ) + + front_pcd = pcd.crop(front_box) + tail_pcd = pcd.crop(tail_box) + + print("[OK] Crop front/tail by X") + print(" X range:", x_min, x_max) + print(" front points:", np.asarray(front_pcd.points).shape) + print(" tail points:", np.asarray(tail_pcd.points).shape) + + return front_pcd, tail_pcd + + +def add_gaussian_noise(pcd, noise_std=0.0): + """ + 给点云添加高斯噪声。 + noise_std=0 表示不加噪声。 + """ + + if noise_std <= 0: + return pcd + + points = np.asarray(pcd.points) + noise = np.random.normal( + loc=0.0, + scale=noise_std, + size=points.shape + ) + + noisy_points = points + noise + + noisy_pcd = o3d.geometry.PointCloud() + noisy_pcd.points = o3d.utility.Vector3dVector(noisy_points) + + if pcd.has_colors(): + noisy_pcd.colors = pcd.colors + + if pcd.has_normals(): + noisy_pcd.normals = pcd.normals + + return noisy_pcd + + +def visualize_pair(cad_pcd, shifted_pcd): + cad_vis = o3d.geometry.PointCloud(cad_pcd) + shifted_vis = o3d.geometry.PointCloud(shifted_pcd) + + cad_vis.paint_uniform_color([0.0, 0.5, 1.0]) + shifted_vis.paint_uniform_color([1.0, 0.0, 0.0]) + + o3d.visualization.draw_geometries( + [cad_vis, shifted_vis], + window_name="Blue = CAD sampled point cloud, Red = shifted test point cloud" + ) + + +# ============================================================ +# 主函数 +# ============================================================ + +def generate_shifted_train_pointcloud( + cad_path, + output_dir, + sample_points=1000000, + mesh_deflection=0.1, + crop_ratio=0.08, + dx=100.0, + dy=20.0, + dz=-10.0, + noise_std=0.0, + visualize=True +): + os.makedirs(output_dir, exist_ok=True) + + # 1. CAD -> mesh + cad_mesh = step_to_open3d_mesh( + cad_path, + mesh_deflection=mesh_deflection + ) + + # 2. mesh -> 点云 + print(f"[INFO] Sampling CAD surface: {sample_points} points") + cad_pcd = cad_mesh.sample_points_uniformly( + number_of_points=sample_points + ) + + cad_pcd_path = os.path.join(output_dir, "cad_sampled_whole.ply") + o3d.io.write_point_cloud(cad_pcd_path, cad_pcd) + print("[OK] Saved:", cad_pcd_path) + + # 3. 裁剪原始 CAD 点云的车头 / 车尾 + cad_front, cad_tail = crop_front_tail_by_x( + cad_pcd, + ratio=crop_ratio + ) + + o3d.io.write_point_cloud( + os.path.join(output_dir, "cad_front.ply"), + cad_front + ) + + o3d.io.write_point_cloud( + os.path.join(output_dir, "cad_tail.ply"), + cad_tail + ) + + # 4. 生成固定平移矩阵 + T = make_translation_matrix(dx, dy, dz) + + print("\n========== Fixed Translation ==========") + print("dx:", dx) + print("dy:", dy) + print("dz:", dz) + print("T:") + print(T) + + # 5. 对整车点云施加固定平移 + shifted_pcd = o3d.geometry.PointCloud(cad_pcd) + shifted_pcd.transform(T) + + # 6. 可选：加噪声，模拟真实扫描点云 + shifted_pcd = add_gaussian_noise( + shifted_pcd, + noise_std=noise_std + ) + + shifted_path = os.path.join(output_dir, "shifted_whole_pointcloud.ply") + o3d.io.write_point_cloud(shifted_path, shifted_pcd) + print("[OK] Saved:", shifted_path) + + # 7. 裁剪平移后的车头 / 车尾 + shifted_front, shifted_tail = crop_front_tail_by_x( + shifted_pcd, + ratio=crop_ratio + ) + + shifted_front_path = os.path.join(output_dir, "shifted_front.ply") + shifted_tail_path = os.path.join(output_dir, "shifted_tail.ply") + + o3d.io.write_point_cloud(shifted_front_path, shifted_front) + o3d.io.write_point_cloud(shifted_tail_path, shifted_tail) + + print("[OK] Saved:", shifted_front_path) + print("[OK] Saved:", shifted_tail_path) + + # 8. 保存真实位移矩阵 + matrix_path = os.path.join(output_dir, "GT_translation_matrix.txt") + np.savetxt(matrix_path, T, fmt="%.10f") + print("[OK] Saved:", matrix_path) + + # 9. 保存真实位移向量 + translation_path = os.path.join(output_dir, "GT_translation_vector.txt") + np.savetxt( + translation_path, + np.array([dx, dy, dz], dtype=np.float64), + fmt="%.10f" + ) + print("[OK] Saved:", translation_path) + + # 10. 可视化 + if visualize: + # 百万级点云显示会卡，所以这里可视化时降采样 + cad_vis = cad_pcd.voxel_down_sample(voxel_size=20.0) + shifted_vis = shifted_pcd.voxel_down_sample(voxel_size=20.0) + + visualize_pair(cad_vis, shifted_vis) + + return { + "cad_sampled_path": cad_pcd_path, + "shifted_pointcloud_path": shifted_path, + "shifted_front_path": shifted_front_path, + "shifted_tail_path": shifted_tail_path, + "translation_matrix_path": matrix_path, + "translation_vector": np.array([dx, dy, dz], dtype=np.float64) + } + + +# ============================================================ +# 命令行入口 +# ============================================================ + +if __name__ == "__main__": + parser = argparse.ArgumentParser( + description="从整车 CAD 生成测试点云，并施加固定前后/整体位移" + ) + + parser.add_argument( + "--cad", + default=r"datasets/CAD/M02车.stp", + help="整车 CAD STEP/STP 文件路径" + ) + + parser.add_argument( + "--output_dir", + default=r"datasets\shift_test_output", + help="输出目录" + ) + + parser.add_argument( + "--sample_points", + type=int, + default=1000000, + help="从 CAD 表面采样的点数，默认 1000000" + ) + + parser.add_argument( + "--mesh_deflection", + type=float, + default=0.1, + help="CAD 三角化精度，越小越精细，默认 0.1" + ) + + parser.add_argument( + "--crop_ratio", + type=float, + default=0.08, + help="车头/车尾裁剪比例，默认 0.08" + ) + + parser.add_argument( + "--dx", + type=float, + default=100.0, + help="X 方向固定平移量" + ) + + parser.add_argument( + "--dy", + type=float, + default=20.0, + help="Y 方向固定平移量" + ) + + parser.add_argument( + "--dz", + type=float, + default=-10.0, + help="Z 方向固定平移量" + ) + + parser.add_argument( + "--noise_std", + type=float, + default=0.0, + help="高斯噪声标准差，默认 0 表示不加噪声" + ) + + parser.add_argument( + "--no_visualize", + action="store_true", + help="不显示可视化窗口" + ) + + args = parser.parse_args() + + result = generate_shifted_train_pointcloud( + cad_path=args.cad, + output_dir=args.output_dir, + sample_points=args.sample_points, + mesh_deflection=args.mesh_deflection, + crop_ratio=args.crop_ratio, + dx=args.dx, + dy=args.dy, + dz=args.dz, + noise_std=args.noise_std, + visualize=not args.no_visualize + ) + + print("\n========== Result ==========") + print("CAD sampled point cloud:", result["cad_sampled_path"]) + print("Shifted whole point cloud:", result["shifted_pointcloud_path"]) + print("Shifted front point cloud:", result["shifted_front_path"]) + print("Shifted tail point cloud:", result["shifted_tail_path"]) + print("GT translation vector:", result["translation_vector"]) + print("GT matrix:", result["translation_matrix_path"]) \ No newline at end of file diff --git a/alg/point_porj/archive/relative_code/3D Douglas-Peucker.py b/alg/point_porj/archive/relative_code/3D Douglas-Peucker.py new file mode 100644 index 0000000..667c9d7 --- /dev/null +++ b/alg/point_porj/archive/relative_code/3D Douglas-Peucker.py @@ -0,0 +1,362 @@ +import numpy as np +from scipy.spatial import KDTree +import matplotlib.pyplot as plt +from mpl_toolkits.mplot3d import Axes3D + + +def point_to_line_distance(point, line_start, line_end): + """ + 计算点到三维线段的最短距离 + + Args: + point: 待计算点 (x, y, z) + line_start: 线段起点 (x, y, z) + line_end: 线段终点 (x, y, z) + + Returns: + 点到线段的垂直距离 + """ + p = np.array(point) + a = np.array(line_start) + b = np.array(line_end) + + # 计算向量 + ab = b - a + ap = p - a + + # 投影参数 t + t = np.dot(ap, ab) / np.dot(ab, ab) + + # 找到线段上的最近点 + if t <= 0: + closest = a + elif t >= 1: + closest = b + else: + closest = a + t * ab + + # 计算距离 + distance = np.linalg.norm(p - closest) + return distance + + +def point_to_plane_distance(point, plane_point1, plane_point2, plane_point3): + """ + 计算点到三维平面的距离（三点确定一个平面） + + Args: + point: 待计算点 (x, y, z) + plane_point1, plane_point2, plane_point3: 平面上的三个点 + + Returns: + 点到平面的垂直距离 + """ + p = np.array(point) + p1 = np.array(plane_point1) + p2 = np.array(plane_point2) + p3 = np.array(plane_point3) + + # 计算平面法向量 + v1 = p2 - p1 + v2 = p3 - p1 + normal = np.cross(v1, v2) + normal = normal / np.linalg.norm(normal) + + # 计算点到平面的距离 + distance = abs(np.dot(normal, p - p1)) + return distance + + +def douglas_peucker_3d(points, epsilon, use_plane=False): + """ + 3D Douglas-Peucker 算法 + + Args: + points: 三维点列表或numpy数组，形状 (N, 3) + epsilon: 容差阈值 + use_plane: 是否使用平面距离（True）还是线段距离（False） + + Returns: + 简化后的点云列表 + """ + if len(points) <= 2: + return points.tolist() if isinstance(points, np.ndarray) else points + + # 转换为列表格式 + if isinstance(points, np.ndarray): + points = points.tolist() + + # 首尾点 + start = points[0] + end = points[-1] + + # 找到距离最大的点 + max_dist = 0 + max_index = 0 + + for i in range(1, len(points) - 1): + if use_plane and len(points) > 2: + # 使用平面距离（需要至少3个点确定平面） + if i > 0 and i < len(points) - 1: + # 使用当前点、前一个点和后一个点确定平面 + plane_p1 = points[i - 1] + plane_p2 = points[i] + plane_p3 = points[i + 1] + dist = point_to_plane_distance(points[i], plane_p1, plane_p2, plane_p3) + else: + dist = point_to_line_distance(points[i], start, end) + else: + # 使用线段距离 + dist = point_to_line_distance(points[i], start, end) + + if dist > max_dist: + max_dist = dist + max_index = i + + # 递归简化 + if max_dist > epsilon: + # 保留该点，递归处理左右两部分 + left = douglas_peucker_3d(points[:max_index + 1], epsilon, use_plane) + right = douglas_peucker_3d(points[max_index:], epsilon, use_plane) + + # 合并结果（避免重复端点） + return left[:-1] + right + else: + # 只保留首尾点 + return [start, end] + + +class DouglasPeucker3D: + """ + 3D Douglas-Peucker 算法的面向对象封装 + 支持点云简化、轨迹压缩、轮廓提取等 + """ + + def __init__(self, epsilon=0.01, use_plane=False): + """ + Args: + epsilon: 容差阈值 + use_plane: 是否使用平面距离（更适合曲面点云） + """ + self.epsilon = epsilon + self.use_plane = use_plane + + def simplify(self, points): + """ + 简化点云 + + Args: + points: (N, 3) 的 numpy 数组或列表 + + Returns: + 简化后的点云 (N', 3) numpy 数组 + """ + if isinstance(points, np.ndarray): + points = points.tolist() + + simplified = douglas_peucker_3d(points, self.epsilon, self.use_plane) + return np.array(simplified) + + def simplify_trajectory(self, trajectory, time_weight=False): + """ + 专门用于轨迹简化（考虑时间维度） + + Args: + trajectory: (N, 4) 的数组 [x, y, z, t] 或 [x, y, z] + time_weight: 是否加入时间权重 + + Returns: + 简化后的轨迹 + """ + if time_weight and trajectory.shape[1] == 4: + # 时间加权：将时间作为第四维参与距离计算 + # 需要自定义距离函数 + pass + + return self.simplify(trajectory[:, :3]) + + def extract_contour(self, points, return_indices=False): + """ + 提取点云轮廓（结合法线估计） + + Args: + points: (N, 3) 点云 + return_indices: 是否返回原始点索引 + + Returns: + 轮廓点集 + """ + # 先进行粗略简化 + simplified = self.simplify(points) + + # 这里可以结合法线估计进一步提取边界点 + # 简化实现：返回简化后的点作为轮廓候选 + + if return_indices: + # 查找简化点在原始点云中的最近邻索引 + from scipy.spatial import KDTree + tree = KDTree(points) + indices = [] + for p in simplified: + _, idx = tree.query(p) + indices.append(idx) + return simplified, np.array(indices) + + return simplified + + +# ========== 示例和使用 ========== + +def generate_test_curve(n_points=100, noise=0.02): + """ + 生成测试用的三维曲线（螺旋线 + 噪声） + """ + t = np.linspace(0, 4 * np.pi, n_points) + x = np.sin(t) + y = np.cos(t) + z = t / (2 * np.pi) + + points = np.column_stack([x, y, z]) + + # 添加噪声 + if noise > 0: + points += np.random.randn(n_points, 3) * noise + + return points + + +def visualize_simplification(original, simplified, title="3D Douglas-Peucker Simplification"): + """ + 可视化原始点云和简化后的点云 + """ + fig = plt.figure(figsize=(12, 6)) + + # 原始点云 + ax1 = fig.add_subplot(121, projection='3d') + ax1.scatter(original[:, 0], original[:, 1], original[:, 2], + c='blue', s=10, alpha=0.6, label='Original') + ax1.plot(original[:, 0], original[:, 1], original[:, 2], + 'b-', alpha=0.3) + ax1.set_title(f'Original: {len(original)} points') + ax1.legend() + + # 简化后点云 + ax2 = fig.add_subplot(122, projection='3d') + ax2.scatter(simplified[:, 0], simplified[:, 1], simplified[:, 2], + c='red', s=30, alpha=0.8, label='Simplified') + ax2.plot(simplified[:, 0], simplified[:, 1], simplified[:, 2], + 'r-', linewidth=2) + ax2.set_title(f'Simplified: {len(simplified)} points (ε={epsilon})') + ax2.legend() + + plt.suptitle(title) + plt.tight_layout() + plt.show() + + +def compare_epsilon(points, epsilons): + """ + 比较不同 epsilon 值的简化效果 + """ + results = {} + for eps in epsilons: + dp = DouglasPeucker3D(epsilon=eps) + simplified = dp.simplify(points) + results[eps] = { + 'points': simplified, + 'count': len(simplified), + 'compression_ratio': len(simplified) / len(points) + } + print(f"ε={eps:.4f}: {len(simplified)} 点 (压缩率: {results[eps]['compression_ratio']:.2%})") + + return results + + +def demo_point_cloud(): + """ + 点云简化演示（使用螺旋线模拟） + """ + print("=" * 50) + print("3D Douglas-Peucker 算法演示") + print("=" * 50) + + # 生成测试数据 + points = generate_test_curve(200, noise=0.01) + print(f"\n原始点云: {len(points)} 个点") + + # 测试不同 epsilon + epsilons = [0.01, 0.03, 0.05, 0.1, 0.2] + print("\n不同容差阈值的简化效果:") + results = compare_epsilon(points, epsilons) + + # 可视化 + dp = DouglasPeucker3D(epsilon=0.05) + simplified = dp.simplify(points) + visualize_simplification(points, simplified, + f"3D DP Simplification (ε=0.05, {len(simplified)}/{len(points)} points)") + + +def demo_trajectory(): + """ + 轨迹简化演示（模拟移动轨迹） + """ + print("\n" + "=" * 50) + print("3D 轨迹简化演示") + print("=" * 50) + + # 生成测试轨迹（随机路径） + np.random.seed(42) + n_points = 100 + t = np.linspace(0, 10, n_points) + x = np.sin(t) + np.random.randn(n_points) * 0.02 + y = np.cos(t) + np.random.randn(n_points) * 0.02 + z = t / 5 + np.random.randn(n_points) * 0.01 + + trajectory = np.column_stack([x, y, z]) + print(f"原始轨迹: {len(trajectory)} 个点") + + # 简化轨迹 + dp = DouglasPeucker3D(epsilon=0.08) + simplified = dp.simplify_trajectory(trajectory) + print(f"简化后轨迹: {len(simplified)} 个点 (压缩率: {len(simplified) / len(trajectory):.2%})") + + # 可视化 + visualize_simplification(trajectory, simplified, "3D Trajectory Simplification") + + +def demo_point_cloud_from_file(pcd_file=None): + """ + 从文件加载点云并简化 + """ + try: + import open3d as o3d + + if pcd_file is None: + # 使用示例：生成随机点云 + print("\n生成随机点云进行演示...") + points = np.random.randn(500, 3) * 0.5 + else: + pcd = o3d.io.read_point_cloud(pcd_file) + points = np.asarray(pcd.points) + + print(f"原始点云: {len(points)} 个点") + + # 简化 + dp = DouglasPeucker3D(epsilon=0.05) + + # 注意：传统 DP 算法需要有序点集，对于无序点云需要先排序或使用其他方法 + # 这里演示对有序轨迹/轮廓的应用 + print("\n注意: DP算法适用于有序点集（如轮廓线、轨迹），") + print("对于无序点云，建议先进行排序或使用其他简化方法（如体素滤波）") + + except ImportError: + print("需要安装 open3d 来读取点云文件: pip install open3d") + + +if __name__ == "__main__": + # 运行演示 + demo_point_cloud() + demo_trajectory() + + # 如果需要处理实际点云文件 + # demo_point_cloud_from_file("your_pointcloud.pcd") \ No newline at end of file diff --git a/alg/point_porj/archive/relative_code/BF.py b/alg/point_porj/archive/relative_code/BF.py new file mode 100644 index 0000000..ae2ebf8 --- /dev/null +++ b/alg/point_porj/archive/relative_code/BF.py @@ -0,0 +1,95 @@ +import open3d as o3d +import numpy as np +from scipy.spatial import KDTree +import numpy as np +from scipy.spatial import KDTree +def chamfer_distance(pcd1, pcd2): + """ + 计算两个点云之间的 CD 距离（Chamfer Distance） + + Args: + pcd1: Open3D 点云对象1 + pcd2: Open3D 点云对象2 + + Returns: + cd: Chamfer Distance 值 + cd1: 点云1 到 点云2 的平均距离 + cd2: 点云2 到 点云1 的平均距离 + """ + # 获取点坐标 + points1 = np.asarray(pcd1.points) + points2 = np.asarray(pcd2.points) + + # 构建 KD-Tree 加速最近邻搜索 + tree1 = KDTree(points1) + tree2 = KDTree(points2) + + # 计算 points1 中每个点到 points2 的最近距离 + dist1, _ = tree2.query(points1, k=1) + cd1 = np.mean(dist1 ** 2) # 平方距离的平均值 + + # 计算 points2 中每个点到 points1 的最近距离 + dist2, _ = tree1.query(points2, k=1) + cd2 = np.mean(dist2 ** 2) # 平方距离的平均值 + + # 总 CD 距离（双向平均） + cd = cd1 + cd2 + + return np.sqrt(cd), cd1, cd2 + +def bilateral_filter(pcd, sigma_s=0.01, sigma_r=0.1): + """ + 对点云进行双边滤波 + Args: + pcd: Open3D点云对象 + sigma_s: 空间标准差 + sigma_r: 特征（法线）标准差 + """ + points = np.asarray(pcd.points) + normals = np.asarray(pcd.normals) + + # 构建KD树 + tree = KDTree(points) + + filtered_points = np.zeros_like(points) + + for i, p in enumerate(points): + # 搜索邻域点 + indices = tree.query_ball_point(p, r=2 * sigma_s) + if len(indices) <= 1: + filtered_points[i] = p + continue + + neigh_points = points[indices] + neigh_normals = normals[indices] + + # 计算空间权重 + spatial_dist = np.linalg.norm(neigh_points - p, axis=1) + w_s = np.exp(-spatial_dist ** 2 / (2 * sigma_s ** 2)) + + # 计算特征（法线）权重 + normal_diff = np.linalg.norm(neigh_normals - normals[i], axis=1) + w_r = np.exp(-normal_diff ** 2 / (2 * sigma_r ** 2)) + + # 双边权重 + w = w_s * w_r + w = w / w.sum() # 归一化 + + # 加权平均 + filtered_points[i] = np.sum(neigh_points.T * w, axis=1) + + # 更新点云 + filtered_pcd = o3d.geometry.PointCloud() + filtered_pcd.points = o3d.utility.Vector3dVector(filtered_points) + filtered_pcd.normals = o3d.utility.Vector3dVector(normals) + + return filtered_pcd + + +# 使用示例 +pcd = o3d.io.read_point_cloud("E:\point_distance\data\part_car_head.ply") +pcd.estimate_normals() +pcd_filtered = bilateral_filter(pcd, sigma_s=10, sigma_r=10) + +cd=chamfer_distance(pcd,pcd_filtered) +print(cd) \ No newline at end of file diff --git a/alg/point_porj/archive/relative_code/CE.py b/alg/point_porj/archive/relative_code/CE.py new file mode 100644 index 0000000..b08f758 --- /dev/null +++ b/alg/point_porj/archive/relative_code/CE.py @@ -0,0 +1,54 @@ +import open3d as o3d +import numpy as np + + +def compute_curvature_open3d(pcd, radius=0.1, max_nn=30): + """ + 使用 Open3D 计算曲率 + + Args: + pcd: Open3D 点云对象 + radius: 搜索半径 + max_nn: 最大邻居数 + + Returns: + curvature: 曲率数组 + """ + # 估计法线 + pcd.estimate_normals( + search_param=o3d.geometry.KDTreeSearchParamHybrid(radius=radius, max_nn=max_nn) + ) + + # 计算曲率（Open3D 在估计法线时已计算） + # 曲率值存储在 pcd.point.normals 的第三个分量？ + # 实际上 Open3D 需要单独计算 + + # 方法：使用 PCA 重新计算 + points = np.asarray(pcd.points) + n_points = len(points) + curvature = np.zeros(n_points) + + tree = o3d.geometry.KDTreeFlann(pcd) + + for i in range(n_points): + [k, idx, _] = tree.search_radius_vector_3d(pcd.points[i], radius) + + if k < 3: + curvature[i] = 1.0 + continue + + neighbors = points[idx] + centroid = np.mean(neighbors, axis=0) + centered = neighbors - centroid + cov = np.cov(centered.T) + eigenvalues = np.linalg.eigvalsh(cov) + eigenvalues = np.sort(eigenvalues) + + curvature[i] = eigenvalues[0] / (eigenvalues.sum() + 1e-8) + + return curvature + + +# 使用示例 +pcd = o3d.io.read_point_cloud("pointcloud.pcd") +curvature = compute_curvature_open3d(pcd, radius=0.05) \ No newline at end of file diff --git a/alg/point_porj/archive/relative_code/DBSCAN.py b/alg/point_porj/archive/relative_code/DBSCAN.py new file mode 100644 index 0000000..ea15b39 --- /dev/null +++ b/alg/point_porj/archive/relative_code/DBSCAN.py @@ -0,0 +1,50 @@ +import open3d as o3d +import numpy as np + + +def dbscan_open3d(pcd, eps=0.1, min_points=10): + """ + 使用 Open3D 进行 DBSCAN 聚类 + Args: + pcd: Open3D 点云对象 + eps: 邻域半径 + min_points: 最小邻域点数 + Returns: + labels: 每个点的簇标签（-1 表示噪声） + clusters: 各簇的点索引列表 + """ + # DBSCAN 聚类 + labels = np.array(pcd.cluster_dbscan(eps=eps, min_points=min_points)) + + # 提取各簇索引 + unique_labels = np.unique(labels) + clusters = [] + for label in unique_labels: + if label == -1: + continue + clusters.append(np.where(labels == label)[0]) + + return labels, clusters + + +# 使用示例 +pcd = o3d.io.read_point_cloud("E:\point_distance\data\part_car_head.ply") + +# 预处理 +pcd = pcd.voxel_down_sample(voxel_size=0.05) +pcd, _ = pcd.remove_statistical_outlier(nb_neighbors=20, std_ratio=2.0) + +# DBSCAN 聚类 +labels, clusters = dbscan_open3d(pcd, eps=0.1, min_points=50) + +print(f"找到 {len(clusters)} 个聚类") +print(f"噪声点: {np.sum(labels == -1)}") + +# 为每个聚类着色 +colors = np.random.rand(len(clusters), 3) +colored_points = np.zeros((len(pcd.points), 3)) +for i, cluster in enumerate(clusters): + colored_points[cluster] = colors[i] +pcd.colors = o3d.utility.Vector3dVector(colored_points) + +o3d.visualization.draw_geometries([pcd]) \ No newline at end of file diff --git a/alg/point_porj/archive/relative_code/EN.py b/alg/point_porj/archive/relative_code/EN.py new file mode 100644 index 0000000..553ff4b --- /dev/null +++ b/alg/point_porj/archive/relative_code/EN.py @@ -0,0 +1,65 @@ +import open3d as o3d +import numpy as np + + +def compute_curvature_open3d(pcd, radius=0.05, max_nn=30): + """ + 使用 Open3D 计算点云曲率 + + Args: + pcd: Open3D 点云对象 + radius: 搜索半径 + max_nn: 最大邻居数 + + Returns: + curvatures: (N,) 曲率数组 + """ + # 估计法线（同时获取协方差矩阵信息） + pcd.estimate_normals( + search_param=o3d.geometry.KDTreeSearchParamHybrid(radius=radius, max_nn=max_nn) + ) + + # 获取点云数据 + points = np.asarray(pcd.points) + n_points = len(points) + curvatures = np.zeros(n_points) + + # 构建 KD-Tree + tree = o3d.geometry.KDTreeFlann(pcd) + + for i in range(n_points): + # 搜索邻域点 + [k, idx, _] = tree.search_radius_vector_3d(pcd.points[i], radius) + + if k < 3: + curvatures[i] = 1.0 + continue + + # 获取邻域点坐标 + neighbors = points[idx] + + # 计算协方差矩阵 + centroid = np.mean(neighbors, axis=0) + centered = neighbors - centroid + cov = np.cov(centered.T) + + # 特征值分解 + eigenvalues = np.linalg.eigvalsh(cov) + eigenvalues = np.sort(eigenvalues) + + # 计算曲率 + curvatures[i] = eigenvalues[0] / (eigenvalues.sum() + 1e-8) + + return curvatures + + +# 使用示例 +pcd = o3d.io.read_point_cloud("E:\point_distance\data\PointCloud\\38.ply") +curvatures = compute_curvature_open3d(pcd, radius=50, max_nn=30) + +# 将曲率作为颜色可视化 +colors = np.zeros((len(pcd.points), 3)) +colors[:, 0] = curvatures*3 # 红色通道表示曲率 +colors[:, 1] = 1 - curvatures*3 # 绿色通道为互补色 +pcd.colors = o3d.utility.Vector3dVector(colors) +o3d.visualization.draw_geometries([pcd]) \ No newline at end of file diff --git a/alg/point_porj/archive/relative_code/Eucilid.py b/alg/point_porj/archive/relative_code/Eucilid.py new file mode 100644 index 0000000..025f498 --- /dev/null +++ b/alg/point_porj/archive/relative_code/Eucilid.py @@ -0,0 +1,47 @@ +import open3d as o3d +import numpy as np + + +def euclidean_clustering_open3d(pcd, radius=0.1, min_cluster_size=50, max_cluster_size=10000): + """ + 使用 Open3D 进行欧几里得聚类 + Args: + pcd: Open3D 点云对象 + radius: 聚类半径 + min_cluster_size: 最小簇大小 + max_cluster_size: 最大簇大小 + Returns: + labels: 每个点的簇标签（-1 表示噪声/无效簇） + clusters: 各簇的点索引列表 + """ + # 欧几里得聚类 + labels = np.array(pcd.cluster_dbscan(eps=radius, min_points=min_cluster_size)) + + # 统计簇信息 + unique_labels = np.unique(labels) + clusters = [] + + for label in unique_labels: + if label == -1: # -1 表示噪声点 + continue + cluster_indices = np.where(labels == label)[0] + if min_cluster_size <= len(cluster_indices) <= max_cluster_size: + clusters.append(cluster_indices) + + return labels, clusters + + +# 使用示例 +pcd = o3d.io.read_point_cloud("E:\point_distance\data\part_car_left_side.ply") +labels, clusters = euclidean_clustering_open3d(pcd, radius=4, min_cluster_size=100) + +print(f"找到 {len(clusters)} 个聚类") + +# 为每个聚类着色 +colors = np.random.rand(len(clusters), 3) +colored_points = np.zeros((len(pcd.points), 3)) +for i, cluster in enumerate(clusters): + colored_points[cluster] = colors[i] + +pcd.colors = o3d.utility.Vector3dVector(colored_points) +o3d.visualization.draw_geometries([pcd]) \ No newline at end of file diff --git a/alg/point_porj/archive/relative_code/FPFH.py b/alg/point_porj/archive/relative_code/FPFH.py new file mode 100644 index 0000000..abc6feb --- /dev/null +++ b/alg/point_porj/archive/relative_code/FPFH.py @@ -0,0 +1,16 @@ +import open3d as o3d +import numpy as np + +pcd = o3d.io.read_point_cloud("pointcloud.pcd") + +# 1. 计算法线 +pcd.estimate_normals(search_param=o3d.geometry.KDTreeSearchParamKNN(k=30)) + +# 2. 提取FPFH特征 +fpfh = o3d.pipelines.registration.compute_fpfh_feature( + pcd, + o3d.geometry.KDTreeSearchParamKNN(k=50) +) + +# fpfh.data 是 (33, N) 的矩阵，每列是一个点的33维FPFH描述子 +print(fpfh.data.shape) # (33, N) \ No newline at end of file diff --git a/alg/point_porj/archive/relative_code/NE.py b/alg/point_porj/archive/relative_code/NE.py new file mode 100644 index 0000000..2a4ef53 --- /dev/null +++ b/alg/point_porj/archive/relative_code/NE.py @@ -0,0 +1,17 @@ +import open3d as o3d +import numpy as np +pcd = o3d.io.read_point_cloud("pointcloud.pcd") + +# 估计法线 +pcd.estimate_normals( + search_param=o3d.geometry.KDTreeSearchParamHybrid( + radius=0.1, # 搜索半径 + max_nn=30 # 最大邻居数 + ) +) + +# 可视化法线 +o3d.visualization.draw_geometries([pcd], point_show_normal=True) + +# 获取法线数据 +normals = np.asarray(pcd.normals) \ No newline at end of file diff --git a/alg/point_porj/archive/relative_code/P2P.py b/alg/point_porj/archive/relative_code/P2P.py new file mode 100644 index 0000000..611ccc8 --- /dev/null +++ b/alg/point_porj/archive/relative_code/P2P.py @@ -0,0 +1,315 @@ + +import copy +from scipy.spatial.transform import Rotation as R +import trimesh +import numpy as np +import open3d as o3d +import time +def rotation_matrix_scipy(roll_deg=10, pitch_deg=10, yaw_deg=10): + """ + 使用 scipy 生成绕 XYZ 轴旋转的矩阵 + """ + r = R.from_euler('xyz', [roll_deg, pitch_deg, yaw_deg], degrees=True) + return r.as_matrix() + + + + +def random_rotation_matrix(): + """生成随机旋转矩阵""" + angle = np.random.uniform(0, 2 * np.pi) + axis = np.random.randn(3) + axis = axis / np.linalg.norm(axis) + + K = np.array([ + [0, -axis[2], axis[1]], + [axis[2], 0, -axis[0]], + [-axis[1], axis[0], 0] + ]) + R = np.eye(3) + np.sin(angle) * K + (1 - np.cos(angle)) * K @ K + return R + + +def random_translation_vector(range_xyz=0.5): + """生成随机平移向量""" + return np.random.uniform(-range_xyz, range_xyz, 3) + + +def transform_point_cloud(pcd, R, t): + """对点云应用变换""" + transformed_pcd = copy.deepcopy(pcd) + points = np.asarray(transformed_pcd.points) + points_transformed = (R @ points.T).T + t + transformed_pcd.points = o3d.utility.Vector3dVector(points_transformed) + return transformed_pcd + + +def registration_open3d(source_pcd, target_pcd, voxel_size=0.005): + """ + 点云配准（RANSAC + ICP） + + Returns: + transformation: 4x4 变换矩阵 + fitness: 配准 fitness 分数 + rmse: 配准 RMSE + """ + # 1. 下采样 + source_down = source_pcd.voxel_down_sample(voxel_size) + target_down = target_pcd.voxel_down_sample(voxel_size) + + # 2. 估计法线 + radius = voxel_size * 2 + source_down.estimate_normals(o3d.geometry.KDTreeSearchParamHybrid(radius=radius, max_nn=30)) + target_down.estimate_normals(o3d.geometry.KDTreeSearchParamHybrid(radius=radius, max_nn=30)) + + # 3. 计算 FPFH 特征 + radius_feature = voxel_size * 5 + source_fpfh = o3d.pipelines.registration.compute_fpfh_feature( + source_down, o3d.geometry.KDTreeSearchParamHybrid(radius=radius_feature, max_nn=100)) + target_fpfh = o3d.pipelines.registration.compute_fpfh_feature( + target_down, o3d.geometry.KDTreeSearchParamHybrid(radius=radius_feature, max_nn=100)) + + # 4. RANSAC 粗配准 + distance_threshold = voxel_size * 1.5 + result_ransac = o3d.pipelines.registration.registration_ransac_based_on_feature_matching( + source_down, target_down, source_fpfh, target_fpfh, + mutual_filter=True, + max_correspondence_distance=distance_threshold, + estimation_method=o3d.pipelines.registration.TransformationEstimationPointToPoint(), + ransac_n=3, + checkers=[ + o3d.pipelines.registration.CorrespondenceCheckerBasedOnEdgeLength(0.9), + o3d.pipelines.registration.CorrespondenceCheckerBasedOnDistance(distance_threshold) + ], + criteria=o3d.pipelines.registration.RANSACConvergenceCriteria(100000, 0.999)) + + print(f"RANSAC 配准完成") + source_pcd.estimate_normals(o3d.geometry.KDTreeSearchParamHybrid(radius=0.1, max_nn=30)) + target_pcd.estimate_normals(o3d.geometry.KDTreeSearchParamHybrid(radius=0.1, max_nn=30)) + # 5. ICP 精配准 + result_icp = o3d.pipelines.registration.registration_icp( + source_pcd, target_pcd, distance_threshold / 2, result_ransac.transformation, + o3d.pipelines.registration.TransformationEstimationPointToPlane(), + o3d.pipelines.registration.ICPConvergenceCriteria(max_iteration=200)) + + print(f"ICP 精配准完成") + print(f" Fitness: {result_icp.fitness:.4f}") + print(f" RMSE: {result_icp.inlier_rmse:.6f} m") + + return result_icp.transformation, result_icp.fitness, result_icp.inlier_rmse + + +def visualize_registration_result(original_pcd, transformed_pcd, registered_pcd, title="Registration Result"): + """ + 可视化原始点云、变换后点云和配准后点云 + + Args: + original_pcd: 原始点云（源） + transformed_pcd: 应用随机变换后的点云（目标） + registered_pcd: 配准后的点云 + title: 窗口标题 + """ + # 复制并着色 + source_colored = copy.deepcopy(original_pcd) + target_colored = copy.deepcopy(transformed_pcd) + registered_colored = copy.deepcopy(registered_pcd) + + # 设置颜色 + source_colored.paint_uniform_color([0, 0, 1]) # 蓝色 - 原始点云 + target_colored.paint_uniform_color([0, 1, 0]) # 绿色 - 目标点云 + registered_colored.paint_uniform_color([1, 0, 0]) # 红色 - 配准后点云 + + # 创建坐标系（可选） + # coordinate_frame = o3d.geometry.TriangleMesh.create_coordinate_frame(size=0.5) + + # 可视化 + o3d.visualization.draw_geometries( + [source_colored, target_colored], + window_name="Before Registration (Blue: Source, Green: Target)", + width=1024, + height=768 + ) + + o3d.visualization.draw_geometries( + [registered_colored, target_colored], + window_name="After Registration (Red: Registered, Green: Target)", + width=1024, + height=768 + ) + + +def visualize_registration_comparison(original_pcd, transformed_pcd, registered_pcd): + """ + 并排可视化配准前后的对比 + + Args: + original_pcd: 原始点云 + transformed_pcd: 变换后的点云（目标） + registered_pcd: 配准后的点云 + """ + # 复制并着色 + source_before = copy.deepcopy(original_pcd) + source_before.paint_uniform_color([0, 0, 1]) # 蓝色 + + target_before = copy.deepcopy(transformed_pcd) + target_before.paint_uniform_color([0, 1, 0]) # 绿色 + + registered_after = copy.deepcopy(registered_pcd) + registered_after.paint_uniform_color([1, 0, 0]) # 红色 + + target_after = copy.deepcopy(transformed_pcd) + target_after.paint_uniform_color([0, 1, 0]) # 绿色 + + # 创建坐标系 + coordinate_frame = o3d.geometry.TriangleMesh.create_coordinate_frame(size=0.5) + + # 方法1：分别显示两个窗口 + o3d.visualization.draw_geometries( + [source_before], + window_name="Before Registration (Blue: Source, Green: Target)", + width=1024, + height=768 + ) + o3d.visualization.draw_geometries( + [target_before], + window_name="Before Registration (Blue: Source, Green: Target)", + width=1024, + height=768 + ) + o3d.visualization.draw_geometries( + [registered_after, source_before], + window_name="After Registration (Red: Registered, Green: Target)", + width=1024, + height=768 + ) + + + + +def compute_point_to_mesh_distance(point_cloud, mesh): + """ + 计算点云到 Mesh 的距离 + + Args: + point_cloud: Open3D 点云对象 + mesh: Open3D 网格对象 + + Returns: + distances: 每个点到最近三角形面的距离 + mean_distance: 平均距离 + rmse: 均方根误差 + max_distance: 最大距离 + """ + # 计算点到 mesh 的距离 + distances = mesh.compute_point_cloud_distance(point_cloud) + distances = np.asarray(distances) + + mean_dist = np.mean(distances) + rmse = np.sqrt(np.mean(distances ** 2)) + max_dist = np.max(distances) + std_dist = np.std(distances) + + return distances, mean_dist, rmse, max_dist, std_dist + + +# 使用示例 +def demo(pcd,mesh): + # 加载点云和 mesh + # 计算距离 + distances, mean_dist, rmse, max_dist, std_dist = compute_point_to_mesh_distance(pcd, mesh) + + print(f"平均距离: {mean_dist:.6f} m ({mean_dist * 1000:.3f} mm)") + print(f"RMSE: {rmse:.6f} m ({rmse * 1000:.3f} mm)") + print(f"最大距离: {max_dist:.6f} m ({max_dist * 1000:.3f} mm)") + print(f"标准差: {std_dist:.6f} m ({std_dist * 1000:.3f} mm)") + + # 可视化（按距离着色） + pcd.colors = o3d.utility.Vector3dVector(get_color_by_distance(distances)) + o3d.visualization.draw_geometries([pcd, mesh]) + + +def get_color_by_distance(distances, max_dist=None): + """ + 根据距离生成颜色（红：远，蓝：近） + """ + if max_dist is None: + max_dist = np.percentile(distances, 95) # 使用95分位数 + + colors = np.zeros((len(distances), 3)) + norm_dist = np.clip(distances / (max_dist + 1e-8), 0, 1) + + # 红色通道（距离越远越红） + colors[:, 0] = norm_dist + # 蓝色通道（距离越近越蓝） + colors[:, 2] = 1 - norm_dist + + return colors +# ========== 主程序 ========== +if __name__ == "__main__": + # 1. 读取原始点云 + file_path = "E:\point_distance\data\\40_noisy.ply" + + source_pcd = o3d.io.read_point_cloud(file_path) + + # points=np.asarray(source_pcd.points) + + t0=time.time() + if len(source_pcd.points) == 0: + print("读取失败，请检查文件路径") + exit() + + print(f"原始点云点数: {len(source_pcd.points)}") + + # 2. 生成随机变换 + # R = random_rotation_matrix() + R = rotation_matrix_scipy(10, 10, 10) + t = random_translation_vector(range_xyz=0) + + print(f"随机旋转矩阵:\n{R}") + print(f"随机平移向量: {t}") + + # 3. 创建目标点云（变换后的源点云） + target_pcd = transform_point_cloud(source_pcd, R, t) + + # 4. 点云配准 + print("\n开始点云配准...") + transformation, fitness, rmse = registration_open3d(source_pcd, target_pcd, voxel_size=10) + + # 5. 应用配准变换 + # registered_pcd = copy.deepcopy(source_pcd) + # registered_pcd.transform(transformation) + inv_transformation = np.linalg.inv(transformation) + # 将目标点云变换回源点云坐标系 + target_recovered = copy.deepcopy(target_pcd) + target_recovered.transform(inv_transformation) + + + t1 = time.time() + print(f"\n配准结果:") + print(f" Fitness: {fitness:.4f}") + print(f" RMSE: {rmse:.6f} m") + print(f" 估计的变换矩阵:\n{transformation}") + print(f"配准time:\n{t1-t0}") + + + t2=time.time() + # 1. 读取数据 + + points = np.asarray(target_recovered.points) + mesh_trimesh = trimesh.load("E:\point_distance\data\\40.stl") + # ========== 1. 读取数据 ========== + # 2. 使用 trimesh 计算每个点到 Mesh 的最近距离 + # trimesh 的 nearest 方法非常高效且精确 + closest_points, distances, triangle_ids = mesh_trimesh.nearest.on_surface(points) + t3 = time.time() + + # distances 即为所求的距离数组 + print(f"平均距离: {np.mean(distances)}") + print(f"最大距离: {np.max(distances)}") + print(f" 距离计算: {t3 - t2:.4f} 秒") + # 如果需要将结果存回 Open3D 点云进行可视化 + + + # 6. 可视化对比 + print("\n开始可视化...") + visualize_registration_comparison(source_pcd, target_pcd,target_recovered) \ No newline at end of file diff --git a/alg/point_porj/archive/relative_code/PF.py b/alg/point_porj/archive/relative_code/PF.py new file mode 100644 index 0000000..2f7507f --- /dev/null +++ b/alg/point_porj/archive/relative_code/PF.py @@ -0,0 +1,94 @@ +import open3d as o3d +import numpy as np +from scipy.spatial import KDTree + + +def compute_keep_ratio_and_density(pcd, filtered_pcd, sample_size=1000): + """ + 计算直通滤波的保留率和密度变化 + + Args: + pcd: 原始点云 + filtered_pcd: 滤波后点云 + sample_size: 密度计算采样点数（加速计算） + + Returns: + keep_ratio: 保留率 (%) + density_change: 密度变化 (%) + """ + n_orig = len(pcd.points) + n_filt = len(filtered_pcd.points) + + # 1. 保留率 + keep_ratio = n_filt / n_orig * 100 + + # 2. 密度变化（基于平均最近邻距离） + points_orig = np.asarray(pcd.points) + points_filt = np.asarray(filtered_pcd.points) + + # 采样（点数太多时加速） + if len(points_orig) > sample_size: + idx_orig = np.random.choice(len(points_orig), sample_size, replace=False) + points_orig_sample = points_orig[idx_orig] + else: + points_orig_sample = points_orig + + if len(points_filt) > sample_size: + idx_filt = np.random.choice(len(points_filt), sample_size, replace=False) + points_filt_sample = points_filt[idx_filt] + else: + points_filt_sample = points_filt + + # 计算平均最近邻距离（密度指标） + if len(points_orig_sample) > 5: + tree_orig = KDTree(points_orig_sample) + dist_orig, _ = tree_orig.query(points_orig_sample, k=2) # k=2 排除自身 + avg_dist_orig = np.mean(dist_orig[:, 1]) + else: + avg_dist_orig = 0 + + if len(points_filt_sample) > 5: + tree_filt = KDTree(points_filt_sample) + dist_filt, _ = tree_filt.query(points_filt_sample, k=2) + avg_dist_filt = np.mean(dist_filt[:, 1]) + else: + avg_dist_filt = 0 + + # 密度变化（距离增大 = 密度减小） + if avg_dist_orig > 0: + density_change = (avg_dist_filt - avg_dist_orig) / avg_dist_orig * 100 + else: + density_change = 0 + + return keep_ratio, density_change + + +# ========== 使用示例 ========== +if __name__ == "__main__": + # 读取点云 + pcd = o3d.io.read_point_cloud(r"E:\point_distance\data\part_car_head.ply") + + # 直通滤波（Z轴） + points = np.asarray(pcd.points) + mask = (points[:, 2] >= 0.0) & (points[:, 2] <= 2.0) + filtered_points = points[mask] + + filtered_pcd = o3d.geometry.PointCloud() + filtered_pcd.points = o3d.utility.Vector3dVector(filtered_points) + + # 计算指标 + keep_ratio, density_change = compute_keep_ratio_and_density(pcd, filtered_pcd) + + print("=" * 40) + print("直通滤波指标") + print("=" * 40) + print(f"保留率: {keep_ratio:.2f}%") + print(f"密度变化: {density_change:+.2f}%") + + # 解释 + if density_change > 0: + print(f" → 点云变稀疏了 {density_change:.2f}%") + elif density_change < 0: + print(f" → 点云变密集了 {abs(density_change):.2f}%") + else: + print(f" → 密度基本不变") \ No newline at end of file diff --git a/alg/point_porj/archive/relative_code/RANSC.py b/alg/point_porj/archive/relative_code/RANSC.py new file mode 100644 index 0000000..c90ee41 --- /dev/null +++ b/alg/point_porj/archive/relative_code/RANSC.py @@ -0,0 +1,380 @@ +# import numpy as np +# import open3d as o3d +# import random +# +# +# def ransac_plane(points, threshold=0.01, max_iterations=1000, min_inliers=100): +# """ +# RANSAC 平面拟合（单个） +# """ +# best_inliers = [] +# best_plane = None +# n_points = len(points) +# +# if n_points < 3: +# return None, [] +# +# for _ in range(max_iterations): +# sample_idx = random.sample(range(n_points), 3) +# p1, p2, p3 = points[sample_idx] +# +# v1 = p2 - p1 +# v2 = p3 - p1 +# normal = np.cross(v1, v2) +# norm = np.linalg.norm(normal) +# if norm < 1e-8: +# continue +# normal = normal / norm +# a, b, c = normal +# d = -np.dot(normal, p1) +# +# distances = np.abs(np.dot(points, normal) + d) +# inliers = np.where(distances < threshold)[0] +# +# if len(inliers) > len(best_inliers) and len(inliers) >= min_inliers: +# best_inliers = inliers +# best_plane = (a, b, c, d) +# +# if len(best_inliers) > 0.9 * n_points: +# break +# +# # 用内点重新拟合 +# if best_inliers is not None and len(best_inliers) > 3: +# inlier_points = points[best_inliers] +# centroid = np.mean(inlier_points, axis=0) +# _, _, vh = np.linalg.svd(inlier_points - centroid) +# normal = vh[2] +# normal = normal / np.linalg.norm(normal) +# a, b, c = normal +# d = -np.dot(normal, centroid) +# best_plane = (a, b, c, d) +# +# return best_plane, best_inliers +# +# +# def sequential_ransac_planes(points, threshold=0.01, max_iterations=1000, min_inliers=100, max_planes=5): +# """ +# 迭代 RANSAC 拟合多个平面 +# """ +# remaining_points = points.copy() +# remaining_indices = np.arange(len(points)) +# +# planes = [] +# inliers_list = [] +# +# for i in range(max_planes): +# if len(remaining_points) < min_inliers: +# break +# +# plane, inliers = ransac_plane(remaining_points, threshold, max_iterations, min_inliers) +# +# if plane is None or len(inliers) < min_inliers: +# break +# +# planes.append(plane) +# original_indices = remaining_indices[inliers] +# inliers_list.append(original_indices) +# +# # 移除内点 +# mask = np.ones(len(remaining_points), dtype=bool) +# mask[inliers] = False +# remaining_points = remaining_points[mask] +# remaining_indices = remaining_indices[mask] +# +# print(f"平面 {len(planes)}: {len(inliers)} 个内点, 剩余 {len(remaining_points)} 个点") +# +# return planes, inliers_list +# +# +# def create_plane_mesh(plane, bounds): +# """ +# 创建平面网格用于可视化 +# """ +# a, b, c, d = plane +# x_min, x_max = bounds[0] +# y_min, y_max = bounds[1] +# +# x = np.linspace(x_min, x_max, 50) +# y = np.linspace(y_min, y_max, 50) +# X, Y = np.meshgrid(x, y) +# +# if abs(c) > 1e-8: +# Z = -(a * X + b * Y + d) / c +# else: +# Z = np.zeros_like(X) +# +# vertices = [] +# triangles = [] +# rows, cols = X.shape +# +# for i in range(rows): +# for j in range(cols): +# vertices.append([X[i, j], Y[i, j], Z[i, j]]) +# +# for i in range(rows - 1): +# for j in range(cols - 1): +# idx = i * cols + j +# triangles.append([idx, idx + 1, idx + cols]) +# triangles.append([idx + 1, idx + cols + 1, idx + cols]) +# +# mesh = o3d.geometry.TriangleMesh() +# mesh.vertices = o3d.utility.Vector3dVector(np.array(vertices)) +# mesh.triangles = o3d.utility.Vector3iVector(np.array(triangles)) +# mesh.compute_vertex_normals() +# +# return mesh +# +# +# def visualize_multiple_planes(points, planes, inliers_list): +# """ +# 可视化多个平面（点云 + 平面网格） +# +# Args: +# points: (N, 3) 点云坐标 +# planes: 平面方程列表 [(a,b,c,d), ...] +# inliers_list: 每个平面对应的内点索引列表 +# """ +# # 颜色列表（每个平面不同颜色） +# colors = [ +# [1, 0, 0], # 红色 +# [0, 1, 0], # 绿色 +# [0, 0, 1], # 蓝色 +# [1, 1, 0], # 黄色 +# [1, 0, 1], # 品红 +# [0, 1, 1], # 青色 +# [1, 0.5, 0], # 橙色 +# [0.5, 0, 1], # 紫色 +# ] +# +# geometries = [] +# +# # 1. 为每个平面的内点着色 +# point_colors = np.zeros((len(points), 3)) +# for i, inliers in enumerate(inliers_list): +# color = colors[i % len(colors)] +# point_colors[inliers] = color +# +# # 未分配的点（剩余点）设为灰色 +# unassigned = np.where(np.sum(point_colors, axis=1) == 0)[0] +# point_colors[unassigned] = [0.5, 0.5, 0.5] +# +# colored_pcd = o3d.geometry.PointCloud() +# colored_pcd.points = o3d.utility.Vector3dVector(points) +# colored_pcd.colors = o3d.utility.Vector3dVector(point_colors) +# geometries.append(colored_pcd) +# +# # 2. 为每个平面创建网格 +# for i, (plane, inliers) in enumerate(zip(planes, inliers_list)): +# # 计算该平面对应点的范围 +# inlier_points = points[inliers] +# if len(inlier_points) > 0: +# x_min, x_max = inlier_points[:, 0].min(), inlier_points[:, 0].max() +# y_min, y_max = inlier_points[:, 1].min(), inlier_points[:, 1].max() +# +# # 稍微扩展边界 +# x_range = x_max - x_min +# y_range = y_max - y_min +# x_min -= x_range * 0.1 +# x_max += x_range * 0.1 +# y_min -= y_range * 0.1 +# y_max += y_range * 0.1 +# +# mesh = create_plane_mesh(plane, [[x_min, x_max], [y_min, y_max]]) +# mesh.paint_uniform_color(colors[i % len(colors)]) +# # 设置半透明 +# mesh.paint_uniform_color([colors[i % len(colors)][0], +# colors[i % len(colors)][1], +# colors[i % len(colors)][2]]) +# geometries.append(mesh) +# +# # 可视化 +# o3d.visualization.draw_geometries( +# geometries, +# window_name="Multiple Planes - RANSAC", +# width=1024, +# height=768 +# ) +# +# +# def visualize_single_plane(points, plane, inliers): +# """ +# 可视化单个平面（兼容单平面情况） +# """ +# visualize_multiple_planes(points, [plane], [inliers]) +# +# +# # ========== 主程序 ========== +# if __name__ == "__main__": +# # 读取点云 +# file_path = r"E:\point_distance\data\part_car_head.ply" +# pcd = o3d.io.read_point_cloud(file_path) +# +# if len(pcd.points) == 0: +# print("读取失败，请检查文件路径") +# exit() +# +# points = np.asarray(pcd.points) +# print(f"原始点云点数: {len(points)}") +# +# # 计算点云范围 +# points_range = points.max(axis=0) - points.min(axis=0) +# print(f"点云范围: X[{points_range[0]:.3f}], Y[{points_range[1]:.3f}], Z[{points_range[2]:.3f}]") +# +# # 设置阈值 +# threshold = points_range[2] * 0.02 +# print(f"使用阈值: {threshold:.4f}") +# +# # ========== 单平面拟合 ========== +# # print("\n" + "=" * 50) +# # print("单平面拟合") +# # print("=" * 50) +# # +# # plane, inliers = ransac_plane(points, threshold=threshold, max_iterations=1, min_inliers=100) +# # +# # if plane is not None: +# # a, b, c, d = plane +# # print(f"拟合平面: {a:.4f}x + {b:.4f}y + {c:.4f}z + {d:.4f} = 0") +# # print(f"内点数量: {len(inliers)} / {len(points)} ({len(inliers) / len(points) * 100:.1f}%)") +# # visualize_single_plane(points, plane, inliers) +# +# # ========== 多平面拟合 ========== +# print("\n" + "=" * 50) +# print("多平面拟合") +# print("=" * 50) +# +# planes, inliers_list = sequential_ransac_planes( +# points, +# threshold=threshold, +# max_iterations=1, +# min_inliers=100, +# max_planes=1 +# ) +# +# print(f"共提取 {len(planes)} 个平面") +# for i, (plane, inliers) in enumerate(zip(planes, inliers_list)): +# a, b, c, d = plane +# print(f"平面 {i + 1}: {a:.4f}x + {b:.4f}y + {c:.4f}z + {d:.4f} = 0, 内点数: {len(inliers)}") +# +# if len(planes) > 0: +# visualize_multiple_planes(points, planes, inliers_list) + +import numpy as np +import open3d as o3d +import random + + +def create_square_plane_with_noise(size_mm=1000, noise_std_mm=0.5, num_points=10000): + """创建正方形平面点云并添加高斯噪声""" + size_m = size_mm / 1000.0 + noise_std_m = noise_std_mm / 1000.0 + + half_size = size_m / 2 + x = np.random.uniform(-half_size, half_size, num_points) + y = np.random.uniform(-half_size, half_size, num_points) + z = np.zeros(num_points) + + points = np.column_stack([x, y, z]) + noise_z = np.random.normal(0, noise_std_m, num_points) + points[:, 2] += noise_z + + return points + + +def estimate_plane_ransac_open3d(points, threshold_mm=0.5, max_iterations=2000): + """使用 Open3D 内置 RANSAC 平面估计""" + pcd = o3d.geometry.PointCloud() + pcd.points = o3d.utility.Vector3dVector(points) + + threshold_m = threshold_mm / 1000.0 + + plane_model, inliers = pcd.segment_plane( + distance_threshold=threshold_m, + ransac_n=3, + num_iterations=max_iterations + ) + + a, b, c, d = plane_model + + # 计算误差 + ideal_normal = np.array([0, 0, 1]) + estimated_normal = np.array([a, b, c]) + cos_angle = np.dot(ideal_normal, estimated_normal) + cos_angle = np.clip(cos_angle, -1, 1) + angle_error = np.degrees(np.arccos(cos_angle)) + offset_error = abs(d) + + inlier_points = points[inliers] + distances = np.abs(np.dot(inlier_points, estimated_normal) + d) + rmse = np.sqrt(np.mean(distances ** 2)) + + print(f"\nRANSAC 平面拟合结果 (Open3D):") + print(f" 估计平面: {a:.6f}x + {b:.6f}y + {c:.6f}z + {d:.6f} = 0") + print(f" 内点数量: {len(inliers)} / {len(points)} ({len(inliers) / len(points) * 100:.1f}%)") + print(f" 法线角度误差: {angle_error:.4f} 度") + print(f" 偏移误差: {offset_error:.6f} m ({offset_error * 1000:.3f} mm)") + print(f" 平面拟合 RMSE: {rmse:.6f} m ({rmse * 1000:.3f} mm)") + + return plane_model, inliers + + +def visualize_plane_fitting(points, plane): + """可视化原始点云和拟合平面""" + # 原始点云 + pcd = o3d.geometry.PointCloud() + pcd.points = o3d.utility.Vector3dVector(points) + pcd.paint_uniform_color([0.6, 0.8, 1.0]) + + # 创建平面点云 + a, b, c, d = plane + x_min, x_max = points[:, 0].min(), points[:, 0].max() + y_min, y_max = points[:, 1].min(), points[:, 1].max() + + x = np.linspace(x_min, x_max, 50) + y = np.linspace(y_min, y_max, 50) + X, Y = np.meshgrid(x, y) + + if abs(c) > 1e-8: + Z = -(a * X + b * Y + d) / c + else: + Z = np.zeros_like(X) + + plane_points = np.column_stack([X.ravel(), Y.ravel(), Z.ravel()]) + + pcd_plane = o3d.geometry.PointCloud() + pcd_plane.points = o3d.utility.Vector3dVector(plane_points) + pcd_plane.paint_uniform_color([1, 0, 0]) # 红色 + + o3d.visualization.draw_geometries( + [pcd, pcd_plane], + window_name="Plane Fitting", + width=1024, + height=768 + ) + + +# ========== 主程序 ========== +if __name__ == "__main__": + # 参数设置 + size_mm = 1000 + noise_std_mm = 0.5 + num_points = 100000 + + print(f"参数:") + print(f" 正方形边长: {size_mm} mm") + print(f" 噪声标准差: {noise_std_mm} mm") + print(f" 点数: {num_points}") + + # 生成点云 + points = create_square_plane_with_noise(size_mm, noise_std_mm, num_points) + + # 检查数据范围 + print(f"\n数据范围:") + print(f" X: [{points[:, 0].min():.3f}, {points[:, 0].max():.3f}]") + print(f" Y: [{points[:, 1].min():.3f}, {points[:, 1].max():.3f}]") + print(f" Z: [{points[:, 2].min():.6f}, {points[:, 2].max():.6f}]") + + # 使用 Open3D RANSAC + plane, inliers = estimate_plane_ransac_open3d(points, threshold_mm=0.5, max_iterations=2000) + + # 可视化 + visualize_plane_fitting(points, plane) \ No newline at end of file diff --git a/alg/point_porj/archive/relative_code/RG.py b/alg/point_porj/archive/relative_code/RG.py new file mode 100644 index 0000000..18cfb9b --- /dev/null +++ b/alg/point_porj/archive/relative_code/RG.py @@ -0,0 +1,147 @@ +import numpy as np +from scipy.spatial import KDTree +from sklearn.neighbors import NearestNeighbors + + +def estimate_normals(points, k=20): + """用 PCA 估计法线""" + n_points = len(points) + normals = np.zeros_like(points) + + nbrs = NearestNeighbors(n_neighbors=k).fit(points) + distances, indices = nbrs.kneighbors(points) + + for i in range(n_points): + neighbors = points[indices[i]] + centroid = np.mean(neighbors, axis=0) + cov = np.cov((neighbors - centroid).T) + eig_vals, eig_vecs = np.linalg.eig(cov) + normal = eig_vecs[:, np.argmin(eig_vals)] + # 统一法线方向 + if normal[2] < 0: + normal = -normal + normals[i] = normal + + return normals + + +def compute_curvature(points, normals, k=20): + """计算曲率（特征值比值）""" + curvature = np.zeros(len(points)) + nbrs = NearestNeighbors(n_neighbors=k).fit(points) + distances, indices = nbrs.kneighbors(points) + + for i in range(len(points)): + neighbors = points[indices[i]] + centroid = np.mean(neighbors, axis=0) + cov = np.cov((neighbors - centroid).T) + eig_vals, _ = np.linalg.eig(cov) + curvature[i] = np.min(eig_vals) / (np.sum(eig_vals) + 1e-8) + + return curvature + + +def region_growing(points, normals, curvature, + angle_threshold=30.0, # 法线夹角阈值（度） + curvature_threshold=0.05, # 曲率阈值 + min_cluster_size=100): + """ + 区域生长算法 + Args: + points: (N, 3) 点云坐标 + normals: (N, 3) 法线 + curvature: (N,) 曲率 + angle_threshold: 法线夹角阈值（度） + curvature_threshold: 曲率阈值 + min_cluster_size: 最小簇大小 + Returns: + labels: (N,) 每个点的簇标签（-1 表示未分类/噪声） + """ + n_points = len(points) + labels = np.full(n_points, -1, dtype=int) + + # 按曲率升序排序（曲率小的作为种子） + sorted_indices = np.argsort(curvature) + + # 构建 KD-Tree 用于快速邻域搜索 + tree = KDTree(points) + + label_id = 0 + angle_threshold_rad = np.radians(angle_threshold) + cos_threshold = np.cos(angle_threshold_rad) + + for seed_idx in sorted_indices: + if labels[seed_idx] != -1: + continue + + # 开始一个新区域 + current_region = [] + seeds = [seed_idx] + labels[seed_idx] = label_id + + while seeds: + current_seed = seeds.pop(0) + current_region.append(current_seed) + + # 搜索半径内的邻居（动态半径或固定半径） + neighbors = tree.query_ball_point(points[current_seed], r=0.2) + + for neighbor in neighbors: + if labels[neighbor] != -1: + continue + + # 检查法线夹角 + dot_product = np.abs(np.dot(normals[current_seed], normals[neighbor])) + if dot_product > cos_threshold: + labels[neighbor] = label_id + + # 如果曲率足够小，作为新种子 + if curvature[neighbor] < curvature_threshold: + seeds.append(neighbor) + + # 检查区域大小 + region_size = np.sum(labels == label_id) + if region_size < min_cluster_size: + # 区域太小，标记为噪声 + labels[labels == label_id] = -1 + else: + label_id += 1 + + return labels + + +# 使用示例 +if __name__ == "__main__": + import open3d as o3d + + # 读取点云 + pcd = o3d.io.read_point_cloud("E:\point_distance\data\\bell_pepper_6.pcd") + points = np.asarray(pcd.points) + + # 预处理：体素下采样 + 去噪 + pcd = pcd.voxel_down_sample(0.0002) + pcd, _ = pcd.remove_statistical_outlier(nb_neighbors=20, std_ratio=2.0) + points = np.asarray(pcd.points) + + # 计算法线和曲率 + print("计算法线...") + normals = estimate_normals(points, k=30) + + print("计算曲率...") + curvature = compute_curvature(points, normals, k=30) + + # 区域生长分割 + print("区域生长...") + labels = region_growing(points, normals, curvature, + angle_threshold=25.0, + curvature_threshold=0.05, + min_cluster_size=100) + + print(f"分割完成，共 {len(np.unique(labels)) - (1 if -1 in labels else 0)} 个区域") + + # 可视化 + colors = np.random.rand(len(np.unique(labels)), 3) + point_colors = np.array([colors[label] if label != -1 else [0.5, 0.5, 0.5] + for label in labels]) + pcd.colors = o3d.utility.Vector3dVector(point_colors) + o3d.visualization.draw_geometries([pcd]) \ No newline at end of file diff --git a/alg/point_porj/archive/relative_code/ROR.py b/alg/point_porj/archive/relative_code/ROR.py new file mode 100644 index 0000000..ac2834e --- /dev/null +++ b/alg/point_porj/archive/relative_code/ROR.py @@ -0,0 +1,52 @@ +import open3d as o3d +import numpy as np +from scipy.spatial import KDTree +def chamfer_distance(pcd1, pcd2): + """ + 计算两个点云之间的 CD 距离（Chamfer Distance） + + Args: + pcd1: Open3D 点云对象1 + pcd2: Open3D 点云对象2 + + Returns: + cd: Chamfer Distance 值 + cd1: 点云1 到 点云2 的平均距离 + cd2: 点云2 到 点云1 的平均距离 + """ + # 获取点坐标 + points1 = np.asarray(pcd1.points) + points2 = np.asarray(pcd2.points) + + # 构建 KD-Tree 加速最近邻搜索 + tree1 = KDTree(points1) + tree2 = KDTree(points2) + + # 计算 points1 中每个点到 points2 的最近距离 + dist1, _ = tree2.query(points1, k=1) + cd1 = np.mean(dist1 ** 2) # 平方距离的平均值 + + # 计算 points2 中每个点到 points1 的最近距离 + dist2, _ = tree1.query(points2, k=1) + cd2 = np.mean(dist2 ** 2) # 平方距离的平均值 + + # 总 CD 距离（双向平均） + cd = cd1 + cd2 + + return np.sqrt(cd), cd1, cd2 +pcd = o3d.io.read_point_cloud("E:\point_distance\data\part_car_left_side.ply") +points = np.asarray(pcd.points) +colors = np.tile([0.8, 0.7, 1.0], (len(pcd.points), 1)) +pcd.colors = o3d.utility.Vector3dVector(colors) +o3d.visualization.draw_geometries([pcd],window_name="1") +# 半径离群点移除 +cl, ind = pcd.remove_radius_outlier(nb_points=5, radius=20) + +# 提取内点 +inlier_cloud = pcd.select_by_index(ind) +outlier_cloud = pcd.select_by_index(ind, invert=True) + +print(f"原始点云: {len(pcd.points)} 点") +print(f"滤波后: {len(inlier_cloud.points)} 点") +cd,cd1,cd2=chamfer_distance(pcd1=pcd,pcd2=inlier_cloud) +print(cd) \ No newline at end of file diff --git a/alg/point_porj/archive/relative_code/SOR.py b/alg/point_porj/archive/relative_code/SOR.py new file mode 100644 index 0000000..5e84431 --- /dev/null +++ b/alg/point_porj/archive/relative_code/SOR.py @@ -0,0 +1,60 @@ +import open3d as o3d +import numpy as np +from scipy.spatial import KDTree +from plyfile import PlyData +def chamfer_distance(pcd1, pcd2): + """ + 计算两个点云之间的 CD 距离（Chamfer Distance） + + Args: + pcd1: Open3D 点云对象1 + pcd2: Open3D 点云对象2 + + Returns: + cd: Chamfer Distance 值 + cd1: 点云1 到 点云2 的平均距离 + cd2: 点云2 到 点云1 的平均距离 + """ + # 获取点坐标 + points1 = np.asarray(pcd1.points) + points2 = np.asarray(pcd2.points) + + # 构建 KD-Tree 加速最近邻搜索 + tree1 = KDTree(points1) + tree2 = KDTree(points2) + + # 计算 points1 中每个点到 points2 的最近距离 + dist1, _ = tree2.query(points1, k=1) + cd1 = np.mean(dist1 ** 2) # 平方距离的平均值 + + # 计算 points2 中每个点到 points1 的最近距离 + dist2, _ = tree1.query(points2, k=1) + cd2 = np.mean(dist2 ** 2) # 平方距离的平均值 + + # 总 CD 距离（双向平均） + cd = cd1 + cd2 + + return cd, cd1, cd2 + +pcd = o3d.io.read_point_cloud("E:\point_distance\data\part_car_head.ply") +colors = np.tile([1, 0, 0], (len(pcd.points), 1)) +pcd.colors = o3d.utility.Vector3dVector(colors) +# ply_data = PlyData.read(r"E:\point_distance\data\part_car_head.ply") +# colors=np.asarray(ply_data.colors) +# with open("E:\point_distance\data\part_car_head.ply", 'r',encoding="utf-8") as f: +# lines = f.readlines() +# for line in lines[:30]: # 打印头部 +# print(line.strip()) +# o3d.visualization.draw_geometries([pcd]) +# points = np.asarray(pcd.points) +# colors = np.asarray(pcd.colors) +# normals = np.asarray(pcd.normals) +cl, ind = pcd.remove_statistical_outlier(nb_neighbors=20, std_ratio=50.0) +inlier_cloud = pcd.select_by_index(ind) +colors=np.tile([0,1,0],(len(inlier_cloud.points),1)) +inlier_cloud.colors=o3d.utility.Vector3dVector(colors) +o3d.visualization.draw_geometries([inlier_cloud]) +o3d.visualization.draw_geometries([pcd]) +o3d.visualization.draw_geometries() +cd,cd1,cd2=chamfer_distance(pcd1=pcd,pcd2=inlier_cloud) +print(cd) \ No newline at end of file diff --git a/alg/point_porj/archive/relative_code/VGF.py b/alg/point_porj/archive/relative_code/VGF.py new file mode 100644 index 0000000..59c8278 --- /dev/null +++ b/alg/point_porj/archive/relative_code/VGF.py @@ -0,0 +1,96 @@ +import open3d as o3d +import numpy as np +from scipy.spatial import KDTree + + +def evaluate_voxel_filter(pcd, leaf_size=0.05): + """ + 评估体素滤波效果，计算各项指标 + + Args: + pcd: 原始点云 + leaf_size: 体素边长 + + Returns: + metrics: 指标字典 + filtered_pcd: 滤波后点云 + """ + # 执行体素滤波 + filtered_pcd = pcd.voxel_down_sample(leaf_size) + + n_orig = len(pcd.points) + n_filt = len(filtered_pcd.points) + + # 1. 基础统计指标 + keep_ratio = n_filt / n_orig * 100 + downsample_rate = (n_orig - n_filt) / n_orig * 100 + compression_ratio = n_orig / n_filt if n_filt > 0 else 0 + + # 2. 密度变化指标 + points_orig = np.asarray(pcd.points) + points_filt = np.asarray(filtered_pcd.points) + + # 计算平均最近邻距离（采样加速） + sample_size = min(1000, n_filt) + if n_filt > sample_size: + idx = np.random.choice(n_filt, sample_size, replace=False) + points_sample = points_filt[idx] + else: + points_sample = points_filt + + if len(points_sample) > 5: + tree = KDTree(points_sample) + dist, _ = tree.query(points_sample, k=2) + avg_distance = np.mean(dist[:, 1]) + else: + avg_distance = 0 + + # 3. 输出指标 + metrics = { + 'leaf_size': leaf_size, + 'n_orig': n_orig, + 'n_filt': n_filt, + 'keep_ratio': keep_ratio, + 'downsample_rate': downsample_rate, + 'compression_ratio': compression_ratio, + 'avg_point_distance': avg_distance, + 'estimated_density': 1 / (avg_distance ** 3) if avg_distance > 0 else 0 + } + + return metrics, filtered_pcd + + +# ========== 使用示例 ========== +if __name__ == "__main__": + # 读取点云 + pcd = o3d.io.read_point_cloud(r"E:\point_distance\data\part_car_head.ply") + + # 测试不同体素大小 + leaf_sizes = [2, 15, 20, 2] + + print("=" * 60) + print("体素滤波指标评估") + print("=" * 60) + print(f"{'体素边长':<10} {'保留率':<12} {'下采样率':<12} {'压缩比':<10} {'平均间距':<12}") + print("-" * 60) + + for leaf_size in leaf_sizes: + metrics, filtered = evaluate_voxel_filter(pcd, leaf_size) + print(f"{leaf_size:<10} {metrics['keep_ratio']:>8.2f}% " + f"{metrics['downsample_rate']:>8.2f}% " + f"{metrics['compression_ratio']:>8.1f}x " + f"{metrics['avg_point_distance']:>8.4f}m") + + # 可视化对比 + print("\n可视化对比...") + orig_colored = o3d.geometry.PointCloud() + orig_colored.points = pcd.points + orig_colored.paint_uniform_color([0, 0, 1]) # 蓝色 + # o3d.visualization.draw_geometries([orig_colored],window_name=f"orignal (blue,{len(pcd.points)}") + metrics, filtered = evaluate_voxel_filter(pcd, leaf_size=20) + filtered.paint_uniform_color([1, 0, 0]) # 红色 + # o3d.visualization.draw_geometries([filtered], window_name=f"Voxel (red,{len(filtered.points)}") + o3d.visualization.draw_geometries( + [orig_colored, filtered], + window_name=f"Original (Blue, {len(pcd.points)} pts) vs Voxel (Red, {len(filtered.points)} pts)" + ) \ No newline at end of file diff --git a/alg/point_porj/archive/relative_code/distance_mesh.py b/alg/point_porj/archive/relative_code/distance_mesh.py new file mode 100644 index 0000000..4fb2118 --- /dev/null +++ b/alg/point_porj/archive/relative_code/distance_mesh.py @@ -0,0 +1,141 @@ +import open3d as o3d +import numpy as np +from scipy.spatial import KDTree +import copy + + +def compute_point_to_mesh_distance(point_cloud, mesh): + """ + 计算点云到 Mesh 的距离 + + Args: + point_cloud: Open3D 点云对象 + mesh: Open3D 网格对象 + + Returns: + distances: 每个点到最近三角形面的距离 + """ + # 方法1：使用 Open3D 内置函数（新版本） + try: + distances = mesh.compute_point_cloud_distance(point_cloud) + return np.asarray(distances) + except AttributeError: + pass + + # 方法2：使用 mesh 顶点最近邻（近似） + mesh_points = np.asarray(mesh.vertices) + points = np.asarray(point_cloud.points) + + tree = KDTree(mesh_points) + distances, indices = tree.query(points) + + return distances + + +def compute_statistics(distances): + """计算距离统计信息""" + stats = { + 'mean': np.mean(distances), + 'rmse': np.sqrt(np.mean(distances ** 2)), + 'max': np.max(distances), + 'min': np.min(distances), + 'std': np.std(distances), + 'median': np.median(distances), + } + + # 分位数 + percentiles = [50, 75, 90, 95, 99] + for p in percentiles: + stats[f'percentile_{p}'] = np.percentile(distances, p) + + return stats + + +def visualize_distance(point_cloud, mesh, distances, max_dist=None): + """可视化距离（按距离着色）""" + pcd_colored = copy.deepcopy(point_cloud) + + if max_dist is None: + max_dist = np.percentile(distances, 95) + + # 生成颜色（红：远，蓝：近） + colors = np.zeros((len(distances), 3)) + norm_dist = np.clip(distances / (max_dist + 1e-8), 0, 1) + colors[:, 0] = norm_dist # 红色通道（远） + colors[:, 2] = 1 - norm_dist # 蓝色通道（近） + + pcd_colored.colors = o3d.utility.Vector3dVector(colors) + + # Mesh 着色为灰色 + mesh_colored = copy.deepcopy(mesh) + mesh_colored.paint_uniform_color([0.5, 0.5, 0.5]) + + # 可视化 + o3d.visualization.draw_geometries( + [mesh_colored, pcd_colored], + window_name="Distance to Mesh (Blue=Near, Red=Far)", + width=1024, + height=768 + ) + + +def print_statistics(stats): + """打印统计信息""" + print("=" * 60) + print("点云到 Mesh 距离统计") + print("=" * 60) + print(f"平均距离: {stats['mean']:.6f} m ({stats['mean'] * 1000:.3f} mm)") + print(f"RMSE: {stats['rmse']:.6f} m ({stats['rmse'] * 1000:.3f} mm)") + print(f"中位数: {stats['median']:.6f} m ({stats['median'] * 1000:.3f} mm)") + print(f"标准差: {stats['std']:.6f} m ({stats['std'] * 1000:.3f} mm)") + print(f"最大距离: {stats['max']:.6f} m ({stats['max'] * 1000:.3f} mm)") + print(f"最小距离: {stats['min']:.6f} m ({stats['min'] * 1000:.3f} mm)") + print("\n距离分位数:") + print(f" 50%: {stats['percentile_50']:.6f} m ({stats['percentile_50'] * 1000:.3f} mm)") + print(f" 75%: {stats['percentile_75']:.6f} m ({stats['percentile_75'] * 1000:.3f} mm)") + print(f" 90%: {stats['percentile_90']:.6f} m ({stats['percentile_90'] * 1000:.3f} mm)") + print(f" 95%: {stats['percentile_95']:.6f} m ({stats['percentile_95'] * 1000:.3f} mm)") + print(f" 99%: {stats['percentile_99']:.6f} m ({stats['percentile_99'] * 1000:.3f} mm)") + + +# ========== 主程序 ========== +if __name__ == "__main__": + # 读取文件（使用原始字符串避免转义问题） + file_path = r"E:\point_distance\data\40_noisy.ply" + mesh_path = r"E:\point_distance\data\40.stl" + + print("读取点云...") + source_pcd = o3d.io.read_point_cloud(file_path) + + print("读取 Mesh...") + mesh = o3d.io.read_triangle_mesh(mesh_path) + + # 检查读取是否成功 + if len(source_pcd.points) == 0: + print("错误：点云读取失败，请检查文件路径") + exit() + + if len(mesh.vertices) == 0: + print("错误：Mesh 读取失败，请检查文件路径") + exit() + + print(f"\n点云点数: {len(source_pcd.points)}") + print(f"Mesh 顶点数: {len(mesh.vertices)}") + print(f"Mesh 面数: {len(mesh.triangles)}") + + # 可选：Mesh 法线计算（用于更好的可视化） + mesh.compute_vertex_normals() + + # 计算距离 + print("\n计算点云到 Mesh 的距离...") + distances = compute_point_to_mesh_distance(source_pcd, mesh) + + # 计算统计信息 + stats = compute_statistics(distances) + + # 打印统计信息 + print_statistics(stats) + + # 可视化 + print("\n可视化...") + visualize_distance(source_pcd, mesh, distances) \ No newline at end of file diff --git a/alg/point_porj/archive/relative_code/test5.py b/alg/point_porj/archive/relative_code/test5.py new file mode 100644 index 0000000..fba0b95 --- /dev/null +++ b/alg/point_porj/archive/relative_code/test5.py @@ -0,0 +1,25 @@ +import trimesh +import numpy as np +import open3d as o3d +import time +# 1. 读取数据 +mesh_trimesh = trimesh.load("E:\point_distance\data\\40.stl") +pcd_o3d = o3d.io.read_point_cloud("E:\point_distance\data\\40_noisy.ply") +points = np.asarray(pcd_o3d.points) +start_time = time.time() + +# ========== 1. 读取数据 ========== + +t0 = time.time() +# 2. 使用 trimesh 计算每个点到 Mesh 的最近距离 +# trimesh 的 nearest 方法非常高效且精确 +closest_points, distances, triangle_ids = mesh_trimesh.nearest.on_surface(points) +t1=time.time() + +# distances 即为所求的距离数组 +print(f"平均距离: {np.mean(distances)}") +print(f"最大距离: {np.max(distances)}") +print(f" 距离计算: {t1 - t0:.4f} 秒") +# 如果需要将结果存回 Open3D 点云进行可视化 +dists_o3d = o3d.utility.DoubleVector(distances.tolist()) +# 注意：Open3D 点云对象本身不直接存储标量距离字段用于内置可视化着色 diff --git a/alg/point_porj/archive/requirements.txt b/alg/point_porj/archive/requirements.txt new file mode 100644 index 0000000..1a4f21f --- /dev/null +++ b/alg/point_porj/archive/requirements.txt @@ -0,0 +1,10 @@ +#python 3.10.8 +--extra-index-url https://download.pytorch.org/whl/cu118 +torch +torchvision +torchaudio +trimesh==4.5.3 +numpy==2.2.2 +open3d==0.19.0 +pythonocc-core==7.5.1 # conda install -c conda-forge pythonocc-core= +git+https://github.com/AutodeskAILab/occwl #pip install git+https://github.com/AutodeskAILab/occwl \ No newline at end of file diff --git a/alg/point_porj/archive/translate_align_pointcloud_to_cad.py b/alg/point_porj/archive/translate_align_pointcloud_to_cad.py new file mode 100644 index 0000000..acff054 --- /dev/null +++ b/alg/point_porj/archive/translate_align_pointcloud_to_cad.py @@ -0,0 +1,1350 @@ +# import os +# import argparse +# import numpy as np +# import open3d as o3d + +# from OCC.Core.STEPControl import STEPControl_Reader +# from OCC.Core.IFSelect import IFSelect_RetDone +# from OCC.Core.BRepMesh import BRepMesh_IncrementalMesh +# from OCC.Core.TopExp import TopExp_Explorer +# from OCC.Core.TopAbs import TopAbs_FACE +# from OCC.Core.BRep import BRep_Tool + +# try: +# from OCC.Core.TopoDS import topods +# USE_TOPODS = "topods" +# except ImportError: +# from OCC.Core.TopoDS import TopoDS +# USE_TOPODS = "TopoDS" + + +# # ============================================================ +# # STEP CAD -> Open3D Mesh +# # ============================================================ + +# def read_step_file(step_path): +# reader = STEPControl_Reader() +# status = reader.ReadFile(step_path) + +# if status != IFSelect_RetDone: +# raise RuntimeError(f"STEP 文件读取失败: {step_path}") + +# reader.TransferRoots() +# shape = reader.OneShape() + +# print("[OK] STEP loaded:", step_path) +# return shape + + +# def occ_shape_to_numpy_mesh(shape): +# vertices = [] +# triangles = [] + +# explorer = TopExp_Explorer(shape, TopAbs_FACE) + +# face_count = 0 +# triangle_count = 0 + +# while explorer.More(): +# current_shape = explorer.Current() + +# if USE_TOPODS == "topods": +# face = topods.Face(current_shape) +# else: +# face = TopoDS.Face(current_shape) + +# location = face.Location() +# triangulation = BRep_Tool.Triangulation(face, location) + +# if triangulation is None: +# explorer.Next() +# continue + +# trsf = location.Transformation() +# base_index = len(vertices) + +# for i in range(1, triangulation.NbNodes() + 1): +# pnt = triangulation.Node(i) +# pnt.Transform(trsf) + +# vertices.append([ +# pnt.X(), +# pnt.Y(), +# pnt.Z() +# ]) + +# for i in range(1, triangulation.NbTriangles() + 1): +# tri = triangulation.Triangle(i) +# n1, n2, n3 = tri.Get() + +# triangles.append([ +# base_index + n1 - 1, +# base_index + n2 - 1, +# base_index + n3 - 1 +# ]) + +# triangle_count += 1 + +# face_count += 1 +# explorer.Next() + +# vertices = np.asarray(vertices, dtype=np.float64) +# triangles = np.asarray(triangles, dtype=np.int32) + +# print("[OK] CAD mesh extracted") +# print(" CAD faces:", face_count) +# print(" vertices:", vertices.shape) +# print(" triangles:", triangles.shape) + +# if vertices.shape[0] == 0 or triangles.shape[0] == 0: +# raise RuntimeError("CAD 三角网格提取失败") + +# return vertices, triangles + + +# def step_to_open3d_mesh(step_path, mesh_deflection=0.1): +# shape = read_step_file(step_path) + +# print("[INFO] Meshing CAD...") +# mesh_algo = BRepMesh_IncrementalMesh(shape, mesh_deflection) +# mesh_algo.Perform() + +# vertices, triangles = occ_shape_to_numpy_mesh(shape) + +# mesh = o3d.geometry.TriangleMesh() +# mesh.vertices = o3d.utility.Vector3dVector(vertices) +# mesh.triangles = o3d.utility.Vector3iVector(triangles) + +# mesh.remove_duplicated_vertices() +# mesh.remove_duplicated_triangles() +# mesh.remove_degenerate_triangles() +# mesh.compute_vertex_normals() + +# return mesh + + +# # ============================================================ +# # 基础工具 +# # ============================================================ + +# def axis_to_index(axis): +# axis = axis.lower() + +# if axis == "x": +# return 0 +# if axis == "y": +# return 1 +# if axis == "z": +# return 2 + +# raise ValueError("axis 必须是 x / y / z") + + +# def get_bounds(pcd): +# points = np.asarray(pcd.points) +# return points.min(axis=0), points.max(axis=0) + + +# def crop_by_axis_ratio(pcd, axis="x", part="front", ratio=0.15): +# """ +# 按某个轴裁剪车头或车尾。 + +# axis: +# 高铁长度方向，通常是 x + +# part: +# front: 取最小端 +# tail: 取最大端 + +# ratio: +# 取前后多少比例，例如 0.15 表示前 15% 和后 15% +# """ + +# axis_id = axis_to_index(axis) + +# points = np.asarray(pcd.points) + +# min_bound = points.min(axis=0) +# max_bound = points.max(axis=0) + +# axis_min = min_bound[axis_id] +# axis_max = max_bound[axis_id] +# axis_len = axis_max - axis_min + +# if part == "front": +# lower = axis_min +# upper = axis_min + axis_len * ratio +# elif part == "tail": +# lower = axis_max - axis_len * ratio +# upper = axis_max +# else: +# raise ValueError("part 必须是 front 或 tail") + +# crop_min = min_bound.copy() +# crop_max = max_bound.copy() + +# crop_min[axis_id] = lower +# crop_max[axis_id] = upper + +# bbox = o3d.geometry.AxisAlignedBoundingBox( +# min_bound=crop_min, +# max_bound=crop_max +# ) + +# cropped = pcd.crop(bbox) + +# print(f"[OK] Crop {part}") +# print(" axis:", axis) +# print(" range:", lower, upper) +# print(" points:", np.asarray(cropped.points).shape) + +# return cropped + + +# def preprocess_point_cloud(pcd, voxel_size): +# pcd_down = pcd.voxel_down_sample(voxel_size) + +# normal_radius = voxel_size * 2.0 +# feature_radius = voxel_size * 5.0 + +# pcd_down.estimate_normals( +# search_param=o3d.geometry.KDTreeSearchParamHybrid( +# radius=normal_radius, +# max_nn=30 +# ) +# ) + +# fpfh = o3d.pipelines.registration.compute_fpfh_feature( +# pcd_down, +# search_param=o3d.geometry.KDTreeSearchParamHybrid( +# radius=feature_radius, +# max_nn=100 +# ) +# ) + +# return pcd_down, fpfh + + +# def global_registration(source_down, target_down, source_fpfh, target_fpfh, voxel_size): +# distance_threshold = voxel_size * 1.5 + +# print("[INFO] RANSAC global registration") +# print(" distance threshold:", distance_threshold) + +# result = o3d.pipelines.registration.registration_ransac_based_on_feature_matching( +# source_down, +# target_down, +# source_fpfh, +# target_fpfh, +# mutual_filter=True, +# max_correspondence_distance=distance_threshold, +# estimation_method=o3d.pipelines.registration.TransformationEstimationPointToPoint(False), +# ransac_n=4, +# checkers=[ +# o3d.pipelines.registration.CorrespondenceCheckerBasedOnEdgeLength(0.9), +# o3d.pipelines.registration.CorrespondenceCheckerBasedOnDistance(distance_threshold) +# ], +# criteria=o3d.pipelines.registration.RANSACConvergenceCriteria( +# 100000, +# 0.999 +# ) +# ) + +# print("[OK] RANSAC result") +# print(" fitness:", result.fitness) +# print(" rmse:", result.inlier_rmse) +# print(" T:\n", result.transformation) + +# return result + + +# def icp_refine(source, target, init_T, voxel_size, max_iteration=100): +# distance_threshold = voxel_size * 0.5 + +# source.estimate_normals( +# search_param=o3d.geometry.KDTreeSearchParamHybrid( +# radius=voxel_size * 2.0, +# max_nn=30 +# ) +# ) + +# target.estimate_normals( +# search_param=o3d.geometry.KDTreeSearchParamHybrid( +# radius=voxel_size * 2.0, +# max_nn=30 +# ) +# ) + +# print("[INFO] ICP refinement") +# print(" distance threshold:", distance_threshold) + +# result = o3d.pipelines.registration.registration_icp( +# source, +# target, +# distance_threshold, +# init_T, +# o3d.pipelines.registration.TransformationEstimationPointToPlane(), +# criteria=o3d.pipelines.registration.ICPConvergenceCriteria( +# max_iteration=max_iteration +# ) +# ) + +# print("[OK] ICP result") +# print(" fitness:", result.fitness) +# print(" rmse:", result.inlier_rmse) +# print(" T:\n", result.transformation) + +# return result + + +# def register_local_region(source_local, target_local, voxel_size): +# """ +# 单个局部区域配准： +# source_local: 扫描点云局部 +# target_local: CAD采样点云局部 +# """ + +# source_down, source_fpfh = preprocess_point_cloud(source_local, voxel_size) +# target_down, target_fpfh = preprocess_point_cloud(target_local, voxel_size) + +# print("[INFO] source_down:", np.asarray(source_down.points).shape) +# print("[INFO] target_down:", np.asarray(target_down.points).shape) + +# ransac_result = global_registration( +# source_down, +# target_down, +# source_fpfh, +# target_fpfh, +# voxel_size +# ) + +# icp_result = icp_refine( +# source_local, +# target_local, +# ransac_result.transformation, +# voxel_size +# ) + +# return icp_result + + +# # ============================================================ +# # 变换融合 +# # ============================================================ + +# def project_rotation_to_so3(R): +# """ +# 将平均后的矩阵重新投影到合法旋转矩阵 SO(3) +# """ +# U, S, Vt = np.linalg.svd(R) +# R_proj = U @ Vt + +# if np.linalg.det(R_proj) < 0: +# U[:, -1] *= -1 +# R_proj = U @ Vt + +# return R_proj + + +# def fuse_two_transformations(T_front, T_tail, weight_front=0.5, weight_tail=0.5): +# """ +# 融合车头和车尾配准结果。 + +# 注意： +# 这里是简单加权平均旋转和平移。 +# 对刚体配准来说，T_front 和 T_tail 理论上应该接近。 +# 如果二者差异很大，说明前/后某一个局部配准可能失败。 +# """ + +# weight_sum = weight_front + weight_tail +# wf = weight_front / weight_sum +# wt = weight_tail / weight_sum + +# R_front = T_front[:3, :3] +# t_front = T_front[:3, 3] + +# R_tail = T_tail[:3, :3] +# t_tail = T_tail[:3, 3] + +# R_avg = wf * R_front + wt * R_tail +# R_final = project_rotation_to_so3(R_avg) + +# t_final = wf * t_front + wt * t_tail + +# T_final = np.eye(4) +# T_final[:3, :3] = R_final +# T_final[:3, 3] = t_final + +# return T_final + + +# def transformation_difference(T1, T2): +# """ +# 查看前后两个配准结果差异。 +# """ + +# R1 = T1[:3, :3] +# R2 = T2[:3, :3] + +# R_delta = R1.T @ R2 + +# cos_theta = (np.trace(R_delta) - 1.0) / 2.0 +# cos_theta = np.clip(cos_theta, -1.0, 1.0) + +# angle_rad = np.arccos(cos_theta) +# angle_deg = np.degrees(angle_rad) + +# t1 = T1[:3, 3] +# t2 = T2[:3, 3] + +# trans_diff = np.linalg.norm(t1 - t2) + +# return angle_deg, trans_diff + + +# # ============================================================ +# # 可视化 +# # ============================================================ + +# def visualize_pair(source, target, T=None, title="Registration"): +# source_vis = o3d.geometry.PointCloud(source) +# target_vis = o3d.geometry.PointCloud(target) + +# source_vis.paint_uniform_color([1.0, 0.0, 0.0]) +# target_vis.paint_uniform_color([0.0, 0.5, 1.0]) + +# if T is not None: +# source_vis.transform(T) + +# o3d.visualization.draw_geometries( +# [source_vis, target_vis], +# window_name=title +# ) + + +# # ============================================================ +# # 主流程：车头 + 车尾 + 整体融合 +# # ============================================================ + +# def register_train_by_front_tail( +# cad_path, +# pointcloud_path, +# output_dir="front_tail_registration_output", +# axis="x", +# crop_ratio=0.15, +# mesh_deflection=0.1, +# cad_sample_points=500000, +# voxel_size=10.0, +# scale_pointcloud=1.0, +# final_icp=True, +# visualize=True +# ): +# os.makedirs(output_dir, exist_ok=True) + +# # ------------------------------------------------------------ +# # 1. CAD STEP -> CAD sampled point cloud +# # ------------------------------------------------------------ + +# cad_mesh = step_to_open3d_mesh( +# cad_path, +# mesh_deflection=mesh_deflection +# ) + +# print(f"[INFO] Sampling CAD mesh: {cad_sample_points} points") +# cad_pcd = cad_mesh.sample_points_uniformly(cad_sample_points) + +# cad_pcd.estimate_normals( +# search_param=o3d.geometry.KDTreeSearchParamHybrid( +# radius=voxel_size * 2.0, +# max_nn=30 +# ) +# ) + +# cad_sample_path = os.path.join(output_dir, "cad_sampled_whole.ply") +# o3d.io.write_point_cloud(cad_sample_path, cad_pcd) + +# # ------------------------------------------------------------ +# # 2. Load source point cloud +# # ------------------------------------------------------------ + +# source_pcd = o3d.io.read_point_cloud(pointcloud_path) + +# if len(source_pcd.points) == 0: +# raise RuntimeError(f"点云读取失败: {pointcloud_path}") + +# print("[OK] Source point cloud loaded") +# print(" points:", np.asarray(source_pcd.points).shape) + +# if scale_pointcloud != 1.0: +# source_pcd.scale(scale_pointcloud, center=(0, 0, 0)) +# print("[INFO] Source scaled:", scale_pointcloud) + +# # ------------------------------------------------------------ +# # 3. Crop front and tail +# # ------------------------------------------------------------ + +# source_front = crop_by_axis_ratio( +# source_pcd, +# axis=axis, +# part="front", +# ratio=crop_ratio +# ) + +# source_tail = crop_by_axis_ratio( +# source_pcd, +# axis=axis, +# part="tail", +# ratio=crop_ratio +# ) + +# cad_front = crop_by_axis_ratio( +# cad_pcd, +# axis=axis, +# part="front", +# ratio=crop_ratio +# ) + +# cad_tail = crop_by_axis_ratio( +# cad_pcd, +# axis=axis, +# part="tail", +# ratio=crop_ratio +# ) + +# o3d.io.write_point_cloud(os.path.join(output_dir, "source_front.ply"), source_front) +# o3d.io.write_point_cloud(os.path.join(output_dir, "source_tail.ply"), source_tail) +# o3d.io.write_point_cloud(os.path.join(output_dir, "cad_front.ply"), cad_front) +# o3d.io.write_point_cloud(os.path.join(output_dir, "cad_tail.ply"), cad_tail) + +# # ------------------------------------------------------------ +# # 4. Front registration +# # ------------------------------------------------------------ + +# print("\n========== Front Registration ==========") + +# front_result = register_local_region( +# source_front, +# cad_front, +# voxel_size=voxel_size +# ) + +# T_front = front_result.transformation + +# np.savetxt( +# os.path.join(output_dir, "T_front.txt"), +# T_front, +# fmt="%.10f" +# ) + +# # ------------------------------------------------------------ +# # 5. Tail registration +# # ------------------------------------------------------------ + +# print("\n========== Tail Registration ==========") + +# tail_result = register_local_region( +# source_tail, +# cad_tail, +# voxel_size=voxel_size +# ) + +# T_tail = tail_result.transformation + +# np.savetxt( +# os.path.join(output_dir, "T_tail.txt"), +# T_tail, +# fmt="%.10f" +# ) + +# # ------------------------------------------------------------ +# # 6. Check difference between front and tail +# # ------------------------------------------------------------ + +# angle_diff, trans_diff = transformation_difference(T_front, T_tail) + +# print("\n========== Front / Tail Difference ==========") +# print(f"rotation difference: {angle_diff:.6f} deg") +# print(f"translation difference: {trans_diff:.6f}") + +# # 根据 ICP 误差自动给权重 +# # RMSE 越小，权重越大 +# front_weight = 1.0 / (front_result.inlier_rmse + 1e-8) +# tail_weight = 1.0 / (tail_result.inlier_rmse + 1e-8) + +# T_fused = fuse_two_transformations( +# T_front, +# T_tail, +# weight_front=front_weight, +# weight_tail=tail_weight +# ) + +# np.savetxt( +# os.path.join(output_dir, "T_fused_before_final_icp.txt"), +# T_fused, +# fmt="%.10f" +# ) + +# print("\n========== Fused Transformation ==========") +# print(T_fused) + +# # ------------------------------------------------------------ +# # 7. Optional final whole-car low-resolution ICP +# # ------------------------------------------------------------ + +# if final_icp: +# print("\n========== Final Whole-Car ICP ==========") + +# source_whole_down = source_pcd.voxel_down_sample(voxel_size) +# cad_whole_down = cad_pcd.voxel_down_sample(voxel_size) + +# final_result = icp_refine( +# source_whole_down, +# cad_whole_down, +# T_fused, +# voxel_size=voxel_size, +# max_iteration=50 +# ) + +# T_final = final_result.transformation +# final_fitness = final_result.fitness +# final_rmse = final_result.inlier_rmse + +# else: +# T_final = T_fused +# final_fitness = None +# final_rmse = None + +# np.savetxt( +# os.path.join(output_dir, "T_final.txt"), +# T_final, +# fmt="%.10f" +# ) + +# # ------------------------------------------------------------ +# # 8. Save aligned whole point cloud +# # ------------------------------------------------------------ + +# aligned_source = o3d.geometry.PointCloud(source_pcd) +# aligned_source.transform(T_final) + +# aligned_path = os.path.join(output_dir, "aligned_whole_pointcloud.ply") +# o3d.io.write_point_cloud(aligned_path, aligned_source) + +# print("[OK] Saved aligned whole point cloud:") +# print(" ", aligned_path) + +# # ------------------------------------------------------------ +# # 9. Visualization +# # ------------------------------------------------------------ + +# if visualize: +# source_vis = aligned_source.voxel_down_sample(voxel_size) +# cad_vis = cad_pcd.voxel_down_sample(voxel_size) + +# source_vis.paint_uniform_color([1.0, 0.0, 0.0]) +# cad_vis.paint_uniform_color([0.0, 0.5, 1.0]) + +# o3d.visualization.draw_geometries( +# [source_vis, cad_vis], +# window_name="Final Whole Train Registration: red=aligned source, blue=CAD" +# ) + +# return { +# "T_front": T_front, +# "T_tail": T_tail, +# "T_fused": T_fused, +# "T_final": T_final, +# "front_fitness": front_result.fitness, +# "front_rmse": front_result.inlier_rmse, +# "tail_fitness": tail_result.fitness, +# "tail_rmse": tail_result.inlier_rmse, +# "front_tail_rotation_diff_deg": angle_diff, +# "front_tail_translation_diff": trans_diff, +# "final_fitness": final_fitness, +# "final_rmse": final_rmse, +# "aligned_path": aligned_path +# } + + +# # ============================================================ +# # 命令行入口 +# # ============================================================ + +# if __name__ == "__main__": +# parser = argparse.ArgumentParser( +# description="高铁点云与标准 CAD 的前后局部配准 + 整体刚体融合" +# ) + +# parser.add_argument( +# "--cad", +# "-c", +# default="datasets/CAD/11.step", +# help="标准 CAD STEP 文件路径" +# ) + +# parser.add_argument( +# "--pointcloud", +# "-p", +# default="datasets/Noise_PointCloud/11_noisy.ply", +# help="待配准点云 PLY 文件路径" +# ) + +# parser.add_argument( +# "--output_dir", +# "-o", +# default="front_tail_registration_output", +# help="输出目录" +# ) + +# parser.add_argument( +# "--axis", +# default="x", +# choices=["x", "y", "z"], +# help="高铁长度方向。默认 x" +# ) + +# parser.add_argument( +# "--crop_ratio", +# type=float, +# default=0.15, +# help="车头/车尾各取整车长度的比例。默认 0.15" +# ) + +# parser.add_argument( +# "--mesh_deflection", +# type=float, +# default=0.1, +# help="CAD 三角化精度。默认 0.1" +# ) + +# parser.add_argument( +# "--cad_sample_points", +# type=int, +# default=500000, +# help="CAD 表面采样点数。默认 500000" +# ) + +# parser.add_argument( +# "--voxel_size", +# type=float, +# default=10.0, +# help="配准体素大小。高铁大尺度建议 5~30。默认 10" +# ) + +# parser.add_argument( +# "--scale_pointcloud", +# type=float, +# default=1.0, +# help="点云缩放。如果点云是米，CAD 是毫米，设为 1000" +# ) + +# parser.add_argument( +# "--no_final_icp", +# action="store_true", +# help="不做最后整车 ICP 微调" +# ) + +# parser.add_argument( +# "--no_visualize", +# action="store_true", +# help="不显示可视化窗口" +# ) + +# args = parser.parse_args() + +# result = register_train_by_front_tail( +# cad_path=args.cad, +# pointcloud_path=args.pointcloud, +# output_dir=args.output_dir, +# axis=args.axis, +# crop_ratio=args.crop_ratio, +# mesh_deflection=args.mesh_deflection, +# cad_sample_points=args.cad_sample_points, +# voxel_size=args.voxel_size, +# scale_pointcloud=args.scale_pointcloud, +# final_icp=not args.no_final_icp, +# visualize=not args.no_visualize +# ) + +# print("\n========== Final Result ==========") +# print("front fitness:", result["front_fitness"]) +# print("front rmse:", result["front_rmse"]) +# print("tail fitness:", result["tail_fitness"]) +# print("tail rmse:", result["tail_rmse"]) +# print("front-tail rotation diff deg:", result["front_tail_rotation_diff_deg"]) +# print("front-tail translation diff:", result["front_tail_translation_diff"]) +# print("final fitness:", result["final_fitness"]) +# print("final rmse:", result["final_rmse"]) +# print("aligned path:", result["aligned_path"]) +# print("T_final:") +# print(result["T_final"]) + + +import os +import argparse +import numpy as np +import open3d as o3d + +from OCC.Core.STEPControl import STEPControl_Reader +from OCC.Core.IFSelect import IFSelect_RetDone +from OCC.Core.BRepMesh import BRepMesh_IncrementalMesh +from OCC.Core.TopExp import TopExp_Explorer +from OCC.Core.TopAbs import TopAbs_FACE +from OCC.Core.BRep import BRep_Tool + +try: + from OCC.Core.TopoDS import topods + USE_TOPODS = "topods" +except ImportError: + from OCC.Core.TopoDS import TopoDS + USE_TOPODS = "TopoDS" + + +# ============================================================ +# STEP CAD -> Open3D Mesh +# ============================================================ + +def read_step_file(step_path): + reader = STEPControl_Reader() + status = reader.ReadFile(step_path) + + if status != IFSelect_RetDone: + raise RuntimeError(f"STEP 文件读取失败: {step_path}") + + reader.TransferRoots() + shape = reader.OneShape() + + print("[OK] STEP loaded:", step_path) + return shape + + +def occ_shape_to_numpy_mesh(shape): + vertices = [] + triangles = [] + + explorer = TopExp_Explorer(shape, TopAbs_FACE) + + face_count = 0 + tri_count = 0 + + while explorer.More(): + current_shape = explorer.Current() + + if USE_TOPODS == "topods": + face = topods.Face(current_shape) + else: + face = TopoDS.Face(current_shape) + + location = face.Location() + triangulation = BRep_Tool.Triangulation(face, location) + + if triangulation is None: + explorer.Next() + continue + + trsf = location.Transformation() + base_index = len(vertices) + + for i in range(1, triangulation.NbNodes() + 1): + pnt = triangulation.Node(i) + pnt.Transform(trsf) + + vertices.append([ + pnt.X(), + pnt.Y(), + pnt.Z() + ]) + + for i in range(1, triangulation.NbTriangles() + 1): + tri = triangulation.Triangle(i) + n1, n2, n3 = tri.Get() + + triangles.append([ + base_index + n1 - 1, + base_index + n2 - 1, + base_index + n3 - 1 + ]) + + tri_count += 1 + + face_count += 1 + explorer.Next() + + vertices = np.asarray(vertices, dtype=np.float64) + triangles = np.asarray(triangles, dtype=np.int32) + + print("[OK] CAD mesh extracted") + print(" CAD faces:", face_count) + print(" mesh vertices:", vertices.shape) + print(" mesh triangles:", triangles.shape) + + if vertices.shape[0] == 0 or triangles.shape[0] == 0: + raise RuntimeError("CAD 三角网格提取失败，请检查 STEP 文件") + + return vertices, triangles + + +def step_to_open3d_mesh(step_path, mesh_deflection=0.1): + shape = read_step_file(step_path) + + print("[INFO] Meshing CAD...") + mesh_algo = BRepMesh_IncrementalMesh(shape, mesh_deflection) + mesh_algo.Perform() + + vertices, triangles = occ_shape_to_numpy_mesh(shape) + + mesh = o3d.geometry.TriangleMesh() + mesh.vertices = o3d.utility.Vector3dVector(vertices) + mesh.triangles = o3d.utility.Vector3iVector(triangles) + + mesh.remove_duplicated_vertices() + mesh.remove_duplicated_triangles() + mesh.remove_degenerate_triangles() + mesh.compute_vertex_normals() + + return mesh + + +# ============================================================ +# 工具函数 +# ============================================================ + +def axis_to_index(axis): + axis = axis.lower() + + if axis == "x": + return 0 + if axis == "y": + return 1 + if axis == "z": + return 2 + + raise ValueError("axis 必须是 x / y / z") + + +def crop_end_region_by_ratio(pcd, axis="x", part="front", ratio=0.08): + """ + 按长度方向裁剪前端/后端区域。 + + axis: + 高铁长度方向，一般是 x + + part: + front: 轴最小端 + tail: 轴最大端 + + ratio: + 取前后端占整体长度的比例，例如 0.08 表示取前 8% 和后 8% + """ + + points = np.asarray(pcd.points) + + axis_id = axis_to_index(axis) + + min_bound = points.min(axis=0) + max_bound = points.max(axis=0) + + axis_min = min_bound[axis_id] + axis_max = max_bound[axis_id] + axis_len = axis_max - axis_min + + if part == "front": + lower = axis_min + upper = axis_min + axis_len * ratio + elif part == "tail": + lower = axis_max - axis_len * ratio + upper = axis_max + else: + raise ValueError("part 必须是 front 或 tail") + + crop_min = min_bound.copy() + crop_max = max_bound.copy() + + crop_min[axis_id] = lower + crop_max[axis_id] = upper + + bbox = o3d.geometry.AxisAlignedBoundingBox( + min_bound=crop_min, + max_bound=crop_max + ) + + cropped = pcd.crop(bbox) + + print(f"[OK] Crop {part}") + print(" range:", lower, upper) + print(" points:", np.asarray(cropped.points).shape) + + return cropped + + +def sample_farthest_points_simple(pcd, num_points=5000): + """ + 为了避免百万点太多，这里随机采样少量点。 + 如果点数不足，则直接返回原点云。 + """ + + points = np.asarray(pcd.points) + n = points.shape[0] + + if n <= num_points: + return pcd + + indices = np.random.choice(n, size=num_points, replace=False) + + sampled = pcd.select_by_index(indices.tolist()) + + return sampled + + +def compute_centroid(pcd): + points = np.asarray(pcd.points) + + if points.shape[0] == 0: + raise RuntimeError("点云为空，无法计算质心") + + return points.mean(axis=0) + + +def build_translation_matrix(t): + T = np.eye(4, dtype=np.float64) + T[:3, 3] = t + return T + + +def visualize_alignment(source_pcd, cad_pcd, title="Translation Alignment Result"): + source_vis = o3d.geometry.PointCloud(source_pcd) + cad_vis = o3d.geometry.PointCloud(cad_pcd) + + source_vis.paint_uniform_color([1.0, 0.0, 0.0]) + cad_vis.paint_uniform_color([0.0, 0.5, 1.0]) + + o3d.visualization.draw_geometries( + [source_vis, cad_vis], + window_name=title + ) + + +# ============================================================ +# 只估计平移的配准 +# ============================================================ + +def resolve_pointcloud_path(pointcloud_path): + if os.path.isdir(pointcloud_path): + preferred = os.path.join(pointcloud_path, "shifted_whole_pointcloud.ply") + if os.path.exists(preferred): + return preferred + + candidates = [ + os.path.join(pointcloud_path, name) + for name in sorted(os.listdir(pointcloud_path)) + if name.lower().endswith(".ply") + ] + if len(candidates) == 1: + return candidates[0] + if not candidates: + raise FileNotFoundError(f"No PLY point cloud found in directory: {pointcloud_path}") + raise ValueError( + "Multiple PLY files found in directory; specify one file explicitly: " + + ", ".join(os.path.basename(path) for path in candidates) + ) + + return pointcloud_path + + +def translate_align_by_front_tail( + cad_path, + pointcloud_path, + output_dir="results", + axis="x", + crop_ratio=0.08, + mesh_deflection=0.1, + cad_sample_points=300000, + local_sample_points=10000, + scale_pointcloud=1.0, + visualize=True +): + os.makedirs(output_dir, exist_ok=True) + + # ------------------------------------------------------------ + # 1. CAD STEP 转 mesh，再采样成 CAD 点云 + # ------------------------------------------------------------ + + cad_mesh = step_to_open3d_mesh( + cad_path, + mesh_deflection=mesh_deflection + ) + + print(f"[INFO] Sampling CAD mesh: {cad_sample_points} points") + cad_pcd = cad_mesh.sample_points_uniformly( + number_of_points=cad_sample_points + ) + + cad_pcd_path = os.path.join(output_dir, "cad_sampled.ply") + o3d.io.write_point_cloud(cad_pcd_path, cad_pcd) + + # ------------------------------------------------------------ + # 2. 读取扫描点云 + # ------------------------------------------------------------ + + pointcloud_path = resolve_pointcloud_path(pointcloud_path) + source_pcd = o3d.io.read_point_cloud(pointcloud_path) + + if len(source_pcd.points) == 0: + raise RuntimeError(f"点云读取失败: {pointcloud_path}") + + print("[OK] Source point cloud loaded:", pointcloud_path) + print(" source points:", np.asarray(source_pcd.points).shape) + + if scale_pointcloud != 1.0: + source_pcd.scale(scale_pointcloud, center=(0, 0, 0)) + print("[INFO] Source scaled:", scale_pointcloud) + + # ------------------------------------------------------------ + # 3. 裁剪 CAD 前端 / 后端 + # ------------------------------------------------------------ + + cad_front = crop_end_region_by_ratio( + cad_pcd, + axis=axis, + part="front", + ratio=crop_ratio + ) + + cad_tail = crop_end_region_by_ratio( + cad_pcd, + axis=axis, + part="tail", + ratio=crop_ratio + ) + + # ------------------------------------------------------------ + # 4. 裁剪扫描点云前端 / 后端 + # ------------------------------------------------------------ + + source_front = crop_end_region_by_ratio( + source_pcd, + axis=axis, + part="front", + ratio=crop_ratio + ) + + source_tail = crop_end_region_by_ratio( + source_pcd, + axis=axis, + part="tail", + ratio=crop_ratio + ) + + # ------------------------------------------------------------ + # 5. 前后区域分别随机取少量点 + # ------------------------------------------------------------ + + cad_front_sample = sample_farthest_points_simple( + cad_front, + num_points=local_sample_points + ) + + cad_tail_sample = sample_farthest_points_simple( + cad_tail, + num_points=local_sample_points + ) + + source_front_sample = sample_farthest_points_simple( + source_front, + num_points=local_sample_points + ) + + source_tail_sample = sample_farthest_points_simple( + source_tail, + num_points=local_sample_points + ) + + # 保存局部采样结果，方便检查 + o3d.io.write_point_cloud(os.path.join(output_dir, "cad_front_sample.ply"), cad_front_sample) + o3d.io.write_point_cloud(os.path.join(output_dir, "cad_tail_sample.ply"), cad_tail_sample) + o3d.io.write_point_cloud(os.path.join(output_dir, "source_front_sample.ply"), source_front_sample) + o3d.io.write_point_cloud(os.path.join(output_dir, "source_tail_sample.ply"), source_tail_sample) + + # ------------------------------------------------------------ + # 6. 计算前端和后端质心 + # ------------------------------------------------------------ + + cad_front_center = compute_centroid(cad_front_sample) + cad_tail_center = compute_centroid(cad_tail_sample) + + source_front_center = compute_centroid(source_front_sample) + source_tail_center = compute_centroid(source_tail_sample) + + print("\n========== Centroids ==========") + print("CAD front center: ", cad_front_center) + print("CAD tail center: ", cad_tail_center) + print("Source front center:", source_front_center) + print("Source tail center: ", source_tail_center) + + # ------------------------------------------------------------ + # 7. 分别根据车头和车尾估计平移 + # ------------------------------------------------------------ + + t_front = cad_front_center - source_front_center + t_tail = cad_tail_center - source_tail_center + + print("\n========== Translation From Front / Tail ==========") + print("t_front:", t_front) + print("t_tail: ", t_tail) + print("front-tail translation difference:", np.linalg.norm(t_front - t_tail)) + + # ------------------------------------------------------------ + # 8. 融合平移 + # ------------------------------------------------------------ + + t_final = 0.5 * t_front + 0.5 * t_tail + + T_final = build_translation_matrix(t_final) + + print("\n========== Final Translation ==========") + print("t_final:", t_final) + print("T_final:") + print(T_final) + + # ------------------------------------------------------------ + # 9. 应用平移到整个点云 + # ------------------------------------------------------------ + + aligned_pcd = o3d.geometry.PointCloud(source_pcd) + aligned_pcd.transform(T_final) + + aligned_path = os.path.join(output_dir, "aligned_pointcloud_translation_only.ply") + matrix_path = os.path.join(output_dir, "T_translation_only.txt") + + o3d.io.write_point_cloud(aligned_path, aligned_pcd) + np.savetxt(matrix_path, T_final, fmt="%.10f") + + print("\n[OK] Saved aligned point cloud:", aligned_path) + print("[OK] Saved transformation matrix:", matrix_path) + + # ------------------------------------------------------------ + # 10. 可视化 + # ------------------------------------------------------------ + + if visualize: + # 为了不卡，显示时降采样 + source_vis = aligned_pcd.voxel_down_sample(voxel_size=max(crop_ratio, 1.0)) + cad_vis = cad_pcd.voxel_down_sample(voxel_size=max(crop_ratio, 1.0)) + + source_vis.paint_uniform_color([1.0, 0.0, 0.0]) + cad_vis.paint_uniform_color([0.0, 0.5, 1.0]) + + o3d.visualization.draw_geometries( + [source_vis, cad_vis], + window_name="Translation Only Alignment: red=aligned source, blue=CAD" + ) + + return { + "translation": t_final, + "T_final": T_final, + "t_front": t_front, + "t_tail": t_tail, + "front_tail_translation_difference": float(np.linalg.norm(t_front - t_tail)), + "aligned_path": aligned_path, + "matrix_path": matrix_path + } + + +# ============================================================ +# 命令行入口 +# ============================================================ + +if __name__ == "__main__": + parser = argparse.ArgumentParser( + description="高铁点云与 CAD 的仅平移配准：取车头车尾少量点估计整体平移" + ) + + parser.add_argument( + "--cad", + "-c", + default="datasets/CAD/M02.stp", + help="CAD STEP 文件路径" + ) + + parser.add_argument( + "--pointcloud", + "-p", + default="datasets/shift_test_output/", + help="待配准点云 PLY 文件路径" + ) + + parser.add_argument( + "--output_dir", + "-o", + default="results", + help="输出目录" + ) + + parser.add_argument( + "--axis", + default="x", + choices=["x", "y", "z"], + help="高铁长度方向，默认 x" + ) + + parser.add_argument( + "--crop_ratio", + type=float, + default=0.01, + help="车头/车尾各取总长度的比例，默认 0.01" + ) + + parser.add_argument( + "--mesh_deflection", + type=float, + default=0.1, + help="CAD 三角化精度，默认 0.1" + ) + + parser.add_argument( + "--cad_sample_points", + type=int, + default=30000, + help="从 CAD mesh 采样的点数，默认 30000" + ) + + parser.add_argument( + "--local_sample_points", + type=int, + default=1000, + help="车头/车尾各随机采样多少点用于计算质心，默认 1000" + ) + + parser.add_argument( + "--scale_pointcloud", + type=float, + default=1.0, + help="点云缩放。如果点云是米，CAD 是毫米，设为 1000" + ) + + parser.add_argument( + "--no_visualize", + action="store_true", + help="不显示可视化窗口" + ) + + args = parser.parse_args() + + result = translate_align_by_front_tail( + cad_path=args.cad, + pointcloud_path=args.pointcloud, + output_dir=args.output_dir, + axis=args.axis, + crop_ratio=args.crop_ratio, + mesh_deflection=args.mesh_deflection, + cad_sample_points=args.cad_sample_points, + local_sample_points=args.local_sample_points, + scale_pointcloud=args.scale_pointcloud, + visualize=not args.no_visualize + ) + + print("\n========== Result ==========") + print("translation:", result["translation"]) + print("front-tail translation difference:", result["front_tail_translation_difference"]) + print("aligned path:", result["aligned_path"]) + print("matrix path:", result["matrix_path"]) diff --git a/alg/point_porj/cad_converter.py b/alg/point_porj/cad_converter.py new file mode 100644 index 0000000..9f08899 --- /dev/null +++ b/alg/point_porj/cad_converter.py @@ -0,0 +1,118 @@ +from __future__ import annotations + +import time +from pathlib import Path + +import numpy as np + + +class CADConverter: + """Convert STEP CAD files to STL meshes.""" + + def read_step_shape(self, input_step: str | Path): + try: + from OCC.Core.IFSelect import IFSelect_RetDone + from OCC.Core.STEPControl import STEPControl_Reader + except ModuleNotFoundError as exc: + raise RuntimeError( + "pythonocc-core is required for CAD conversion. " + "Install it with conda-forge before running CAD workflows." + ) from exc + + input_step = Path(input_step) + step_reader = STEPControl_Reader() + if step_reader.ReadFile(str(input_step)) != IFSelect_RetDone: + raise ValueError(f"Failed to read STEP file: {input_step}") + step_reader.TransferRoots() + return step_reader.OneShape() + + def step_to_numpy_mesh( + self, + input_step: str | Path, + mesh_deflection: float = 0.1, + ) -> tuple[np.ndarray, np.ndarray]: + try: + from OCC.Core.BRep import BRep_Tool + from OCC.Core.BRepMesh import BRepMesh_IncrementalMesh + from OCC.Core.TopAbs import TopAbs_FACE + from OCC.Core.TopExp import TopExp_Explorer + + try: + from OCC.Core.TopoDS import topods + except ImportError: + from OCC.Core.TopoDS import TopoDS + + topods = None + except ModuleNotFoundError as exc: + raise RuntimeError( + "pythonocc-core is required for CAD mesh extraction. " + "Install it with conda-forge before running CAD workflows." + ) from exc + + shape = self.read_step_shape(input_step) + mesh = BRepMesh_IncrementalMesh(shape, mesh_deflection, False) + mesh.Perform() + if not mesh.IsDone(): + raise RuntimeError("CAD mesh discretization failed.") + + vertices: list[list[float]] = [] + faces: list[list[int]] = [] + explorer = TopExp_Explorer(shape, TopAbs_FACE) + + while explorer.More(): + current_shape = explorer.Current() + face = topods.Face(current_shape) if topods is not None else TopoDS.Face(current_shape) + location = face.Location() + triangulation = BRep_Tool.Triangulation(face, location) + if triangulation is None: + explorer.Next() + continue + + transform = location.Transformation() + base_index = len(vertices) + for index in range(1, triangulation.NbNodes() + 1): + point = triangulation.Node(index) + point.Transform(transform) + vertices.append([point.X(), point.Y(), point.Z()]) + + for index in range(1, triangulation.NbTriangles() + 1): + n1, n2, n3 = triangulation.Triangle(index).Get() + faces.append([base_index + n1 - 1, base_index + n2 - 1, base_index + n3 - 1]) + + explorer.Next() + + vertices_np = np.asarray(vertices, dtype=np.float64) + faces_np = np.asarray(faces, dtype=np.int64) + if len(vertices_np) == 0 or len(faces_np) == 0: + raise RuntimeError("CAD triangle mesh extraction failed; try a smaller mesh deflection.") + return vertices_np, faces_np + + def convert_step_to_stl(self, input_step: str | Path, output_stl: str | Path, precision: float = 1.0) -> Path: + try: + from OCC.Core.BRepMesh import BRepMesh_IncrementalMesh + from OCC.Core.StlAPI import StlAPI_Writer + except ModuleNotFoundError as exc: + raise RuntimeError( + "pythonocc-core is required for CAD conversion. " + "Install it with conda-forge before running cad2mesh." + ) from exc + + input_step = Path(input_step) + output_stl = Path(output_stl) + output_stl.parent.mkdir(parents=True, exist_ok=True) + + start = time.time() + print(f"[1/3] Reading STEP: {input_step}") + shape = self.read_step_shape(input_step) + + print(f"[2/3] Meshing with precision {precision} mm") + mesh = BRepMesh_IncrementalMesh(shape, precision, False) + mesh.Perform() + if not mesh.IsDone(): + raise RuntimeError("Mesh discretization failed.") + + print(f"[3/3] Writing STL: {output_stl}") + writer = StlAPI_Writer() + writer.Write(shape, str(output_stl)) + print(f"CAD conversion finished in {time.time() - start:.2f}s") + return output_stl diff --git a/alg/point_porj/config.yaml b/alg/point_porj/config.yaml new file mode 100644 index 0000000..fab15fe --- /dev/null +++ b/alg/point_porj/config.yaml @@ -0,0 +1,59 @@ +paths: + cad: datasets/CAD/11.step + mesh: datasets/Mesh/11.stl + pointcloud: datasets/PointCloud/11.ply + noisy_pointcloud: datasets/Noise_PointCloud/11_noisy.ply + output_mesh: datasets/Mesh/11.stl + curvature_output: + +cad: + enabled: false + precision: 1.0 + +noise: + enabled: true + std: 4.0 + seed: + +filters: + enabled: true + methods: + - SOR + sor_neighbors: 20 + sor_std_ratio: 2.0 + ror_points: 5 + ror_radius: 20.0 + voxel_size: 0.05 + pass_axis: z + pass_min: + pass_max: + bf_sigma_s: 0.01 + bf_sigma_r: 0.1 + +registration: + voxel_size: 10.0 + roll: 10.0 + pitch: 10.0 + yaw: 10.0 + trans_range: 0.0 + random_seed: + +evaluation: + chunk_size: 2048 + triangle_chunk_size: 4096 + cad_mesh_deflection: 0.1 + +curvature: + radius: 2.0 + max_nn: 30 + color_scale: 3.0 + +plane: + size_mm: 1000.0 + noise_std_mm: 0.5 + num_points: 10000 + threshold_mm: 0.5 + max_iterations: 2000 + +visualization: + enabled: true diff --git a/alg/point_porj/config_loader.py b/alg/point_porj/config_loader.py new file mode 100644 index 0000000..d3a771d --- /dev/null +++ b/alg/point_porj/config_loader.py @@ -0,0 +1,104 @@ +from __future__ import annotations + +from copy import deepcopy +from pathlib import Path +from typing import Any + +import yaml + + +DEFAULT_CONFIG: dict[str, Any] = { + "paths": { + "cad": "CAD/11.step", + "mesh": "Mesh/11.stl", + "pointcloud": "PointCloud/11.ply", + "noisy_pointcloud": "Noise_PointCloud/11_noisy.ply", + "output_mesh": "Mesh/11.stl", + "curvature_output": None, + }, + "cad": { + "precision": 1.0, + "enabled": False, + }, + "noise": { + "enabled": True, + "std": 4.0, + "seed": None, + }, + "filters": { + "enabled": True, + "methods": ["SOR"], + "sor_neighbors": 20, + "sor_std_ratio": 2.0, + "ror_points": 5, + "ror_radius": 20.0, + "voxel_size": 0.05, + "pass_axis": "z", + "pass_min": None, + "pass_max": None, + "bf_sigma_s": 0.01, + "bf_sigma_r": 0.1, + }, + "registration": { + "voxel_size": 10.0, + "roll": 10.0, + "pitch": 10.0, + "yaw": 10.0, + "trans_range": 0.0, + "random_seed": None, + }, + "evaluation": { + "chunk_size": 2048, + "triangle_chunk_size": 4096, + "cad_mesh_deflection": 0.1, + }, + "curvature": { + "radius": 2.0, + "max_nn": 30, + "color_scale": 3.0, + }, + "plane": { + "size_mm": 1000.0, + "noise_std_mm": 0.5, + "num_points": 10000, + "threshold_mm": 0.5, + "max_iterations": 2000, + }, + "visualization": { + "enabled": True, + }, +} + + +def deep_update(base: dict[str, Any], updates: dict[str, Any]) -> dict[str, Any]: + result = deepcopy(base) + for key, value in updates.items(): + if isinstance(value, dict) and isinstance(result.get(key), dict): + result[key] = deep_update(result[key], value) + else: + result[key] = value + return result + + +class ConfigLoader: + """Load YAML config and merge it with project defaults.""" + + def __init__(self, default_config: dict[str, Any] | None = None) -> None: + self.default_config = default_config or DEFAULT_CONFIG + + def load(self, config_path: str | None = None) -> dict[str, Any]: + config = deepcopy(self.default_config) + if not config_path: + return config + + path = Path(config_path) + if not path.exists(): + raise FileNotFoundError(f"Config file not found: {path}") + + with path.open("r", encoding="utf-8") as f: + user_config = yaml.safe_load(f) or {} + + if not isinstance(user_config, dict): + raise ValueError("Config file must contain a YAML mapping at the top level.") + + return deep_update(config, user_config) diff --git a/alg/point_porj/environment.yml b/alg/point_porj/environment.yml new file mode 100644 index 0000000..a97f8e3 --- /dev/null +++ b/alg/point_porj/environment.yml @@ -0,0 +1,7 @@ +name: point-porj +channels: + - https://mirrors.tuna.tsinghua.edu.cn/anaconda/cloud/conda-forge +dependencies: + - python=3.10 + - pip + - pythonocc-core=7.5.1 diff --git a/alg/point_porj/evaluation.py b/alg/point_porj/evaluation.py new file mode 100644 index 0000000..6ae7b23 --- /dev/null +++ b/alg/point_porj/evaluation.py @@ -0,0 +1,203 @@ +from __future__ import annotations + +from dataclasses import dataclass +from typing import Any + +import numpy as np +import open3d as o3d +import torch +import trimesh +from scipy.spatial import KDTree + + +@dataclass +class DistanceStats: + mean: float + max: float + rmse: float + min: float + std: float + + +class Evaluator: + """Evaluation metrics for point-cloud workflows.""" + + def chamfer_distance( + self, + pointcloud_a: o3d.geometry.PointCloud, + pointcloud_b: o3d.geometry.PointCloud, + sqrt: bool = True, + ) -> tuple[float, float, float]: + points_a = np.asarray(pointcloud_a.points) + points_b = np.asarray(pointcloud_b.points) + if len(points_a) == 0 or len(points_b) == 0: + return float("inf"), 0.0, 0.0 + + tree_a = KDTree(points_a) + tree_b = KDTree(points_b) + dist_a, _ = tree_b.query(points_a, k=1) + dist_b, _ = tree_a.query(points_b, k=1) + cd_a = float(np.mean(dist_a ** 2)) + cd_b = float(np.mean(dist_b ** 2)) + cd = cd_a + cd_b + return (float(np.sqrt(cd)), cd_a, cd_b) if sqrt else (float(cd), cd_a, cd_b) + + def point_to_mesh_distance_trimesh(self, points: np.ndarray, mesh_path: str) -> tuple[np.ndarray, DistanceStats]: + mesh = trimesh.load(mesh_path, force="mesh") + _, distances, _ = mesh.nearest.on_surface(points) + distances = np.asarray(distances, dtype=np.float64) + return distances, self.distance_stats(distances) + + def point_to_mesh_distance_torch( + self, + points: np.ndarray, + mesh: trimesh.Trimesh, + chunk_size: int = 2048, + triangle_chunk_size: int | None = 4096, + ) -> tuple[np.ndarray, DistanceStats]: + vertices, faces = self._mesh_arrays(mesh) + return self.point_to_triangles_distance_torch( + points, + vertices, + faces, + point_chunk_size=chunk_size, + triangle_chunk_size=triangle_chunk_size, + ) + + def point_to_triangles_distance_torch( + self, + points: np.ndarray, + vertices: np.ndarray, + faces: np.ndarray, + point_chunk_size: int = 2048, + triangle_chunk_size: int | None = 4096, + ) -> tuple[np.ndarray, DistanceStats]: + device = torch.device("cuda" if torch.cuda.is_available() else "cpu") + dtype = torch.float64 + vertices_t = torch.as_tensor(np.asarray(vertices, dtype=np.float64), dtype=dtype, device=device) + faces_t = torch.as_tensor(np.asarray(faces, dtype=np.int64), dtype=torch.long, device=device) + triangles = vertices_t[faces_t] + points_t = torch.as_tensor(points, dtype=dtype, device=device) + + distances = [] + with torch.no_grad(): + for start in range(0, points_t.shape[0], point_chunk_size): + chunk = points_t[start:start + point_chunk_size] + if triangle_chunk_size is None or triangle_chunk_size <= 0: + dist2 = self._point_to_triangle_distance_squared(chunk, triangles) + min_dist2 = dist2.min(dim=1).values + else: + min_dist2 = torch.full( + (chunk.shape[0],), + torch.inf, + dtype=points_t.dtype, + device=device, + ) + for triangle_start in range(0, triangles.shape[0], triangle_chunk_size): + triangle_chunk = triangles[triangle_start:triangle_start + triangle_chunk_size] + dist2 = self._point_to_triangle_distance_squared(chunk, triangle_chunk) + min_dist2 = torch.minimum(min_dist2, dist2.min(dim=1).values) + + distances.append(torch.sqrt(min_dist2).cpu().numpy()) + + all_distances = np.concatenate(distances) if distances else np.array([], dtype=np.float64) + return all_distances, self.distance_stats(all_distances) + + def colorize_distances(self, points: np.ndarray, distances: np.ndarray) -> o3d.geometry.PointCloud: + if len(points) != len(distances): + raise ValueError("points and distances must have the same length.") + pointcloud = o3d.geometry.PointCloud() + pointcloud.points = o3d.utility.Vector3dVector(np.asarray(points, dtype=np.float64)) + + if len(distances) == 0: + pointcloud.colors = o3d.utility.Vector3dVector(np.empty((0, 3), dtype=np.float64)) + return pointcloud + + lower = float(np.min(distances)) + upper = float(np.max(distances)) + normalized = (distances - lower) / (upper - lower + 1e-12) + colors = self._blue_to_red_colormap(normalized) + pointcloud.colors = o3d.utility.Vector3dVector(colors) + return pointcloud + + def distance_stats(self, distances: np.ndarray) -> DistanceStats: + if len(distances) == 0: + return DistanceStats(0.0, 0.0, 0.0, 0.0, 0.0) + return DistanceStats( + mean=float(np.mean(distances)), + max=float(np.max(distances)), + rmse=float(np.sqrt(np.mean(distances ** 2))), + min=float(np.min(distances)), + std=float(np.std(distances)), + ) + + def registration_summary(self, transformation: np.ndarray, fitness: float, rmse: float) -> dict[str, Any]: + return { + "fitness": fitness, + "rmse": rmse, + "transformation": transformation, + } + + def _mesh_arrays(self, mesh: trimesh.Trimesh) -> tuple[np.ndarray, np.ndarray]: + vertices = np.asarray(mesh.vertices, dtype=np.float64) + faces = np.asarray(mesh.faces, dtype=np.int64) + if len(vertices) == 0 or len(faces) == 0: + raise ValueError("Mesh must contain vertices and faces.") + return vertices, faces + + def _blue_to_red_colormap(self, values: np.ndarray) -> np.ndarray: + values = np.clip(np.asarray(values, dtype=np.float64), 0.0, 1.0) + colors = np.zeros((len(values), 3), dtype=np.float64) + colors[:, 0] = np.clip(1.8 * values - 0.2, 0.0, 1.0) + colors[:, 1] = np.clip(1.0 - np.abs(values - 0.5) * 2.0, 0.0, 1.0) + colors[:, 2] = np.clip(1.2 - 1.8 * values, 0.0, 1.0) + return colors + + def _point_to_triangle_distance_squared(self, points: torch.Tensor, triangles: torch.Tensor) -> torch.Tensor: + p = points[:, None, :] + a = triangles[None, :, 0, :] + b = triangles[None, :, 1, :] + c = triangles[None, :, 2, :] + + ab = b - a + ac = c - a + ap = p - a + + d1 = (ab * ap).sum(dim=-1) + d2 = (ac * ap).sum(dim=-1) + inf = torch.full_like(d1, torch.inf) + dist2 = torch.where((d1 <= 0.0) & (d2 <= 0.0), (ap * ap).sum(dim=-1), inf) + + bp = p - b + d3 = (ab * bp).sum(dim=-1) + d4 = (ac * bp).sum(dim=-1) + dist2 = torch.minimum(dist2, torch.where((d3 >= 0.0) & (d4 <= d3), (bp * bp).sum(dim=-1), inf)) + + vc = d1 * d4 - d3 * d2 + mask = (vc <= 0.0) & (d1 >= 0.0) & (d3 <= 0.0) + v = d1 / (d1 - d3).clamp_min(torch.finfo(points.dtype).eps) + closest = a + v[..., None] * ab + dist2 = torch.minimum(dist2, torch.where(mask, ((p - closest) ** 2).sum(dim=-1), inf)) + + cp = p - c + d5 = (ab * cp).sum(dim=-1) + d6 = (ac * cp).sum(dim=-1) + dist2 = torch.minimum(dist2, torch.where((d6 >= 0.0) & (d5 <= d6), (cp * cp).sum(dim=-1), inf)) + + vb = d5 * d2 - d1 * d6 + mask = (vb <= 0.0) & (d2 >= 0.0) & (d6 <= 0.0) + w = d2 / (d2 - d6).clamp_min(torch.finfo(points.dtype).eps) + closest = a + w[..., None] * ac + dist2 = torch.minimum(dist2, torch.where(mask, ((p - closest) ** 2).sum(dim=-1), inf)) + + va = d3 * d6 - d5 * d4 + mask = (va <= 0.0) & ((d4 - d3) >= 0.0) & ((d5 - d6) >= 0.0) + w = (d4 - d3) / ((d4 - d3) + (d5 - d6)).clamp_min(torch.finfo(points.dtype).eps) + closest = b + w[..., None] * (c - b) + dist2 = torch.minimum(dist2, torch.where(mask, ((p - closest) ** 2).sum(dim=-1), inf)) + + denom = (va + vb + vc).clamp_min(torch.finfo(points.dtype).eps) + v = vb / denom + w = vc / denom + closest = a + ab * v[..., None] + ac * w[..., None] + return torch.minimum(dist2, ((p - closest) ** 2).sum(dim=-1)) diff --git a/alg/point_porj/io_handler.py b/alg/point_porj/io_handler.py new file mode 100644 index 0000000..c921787 --- /dev/null +++ b/alg/point_porj/io_handler.py @@ -0,0 +1,69 @@ +from __future__ import annotations + +from pathlib import Path +from typing import Any + +import numpy as np +import open3d as o3d +import trimesh + + +class IOHandler: + """Centralized input validation and file loading.""" + + def resolve_path(self, path: str | Path) -> Path: + return Path(path).expanduser() + + def require_file(self, path: str | Path, label: str = "file") -> Path: + resolved = self.resolve_path(path) + if not resolved.exists(): + raise FileNotFoundError(f"{label} not found: {resolved}") + if not resolved.is_file(): + raise ValueError(f"{label} is not a file: {resolved}") + return resolved + + def ensure_parent_dir(self, path: str | Path) -> Path: + resolved = self.resolve_path(path) + resolved.parent.mkdir(parents=True, exist_ok=True) + return resolved + + def load_point_cloud(self, pointcloud_path: str | Path) -> o3d.geometry.PointCloud: + path = self.require_file(pointcloud_path, "Point cloud") + pcd = o3d.io.read_point_cloud(str(path)) + if len(pcd.points) == 0: + raise ValueError(f"Point cloud is empty or unreadable: {path}") + return pcd + + def load_mesh(self, mesh_path: str | Path) -> trimesh.Trimesh: + path = self.require_file(mesh_path, "Mesh") + mesh = trimesh.load(str(path), force="mesh") + if mesh.is_empty: + raise ValueError(f"Mesh is empty or unreadable: {path}") + return mesh + + def require_cad(self, cad_path: str | Path) -> Path: + return self.require_file(cad_path, "CAD") + + def save_point_cloud(self, pointcloud: o3d.geometry.PointCloud, output_path: str | Path) -> Path: + path = self.ensure_parent_dir(output_path) + ok = o3d.io.write_point_cloud(str(path), pointcloud) + if not ok: + raise IOError(f"Failed to save point cloud: {path}") + return path + + def point_cloud_to_numpy(self, pointcloud: o3d.geometry.PointCloud) -> np.ndarray: + points = np.asarray(pointcloud.points, dtype=np.float64) + if points.size == 0: + raise ValueError("Point cloud has no points.") + return points + + def mesh_triangles_to_numpy(self, mesh: trimesh.Trimesh) -> tuple[np.ndarray, np.ndarray]: + vertices = np.asarray(mesh.vertices, dtype=np.float64) + faces = np.asarray(mesh.faces, dtype=np.int64) + if len(vertices) == 0 or len(faces) == 0: + raise ValueError("Mesh must contain vertices and faces.") + return vertices, faces + + def print_mapping(self, data: dict[str, Any]) -> None: + for key, value in data.items(): + print(f"{key}: {value}") diff --git a/alg/point_porj/main.py b/alg/point_porj/main.py new file mode 100644 index 0000000..6924a38 --- /dev/null +++ b/alg/point_porj/main.py @@ -0,0 +1,189 @@ +from __future__ import annotations + +import argparse + + +def build_parser() -> argparse.ArgumentParser: + common = argparse.ArgumentParser(add_help=False) + common.add_argument("--config", default=argparse.SUPPRESS, help="YAML config path, default uses built-in settings") + + parser = argparse.ArgumentParser(description="Industrial geometry measurement workflow") + parser.add_argument("--config", default=None, help="YAML config path, default uses built-in settings") + subparsers = parser.add_subparsers(dest="command", required=True) + + cad = subparsers.add_parser("cad2mesh", parents=[common], help="Convert STEP CAD to STL Mesh") + cad.add_argument("--cad", required=False, help="Input STEP file") + cad.add_argument("--mesh", required=False, help="Output STL file") + cad.add_argument("--precision", type=float, default=None, help="Mesh precision in mm") + + denoise = subparsers.add_parser("denoise", parents=[common], help="Add noise and run denoise filters") + denoise.add_argument("--pointcloud", "-p", default=None, help="Input clean PLY point cloud") + denoise.add_argument("--noise-std", type=float, default=None, help="Gaussian noise std") + denoise.add_argument("--no-visualize", action="store_true", help="Disable Open3D windows") + + registration = subparsers.add_parser("registration", parents=[common], help="Run simulated point-cloud registration") + registration.add_argument("--pointcloud", "-p", default=None, help="Input PLY point cloud") + registration.add_argument("--mesh", "-m", default=None, help="Reference STL/OBJ/PLY mesh") + registration.add_argument("--voxel-size", "-v", type=float, default=None, help="Registration voxel size") + registration.add_argument("--roll", type=float, default=None, help="Rotation around X axis in degrees") + registration.add_argument("--pitch", type=float, default=None, help="Rotation around Y axis in degrees") + registration.add_argument("--yaw", type=float, default=None, help="Rotation around Z axis in degrees") + registration.add_argument("--trans-range", type=float, default=None, help="Random translation range") + registration.add_argument("--no-visualize", action="store_true", help="Disable Open3D windows") + + contour = subparsers.add_parser( + "contour", + parents=[common], + help="Quick contour/flatness/planarity evaluation against STEP CAD", + ) + contour.add_argument("--pointcloud", "-p", default=None, help="Input PLY point cloud") + contour.add_argument("--cad", "-c", default=None, help="Reference STEP CAD file") + contour.add_argument("--mesh", "-m", default=None, help="Legacy reference STL/OBJ/PLY mesh") + contour.add_argument("--tasks", default=None, help="Comma-separated task ids for batch contour evaluation") + contour.add_argument("--all", action="store_true", help="Run contour evaluation for all CAD ids under data root") + contour.add_argument("--data-root", default="datasets", help="Dataset root for --tasks/--all batch mode") + contour.add_argument( + "--pointcloud-kind", + choices=["noise", "clean"], + default="noise", + help="Point cloud source for batch mode: noise=Noise_PointCloud, clean=PointCloud", + ) + contour.add_argument("--mesh-deflection", type=float, default=None, help="CAD meshing deflection") + contour.add_argument("--point-chunk-size", type=int, default=None, help="Point chunk size") + contour.add_argument("--triangle-chunk-size", type=int, default=None, help="Triangle chunk size") + contour.add_argument("--save", default=None, help="Optional colored point-cloud output path") + contour.add_argument( + "--excel-output", + nargs="?", + const="excel/contour.xlsx", + default=None, + help="Optional Excel output path; defaults to excel/contour.xlsx when used without a value", + ) + contour.add_argument("--no-visualize", action="store_true", help="Disable Open3D windows") + + curvature = subparsers.add_parser("curvature", parents=[common], help="Estimate and visualize point-cloud curvature") + curvature.add_argument("--pointcloud", "-p", default=None, help="Input PLY point cloud") + curvature.add_argument("--radius", type=float, default=None, help="Neighborhood radius") + curvature.add_argument("--max-nn", type=int, default=None, help="Max neighbor count") + curvature.add_argument("--color-scale", type=float, default=None, help="Color scale") + curvature.add_argument("--save", default=None, help="Optional colored point-cloud output path") + curvature.add_argument("--no-visualize", action="store_true", help="Disable Open3D windows") + + plane = subparsers.add_parser("plane", parents=[common], help="Estimate plane with RANSAC") + plane.add_argument("--pointcloud", "-p", default=None, help="Optional input PLY point cloud") + plane.add_argument("--threshold-mm", type=float, default=None, help="RANSAC distance threshold in mm") + plane.add_argument("--max-iterations", type=int, default=None, help="RANSAC max iterations") + plane.add_argument("--no-visualize", action="store_true", help="Disable Open3D windows") + + translate_align = subparsers.add_parser( + "translate-align", + parents=[common], + help="Translate-align point cloud to STEP CAD with front/tail centroids", + ) + translate_align.add_argument("--cad", "-c", default="datasets/CAD/M02.stp", help="Reference STEP CAD file") + translate_align.add_argument( + "--pointcloud", + "-p", + default="datasets/shift_test_output/", + help="Input PLY point cloud or a directory containing shifted_whole_pointcloud.ply", + ) + translate_align.add_argument("--output-dir", "-o", default="results", help="Output directory") + translate_align.add_argument("--axis", choices=["x", "y", "z"], default="x", help="Train length axis") + translate_align.add_argument("--crop-ratio", type=float, default=0.01, help="Front/tail crop ratio") + translate_align.add_argument("--mesh-deflection", type=float, default=0.1, help="CAD meshing deflection") + translate_align.add_argument("--cad-sample-points", type=int, default=30000, help="Points sampled from CAD mesh") + translate_align.add_argument("--local-sample-points", type=int, default=1000, help="Points sampled from each end") + translate_align.add_argument("--scale-pointcloud", type=float, default=1.0, help="Scale factor applied to point cloud") + translate_align.add_argument("--no-visualize", action="store_true", help="Disable Open3D windows") + + full = subparsers.add_parser("full", parents=[common], help="Run configured full workflow") + full.add_argument("--no-visualize", action="store_true", help="Disable Open3D windows") + + return parser + + +def main() -> None: + parser = build_parser() + args = parser.parse_args() + from pipeline import MeasurementPipeline + + pipeline = MeasurementPipeline(config_path=args.config) + + if args.command == "cad2mesh": + pipeline.run_cad2mesh(cad_path=args.cad, mesh_path=args.mesh, precision=args.precision) + elif args.command == "denoise": + pipeline.run_denoise(pointcloud_path=args.pointcloud, noise_std=args.noise_std, no_visualize=args.no_visualize) + elif args.command == "registration": + pipeline.run_registration( + pointcloud_path=args.pointcloud, + mesh_path=args.mesh, + voxel_size=args.voxel_size, + roll=args.roll, + pitch=args.pitch, + yaw=args.yaw, + trans_range=args.trans_range, + no_visualize=args.no_visualize, + ) + elif args.command == "contour": + if args.all or args.tasks: + pipeline.run_contour_batch( + task_ids=args.tasks, + all_tasks=args.all, + data_root=args.data_root, + pointcloud_kind=args.pointcloud_kind, + mesh_deflection=args.mesh_deflection, + point_chunk_size=args.point_chunk_size, + triangle_chunk_size=args.triangle_chunk_size, + save_path=args.save, + excel_output_path=args.excel_output, + no_visualize=args.no_visualize, + ) + else: + pipeline.run_contour( + pointcloud_path=args.pointcloud, + cad_path=args.cad, + mesh_path=args.mesh, + mesh_deflection=args.mesh_deflection, + point_chunk_size=args.point_chunk_size, + triangle_chunk_size=args.triangle_chunk_size, + save_path=args.save, + excel_output_path=args.excel_output, + no_visualize=args.no_visualize, + ) + elif args.command == "curvature": + pipeline.run_curvature( + pointcloud_path=args.pointcloud, + radius=args.radius, + max_nn=args.max_nn, + color_scale=args.color_scale, + save_path=args.save, + no_visualize=args.no_visualize, + ) + elif args.command == "plane": + pipeline.run_plane( + pointcloud_path=args.pointcloud, + threshold_mm=args.threshold_mm, + max_iterations=args.max_iterations, + no_visualize=args.no_visualize, + ) + elif args.command == "translate-align": + pipeline.run_translate_align_pointcloud_to_cad( + cad_path=args.cad, + pointcloud_path=args.pointcloud, + output_dir=args.output_dir, + axis=args.axis, + crop_ratio=args.crop_ratio, + mesh_deflection=args.mesh_deflection, + cad_sample_points=args.cad_sample_points, + local_sample_points=args.local_sample_points, + scale_pointcloud=args.scale_pointcloud, + no_visualize=args.no_visualize, + ) + elif args.command == "full": + pipeline.run_full(no_visualize=args.no_visualize) + else: + parser.error(f"Unsupported command: {args.command}") + + +if __name__ == "__main__": + main() diff --git a/alg/point_porj/packaging/.gitignore b/alg/point_porj/packaging/.gitignore new file mode 100644 index 0000000..dc4bcad --- /dev/null +++ b/alg/point_porj/packaging/.gitignore @@ -0,0 +1,5 @@ +build/ +dist/ +*.egg-info/ +__pycache__/ +.pytest_cache/ diff --git a/alg/point_porj/packaging/MANIFEST.in b/alg/point_porj/packaging/MANIFEST.in new file mode 100644 index 0000000..79a9e42 --- /dev/null +++ b/alg/point_porj/packaging/MANIFEST.in @@ -0,0 +1,6 @@ +include README.md +recursive-include geometry_measure *.yaml +global-exclude __pycache__/* +global-exclude *.py[cod] +prune datasets +prune archive diff --git a/alg/point_porj/packaging/README.md b/alg/point_porj/packaging/README.md new file mode 100644 index 0000000..12d35cf --- /dev/null +++ b/alg/point_porj/packaging/README.md @@ -0,0 +1,139 @@ +# geometry-measure 打包说明 + +这个目录只负责打包，不存放数据集；仅包含已接入统一 CLI 的必要归档工作流。 + +## 本地开发安装 + +在项目根目录执行： + +```bash +python packaging/build_package.py +pip install -e packaging/build +geometry-measure --help +``` + +也可以进入 `packaging/` 后执行： + +```bash +python build_package.py +pip install -e build +geometry-measure --help +``` + +## 构建 wheel + +需要先安装构建工具： + +```bash +pip install build +``` + +然后执行： + +```bash +python packaging/build_package.py +python -m build packaging/build +``` + +构建产物会出现在： + +```text +packaging/build/dist/ +``` + +## 运行示例 + +从项目根目录运行： + +```bash +geometry-measure contour --pointcloud datasets/PointCloud/11.ply --cad datasets/CAD/11.step --no-visualize +geometry-measure contour --cad datasets/CAD/11.step --pointcloud datasets/Noise_PointCloud/11_noisy.ply --save vis/11_contour.ply --excel-output --no-visualize +geometry-measure registration --pointcloud datasets/Noise_PointCloud/11_noisy.ply --mesh datasets/Mesh/11.stl --no-visualize +geometry-measure denoise --pointcloud datasets/PointCloud/11.ply --noise-std 4 --no-visualize +``` + +`contour` 是轮廓/平整度/平面度评估的统一入口，本质上是点云到 STEP CAD 的距离计算。需要保存距离热力图时传 `--save`，需要保存 Excel 时传 `--excel-output`。如需兼容旧网格流程，可显式传入 `--mesh datasets/Mesh/11.stl`。 +`contour --excel-output` 不带值时会保存 `excel/contour.xlsx`；只传文件名时会自动放到 `excel/` 文件夹。Excel 只包含四列：任务号、最大值、平均值、状态。 +`contour` 也支持批量运行：`--tasks 11,31,40` 执行指定编号，`--all` 执行 `datasets/CAD` 下全部 STEP；批量模式用 `--pointcloud-kind noise|clean` 选择加噪点云或干净点云。 + +使用配置文件： + +```bash +geometry-measure --config config.yaml full --no-visualize +``` + +如果使用打包生成的默认配置，构建脚本会把默认路径改为： + +```text +datasets/CAD/11.step +datasets/Mesh/11.stl +datasets/PointCloud/11.ply +datasets/Noise_PointCloud/11_noisy.ply +``` + +## 数据要求 + +wheel 不包含 `datasets/`，运行时需要自己提供数据路径。建议保持以下结构： + +```text +datasets/ + CAD/ # STEP CAD，例如 11.step + Mesh/ # STL/OBJ/PLY Mesh，例如 11.stl + PointCloud/ # 原始点云 PLY，例如 11.ply + Noise_PointCloud/ # 加噪或待配准点云 PLY，例如 11_noisy.ply +``` + +CAD、Mesh 和点云需要处在同一坐标系和同一单位下。示例数据按毫米量级处理。`contour` 默认认为输入点云已经与参考模型对齐。 + +大规模点云可能运行较慢。可以通过配置文件调整： + +```yaml +evaluation: + chunk_size: 2048 + triangle_chunk_size: 4096 + cad_mesh_deflection: 0.1 +``` + +`chunk_size` 控制点云分块，`triangle_chunk_size` 控制三角面分块，`cad_mesh_deflection` 控制 STEP 三角化精度。 + +## 输出建议 + +建议把日志和可视化产物放到项目根目录的 `vis/`： + +```bash +mkdir -p vis +geometry-measure curvature --pointcloud datasets/PointCloud/11.ply --save vis/11_curvature.ply --no-visualize +geometry-measure contour --cad datasets/CAD/11.step --pointcloud datasets/Noise_PointCloud/11_noisy.ply --save vis/11_contour.ply --excel-output 11_contour.xlsx --no-visualize +geometry-measure contour --tasks 11,31,40 --data-root datasets --pointcloud-kind noise --save vis/contour --excel-output selected_contour.xlsx --no-visualize +``` + +如果需要保存命令行指标，可以使用重定向： + +```bash +geometry-measure contour --pointcloud datasets/PointCloud/11.ply --cad datasets/CAD/11.step --no-visualize > vis/contour.log 2>&1 +``` + +## CAD 转换依赖 + +基础安装不强制安装 CAD 转换依赖。需要使用 `cad2mesh` 时，建议使用项目根目录的 `environment.yml` 创建或更新独立 Python 3.10 环境： + +```bash +conda env create -f environment.yml +conda activate point-porj +pip install -r requirements.txt +``` + +如果环境已存在，则执行： + +```bash +conda env update -n point-porj -f environment.yml +pip install git+https://github.com/AutodeskAILab/occwl +``` + +`pythonocc-core` 在很多平台无法通过 pip 安装，请使用 conda 方式。`pythonocc-core=7.5.1` 不支持 Python 3.12，如果 base 环境被固定为 Python 3.12，请新建 Python 3.10 环境。 + +## 说明 + +- `datasets/` 不会打进 wheel，请通过命令行参数或配置文件指定本地数据路径。 +- 只有 `translate-align` 依赖的归档脚本会打进 wheel，其他 `archive/` 实验脚本不会打包。 +- 根目录源码保持原样；`packaging/build_package.py` 会生成临时标准包 `geometry_measure`。 diff --git a/alg/point_porj/packaging/build_package.py b/alg/point_porj/packaging/build_package.py new file mode 100644 index 0000000..dec7e4d --- /dev/null +++ b/alg/point_porj/packaging/build_package.py @@ -0,0 +1,135 @@ +from __future__ import annotations + +import shutil +from pathlib import Path + + +PACKAGING_DIR = Path(__file__).resolve().parent +PROJECT_ROOT = PACKAGING_DIR.parent +BUILD_DIR = PACKAGING_DIR / "build" +PACKAGE_DIR = BUILD_DIR / "geometry_measure" + +SOURCE_FILES = [ + "main.py", + "pipeline.py", + "io_handler.py", + "config_loader.py", + "cad_converter.py", + "registration.py", + "evaluation.py", + "visualization.py", + "config.yaml", +] + +SOURCE_DIRS = [ + "preprocessing", +] + +ARCHIVE_FILES = [ + "translate_align_pointcloud_to_cad.py", +] + +IMPORT_REWRITES = { + "from pipeline import MeasurementPipeline": "from geometry_measure.pipeline import MeasurementPipeline", + "from cad_converter import CADConverter": "from geometry_measure.cad_converter import CADConverter", + "from config_loader import ConfigLoader": "from geometry_measure.config_loader import ConfigLoader", + "from evaluation import Evaluator": "from geometry_measure.evaluation import Evaluator", + "from io_handler import IOHandler": "from geometry_measure.io_handler import IOHandler", + "from preprocessing.curvature import CurvatureEstimator": ( + "from geometry_measure.preprocessing.curvature import CurvatureEstimator" + ), + "from preprocessing.filters import PreprocessingRunner": ( + "from geometry_measure.preprocessing.filters import PreprocessingRunner" + ), + "from preprocessing.noise import GaussianNoiseAdder": ( + "from geometry_measure.preprocessing.noise import GaussianNoiseAdder" + ), + "from preprocessing.plane import PlaneEstimator": "from geometry_measure.preprocessing.plane import PlaneEstimator", + "from preprocessing.plane import PlaneResult": "from geometry_measure.preprocessing.plane import PlaneResult", + "from registration import PointCloudRegistration": "from geometry_measure.registration import PointCloudRegistration", + "from visualization import Visualizer": "from geometry_measure.visualization import Visualizer", +} + +PATH_REWRITES = { + "CAD/11.step": "datasets/CAD/11.step", + "Mesh/11.stl": "datasets/Mesh/11.stl", + "PointCloud/11.ply": "datasets/PointCloud/11.ply", + "Noise_PointCloud/11_noisy.ply": "datasets/Noise_PointCloud/11_noisy.ply", +} + + +def main() -> None: + rebuild_dir() + copy_packaging_metadata() + copy_sources() + write_package_markers() + rewrite_python_imports() + rewrite_default_paths() + print(f"Build package prepared at: {BUILD_DIR}") + print("Install with: pip install -e packaging/build") + + +def rebuild_dir() -> None: + if BUILD_DIR.exists(): + shutil.rmtree(BUILD_DIR) + PACKAGE_DIR.mkdir(parents=True) + + +def copy_packaging_metadata() -> None: + shutil.copy2(PACKAGING_DIR / "pyproject.toml", BUILD_DIR / "pyproject.toml") + shutil.copy2(PACKAGING_DIR / "MANIFEST.in", BUILD_DIR / "MANIFEST.in") + shutil.copy2(PACKAGING_DIR / "README.md", BUILD_DIR / "README.md") + + +def copy_sources() -> None: + for relative in SOURCE_FILES: + src = PROJECT_ROOT / relative + if not src.exists(): + raise FileNotFoundError(f"Missing source file: {src}") + shutil.copy2(src, PACKAGE_DIR / relative) + + for relative in SOURCE_DIRS: + src = PROJECT_ROOT / relative + if not src.exists(): + raise FileNotFoundError(f"Missing source directory: {src}") + shutil.copytree(src, PACKAGE_DIR / relative) + + archive_dir = PACKAGE_DIR / "archive" + archive_dir.mkdir(parents=True, exist_ok=True) + for relative in ARCHIVE_FILES: + src = PROJECT_ROOT / "archive" / relative + if not src.exists(): + raise FileNotFoundError(f"Missing archive file: {src}") + shutil.copy2(src, archive_dir / relative) + + +def write_package_markers() -> None: + (PACKAGE_DIR / "__init__.py").write_text( + '"""Industrial geometry measurement toolkit."""\n\n__version__ = "0.1.0"\n', + encoding="utf-8", + ) + preprocessing_init = PACKAGE_DIR / "preprocessing" / "__init__.py" + preprocessing_init.write_text('"""Preprocessing tools for point-cloud workflows."""\n', encoding="utf-8") + archive_init = PACKAGE_DIR / "archive" / "__init__.py" + archive_init.write_text('"""Archived developer workflows exposed by the CLI."""\n', encoding="utf-8") + + +def rewrite_python_imports() -> None: + for path in PACKAGE_DIR.rglob("*.py"): + text = path.read_text(encoding="utf-8") + for old, new in IMPORT_REWRITES.items(): + text = text.replace(old, new) + path.write_text(text, encoding="utf-8") + + +def rewrite_default_paths() -> None: + targets = [PACKAGE_DIR / "config.yaml", PACKAGE_DIR / "config_loader.py"] + for path in targets: + text = path.read_text(encoding="utf-8") + for old, new in PATH_REWRITES.items(): + text = text.replace(old, new) + path.write_text(text, encoding="utf-8") + + +if __name__ == "__main__": + main() diff --git a/alg/point_porj/packaging/pyproject.toml b/alg/point_porj/packaging/pyproject.toml new file mode 100644 index 0000000..24639a5 --- /dev/null +++ b/alg/point_porj/packaging/pyproject.toml @@ -0,0 +1,40 @@ +[build-system] +requires = ["setuptools>=68", "wheel"] +build-backend = "setuptools.build_meta" + +[project] +name = "geometry-measure" +version = "0.1.0" +description = "Industrial point cloud and mesh geometry measurement toolkit" +readme = "README.md" +requires-python = ">=3.10" +dependencies = [ + "numpy==2.2.2", + "scipy", + "open3d==0.19.0", + "trimesh==4.5.3", + "torch", + "torchvision", + "torchaudio", + "PyYAML", +] + +[project.optional-dependencies] +cad = [ + "occwl @ git+https://github.com/AutodeskAILab/occwl", +] +build = [ + "build", +] + +[project.scripts] +geometry-measure = "geometry_measure.main:main" + +[tool.setuptools] +include-package-data = true + +[tool.setuptools.packages.find] +include = ["geometry_measure*"] + +[tool.setuptools.package-data] +geometry_measure = ["config.yaml"] diff --git a/alg/point_porj/pipeline.py b/alg/point_porj/pipeline.py new file mode 100644 index 0000000..b1a8ab1 --- /dev/null +++ b/alg/point_porj/pipeline.py @@ -0,0 +1,644 @@ +from __future__ import annotations + +import importlib.util +from pathlib import Path +from typing import Any +from zipfile import ZIP_DEFLATED, ZipFile +from xml.sax.saxutils import escape + +import numpy as np + +from cad_converter import CADConverter +from config_loader import ConfigLoader +from evaluation import Evaluator +from io_handler import IOHandler +from preprocessing.curvature import CurvatureEstimator +from preprocessing.filters import PreprocessingRunner +from preprocessing.noise import GaussianNoiseAdder +from preprocessing.plane import PlaneEstimator +from registration import PointCloudRegistration +from visualization import Visualizer + + +class MeasurementPipeline: + """Main workflow entry class for geometry measurement tasks.""" + + def __init__(self, config_path: str | None = None) -> None: + self.config = ConfigLoader().load(config_path) + self.io = IOHandler() + self.cad_converter = CADConverter() + self.noise_adder = GaussianNoiseAdder() + self.filters = PreprocessingRunner() + self.registration = PointCloudRegistration() + self.evaluator = Evaluator() + self.curvature = CurvatureEstimator() + self.plane = PlaneEstimator() + self.visualizer = Visualizer() + + def run_cad2mesh(self, cad_path: str | None = None, mesh_path: str | None = None, precision: float | None = None) -> Path: + paths = self.config["paths"] + cad_config = self.config["cad"] + input_step = self.io.require_cad(cad_path or paths["cad"]) + output_stl = self.io.ensure_parent_dir(mesh_path or paths["output_mesh"] or paths["mesh"]) + return self.cad_converter.convert_step_to_stl( + input_step=input_step, + output_stl=output_stl, + precision=float(precision if precision is not None else cad_config["precision"]), + ) + + def run_denoise( + self, + pointcloud_path: str | None = None, + noise_std: float | None = None, + no_visualize: bool = False, + ) -> dict[str, Any]: + path = pointcloud_path or self.config["paths"]["pointcloud"] + clean = self.io.load_point_cloud(path) + noise_config = self.config["noise"] + adder = GaussianNoiseAdder( + noise_std=float(noise_std if noise_std is not None else noise_config["std"]), + seed=noise_config.get("seed"), + ) + noisy = adder.apply(clean) + noise_level = adder.noise_level(clean, noisy) + + filter_results = {} + if self.config["filters"].get("enabled", True): + filter_results = self.filters.run(noisy, self.config["filters"]) + + print(f"Clean points: {len(clean.points)}") + print(f"Noisy points: {len(noisy.points)}") + print(f"Mean noise offset: {noise_level:.6f}") + for name, result in filter_results.items(): + cd, _, _ = self.evaluator.chamfer_distance(clean, result.pointcloud) + result.metrics["chamfer_distance"] = cd + print(f"{name}: keep={result.metrics['keep_ratio']:.2f}% CD={cd:.6f}") + + cd_noisy, _, _ = self.evaluator.chamfer_distance(clean, noisy) + print(f"Noisy without filtering: CD={cd_noisy:.6f}") + + if self._should_visualize(no_visualize): + first_result = next(iter(filter_results.values()), None) + self.visualizer.show_denoise( + clean, + noisy, + first_result.pointcloud if first_result else None, + first_result.name if first_result else "Filtered", + ) + + return { + "clean": clean, + "noisy": noisy, + "noise_level": noise_level, + "filter_results": filter_results, + "noisy_chamfer_distance": cd_noisy, + } + + def run_registration( + self, + pointcloud_path: str | None = None, + mesh_path: str | None = None, + voxel_size: float | None = None, + roll: float | None = None, + pitch: float | None = None, + yaw: float | None = None, + trans_range: float | None = None, + no_visualize: bool = False, + ) -> dict[str, Any]: + paths = self.config["paths"] + reg_config = self.config["registration"] + source = self.io.load_point_cloud(pointcloud_path or paths["noisy_pointcloud"] or paths["pointcloud"]) + mesh_file = str(self.io.require_file(mesh_path or paths["mesh"], "Mesh")) + return self._run_registration_with_pointcloud( + source=source, + mesh_file=mesh_file, + voxel_size=voxel_size, + roll=roll, + pitch=pitch, + yaw=yaw, + trans_range=trans_range, + no_visualize=no_visualize, + reg_config=reg_config, + ) + + def _run_registration_with_pointcloud( + self, + source: Any, + mesh_file: str, + voxel_size: float | None, + roll: float | None, + pitch: float | None, + yaw: float | None, + trans_range: float | None, + no_visualize: bool, + reg_config: dict[str, Any], + ) -> dict[str, Any]: + + result = self.registration.run_simulated_registration( + source=source, + voxel_size=float(voxel_size if voxel_size is not None else reg_config["voxel_size"]), + roll=float(roll if roll is not None else reg_config["roll"]), + pitch=float(pitch if pitch is not None else reg_config["pitch"]), + yaw=float(yaw if yaw is not None else reg_config["yaw"]), + trans_range=float(trans_range if trans_range is not None else reg_config["trans_range"]), + random_seed=reg_config.get("random_seed"), + ) + + points = self.io.point_cloud_to_numpy(result.recovered) + mesh = self.io.load_mesh(mesh_file) + _, distance_stats = self.evaluator.point_to_mesh_distance_torch( + points, + mesh, + chunk_size=int(self.config["evaluation"]["chunk_size"]), + triangle_chunk_size=int(self.config["evaluation"]["triangle_chunk_size"]), + ) + + print("Registration result") + print(f"Fitness: {result.fitness:.4f}") + print(f"RMSE: {result.rmse:.6f}") + print(f"Transformation:\n{result.transformation}") + self._print_distance_stats(distance_stats) + + if self._should_visualize(no_visualize): + self.visualizer.show_registration(source, result.target, result.recovered) + + return { + "source": source, + "registration": result, + "distance_stats": distance_stats, + } + + def run_contour( + self, + pointcloud_path: str | None = None, + cad_path: str | None = None, + mesh_path: str | None = None, + mesh_deflection: float | None = None, + point_chunk_size: int | None = None, + triangle_chunk_size: int | None = None, + save_path: str | None = None, + excel_output_path: str | None = None, + no_visualize: bool = False, + ) -> dict[str, Any]: + paths = self.config["paths"] + eval_config = self.config["evaluation"] + + if mesh_path: + pointcloud = self.io.load_point_cloud(pointcloud_path or paths["pointcloud"]) + mesh = self.io.load_mesh(mesh_path) + points = self.io.point_cloud_to_numpy(pointcloud) + distances, stats = self.evaluator.point_to_mesh_distance_torch( + points, + mesh, + chunk_size=int(point_chunk_size if point_chunk_size is not None else eval_config["chunk_size"]), + triangle_chunk_size=int( + triangle_chunk_size if triangle_chunk_size is not None else eval_config["triangle_chunk_size"] + ), + ) + self._print_distance_stats(stats) + return {"pointcloud": pointcloud, "mesh": mesh, "distances": distances, "distance_stats": stats} + + return self.run_contour_distance( + cad_path=cad_path or paths["cad"], + pointcloud_path=pointcloud_path or paths["pointcloud"], + mesh_deflection=mesh_deflection, + point_chunk_size=point_chunk_size, + triangle_chunk_size=triangle_chunk_size, + save_path=save_path, + excel_output_path=excel_output_path, + no_visualize=no_visualize, + ) + + def run_contour_distance( + self, + cad_path: str | None = None, + pointcloud_path: str | None = None, + mesh_deflection: float | None = None, + point_chunk_size: int | None = None, + triangle_chunk_size: int | None = None, + save_path: str | None = None, + excel_output_path: str | None = None, + no_visualize: bool = False, + ) -> dict[str, Any]: + paths = self.config["paths"] + eval_config = self.config["evaluation"] + input_step = self.io.require_cad(cad_path or paths["cad"]) + pointcloud = self.io.load_point_cloud(pointcloud_path or paths["noisy_pointcloud"] or paths["pointcloud"]) + points = self.io.point_cloud_to_numpy(pointcloud) + + vertices, faces = self.cad_converter.step_to_numpy_mesh( + input_step, + mesh_deflection=float( + mesh_deflection if mesh_deflection is not None else eval_config["cad_mesh_deflection"] + ), + ) + print(f"CAD mesh vertices: {vertices.shape[0]}") + print(f"CAD mesh triangles: {faces.shape[0]}") + distances, stats = self.evaluator.point_to_triangles_distance_torch( + points, + vertices, + faces, + point_chunk_size=int(point_chunk_size if point_chunk_size is not None else eval_config["chunk_size"]), + triangle_chunk_size=int( + triangle_chunk_size if triangle_chunk_size is not None else eval_config["triangle_chunk_size"] + ), + ) + self._print_distance_stats(stats) + + heatmap = self.evaluator.colorize_distances(points, distances) + output = save_path + if output: + self.io.save_point_cloud(heatmap, output) + print(f"Saved contour heatmap: {output}") + + excel_output = None + if excel_output_path: + excel_output = self._save_contour_excel( + excel_output_path, + cad_path=input_step, + stats=stats, + ) + print(f"Saved contour Excel: {excel_output}") + + if self._should_visualize(no_visualize): + self.visualizer.show_curvature(heatmap, window_name="Point to CAD Distance Heatmap") + + return { + "pointcloud": heatmap, + "distances": distances, + "distance_stats": stats, + "cad_vertices": vertices, + "cad_faces": faces, + "excel_output": excel_output, + } + + def run_contour_batch( + self, + task_ids: str | list[int | str] | None = None, + all_tasks: bool = False, + data_root: str | Path = "datasets", + pointcloud_kind: str = "noise", + mesh_deflection: float | None = None, + point_chunk_size: int | None = None, + triangle_chunk_size: int | None = None, + save_path: str | None = None, + excel_output_path: str | None = None, + no_visualize: bool = False, + ) -> dict[str, Any]: + data_root = Path(data_root) + task_id_list = self._resolve_contour_task_ids(task_ids, all_tasks, data_root) + rows: list[dict[str, Any]] = [] + results: dict[str, Any] = {} + + for task_id in task_id_list: + cad_path = data_root / "CAD" / f"{task_id}.step" + pointcloud_path = self._contour_pointcloud_path(data_root, task_id, pointcloud_kind) + task_save_path = self._contour_task_save_path(save_path, task_id) + + print(f"===== contour task {task_id} =====") + missing = [str(path) for path in (cad_path, pointcloud_path) if not path.exists()] + if missing: + print("Missing input: " + "; ".join(missing)) + rows.append({"task_id": task_id, "max": None, "mean": None, "status": "失败"}) + continue + + try: + result = self.run_contour_distance( + cad_path=cad_path, + pointcloud_path=pointcloud_path, + mesh_deflection=mesh_deflection, + point_chunk_size=point_chunk_size, + triangle_chunk_size=triangle_chunk_size, + save_path=str(task_save_path) if task_save_path else None, + excel_output_path=None, + no_visualize=True if len(task_id_list) > 1 else no_visualize, + ) + except Exception as exc: + print(f"Task {task_id} failed: {exc}") + rows.append({"task_id": task_id, "max": None, "mean": None, "status": "失败"}) + continue + + stats = result["distance_stats"] + rows.append({"task_id": task_id, "max": stats.max, "mean": stats.mean, "status": "成功"}) + results[task_id] = result + + excel_output = None + if excel_output_path: + excel_output = self._save_contour_excel(excel_output_path, rows=rows) + print(f"Saved contour Excel: {excel_output}") + + return {"rows": rows, "results": results, "excel_output": excel_output} + + def run_curvature( + self, + pointcloud_path: str | None = None, + radius: float | None = None, + max_nn: int | None = None, + color_scale: float | None = None, + save_path: str | None = None, + no_visualize: bool = False, + ) -> dict[str, Any]: + config = self.config["curvature"] + pointcloud = self.io.load_point_cloud(pointcloud_path or self.config["paths"]["pointcloud"]) + curvatures = self.curvature.compute( + pointcloud, + radius=float(radius if radius is not None else config["radius"]), + max_nn=int(max_nn if max_nn is not None else config["max_nn"]), + ) + colored = self.curvature.colorize( + pointcloud, + curvatures, + color_scale=float(color_scale if color_scale is not None else config["color_scale"]), + ) + output = save_path or self.config["paths"].get("curvature_output") + if output: + self.io.save_point_cloud(colored, output) + print(f"Saved curvature point cloud: {output}") + print(f"Curvature mean={np.mean(curvatures):.8f}, max={np.max(curvatures):.8f}") + if self._should_visualize(no_visualize): + self.visualizer.show_curvature(colored) + return {"pointcloud": colored, "curvatures": curvatures} + + def run_plane( + self, + pointcloud_path: str | None = None, + threshold_mm: float | None = None, + max_iterations: int | None = None, + no_visualize: bool = False, + ) -> dict[str, Any]: + config = self.config["plane"] + if pointcloud_path: + pointcloud = self.io.load_point_cloud(pointcloud_path) + else: + pointcloud = self.plane.create_square_plane_with_noise( + size_mm=float(config["size_mm"]), + noise_std_mm=float(config["noise_std_mm"]), + num_points=int(config["num_points"]), + ) + + result = self.plane.estimate( + pointcloud, + threshold_mm=float(threshold_mm if threshold_mm is not None else config["threshold_mm"]), + max_iterations=int(max_iterations if max_iterations is not None else config["max_iterations"]), + ) + + print(f"Plane model: {result.model}") + print(f"Inlier ratio: {result.inlier_ratio:.2f}%") + print(f"Normal angle error: {result.normal_angle_deg:.6f} deg") + print(f"Plane RMSE: {result.rmse:.8f} m") + if self._should_visualize(no_visualize): + self.visualizer.show_plane(result) + return {"plane_result": result} + + def run_full(self, no_visualize: bool = False) -> dict[str, Any]: + outputs: dict[str, Any] = {} + if self.config["cad"].get("enabled", False): + outputs["cad2mesh"] = self.run_cad2mesh() + if self.config["noise"].get("enabled", True): + outputs["denoise"] = self.run_denoise(no_visualize=True) + mesh_file = str(self.io.require_file(self.config["paths"]["mesh"], "Mesh")) + outputs["registration"] = self._run_registration_with_pointcloud( + source=outputs["denoise"]["noisy"], + mesh_file=mesh_file, + voxel_size=None, + roll=None, + pitch=None, + yaw=None, + trans_range=None, + no_visualize=no_visualize, + reg_config=self.config["registration"], + ) + else: + registration_input = self.config["paths"].get("noisy_pointcloud") or self.config["paths"]["pointcloud"] + outputs["registration"] = self.run_registration(pointcloud_path=registration_input, no_visualize=no_visualize) + outputs["contour"] = self.run_contour() + return outputs + + def _should_visualize(self, no_visualize: bool) -> bool: + return bool(self.config["visualization"].get("enabled", True)) and not no_visualize + + def _print_distance_stats(self, stats: Any) -> None: + print("Distance stats") + print(f"Mean: {stats.mean:.8f}") + print(f"Max: {stats.max:.8f}") + print(f"RMSE: {stats.rmse:.8f}") + print(f"Min: {stats.min:.8f}") + print(f"Std: {stats.std:.8f}") + + def _save_contour_excel( + self, + output_path: str | Path, + *, + rows: list[dict[str, Any]] | None = None, + cad_path: str | Path | None = None, + stats: Any | None = None, + ) -> Path: + output = self._normalize_excel_output_path(output_path) + if rows is None: + if cad_path is None or stats is None: + raise ValueError("cad_path and stats are required when rows is not provided.") + rows = [{"task_id": Path(cad_path).stem, "max": stats.max, "mean": stats.mean, "status": "成功"}] + + table = [ + [ + "任务号", + "最大值", + "平均值", + "状态", + ] + ] + for row in rows: + table.append([ + row.get("task_id"), + row.get("max"), + row.get("mean"), + row.get("status"), + ]) + self._write_xlsx(table, output, sheet_name="contour") + return output + + def _resolve_contour_task_ids( + self, + task_ids: str | list[int | str] | None, + all_tasks: bool, + data_root: Path, + ) -> list[str]: + if all_tasks: + cad_dir = data_root / "CAD" + if not cad_dir.exists(): + raise FileNotFoundError(f"CAD directory not found: {cad_dir}") + return sorted(path.stem for path in cad_dir.glob("*.step")) + + if task_ids is None: + raise ValueError("Batch contour requires --tasks or --all.") + + if isinstance(task_ids, str): + values = [item.strip() for item in task_ids.split(",")] + else: + values = [str(item).strip() for item in task_ids] + return [item for item in values if item] + + def _contour_pointcloud_path(self, data_root: Path, task_id: str, pointcloud_kind: str) -> Path: + if pointcloud_kind == "clean": + return data_root / "PointCloud" / f"{task_id}.ply" + if pointcloud_kind == "noise": + return data_root / "Noise_PointCloud" / f"{task_id}_noisy.ply" + raise ValueError("pointcloud_kind must be 'noise' or 'clean'.") + + def _contour_task_save_path(self, save_path: str | None, task_id: str) -> Path | None: + if not save_path: + return None + + output = Path(save_path) + if output.suffix.lower() != ".ply": + output.mkdir(parents=True, exist_ok=True) + return output / f"{task_id}_contour.ply" + + output.parent.mkdir(parents=True, exist_ok=True) + if "{task_id}" in output.name: + return output.with_name(output.name.replace("{task_id}", task_id)) + return output.with_name(f"{output.stem}_{task_id}{output.suffix}") + + def run_translate_align_pointcloud_to_cad( + self, + cad_path: str | None = None, + pointcloud_path: str | None = None, + output_dir: str = "results", + axis: str = "x", + crop_ratio: float = 0.01, + mesh_deflection: float = 0.1, + cad_sample_points: int = 30000, + local_sample_points: int = 1000, + scale_pointcloud: float = 1.0, + no_visualize: bool = False, + ) -> dict[str, Any]: + """Run the archived front/tail centroid translation alignment workflow.""" + paths = self.config["paths"] + input_step = self.io.require_cad(cad_path or paths["cad"]) + input_pointcloud = self._resolve_translate_align_pointcloud( + pointcloud_path or paths["noisy_pointcloud"] or paths["pointcloud"] + ) + + module = self._load_archive_module("translate_align_pointcloud_to_cad.py") + return module.translate_align_by_front_tail( + cad_path=str(input_step), + pointcloud_path=str(input_pointcloud), + output_dir=output_dir, + axis=axis, + crop_ratio=float(crop_ratio), + mesh_deflection=float(mesh_deflection), + cad_sample_points=int(cad_sample_points), + local_sample_points=int(local_sample_points), + scale_pointcloud=float(scale_pointcloud), + visualize=not no_visualize, + ) + + def _resolve_translate_align_pointcloud(self, pointcloud_path: str | Path) -> Path: + path = Path(pointcloud_path) + if path.is_dir(): + preferred = path / "shifted_whole_pointcloud.ply" + if preferred.exists(): + return preferred + + candidates = sorted(path.glob("*.ply")) + if len(candidates) == 1: + return candidates[0] + if not candidates: + raise FileNotFoundError(f"No PLY point cloud found in directory: {path}") + names = ", ".join(candidate.name for candidate in candidates) + raise ValueError(f"Multiple PLY files found in {path}; specify one file explicitly. Found: {names}") + + return self.io.require_file(path, "PointCloud") + + def _load_archive_module(self, filename: str) -> Any: + script_path = Path(__file__).resolve().parent / "archive" / filename + if not script_path.exists(): + raise FileNotFoundError(f"Archive script not found: {script_path}") + + module_name = f"_archive_{script_path.stem}" + spec = importlib.util.spec_from_file_location(module_name, script_path) + if spec is None or spec.loader is None: + raise ImportError(f"Cannot load archive script: {script_path}") + + module = importlib.util.module_from_spec(spec) + spec.loader.exec_module(module) + return module + + def _normalize_excel_output_path(self, output_path: str | Path) -> Path: + output = Path(output_path) + if output.suffix.lower() != ".xlsx": + output = output / "contour.xlsx" + elif output.parent == Path("."): + output = Path("excel") / output + output.parent.mkdir(parents=True, exist_ok=True) + return output + + def _write_xlsx(self, rows: list[list[Any]], output_path: Path, sheet_name: str) -> None: + sheet_rows = [] + for row_index, row_values in enumerate(rows, start=1): + cells = [] + for col_index, value in enumerate(row_values, start=1): + cell_ref = self._xlsx_cell_ref(row_index, col_index) + if isinstance(value, (int, float, np.integer, np.floating)) and not isinstance(value, bool): + cells.append(f'{value}') + else: + text = escape("" if value is None else str(value)) + cells.append(f'{text}') + sheet_rows.append(f'{"".join(cells)}') + + workbook_xml = ( + '' + '' + f'' + '' + ) + worksheet_xml = ( + '' + '' + '' + f'{"".join(sheet_rows)}' + '' + '' + ) + content_types_xml = ( + '' + '' + '' + '' + '' + '' + '' + ) + root_rels_xml = ( + '' + '' + '' + '' + ) + workbook_rels_xml = ( + '' + '' + '' + '' + ) + with ZipFile(output_path, "w", ZIP_DEFLATED) as xlsx: + xlsx.writestr("[Content_Types].xml", content_types_xml) + xlsx.writestr("_rels/.rels", root_rels_xml) + xlsx.writestr("xl/workbook.xml", workbook_xml) + xlsx.writestr("xl/_rels/workbook.xml.rels", workbook_rels_xml) + xlsx.writestr("xl/worksheets/sheet1.xml", worksheet_xml) + + def _xlsx_cell_ref(self, row_index: int, col_index: int) -> str: + letters = "" + col = col_index + while col: + col, remainder = divmod(col - 1, 26) + letters = chr(65 + remainder) + letters + return f"{letters}{row_index}" diff --git a/alg/point_porj/preprocessing/curvature.py b/alg/point_porj/preprocessing/curvature.py new file mode 100644 index 0000000..ef122a4 --- /dev/null +++ b/alg/point_porj/preprocessing/curvature.py @@ -0,0 +1,39 @@ +from __future__ import annotations + +import copy +import numpy as np +import open3d as o3d + + +class CurvatureEstimator: + """Estimate local point-cloud curvature using neighborhood covariance.""" + + def compute(self, pointcloud: o3d.geometry.PointCloud, radius: float = 2.0, max_nn: int = 30) -> np.ndarray: + pointcloud.estimate_normals(o3d.geometry.KDTreeSearchParamHybrid(radius=radius, max_nn=max_nn)) + points = np.asarray(pointcloud.points) + kdtree = o3d.geometry.KDTreeFlann(pointcloud) + curvatures = np.zeros(len(points), dtype=np.float64) + + for i, point in enumerate(points): + _, indices, _ = kdtree.search_hybrid_vector_3d(point, radius, max_nn) + if len(indices) < 3: + curvatures[i] = 1.0 + continue + + neighbors = points[indices] + cov = np.cov((neighbors - neighbors.mean(axis=0)).T) + eigenvalues = np.linalg.eigvalsh(cov) + denom = eigenvalues.sum() + curvatures[i] = float(eigenvalues[0] / denom) if denom > 0 else 0.0 + + return curvatures + + def colorize(self, pointcloud: o3d.geometry.PointCloud, curvatures: np.ndarray, color_scale: float = 3.0) -> o3d.geometry.PointCloud: + pcd = copy.deepcopy(pointcloud) + normalized = curvatures / (np.max(curvatures) + 1e-12) + normalized = np.clip(normalized * color_scale, 0, 1) + colors = np.zeros((len(normalized), 3)) + colors[:, 0] = normalized + colors[:, 1] = 1 - normalized + pcd.colors = o3d.utility.Vector3dVector(colors) + return pcd diff --git a/alg/point_porj/preprocessing/filters.py b/alg/point_porj/preprocessing/filters.py new file mode 100644 index 0000000..dc6f305 --- /dev/null +++ b/alg/point_porj/preprocessing/filters.py @@ -0,0 +1,190 @@ +from __future__ import annotations + +import copy +from dataclasses import dataclass +from typing import Any + +import numpy as np +import open3d as o3d +from scipy.spatial import KDTree + + +@dataclass +class FilterResult: + name: str + pointcloud: o3d.geometry.PointCloud + metrics: dict[str, Any] + outliers: o3d.geometry.PointCloud | None = None + + +def _base_metrics(original_count: int, filtered_count: int, method: str, params: dict[str, Any]) -> dict[str, Any]: + removed_count = original_count - filtered_count + return { + "method": method, + "original_points": original_count, + "filtered_points": filtered_count, + "removed_points": removed_count, + "keep_ratio": filtered_count / original_count * 100 if original_count > 0 else 0, + "params": params, + } + + +class SORFilter: + def apply(self, pointcloud: o3d.geometry.PointCloud, nb_neighbors: int = 20, std_ratio: float = 2.0) -> FilterResult: + _, indices = pointcloud.remove_statistical_outlier(nb_neighbors=nb_neighbors, std_ratio=std_ratio) + inliers = pointcloud.select_by_index(indices) + outliers = pointcloud.select_by_index(indices, invert=True) + metrics = _base_metrics(len(pointcloud.points), len(inliers.points), "SOR", { + "nb_neighbors": nb_neighbors, + "std_ratio": std_ratio, + }) + return FilterResult("SOR", inliers, metrics, outliers) + + +class RORFilter: + def apply(self, pointcloud: o3d.geometry.PointCloud, nb_points: int = 5, radius: float = 20.0) -> FilterResult: + _, indices = pointcloud.remove_radius_outlier(nb_points=nb_points, radius=radius) + inliers = pointcloud.select_by_index(indices) + outliers = pointcloud.select_by_index(indices, invert=True) + metrics = _base_metrics(len(pointcloud.points), len(inliers.points), "ROR", { + "nb_points": nb_points, + "radius": radius, + }) + return FilterResult("ROR", inliers, metrics, outliers) + + +class PassThroughFilter: + def apply( + self, + pointcloud: o3d.geometry.PointCloud, + axis: str = "z", + min_val: float | None = None, + max_val: float | None = None, + ) -> FilterResult: + axis_map = {"x": 0, "y": 1, "z": 2} + if axis not in axis_map: + raise ValueError("axis must be one of: x, y, z") + + points = np.asarray(pointcloud.points) + idx = axis_map[axis] + if min_val is None: + min_val = float(points[:, idx].min()) + if max_val is None: + max_val = float(points[:, idx].max()) + + mask = (points[:, idx] >= min_val) & (points[:, idx] <= max_val) + filtered = o3d.geometry.PointCloud() + filtered.points = o3d.utility.Vector3dVector(points[mask]) + if pointcloud.has_colors(): + colors = np.asarray(pointcloud.colors) + filtered.colors = o3d.utility.Vector3dVector(colors[mask]) + + metrics = _base_metrics(len(pointcloud.points), len(filtered.points), "PF", { + "axis": axis, + "min_val": min_val, + "max_val": max_val, + }) + return FilterResult("PF", filtered, metrics) + + +class VoxelGridFilter: + def apply(self, pointcloud: o3d.geometry.PointCloud, leaf_size: float = 0.05) -> FilterResult: + filtered = pointcloud.voxel_down_sample(leaf_size) + metrics = _base_metrics(len(pointcloud.points), len(filtered.points), "VGF", {"leaf_size": leaf_size}) + metrics["downsample_rate"] = ( + (len(pointcloud.points) - len(filtered.points)) / len(pointcloud.points) * 100 + if len(pointcloud.points) > 0 + else 0 + ) + metrics["compression_ratio"] = len(pointcloud.points) / len(filtered.points) if len(filtered.points) > 0 else 0 + return FilterResult("VGF", filtered, metrics) + + +class BilateralFilter: + def apply(self, pointcloud: o3d.geometry.PointCloud, sigma_s: float = 0.01, sigma_r: float = 0.1) -> FilterResult: + if len(pointcloud.points) == 0: + metrics = _base_metrics(0, 0, "BF", {"sigma_s": sigma_s, "sigma_r": sigma_r}) + return FilterResult("BF", copy.deepcopy(pointcloud), metrics) + + pcd = copy.deepcopy(pointcloud) + if not pcd.has_normals(): + pcd.estimate_normals() + + points = np.asarray(pcd.points) + normals = np.asarray(pcd.normals) + tree = KDTree(points) + filtered_points = np.zeros_like(points) + + for i, point in enumerate(points): + indices = tree.query_ball_point(point, r=2 * sigma_s) + if len(indices) <= 1: + filtered_points[i] = point + continue + + neighbor_points = points[indices] + neighbor_normals = normals[indices] + spatial_dist = np.linalg.norm(neighbor_points - point, axis=1) + spatial_weight = np.exp(-(spatial_dist ** 2) / (2 * sigma_s ** 2)) + normal_dist = np.linalg.norm(neighbor_normals - normals[i], axis=1) + range_weight = np.exp(-(normal_dist ** 2) / (2 * sigma_r ** 2)) + weights = spatial_weight * range_weight + if weights.sum() > 0: + weights = weights / weights.sum() + filtered_points[i] = np.sum(neighbor_points.T * weights, axis=1) + + filtered = o3d.geometry.PointCloud() + filtered.points = o3d.utility.Vector3dVector(filtered_points) + metrics = _base_metrics(len(pointcloud.points), len(filtered.points), "BF", { + "sigma_s": sigma_s, + "sigma_r": sigma_r, + }) + return FilterResult("BF", filtered, metrics) + + +class PreprocessingRunner: + """Run configured filters in sequence or independently.""" + + def __init__(self) -> None: + self.filters = { + "SOR": SORFilter(), + "ROR": RORFilter(), + "PF": PassThroughFilter(), + "VGF": VoxelGridFilter(), + "BF": BilateralFilter(), + } + + def run(self, pointcloud: o3d.geometry.PointCloud, config: dict[str, Any]) -> dict[str, FilterResult]: + results: dict[str, FilterResult] = {} + for method in config.get("methods", ["SOR"]): + name = method.upper() + if name not in self.filters: + raise ValueError(f"Unsupported filter method: {method}") + if name == "SOR": + result = self.filters[name].apply( + pointcloud, + nb_neighbors=int(config.get("sor_neighbors", 20)), + std_ratio=float(config.get("sor_std_ratio", 2.0)), + ) + elif name == "ROR": + result = self.filters[name].apply( + pointcloud, + nb_points=int(config.get("ror_points", 5)), + radius=float(config.get("ror_radius", 20.0)), + ) + elif name == "PF": + result = self.filters[name].apply( + pointcloud, + axis=str(config.get("pass_axis", "z")), + min_val=config.get("pass_min"), + max_val=config.get("pass_max"), + ) + elif name == "VGF": + result = self.filters[name].apply(pointcloud, leaf_size=float(config.get("voxel_size", 0.05))) + else: + result = self.filters[name].apply( + pointcloud, + sigma_s=float(config.get("bf_sigma_s", 0.01)), + sigma_r=float(config.get("bf_sigma_r", 0.1)), + ) + results[name] = result + return results diff --git a/alg/point_porj/preprocessing/noise.py b/alg/point_porj/preprocessing/noise.py new file mode 100644 index 0000000..21bb6a4 --- /dev/null +++ b/alg/point_porj/preprocessing/noise.py @@ -0,0 +1,29 @@ +from __future__ import annotations + +import copy + +import numpy as np +import open3d as o3d + + +class GaussianNoiseAdder: + """Add Gaussian noise to an Open3D point cloud.""" + + def __init__(self, noise_std: float = 4.0, seed: int | None = None) -> None: + self.noise_std = noise_std + self.seed = seed + + def apply(self, pointcloud: o3d.geometry.PointCloud) -> o3d.geometry.PointCloud: + noisy = copy.deepcopy(pointcloud) + points = np.asarray(noisy.points) + rng = np.random.default_rng(self.seed) + noise = rng.normal(0, self.noise_std, points.shape) + noisy.points = o3d.utility.Vector3dVector(points + noise) + return noisy + + def noise_level(self, clean: o3d.geometry.PointCloud, noisy: o3d.geometry.PointCloud) -> float: + clean_points = np.asarray(clean.points) + noisy_points = np.asarray(noisy.points) + if clean_points.shape != noisy_points.shape: + raise ValueError("Clean and noisy point clouds must have the same point count.") + return float(np.mean(np.linalg.norm(noisy_points - clean_points, axis=1))) diff --git a/alg/point_porj/preprocessing/plane.py b/alg/point_porj/preprocessing/plane.py new file mode 100644 index 0000000..df271fb --- /dev/null +++ b/alg/point_porj/preprocessing/plane.py @@ -0,0 +1,62 @@ +from __future__ import annotations + +from dataclasses import dataclass + +import numpy as np +import open3d as o3d + + +@dataclass +class PlaneResult: + model: tuple[float, float, float, float] + inliers: list[int] + inlier_ratio: float + normal_angle_deg: float + rmse: float + pointcloud: o3d.geometry.PointCloud + + +class PlaneEstimator: + """Generate noisy planes and estimate planes using Open3D RANSAC.""" + + def create_square_plane_with_noise( + self, + size_mm: float = 1000.0, + noise_std_mm: float = 0.5, + num_points: int = 10000, + ) -> o3d.geometry.PointCloud: + half_size_m = size_mm / 2000.0 + noise_std_m = noise_std_mm / 1000.0 + xy = np.random.uniform(-half_size_m, half_size_m, size=(num_points, 2)) + z = np.random.normal(0, noise_std_m, size=(num_points, 1)) + points = np.hstack([xy, z]) + pcd = o3d.geometry.PointCloud() + pcd.points = o3d.utility.Vector3dVector(points) + return pcd + + def estimate( + self, + pointcloud: o3d.geometry.PointCloud, + threshold_mm: float = 0.5, + max_iterations: int = 2000, + ) -> PlaneResult: + threshold_m = threshold_mm / 1000.0 + plane_model, inliers = pointcloud.segment_plane( + distance_threshold=threshold_m, + ransac_n=3, + num_iterations=max_iterations, + ) + + a, b, c, d = plane_model + normal = np.array([a, b, c], dtype=np.float64) + normal = normal / (np.linalg.norm(normal) + 1e-12) + ideal = np.array([0.0, 0.0, 1.0]) + angle = float(np.degrees(np.arccos(np.clip(abs(np.dot(normal, ideal)), -1.0, 1.0)))) + + points = np.asarray(pointcloud.points) + inlier_points = points[inliers] + distances = np.abs(np.dot(inlier_points, np.array([a, b, c])) + d) + rmse = float(np.sqrt(np.mean(distances ** 2))) if len(distances) else 0.0 + ratio = len(inliers) / len(points) * 100 if len(points) else 0.0 + + return PlaneResult(tuple(float(x) for x in plane_model), list(inliers), ratio, angle, rmse, pointcloud) diff --git a/alg/point_porj/registration.py b/alg/point_porj/registration.py new file mode 100644 index 0000000..b052831 --- /dev/null +++ b/alg/point_porj/registration.py @@ -0,0 +1,110 @@ +from __future__ import annotations + +import copy +from dataclasses import dataclass + +import numpy as np +import open3d as o3d +from scipy.spatial.transform import Rotation + + +@dataclass +class RegistrationResult: + transformation: np.ndarray + fitness: float + rmse: float + target: o3d.geometry.PointCloud + recovered: o3d.geometry.PointCloud + + +class PointCloudRegistration: + """RANSAC + ICP point-cloud registration.""" + + def rotation_matrix(self, roll: float = 10.0, pitch: float = 10.0, yaw: float = 10.0) -> np.ndarray: + return Rotation.from_euler("xyz", [roll, pitch, yaw], degrees=True).as_matrix() + + def translation_vector(self, trans_range: float = 0.0, seed: int | None = None) -> np.ndarray: + rng = np.random.default_rng(seed) + return rng.uniform(-trans_range, trans_range, 3) + + def transform_point_cloud( + self, + pointcloud: o3d.geometry.PointCloud, + rotation: np.ndarray, + translation: np.ndarray, + ) -> o3d.geometry.PointCloud: + transformed = copy.deepcopy(pointcloud) + points = np.asarray(transformed.points) + transformed.points = o3d.utility.Vector3dVector((rotation @ points.T).T + translation) + return transformed + + def register( + self, + source: o3d.geometry.PointCloud, + target: o3d.geometry.PointCloud, + voxel_size: float = 10.0, + ) -> tuple[np.ndarray, float, float]: + source_down = source.voxel_down_sample(voxel_size) + target_down = target.voxel_down_sample(voxel_size) + + normal_radius = voxel_size * 2 + source_down.estimate_normals(o3d.geometry.KDTreeSearchParamHybrid(radius=normal_radius, max_nn=30)) + target_down.estimate_normals(o3d.geometry.KDTreeSearchParamHybrid(radius=normal_radius, max_nn=30)) + + feature_radius = voxel_size * 5 + source_fpfh = o3d.pipelines.registration.compute_fpfh_feature( + source_down, + o3d.geometry.KDTreeSearchParamHybrid(radius=feature_radius, max_nn=100), + ) + target_fpfh = o3d.pipelines.registration.compute_fpfh_feature( + target_down, + o3d.geometry.KDTreeSearchParamHybrid(radius=feature_radius, max_nn=100), + ) + + distance_threshold = voxel_size * 1.5 + result_ransac = o3d.pipelines.registration.registration_ransac_based_on_feature_matching( + source_down, + target_down, + source_fpfh, + target_fpfh, + mutual_filter=True, + max_correspondence_distance=distance_threshold, + estimation_method=o3d.pipelines.registration.TransformationEstimationPointToPoint(), + ransac_n=3, + checkers=[ + o3d.pipelines.registration.CorrespondenceCheckerBasedOnEdgeLength(0.9), + o3d.pipelines.registration.CorrespondenceCheckerBasedOnDistance(distance_threshold), + ], + criteria=o3d.pipelines.registration.RANSACConvergenceCriteria(100000, 0.999), + ) + + source.estimate_normals(o3d.geometry.KDTreeSearchParamHybrid(radius=0.1, max_nn=30)) + target.estimate_normals(o3d.geometry.KDTreeSearchParamHybrid(radius=0.1, max_nn=30)) + + result_icp = o3d.pipelines.registration.registration_icp( + source, + target, + distance_threshold / 2, + result_ransac.transformation, + o3d.pipelines.registration.TransformationEstimationPointToPlane(), + o3d.pipelines.registration.ICPConvergenceCriteria(max_iteration=200), + ) + return result_icp.transformation, float(result_icp.fitness), float(result_icp.inlier_rmse) + + def run_simulated_registration( + self, + source: o3d.geometry.PointCloud, + voxel_size: float = 10.0, + roll: float = 10.0, + pitch: float = 10.0, + yaw: float = 10.0, + trans_range: float = 0.0, + random_seed: int | None = None, + ) -> RegistrationResult: + rotation = self.rotation_matrix(roll, pitch, yaw) + translation = self.translation_vector(trans_range, random_seed) + target = self.transform_point_cloud(source, rotation, translation) + transformation, fitness, rmse = self.register(source, target, voxel_size) + recovered = copy.deepcopy(target) + recovered.transform(np.linalg.inv(transformation)) + return RegistrationResult(transformation, fitness, rmse, target, recovered) diff --git a/alg/point_porj/requirements.txt b/alg/point_porj/requirements.txt new file mode 100644 index 0000000..856aad5 --- /dev/null +++ b/alg/point_porj/requirements.txt @@ -0,0 +1,16 @@ +# Python 3.10 is recommended. +# Use environment.yml for conda-only dependencies such as pythonocc-core. +--extra-index-url https://download.pytorch.org/whl/cu118 + +numpy==2.2.2 +scipy +open3d==0.19.0 +trimesh==4.5.3 +torch +torchvision +torchaudio +PyYAML + +# CAD STEP/STL conversion: +# conda env update -n point-porj -f environment.yml +# pip install git+https://github.com/AutodeskAILab/occwl diff --git a/alg/point_porj/visualization.py b/alg/point_porj/visualization.py new file mode 100644 index 0000000..1983873 --- /dev/null +++ b/alg/point_porj/visualization.py @@ -0,0 +1,97 @@ +from __future__ import annotations + +import copy + +import numpy as np +import open3d as o3d + +from preprocessing.plane import PlaneResult + + +class Visualizer: + """Open3D visualization utilities.""" + + def show_registration( + self, + source: o3d.geometry.PointCloud, + target: o3d.geometry.PointCloud, + recovered: o3d.geometry.PointCloud, + ) -> None: + source_colored = copy.deepcopy(source) + target_colored = copy.deepcopy(target) + recovered_colored = copy.deepcopy(recovered) + source_colored.paint_uniform_color([0, 0, 1]) + target_colored.paint_uniform_color([0, 1, 0]) + recovered_colored.paint_uniform_color([1, 0, 0]) + + o3d.visualization.draw_geometries( + [source_colored, target_colored], + window_name="Before Registration (Blue: Source, Green: Target)", + width=1024, + height=768, + ) + o3d.visualization.draw_geometries( + [recovered_colored, target_colored], + window_name="After Registration (Red: Recovered, Green: Target)", + width=1024, + height=768, + ) + + def show_denoise( + self, + clean: o3d.geometry.PointCloud, + noisy: o3d.geometry.PointCloud, + filtered: o3d.geometry.PointCloud | None = None, + filter_name: str = "Filtered", + ) -> None: + clean_colored = copy.deepcopy(clean) + noisy_colored = copy.deepcopy(noisy) + clean_colored.paint_uniform_color([0, 0, 1]) + noisy_colored.paint_uniform_color([1.0, 0.7, 0.5]) + o3d.visualization.draw_geometries( + [clean_colored, noisy_colored], + window_name=f"Clean (Blue, {len(clean.points)}) vs Noisy (Orange, {len(noisy.points)})", + ) + + if filtered is not None: + filtered_colored = copy.deepcopy(filtered) + filtered_colored.paint_uniform_color([0, 1, 0]) + o3d.visualization.draw_geometries( + [clean_colored, filtered_colored], + window_name=f"{filter_name}: Clean (Blue) vs Filtered (Green)", + ) + + def show_curvature(self, pointcloud: o3d.geometry.PointCloud, window_name: str = "Curvature Visualization") -> None: + o3d.visualization.draw_geometries([pointcloud], window_name=window_name) + + def show_plane(self, plane_result: PlaneResult) -> None: + pointcloud = copy.deepcopy(plane_result.pointcloud) + pointcloud.paint_uniform_color([0.2, 0.4, 1.0]) + inlier_cloud = pointcloud.select_by_index(plane_result.inliers) + inlier_cloud.paint_uniform_color([1.0, 0.2, 0.2]) + + plane_mesh = self._create_plane_mesh(plane_result.model) + o3d.visualization.draw_geometries( + [pointcloud, inlier_cloud, plane_mesh], + window_name="Plane Fitting", + width=1024, + height=768, + ) + + def _create_plane_mesh(self, plane_model: tuple[float, float, float, float]) -> o3d.geometry.TriangleMesh: + a, b, c, d = plane_model + size = 1.0 + xx, yy = np.meshgrid(np.linspace(-size, size, 2), np.linspace(-size, size, 2)) + if abs(c) < 1e-12: + zz = np.zeros_like(xx) + else: + zz = -(a * xx + b * yy + d) / c + + vertices = np.column_stack([xx.ravel(), yy.ravel(), zz.ravel()]) + triangles = np.array([[0, 1, 2], [1, 3, 2]]) + mesh = o3d.geometry.TriangleMesh() + mesh.vertices = o3d.utility.Vector3dVector(vertices) + mesh.triangles = o3d.utility.Vector3iVector(triangles) + mesh.paint_uniform_color([0.1, 0.9, 0.1]) + mesh.compute_vertex_normals() + return mesh diff --git a/alg/点云测距v1.1/train_measurement_system/.claude/settings.local.json b/alg/点云测距v1.1/train_measurement_system/.claude/settings.local.json new file mode 100644 index 0000000..5f0b6a8 --- /dev/null +++ b/alg/点云测距v1.1/train_measurement_system/.claude/settings.local.json @@ -0,0 +1,15 @@ +{ + "permissions": { + "allow": [ + "Bash(conda activate *)", + "Bash(pip list *)", + "Bash(conda run *)", + "Bash(python main.py --help)", + "Bash(C:/Users/suzhe/anaconda3/envs/pcc-experiment/python.exe main.py)", + "Bash(C:/Users/suzhe/anaconda3/envs/pcc-experiment/python.exe main.py -o results/test_output)", + "Bash(conda create *)", + "Bash(conda env *)", + "Bash(python *)" + ] + } +} diff --git a/alg/点云测距v1.1/train_measurement_system/CLAUDE.md b/alg/点云测距v1.1/train_measurement_system/CLAUDE.md new file mode 100644 index 0000000..a66d67c --- /dev/null +++ b/alg/点云测距v1.1/train_measurement_system/CLAUDE.md @@ -0,0 +1,145 @@ +# Train Measurement System - Developer Guide + +## Architecture Overview + +This is a **configuration-driven, plugin-based point cloud measurement system** for train inspection. The core design principle: measurements are defined in YAML, not hardcoded in Python. + +### Key Design Decisions + +**Why configuration-driven?** +- Users need to measure 20+ different features per train scan +- Measurement locations are consistent across scans but vary by train model +- Non-programmers should be able to add new measurement tasks by editing YAML + +**Why plugin-based measurement types?** +- Three fundamentally different measurement algorithms: plane-to-plane, point-to-plane, point-to-point +- Each type has different input requirements (ROI bboxes vs exact coordinates) +- Easy to add new types (e.g., circle-to-circle) without modifying core logic + +**Why single large point cloud input?** +- Train scans are 100M points covering the entire vehicle +- ROI extraction happens at runtime based on bbox parameters +- Avoids pre-segmentation and maintains spatial context + +## Common Development Tasks + +### Running measurements +```bash +conda activate pcc-experiment +cd train_measurement_system +python main.py +``` + +Results: `results/measurement_results_YYYYMMDD_HHMMSS.xlsx` +Visualizations: `visualizations/*.ply` + +### Adding a new measurement task + +1. **Edit `config/measurements.yaml`** - add task definition with appropriate type +2. **Set `enabled: true`** and provide required parameters for that type +3. **Run `python main.py`** - the task will execute automatically +4. **Check Excel results** - verify mean, std dev, inlier ratio +5. **Inspect PLY files** - confirm ROI extraction is correct + +### Adding a new measurement type + +1. **Create `src/measurements/your_type.py`** inheriting from `BaseMeasurement` +2. **Implement `execute() -> MeasurementResult`** with your algorithm +3. **Register in `src/measurements/__init__.py`** - add to `__all__` +4. **Update `src/task_manager.py`** - add branch in `_create_measurement_task()` +5. **Document in README.md** - add config example and parameter descriptions + +## Architecture Flow + +``` +YAML Config → TaskManager → [Measurement Plugin] → Excel + PLY + ↓ + PointCloudLoader (cached) + ↓ + ROIExtractor / PlaneFitter (shared utilities) +``` + +**TaskManager** orchestrates everything: +- Loads point cloud once, caches in memory +- Iterates through enabled tasks +- Instantiates appropriate measurement class based on `type` field +- Collects results and delegates to ExcelExporter + +**Measurement plugins** are stateless: +- Receive full point cloud + task config in constructor +- Extract ROI, run algorithm, return `MeasurementResult` +- No shared state between tasks + +## Critical Implementation Details + +### Coordinate System +- Point cloud uses **global coordinates** (mm units) +- Z-axis is vertical (up positive) +- ROI bboxes are in global coordinates +- No coordinate transformations applied during extraction + +### RANSAC Plane Fitting +- `inlier_ratio` in results indicates ROI quality, not measurement accuracy +- 100% inlier ratio = ROI perfectly contains only the target plane +- Low inlier ratio (<70%) = ROI includes multiple surfaces or noise +- **Automatic downsampling**: ROIs >10k points are sampled to 10k before SVD fitting (prevents memory explosion) + +### Point-to-Plane Distance +- Two modes: `exact_points` (KDTree nearest neighbor) vs `bbox_filtered` (extract then filter) +- `exact_points` is faster but requires knowing precise coordinates +- `bbox_filtered` supports filters like `max_abs_coordinate` for finding extreme points + +### Plane-to-Plane Distance +- **Unidirectional sampling**: samples points from source plane, measures to target plane +- Not symmetric: swapping source/target may give different results if planes aren't parallel +- Standard deviation indicates parallelism quality + +## Testing Strategy + +**When modifying measurement algorithms:** +1. Disable all tasks except one test case (`enabled: false`) +2. Run and verify Excel output matches expected values +3. Open PLY files in CloudCompare/MeshLab to visually verify ROI extraction +4. Check console output for warnings (low inlier ratio, insufficient points) + +**When adding new measurement types:** +1. Start with a simple test case with known ground truth +2. Verify the `MeasurementResult.data` dictionary contains all required fields +3. Ensure visualization saves the relevant points (not the entire ROI) +4. Test with edge cases: empty ROI, single point, coplanar points + +## Common Pitfalls + +- **Forgetting to set `enabled: true`** - task silently skipped +- **Wrong coordinate order** - YAML uses `[x, y, z]`, some tools use `[z, y, x]` +- **Bbox too large** - includes multiple surfaces, low inlier ratio +- **Bbox too small** - insufficient points for robust fitting +- **Missing `visualization_dir`** - no PLY files generated (optional but recommended) +- **Modifying cached point cloud** - reload by restarting Python process + +## Performance Notes + +- Point cloud loading: ~10-30 seconds for 100M points (cached after first load) +- Single plane-to-plane task: ~5-10 seconds (ROI extraction + RANSAC + sampling) +- Bottleneck is usually RANSAC iterations, not point cloud I/O +- Visualization (PLY save) adds ~1-2 seconds per task + +## Extension Points + +**Want to add CAD-based ROI extraction?** +- Modify `roi_extractor.py` to accept plane parameters from CAD +- Keep bbox-based extraction as fallback +- Add `roi_source: "cad"` vs `"bbox"` in config + +**Want to add statistical outlier removal?** +- Add preprocessing step in measurement plugins before RANSAC +- Make it optional via `params.remove_outliers: true` + +**Want to support multiple point cloud files?** +- Modify `task_manager.py` to accept per-task `pointcloud_file` override +- Update `pointcloud_loader.py` to cache multiple files by path + +**Want to add 2D projections or cross-sections?** +- Create new measurement type `cross_section_profile` +- Use `roi_extractor` to get 3D points, then project to 2D plane +- Output profile data as additional sheet in Excel diff --git a/alg/点云测距v1.1/train_measurement_system/README.md b/alg/点云测距v1.1/train_measurement_system/README.md new file mode 100644 index 0000000..6f53eb0 --- /dev/null +++ b/alg/点云测距v1.1/train_measurement_system/README.md @@ -0,0 +1,486 @@ +# 列车点云测量系统 + +基于配置驱动的点云测量系统，支持多种测量任务类型。通过YAML配置文件定义测量任务，自动完成ROI提取、平面拟合、距离计算，并生成Excel报告和可视化点云文件。 + +## 项目结构 + +``` +train_measurement_system/ +├── config/ +│ └── measurements.yaml # 测量任务配置文件 +├── src/ +│ ├── __init__.py # 包初始化文件 +│ ├── measurements/ +│ │ ├── __init__.py # 子包初始化文件 +│ │ ├── base.py # 测量任务基类 +│ │ ├── plane_distance.py # 平面到平面距离测量 +│ │ ├── point_to_plane.py # 点到平面距离测量 +│ │ └── point_to_point.py # 点到点距离测量 +│ ├── config_loader.py # 配置加载器 +│ ├── task_manager.py # 任务管理器 +│ ├── roi_extractor.py # ROI提取 +│ ├── plane_fitter.py # 平面拟合（RANSAC） +│ ├── pointcloud_loader.py # 点云加载 +│ ├── visualizer.py # 可视化工具 +│ └── excel_exporter.py # Excel导出 +├── results/ # 测量结果输出目录（Excel文件） +├── visualizations/ # 可视化点云输出目录（PLY文件） +├── main.py # 主程序入口 +├── pyproject.toml # 项目配置文件（PEP 621） +├── setup.py # 安装脚本（向后兼容） +├── requirements.txt # 依赖清单 +├── USAGE.md # 使用说明 +└── README.md # 本文件 +``` + +## 功能特性 + +- **配置驱动**: 通过YAML配置文件定义测量任务，无需修改代码 +- **多任务类型**: 支持平面到平面、点到平面、点到点三种测量类型 +- **批量处理**: 一次运行可执行多个测量任务 +- **自动可视化**: 自动保存每个任务的ROI点云（PLY格式） +- **结果导出**: 自动生成格式化的Excel报告，包含详细统计信息 +- **鲁棒性强**: RANSAC平面拟合，自动处理离群点和噪声 +- **错误处理**: 单个任务失败不影响其他任务执行 +- **可扩展**: 易于添加新的测量类型 + +## 环境要求 + +- Python 3.9+ +- 推荐使用 conda 环境管理 + +## 快速开始 + +### 1. 创建并激活环境 + +```bash +# 创建 conda 环境 +conda create -n pcc-experiment python=3.9 +conda activate pcc-experiment +``` + +### 2. 安装依赖 + +**方式一：使用 requirements.txt** +```bash +pip install -r requirements.txt +``` + +**方式二：安装项目** +```bash +# 正常安装 +pip install . + +# 或开发模式安装（代码修改立即生效） +pip install -e . +``` + +安装项目会自动安装所有依赖包，并将项目注册到 Python 环境中。 + +### 2. 准备点云文件 + +确保你有完整的列车点云文件（PLY格式），例如： +``` +D:\lab\PCC\PCC\head_train_pointcloud\train_2_100M.ply +``` + +### 3. 配置测量任务 + +编辑 `config/measurements.yaml`，设置点云文件路径和测量任务。 + +**支持相对路径（相对于配置文件）或绝对路径：** + +```yaml +# 相对路径（推荐） +pointcloud_file: "../../../head_train_pointcloud/train_2_100M.ply" + +# 或绝对路径 +pointcloud_file: "D:\\lab\\PCC\\PCC\\head_train_pointcloud\\train_2_100M.ply" + +tasks: + - name: "任务21_侧门高度" + type: "plane_to_plane" + enabled: true + description: "测量侧门顶部到底部的距离" + visualization_dir: "visualizations" + + source_roi: + bbox_min: [-2500, 3358, -1545] + bbox_max: [-1500, 3362, -1525] + + target_roi: + bbox_min: [-2500, 1180, -1621] + bbox_max: [-1500, 1180, -1500] + + params: + sample_points: 100 + ransac_threshold: 2.0 + ransac_max_iterations: 1000 + min_inlier_ratio: 0.6 +``` + +### 4. 运行测量 + +```bash +# 使用默认配置文件 +python main.py + +# 指定配置文件 +python main.py -c config/custom.yaml + +# 指定点云文件（覆盖配置文件中的路径） +python main.py -p data/train.ply + +# 指定输出目录 +python main.py -o results/ + +# 组合使用 +python main.py -c config/custom.yaml -p data/train.ply -o results/ +``` + +**命令行参数：** +- `-c, --config`: 配置文件路径（默认: config/measurements.yaml） +- `-p, --pointcloud`: 点云文件路径（覆盖配置文件） +- `-o, --output`: 结果输出目录（默认: results/） + +### 5. 查看结果 + +**Excel报告：** 保存在 `results/` 目录下 +- 文件名格式：`measurement_results_YYYYMMDD_HHMMSS.xlsx` +- 包含每个任务的详细统计信息： + - 任务名称、类型、描述 + - 点位参数（ROI 范围、目标点坐标等） + - 测量数据（平均距离、标准差、最小/最大距离） + - 采样点数、ROI 点数、内点率 + - 错误信息（如果失败） + +**可视化点云：** 保存在 `visualizations/` 目录下 +- 文件格式：PLY +- 可使用CloudCompare或Open3D查看 +- 用于验证ROI提取和测量点位置是否正确 + +## 测量类型详解 + +### 1. plane_to_plane（平面到平面距离） + +从源平面采样点，计算到目标平面的距离。适用于测量车门高度、车体长度、窗口高度等。 + +**工作流程：** +1. 从source_roi包围盒提取点云 +2. 使用RANSAC拟合源平面 +3. 从拟合的内点中随机采样指定数量的点 +4. 从target_roi包围盒提取点云 +5. 使用RANSAC拟合目标平面 +6. 计算采样点到目标平面的有向距离 +7. 统计平均值、标准差、最小/最大距离 + +**配置参数：** +```yaml +type: "plane_to_plane" +source_roi: + bbox_min: [x_min, y_min, z_min] + bbox_max: [x_max, y_max, z_max] +target_roi: + bbox_min: [x_min, y_min, z_min] + bbox_max: [x_max, y_max, z_max] +params: + sample_points: 100 # 采样点数 + ransac_threshold: 2.0 # RANSAC距离阈值（mm） + ransac_max_iterations: 1000 # RANSAC最大迭代次数 + min_inlier_ratio: 0.6 # 最小内点比例 +``` + +**输出结果：** +- 平均距离（mm） +- 标准差（mm） +- 最小/最大距离（mm） +- 采样点数 +- Source ROI点数 +- Target ROI点数 +- 平面拟合内点率 + +**可视化文件：** +- `{任务名}_source_roi.ply`：源平面ROI点云 +- `{任务名}_target_roi.ply`：目标平面ROI点云 + +--- + +### 2. point_to_plane（点到平面距离） + +测量指定点到参考平面的距离。支持两种模式：精确坐标查找和包围盒筛选。 + +**工作流程：** +1. 根据模式获取测量点： + - `exact_points`模式：在点云中查找最近点 + - `bbox_filtered`模式：从包围盒提取点云并筛选 +2. 从target_roi提取点云并拟合参考平面（目前的实现中，直接用z=0平面，不做拟合） +3. 计算每个点到平面的距离 +4. 统计平均值、标准差等 + +**配置参数（exact_points模式）：** +```yaml +type: "point_to_plane" +mode: "exact_points" +points: + - [-753.5, 1052.5, 1445] + - [3446.5, 1053, 1445] + - [-753.5, 1052.5, -1445] +target_roi: + bbox_min: [x_min, y_min, z_min] + bbox_max: [x_max, y_max, z_max] +params: + search_radius: 50.0 # 查找最近点的搜索半径（mm） + ransac_threshold: 2.0 + ransac_max_iterations: 1000 + min_inlier_ratio: 0.6 + use_absolute_distance: true # 是否使用绝对距离 +``` + +**配置参数（bbox_filtered模式）：** +```yaml +type: "point_to_plane" +mode: "bbox_filtered" +source_rois: + - bbox_min: [x1_min, y1_min, z1_min] + bbox_max: [x1_max, y1_max, z1_max] + - bbox_min: [x2_min, y2_min, z2_min] + bbox_max: [x2_max, y2_max, z2_max] +filter: + type: "max_abs_coordinate" # 筛选类型 + axis: "z" # 筛选坐标轴 + top_n: 10 # 每个ROI选择前N个点 +target_roi: + bbox_min: [x_min, y_min, z_min] + bbox_max: [x_max, y_max, z_max] +params: + ransac_threshold: 2.0 + ransac_max_iterations: 1000 + min_inlier_ratio: 0.6 + use_absolute_distance: true +``` + +**输出结果：** +- 平均距离（mm） +- 标准差（mm） +- 最小/最大距离（mm） +- 测量点数 +- Target ROI点数 +- 平面拟合内点率 + +**可视化文件：** +- `{任务名}_found_points.ply`：测量点点云 + +--- + +### 3. point_to_point（点到点距离） + +测量点对之间的欧氏距离。支持对称点自动配对（例如测量关于z=0对称的点对）。 + +**工作流程：** +1. 获取测量点：在点云中查找最近点 +2. 根据z坐标符号自动配对（z>0和z<0的点配对） +3. 计算每对点之间的欧氏距离 +4. 统计平均值、标准差等 + +**配置参数：** +``` yaml +type: "point_to_point" + +# 点对配置（使用任务3的6个点，配对测量） +point_pairs: + - name: "前端点对" + point1: [-753.5, 1052.5, -1445] + point2: [-753.5, 1052.5, 1445] + - name: "中间点对" + point1: [3446.5, 1053, -1445] + point2: [3446.5, 1053, 1445] + - name: "后端点对" + point1: [8146.5, 1053, -1445] + point2: [8146.5, 1053, 1445] + +# 最近点搜索半径(mm) +search_radius: 10.0 +``` + +**输出结果：** +- 平均距离（mm） +- 标准差（mm） +- 最小/最大距离（mm） +- 点对数量 + +**可视化文件：** +- `{任务名}_found_points.ply`：包含测量点的点云 + +--- + +## 配置文件说明 + +### 基本结构 + +```yaml +# 点云文件路径（必填） +pointcloud_file: "D:\\path\\to\\pointcloud.ply" + +# 测量任务列表 +tasks: + - name: "任务名称" + type: "测量类型" + enabled: true/false + description: "任务描述" + visualization_dir: "visualizations" + # ... 其他参数 +``` + +### 通用参数 + +- `name`：任务名称（必填），用于标识任务和命名输出文件 +- `type`：测量类型（必填），可选值：`plane_to_plane`、`point_to_plane`、`point_to_point` +- `enabled`：是否启用任务（必填），设为`false`可跳过该任务 +- `description`：任务描述（可选），用于文档说明 +- `visualization_dir`：可视化输出目录（可选），设置后会保存ROI点云文件 + +### 包围盒格式 + +所有包围盒使用以下格式，坐标单位为毫米（mm）： + +```yaml +bbox_min: [x_min, y_min, z_min] +bbox_max: [x_max, y_max, z_max] +``` + +**坐标系说明：** +- X轴：列车横向（车头到车尾） +- Y轴：列车高度（底部到顶部） +- Z轴：列车纵向（左侧到右侧） + +### RANSAC参数调优 + +RANSAC用于平面拟合，以下参数影响拟合质量： + +- `ransac_threshold`：内点距离阈值（mm） + - 较小值（1.0-2.0）：严格拟合，适合高质量点云 + - 较大值（3.0-5.0）：宽松拟合，适合噪声较多的点云 + +- `ransac_max_iterations`：最大迭代次数 + - 默认1000次，通常足够 + - 如果内点率很低，可增加到2000-5000 + +- `min_inlier_ratio`：最小内点比例 + - 默认0.6（60%），表示至少60%的点在平面上 + - 如果点云包含多个平面或噪声多，可降低到0.4-0.5 + +**调优建议：** +1. 先运行一次，查看内点率 +2. 如果内点率低于min_inlier_ratio，任务会失败 +3. 根据实际内点率调整参数： + - 内点率50%-60%：降低min_inlier_ratio或增大ransac_threshold + - 内点率>90%：可以提高要求，减小ransac_threshold + +--- + +## 添加新任务 + +### 步骤1：确定测量类型 + +根据测量需求选择合适的类型： +- **平面到平面距离**：车门高度、车体长度、窗口高度等 +- **点到平面距离**：特定点到参考平面的高度 +- **点到点距离**：对称点对之间的距离 + +### 步骤2：确定ROI包围盒 + +**方法1：使用点云浏览器工具** + + +**方法2：使用CloudCompare或Open3D或Freecad手动查看** +- 打开点云文件 +- 使用测量工具确定目标区域的坐标范围 +- 记录包围盒的最小和最大坐标 + +### 步骤3：编辑配置文件 + +在 `config/measurements.yaml` 中添加新任务： + +```yaml +- name: "新任务名称" + type: "plane_to_plane" # 或其他类型 + enabled: true + description: "任务描述" + visualization_dir: "visualizations" + + source_roi: + bbox_min: [x_min, y_min, z_min] + bbox_max: [x_max, y_max, z_max] + + target_roi: + bbox_min: [x_min, y_min, z_min] + bbox_max: [x_max, y_max, z_max] + + params: + sample_points: 100 + ransac_threshold: 2.0 + ransac_max_iterations: 1000 + min_inlier_ratio: 0.6 +``` + +### 步骤4：运行测试 + +```bash +python main.py +``` + +### 步骤5：验证结果 + +1. **查看Excel报告**：检查测量结果是否合理 +2. **查看可视化点云**：使用CloudCompare或Open3D打开PLY文件 + - 确认ROI提取的位置正确 + - 确认平面拟合质量（内点率） +3. **调整参数**：如果结果不理想，调整包围盒或RANSAC参数 + +--- + +## 技术细节 + +### 平面拟合算法 + +系统使用Open3D的RANSAC算法进行平面拟合： + +1. **随机采样**：从点云中随机选择3个点 +2. **拟合平面**：计算平面方程 ax + by + cz + d = 0 +3. **统计内点**：计算所有点到平面的距离，距离小于阈值的为内点 +4. **迭代优化**：重复上述步骤，选择内点最多的平面 +5. **最终拟合**：使用所有内点进行最小二乘拟合（SVD），得到最优平面 + +**平面方程：** ax + by + cz + d = 0 +- (a, b, c)：平面法向量（单位向量） +- d：平面到原点的有向距离 + +### 距离计算 + +**点到平面距离：** +``` +distance = |ax + by + cz + d| / sqrt(a² + b² + c²) +``` + +由于法向量已归一化（sqrt(a² + b² + c²) = 1），简化为： +``` +distance = |ax + by + cz + d| +``` + +**有向距离：** +- 正值：点在法向量指向的一侧 +- 负值：点在法向量相反的一侧 +- 绝对距离：取绝对值，不考虑方向 + +**点到点距离（欧氏距离）：** +``` +distance = sqrt((x2-x1)² + (y2-y1)² + (z2-z1)²) +``` + + +### 性能优化 + +- **点云缓存**：点云文件只加载一次，所有任务共享 +- **自动降采样**：ROI超过10000点时自动降采样到10000点进行SVD拟合 +- **并行处理**：多个任务顺序执行，单个任务失败不影响其他任务 + diff --git a/alg/点云测距v1.1/train_measurement_system/USAGE.md b/alg/点云测距v1.1/train_measurement_system/USAGE.md new file mode 100644 index 0000000..1ce100c --- /dev/null +++ b/alg/点云测距v1.1/train_measurement_system/USAGE.md @@ -0,0 +1,122 @@ +# 使用说明 + +## 环境安装 + +### 1. 创建 Conda 环境 +```bash +conda create -n pcc-experiment python=3.9 +conda activate pcc-experiment +``` + +### 2. 安装依赖包 + +**方式一：使用 requirements.txt** +```bash +pip install -r requirements.txt +``` + +**方式二：安装项目** +```bash +# 正常安装 +pip install . + +# 或开发模式安装（代码修改立即生效） +pip install -e . +``` + +安装项目会自动安装所有依赖包，并将项目添加到 Python 环境中。 + +## 配置测量任务 + +编辑 `config/measurements.yaml` 文件： + +### 1. 设置点云文件路径 +```yaml +# 支持相对路径（相对于配置文件）或绝对路径 +pointcloud_file: "../../head_train_pointcloud/train_2_100M.ply" +``` + +### 2. 配置测量任务 +```yaml +tasks: + - name: "任务名称" + type: "plane_to_plane" # 任务类型: plane_to_plane, point_to_plane, point_to_point + enabled: true # 是否启用 + description: "任务描述" + visualization_dir: "visualizations" # 可视化输出目录（可选） + + # 根据任务类型配置相应参数 + source_roi: + bbox_min: [x_min, y_min, z_min] + bbox_max: [x_max, y_max, z_max] + + target_roi: + bbox_min: [x_min, y_min, z_min] + bbox_max: [x_max, y_max, z_max] + + params: + sample_points: 100 + ransac_threshold: 2.0 + ransac_max_iterations: 1000 + min_inlier_ratio: 0.6 +``` + +## 运行测量 + +### 基本用法 +```bash +# 使用默认配置文件 +python main.py + +# 指定配置文件 +python main.py -c config/custom.yaml + +# 指定点云文件（覆盖配置文件中的路径） +python main.py -p data/train.ply + +# 指定输出目录 +python main.py -o results/output + +# 组合使用 +python main.py -c config/custom.yaml -p data/train.ply -o results/ +``` + +### 命令行参数 +- `-c, --config`: 配置文件路径（默认: config/measurements.yaml） +- `-p, --pointcloud`: 点云文件路径（覆盖配置文件） +- `-o, --output`: 结果输出目录（默认: results/） + +## 查看结果 + +### Excel 结果文件 +位置: `results/measurement_results_YYYYMMDD_HHMMSS.xlsx` + +包含内容: +- 任务名称、类型、描述 +- 点位参数（ROI 范围、目标点坐标等） +- 测量数据（平均距离、标准差、最小/最大距离） +- 采样点数、ROI 点数、内点率 +- 错误信息（如果失败） + +### 可视化文件 +位置: `visualizations/*.ply` + +使用 CloudCompare 或 MeshLab 打开查看： +- 蓝色：源平面点云 +- 绿色：目标平面点云 +- 红色：采样点 + +## 常见问题 + +### 任务没有执行 +检查配置文件中 `enabled: true` 是否设置 + +### 点云文件找不到 +- 使用绝对路径，或 +- 使用相对于配置文件的相对路径，或 +- 使用 `-p` 参数指定点云文件 + +### 测量结果异常 +- 检查 ROI 包围盒是否正确 +- 查看 Excel 中的内点率（建议 >70%） +- 打开 PLY 文件检查 ROI 提取是否准确 diff --git a/alg/点云测距v1.1/train_measurement_system/config/measurements.yaml b/alg/点云测距v1.1/train_measurement_system/config/measurements.yaml new file mode 100644 index 0000000..599002c --- /dev/null +++ b/alg/点云测距v1.1/train_measurement_system/config/measurements.yaml @@ -0,0 +1,171 @@ +# 列车测量任务配置文件 + +# 点云文件路径（支持相对路径和绝对路径） +# 相对路径：相对于本配置文件所在目录 +# 绝对路径：完整路径 +pointcloud_file: "../train_2_100M.py" +# pointcloud_file: "../../../head_train_pointcloud/train_2_100M.ply" + +# 测量任务列表 +tasks: + - name: "任务21_侧门高度" + type: "plane_to_plane" + enabled: true + description: "测量侧门顶部到底部的距离" + visualization_dir: "visualizations" # 可视化输出目录 + + # 源平面（采样点的来源） + source_roi: + bbox_min: [-2500, 3358, -1545] + bbox_max: [-1500, 3362, -1525] + + # 目标平面（测量距离的目标） + target_roi: + bbox_min: [-2500, 1180, -1621] + bbox_max: [-1500, 1180, -1500] + + # 算法参数 + params: + # 从源平面采样的点数 + sample_points: 100 + # RANSAC平面拟合参数 + ransac_threshold: 2.0 # 内点距离阈值(mm) + ransac_max_iterations: 1000 + min_inlier_ratio: 0.6 # 最小内点比例 + + - name: "任务2_车体长度" + type: "plane_to_plane" + enabled: true + description: "测量车体前后端面的距离" + visualization_dir: "visualizations" + + source_roi: + bbox_min: [-14007, 1500, -800] + bbox_max: [-14006, 1900, 800] + + target_roi: + bbox_min: [12499, 1216, -1300] + bbox_max: [12501, 3200, 1300] + + params: + sample_points: 100 + ransac_threshold: 2.0 + ransac_max_iterations: 1000 + min_inlier_ratio: 0.6 + + - name: "任务43_窗口下沿高度" + type: "plane_to_plane" + enabled: true + description: "测量窗口下沿到地面的距离" + visualization_dir: "visualizations" + + source_roi: + bbox_min: [5562, 1982, 1672] + bbox_max: [6772, 1983, 1677] + + target_roi: + bbox_min: [-10222, 1180, -302] + bbox_max: [12428, 1180, 302] + + params: + sample_points: 100 + ransac_threshold: 2.0 + ransac_max_iterations: 1000 + min_inlier_ratio: 0.6 + + - name: "任务44_车顶高度" + type: "plane_to_plane" + enabled: true + description: "测量车顶到地面的距离" + visualization_dir: "visualizations" + + source_roi: + bbox_min: [-1419, 4000, -237] + bbox_max: [9030, 4000, 237] + + target_roi: + bbox_min: [-10222, 1180, -302] + bbox_max: [12428, 1180, 302] + + params: + sample_points: 100 + ransac_threshold: 2.0 + ransac_max_iterations: 1000 + min_inlier_ratio: 0.6 + + - name: "任务3_边梁立面6点到z=0平面距离" + type: "point_to_plane" + enabled: true + description: "测量边梁立面6个精确点到z=0平面的距离" + mode: "exact_points" + visualization_dir: "visualizations" # 可视化输出目录 + + # 目标点坐标（左右各3个点） + target_points: + - [-753.5, 1052.5, -1445] + - [-753.5, 1052.5, 1445] + - [3446.5, 1053, -1445] + - [3446.5, 1053, 1445] + - [8146.5, 1053, -1445] + - [8146.5, 1053, 1445] + + # 平面参数 (ax + by + cz + d = 0)，z=0即c=1, d=0 + plane: + a: 0 + b: 0 + c: 1 + d: 0 + + # 最近点搜索半径(mm) + search_radius: 10.0 + + - name: "任务15_侧墙z最大值点到z=0平面距离" + type: "point_to_plane" + enabled: true + description: "从两个包围盒提取侧墙点云，选取z绝对值最大的10个点，计算到z=0平面的距离" + mode: "bbox_filtered" + visualization_dir: "visualizations" # 可视化输出目录 + + # 包围盒列表 + bboxes: + - name: "左侧墙" + min: [-1468, 1899, -1680] + max: [12300, 1901, -1677] + - name: "右侧墙" + min: [-1468, 1899, 1677] + max: [12300, 1901, 1680] + + # 过滤条件 + filter: + type: "max_abs_coordinate" + axis: "z" + top_n: 10 + + # 平面参数 (z=0) + plane: + a: 0 + b: 0 + c: 1 + d: 0 + + - name: "任务4_纵向梁距离" + type: "point_to_point" + enabled: true + description: "测量关于z=0对称的点对之间的距离（纵向梁）" + visualization_dir: "visualizations" + + # 点对配置（使用任务3的6个点，配对测量） + point_pairs: + - name: "前端点对" + point1: [-753.5, 1052.5, -1445] + point2: [-753.5, 1052.5, 1445] + - name: "中间点对" + point1: [3446.5, 1053, -1445] + point2: [3446.5, 1053, 1445] + - name: "后端点对" + point1: [8146.5, 1053, -1445] + point2: [8146.5, 1053, 1445] + + # 最近点搜索半径(mm) + search_radius: 10.0 + diff --git a/alg/点云测距v1.1/train_measurement_system/main.py b/alg/点云测距v1.1/train_measurement_system/main.py new file mode 100644 index 0000000..08ad98f --- /dev/null +++ b/alg/点云测距v1.1/train_measurement_system/main.py @@ -0,0 +1,126 @@ +"""列车点云测量系统 - 主程序入口""" +import sys +import argparse +from pathlib import Path + +# 添加src目录到Python路径 +sys.path.insert(0, str(Path(__file__).parent / "src")) + +from task_manager import TaskManager + + +def parse_args(): + """解析命令行参数""" + parser = argparse.ArgumentParser( + description="列车点云测量系统", + formatter_class=argparse.RawDescriptionHelpFormatter, + epilog=""" +示例: + python main.py # 使用默认配置文件 + python main.py -c config/custom.yaml # 指定配置文件 + python main.py -p data/train.ply # 指定点云文件（覆盖配置） + python main.py -o results/output # 指定输出目录 + python main.py -c config/custom.yaml -p data/train.ply -o results/ + """ + ) + + parser.add_argument( + '-c', '--config', + type=str, + default='config/measurements.yaml', + help='配置文件路径 (默认: config/measurements.yaml)' + ) + + parser.add_argument( + '-p', '--pointcloud', + type=str, + default=None, + help='点云文件路径 (覆盖配置文件中的路径)' + ) + + parser.add_argument( + '-o', '--output', + type=str, + default=None, + help='结果输出目录 (覆盖配置文件中的路径，默认: results/)' + ) + + return parser.parse_args() + + +def main(): + """主函数""" + print("=" * 60) + print("列车点云测量系统") + print("=" * 60) + + # 解析命令行参数 + args = parse_args() + + # 配置文件路径（支持相对路径和绝对路径） + config_file = Path(args.config) + if not config_file.is_absolute(): + config_file = Path(__file__).parent / config_file + + if not config_file.exists(): + print(f"错误: 配置文件不存在: {config_file}") + return 1 + + print(f"\n加载配置文件: {config_file}") + + # 创建任务管理器 + manager = TaskManager() + + # 加载配置 + if not manager.load_config(str(config_file)): + print("错误: 配置文件加载失败") + return 1 + + # 命令行参数覆盖配置 + if args.pointcloud: + pointcloud_path = Path(args.pointcloud) + if not pointcloud_path.is_absolute(): + pointcloud_path = Path(__file__).parent / pointcloud_path + manager.override_pointcloud_path(str(pointcloud_path)) + print(f"使用命令行指定的点云文件: {pointcloud_path}") + + if args.output: + output_dir = Path(args.output) + if not output_dir.is_absolute(): + output_dir = Path(__file__).parent / output_dir + manager.override_output_dir(str(output_dir)) + print(f"使用命令行指定的输出目录: {output_dir}") + + # 执行所有测量任务 + print("\n开始执行测量任务...") + print("-" * 60) + + results = manager.run_all_tasks() + + # 输出结果摘要 + print("\n" + "=" * 60) + print("测量结果摘要") + print("=" * 60) + + success_count = sum(1 for r in results if r['status'] == 'success') + failed_count = len(results) - success_count + + print(f"\n总任务数: {len(results)}") + print(f"成功: {success_count}") + print(f"失败: {failed_count}") + + if failed_count > 0: + print("\n失败的任务:") + for result in results: + if result['status'] == 'failed': + print(f" - {result['name']}: {result.get('error', '未知错误')}") + + print("\n" + "=" * 60) + print("测量完成!") + print("=" * 60) + + return 0 if failed_count == 0 else 1 + + +if __name__ == "__main__": + sys.exit(main()) diff --git a/alg/点云测距v1.1/train_measurement_system/pyproject.toml b/alg/点云测距v1.1/train_measurement_system/pyproject.toml new file mode 100644 index 0000000..e87352f --- /dev/null +++ b/alg/点云测距v1.1/train_measurement_system/pyproject.toml @@ -0,0 +1,52 @@ +[build-system] +requires = ["setuptools>=61.0", "wheel"] +build-backend = "setuptools.build_meta" + +[project] +name = "train-measurement-system" +version = "1.1.0" +description = "配置驱动的列车点云测量系统" +readme = "README.md" +requires-python = ">=3.9,<3.12" +license = {text = "MIT"} +keywords = ["point-cloud", "measurement", "train", "3d"] +classifiers = [ + "Development Status :: 4 - Beta", + "Intended Audience :: Science/Research", + "Programming Language :: Python :: 3", + "Programming Language :: Python :: 3.9", + "Programming Language :: Python :: 3.10", + "Programming Language :: Python :: 3.11", +] + +dependencies = [ + "numpy>=2.0.0", + "open3d>=0.18.0", + "scipy>=1.10.0", + "pandas>=2.0.0", + "openpyxl>=3.1.0", + "PyYAML>=6.0.0", +] + +[project.optional-dependencies] +viz = [ + "matplotlib>=3.7.0", + "plotly>=5.0.0", +] +dev = [ + "pytest>=7.0.0", + "black>=23.0.0", + "flake8>=6.0.0", +] + +[project.urls] +Homepage = "https://github.com/yourusername/train-measurement-system" +Documentation = "https://github.com/yourusername/train-measurement-system/blob/main/README.md" +Repository = "https://github.com/yourusername/train-measurement-system" + +[tool.setuptools] +package-dir = {"" = "."} +packages = ["src", "src.measurements"] + +[tool.setuptools.package-data] +"*" = ["*.yaml", "*.yml"] diff --git a/alg/点云测距v1.1/train_measurement_system/requirements.txt b/alg/点云测距v1.1/train_measurement_system/requirements.txt new file mode 100644 index 0000000..55dbfd1 --- /dev/null +++ b/alg/点云测距v1.1/train_measurement_system/requirements.txt @@ -0,0 +1,19 @@ +# 列车点云测量系统依赖包 +# Python 版本要求: 3.9, 3.10, 3.11 (open3d 暂不支持 3.12+) +# 推荐使用 Python 3.10 + +# 核心依赖 +numpy>=2.0.0 +open3d>=0.18.0 +scipy>=1.10.0 + +# 数据处理 +pandas>=2.0.0 +openpyxl>=3.1.0 + +# 配置文件 +PyYAML>=6.0.0 + +# 可视化（可选） +matplotlib>=3.7.0 +plotly>=5.0.0 diff --git a/alg/点云测距v1.1/train_measurement_system/setup.py b/alg/点云测距v1.1/train_measurement_system/setup.py new file mode 100644 index 0000000..dfc396b --- /dev/null +++ b/alg/点云测距v1.1/train_measurement_system/setup.py @@ -0,0 +1,11 @@ +""" +Setup script for train-measurement-system. + +This is a minimal setup.py for backward compatibility. +All configuration is in pyproject.toml. +""" + +from setuptools import setup + +if __name__ == "__main__": + setup() diff --git a/alg/点云测距v1.1/train_measurement_system/src/__init__.py b/alg/点云测距v1.1/train_measurement_system/src/__init__.py new file mode 100644 index 0000000..2ae3a5e --- /dev/null +++ b/alg/点云测距v1.1/train_measurement_system/src/__init__.py @@ -0,0 +1,25 @@ +""" +Train Measurement System + +配置驱动的列车点云测量系统 +""" + +__version__ = "1.1.0" + +from .config_loader import ConfigLoader +from .pointcloud_loader import PointCloudLoader +from .roi_extractor import ROIExtractor +from .plane_fitter import PlaneFitter +from .task_manager import TaskManager +from .excel_exporter import ExcelExporter +from .visualizer import Visualizer + +__all__ = [ + "ConfigLoader", + "PointCloudLoader", + "ROIExtractor", + "PlaneFitter", + "TaskManager", + "ExcelExporter", + "Visualizer", +] diff --git a/alg/点云测距v1.1/train_measurement_system/src/config_loader.py b/alg/点云测距v1.1/train_measurement_system/src/config_loader.py new file mode 100644 index 0000000..b881b22 --- /dev/null +++ b/alg/点云测距v1.1/train_measurement_system/src/config_loader.py @@ -0,0 +1,82 @@ +"""配置加载模块""" +import yaml +from pathlib import Path +from typing import Dict, Any, List + + +class ConfigLoader: + """加载和验证YAML配置文件""" + + def __init__(self, config_path: str): + """ + Args: + config_path: 配置文件路径 + """ + self.config_path = Path(config_path) + self.config = None + + def load(self) -> Dict[str, Any]: + """加载配置文件""" + if not self.config_path.exists(): + raise FileNotFoundError(f"配置文件不存在: {self.config_path}") + + with open(self.config_path, 'r', encoding='utf-8') as f: + self.config = yaml.safe_load(f) + + self._validate() + return self.config + + def _validate(self): + """验证配置文件格式""" + if not isinstance(self.config, dict): + raise ValueError("配置文件格式错误：根节点必须是字典") + + # 检查必需字段 + if 'point_cloud' not in self.config: + raise ValueError("配置文件缺少 'point_cloud' 字段") + + if 'tasks' not in self.config: + raise ValueError("配置文件缺少 'tasks' 字段") + + if not isinstance(self.config['tasks'], list): + raise ValueError("'tasks' 必须是列表") + + # 验证每个任务 + for i, task in enumerate(self.config['tasks']): + self._validate_task(task, i) + + def _validate_task(self, task: Dict[str, Any], index: int): + """验证单个任务配置""" + required_fields = ['name', 'type'] + for field in required_fields: + if field not in task: + raise ValueError(f"任务 {index} 缺少必需字段: {field}") + + # 验证任务类型 + valid_types = ['plane_to_plane', 'point_to_plane', 'point_to_point'] + if task['type'] not in valid_types: + raise ValueError(f"任务 {index} 类型无效: {task['type']}, 必须是 {valid_types} 之一") + + def get_point_cloud_path(self) -> str: + """获取点云文件路径""" + return self.config['point_cloud'] + + def get_tasks(self) -> List[Dict[str, Any]]: + """获取所有任务配置""" + return self.config['tasks'] + + def get_global_settings(self) -> Dict[str, Any]: + """获取全局设置""" + return self.config.get('global_settings', {}) + + def is_visualization_enabled(self) -> bool: + """是否启用可视化""" + return self.get_global_settings().get('enable_visualization', False) + + def get_visualization_dir(self) -> str: + """获取可视化输出目录""" + return self.get_global_settings().get('visualization_dir', 'visualizations') + + def get_results_dir(self) -> str: + """获取结果输出目录""" + return self.get_global_settings().get('results_dir', 'results') diff --git a/alg/点云测距v1.1/train_measurement_system/src/excel_exporter.py b/alg/点云测距v1.1/train_measurement_system/src/excel_exporter.py new file mode 100644 index 0000000..a2e0f66 --- /dev/null +++ b/alg/点云测距v1.1/train_measurement_system/src/excel_exporter.py @@ -0,0 +1,245 @@ +"""Excel结果导出器""" +from pathlib import Path +from typing import List, Dict, Any +from datetime import datetime +from openpyxl import Workbook +from openpyxl.styles import Font, Alignment, PatternFill, Border, Side +from openpyxl.utils import get_column_letter + +from measurements.base import MeasurementResult + + +class ExcelExporter: + """将测量结果导出为Excel文件""" + + def __init__(self, output_dir: str): + """ + 初始化导出器 + + Args: + output_dir: 输出目录路径 + """ + self.output_dir = Path(output_dir) + self.output_dir.mkdir(parents=True, exist_ok=True) + + def export(self, results: List[MeasurementResult], filename: str = None, task_configs: List[Dict[str, Any]] = None) -> str: + """ + 导出测量结果到Excel + + Args: + results: 测量结果列表 + filename: 输出文件名（不含扩展名），默认使用时间戳 + task_configs: 任务配置列表（用于提取描述和参数） + + Returns: + str: 导出的Excel文件路径 + """ + if filename is None: + filename = f"measurement_results_{datetime.now().strftime('%Y%m%d_%H%M%S')}" + + output_path = self.output_dir / f"{filename}.xlsx" + + # 创建任务名称到配置的映射 + task_config_map = {} + if task_configs: + for config in task_configs: + task_config_map[config.get('name')] = config + + # 创建工作簿 + wb = Workbook() + ws = wb.active + ws.title = "测量结果" + + # 写入标题 + self._write_header(ws) + + # 写入数据 + row = 2 + for result in results: + task_config = task_config_map.get(result.task_name, {}) + row = self._write_result(ws, result, row, task_config) + + # 调整列宽 + self._adjust_column_widths(ws) + + # 保存文件 + wb.save(output_path) + + return str(output_path) + + def _write_header(self, ws): + """写入表头""" + headers = [ + "任务名称", + "任务类型", + "任务描述", + "点位参数", + "状态", + "平均距离 (mm)", + "标准差 (mm)", + "最小距离 (mm)", + "最大距离 (mm)", + "采样点数", + "源ROI点数", + "目标ROI点数", + "源平面内点率", + "目标平面内点率", + "错误信息" + ] + + # 样式 + header_font = Font(bold=True, color="FFFFFF") + header_fill = PatternFill(start_color="4472C4", end_color="4472C4", fill_type="solid") + header_alignment = Alignment(horizontal="center", vertical="center") + border = Border( + left=Side(style='thin'), + right=Side(style='thin'), + top=Side(style='thin'), + bottom=Side(style='thin') + ) + + for col, header in enumerate(headers, 1): + cell = ws.cell(row=1, column=col) + cell.value = header + cell.font = header_font + cell.fill = header_fill + cell.alignment = header_alignment + cell.border = border + + def _format_task_params(self, task_config: Dict[str, Any]) -> str: + """ + 格式化任务参数为字符串 + + Args: + task_config: 任务配置字典 + + Returns: + str: 格式化后的参数字符串 + """ + task_type = task_config.get('type', '') + params_parts = [] + + if task_type == 'plane_to_plane': + # 平面到平面：显示源ROI和目标ROI + source_roi = task_config.get('source_roi', {}) + target_roi = task_config.get('target_roi', {}) + if source_roi: + params_parts.append(f"源ROI: min{source_roi.get('bbox_min', [])} max{source_roi.get('bbox_max', [])}") + if target_roi: + params_parts.append(f"目标ROI: min{target_roi.get('bbox_min', [])} max{target_roi.get('bbox_max', [])}") + + elif task_type == 'point_to_plane': + mode = task_config.get('mode', '') + if mode == 'exact_points': + # 精确点模式：显示目标点坐标 + target_points = task_config.get('target_points', []) + if target_points: + params_parts.append(f"目标点({len(target_points)}个): {target_points}") + plane = task_config.get('plane', {}) + if plane: + params_parts.append(f"平面: {plane.get('a', 0)}x+{plane.get('b', 0)}y+{plane.get('c', 0)}z+{plane.get('d', 0)}=0") + elif mode == 'bbox_filtered': + # 包围盒过滤模式：显示包围盒 + bboxes = task_config.get('bboxes', []) + if bboxes: + bbox_strs = [f"{bbox.get('name', '')}: min{bbox.get('min', [])} max{bbox.get('max', [])}" for bbox in bboxes] + params_parts.append(f"包围盒: {'; '.join(bbox_strs)}") + filter_info = task_config.get('filter', {}) + if filter_info: + params_parts.append(f"过滤: {filter_info.get('type', '')} axis={filter_info.get('axis', '')} top_n={filter_info.get('top_n', '')}") + + elif task_type == 'point_to_point': + # 点到点：显示点对 + point_pairs = task_config.get('point_pairs', []) + if point_pairs: + pair_strs = [f"{pair.get('name', '')}: {pair.get('point1', [])} <-> {pair.get('point2', [])}" for pair in point_pairs] + params_parts.append(f"点对({len(point_pairs)}对): {'; '.join(pair_strs)}") + + return '\n'.join(params_parts) if params_parts else "无" + + def _write_result(self, ws, result: MeasurementResult, row: int, task_config: Dict[str, Any]) -> int: + """ + 写入单个测量结果 + + Args: + ws: 工作表 + result: 测量结果 + row: 起始行号 + task_config: 任务配置 + + Returns: + int: 下一行号 + """ + border = Border( + left=Side(style='thin'), + right=Side(style='thin'), + top=Side(style='thin'), + bottom=Side(style='thin') + ) + + # 基本信息 + ws.cell(row=row, column=1).value = result.task_name + ws.cell(row=row, column=2).value = result.task_type + ws.cell(row=row, column=3).value = task_config.get('description', '') + ws.cell(row=row, column=4).value = self._format_task_params(task_config) + ws.cell(row=row, column=5).value = "成功" if result.success else "失败" + + # 状态颜色 + status_cell = ws.cell(row=row, column=5) + if result.success: + status_cell.fill = PatternFill(start_color="C6EFCE", end_color="C6EFCE", fill_type="solid") + status_cell.font = Font(color="006100") + else: + status_cell.fill = PatternFill(start_color="FFC7CE", end_color="FFC7CE", fill_type="solid") + status_cell.font = Font(color="9C0006") + + # 测量数据 + if result.success and result.data: + data = result.data + ws.cell(row=row, column=6).value = round(data.get('mean_distance', 0), 2) + ws.cell(row=row, column=7).value = round(data.get('std_distance', 0), 2) + ws.cell(row=row, column=8).value = round(data.get('min_distance', 0), 2) + ws.cell(row=row, column=9).value = round(data.get('max_distance', 0), 2) + ws.cell(row=row, column=10).value = data.get('sample_count', 0) + ws.cell(row=row, column=11).value = data.get('source_roi_points', 0) + ws.cell(row=row, column=12).value = data.get('target_roi_points', 0) + ws.cell(row=row, column=13).value = f"{data.get('source_inlier_ratio', 0) * 100:.1f}%" + ws.cell(row=row, column=14).value = f"{data.get('target_inlier_ratio', 0) * 100:.1f}%" + else: + # 失败时显示错误信息 + ws.cell(row=row, column=15).value = result.error_message + + # 应用边框和对齐 + for col in range(1, 16): + cell = ws.cell(row=row, column=col) + cell.border = border + # 点位参数列使用左对齐，其他居中 + if col == 4: + cell.alignment = Alignment(horizontal="left", vertical="center", wrap_text=True) + else: + cell.alignment = Alignment(horizontal="center", vertical="center") + + return row + 1 + + def _adjust_column_widths(self, ws): + """自动调整列宽""" + column_widths = { + 1: 25, # 任务名称 + 2: 18, # 任务类型 + 3: 30, # 任务描述 + 4: 60, # 点位参数 + 5: 10, # 状态 + 6: 15, # 平均距离 + 7: 15, # 标准差 + 8: 15, # 最小距离 + 9: 15, # 最大距离 + 10: 12, # 采样点数 + 11: 12, # 源ROI点数 + 12: 12, # 目标ROI点数 + 13: 15, # 源平面内点率 + 14: 15, # 目标平面内点率 + 15: 40 # 错误信息 + } + + for col, width in column_widths.items(): + ws.column_dimensions[get_column_letter(col)].width = width diff --git a/alg/点云测距v1.1/train_measurement_system/src/measurements/__init__.py b/alg/点云测距v1.1/train_measurement_system/src/measurements/__init__.py new file mode 100644 index 0000000..a04725c --- /dev/null +++ b/alg/点云测距v1.1/train_measurement_system/src/measurements/__init__.py @@ -0,0 +1,7 @@ +"""measurements包初始化""" +from .base import BaseMeasurement, MeasurementResult +from .plane_distance import PlaneToPlaneDistance +from .point_to_plane import PointToPlaneDistance +from .point_to_point import PointToPointDistance + +__all__ = ['BaseMeasurement', 'MeasurementResult', 'PlaneToPlaneDistance', 'PointToPlaneDistance', 'PointToPointDistance'] diff --git a/alg/点云测距v1.1/train_measurement_system/src/measurements/base.py b/alg/点云测距v1.1/train_measurement_system/src/measurements/base.py new file mode 100644 index 0000000..0db578d --- /dev/null +++ b/alg/点云测距v1.1/train_measurement_system/src/measurements/base.py @@ -0,0 +1,47 @@ +"""测量任务基类""" +from abc import ABC, abstractmethod +from typing import Dict, Any, Optional +from dataclasses import dataclass +import numpy as np + + +@dataclass +class MeasurementResult: + """测量结果数据类""" + task_name: str + task_type: str + success: bool + data: Optional[Dict[str, Any]] = None + error_message: Optional[str] = None + + +class BaseMeasurement(ABC): + """测量任务基类""" + + def __init__(self, name: str, point_cloud: np.ndarray, config: Dict[str, Any]): + """ + 初始化测量任务 + + Args: + name: 任务名称 + point_cloud: 完整点云数组 (N, 3) + config: 任务配置字典 + """ + self.name = name + self.point_cloud = point_cloud + self.config = config + self.task_type = config.get('type', 'unknown') + + @abstractmethod + def execute(self) -> MeasurementResult: + """ + 执行测量任务 + + Returns: + MeasurementResult: 测量结果 + """ + pass + + def is_enabled(self) -> bool: + """检查任务是否启用""" + return self.config.get('enabled', True) diff --git a/alg/点云测距v1.1/train_measurement_system/src/measurements/plane_distance.py b/alg/点云测距v1.1/train_measurement_system/src/measurements/plane_distance.py new file mode 100644 index 0000000..496e3c7 --- /dev/null +++ b/alg/点云测距v1.1/train_measurement_system/src/measurements/plane_distance.py @@ -0,0 +1,139 @@ +"""平面到平面距离测量""" +import numpy as np +from typing import Dict, Any +import sys +from pathlib import Path + +# 添加父目录到路径 +sys.path.insert(0, str(Path(__file__).parent.parent)) + +from measurements.base import BaseMeasurement, MeasurementResult +from roi_extractor import ROIExtractor +from plane_fitter import PlaneFitter +from visualizer import Visualizer + + +class PlaneToPlaneDistance(BaseMeasurement): + """测量两个平面之间的距离（单向采样）""" + + def execute(self) -> MeasurementResult: + """执行平面到平面距离测量""" + try: + # 1. 提取源平面ROI + source_roi = self.config.get('source_roi', {}) + source_points, _ = ROIExtractor.extract_bbox_points( + self.point_cloud, + source_roi.get('bbox_min'), + source_roi.get('bbox_max') + ) + + if len(source_points) == 0: + return MeasurementResult( + task_name=self.name, + task_type=self.task_type, + success=False, + error_message="源平面ROI区域内没有点" + ) + + # 2. 提取目标平面ROI + target_roi = self.config.get('target_roi', {}) + target_points, _ = ROIExtractor.extract_bbox_points( + self.point_cloud, + target_roi.get('bbox_min'), + target_roi.get('bbox_max') + ) + + if len(target_points) == 0: + return MeasurementResult( + task_name=self.name, + task_type=self.task_type, + success=False, + error_message="目标平面ROI区域内没有点" + ) + + # 3. 拟合源平面 + params = self.config.get('params', {}) + source_fitter = PlaneFitter( + threshold=params.get('ransac_threshold', 2.0), + max_iterations=params.get('ransac_max_iterations', 1000), + min_inlier_ratio=params.get('min_inlier_ratio', 0.6) + ) + source_plane = source_fitter.fit(source_points) + + if source_plane is None: + return MeasurementResult( + task_name=self.name, + task_type=self.task_type, + success=False, + error_message=f"源平面拟合失败（点数: {len(source_points)}）" + ) + + # 4. 拟合目标平面 + target_fitter = PlaneFitter( + threshold=params.get('ransac_threshold', 2.0), + max_iterations=params.get('ransac_max_iterations', 1000), + min_inlier_ratio=params.get('min_inlier_ratio', 0.6) + ) + target_plane = target_fitter.fit(target_points) + + if target_plane is None: + return MeasurementResult( + task_name=self.name, + task_type=self.task_type, + success=False, + error_message=f"目标平面拟合失败（点数: {len(target_points)}）" + ) + + # 5. 从源平面采样点 + n_samples = params.get('sample_points', 100) + source_inlier_points = source_points[source_plane.inliers] + sampled_points = ROIExtractor.sample_points(source_inlier_points, n_samples) + + # 6. 计算采样点到目标平面的距离 + distances = target_plane.distance_to_points(sampled_points) + + # 7. 统计结果 + mean_distance = np.mean(np.abs(distances)) + std_distance = np.std(distances) + min_distance = np.min(np.abs(distances)) + max_distance = np.max(np.abs(distances)) + + # 8. 生成可视化（如果配置了输出目录） + visualization_dir = self.config.get('visualization_dir') + if visualization_dir: + vis_dir = Path(visualization_dir) + vis_dir.mkdir(parents=True, exist_ok=True) + + # 只保存ROI点云文件 + source_ply_path = vis_dir / f"{self.name}_source_roi.ply" + target_ply_path = vis_dir / f"{self.name}_target_roi.ply" + Visualizer.save_roi_pointcloud(source_points, str(source_ply_path)) + Visualizer.save_roi_pointcloud(target_points, str(target_ply_path)) + + return MeasurementResult( + task_name=self.name, + task_type=self.task_type, + success=True, + data={ + 'mean_distance': float(mean_distance), + 'std_distance': float(std_distance), + 'min_distance': float(min_distance), + 'max_distance': float(max_distance), + 'sample_count': len(sampled_points), + 'source_roi_points': len(source_points), + 'target_roi_points': len(target_points), + 'source_inlier_ratio': float(source_plane.inlier_ratio), + 'target_inlier_ratio': float(target_plane.inlier_ratio), + 'source_plane_normal': source_plane.normal.tolist(), + 'target_plane_normal': target_plane.normal.tolist() + } + ) + + except Exception as e: + import traceback + return MeasurementResult( + task_name=self.name, + task_type=self.task_type, + success=False, + error_message=f"执行过程中发生错误: {str(e)}\n{traceback.format_exc()}" + ) diff --git a/alg/点云测距v1.1/train_measurement_system/src/measurements/point_to_plane.py b/alg/点云测距v1.1/train_measurement_system/src/measurements/point_to_plane.py new file mode 100644 index 0000000..57d577f --- /dev/null +++ b/alg/点云测距v1.1/train_measurement_system/src/measurements/point_to_plane.py @@ -0,0 +1,207 @@ +""" +点到平面距离测量模块 + +支持两种模式： +1. exact_points: 在点云中查找给定坐标的最近点，计算到平面的距离 +2. bbox_filtered: 从包围盒提取点云，筛选特定条件的点，计算到平面的距离 +""" + +import numpy as np +import open3d as o3d +import sys +from pathlib import Path +from typing import Dict, Any, List, Tuple +from scipy.spatial import KDTree + +# 添加父目录到路径 +sys.path.insert(0, str(Path(__file__).parent.parent)) + +from measurements.base import BaseMeasurement, MeasurementResult +from visualizer import Visualizer + + +class PointToPlaneDistance(BaseMeasurement): + """点到平面距离测量""" + + def __init__(self, name: str, point_cloud: np.ndarray, config: Dict[str, Any]): + super().__init__(name, point_cloud, config) + self.mode = config.get('mode', 'exact_points') + + if self.mode == 'exact_points': + self.target_points = np.array(config['target_points']) + self.search_radius = config.get('search_radius', 10.0) + elif self.mode == 'bbox_filtered': + self.bboxes = config['bboxes'] + self.filter_config = config['filter'] + else: + raise ValueError(f"Unknown mode: {self.mode}") + + # 平面参数 (ax + by + cz + d = 0) + plane_config = config['plane'] + self.plane_normal = np.array([ + plane_config.get('a', 0), + plane_config.get('b', 0), + plane_config.get('c', 1) + ]) + self.plane_d = plane_config.get('d', 0) + + # 归一化法向量 + norm = np.linalg.norm(self.plane_normal) + if norm > 0: + self.plane_normal = self.plane_normal / norm + self.plane_d = self.plane_d / norm + + def execute(self) -> MeasurementResult: + """执行测量""" + try: + if self.mode == 'exact_points': + return self._measure_exact_points() + elif self.mode == 'bbox_filtered': + return self._measure_bbox_filtered() + except Exception as e: + return MeasurementResult( + task_name=self.name, + task_type=self.task_type, + success=False, + error_message=str(e) + ) + + def _measure_exact_points(self) -> MeasurementResult: + """精确点模式：查找最近点并计算距离""" + # 构建KDTree + kdtree = KDTree(self.point_cloud) + + found_points = [] + distances_to_plane = [] + + for target_point in self.target_points: + # 查找最近点 + dist, idx = kdtree.query(target_point, k=1) + + if dist > self.search_radius: + print(f"警告: 目标点 {target_point} 的最近点距离 {dist:.2f}mm 超过搜索半径 {self.search_radius}mm") + + nearest_point = self.point_cloud[idx] + found_points.append(nearest_point) + + # 计算点到平面的距离 + distance = self._point_to_plane_distance(nearest_point) + distances_to_plane.append(distance) + + distances_array = np.array(distances_to_plane) + found_points_array = np.array(found_points) + + # 保存找到的点为点云文件 + visualization_dir = self.config.get('visualization_dir') + if visualization_dir: + vis_dir = Path(visualization_dir) + vis_dir.mkdir(parents=True, exist_ok=True) + ply_path = vis_dir / f"{self.name}_found_points.ply" + Visualizer.save_roi_pointcloud(found_points_array, str(ply_path)) + + return MeasurementResult( + task_name=self.name, + task_type=self.task_type, + success=True, + data={ + 'mean_distance': float(np.mean(np.abs(distances_array))), + 'std_distance': float(np.std(distances_array)), + 'min_distance': float(np.min(distances_array)), + 'max_distance': float(np.max(distances_array)), + 'point_count': len(distances_array), + 'mode': 'exact_points', + 'target_points': self.target_points.tolist(), + 'found_points': [p.tolist() for p in found_points], + 'distances': distances_array.tolist() + } + ) + + def _measure_bbox_filtered(self) -> MeasurementResult: + """包围盒过滤模式：提取点云，筛选特定点，计算距离""" + all_filtered_points = [] + all_distances = [] + + for bbox_config in self.bboxes: + bbox_min = np.array(bbox_config['min']) + bbox_max = np.array(bbox_config['max']) + + # 提取包围盒内的点 + mask = np.all((self.point_cloud >= bbox_min) & (self.point_cloud <= bbox_max), axis=1) + bbox_points = self.point_cloud[mask] + + if len(bbox_points) == 0: + print(f"警告: 包围盒 {bbox_min} 到 {bbox_max} 内没有点") + continue + + # 应用过滤条件 + filtered_points = self._apply_filter(bbox_points) + + if len(filtered_points) == 0: + print(f"警告: 包围盒 {bbox_min} 到 {bbox_max} 过滤后没有点") + continue + + all_filtered_points.extend(filtered_points) + + # 计算每个点到平面的距离 + for point in filtered_points: + distance = self._point_to_plane_distance(point) + all_distances.append(distance) + + if len(all_distances) == 0: + raise ValueError("没有找到符合条件的点") + + distances_array = np.array(all_distances) + filtered_points_array = np.array(all_filtered_points) + + # 保存筛选后的点为点云文件 + visualization_dir = self.config.get('visualization_dir') + if visualization_dir: + vis_dir = Path(visualization_dir) + vis_dir.mkdir(parents=True, exist_ok=True) + ply_path = vis_dir / f"{self.name}_filtered_points.ply" + Visualizer.save_roi_pointcloud(filtered_points_array, str(ply_path)) + + return MeasurementResult( + task_name=self.name, + task_type=self.task_type, + success=True, + data={ + 'mean_distance': float(np.mean(np.abs(distances_array))), + 'std_distance': float(np.std(distances_array)), + 'min_distance': float(np.min(distances_array)), + 'max_distance': float(np.max(distances_array)), + 'point_count': len(distances_array), + 'mode': 'bbox_filtered', + 'num_bboxes': len(self.bboxes), + 'filtered_points': all_filtered_points, + 'distances': distances_array.tolist() + } + ) + + def _apply_filter(self, points: np.ndarray) -> List[np.ndarray]: + """应用过滤条件""" + filter_type = self.filter_config['type'] + + if filter_type == 'max_abs_coordinate': + axis = self.filter_config['axis'] + top_n = self.filter_config['top_n'] + + axis_map = {'x': 0, 'y': 1, 'z': 2} + axis_idx = axis_map[axis.lower()] + + # 计算绝对值 + abs_values = np.abs(points[:, axis_idx]) + + # 获取top_n个最大值的索引 + if len(points) <= top_n: + return points.tolist() + + top_indices = np.argpartition(abs_values, -top_n)[-top_n:] + return points[top_indices].tolist() + + else: + raise ValueError(f"Unknown filter type: {filter_type}") + + def _point_to_plane_distance(self, point: np.ndarray) -> float: + """计算点到平面的有符号距离""" + return np.dot(self.plane_normal, point) + self.plane_d diff --git a/alg/点云测距v1.1/train_measurement_system/src/measurements/point_to_point.py b/alg/点云测距v1.1/train_measurement_system/src/measurements/point_to_point.py new file mode 100644 index 0000000..9fa0ea9 --- /dev/null +++ b/alg/点云测距v1.1/train_measurement_system/src/measurements/point_to_point.py @@ -0,0 +1,113 @@ +""" +点到点距离测量模块 + +用于测量点云中两个指定点之间的距离 +""" + +import numpy as np +import sys +from pathlib import Path +from typing import Dict, Any, List, Tuple +from scipy.spatial import KDTree + +# 添加父目录到路径 +sys.path.insert(0, str(Path(__file__).parent.parent)) + +from measurements.base import BaseMeasurement, MeasurementResult +from visualizer import Visualizer + + +class PointToPointDistance(BaseMeasurement): + """点到点距离测量""" + + def __init__(self, name: str, point_cloud: np.ndarray, config: Dict[str, Any]): + super().__init__(name, point_cloud, config) + + # 点对列表，每对包含两个点的坐标 + self.point_pairs = config['point_pairs'] + self.search_radius = config.get('search_radius', 10.0) + + def execute(self) -> MeasurementResult: + """执行测量""" + try: + # 构建KDTree用于快速查找最近点 + kdtree = KDTree(self.point_cloud) + + pair_results = [] + all_found_points = [] + + for i, pair in enumerate(self.point_pairs): + point1_target = np.array(pair['point1']) + point2_target = np.array(pair['point2']) + pair_name = pair.get('name', f'点对{i+1}') + + # 查找point1的最近点 + dist1, idx1 = kdtree.query(point1_target, k=1) + if dist1 > self.search_radius: + print(f"警告: {pair_name} - 点1 {point1_target} 的最近点距离 {dist1:.2f}mm 超过搜索半径 {self.search_radius}mm") + + point1_found = self.point_cloud[idx1] + + # 查找point2的最近点 + dist2, idx2 = kdtree.query(point2_target, k=1) + if dist2 > self.search_radius: + print(f"警告: {pair_name} - 点2 {point2_target} 的最近点距离 {dist2:.2f}mm 超过搜索半径 {self.search_radius}mm") + + point2_found = self.point_cloud[idx2] + + # 计算两点之间的欧氏距离 + distance = np.linalg.norm(point2_found - point1_found) + + pair_results.append({ + 'name': pair_name, + 'point1_target': point1_target.tolist(), + 'point2_target': point2_target.tolist(), + 'point1_found': point1_found.tolist(), + 'point2_found': point2_found.tolist(), + 'point1_search_distance': float(dist1), + 'point2_search_distance': float(dist2), + 'distance': float(distance) + }) + + all_found_points.extend([point1_found, point2_found]) + + # 统计所有距离 + distances = np.array([pr['distance'] for pr in pair_results]) + mean_distance = float(np.mean(distances)) + std_distance = float(np.std(distances)) + min_distance = float(np.min(distances)) + max_distance = float(np.max(distances)) + + # 保存找到的点为点云文件 + visualization_dir = self.config.get('visualization_dir') + if visualization_dir: + vis_dir = Path(visualization_dir) + vis_dir.mkdir(parents=True, exist_ok=True) + + # 保存所有找到的点 + all_points_array = np.array(all_found_points) + ply_path = vis_dir / f"{self.name}_found_points.ply" + Visualizer.save_roi_pointcloud(all_points_array, str(ply_path)) + + return MeasurementResult( + task_name=self.name, + task_type=self.task_type, + success=True, + data={ + 'mean_distance': mean_distance, + 'std_distance': std_distance, + 'min_distance': min_distance, + 'max_distance': max_distance, + 'pair_count': len(pair_results), + 'pair_results': pair_results + } + ) + + except Exception as e: + import traceback + return MeasurementResult( + task_name=self.name, + task_type=self.task_type, + success=False, + error_message=f"执行过程中发生错误: {str(e)}\n{traceback.format_exc()}" + ) diff --git a/alg/点云测距v1.1/train_measurement_system/src/plane_fitter.py b/alg/点云测距v1.1/train_measurement_system/src/plane_fitter.py new file mode 100644 index 0000000..adb397c --- /dev/null +++ b/alg/点云测距v1.1/train_measurement_system/src/plane_fitter.py @@ -0,0 +1,169 @@ +"""平面拟合模块 - 使用RANSAC算法拟合平面""" +import numpy as np +from dataclasses import dataclass +from typing import Tuple, Optional + + +@dataclass +class PlaneModel: + """平面模型""" + normal: np.ndarray # 法向量 (3,) + point: np.ndarray # 平面上的点 (3,) + d: float # 平面方程 ax+by+cz+d=0 中的d + inliers: np.ndarray # 内点索引 + inlier_ratio: float # 内点比例 + + def distance_to_point(self, point: np.ndarray) -> float: + """计算点到平面的有向距离""" + return np.dot(self.normal, point) + self.d + + def distance_to_points(self, points: np.ndarray) -> np.ndarray: + """计算多个点到平面的有向距离""" + return np.dot(points, self.normal) + self.d + + +class PlaneFitter: + """RANSAC平面拟合器""" + + def __init__( + self, + threshold: float = 2.0, + max_iterations: int = 1000, + min_inlier_ratio: float = 0.6 + ): + """ + Args: + threshold: 内点距离阈值(mm) + max_iterations: 最大迭代次数 + min_inlier_ratio: 最小内点比例 + """ + self.threshold = threshold + self.max_iterations = max_iterations + self.min_inlier_ratio = min_inlier_ratio + + def fit(self, points: np.ndarray) -> Optional[PlaneModel]: + """ + 使用RANSAC拟合平面 + + Args: + points: 点云数组 (N, 3) + + Returns: + PlaneModel或None（拟合失败） + """ + if len(points) < 3: + return None + + best_inliers = None + best_model = None + best_inlier_count = 0 + + for _ in range(self.max_iterations): + # 随机选择3个点 + sample_indices = np.random.choice(len(points), 3, replace=False) + sample_points = points[sample_indices] + + # 计算平面参数 + model = self._fit_plane_from_points(sample_points) + if model is None: + continue + + # 计算所有点到平面的距离 + distances = np.abs(model.distance_to_points(points)) + + # 找到内点 + inliers = np.where(distances < self.threshold)[0] + + if len(inliers) > best_inlier_count: + best_inlier_count = len(inliers) + best_inliers = inliers + best_model = model + + if best_inliers is None or len(best_inliers) < 3: + return None + + # 使用所有内点重新拟合平面 + inlier_points = points[best_inliers] + final_model = self._fit_plane_least_squares(inlier_points) + + if final_model is None: + return None + + final_model.inliers = best_inliers + final_model.inlier_ratio = len(best_inliers) / len(points) + + # 检查内点比例 + if final_model.inlier_ratio < self.min_inlier_ratio: + return None + + return final_model + + def _fit_plane_from_points(self, points: np.ndarray) -> Optional[PlaneModel]: + """从3个点拟合平面""" + if len(points) != 3: + return None + + # 计算两个向量 + v1 = points[1] - points[0] + v2 = points[2] - points[0] + + # 法向量 = v1 × v2 + normal = np.cross(v1, v2) + norm = np.linalg.norm(normal) + + if norm < 1e-6: + return None + + normal = normal / norm + + # 平面方程: n·(p - p0) = 0 => n·p = n·p0 => ax+by+cz+d=0, d=-n·p0 + d = -np.dot(normal, points[0]) + + return PlaneModel( + normal=normal, + point=points[0], + d=d, + inliers=np.array([]), + inlier_ratio=0.0 + ) + + def _fit_plane_least_squares(self, points: np.ndarray) -> Optional[PlaneModel]: + """使用最小二乘法拟合平面""" + if len(points) < 3: + return None + + # 如果点数过多，降采样以避免内存爆炸 + # SVD需要O(n^2)内存，超过10000个点会导致内存问题 + max_points_for_svd = 10000 + if len(points) > max_points_for_svd: + indices = np.random.choice(len(points), max_points_for_svd, replace=False) + points_for_fit = points[indices] + else: + points_for_fit = points + + # 计算质心 + centroid = np.mean(points_for_fit, axis=0) + + # 中心化点云 + centered = points_for_fit - centroid + + # SVD分解 + try: + _, _, vh = np.linalg.svd(centered) + normal = vh[2] # 最小奇异值对应的向量 + + # 确保法向量指向正方向（可选） + if normal[2] < 0: + normal = -normal + + d = -np.dot(normal, centroid) + + return PlaneModel( + normal=normal, + point=centroid, + d=d, + inliers=np.array([]), + inlier_ratio=0.0 + ) + except np.linalg.LinAlgError: + return None diff --git a/alg/点云测距v1.1/train_measurement_system/src/pointcloud_loader.py b/alg/点云测距v1.1/train_measurement_system/src/pointcloud_loader.py new file mode 100644 index 0000000..cb85bd6 --- /dev/null +++ b/alg/点云测距v1.1/train_measurement_system/src/pointcloud_loader.py @@ -0,0 +1,103 @@ +"""点云加载模块""" +import numpy as np +from pathlib import Path +import struct + + +class PointCloudLoader: + """加载点云文件""" + + @staticmethod + def load_ply(file_path: str) -> np.ndarray: + """ + 加载PLY格式点云 + + Args: + file_path: PLY文件路径 + + Returns: + np.ndarray: 点云数组 (N, 3) + """ + path = Path(file_path) + if not path.exists(): + raise FileNotFoundError(f"点云文件不存在: {file_path}") + + print(f"正在加载点云: {file_path}") + + with open(path, 'rb') as f: + # 读取头部 + header_lines = [] + while True: + line = f.readline().decode('ascii').strip() + header_lines.append(line) + if line == 'end_header': + break + + # 解析头部 + vertex_count = 0 + format_type = None + properties = [] + + for line in header_lines: + if line.startswith('element vertex'): + vertex_count = int(line.split()[-1]) + elif line.startswith('format'): + format_type = line.split()[1] + elif line.startswith('property'): + parts = line.split() + properties.append({ + 'type': parts[1], + 'name': parts[2] + }) + + if vertex_count == 0: + raise ValueError("PLY文件中没有顶点数据") + + # 读取顶点数据 + if format_type == 'binary_little_endian': + points = PointCloudLoader._read_binary_ply(f, vertex_count, properties) + elif format_type == 'ascii': + points = PointCloudLoader._read_ascii_ply(f, vertex_count) + else: + raise ValueError(f"不支持的PLY格式: {format_type}") + + print(f" 加载完成: {len(points)} 个点") + return points + + @staticmethod + def _read_binary_ply(f, vertex_count: int, properties: list) -> np.ndarray: + """读取二进制PLY数据""" + # 构建struct格式字符串 + type_map = { + 'float': 'f', + 'double': 'd', + 'uchar': 'B', + 'uint': 'I', + 'int': 'i' + } + + fmt = '<' # 小端序 + for prop in properties: + fmt += type_map.get(prop['type'], 'f') + + struct_size = struct.calcsize(fmt) + points = [] + + for _ in range(vertex_count): + data = struct.unpack(fmt, f.read(struct_size)) + # 只取前3个值作为xyz坐标 + points.append(data[:3]) + + return np.array(points, dtype=np.float64) + + @staticmethod + def _read_ascii_ply(f, vertex_count: int) -> np.ndarray: + """读取ASCII PLY数据""" + points = [] + for _ in range(vertex_count): + line = f.readline().decode('ascii').strip() + values = line.split() + # 只取前3个值作为xyz坐标 + points.append([float(values[0]), float(values[1]), float(values[2])]) + + return np.array(points, dtype=np.float64) diff --git a/alg/点云测距v1.1/train_measurement_system/src/roi_extractor.py b/alg/点云测距v1.1/train_measurement_system/src/roi_extractor.py new file mode 100644 index 0000000..b4bd02b --- /dev/null +++ b/alg/点云测距v1.1/train_measurement_system/src/roi_extractor.py @@ -0,0 +1,59 @@ +"""ROI提取模块 - 从点云中提取包围盒区域的点""" +import numpy as np +from typing import Tuple, Optional + + +class ROIExtractor: + """从点云中提取指定包围盒区域的点""" + + @staticmethod + def extract_bbox_points( + points: np.ndarray, + bbox_min: list, + bbox_max: list + ) -> Tuple[np.ndarray, np.ndarray]: + """ + 提取包围盒内的点 + + Args: + points: 点云数组 (N, 3) + bbox_min: 包围盒最小坐标 [x_min, y_min, z_min] + bbox_max: 包围盒最大坐标 [x_max, y_max, z_max] + + Returns: + roi_points: 提取的点 (M, 3) + indices: 原始点云中的索引 (M,) + """ + bbox_min = np.array(bbox_min) + bbox_max = np.array(bbox_max) + + # 找到在包围盒内的点 + mask = np.all((points >= bbox_min) & (points <= bbox_max), axis=1) + indices = np.where(mask)[0] + roi_points = points[indices] + + return roi_points, indices + + @staticmethod + def sample_points(points: np.ndarray, n_samples: int, seed: Optional[int] = 42) -> np.ndarray: + """ + 从点云中随机采样点 + + Args: + points: 点云数组 (N, 3) + n_samples: 采样点数 + seed: 随机种子 + + Returns: + sampled_points: 采样后的点 (n_samples, 3) + """ + if len(points) == 0: + return np.array([]).reshape(0, 3) + + n_samples = min(n_samples, len(points)) + + if seed is not None: + np.random.seed(seed) + + indices = np.random.choice(len(points), size=n_samples, replace=False) + return points[indices] diff --git a/alg/点云测距v1.1/train_measurement_system/src/task_manager.py b/alg/点云测距v1.1/train_measurement_system/src/task_manager.py new file mode 100644 index 0000000..ce86632 --- /dev/null +++ b/alg/点云测距v1.1/train_measurement_system/src/task_manager.py @@ -0,0 +1,282 @@ +"""任务管理器 - 负责加载配置、调度任务、汇总结果""" +import yaml +import numpy as np +from pathlib import Path +from typing import List, Dict, Any +from datetime import datetime + +from pointcloud_loader import PointCloudLoader +from measurements.base import MeasurementResult +from measurements.plane_distance import PlaneToPlaneDistance +from measurements.point_to_plane import PointToPlaneDistance +from measurements.point_to_point import PointToPointDistance +from excel_exporter import ExcelExporter + + +class TaskManager: + """测量任务管理器""" + + def __init__(self): + """初始化任务管理器""" + self.config = None + self.point_cloud = None + self.tasks = [] + self.results = [] + self.pointcloud_path_override = None + self.output_dir_override = None + + def load_config(self, config_path: str) -> bool: + """ + 加载配置文件 + + Args: + config_path: 配置文件路径 + + Returns: + bool: 是否加载成功 + """ + try: + with open(config_path, 'r', encoding='utf-8') as f: + self.config = yaml.safe_load(f) + + # 加载点云（支持相对路径） + pointcloud_path = self.pointcloud_path_override or self.config.get('pointcloud_file') + if not pointcloud_path: + print("错误: 配置文件中未指定点云文件路径") + return False + + # 处理相对路径 + pointcloud_path = self._resolve_path(pointcloud_path, config_path) + + print(f"\n正在加载点云文件: {pointcloud_path}") + self.point_cloud = PointCloudLoader.load_ply(pointcloud_path) + print(f"点云加载完成: {len(self.point_cloud)} 个点") + + # 解析任务 + tasks_config = self.config.get('tasks', []) + if not tasks_config: + print("警告: 配置文件中没有定义任务") + return True + + print(f"\n找到 {len(tasks_config)} 个测量任务") + + for task_config in tasks_config: + task = self._create_task(task_config) + if task: + self.tasks.append(task) + print(f" - {task.name} ({task.task_type})") + + return True + + except FileNotFoundError: + print(f"错误: 配置文件不存在: {config_path}") + return False + except yaml.YAMLError as e: + print(f"错误: 配置文件格式错误: {e}") + return False + except Exception as e: + print(f"错误: 加载配置失败: {e}") + return False + + def _resolve_path(self, path: str, config_path: str) -> str: + """ + 解析路径（支持相对路径和绝对路径） + + Args: + path: 待解析的路径 + config_path: 配置文件路径（用于相对路径的基准） + + Returns: + str: 解析后的绝对路径 + """ + path_obj = Path(path) + if path_obj.is_absolute(): + return str(path_obj) + else: + # 相对于配置文件所在目录 + config_dir = Path(config_path).parent + return str(config_dir / path_obj) + + def override_pointcloud_path(self, path: str): + """ + 覆盖点云文件路径（用于命令行参数） + + Args: + path: 点云文件路径 + """ + self.pointcloud_path_override = path + + def override_output_dir(self, output_dir: str): + """ + 覆盖输出目录（用于命令行参数） + + Args: + output_dir: 输出目录路径 + """ + self.output_dir_override = output_dir + + def _create_task(self, task_config: Dict[str, Any]): + """ + 根据配置创建测量任务 + + Args: + task_config: 任务配置字典 + + Returns: + BaseMeasurement: 测量任务实例 + """ + task_type = task_config.get('type') + task_name = task_config.get('name', 'Unnamed Task') + + if task_type == 'plane_to_plane': + return PlaneToPlaneDistance( + name=task_name, + point_cloud=self.point_cloud, + config=task_config + ) + elif task_type == 'point_to_plane': + return PointToPlaneDistance( + name=task_name, + point_cloud=self.point_cloud, + config=task_config + ) + elif task_type == 'point_to_point': + return PointToPointDistance( + name=task_name, + point_cloud=self.point_cloud, + config=task_config + ) + else: + print(f"警告: 未知的任务类型: {task_type}") + return None + + def run_all_tasks(self) -> List[Dict[str, Any]]: + """ + 执行所有测量任务 + + Returns: + List[Dict]: 任务结果列表 + """ + self.results = [] + + for i, task in enumerate(self.tasks, 1): + print(f"\n[{i}/{len(self.tasks)}] 执行任务: {task.name}") + print("-" * 60) + + # 执行任务 + result = task.execute() + + # 记录结果 + result_dict = { + 'name': result.task_name, + 'type': result.task_type, + 'status': 'success' if result.success else 'failed', + 'data': result.data if result.success else None, + 'error': result.error_message if not result.success else None + } + self.results.append(result_dict) + + # 打印结果 + if result.success: + print(f"[成功] 任务完成") + if result.data: + print(f" 平均距离: {result.data.get('mean_distance', 0):.2f} mm") + print(f" 标准差: {result.data.get('std_distance', 0):.2f} mm") + print(f" 范围: [{result.data.get('min_distance', 0):.2f}, {result.data.get('max_distance', 0):.2f}] mm") + print(f" 测量点数: {result.data.get('point_count', result.data.get('sample_count', 0))}") + if 'source_inlier_ratio' in result.data: + print(f" 源平面内点率: {result.data.get('source_inlier_ratio', 0):.1%}") + if 'target_inlier_ratio' in result.data: + print(f" 目标平面内点率: {result.data.get('target_inlier_ratio', 0):.1%}") + if 'plane_inlier_ratio' in result.data: + print(f" 平面内点率: {result.data.get('plane_inlier_ratio', 0):.1%}") + else: + print(f"[失败] {result.error_message}") + + # 可视化（如果配置中启用） + if result.success and task.config.get('visualization', False): + self._visualize_result(task, result) + + # 导出Excel结果 + self._export_results() + + return self.results + + def _visualize_result(self, task, result: MeasurementResult): + """ + 可视化测量结果 + + Args: + task: 测量任务 + result: 测量结果 + """ + try: + import open3d as o3d + import matplotlib.pyplot as plt + + print(" 生成可视化...") + + data = result.data + if not data: + return + + # 创建可视化窗口 + vis = o3d.visualization.Visualizer() + vis.create_window(window_name=f"测量结果: {task.name}", width=1200, height=800) + + # 显示源平面点云（蓝色） + source_pcd = o3d.geometry.PointCloud() + source_pcd.points = o3d.utility.Vector3dVector(data['_source_points']) + source_pcd.paint_uniform_color([0.2, 0.4, 0.8]) + vis.add_geometry(source_pcd) + + # 显示目标平面点云（绿色） + target_pcd = o3d.geometry.PointCloud() + target_pcd.points = o3d.utility.Vector3dVector(data['_target_points']) + target_pcd.paint_uniform_color([0.2, 0.8, 0.4]) + vis.add_geometry(target_pcd) + + # 显示采样点（红色） + sampled_pcd = o3d.geometry.PointCloud() + sampled_pcd.points = o3d.utility.Vector3dVector(data['_sampled_points']) + sampled_pcd.paint_uniform_color([0.9, 0.2, 0.2]) + vis.add_geometry(sampled_pcd) + + # 运行可视化 + vis.run() + vis.destroy_window() + + except Exception as e: + print(f" 可视化失败: {e}") + + def _export_results(self): + """导出结果到Excel""" + try: + # 转换为MeasurementResult对象列表 + measurement_results = [] + for result_dict in self.results: + measurement_results.append(MeasurementResult( + task_name=result_dict['name'], + task_type=result_dict['type'], + success=result_dict['status'] == 'success', + data=result_dict.get('data'), + error_message=result_dict.get('error') + )) + + # 导出Excel（支持命令行覆盖输出目录） + if self.output_dir_override: + output_dir = Path(self.output_dir_override) + else: + output_dir = Path(__file__).parent.parent / "results" + + exporter = ExcelExporter(str(output_dir)) + + timestamp = datetime.now().strftime("%Y%m%d_%H%M%S") + filename = f"measurement_results_{timestamp}" + + # 传递任务配置信息到导出器 + excel_path = exporter.export(measurement_results, filename, self.config.get('tasks', [])) + print(f"\n结果已导出到: {excel_path}") + + except Exception as e: + print(f"\n导出Excel失败: {e}") diff --git a/alg/点云测距v1.1/train_measurement_system/src/visualizer.py b/alg/点云测距v1.1/train_measurement_system/src/visualizer.py new file mode 100644 index 0000000..2e63ded --- /dev/null +++ b/alg/点云测距v1.1/train_measurement_system/src/visualizer.py @@ -0,0 +1,195 @@ +"""可视化工具 - 用于显示点云、平面、ROI等""" +import numpy as np +import open3d as o3d +from pathlib import Path +from typing import List, Tuple, Optional + + +class Visualizer: + """点云和测量结果可视化工具""" + + @staticmethod + def visualize_roi_in_context( + full_cloud: np.ndarray, + roi_cloud: np.ndarray, + bbox_min: List[float], + bbox_max: List[float], + title: str = "ROI in Context", + downsample_full: int = 10, + save_path: Optional[str] = None + ): + """ + 在完整点云上下文中可视化ROI区域 + + Args: + full_cloud: 完整点云 (N, 3) + roi_cloud: ROI区域点云 (M, 3) + bbox_min: 包围盒最小坐标 [x, y, z] + bbox_max: 包围盒最大坐标 [x, y, z] + title: 窗口标题 + downsample_full: 完整点云降采样倍数（每N个点取1个） + save_path: 保存截图路径（可选） + """ + # 降采样完整点云以提高显示性能 + full_cloud_downsampled = full_cloud[::downsample_full] + + # 创建完整点云对象（灰色） + pcd_full = o3d.geometry.PointCloud() + pcd_full.points = o3d.utility.Vector3dVector(full_cloud_downsampled) + pcd_full.paint_uniform_color([0.7, 0.7, 0.7]) # 灰色 + + # 创建ROI点云对象（红色） + pcd_roi = o3d.geometry.PointCloud() + pcd_roi.points = o3d.utility.Vector3dVector(roi_cloud) + pcd_roi.paint_uniform_color([1.0, 0.0, 0.0]) # 红色 + + # 创建包围盒线框 + bbox = Visualizer._create_bbox_lineset(bbox_min, bbox_max, color=[0, 1, 0]) + + # 可视化 + geometries = [pcd_full, pcd_roi, bbox] + o3d.visualization.draw_geometries( + geometries, + window_name=title, + width=1200, + height=800, + point_show_normal=False + ) + + # 保存截图（如果指定） + if save_path: + vis = o3d.visualization.Visualizer() + vis.create_window(window_name=title, width=1200, height=800, visible=False) + for geom in geometries: + vis.add_geometry(geom) + vis.update_renderer() + vis.capture_screen_image(save_path) + vis.destroy_window() + print(f"截图已保存: {save_path}") + + @staticmethod + def visualize_plane_measurement( + source_cloud: np.ndarray, + target_cloud: np.ndarray, + source_plane_normal: np.ndarray, + source_plane_point: np.ndarray, + target_plane_normal: np.ndarray, + target_plane_point: np.ndarray, + sample_points: np.ndarray, + distances: np.ndarray, + title: str = "Plane Measurement", + save_path: Optional[str] = None + ): + """ + 可视化平面到平面的距离测量 + + Args: + source_cloud: 源平面点云 + target_cloud: 目标平面点云 + source_plane_normal: 源平面法向量 + source_plane_point: 源平面上的点 + target_plane_normal: 目标平面法向量 + target_plane_point: 目标平面上的点 + sample_points: 采样点 + distances: 每个采样点到目标平面的距离 + title: 窗口标题 + save_path: 保存截图路径 + """ + # 创建源平面点云（蓝色） + pcd_source = o3d.geometry.PointCloud() + pcd_source.points = o3d.utility.Vector3dVector(source_cloud) + pcd_source.paint_uniform_color([0.0, 0.5, 1.0]) # 蓝色 + + # 创建目标平面点云（绿色） + pcd_target = o3d.geometry.PointCloud() + pcd_target.points = o3d.utility.Vector3dVector(target_cloud) + pcd_target.paint_uniform_color([0.0, 1.0, 0.0]) # 绿色 + + # 创建采样点（红色） + pcd_samples = o3d.geometry.PointCloud() + pcd_samples.points = o3d.utility.Vector3dVector(sample_points) + pcd_samples.paint_uniform_color([1.0, 0.0, 0.0]) # 红色 + + # 创建距离线（从采样点到目标平面的投影） + lines = [] + for i, (point, dist) in enumerate(zip(sample_points, distances)): + # 计算投影点 + projection = point - dist * target_plane_normal + lines.append([point, projection]) + + # 创建线集 + line_points = [] + line_indices = [] + for i, (start, end) in enumerate(lines): + line_points.append(start) + line_points.append(end) + line_indices.append([2*i, 2*i+1]) + + lineset = o3d.geometry.LineSet() + lineset.points = o3d.utility.Vector3dVector(line_points) + lineset.lines = o3d.utility.Vector2iVector(line_indices) + lineset.paint_uniform_color([1.0, 0.5, 0.0]) # 橙色 + + # 可视化 + geometries = [pcd_source, pcd_target, pcd_samples, lineset] + o3d.visualization.draw_geometries( + geometries, + window_name=title, + width=1200, + height=800, + point_show_normal=False + ) + + if save_path: + vis = o3d.visualization.Visualizer() + vis.create_window(window_name=title, width=1200, height=800, visible=False) + for geom in geometries: + vis.add_geometry(geom) + vis.update_renderer() + vis.capture_screen_image(save_path) + vis.destroy_window() + print(f"截图已保存: {save_path}") + + @staticmethod + def _create_bbox_lineset( + bbox_min: List[float], + bbox_max: List[float], + color: List[float] = [0, 1, 0] + ) -> o3d.geometry.LineSet: + """创建包围盒线框""" + # 8个顶点 + points = [ + [bbox_min[0], bbox_min[1], bbox_min[2]], # 0 + [bbox_max[0], bbox_min[1], bbox_min[2]], # 1 + [bbox_max[0], bbox_max[1], bbox_min[2]], # 2 + [bbox_min[0], bbox_max[1], bbox_min[2]], # 3 + [bbox_min[0], bbox_min[1], bbox_max[2]], # 4 + [bbox_max[0], bbox_min[1], bbox_max[2]], # 5 + [bbox_max[0], bbox_max[1], bbox_max[2]], # 6 + [bbox_min[0], bbox_max[1], bbox_max[2]], # 7 + ] + + # 12条边 + lines = [ + [0, 1], [1, 2], [2, 3], [3, 0], # 底面 + [4, 5], [5, 6], [6, 7], [7, 4], # 顶面 + [0, 4], [1, 5], [2, 6], [3, 7], # 竖边 + ] + + lineset = o3d.geometry.LineSet() + lineset.points = o3d.utility.Vector3dVector(points) + lineset.lines = o3d.utility.Vector2iVector(lines) + lineset.paint_uniform_color(color) + + return lineset + + @staticmethod + def save_roi_pointcloud( + roi_cloud: np.ndarray, + save_path: str + ): + """保存ROI点云为PLY文件""" + pcd = o3d.geometry.PointCloud() + pcd.points = o3d.utility.Vector3dVector(roi_cloud) + o3d.io.write_point_cloud(save_path, pcd) + print(f"ROI点云已保存: {save_path}") diff --git a/client/index.html b/client/index.html new file mode 100644 index 0000000..58ddc9c --- /dev/null +++ b/client/index.html @@ -0,0 +1,22 @@ + + + + + + + 宏量点云处理软件 + + + + +
+ + + + diff --git a/client/public/.gitkeep b/client/public/.gitkeep new file mode 100644 index 0000000..e69de29 diff --git a/client/public/__manus__/debug-collector.js b/client/public/__manus__/debug-collector.js new file mode 100644 index 0000000..0504555 --- /dev/null +++ b/client/public/__manus__/debug-collector.js @@ -0,0 +1,821 @@ +/** + * Manus Debug Collector (agent-friendly) + * + * Captures: + * 1) Console logs + * 2) Network requests (fetch + XHR) + * 3) User interactions (semantic uiEvents: click/type/submit/nav/scroll/etc.) + * + * Data is periodically sent to /__manus__/logs + * Note: uiEvents are mirrored to sessionEvents for sessionReplay.log + */ +(function () { + "use strict"; + + // Prevent double initialization + if (window.__MANUS_DEBUG_COLLECTOR__) return; + + // ========================================================================== + // Configuration + // ========================================================================== + const CONFIG = { + reportEndpoint: "/__manus__/logs", + bufferSize: { + console: 500, + network: 200, + // semantic, agent-friendly UI events + ui: 500, + }, + reportInterval: 2000, + sensitiveFields: [ + "password", + "token", + "secret", + "key", + "authorization", + "cookie", + "session", + ], + maxBodyLength: 10240, + // UI event logging privacy policy: + // - inputs matching sensitiveFields or type=password are masked by default + // - non-sensitive inputs log up to 200 chars + uiInputMaxLen: 200, + uiTextMaxLen: 80, + // Scroll throttling: minimum ms between scroll events + scrollThrottleMs: 500, + }; + + // ========================================================================== + // Storage + // ========================================================================== + const store = { + consoleLogs: [], + networkRequests: [], + uiEvents: [], + lastReportTime: Date.now(), + lastScrollTime: 0, + }; + + // ========================================================================== + // Utility Functions + // ========================================================================== + + function sanitizeValue(value, depth) { + if (depth === void 0) depth = 0; + if (depth > 5) return "[Max Depth]"; + if (value === null) return null; + if (value === undefined) return undefined; + + if (typeof value === "string") { + return value.length > 1000 ? value.slice(0, 1000) + "...[truncated]" : value; + } + + if (typeof value !== "object") return value; + + if (Array.isArray(value)) { + return value.slice(0, 100).map(function (v) { + return sanitizeValue(v, depth + 1); + }); + } + + var sanitized = {}; + for (var k in value) { + if (Object.prototype.hasOwnProperty.call(value, k)) { + var isSensitive = CONFIG.sensitiveFields.some(function (f) { + return k.toLowerCase().indexOf(f) !== -1; + }); + if (isSensitive) { + sanitized[k] = "[REDACTED]"; + } else { + sanitized[k] = sanitizeValue(value[k], depth + 1); + } + } + } + return sanitized; + } + + function formatArg(arg) { + try { + if (arg instanceof Error) { + return { type: "Error", message: arg.message, stack: arg.stack }; + } + if (typeof arg === "object") return sanitizeValue(arg); + return String(arg); + } catch (e) { + return "[Unserializable]"; + } + } + + function formatArgs(args) { + var result = []; + for (var i = 0; i < args.length; i++) result.push(formatArg(args[i])); + return result; + } + + function pruneBuffer(buffer, maxSize) { + if (buffer.length > maxSize) buffer.splice(0, buffer.length - maxSize); + } + + function tryParseJson(str) { + if (typeof str !== "string") return str; + try { + return JSON.parse(str); + } catch (e) { + return str; + } + } + + // ========================================================================== + // Semantic UI Event Logging (agent-friendly) + // ========================================================================== + + function shouldIgnoreTarget(target) { + try { + if (!target || !(target instanceof Element)) return false; + return !!target.closest(".manus-no-record"); + } catch (e) { + return false; + } + } + + function compactText(s, maxLen) { + try { + var t = (s || "").trim().replace(/\s+/g, " "); + if (!t) return ""; + return t.length > maxLen ? t.slice(0, maxLen) + "…" : t; + } catch (e) { + return ""; + } + } + + function elText(el) { + try { + var t = el.innerText || el.textContent || ""; + return compactText(t, CONFIG.uiTextMaxLen); + } catch (e) { + return ""; + } + } + + function describeElement(el) { + if (!el || !(el instanceof Element)) return null; + + var getAttr = function (name) { + return el.getAttribute(name); + }; + + var tag = el.tagName ? el.tagName.toLowerCase() : null; + var id = el.id || null; + var name = getAttr("name") || null; + var role = getAttr("role") || null; + var ariaLabel = getAttr("aria-label") || null; + + var dataLoc = getAttr("data-loc") || null; + var testId = + getAttr("data-testid") || + getAttr("data-test-id") || + getAttr("data-test") || + null; + + var type = tag === "input" ? (getAttr("type") || "text") : null; + var href = tag === "a" ? getAttr("href") || null : null; + + // a small, stable hint for agents (avoid building full CSS paths) + var selectorHint = null; + if (testId) selectorHint = '[data-testid="' + testId + '"]'; + else if (dataLoc) selectorHint = '[data-loc="' + dataLoc + '"]'; + else if (id) selectorHint = "#" + id; + else selectorHint = tag || "unknown"; + + return { + tag: tag, + id: id, + name: name, + type: type, + role: role, + ariaLabel: ariaLabel, + testId: testId, + dataLoc: dataLoc, + href: href, + text: elText(el), + selectorHint: selectorHint, + }; + } + + function isSensitiveField(el) { + if (!el || !(el instanceof Element)) return false; + var tag = el.tagName ? el.tagName.toLowerCase() : ""; + if (tag !== "input" && tag !== "textarea") return false; + + var type = (el.getAttribute("type") || "").toLowerCase(); + if (type === "password") return true; + + var name = (el.getAttribute("name") || "").toLowerCase(); + var id = (el.id || "").toLowerCase(); + + return CONFIG.sensitiveFields.some(function (f) { + return name.indexOf(f) !== -1 || id.indexOf(f) !== -1; + }); + } + + function getInputValueSafe(el) { + if (!el || !(el instanceof Element)) return null; + var tag = el.tagName ? el.tagName.toLowerCase() : ""; + if (tag !== "input" && tag !== "textarea" && tag !== "select") return null; + + var v = ""; + try { + v = el.value != null ? String(el.value) : ""; + } catch (e) { + v = ""; + } + + if (isSensitiveField(el)) return { masked: true, length: v.length }; + + if (v.length > CONFIG.uiInputMaxLen) v = v.slice(0, CONFIG.uiInputMaxLen) + "…"; + return v; + } + + function logUiEvent(kind, payload) { + var entry = { + timestamp: Date.now(), + kind: kind, + url: location.href, + viewport: { width: window.innerWidth, height: window.innerHeight }, + payload: sanitizeValue(payload), + }; + store.uiEvents.push(entry); + pruneBuffer(store.uiEvents, CONFIG.bufferSize.ui); + } + + function installUiEventListeners() { + // Clicks + document.addEventListener( + "click", + function (e) { + var t = e.target; + if (shouldIgnoreTarget(t)) return; + logUiEvent("click", { + target: describeElement(t), + x: e.clientX, + y: e.clientY, + }); + }, + true + ); + + // Typing "commit" events + document.addEventListener( + "change", + function (e) { + var t = e.target; + if (shouldIgnoreTarget(t)) return; + logUiEvent("change", { + target: describeElement(t), + value: getInputValueSafe(t), + }); + }, + true + ); + + document.addEventListener( + "focusin", + function (e) { + var t = e.target; + if (shouldIgnoreTarget(t)) return; + logUiEvent("focusin", { target: describeElement(t) }); + }, + true + ); + + document.addEventListener( + "focusout", + function (e) { + var t = e.target; + if (shouldIgnoreTarget(t)) return; + logUiEvent("focusout", { + target: describeElement(t), + value: getInputValueSafe(t), + }); + }, + true + ); + + // Enter/Escape are useful for form flows & modals + document.addEventListener( + "keydown", + function (e) { + if (e.key !== "Enter" && e.key !== "Escape") return; + var t = e.target; + if (shouldIgnoreTarget(t)) return; + logUiEvent("keydown", { key: e.key, target: describeElement(t) }); + }, + true + ); + + // Form submissions + document.addEventListener( + "submit", + function (e) { + var t = e.target; + if (shouldIgnoreTarget(t)) return; + logUiEvent("submit", { target: describeElement(t) }); + }, + true + ); + + // Throttled scroll events + window.addEventListener( + "scroll", + function () { + var now = Date.now(); + if (now - store.lastScrollTime < CONFIG.scrollThrottleMs) return; + store.lastScrollTime = now; + + logUiEvent("scroll", { + scrollX: window.scrollX, + scrollY: window.scrollY, + documentHeight: document.documentElement.scrollHeight, + viewportHeight: window.innerHeight, + }); + }, + { passive: true } + ); + + // Navigation tracking for SPAs + function nav(reason) { + logUiEvent("navigate", { reason: reason }); + } + + var origPush = history.pushState; + history.pushState = function () { + origPush.apply(this, arguments); + nav("pushState"); + }; + + var origReplace = history.replaceState; + history.replaceState = function () { + origReplace.apply(this, arguments); + nav("replaceState"); + }; + + window.addEventListener("popstate", function () { + nav("popstate"); + }); + window.addEventListener("hashchange", function () { + nav("hashchange"); + }); + } + + // ========================================================================== + // Console Interception + // ========================================================================== + + var originalConsole = { + log: console.log.bind(console), + debug: console.debug.bind(console), + info: console.info.bind(console), + warn: console.warn.bind(console), + error: console.error.bind(console), + }; + + ["log", "debug", "info", "warn", "error"].forEach(function (method) { + console[method] = function () { + var args = Array.prototype.slice.call(arguments); + + var entry = { + timestamp: Date.now(), + level: method.toUpperCase(), + args: formatArgs(args), + stack: method === "error" ? new Error().stack : null, + }; + + store.consoleLogs.push(entry); + pruneBuffer(store.consoleLogs, CONFIG.bufferSize.console); + + originalConsole[method].apply(console, args); + }; + }); + + window.addEventListener("error", function (event) { + store.consoleLogs.push({ + timestamp: Date.now(), + level: "ERROR", + args: [ + { + type: "UncaughtError", + message: event.message, + filename: event.filename, + lineno: event.lineno, + colno: event.colno, + stack: event.error ? event.error.stack : null, + }, + ], + stack: event.error ? event.error.stack : null, + }); + pruneBuffer(store.consoleLogs, CONFIG.bufferSize.console); + + // Mark an error moment in UI event stream for agents + logUiEvent("error", { + message: event.message, + filename: event.filename, + lineno: event.lineno, + colno: event.colno, + }); + }); + + window.addEventListener("unhandledrejection", function (event) { + var reason = event.reason; + store.consoleLogs.push({ + timestamp: Date.now(), + level: "ERROR", + args: [ + { + type: "UnhandledRejection", + reason: reason && reason.message ? reason.message : String(reason), + stack: reason && reason.stack ? reason.stack : null, + }, + ], + stack: reason && reason.stack ? reason.stack : null, + }); + pruneBuffer(store.consoleLogs, CONFIG.bufferSize.console); + + logUiEvent("unhandledrejection", { + reason: reason && reason.message ? reason.message : String(reason), + }); + }); + + // ========================================================================== + // Fetch Interception + // ========================================================================== + + var originalFetch = window.fetch.bind(window); + + window.fetch = function (input, init) { + init = init || {}; + var startTime = Date.now(); + // Handle string, Request object, or URL object + var url = typeof input === "string" + ? input + : (input && (input.url || input.href || String(input))) || ""; + var method = init.method || (input && input.method) || "GET"; + + // Don't intercept internal requests + if (url.indexOf("/__manus__/") === 0) { + return originalFetch(input, init); + } + + // Safely parse headers (avoid breaking if headers format is invalid) + var requestHeaders = {}; + try { + if (init.headers) { + requestHeaders = Object.fromEntries(new Headers(init.headers).entries()); + } + } catch (e) { + requestHeaders = { _parseError: true }; + } + + var entry = { + timestamp: startTime, + type: "fetch", + method: method.toUpperCase(), + url: url, + request: { + headers: requestHeaders, + body: init.body ? sanitizeValue(tryParseJson(init.body)) : null, + }, + response: null, + duration: null, + error: null, + }; + + return originalFetch(input, init) + .then(function (response) { + entry.duration = Date.now() - startTime; + + var contentType = (response.headers.get("content-type") || "").toLowerCase(); + var contentLength = response.headers.get("content-length"); + + entry.response = { + status: response.status, + statusText: response.statusText, + headers: Object.fromEntries(response.headers.entries()), + body: null, + }; + + // Semantic network hint for agents on failures (sync, no need to wait for body) + if (response.status >= 400) { + logUiEvent("network_error", { + kind: "fetch", + method: entry.method, + url: entry.url, + status: response.status, + statusText: response.statusText, + }); + } + + // Skip body capture for streaming responses (SSE, etc.) to avoid memory leaks + var isStreaming = contentType.indexOf("text/event-stream") !== -1 || + contentType.indexOf("application/stream") !== -1 || + contentType.indexOf("application/x-ndjson") !== -1; + if (isStreaming) { + entry.response.body = "[Streaming response - not captured]"; + store.networkRequests.push(entry); + pruneBuffer(store.networkRequests, CONFIG.bufferSize.network); + return response; + } + + // Skip body capture for large responses to avoid memory issues + if (contentLength && parseInt(contentLength, 10) > CONFIG.maxBodyLength) { + entry.response.body = "[Response too large: " + contentLength + " bytes]"; + store.networkRequests.push(entry); + pruneBuffer(store.networkRequests, CONFIG.bufferSize.network); + return response; + } + + // Skip body capture for binary content types + var isBinary = contentType.indexOf("image/") !== -1 || + contentType.indexOf("video/") !== -1 || + contentType.indexOf("audio/") !== -1 || + contentType.indexOf("application/octet-stream") !== -1 || + contentType.indexOf("application/pdf") !== -1 || + contentType.indexOf("application/zip") !== -1; + if (isBinary) { + entry.response.body = "[Binary content: " + contentType + "]"; + store.networkRequests.push(entry); + pruneBuffer(store.networkRequests, CONFIG.bufferSize.network); + return response; + } + + // For text responses, clone and read body in background + var clonedResponse = response.clone(); + + // Async: read body in background, don't block the response + clonedResponse + .text() + .then(function (text) { + if (text.length <= CONFIG.maxBodyLength) { + entry.response.body = sanitizeValue(tryParseJson(text)); + } else { + entry.response.body = text.slice(0, CONFIG.maxBodyLength) + "...[truncated]"; + } + }) + .catch(function () { + entry.response.body = "[Unable to read body]"; + }) + .finally(function () { + store.networkRequests.push(entry); + pruneBuffer(store.networkRequests, CONFIG.bufferSize.network); + }); + + // Return response immediately, don't wait for body reading + return response; + }) + .catch(function (error) { + entry.duration = Date.now() - startTime; + entry.error = { message: error.message, stack: error.stack }; + + store.networkRequests.push(entry); + pruneBuffer(store.networkRequests, CONFIG.bufferSize.network); + + logUiEvent("network_error", { + kind: "fetch", + method: entry.method, + url: entry.url, + message: error.message, + }); + + throw error; + }); + }; + + // ========================================================================== + // XHR Interception + // ========================================================================== + + var originalXHROpen = XMLHttpRequest.prototype.open; + var originalXHRSend = XMLHttpRequest.prototype.send; + + XMLHttpRequest.prototype.open = function (method, url) { + this._manusData = { + method: (method || "GET").toUpperCase(), + url: url, + startTime: null, + }; + return originalXHROpen.apply(this, arguments); + }; + + XMLHttpRequest.prototype.send = function (body) { + var xhr = this; + + if ( + xhr._manusData && + xhr._manusData.url && + xhr._manusData.url.indexOf("/__manus__/") !== 0 + ) { + xhr._manusData.startTime = Date.now(); + xhr._manusData.requestBody = body ? sanitizeValue(tryParseJson(body)) : null; + + xhr.addEventListener("load", function () { + var contentType = (xhr.getResponseHeader("content-type") || "").toLowerCase(); + var responseBody = null; + + // Skip body capture for streaming responses + var isStreaming = contentType.indexOf("text/event-stream") !== -1 || + contentType.indexOf("application/stream") !== -1 || + contentType.indexOf("application/x-ndjson") !== -1; + + // Skip body capture for binary content types + var isBinary = contentType.indexOf("image/") !== -1 || + contentType.indexOf("video/") !== -1 || + contentType.indexOf("audio/") !== -1 || + contentType.indexOf("application/octet-stream") !== -1 || + contentType.indexOf("application/pdf") !== -1 || + contentType.indexOf("application/zip") !== -1; + + if (isStreaming) { + responseBody = "[Streaming response - not captured]"; + } else if (isBinary) { + responseBody = "[Binary content: " + contentType + "]"; + } else { + // Safe to read responseText for text responses + try { + var text = xhr.responseText || ""; + if (text.length > CONFIG.maxBodyLength) { + responseBody = text.slice(0, CONFIG.maxBodyLength) + "...[truncated]"; + } else { + responseBody = sanitizeValue(tryParseJson(text)); + } + } catch (e) { + // responseText may throw for non-text responses + responseBody = "[Unable to read response: " + e.message + "]"; + } + } + + var entry = { + timestamp: xhr._manusData.startTime, + type: "xhr", + method: xhr._manusData.method, + url: xhr._manusData.url, + request: { body: xhr._manusData.requestBody }, + response: { + status: xhr.status, + statusText: xhr.statusText, + body: responseBody, + }, + duration: Date.now() - xhr._manusData.startTime, + error: null, + }; + + store.networkRequests.push(entry); + pruneBuffer(store.networkRequests, CONFIG.bufferSize.network); + + if (entry.response && entry.response.status >= 400) { + logUiEvent("network_error", { + kind: "xhr", + method: entry.method, + url: entry.url, + status: entry.response.status, + statusText: entry.response.statusText, + }); + } + }); + + xhr.addEventListener("error", function () { + var entry = { + timestamp: xhr._manusData.startTime, + type: "xhr", + method: xhr._manusData.method, + url: xhr._manusData.url, + request: { body: xhr._manusData.requestBody }, + response: null, + duration: Date.now() - xhr._manusData.startTime, + error: { message: "Network error" }, + }; + + store.networkRequests.push(entry); + pruneBuffer(store.networkRequests, CONFIG.bufferSize.network); + + logUiEvent("network_error", { + kind: "xhr", + method: entry.method, + url: entry.url, + message: "Network error", + }); + }); + } + + return originalXHRSend.apply(this, arguments); + }; + + // ========================================================================== + // Data Reporting + // ========================================================================== + + function reportLogs() { + var consoleLogs = store.consoleLogs.splice(0); + var networkRequests = store.networkRequests.splice(0); + var uiEvents = store.uiEvents.splice(0); + + // Skip if no new data + if ( + consoleLogs.length === 0 && + networkRequests.length === 0 && + uiEvents.length === 0 + ) { + return Promise.resolve(); + } + + var payload = { + timestamp: Date.now(), + consoleLogs: consoleLogs, + networkRequests: networkRequests, + // Mirror uiEvents to sessionEvents for sessionReplay.log + sessionEvents: uiEvents, + // agent-friendly semantic events + uiEvents: uiEvents, + }; + + return originalFetch(CONFIG.reportEndpoint, { + method: "POST", + headers: { "Content-Type": "application/json" }, + body: JSON.stringify(payload), + }).catch(function () { + // Put data back on failure (but respect limits) + store.consoleLogs = consoleLogs.concat(store.consoleLogs); + store.networkRequests = networkRequests.concat(store.networkRequests); + store.uiEvents = uiEvents.concat(store.uiEvents); + + pruneBuffer(store.consoleLogs, CONFIG.bufferSize.console); + pruneBuffer(store.networkRequests, CONFIG.bufferSize.network); + pruneBuffer(store.uiEvents, CONFIG.bufferSize.ui); + }); + } + + // Periodic reporting + setInterval(reportLogs, CONFIG.reportInterval); + + // Report on page unload + window.addEventListener("beforeunload", function () { + var consoleLogs = store.consoleLogs; + var networkRequests = store.networkRequests; + var uiEvents = store.uiEvents; + + if ( + consoleLogs.length === 0 && + networkRequests.length === 0 && + uiEvents.length === 0 + ) { + return; + } + + var payload = { + timestamp: Date.now(), + consoleLogs: consoleLogs, + networkRequests: networkRequests, + // Mirror uiEvents to sessionEvents for sessionReplay.log + sessionEvents: uiEvents, + uiEvents: uiEvents, + }; + + if (navigator.sendBeacon) { + var payloadStr = JSON.stringify(payload); + // sendBeacon has ~64KB limit, truncate if too large + var MAX_BEACON_SIZE = 60000; // Leave some margin + if (payloadStr.length > MAX_BEACON_SIZE) { + // Prioritize: keep recent events, drop older logs + var truncatedPayload = { + timestamp: Date.now(), + consoleLogs: consoleLogs.slice(-50), + networkRequests: networkRequests.slice(-20), + sessionEvents: uiEvents.slice(-100), + uiEvents: uiEvents.slice(-100), + _truncated: true, + }; + payloadStr = JSON.stringify(truncatedPayload); + } + navigator.sendBeacon(CONFIG.reportEndpoint, payloadStr); + } + }); + + // ========================================================================== + // Initialization + // ========================================================================== + + // Install semantic UI listeners ASAP + try { + installUiEventListeners(); + } catch (e) { + console.warn("[Manus] Failed to install UI listeners:", e); + } + + // Mark as initialized + window.__MANUS_DEBUG_COLLECTOR__ = { + version: "2.0-no-rrweb", + store: store, + forceReport: reportLogs, + }; + + console.debug("[Manus] Debug collector initialized (no rrweb, UI events only)"); +})(); diff --git a/client/src/App.tsx b/client/src/App.tsx new file mode 100644 index 0000000..6f24bf4 --- /dev/null +++ b/client/src/App.tsx @@ -0,0 +1,42 @@ +import { Toaster } from "@/components/ui/sonner"; +import { TooltipProvider } from "@/components/ui/tooltip"; +import NotFound from "@/pages/NotFound"; +import { Route, Switch } from "wouter"; +import ErrorBoundary from "./components/ErrorBoundary"; +import { ThemeProvider } from "./contexts/ThemeContext"; +import Home from "./pages/Home"; + +function Router() { + // make sure to consider if you need authentication for certain routes + return ( + + + + {/* Final fallback route */} + + + ); +} + +// NOTE: About Theme +// - First choose a default theme according to your design style (dark or light bg), than change color palette in index.css +// to keep consistent foreground/background color across components +// - If you want to make theme switchable, pass `switchable` ThemeProvider and use `useTheme` hook + +function App() { + return ( + + + + + + + + + ); +} + +export default App; diff --git a/client/src/_core/hooks/useAuth.ts b/client/src/_core/hooks/useAuth.ts new file mode 100644 index 0000000..dcef9bd --- /dev/null +++ b/client/src/_core/hooks/useAuth.ts @@ -0,0 +1,84 @@ +import { getLoginUrl } from "@/const"; +import { trpc } from "@/lib/trpc"; +import { TRPCClientError } from "@trpc/client"; +import { useCallback, useEffect, useMemo } from "react"; + +type UseAuthOptions = { + redirectOnUnauthenticated?: boolean; + redirectPath?: string; +}; + +export function useAuth(options?: UseAuthOptions) { + const { redirectOnUnauthenticated = false, redirectPath = getLoginUrl() } = + options ?? {}; + const utils = trpc.useUtils(); + + const meQuery = trpc.auth.me.useQuery(undefined, { + retry: false, + refetchOnWindowFocus: false, + }); + + const logoutMutation = trpc.auth.logout.useMutation({ + onSuccess: () => { + utils.auth.me.setData(undefined, null); + }, + }); + + const logout = useCallback(async () => { + try { + await logoutMutation.mutateAsync(); + } catch (error: unknown) { + if ( + error instanceof TRPCClientError && + error.data?.code === "UNAUTHORIZED" + ) { + return; + } + throw error; + } finally { + utils.auth.me.setData(undefined, null); + await utils.auth.me.invalidate(); + } + }, [logoutMutation, utils]); + + const state = useMemo(() => { + localStorage.setItem( + "manus-runtime-user-info", + JSON.stringify(meQuery.data) + ); + return { + user: meQuery.data ?? null, + loading: meQuery.isLoading || logoutMutation.isPending, + error: meQuery.error ?? logoutMutation.error ?? null, + isAuthenticated: Boolean(meQuery.data), + }; + }, [ + meQuery.data, + meQuery.error, + meQuery.isLoading, + logoutMutation.error, + logoutMutation.isPending, + ]); + + useEffect(() => { + if (!redirectOnUnauthenticated) return; + if (meQuery.isLoading || logoutMutation.isPending) return; + if (state.user) return; + if (typeof window === "undefined") return; + if (window.location.pathname === redirectPath) return; + + window.location.href = redirectPath + }, [ + redirectOnUnauthenticated, + redirectPath, + logoutMutation.isPending, + meQuery.isLoading, + state.user, + ]); + + return { + ...state, + refresh: () => meQuery.refetch(), + logout, + }; +} diff --git a/client/src/components/AIChatBox.tsx b/client/src/components/AIChatBox.tsx new file mode 100644 index 0000000..1c00871 --- /dev/null +++ b/client/src/components/AIChatBox.tsx @@ -0,0 +1,335 @@ +import { Button } from "@/components/ui/button"; +import { Textarea } from "@/components/ui/textarea"; +import { ScrollArea } from "@/components/ui/scroll-area"; +import { cn } from "@/lib/utils"; +import { Loader2, Send, User, Sparkles } from "lucide-react"; +import { useState, useEffect, useRef } from "react"; +import { Streamdown } from "streamdown"; + +/** + * Message type matching server-side LLM Message interface + */ +export type Message = { + role: "system" | "user" | "assistant"; + content: string; +}; + +export type AIChatBoxProps = { + /** + * Messages array to display in the chat. + * Should match the format used by invokeLLM on the server. + */ + messages: Message[]; + + /** + * Callback when user sends a message. + * Typically you'll call a tRPC mutation here to invoke the LLM. + */ + onSendMessage: (content: string) => void; + + /** + * Whether the AI is currently generating a response + */ + isLoading?: boolean; + + /** + * Placeholder text for the input field + */ + placeholder?: string; + + /** + * Custom className for the container + */ + className?: string; + + /** + * Height of the chat box (default: 600px) + */ + height?: string | number; + + /** + * Empty state message to display when no messages + */ + emptyStateMessage?: string; + + /** + * Suggested prompts to display in empty state + * Click to send directly + */ + suggestedPrompts?: string[]; +}; + +/** + * A ready-to-use AI chat box component that integrates with the LLM system. + * + * Features: + * - Matches server-side Message interface for seamless integration + * - Markdown rendering with Streamdown + * - Auto-scrolls to latest message + * - Loading states + * - Uses global theme colors from index.css + * + * @example + * ```tsx + * const ChatPage = () => { + * const [messages, setMessages] = useState([ + * { role: "system", content: "You are a helpful assistant." } + * ]); + * + * const chatMutation = trpc.ai.chat.useMutation({ + * onSuccess: (response) => { + * // Assuming your tRPC endpoint returns the AI response as a string + * setMessages(prev => [...prev, { + * role: "assistant", + * content: response + * }]); + * }, + * onError: (error) => { + * console.error("Chat error:", error); + * // Optionally show error message to user + * } + * }); + * + * const handleSend = (content: string) => { + * const newMessages = [...messages, { role: "user", content }]; + * setMessages(newMessages); + * chatMutation.mutate({ messages: newMessages }); + * }; + * + * return ( + * + * ); + * }; + * ``` + */ +export function AIChatBox({ + messages, + onSendMessage, + isLoading = false, + placeholder = "Type your message...", + className, + height = "600px", + emptyStateMessage = "Start a conversation with AI", + suggestedPrompts, +}: AIChatBoxProps) { + const [input, setInput] = useState(""); + const scrollAreaRef = useRef(null); + const containerRef = useRef(null); + const inputAreaRef = useRef(null); + const textareaRef = useRef(null); + + // Filter out system messages + const displayMessages = messages.filter((msg) => msg.role !== "system"); + + // Calculate min-height for last assistant message to push user message to top + const [minHeightForLastMessage, setMinHeightForLastMessage] = useState(0); + + useEffect(() => { + if (containerRef.current && inputAreaRef.current) { + const containerHeight = containerRef.current.offsetHeight; + const inputHeight = inputAreaRef.current.offsetHeight; + const scrollAreaHeight = containerHeight - inputHeight; + + // Reserve space for: + // - padding (p-4 = 32px top+bottom) + // - user message: 40px (item height) + 16px (margin-top from space-y-4) = 56px + // Note: margin-bottom is not counted because it naturally pushes the assistant message down + const userMessageReservedHeight = 56; + const calculatedHeight = scrollAreaHeight - 32 - userMessageReservedHeight; + + setMinHeightForLastMessage(Math.max(0, calculatedHeight)); + } + }, []); + + // Scroll to bottom helper function with smooth animation + const scrollToBottom = () => { + const viewport = scrollAreaRef.current?.querySelector( + '[data-radix-scroll-area-viewport]' + ) as HTMLDivElement; + + if (viewport) { + requestAnimationFrame(() => { + viewport.scrollTo({ + top: viewport.scrollHeight, + behavior: 'smooth' + }); + }); + } + }; + + const handleSubmit = (e: React.FormEvent) => { + e.preventDefault(); + const trimmedInput = input.trim(); + if (!trimmedInput || isLoading) return; + + onSendMessage(trimmedInput); + setInput(""); + + // Scroll immediately after sending + scrollToBottom(); + + // Keep focus on input + textareaRef.current?.focus(); + }; + + const handleKeyDown = (e: React.KeyboardEvent) => { + if (e.key === "Enter" && !e.shiftKey) { + e.preventDefault(); + handleSubmit(e); + } + }; + + return ( +
+ {/* Messages Area */} +
+ {displayMessages.length === 0 ? ( +
+
+
+ +

{emptyStateMessage}

+
+ + {suggestedPrompts && suggestedPrompts.length > 0 && ( +
+ {suggestedPrompts.map((prompt, index) => ( + + ))} +
+ )} +
+
+ ) : ( + +
+ {displayMessages.map((message, index) => { + // Apply min-height to last message only if NOT loading (when loading, the loading indicator gets it) + const isLastMessage = index === displayMessages.length - 1; + const shouldApplyMinHeight = + isLastMessage && !isLoading && minHeightForLastMessage > 0; + + return ( +
+ {message.role === "assistant" && ( +
+ +
+ )} + +
+ {message.role === "assistant" ? ( +
+ {message.content} +
+ ) : ( +

+ {message.content} +

+ )} +
+ + {message.role === "user" && ( +
+ +
+ )} +
+ ); + })} + + {isLoading && ( +
0 + ? { minHeight: `${minHeightForLastMessage}px` } + : undefined + } + > +
+ +
+
+ +
+
+ )} +
+ + )} +
+ + {/* Input Area */} +
+