第九篇 位姿优化问题 四：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 问题求解

两个点集的质心描述如下：
$$Q_m=$$