Metadata-Version: 2.4
Name: fricp-cpp
Version: 1.0.0
Summary: Fast and Robust ICP (TPAMI 2022) — pybind11 bindings for the reference C++ implementation, drop-in for open3d's registration_icp
Author: Yuxin Yao, Juyong Zhang, Bailin Deng
Maintainer: zlt
License: MIT License
        
        Copyright (c) 2020 yaoyuxin
        
        Permission is hereby granted, free of charge, to any person obtaining a copy
        of this software and associated documentation files (the "Software"), to deal
        in the Software without restriction, including without limitation the rights
        to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
        copies of the Software, and to permit persons to whom the Software is
        furnished to do so, subject to the following conditions:
        
        The above copyright notice and this permission notice shall be included in all
        copies or substantial portions of the Software.
        
        THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
        IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
        FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
        AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
        LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
        OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
        SOFTWARE.
        
Project-URL: Homepage, https://github.com/ZhangLanTao/Fast-Robust-ICP
Project-URL: Issues, https://github.com/ZhangLanTao/Fast-Robust-ICP/issues
Project-URL: Upstream C++ implementation, https://github.com/yaoyx689/Fast-Robust-ICP
Project-URL: Paper, https://arxiv.org/abs/2007.07627
Keywords: icp,point-cloud,registration,robust-estimation,welsch,anderson-acceleration,open3d,pybind11
Classifier: Development Status :: 4 - Beta
Classifier: Intended Audience :: Science/Research
Classifier: License :: OSI Approved :: MIT License
Classifier: Operating System :: POSIX :: Linux
Classifier: Operating System :: MacOS
Classifier: Operating System :: Microsoft :: Windows
Classifier: Programming Language :: C++
Classifier: Programming Language :: Python :: 3
Classifier: Programming Language :: Python :: 3.9
Classifier: Programming Language :: Python :: 3.10
Classifier: Programming Language :: Python :: 3.11
Classifier: Programming Language :: Python :: 3.12
Classifier: Programming Language :: Python :: 3.13
Classifier: Topic :: Scientific/Engineering :: Image Recognition
Classifier: Topic :: Scientific/Engineering :: Mathematics
Requires-Python: >=3.9
Description-Content-Type: text/markdown
License-File: LICENSE
Requires-Dist: numpy>=1.17
Provides-Extra: open3d
Requires-Dist: open3d>=0.13; extra == "open3d"
Provides-Extra: test
Requires-Dist: pytest>=7; extra == "test"
Requires-Dist: scipy>=1.5; extra == "test"
Requires-Dist: open3d>=0.13; extra == "test"
Dynamic: license-file

# fricp-cpp — Fast and Robust ICP (Python bindings)

