利用SVD的方法求解ICP（详细推导）

最新推荐文章于 2021-12-18 20:27:27 发布
最新推荐文章于 2021-12-18 20:27:27 发布
阅读量2k 收藏 2
点赞数
CC 4.0 BY-SA版权
分类专栏： 算法
原文链接：http://nghiaho.com/?page_id=363
视觉SLAM笔记（39） 求解 ICP
氢键H-H
10-17 1万+
SVD 方法和非线性优化方法求解 ICP
ICP问题的最小二乘法详细推导
qq_38555897的博客
05-01 1442
ICP问题最小二乘法详细推导
ICP算法公式推导
Coulson的博客
11-18 5833
一、引言 ICP算法可以用于匹配两个点云，得到两个点云之间的位姿转换关系。本文主要介绍已知对应点的ICP匹配算法。 二、ICP算法 首先计算两个点云的质心： 然后将原始点云减去对应的质心，得到去除质心的点云： 计算新产生的两个点云形成的W矩阵，并SVD分解： 那么，两个原始点云的转换关系R和t如下所示： 三、推导过程 对于两组点云： 求解R和t，使得下式最小： 上式进行变换： 其中，...
SVD推导
qq_34116958的博客
07-09 331
SVD与PCA的关系 PCA的全部工作简单点说，就是对原始的空间中顺序地找一组相互正交的坐标轴，第一个轴是使得方差最大的，第二个轴是在与第一个轴正交的平面中使得方差最大的，第三个轴是在与第1、2个轴正交的平面中方差最大的，这样假设在N维空间中，我们可以找到N个这样的坐标轴，我们取前r个去近似这个空间，这样就从一个N维的空间压缩到r维的空间了，但是我们选择的r个坐标轴能够使得空间的压缩使得数据的...
ICP 问题之 SVD
sinoai
01-03 3356
欢迎访问我的博客首页。 ICP 之 SVD1. SVD法1.1 构造最小二乘问题1.2 求解最小二乘问题1.1 求旋转矩阵1.2 求平移向量1.3 例子2. 使用程序计算2.1 源码2.2 配置文件   ICP 即 Iterative Closest Point，迭代最近点。已知三维点在两个坐标系中的坐标，求这两个坐标系的变换关系，称为 ICP 问题。 1. SVD法 1.1 构造最小二乘问题   假设从空间坐标系 O1O_1O1​ 到空间坐标系 O2O_2O2​ 的变换是 (R,t⃗ )(R, \.
ICP算法详细推导过程--SVD方法(包含SVD的分解过程)
weixin_43628032的博客
12-18 3353
现有两组点云P,Q： 求两组点云的质心： 以质心为坐标原点的点云的坐标为: 求解R，t使得误差函数E(R,t)最小： 又： 所以： E(R,t)要为最小，则 ，即 则最优旋转矩阵为： 所以要使R取得最优，则： ...
svd求解点云旋转矩阵方法,利用SVD的方法求解ICP（详细推导）
weixin_36167031的博客
03-27 1393
引用资料(理论部分其实就是把第一个的不详细和错误的地方说了一下，翻译了一下第二个文献以及不明了的地方说一下O(∩_∩)O哈哈~)：高翔《视觉SLAM十四讲》K. S. ARUN, T. S. HUANG, AND S. D. BLOSTEINLeast-Squares Fitting of Two 3-D Point Sets问题描述假设存在两个点云集合和求：一个欧氏变换使得求解问题解：假设误差项...
svd求解点云旋转矩阵方法_使用 SVD 方法求解 ICP 问题
weixin_28744573的博客
01-17 1724
本文是结合《Least-Squares Rigid Motion Using SVD》和《Least-Squares Fitting of Two 3-D Point Sets》两篇文章写的一个总结，里面有一些是自己的理解不一定正确。1 问题定义假设我们有两个点云集合 \mathcal{P}=\left\{\mathbf{p}_{1}, \mathbf{p}_{2}, \ldots, \mathb...
ICP的不同解法：SVD分解，高斯牛顿法。代码实现
Limiao_123的博客
04-13 4141
ICP （Iterative Closest Point，最近迭代）是点云配准中的一个很重要的算法。假设有两个点云P和Q，想要找到一个变换T，使得P=TQ，那么就需要使用到这个算法。 1.最近邻查找 这一步是为Q中的点Qi找到Q中相对应的点。 这个要分情况讨论：比如在RGBD-slam中，特征点的匹配已经为我们找到了P、Q中点的对应关系，那么这一步其实是已经完成了。但是对于两坨点云，没有点和点的对应关系，我们只好先做这样一个假设：离Qi最近的P中的点Pi就是它对应的点，显然这个是不准确的，但是我们可以通过
SVD详细介绍及理论推导和应用举例
09-11
SVD详细介绍及理论推导和应用举例，是了解SVD理论的很好的参考资料
基于SVD-ICP算法配准CT切片重构模型与CAD模型.pdf
08-05
基于SVD-ICP算法配准CT切片重构模型与CAD模型.pdf
svd分解详解过程与原理
11-21
svd分解详解过程与原理，清楚描述了svd分解的原理和作用
ICP（Iterative Closest Point）算法实现源码
07-14
通过C++实现了ICP算法的点云匹配过程，内容包括KdTree搜索算法和SVD算法的实现源码，希望给大家带来参考。
ICP算法推导详解
mystyle_的博客
10-29 768
知乎https://zhuanlan.zhihu.com/p/104735380
算法公式推导——ICP
sunqin的博客
05-16 1684
前言：点云配准中的迭代最近点（Iterative Closest Points，ICP）算法的推导，网上很多资料都有提到，但是对我来说，看完这些资料还是有写些太明白的地方，前段时间学习的相关课程中讲到了ICP的算法推导，感觉很详细，因此这里详细总结一下，确保任何一个有线性代数基础的小伙伴都能看懂。 1，算法输入 两堆点集：X={x1,x2,...,xM}\mathbf{X} = \{x_1, x_2, ..., x_M\}X={x1​,x2​,...,xM​} 和 Y={y1,y2,...,yN}\math
ICP算法公式推导和PCL源码解析
supengufo的博客
05-30 3720
ICP算法可以使用SVD或者非线性优化的方法进行求解。因为PCL源码中对ICP的求解就是使用SVD，所以这里对其进行一些探究。 1.公式推导 1.首先对第iii个点构建误差项： ei=pi−(Rpi′+t) e_{i}=p_{i}-\left(\boldsymbol{R} \boldsymbol{p}_{i}^{\prime}+\boldsymbol{t}\right) ei​=pi​−(Rpi′​+t) 2.构建最小二乘问题：求得使得目标函数最小的未知量R,tR,tR,t min⁡R,tJ=12∑
SVD奇异值分解推导过程
qq_35181586的博客
05-29 180
点云icp算法原理以及构成匹配点对的公式推导
最新发布
05-13
<think>好的，我现在需要帮助用户理解点云ICP算法的原理以及匹配点对公式的推导过程。用户之前已经询问过这个问题，现在再次提出，可能是因为之前的回答还有不清楚的地方，或者用户想深入了解更多细节。首先，我要回顾之前的回答，确保这次的内容保持一致，同时补充可能遗漏的信息。 首先，ICP算法的核心是迭代优化旋转矩阵R和平移向量t，最小化两个点云之间的距离。用户提供的引用中提到了ICP的基本流程：最近邻匹配和位姿优化。我需要确保在解释时涵盖这两个步骤，并详细说明公式推导，特别是如何通过SVD分解求解R和t。 用户引用的资料提到质心去中心化的步骤，这是简化目标函数的关键。我需要解释为什么这一步有效，即通过将点云移动到质心，可以分离旋转和平移的优化问题。同时，用户引用的资料3中提到ICP对初始位置敏感，容易陷入局部最优，这一点也需要在原理部分强调，说明ICP的局限性。 在公式推导部分，用户提到Point-to-Point和Point-to-Plane的区别。引用2指出Point-to-Plane考虑了局部结构，但导致非线性问题。我需要比较这两种方法，但用户当前的问题主要集中在Point-to-Point的推导上，所以应详细展开这一点，同时简要提及变体作为补充。 用户的引用4提到了通过最大化迹来求解R，这里涉及SVD分解和正交矩阵的性质。我需要详细解释如何通过协方差矩阵H的SVD分解得到R，并确保R的正交性，可能涉及行列式的调整。同时，引用3中的代码示例显示ICP在实际应用中的收敛问题，这可能与初始位置有关，需要在原理中说明。 用户可能存在的疑问点包括：为什么使用SVD？如何保证R是旋转矩阵而不是反射矩阵？质心去中心化的数学依据是什么？我需要确保在推导过程中逐步解答这些问题，避免过于跳跃。 此外，用户可能对目标函数的分解存在疑问，即如何将原始目标函数分解为旋转相关项和平移相关项。需要详细展示分解过程，说明平移项如何通过质心消除，从而简化优化问题。 最后，用户可能需要了解ICP的收敛性和应用中的注意事项，如初始估计的重要性。虽然用户的问题集中在原理和推导，但简要提及这些实际应用中的问题可以增强回答的实用性。 总结下来，我需要结构清晰地分步骤讲解ICP原理，详细推导目标函数，解释质心去中心化的作用，SVD求解R的过程，以及如何计算t。同时，指出算法的局限性和变体，确保用户全面理解。</think>#### 点云ICP算法原理与匹配点对公式推导详解 --- ### 一、算法核心思想 迭代最近点（Iterative Closest Points, ICP）通过**迭代优化**解决两组点云$X=\{\mathbf{x}_i\}$和$P=\{\mathbf{p}_i\}$的刚性配准问题，目标是找到最优的**旋转矩阵$R$**和**平移向量$\mathbf{t}$**，使得以下误差最小： $$ \sum_{i=1}^n \| R\mathbf{x}_i + \mathbf{t} - \mathbf{p}_i \|^2 $$ 其核心流程包括两步迭代： 1. **最近邻匹配**：为源点云$X$中的每个点$\mathbf{x}_i$，在目标点云$P$中找到最近点$\mathbf{p}_i$； 2. **位姿优化**：通过最小二乘法更新$R$和$\mathbf{t}$，使对齐误差最小化[^1]。 --- ### 二、公式推导（Point-to-Point ICP） #### 1. 目标函数构建 定义误差函数为所有对应点对的欧氏距离平方和： $$ E(R,\mathbf{t}) = \sum_{i=1}^n \| R\mathbf{x}_i + \mathbf{t} - \mathbf{p}_i \|^2 $$ 其中$R$需满足正交性约束$R^T R = I$且$\det(R)=1$[^3]。 #### 2. 去质心简化问题 引入**质心**$\boldsymbol{\mu}_x = \frac{1}{n}\sum \mathbf{x}_i$和$\boldsymbol{\mu}_p = \frac{1}{n}\sum \mathbf{p}_i$，将误差函数分解为： $$ E = \sum_{i=1}^n \| R(\mathbf{x}_i - \boldsymbol{\mu}_x) - (\mathbf{p}_i - \boldsymbol{\mu}_p) \|^2 + n\| R\boldsymbol{\mu}_x + \mathbf{t} - \boldsymbol{\mu}_p \|^2 $$ 通过令$\mathbf{t} = \boldsymbol{\mu}_p - R\boldsymbol{\mu}_x$，第二项被消除，只需优化第一项： $$ \min_R \sum_{i=1}^n \| R(\mathbf{x}_i - \boldsymbol{\mu}_x) - (\mathbf{p}_i - \boldsymbol{\mu}_p) \|^2 $$ #### 3. 旋转矩阵求解 定义协方差矩阵$H$： $$ H = \sum_{i=1}^n (\mathbf{x}_i - \boldsymbol{\mu}_x)(\mathbf{p}_i - \boldsymbol{\mu}_p)^T $$ 目标转化为最大化$\text{Tr}(RH)$。对$H$进行**奇异值分解（SVD）**： $$ H = U \Sigma V^T $$ 最优旋转矩阵为： $$ R = V U^T $$ 若$\det(R) = -1$（即反射矩阵），需将$V$的最后一列取反以保证$R$为纯旋转矩阵[^4]。 #### 4. 平移向量计算 直接通过质心关系求得： $$ \mathbf{t} = \boldsymbol{\mu}_p - R\boldsymbol{\mu}_x $$ --- ### 三、数学推导关键步骤（详细版） #### 步骤1：误差函数展开 $$ \| R\mathbf{x}_i + \mathbf{t} - \mathbf{p}_i \|^2 = (R\mathbf{x}_i + \mathbf{t} - \mathbf{p}_i)^T (R\mathbf{x}_i + \mathbf{t} - \mathbf{p}_i) $$ 展开后包含交叉项： $$ \mathbf{x}_i^T R^T R \mathbf{x}_i + 2\mathbf{t}^T (R\mathbf{x}_i - \mathbf{p}_i) + \|\mathbf{t} - \mathbf{p}_i\|^2 $$ #### 步骤2：质心去中心化 令$\mathbf{x}_i' = \mathbf{x}_i - \boldsymbol{\mu}_x$，$\mathbf{p}_i' = \mathbf{p}_i - \boldsymbol{\mu}_p$，目标函数简化为： $$ \sum_{i=1}^n \| R\mathbf{x}_i' - \mathbf{p}_i' \|^2 $$ 展开后得： $$ \text{Tr}(R H) = \sum_{i=1}^n \mathbf{x}_i'^T R^T \mathbf{p}_i' $$ #### 步骤3：SVD分解的几何意义 最大化$\text{Tr}(RH)$等价于寻找$R$使得两去质心点云的分布方向对齐。SVD分解通过正交矩阵$U$和$V$描述两组点云的主轴方向，$\Sigma$表示缩放因子[^4]。 --- ### 四、算法特性与局限性 1. **优点**：计算直观，精度较高； 2. **缺点**： - 依赖初始位姿，易陷入局部最优（如目标点云平移较大时可能不收敛[^3]）； - 计算效率受最近邻搜索影响； 3. **改进方向**： - 使用Point-to-Plane误差（需解非线性优化问题[^2]）； - 引入鲁棒损失函数（如Huber损失）抑制异常值。 --- ### 五、算法流程总结 1. **输入**：源点云$X$、目标点云$P$、最大迭代次数； 2. **初始化**：设$R_0=I$，$\mathbf{t}_0=0$； 3. **迭代**： - 最近邻搜索建立对应点对； - 计算质心$\boldsymbol{\mu}_x$和$\boldsymbol{\mu}_p$； - 构建协方差矩阵$H$并进行SVD分解； - 更新$R$和$\mathbf{t}$； 4. **终止条件**：误差变化小于阈值或达到最大迭代次数。 --- ### 六、示例代码（关键步骤） ```python import numpy as np def icp_step(source, target): # 计算质心 mu_s = np.mean(source, axis=0) mu_t = np.mean(target, axis=0) # 去质心 src_centered = source - mu_s tgt_centered = target - mu_t # 构建协方差矩阵H H = src_centered.T @ tgt_centered # 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_t - R @ mu_s return R, t ``` --- ### 相关问题 1. **为什么ICP算法需要依赖初始位姿？** ICP通过局部迭代优化，若初始位姿偏离真实值过大，最近邻匹配可能错误，导致优化陷入局部最优[^3]。 2. **如何加速最近邻搜索过程？** 常用方法包括KD-Tree、Octree或近似最近邻（ANN）算法。 3. **Point-to-Plane ICP为何能提高精度？** 其优化目标为点到目标点所在平面的距离，利用了局部表面法向量信息[^2]。
评论
添加红包

请填写红包祝福语或标题

个

红包个数最小为10个

元

红包金额最低5元

当前余额3.43元 前往充值 >
需支付：10.00元
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付元
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明：

1.余额是钱包充值的虚拟货币，按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载，可以购买VIP、付费专栏及课程。

余额充值