Object Pose 6D Estimation English
← 返回知识体系
07 3D 视觉 视觉 SLAM 机器人 机器人抓取

3D–3D Alignment & Point Cloud Registration

3D–3D 对齐
与点云配准

当 RGB-D、双目或多视角重建把观测提升为三维点，位姿估计就从像素重投影转为三维几何对齐。已知对应时可以用 SVD 一步求解；对应未知时则要交替寻找匹配和更新变换。二者输入契约、吸引域和失败方式完全不同。

打开配准实验室 先区分三类问题

Problem Contract

先写清 source → target，再选择求解器

本站使用列向量与 {}^T T_S 记号：它把 source frame 中的点变换到 target frame。源点记为 qi，目标点记为 pi；同一公式可表示 CAD→相机、前一帧→当前帧、扫描→地图或相机→机器人基座。

Rigid 3D–3D alignment
pi≃TRSqi+TtS,TRS∈SO(3)\mathbf p_i \simeq {}^{T}\mathbf R_{S}\mathbf q_i+{}^{T}\mathbf t_{S},\qquad {}^{T}\mathbf R_{S}\in SO(3)
qi 在 source 中，pi 在 target 中；t 的单位与点坐标一致。求得的是 source-to-target。逆变换不是简单给 t 加负号，而是 Rᵀ 与 −Rᵀt。
KNOWN CORRESPONDENCE

点集对齐

语义关键点、同一 surfel、NOCS 坐标或轨迹时间戳已经给出点的身份。目标函数固定，SVD 可一次求解。

UNKNOWN CORRESPONDENCE

点云配准

原始点云没有同序关系。必须通过最近邻、局部描述子或概率模型推断哪些观测属于同一表面位置。

PARTIAL OVERLAP

只有交集能约束位姿

遮挡、视场与扫描路径导致非重叠区域。强迫每个源点匹配目标，必然把外点拉进解中。

SOURCE

要被移动的数据；例如 CAD、当前扫描、相机帧 t−1。

→
TTS

将 source 表达改写到 target frame 的刚体变换。

→
TARGET

固定参考；例如观测点云、地图、相机帧 t 或机器人 base。

↔
INVERSE

STT = [Rᵀ, −Rᵀt]；交换源目标必须求逆。

Closed-form Alignment

中心化分离平移，SVD 恢复旋转

Kabsch / Orthogonal Procrustes 在已知对应、各向同性平方误差下求最优刚体旋转和平移。Umeyama 在同一结构上增加全局尺度，并显式保证旋转不含镜像。

Centroids and cross-covariance
qˉ=1n∑iqi,pˉ=1n∑ipi,H=∑i(qi−qˉ)(pi−pˉ)⊤=UΣV⊤\bar{\mathbf q}=\frac{1}{n}\sum_i\mathbf q_i,\quad \bar{\mathbf p}=\frac{1}{n}\sum_i\mathbf p_i,\quad \mathbf H=\sum_i(\mathbf q_i-\bar{\mathbf q})(\mathbf p_i-\bar{\mathbf p})^\top=\mathbf U\mathbf\Sigma\mathbf V^\top
这里采用 source-centered × target-centeredᵀ 的 H 定义；若代码把两个因子交换，最终 R 公式也要相应转置。不同资料公式看似相反，常只是协方差方向不同。
Proper rigid transform
D=diag⁡(1,1,det⁡(VU⊤)),R=VDU⊤,t=pˉ−Rqˉ\mathbf D=\operatorname{diag}(1,1,\det(\mathbf V\mathbf U^\top)),\quad \mathbf R=\mathbf V\mathbf D\mathbf U^\top,\quad \mathbf t=\bar{\mathbf p}-\mathbf R\bar{\mathbf q}
D 把 O(3) 中可能出现的镜像解投影回 SO(3)。只检查 RᵀR≈I 不够：反射同样正交，必须再检查 det(R)≈+1。
Umeyama similarity transform
pi≃sRqi+t,s=tr⁡(ΣD)∑i∥qi−qˉ∥2,t=pˉ−sRqˉ\mathbf p_i\simeq s\mathbf R\mathbf q_i+\mathbf t,\quad s=\frac{\operatorname{tr}(\mathbf\Sigma\mathbf D)}{\sum_i\|\mathbf q_i-\bar{\mathbf q}\|^2},\quad \mathbf t=\bar{\mathbf p}-s\mathbf R\bar{\mathbf q}
上式省略了分子分母共同的 1/n。s 只表达单一各向同性尺度；不能修复非均匀 CAD 变形、深度依赖偏差或错误内参。
KABSCH

