ICP-通过一组匹配的3D点估计相机运动
- 人工智能
- 2025-09-13 21:21:02
问题描述
假设有两组已匹配的3D点：
源点云： { p i ∈ R 3 } i = 1 N \{ \mathbf{p}_i \in \mathbb{R}^3 \}_{i=1}^N {pi∈R3}i=1N目标点云： { q i ∈ R 3 } i = 1 N \{ \mathbf{q}_i \in \mathbb{R}^3 \}_{i=1}^N {qi∈R3}i=1N目标是求解刚体变换旋转矩阵 R ∈ SO ( 3 ) \mathbf{R} \in \text{SO}(3) R∈SO(3) 和平移向量 t ∈ R 3 \mathbf{t} \in \mathbb{R}^3 t∈R3，使得变换后的源点云与目标点云对齐，即最小化以下目标函数： min R , t ∑ i = 1 N ∥ R p i + t − q i ∥ 2 . \min_{\mathbf{R}, \mathbf{t}} \sum_{i=1}^N \left\| \mathbf{R} \mathbf{p}_i + \mathbf{t} - \mathbf{q}_i \right\|^2. R,tmini=1∑N∥Rpi+t−qi∥2.
SVD方法 分析计算 1. 去中心化与平移向量的求解首先计算两组点的质心： μ p = 1 N ∑ i = 1 N p i , μ q = 1 N ∑ i = 1 N q i . \boldsymbol{\mu}_p = \frac{1}{N} \sum_{i=1}^N \mathbf{p}_i, \quad \boldsymbol{\mu}_q = \frac{1}{N} \sum_{i=1}^N \mathbf{q}_i. μp=N1i=1∑Npi,μq=N1i=1∑Nqi. 将每个点减去质心，得到去中心化的坐标： x i = p i − μ p , y i = q i − μ q . \mathbf{x}_i = \mathbf{p}_i - \boldsymbol{\mu}_p, \quad \mathbf{y}_i = \mathbf{q}_i - \boldsymbol{\mu}_q. xi=pi−μp,yi=qi−μq. 此时，目标函数可简化为： min R , t ∑ i = 1 N ∥ R x i − y i + ( t − μ q + R μ p ) ∥ 2 . \min_{\mathbf{R}, \mathbf{t}} \sum_{i=1}^N \left\| \mathbf{R} \mathbf{x}_i - \mathbf{y}_i + (\mathbf{t} - \boldsymbol{\mu}_q + \mathbf{R} \boldsymbol{\mu}_p) \right\|^2. R,tmini=1∑N Rxi−yi+(t−μq+Rμp) 2. 令括号内项为零，可得平移向量的最优解： t = μ q − R μ p . \mathbf{t} = \boldsymbol{\mu}_q - \mathbf{R} \boldsymbol{\mu}_p. t=μq−Rμp. 代入后，优化问题简化为仅关于旋转矩阵 R \mathbf{R} R 的最小化问题： min R ∑ i = 1 N ∥ R x i − y i ∥ 2 . \min_{\mathbf{R}} \sum_{i=1}^N \left\| \mathbf{R} \mathbf{x}_i - \mathbf{y}_i \right\|^2. Rmini=1∑N∥Rxi−yi∥2.
2. 目标函数的展开与迹形式转换
展开平方误差： ∑ i = 1 N ∥ R x i − y i ∥ 2 = ∑ i = 1 N ( R x i − y i ) ⊤ ( R x i − y i ) . \sum_{i=1}^N \left\| \mathbf{R} \mathbf{x}_i - \mathbf{y}_i \right\|^2 = \sum_{i=1}^N \left( \mathbf{R} \mathbf{x}_i - \mathbf{y}_i \right)^\top \left( \mathbf{R} \mathbf{x}_i - \mathbf{y}_i \right). i=1∑N∥Rxi−yi∥2=i=1∑N(Rxi−yi)⊤(Rxi−yi). 进一步展开并利用迹的线性性质： = ∑ i = 1 N ( x i ⊤ R ⊤ R x i − 2 y i ⊤ R x i + y i ⊤ y i ) . = \sum_{i=1}^N \left( \mathbf{x}_i^\top \mathbf{R}^\top \mathbf{R} \mathbf{x}_i - 2 \mathbf{y}_i^\top \mathbf{R} \mathbf{x}_i + \mathbf{y}_i^\top \mathbf{y}_i \right). =i=1∑N(xi⊤R⊤Rxi−2yi⊤Rxi+yi⊤yi). 由于 R ⊤ R = I \mathbf{R}^\top \mathbf{R} = \mathbf{I} R⊤R=I，第一项简化为 x i ⊤ x i \mathbf{x}_i^\top \mathbf{x}_i xi⊤xi，与 R \mathbf{R} R 无关。优化问题等价于最大化交叉项： max R ∑ i = 1 N y i ⊤ R x i = max R tr ( R ⊤ ∑ i = 1 N y i x i ⊤ ) . \max_{\mathbf{R}} \sum_{i=1}^N \mathbf{y}_i^\top \mathbf{R} \mathbf{x}_i = \max_{\mathbf{R}} \text{tr}\left( \mathbf{R}^\top \sum_{i=1}^N \mathbf{y}_i \mathbf{x}_i^\top \right). Rmaxi=1∑Nyi⊤Rxi=Rmaxtr(R⊤i=1∑Nyixi⊤). 定义协方差矩阵： H = ∑ i = 1 N y i x i ⊤ . \mathbf{H} = \sum_{i=1}^N \mathbf{y}_i \mathbf{x}_i^\top. H=i=1∑Nyixi⊤.
3. 通过SVD求解旋转矩阵
对协方差矩阵 H \mathbf{H} H 进行奇异值分解（SVD）： H = U Σ V ⊤ . \mathbf{H} = \mathbf{U} \mathbf{\Sigma} \mathbf{V}^\top. H=UΣV⊤. 目标函数可写为： tr ( R ⊤ H ) = tr ( R ⊤ U Σ V ⊤ ) . \text{tr}(\mathbf{R}^\top \mathbf{H}) = \text{tr}(\mathbf{R}^\top \mathbf{U} \mathbf{\Sigma} \mathbf{V}^\top). tr(R⊤H)=tr(R⊤UΣV⊤). 利用迹的循环置换性质 ( tr ( A B C ) = tr ( B C A ) (\text{tr}(ABC) = \text{tr}(BCA) (tr(ABC)=tr(BCA)，可重写为： tr ( V ⊤ R ⊤ U Σ ) . \text{tr}(\mathbf{V}^\top \mathbf{R}^\top \mathbf{U} \mathbf{\Sigma}). tr(V⊤R⊤UΣ). 令 M = V ⊤ R ⊤ U \mathbf{M} = \mathbf{V}^\top \mathbf{R}^\top \mathbf{U} M=V⊤R⊤U，则目标函数简化为： tr ( M Σ ) . \text{tr}(\mathbf{M} \mathbf{\Sigma}). tr(MΣ). 由于 M \mathbf{M} M 是正交矩阵 ( M ⊤ M = I (\mathbf{M}^\top \mathbf{M} = \mathbf{I} (M⊤M=I），其对角线元素绝对值不超过1。为了使 tr ( M Σ ) \text{tr}(\mathbf{M} \mathbf{\Sigma}) tr(MΣ) 最大，需使 M \mathbf{M} M的对角线元素尽可能大。当 M = I \mathbf{M} = \mathbf{I} M=I 时，迹达到最大值 tr ( Σ ) \text{tr}(\mathbf{\Sigma}) tr(Σ)，即： M = I ⟹ V ⊤ R ⊤ U = I . \mathbf{M} = \mathbf{I} \implies \mathbf{V}^\top \mathbf{R}^\top \mathbf{U} = \mathbf{I}. M=I⟹V⊤R⊤U=I. 解得旋转矩阵： R ⊤ = V U ⊤ ⟹ R = U V ⊤ . \mathbf{R}^\top = \mathbf{V} \mathbf{U}^\top \implies \mathbf{R} = \mathbf{U} \mathbf{V}^\top. R⊤=VU⊤⟹R=UV⊤.
验证行列式：
若 det ( R ) = 1 \det(\mathbf{R}) = 1 det(R)=1，解有效。若 det ( R ) = − 1 \det(\mathbf{R}) = -1 det(R)=−1（存在反射），需修正为： R = V [ 1 0 0 0 1 0 0 0 − 1 ] U ⊤ . \mathbf{R} = \mathbf{V} \begin{bmatrix} 1 & 0 & 0 \\ 0 & 1 & 0 \\ 0 & 0 & -1 \end{bmatrix} \mathbf{U}^\top. R=V 10001000−1 U⊤. 4. 迭代优化（可选）若点对匹配不完美或存在噪声，可多次迭代：
用当前 R , t \mathbf{R}, \mathbf{t} R,t 变换源点云： p i ′ = R p i + t \mathbf{p}'_i = \mathbf{R} \mathbf{p}_i + \mathbf{t} pi′=Rpi+t。重新计算误差并更新变换，直至收敛。5. 公式总结 质心计算： μ p = 1 N ∑ i = 1 N p i , μ q = 1 N ∑ i = 1 N q i . \boldsymbol{\mu}_p = \frac{1}{N} \sum_{i=1}^N \mathbf{p}_i, \quad \boldsymbol{\mu}_q = \frac{1}{N} \sum_{i=1}^N \mathbf{q}_i. μp=N1i=1∑Npi,μq=N1i=1∑Nqi.协方差矩阵： H = ∑ i = 1 N ( q i − μ q ) ( p i − μ p ) ⊤ . \mathbf{H} = \sum_{i=1}^N (\mathbf{q}_i - \boldsymbol{\mu}_q)(\mathbf{p}_i - \boldsymbol{\mu}_p)^\top. H=i=1∑N(qi−μq)(pi−μp)⊤.SVD分解与旋转矩阵： H = U Σ V ⊤ , R = V U ⊤ ( 确保 det ( R ) = 1 ) . \mathbf{H} = \mathbf{U} \mathbf{\Sigma} \mathbf{V}^\top, \quad \mathbf{R} = \mathbf{V} \mathbf{U}^\top \quad (\text{确保} \det(\mathbf{R}) = 1). H=UΣV⊤,R=VU⊤(确保det(R)=1).平移向量： t = μ q − R μ p . \mathbf{t} = \boldsymbol{\mu}_q - \mathbf{R} \boldsymbol{\mu}_p. t=μq−Rμp.
几何解释
通过SVD分解，协方差矩阵 H \mathbf{H} H 的奇异向量反映了点云之间的主要对齐方向。最优旋转矩阵 R \mathbf{R} R 通过最大化点对之间的协方差，使得变换后的源点云与目标点云在最小二乘意义下对齐。平移向量 t \mathbf{t} t 则通过质心差校正整体偏移。
算法步骤总结 去中心化：计算质心并减去，得到 x i \mathbf{x}_i xi和 y i \mathbf{y}_i yi。构建协方差矩阵： H = ∑ y i x i ⊤ \mathbf{H} = \sum \mathbf{y}_i \mathbf{x}_i^\top H=∑yixi⊤。SVD分解： H = U Σ V ⊤ \mathbf{H} = \mathbf{U} \mathbf{\Sigma} \mathbf{V}^\top H=UΣV⊤。求解旋转矩阵： R = V U ⊤ \mathbf{R} = \mathbf{V} \mathbf{U}^\top R=VU⊤，调整行列式。计算平移向量： t = μ q − R μ p \mathbf{t} = \boldsymbol{\mu}_q - \mathbf{R} \boldsymbol{\mu}_p t=μq−Rμp。
代码示例（Python） import numpy as np def solve_icp(source_points, target_points): # 输入：source_points (n×3), target_points (n×3) # 输出：R (3×3), t (3×1) # 计算质心 mu_p = np.mean(source_points, axis=0) mu_q = np.mean(target_points, axis=0) # 去中心化 X = source_points - mu_p Y = target_points - mu_q # 协方差矩阵 H = X.T @ Y # SVD分解 U, S, Vt = np.linalg.svd(H) # 计算旋转矩阵 R = Vt.T @ U.T # 处理反射情况 if np.linalg.det(R) < 0: Vt[-1, :] *= -1 R = Vt.T @ U.T # 计算平移向量 t = mu_q - R @ mu_p return R, t # 示例数据 source = np.array([[1, 2, 3], [4, 5, 6], [7, 8, 9]]) target = np.array([[2, 3, 4], [5, 6, 7], [8, 9, 10]]) R, t = solve_icp(source, target) print("Rotation Matrix:\n", R) print("Translation Vector:\n", t)
关键点 闭式解：通过SVD直接求解最优变换，无需迭代（适用于精确匹配）。反射问题：需检查 det ( R ) \det(\mathbf{R}) det(R)，避免反射变换。迭代优化：若存在噪声或匹配误差，需多次迭代优化。
通过上述步骤，可高效求解已知匹配3D点对的刚体变换，适用于点云配准、物体位姿估计等场景。
ICP-通过一组匹配的3D点估计相机运动由讯客互联人工智能栏目发布，感谢您对讯客互联的认可，以及对我们原创作品以及文章的青睐，非常欢迎各位朋友分享到个人网站或者朋友圈，但转载请说明文章出处“ICP-通过一组匹配的3D点估计相机运动”
