ICP算法对应点姿态估计算法推导

欢迎关注更多精彩
关注我，学习常用算法与数据结构，一题多解，降维打击。

背景

在icp算法中，一般一次迭代分两步，先找到对应的匹配点，然后计算一个RT将匹配点对齐。

第二步是比较难理解，推导一下实现原理。

问题描述

现在有配对好的两个点集

$P=p_1,\cdots ,p_n,P^{\prime }=p_1^{\prime },\cdots ,p_n^{\prime }$

想要找到一个R, t 使得

$\forall i,p_i=R{p}_i^{\prime }+t$

能量方程建立

可以通过优化的思路来求解建立能量方程

为了后续求解方程方便，带上了1/2

$E=\underset{R,t}{\min }\frac{1}{2}\sum _{i=1}^n\Vert p_i-(R{p}_i^{\prime }+t){\Vert }^2$

变量定义和基本定理

为了后续推导方便，先定义一些常量符号和基本定理

质心定义

$p=\frac{1}{n}\sum _{i=1}^n{p}_i,p^{\prime }=\frac{1}{n}\sum _{i=1}^n{p}_i^{\prime }$

去质心替换

$q_i=p_i-p,q_i^{\prime }=p_i^{\prime }-p^{\prime }$

Schwarz 不等式

对于向量A, B, A与B的内积不大于A与B模长的乘积。

$A\cdot B\le \Vert A\Vert \cdot \Vert B\Vert$

正交矩阵性质

$R{R}^T=1$

引理证明

后面会用到几个公式，这里先证明一下。

1 $q^{T}R{q}^{\prime }=tr(R{q}^{\prime }{q}^T)$

tr(A) 表示A的对角线元素的和

基本思路就是把等式左右两边展开，计算出最后结果即可

设 $R=\left[\begin{array}{ccc}{A}_{11}& A_{12}& A_{13}\\ A_{21}& A_{22}& A_{23}\\ A_{31}& A_{32}& A_{33}\end{array}\right]$

$q=\left[\begin{array}{c}{q}_1\\ q_2\\ q_3\end{array}\right],q^{\prime }=\left[\begin{array}{c}{q}_1^{\prime }\\ q_2^{\prime }\\ q_3^{\prime }\end{array}\right]$

证明:

等式左边

$q^{T}R{q}^{\prime }=q^T[\begin{array}{c}{A}_{11}\end{array}$