固定尺度的最优旋转

常指中心化点集的最优旋转步骤；配上质心平移就是完整刚体对齐。

PROCRUSTES

更宽泛的问题家族

文献中可能允许平移、尺度或反射。看到“Procrustes”时要继续核对允许的变换群。

UMEYAMA

带尺度且防反射

适合单目轨迹尺度对齐、类别级规范坐标；RGB-D / CAD 同为米时通常关闭尺度估计。

Minimal geometry

点数够不代表几何稳定

1 POINT

只约束平移组合，旋转完全不可观。

2 POINTS

约束一条方向，但绕连线的旋转仍自由。

3 NON-COLLINEAR

无噪声、带身份的三点可定义有手性的刚体 frame。

COLLINEAR / CLUSTERED

秩不足或条件数差；即使残差很小，某些旋转方向仍不可信。

从均匀最小二乘到加权对齐

如果每个点的精度不同，可对质心与协方差使用置信度权重。若误差具有方向性，例如深度沿视线更不确定，则单个标量权重不够，应使用协方差、GICP 或完整的最大似然目标。鲁棒核适合重尾噪声，但大量离群对应仍优先用 RANSAC 建立共识。

Interactive Lab

真正运行三种配准机制

闭式视图对合成 3D 对应执行 3×3 SVD，可开关尺度与镜像；鲁棒视图实际抽取非共线三点、评分全部对应并用全内点重估；ICP 视图在二维切片上执行与三维 point-to-point 相同的“最近邻 ↔ 刚体更新”交替机制，用来直接观察吸引域。

Interactive · 3D–3D Registration

Umeyama、刚体 RANSAC 与 ICP 实验室

三个实验分别处理已知对应、错误对应和未知对应：观察质心与 SVD 如何给出闭式解，再用最小集建立鲁棒共识，最后体验 ICP 对初值、重叠与对应距离的敏感性。
source → target right-handed · det(R)=+1 distance · m
source target transformed source 点编号表示已知一一对应

Centroid

质心位移

—平移在旋转与尺度之后恢复

Spectrum

协方差奇异值

—过小奇异值提示几何退化

Handedness

原始正交解

——

Scale / translation

估计参数

——

Fit

对应点 RMSE

—目标含镜像时 proper rotation 无法零误差拟合
闭式解的前提是对应已知且语义正确。 Kabsch/Umeyama 不搜索最近邻，也不判断某对点是不是错配；它们精确解决的是给定对应与平方损失下的最优刚体/相似变换。离群对应需要加权、RANSAC、截断损失或其他鲁棒估计。

Robust Paired Alignment

对应已经建立，也可能有大量错配

语义关键点、特征匹配或网络预测的 3D–3D 对应都可能含粗大误差。此时不应直接把全部点送入 SVD，而应让最小集提出模型、全体对应建立共识，再对共识集重估。

Rigid correspondence residual
ri=∥pi−(Rqi+t)∥2,i∈I  ⟺  ri<τ3Dr_i=\left\|\mathbf p_i-(\mathbf R\mathbf q_i+\mathbf t)\right\|_2,\qquad i\in\mathcal I\iff r_i<\tau_{3D}
τ3D 是长度单位，不是像素。它应随深度噪声、体素尺寸、物体尺度与点的协方差变化；毫米数据和米数据不能共享同一数值。
  1. 01

    抽取三个非共线对应

    同时检查源三角形和目标三角形的面积、边长比例与重复索引；病态样本不进入模型估计。

  2. 02

    用闭式解生成 proper rigid transform

    估计 R、t，并拒绝数值异常、det(R) 偏离 +1 或明显超出任务先验的候选。

  3. 03

    对全部对应评分

    按三维残差形成内点集；可使用截断损失、M-estimator 或由点协方差归一化后的 Mahalanobis 距离。

  4. 04

    全部内点重新估计

    最小三点只负责提出假设。最终 R、t 必须用最佳共识集重新做 SVD，并再次更新内点和质量指标。

From Unknown Pose to Coarse Alignment

原始点云应先全局粗配准，再交给 ICP

