第九篇 位姿优化问题 四：SVD求解ICP问题，坐标系转换亦可使用

一 普通ICP问题

1.1 问题描述

存在两组互相匹配的点集：
P={P1,P2,...,Pn}Q={Q1,Q2,...,Qn} P = \{P_1,P_2,...,P_n\} \newline Q = \{Q_1,Q_2,...,Q_n\} P={P1​,P2​,...,Pn​}Q={Q1​,Q2​,...,Qn​}
匹配点集存在如下关系：
$$Q_i=R\ast P_i+t$$

1.2 问题建模

如何求解R和t 呢？使用SVD分解法。
$$e_i=Q_i-(R\ast P_i+t)$$
构建最小二乘问题：
$$minJ=0.5\ast \sum _{i=1}^n||Q_i-(R\ast P_i+t)|{|}^2$$

1.3 问题求解

两个点集的质心描述如下：
$$\begin{gathered} Q_m=\frac{1}{n}\ast \sum _{i=1}^n{Q}_i \\ {P}_m=\frac{1}{n}\ast \sum _{i=1}^n{P}_i \end{gathered}$$
对最小二乘误差函数进行处理：
$$\begin{gathered} \sum _{i=1}^n||Q_i-(R\ast P_i+t)|{|}^2=\sum _{i=1}^n||Q_i-R\ast P_i-t-Q_m+Q_m-R\ast P_m+R\ast P_m|{|}^2 \\ =\sum _{i=1}^n||((Q_i-Q_m)-R\ast (P_i-P_m))+(Q_m-R\ast P_m-t)|{|}^2 \\ =\sum _{i=1}^n||((Q_i-Q_m)-R\ast (P_i-P_m))|{|}^2 \\ +\sum _{i=1}^n||(Q_m-R\ast P_m-t)|{|}^2 \\ +\sum _{i=1}^n((Q_i-Q_m)-R\ast (P_i-P_m))\ast (Q_m-R\ast P_m-t) \end{gathered}$$
注意要上式第三项结果累加求和之后为0，因此优化目标函数如下：
$$\begin{gathered} minJ==\sum _{i=1}^n||((Q_i-Q_m)-R\ast (P_i-P_m))|{|}^2 \\ +\sum _{i=1}^n||(Q_m-R\ast P_m-t)|{|}^2 \end{gathered}$$
这个式子，第二项中对于任意的R总能凑出t,使得第二项为零。因此目标函数如下：
$$\begin{gathered} minJ=\sum _{i=1}^n||((Q_i-Q_m)-R\ast (P_i-P_m))|{|}^2 \\ =\sum _{i=1}^n||q_i-R\ast p_i|{|}^2 \end{gathered}$$
参见 SLAM十四讲的求解证明
$$\begin{gathered} W=\sum _{i=1}^n{q}_i{p}_i^T \\ W=U\Sigma V^T \end{gathered}$$
于是得到结果：
$$\begin{gathered} R=(U{V}^T{)}^T \\ t=Q_m-R\ast P_m \end{gathered}$$

二 举一反三 坐标系转换

如果有两组坐标，分别互相对应，求出两个坐标系之间的关系，类似于ICP问题，亦可用SVD分解办法。