Python 接口（pybind11 + C++）：[Fast and Robust Iterative Closest Point](https://arxiv.org/abs/2007.07627)，
Zhang, Yao, Deng, IEEE TPAMI 2022。

```bash
pip install fricp-cpp
```

```python
import fricp_cpp

res = fricp_cpp.registration_icp(source, target, 0.02, init, "point_to_point")
res.transformation, res.fitness, res.inlier_rmse, res.correspondence_set
```

> **非官方发布。** 算法与 C++ 内核来自原作者 Juyong Zhang / Yuxin Yao / Bailin Deng
> 的 [yaoyx689/Fast-Robust-ICP](https://github.com/yaoyx689/Fast-Robust-ICP)（MIT）。
> 本包只是给它加了 Python 绑定，与原作者无关联、未经其背书。
> PyPI 上的 [`fricp`](https://pypi.org/project/fricp/) 是同一篇论文的另一个
> （Rust）实现，与本包无关。

包含两层 API：

| 层 | 入口 | 说明 |
|---|---|---|
| **兼容层**（默认） | `fricp_cpp.registration_icp` | 与 `temp/robust_icp.py`、以及 `open3d.pipelines.registration.registration_icp` 签名/返回值完全一致，可直接替换 |
| **原生层** | `fricp_cpp.native.register_` | 本仓库 `main.cpp` 里的 8 种方法（ICP / AA-ICP / Fast ICP / Robust ICP / point-to-plane / Sparse ICP …），含它的 scale + de-mean 预处理 |

## 安装

装 wheel（`pip install fricp-cpp`）不需要编译器也不需要 Eigen。从源码装
（sdist 或本仓库）则需要 C++14 编译器 + Eigen 3.3+（nanoflann 已内置于
`../include`）：

```bash
sudo apt install libeigen3-dev          # 或者把 Eigen 放到 ../include/eigen

cd python
pip install .                           # 或 pip install -e .
```

Eigen 若不在标准路径，用 `EIGEN3_INCLUDE_DIR=/path/to/eigen3 pip install .`。
其他环境变量：`FRICP_NATIVE=1` 开 `-march=native`（会让 wheel 不可移植，只在
自用编译时开），`FRICP_NO_OPENMP=1` 关 OpenMP。

不装的话也可以直接就地编译：

```bash
cd python && python setup.py build_ext --inplace
export PYTHONPATH=$PWD:$PYTHONPATH
```

## 打包发布（在别的项目里使用）

### 打包

```bash
cd python
uv build                                  # 或: pip install build && python -m build
# -> dist/fricp_cpp-1.0.0-cp312-cp312-linux_x86_64.whl
# -> dist/fricp_cpp-1.0.0.tar.gz
```

会同时产出 wheel 和 sdist：

- **wheel**：已编译好的二进制。装的时候不需要编译器、不需要 Eigen、不需要本
  仓库。但它带 ABI 标签（`cp312` = CPython 3.12、`linux_x86_64` = glibc x86-64），
  **只能装到同样的 Python 版本和平台上**。
- **sdist**（`.tar.gz`）：源码包，装的时候现场编译，需要编译器 + Eigen。它是
  自包含的 —— `setup.py` 会把 `ICP.h` / `FRICP.h` / `nanoflann.hpp` 等镜像到
  `python/vendor/`（`.gitignore` 掉，不进版本库），`MANIFEST.in` 把这份镜像打进
  sdist，所以脱离本仓库也能编译。

> **wheel 的 ABI 标签取决于"用哪个解释器打包"**，不是"用哪个解释器装"。
> 要给 3.12 的环境打包就必须用 3.12 打：
> `uv build --python /path/to/.venv/bin/python`。用错解释器会得到装不上的
> `cp314-...whl`。

### 在别的项目里安装

```bash
# 1) 直接装 wheel 文件（最简单，同机自用推荐）
pip install /home/zlt/Project/Fast-Robust-ICP/python/dist/fricp_cpp-1.0.0-*.whl

# 2) 从源码目录装（会现场编译）
pip install /home/zlt/Project/Fast-Robust-ICP/python

# 3) 开发时联动：改了 C++ 要重新 pip install，改了 .py 立即生效
pip install -e /home/zlt/Project/Fast-Robust-ICP/python

# 4) 从 git 装（需要目标机有编译器 + Eigen）
pip install "git+https://github.com/ZhangLanTao/Fast-Robust-ICP.git#subdirectory=python"
```

写进依赖声明：

```toml
# 目标项目的 pyproject.toml
[project]
dependencies = ["fricp-cpp"]

[tool.uv.sources]                       # uv
fricp-cpp = { path = "../Fast-Robust-ICP/python", editable = true }
```

```txt
# 或者 requirements.txt
fricp-cpp @ file:///home/zlt/Project/Fast-Robust-ICP/python/dist/fricp_cpp-1.0.0-cp312-cp312-manylinux_2_24_x86_64.manylinux_2_28_x86_64.whl
```

### 需要跨机器 / 跨 Python 版本分发（manylinux）

本地 wheel 的 `linux_x86_64` 标签 pip 不会从索引里挑（只能装本地文件），而且
**PyPI 会直接拒收**这种标签。要能 `pip install fricp-cpp` 就装上，必须打
**manylinux** wheel：

```bash
cd python
pipx run cibuildwheel --platform linux          # 需要 docker
# 只打某几个目标：
CIBW_BUILD="cp312-manylinux_x86_64" pipx run cibuildwheel --platform linux
# -> wheelhouse/fricp_cpp-1.0.0-cp312-cp312-manylinux_2_24_x86_64.manylinux_2_28_x86_64.whl
```

配置在 `pyproject.toml` 的 `[tool.cibuildwheel]`：容器里下载 Eigen、装完跑冒烟
测试、`before-build` 先清 `build/`。默认出 cp39~cp313 的 manylinux + musllinux
wheel，`auditwheel` 会把 `libgomp` 打进 wheel（所以 OpenMP 仍然可用）。

> **先清 `build/`**：`python setup.py build_ext --inplace` 留下的宿主机 `.so`
> 会被容器里的 setuptools 当成"已是最新"直接复用，产出的 wheel 链接的是宿主
> glibc，`auditwheel` 报 *too-recent versioned symbols* 而失败。`before-build`
> 已经处理，手动跑的话也别忘了。

## 发布到 PyPI

### 一次性准备

1. 注册 <https://pypi.org/account/register/> 并开启 2FA（现在是强制的）。
2. **Trusted Publishing（推荐，不用管 token）**：在 PyPI 的
   *Your projects → Publishing → Add a new pending publisher* 里填
   - PyPI Project Name: `fricp-cpp`
   - Owner / Repository: `ZhangLanTao/Fast-Robust-ICP`
   - Workflow name: `publish.yml`
   - Environment name: `pypi`
3. 或者走 API token：*Account settings → API tokens → Add API token*，
   然后 `~/.pypirc`：
   ```ini
   [pypi]
     username = __token__
     password = pypi-AgEIcHlwaS5vcmc...
   ```

### 方式 A：GitHub Actions（推荐，能出全平台 wheel）

[`.github/workflows/publish.yml`](../.github/workflows/publish.yml) 已经写好：
每次 push 构建 Linux x86_64/aarch64 + macOS + Windows 的 wheel 和 sdist，
**打 `v*` tag 才发布**，用 OIDC trusted publishing（仓库里不存任何密钥）。

```bash
# 改版本号（PyPI 上同一个版本号不能覆盖，必须递增）
vim python/fricp_cpp/_version.py
git commit -am "release v1.0.0" && git push
git tag v1.0.0 && git push --tags        # -> 触发发布
```

`environment: pypi` 那一行让你可以在 GitHub 的 *Settings → Environments → pypi*
加一个 required reviewer，作为上传前的人工闸门。

### 方式 B：本地手动上传

只发当前机器能打的那些 wheel（Linux 的话至少要 manylinux）：

```bash
cd python
rm -rf build dist wheelhouse

python -m build --sdist --outdir dist            # sdist
pipx run cibuildwheel --platform linux           # manylinux wheels -> wheelhouse/
cp wheelhouse/*.whl dist/

pipx run twine check dist/*                      # 元数据自检
pipx run twine upload --repository testpypi dist/*   # 先排练一遍（强烈建议）
pipx run twine upload dist/*                     # 正式发布
```

TestPyPI 验收（`--extra-index-url` 是因为 numpy 只在正式 PyPI 上）：

```bash
pip install -i https://test.pypi.org/simple/ \
    --extra-index-url https://pypi.org/simple fricp-cpp
```

### 发布前检查清单

- [ ] `fricp_cpp/_version.py` 的版本号已递增（**PyPI 不允许重复上传同一版本，删了也不能复用**）
- [ ] `pyproject.toml` 里 `name = "fricp-cpp"`，`Homepage` 指向 fork（已设）
- [ ] `twine check dist/*` 通过
- [ ] Linux wheel 是 `manylinux_*` 标签，不是 `linux_x86_64`
- [ ] `pytest tests/` 通过
- [ ] LICENSE（MIT）和原作者署名保留 —— 已在 `pyproject.toml` 的 `authors` 和
      README 顶部的非官方声明里

### 打包相关文件

| 文件 | 作用 |
|---|---|
| `pyproject.toml` | 包元数据（PEP 621）、build backend、cibuildwheel 配置 |
| `setup.py` | 只负责 C++ 扩展：找 Eigen、造 `eigen/` 前缀 shim、镜像 vendor 头文件、编译开关 |
| `MANIFEST.in` | 决定哪些文件进 sdist（`src/`、`vendor/`、`tests/`、README、LICENSE） |
| `fricp_cpp/_version.py` | 版本号唯一来源，`pyproject.toml` 静态读它 |
| `../.github/workflows/publish.yml` | CI：全平台 wheel + sdist，打 `v*` tag 发 PyPI |
| `vendor/` | 由 `setup.py` 自动镜像的仓库根 C++ 头文件（不进版本库） |

## 用法

### 1. 直接替换纯 Python 版 / Open3D 版

```python
import numpy as np
import open3d as o3d
import fricp_cpp                      # 也可以 `import fricp_cpp as robust_icp`

res = fricp_cpp.registration_icp(
    source, target,               # o3d.geometry.PointCloud 或 (N, 3) ndarray
    max_correspondence_distance=0.02,
    init=np.eye(4),
    estimation_method=o3d.pipelines.registration.TransformationEstimationPointToPoint(),
)

res.transformation          # (4, 4)
res.fitness                 # Open3D 语义
res.inlier_rmse
res.correspondence_set      # o3d.utility.Vector2iVector
res.robust_info             # {'nu', 'nu_begin', 'nu_end', 'res_med', 'num_iterations',
                            #  'num_stages', 'energy', 'time', 'mode', 'kernel', 'backend'}
```

返回值是 `o3d.pipelines.registration.RegistrationResult` 的子类，
`isinstance(res, o3d...RegistrationResult)` 为 `True`，下游代码不用改。

参数可以走 `RobustICPOption`，也可以直接用关键字：

```python
res = fricp_cpp.registration_icp(src, trg, np.inf, init, "point_to_plane",
                             kernel="gm", use_anderson=False, nu_begin_k=3.0)

opt = fricp_cpp.RobustICPOption(kernel="tukey", anneal=False, max_total_iteration=80)
res = fricp_cpp.registration_icp(src, trg, 0.01, init, option=opt)
```

字段与默认值和 `temp/robust_icp.py` 的 `RobustICPOption` 逐一对应
（`kernel` 支持 `welsch/tukey/gm/cauchy/huber/l1/l2`；
`nu_begin_k` 默认 `0.5` 而非论文的 `3.0`，理由见 dataclass 的 docstring）。

### 2. 复用 target 的 KD-tree（重复配准同一个模型时）

建树 + 算点云分辨率是每次调用的固定开销。同一个 target 反复用时先构造一次：

```python
tgt = fricp_cpp.Target(target)                        # 建 KD-tree、缓存 kNN 分辨率
# 需要 point-to-plane 时把法向一起传进去，避免每次重新估计
tgt = fricp_cpp.Target(target, target_normals)

for frame in frames:
    res = fricp_cpp.registration_icp(frame, tgt, 0.02, init)
```

`fricp_cpp.Target` 有 `size / has_normals / resolution / diag / points / normals /
set_normals() / query()`。`query` 是一个和 `scipy.spatial.cKDTree.query` 同
约定的 k 近邻查询（返回 `(distances, indices)`，`k=1` 时形状为 `(N,)`），
可以直接当独立的 KD-tree 用：

```python
dist, idx = tgt.query(points)          # 1-NN
dist, idx = tgt.query(points, k=7)     # (N, 7)
```

### 3. 线程

```python
fricp_cpp.has_openmp, fricp_cpp.get_num_threads()
fricp_cpp.set_num_threads(8)                       # 进程级
res = fricp_cpp.registration_icp(src, trg, 0.02, init, num_threads=4)   # 单次调用
```

默认按点数自动选线程数（`ceil(N / grain_size)`，`grain_size=256`，上限为 OMP
线程数）。内层循环用 `schedule(guided)`：机器上有其他负载时，`static` 划分会
让一个被抢占的线程拖住整个 barrier，每次迭代多花一个调度时间片。

### 4. 原生方法（逐字复现 `main.cpp`）

```python
M = fricp_cpp.native.Method          # ICP AA_ICP FICP RICP PPL RPPL SparseICP SICPPPL
res = fricp_cpp.native.register_(source, target, method=M.RICP)
res.transformation               # 已换算回输入的原始坐标系（含 scale/de-mean 的还原）

pars = fricp_cpp.native.Parameters() # 完整的 ICP::Parameters
pars.max_icp = 200
pars.nu_begin_k = 3.0
res = fricp_cpp.native.register_(source, target, method=M.RICP, parameters=pars)

# point-to-plane 需要 target 法向
res = fricp_cpp.native.register_(source, target, method=M.RPPL, target_normals=normals)
```

`normalize=True` / `demean=True`（默认）复现 `main.cpp` 的预处理；
低重叠率场景按 README 的建议传 `demean=False`。
`apply_method_presets=False` 则不覆盖你自己设置的 `pars.f` / `pars.use_AA`。

### 5. 也可以只当 KD-tree 用

`Target` 是一个独立的 3-D KD-tree（自己实现的，不依赖 nanoflann），
1-NN 查询在本机实测比 `scipy.spatial.cKDTree` 单核快 2~3 倍，结果逐位一致
（见 `tests/test_kdtree.py`）。

## 兼容层与原生 FRICP 的区别

兼容层是 `temp/robust_icp.py` 的 C++ 移植，语义以它为准，与 `FRICP.h` 有意不同的地方：

| | 兼容层 (`fricp_cpp.registration_icp`) | 原生 (`fricp_cpp.native`) |
|---|---|---|
| 鲁棒核 | welsch/tukey/gm/cauchy/huber/l1/l2 | `ICP::Function` 全套（含 pnorm/fair/logistic/trimmed/autowelsch） |
| `max_correspondence_distance` | 支持，且随 ν 自适应放宽（`adaptive_trim`） | 不支持（无硬截断） |
| `nu_begin_k` 默认 | 0.5 | 3.0 |
| se(3) log/exp | 闭式（与 scipy `rotvec` 一致） | Schur / `MatrixFunctions` |
| 预处理 | 不缩放、不去质心（可选 `center_init`） | scale + de-mean（`main.cpp` 行为） |
| fitness / rmse / correspondences | Open3D 语义，直接给出 | 不提供 |

## 测试

```bash
cd python
PYTHONPATH=$PWD python tests/test_parity.py     # 完整报告：一致性 + benchmark
PYTHONPATH=$PWD python tests/test_kdtree.py     # KD-tree 与 scipy 逐位比对
PYTHONPATH=$PWD pytest tests/                   # 只跑断言
```

`tests/test_parity.py` 会把每个用例分别跑一遍 C++ 版和 `temp/robust_icp.py`，
比较最终位姿（判据：< 0.5° 且 < 0.5% 物体尺寸，或最终鲁棒能量相对差 < 0.5%），
并打印加速比。`tests/bench.py` / `bench_iter.py` / `bench_nn.py` 是分项 benchmark
（端到端 / 单次迭代-线程扫描 / 最近邻查询）。

> benchmark 注意：这台机器上有其他负载（实测 loadavg 8~25）。所有计时都取多次
> 重复的最小值，但仍会偶尔出现被抢占的离群行（速度比掉到 1x 以下、耗时是
> 调度时间片的整数倍）。看中位数，不要看单行。

## 性能

**一致性**：64 个用例（7 种核 × p2point/p2plane × 4 个随机种子 × 有/无裁剪 …）中
63 个与纯 Python 版的最终位姿差在 1e-11 以内（多数在 1e-15），剩下 1 个
（tukey/seed 4）两边都收敛到同一个坏盆地、最终能量相差 0.015%
（关掉 Anderson 后两边逐位相同）。两边与 ground truth 的误差中位数完全一致。

**加速比**（本机 20 核，多次重复取最小值，`tests/bench.py`；`temp/robust_icp.py`
本身已经用了 scipy cKDTree 的多线程查询，所以这是对一个不算慢的基线的加速）：

| 点数 | fricp_cpp (C++) | robust_icp.py | 加速 |
|---:|---:|---:|---:|
| 2 000 | 3.2 ms | 80 ms | **25x** |
| 10 000 | 20 ms | 243 ms | **12x** |
| 50 000 | 234 ms | 3 766 ms | **16x** |
| 200 000 | 1 383 ms | 7 588 ms | **5.5x** |

`tests/test_parity.py` 那 64 个用例（4000 点）的整体加速比中位数约 **15x**。

主要来自：

| 项 | 说明 |
|---|---|
| 单次迭代融合成 1 个 OpenMP region | transform / 最近邻 / 残差 / 能量 / 权重原本是多趟，几千点规模下 barrier 就是主要开销 |
| `schedule(guided)` | 机器有负载时 `static` 会被单个被抢占的线程拖住整个 barrier |
| 自己实现的 3-D KD-tree | 无每次查询的堆分配（内置的 nanoflann 1.13 每次查询都 `new` 一个 `std::vector`），点按树序重排，增量式 box 距离剪枝；1M 点时每次查询只访问 ~19 个节点 |
| 用上一次迭代的对应点做初始上界 | ICP 每次迭代位姿变化很小，上次的最近邻通常仍然最优，剪枝立刻收紧。**结果不变**，仍是精确最近邻 |
| 默认跳过每阶段的 fitness/rmse 统计 | `relative_fitness`/`relative_rmse` 默认为 0，该判据本来不可能触发，省掉每阶段一整趟最近邻 |

复用 `fricp_cpp.Target`（50k target × 20 帧）另外还能省约 1.4x。

## 文件

```
python/
  pyproject.toml           包元数据 + cibuildwheel 配置
  setup.py                 C++ 扩展（找 Eigen / vendor 镜像 / 编译开关）
  MANIFEST.in              sdist 内容
  LICENSE                  MIT（同上游）
  src/robust_icp_core.h    C++ 算法内核（robust_icp.py 的逐行移植）+ 自带 3-D KD-tree
  src/fricp_cpp_module.cpp     pybind11 绑定（兼容层 + native 层）
  fricp_cpp/__init__.py        Python 公开 API（drop-in for robust_icp.py）
  fricp_cpp/_version.py        版本号
  tests/test_parity.py     与纯 Python 版的一致性 + 性能对比
  tests/test_kdtree.py     KD-tree 与 scipy 的逐位比对
  tests/bench*.py          分项 benchmark
  vendor/                  setup.py 自动镜像的仓库根 C++ 头文件（不进版本库）
```