若 source 与 target 没有对应且相对位姿较大，最近邻通常是错的。全局阶段通过局部几何描述子和鲁棒一致性建立宽基线假设，目标不是做到最后一毫米，而是进入局部优化器的正确吸引域。

  1. 01

    统一数据契约

    确认 source / target 的方向、frame、米或毫米、时间戳、深度尺度与坐标轴；先裁掉任务无关背景。

  2. 02

    尺度匹配的预处理

    用与物体尺寸和传感器噪声匹配的 voxel 下采样；估计法向时记录半径、近邻数与朝向规则。

  3. 03

    建立宽基线候选

    计算 FPFH 或学习型局部描述子，互检特征对应；用 RANSAC、FGR 或其他鲁棒全局方法提出粗变换。

  4. 04

    几何一致性验证

    检查边长保持、法向夹角、最大对应距离、变换合法性、重叠区域和对称等价，而不只看特征相似度。

  5. 05

    多尺度局部精修

    从粗到细减小 voxel 与对应阈值；在点到面、GICP 或彩色 ICP 中配合鲁棒核与对应过滤。

  6. 06

    任务级验收

    报告 fitness、inlier RMSE、阈值、重叠、姿态误差与失败原因；需要时加入渲染、轮廓、碰撞或闭环一致性检查。

Canonical global-to-local chain

voxel downsample→normals→FPFH / learned feature→mutual matches→RANSAC / FGR→multi-scale ICP
REPETITION

重复结构

货架、齿轮、圆柱与规则孔阵列会产生大量局部外观相同的描述子，需要全局几何或对称感知。

OVERLAP

低重叠

描述子匹配数量可能很多，但真正来自公共可见表面的对应很少；要显式估计 overlap。

NORMAL

法向方向

FPFH 与 point-to-plane 都依赖法向。朝向翻转、半径错误或跨边界邻域会破坏匹配与优化。

SYMMETRY

多等价解

物体对称时多个变换都能同样对齐几何；任务应保留等价类，而不是把另一个正确对称姿态算作失败。

Local Registration

ICP 交替做对应与变换更新

经典 ICP 从当前位姿变换源点，寻找目标最近邻，过滤不可信对应，求解增量变换，再重复直到代价或位姿更新足够小。由于对应随位姿变化，整体目标非凸，初值直接决定会落入哪个吸引域。

Point-to-point ICP
min⁡R,t∑(i,j)∈K∥pj−(Rqi+t)∥22\min_{\mathbf R,\mathbf t}\sum_{(i,j)\in\mathcal K}\left\|\mathbf p_j-(\mathbf R\mathbf q_i+\mathbf t)\right\|_2^2
给定对应集 K 时可用闭式刚体对齐；每轮更新后重新找最近邻。它平等惩罚三个方向的偏差，适合无可靠法向或表面采样较均匀的情况。
Point-to-plane ICP
min⁡R,t∑(i,j)∈K[nj⊤(Rqi+t−pj)]2\min_{\mathbf R,\mathbf t}\sum_{(i,j)\in\mathcal K}\left[\mathbf n_j^\top(\mathbf R\mathbf q_i+\mathbf t-\mathbf p_j)\right]^2
只惩罚沿目标法向的分量，局部线性化后求 SE(3) 增量。在大平面上法向方向约束强，而沿平面切向仍可能弱，需要边界、颜色或其他结构补充。
① TRANSFORM

用当前 T 移动 source

→
② ASSOCIATE

最近邻或投影对应

→
③ REJECT

距离、法向、边界与互检

→
④ SOLVE

估计 ΔT 并复合

→
⑤ TEST

代价、增量与迭代上限

