1 ICP [1]
1.1 问题描述
假设有一组配对好的点(即点与点之间的对应关系已知):
寻找一个欧氏变换\(R,t\),使得
这个问题可以用迭代最近点法(ICP)求解。其求解方式分为两种:
(1)利用线性代数求解(SVD)
(2)利用非线性优化求解
1.2 SVD方法
1.2.1 前置知识:奇异值分解(SVD)
1.2.1.1 一个m×n矩阵的奇异值
令A是m×n矩阵,那么\(A^TA\)是对称矩阵且可以正交对角化,则A的奇异值是\(A^TA\)的特征值的平方根,记为\(\sigma_1,...,\sigma_n\),且用递减顺序排列
1.2.1.2 奇异值分解 [2]
矩阵A的分解涉及一个m×n的“对角”矩阵\(\Sigma\),其形式为:
其中D是一个r×r矩阵,且\(r<=min\{m,n\}\)
定义:设A是秩为r的m × n矩阵,那么存在m×n矩阵\(\Sigma\),其中D的对角线元素是A的前r个奇异值,\(\sigma_1>=\sigma_2>=...\sigma_r\),并且存在一个m×m正交矩阵U和一个nxn正交矩阵V使得\(A=U \Sigma V^T\)
任何分解\(A=U \Sigma V^T\)称为A的一个奇异值分解(或SVD),其中U和V是正交矩阵,\(\Sigma\)中D具有正的对角线元素.矩阵U和V不是由A惟一确定的(即奇异值分解不惟一),但\(\Sigma\)的对角线元素必须是A的奇异值,.这样的一个分解中U的列称为的左奇异向量,而V的列称为A的右奇异向量.
1.2.1.3 求解步骤及示例
求
的一个奇异值分解
解:
第一步:将矩阵\(A^TA\)正交对角化,即求矩阵\(A^TA\)的特征值及其对应的特征向量的单位正交集。
矩阵\(A^TA\)的特征值是\(\lambda_1=360\),\(\lambda_1=90\),\(\lambda_3=0\),对应的单位特征向量是,
第二步:算出\(V\)和\(\Sigma\)。将\(A^TA\)的特征值按降序排列(在第一步已完成),对应的单位特征向量\(v_1,v_2,v_3\)是A的右奇异值向量,构造
特征值的平方根就是奇异值:\(\sigma_1=6\sqrt{10},\sigma_2=3\sqrt{10},\sigma_3=0\),非零奇异值为矩阵D对角线元素。构造矩阵\(\Sigma\)
第三步:构造U。由于\(A=U \Sigma V^T\),可解出\(U=AV\Sigma^T\)
所以A的奇异值分解是
1.2.1.4 编程实现
matlab
C++
1.2.2 求解过程
首先定义第i对点的误差项:
然后构建最小二乘问题,求使误差平方和达到最小的R,t(以下是手写的化简过程):
![](https://img2024.cnblogs.com/blog/1437041/202405/1437041-20240513100613414-775872754.jpg)
![](https://img2024.cnblogs.com/blog/1437041/202405/1437041-20240512213827844-258733610.jpg)
![](https://img2024.cnblogs.com/blog/1437041/202405/1437041-20240512213804622-1616548361.jpg)
1.3 非线性优化方法
参考资料: