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\left[\begin{array}{c}{A}_{11}{q}_1^{\prime }+A_{12}{q}_2^{\prime }+A_{13}{q}_3^{\prime }\\ A_{21}{q}_1^{\prime }+A_{22}{q}_2^{\prime }+A_{23}{q}_3^{\prime }\\ A_{31}{q}_1^{\prime }+A_{32}{q}_2^{\prime }+A_{33}{q}_3^{\prime }\end{array}\right]$

$=A_{11}{q}_1{q}_1^{\prime }+A_{12}{q}_1{q}_2^{\prime }+A_{13}{q}_1{q}_3^{\prime }+A_{21}{q}_2{q}_1^{\prime }+A_{22}{q}_2{q}_2^{\prime }+A_{23}{q}_2{q}_3^{\prime }+A_{31}{q}_3{q}_1^{\prime }+A_{32}{q}_3{q}_2^{\prime }+A_{33}{q}_3{q}_3^{\prime }$

等式右边

$R{q}^{\prime }{q}^T=\left[\begin{array}{c}{A}_{11}{q}_1^{\prime }+A_{12}{q}_2^{\prime }+A_{13}{q}_3^{\prime }\\ A_{21}{q}_1^{\prime }+A_{22}{q}_2^{\prime }+A_{23}{q}_3^{\prime }\\ A_{31}{q}_1^{\prime }+A_{32}{q}_2^{\prime }+A_{33}{q}_3^{\prime }\end{array}\right]q^T$

$=\left[\begin{array}{ccc}{A}_{11}{q}_1{q}_1^{\prime }+A_{12}{q}_1{q}_2^{\prime }+A_{13}{q}_1{q}_3^{\prime }& A_{11}{q}_2{q}_1^{\prime }+A_{12}{q}_2{q}_2^{\prime }+A_{13}{q}_2{q}_3^{\prime }& A_{11}{q}_3{q}_1^{\prime }+A_{12}{q}_3{q}_2^{\prime }+A_{13}{q}_3{q}_3^{\prime }\\ A_{21}{q}_1{q}_1^{\prime }+A_{22}{q}_1{q}_2^{\prime }+A_{23}{q}_1{q}_3^{\prime }& A_{21}{q}_2{q}_1^{\prime }+A_{22}{q}_2{q}_2^{\prime }+A_{23}{q}_2{q}_3^{\prime }& A_{21}{q}_3{q}_1^{\prime }+A_{22}{q}_3{q}_2^{\prime }+A_{23}{q}_3{q}_3^{\prime }\\ A_{31}{q}_1{q}_1^{\prime }+A_{32}{q}_1{q}_2^{\prime }+A_{33}{q}_1{q}_3^{\prime }& A_{31}{q}_2{q}_1^{\prime }+A_{32}{q}_2{q}_2^{\prime }+A_{33}{q}_2{q}_3^{\prime }& A_{31}{q}_3{q}_1^{\prime }+A_{32}{q}_3{q}_2^{\prime }+A_{33}{q}_3{q}_3^{\prime }\end{array}\right]$

提取出对角线元素刚好与左边相等。

2 M是一个对称正定矩阵，那么对于任意正交矩阵R, 会有tr(M)>=tr(RM)

证明

可以设$M=A{A}^T$

$tr(RA{A}^T)=tr(A^{T}RA)=\sum a_i^T(R{a}_i)$

根据schwarz 不等式

$\sum a_i^T(R{a}_i)\le \sum \sqrt{a_i^T{a}_i}\sqrt{(a_i^T{R}^{T}R{a}_i)}=\sum a_i^T{a}_i=tr(M)$

定理得证。

3 等式 $\sum _{i=1}^n(p_i-p-R(p_i^{\prime }-p^{\prime }))=0$

对上式展开得到

$(\sum _{i=1}^n{p}_i)-np-R(\sum _{i=1}^n{p}_i^{\prime })+nR{p}^{\prime }$

所有pi加起来就是np，np’同理，所以上式等于0。

能量方程化简

利用构造法化简

1/2常数项先拿掉

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

$=\sum _{i=1}^n\Vert (p_i-p-R(p_i^{\prime }-p^{\prime }))+(p-R{p}^{\prime }-t){\Vert }^2$

利用 $\Vert \mathbf{a}+\mathbf{b}{\Vert }^2={\mathbf{a}}^T\mathbf{a}+2\mathbf{a}\mathbf{b}+{\mathbf{b}}^T\mathbf{b}$二次多项式展开

$=\sum _{i=1}^n(\Vert p_i-p-R(p_i^{\prime }-p^{\prime }){\Vert }^2+\Vert (p-R{p}^{\prime }-t){\Vert }^2)+\sum _{i=1}^n(2(p_i-p-R(p_i^{\prime }-p^{\prime }){)}^T(p-R{p}^{\prime }-t))$

上式中根据定义引理3 后面一项结果为0.

能量方程转化为：

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

上述方程中，对于给定的R，总是可以找到一个t使得后一项为0, 那只要优化前一项就可以了。

$J=\underset{R}{\min }\frac{1}{2}\sum _{i=1}^n\Vert p_i-p-R(p_i^{\prime }-p^{\prime }){\Vert }^2＝\underset{R}{\min }\frac{1}{2}\sum _{i=1}^n\Vert q_i-R{q}_i^{\prime }{\Vert }^2$

$=\underset{R}{\min }\frac{1}{2}{\sum }_{i=1}^n(q_i^T{q}_i+q_i^{\prime T}{R}^{T}R{q}_i^{\prime }-2q_i^{T}R{q}_i^{\prime })$

优化求解

上式中第1，2项为常数，只要优化第三项就可以了。

根据引理1（去负号时优化最小值变成优化最大值）

$-\underset{R}{\min }\sum _{i=1}^n(q_i^{T}R{q}_i^{\prime })＝\underset{R}{\max }\sum _{i=1}^{n}tr(R{q}_i^{\prime }{q}_i^T)=\underset{R}{\max }tr(R\sum _{i=1}^n(q_i^{\prime }{q}_i^T))$

根据引理2求解R

令
$H=\sum _{i=1}^n(q_i^{\prime }{q}_i^T)=U\Sigma V^T,X=V{U}^T为正交矩阵$

$XH=V{U}^{T}U\Sigma V^T,X=V\Sigma V^T$

$XH为对称正定矩阵$

设B为正交矩阵, BX也为正交矩阵

则根据引理2 可得

tr(XH)>=tr(BXH)

可知J方程的最优解为 R＝BX＝X。

算法步骤

1. 根据定义主算出p, p。
2. 根据定义计算矩阵H。
3. 对H进行svd分解。
4. 求解出R。（当R的行列式值为负数时，取-R作为最优解）
5. t = p-Rp。

---

本人码农，希望通过自己的分享，让大家更容易学懂计算机知识。创作不易，帮忙点击公众号的链接，帮忙转发，感激不尽。