↺
方法输入主要目标初值优势主要风险
Kabsch / Orthogonal Procrustes已知、同序的 3D–3D 对应点到点平方误差不需要SVD 闭式解；适合无离群或 RANSAC 内点重估对应错误会直接污染解；必须处理 det(R) = −1
Umeyama已知、同序的 3D–3D 对应相似变换点到点平方误差不需要可同时估计尺度；常用于轨迹、NOCS 与跨尺度对齐度量点云通常应固定 s = 1；尺度会吸收错误单位或模型偏差
Point-to-point ICP两组原始点云最近邻点到点距离通常需要接近真值实现简单；不依赖法向沿平面滑动收敛慢，易受密度差、部分重叠和局部最优影响
Point-to-plane ICP点云 + 目标法向沿目标法向的有符号距离通常需要接近真值在光滑表面附近通常比点到点收敛快法向方向、邻域尺度和局部平面质量决定结果
Generalized ICP点云 + 局部协方差协方差加权的局部平面误差通常需要粗初值统一点到点与点到面思想，利用两侧局部结构协方差估计与邻域设置敏感，计算成本更高
Colored ICP带颜色点云 + 法向几何项 + 光度项通常需要粗初值几何退化区域可由颜色梯度补充切向约束曝光、白平衡、非朗伯反射和跨帧颜色差会误导优化
NDT源点云 + 目标体素高斯点落入局部概率分布的似然一般仍受初值影响目标被压缩为体素分布；常见于激光扫描匹配体素分辨率与优化步长决定平滑程度、速度和局部极值
FPFH + RANSAC / FGR下采样点云 + 法向 + 局部描述子特征一致性与几何一致性可从未知位姿开始为 ICP 提供全局或宽基线初值重复结构、低重叠与法向/尺度设置不当会产生伪匹配
CORRESPONDENCE GATE

最大距离不是越大越好

过小会在初值较差时没有对应；过大会把非重叠区域和错误表面拉入优化。多尺度逐步收紧更稳妥。

ROBUST LOSS

核函数抑制大残差

Huber、Cauchy、Tukey 等通过 IRLS 降低残余外点权重，但不能凭空制造正确对应或修复无重叠。

TRIMMING

只保留可信残差分位

已知大致重叠率时，可丢弃最大一部分残差；重叠率估计错误也会删除真对应或保留外点。

MULTI-SCALE

粗层扩大吸引域，细层补精度

较大 voxel 平滑局部几何并降低计算量，随后逐层减小 voxel 与对应阈值恢复细节。

Evaluation & Acceptance

没有阈值和重叠定义的配准分数不可复现

配准可以从对应残差、点云覆盖、真值位姿、表面距离和下游任务多个层面评估。不同层面回答的问题不同，应避免用一个漂亮的 RMSE 掩盖低重叠、对称或错误局部解。

Fitness and inlier RMSE
fitness⁡=∣Kτ∣∣Q∣,RMSE⁡inlier=1∣Kτ∣∑(i,j)∈Kτdij 2\operatorname{fitness}=\frac{|\mathcal K_\tau|}{|\mathcal Q|},\qquad \operatorname{RMSE}_{inlier}=\sqrt{\frac{1}{|\mathcal K_\tau|}\sum_{(i,j)\in\mathcal K_\tau}d_{ij}^{\,2}}
Kτ 是最大对应距离 τ 下的内点对应。fitness 高可能来自过大的 τ；RMSE 低也可能只覆盖很小区域。必须同时报告 τ、source 点数、采样尺度与 overlap 规则。
Pose error with ground truth
eR=cos⁡−1 ⁣(tr⁡(R^Rgt⊤)−12),et=∥t^−tgt∥2e_R=\cos^{-1}\!\left(\frac{\operatorname{tr}(\hat{\mathbf R}\mathbf R_{gt}^{\top})-1}{2}\right),\qquad e_t=\|\hat{\mathbf t}-\mathbf t_{gt}\|_2
物体存在旋转对称时，应在等价姿态集合上取最小误差，或使用对称感知表面指标。直接比较单一 Rgt 会把几何上等价的姿态误判为错误。
PAIR RESIDUAL

对应是否自洽

看 RMS、median、P90/P95 与残差直方图；平均值会掩盖长尾和局部错误。

COVERAGE

公共表面是否充分

双向最近邻、overlap 与空间分布共同检查，避免只对齐一个局部平面。

POSE GT

位姿是否恢复正确

在有真值时报告旋转、平移、尺度误差，并处理坐标方向和对称等价。

TASK CHECK

下游是否真正可用

重建看闭环与表面一致性；抓取看 TCP、碰撞与接近方向；6D pose 看 CAD 渲染和可见轮廓。

Recommended log contract

每次配准至少记录这些字段

source→target · units · voxel · normal radius · init source · solver · max correspondence · robust loss · iterations · inlier/all · fitness · RMSE/P95 · ΔR/Δt · overlap · stop reason

Across Application Domains

同一个配准核心，在四类系统中扮演不同角色

6D POSE · RGB-D

CAD / object → observed camera cloud

分割目标深度、反投影为相机点云，以网络位姿或全局特征作初值，再用几何/颜色 ICP 精修 CTO。必须处理可见表面与完整 CAD 的部分重叠。

VISUAL / LIDAR SLAM

scan → scan 或 scan → map

连续帧提供较好运动初值，ICP、GICP 或 NDT 恢复相对运动；错误匹配会累积为漂移，最终还需回环与 pose graph 优化。

ROBOTICS

标定、工件定位与多传感器外参

对应几何体或扫描之间的刚体变换进入 base–camera–tool 链。时间同步、结构挠曲和单位错误常比求解器误差更大。

GRASPING

model → scene 后迁移抓取

把离线 CAD 抓取位姿随 sceneTmodel 迁移到现场；局部表面对齐正确仍不保证物体全局姿态、碰撞和夹爪可达性正确。

Engineering recipe · Open3D Python

把全局候选与局部精修分开保存

下面只展示流水线边界。实际系统还应显式记录 voxel、法向/特征半径、RANSAC 检查器、最大对应距离和源目标方向。 
# source -> target
source_down, source_fpfh = preprocess(source, voxel)
target_down, target_fpfh = preprocess(target, voxel)

coarse = o3d.pipelines.registration.registration_ransac_based_on_feature_matching(
    source_down, target_down, source_fpfh, target_fpfh,
    mutual_filter=True,
    max_correspondence_distance=1.5 * voxel,
    estimation_method=o3d.pipelines.registration.TransformationEstimationPointToPoint(False),
    ransac_n=3,
    checkers=checkers,
    criteria=criteria,
)

fine = o3d.pipelines.registration.registration_icp(
    source, target, 0.4 * voxel, coarse.transformation,
    o3d.pipelines.registration.TransformationEstimationPointToPlane(),
)
DIRECTION

多数库返回把 source 送到 target 的 4×4 变换；把参数顺序交换后，结果应与原结果互逆，而不是只差平移符号。

NORMALS

point-to-plane 前必须确认目标法向存在、尺度合理且方向稳定；下采样后通常需要重新估计或正确聚合法向。

ARTIFACTS

保存 coarse、fine、每层指标和对齐点云。只保存最终矩阵会让全局失败与局部失败无法区分。

Debug Protocol

先验证一对已知对应，再扩大到整片点云

配准错误常来自方向、单位、深度反投影、点云筛选和法向，而不只是优化参数。建立从单点、已知变换、无离群闭式解到真实未知对应的分层测试，可以快速定位问题落在哪一层。

  1. 01

    打印 source / target 的 frame、单位和范围

    比较 AABB、质心、点间距中位数与深度范围。米/毫米错误会让所有阈值失效，也可能被误开的尺度估计暂时掩盖。

  2. 02

    用一个测试点验证变换方向

    手算 p = Rq+t，确认库输出确实是 source-to-target；再验证 T⁻¹(Tq)=q。

  3. 03

    先跑合成无噪声已知对应

    Kabsch 应把残差降到数值精度，det(R)≈+1。若失败，优先检查 H 的方向、矩阵乘法侧和质心平移。

  4. 04

    逐步加入噪声、错配和缺失

    一次只改变一个因素，记录内点率、姿态误差和退化样本数，避免在真实场景中同时猜十个参数。

  5. 05

    分别可视化 coarse 与 fine

    若 coarse 落在错误对称分支，ICP 精修得再漂亮也不会回到真值；若 coarse 正确而 fine 变坏，检查对应门限、法向和非重叠区域。

  6. 06

    画对应线并按残差着色

    长线集中在轮廓/遮挡处提示部分重叠；沿同一方向偏移提示外参或同步；跨重复结构连线提示描述子歧义。

  7. 07

    同时看任务几何与数值终止

    检查 CAD 渲染、表面覆盖、位姿跳变、碰撞或闭环，而不只接受 “converged=true”。

算法、实现与原始来源展开查看代表性论文和官方实现文档

Next systems topic · 08

通用位姿估计流程

下一专题将把检测、分割、几何线索、候选求解、位姿精修、置信度与跟踪串成完整 6D pose pipeline，并明确 RGB-only 与 RGB-D 分支在系统中的接入位置。
进入完整专题