ICP问题的最小二乘法详细推导

ICP求解

问题定义

输入：在坐标系一中$n$个坐标点 p ∈ { p 1 , p 2 , … , p n } \mathscr{p}\in\{p_1,p_2,\dots,p_n\} p∈{ p1​,p2​,…,pn​}，坐标系二中$n$个坐标点 q ∈ { q 1 , q 2 , ⋯   , q n } \mathscr{q}\in\{q_1,q_2,\cdots,q_n\} q∈{ q1​,q2​,⋯,qn​}。

输出：坐标系一到坐标系二的转换关系$R\in SO(3)、t\in {\mathbb{R}}^3$。

推导过程

位移推导

根据问题我们可以知道如下关系成立:
$$p_i=R{q}_i+t$$
于是我们可以建立一个优化问题：
$$(R,t)=\arg \underset{R\in SO(3),t\in {\mathbb{R}}^3}{\min }\sum _{i=1}^n{w}_i\Vert R{q}_i+t-p_i{\Vert }^2$$
其中，$w_i$表示每一项残差的贡献。将上式看成只关于$t$的函数：
$$F(t)=\arg \min \sum _{i=1}^n{w}_i\Vert R{q}_i+t-p_i{\Vert }^2$$
将上式对$t$求偏导得到:
∂ F ( t ) ∂ t = arg ⁡ min ⁡ 2 ∑ i = 1 n w i ( R q i + t − p i ) = arg ⁡ min ⁡ ( 2 R ∑ i = 1 n ( w i q i ) + 2 t ∑ i = 1 n ( w i ) − 2 ∑ i = 1 n ( w i p i ) ) = arg ⁡ min ⁡ ( 2 R q ˉ + 2 t − p ˉ ) = 0 \begin{aligned} \frac{\partial F(t)}{\partial t} &=\arg \min 2\sum_{i=1}^{n}w_i( Rq_i+t-p_i)\\ &=\arg \min \left( 2R\sum_{i=1}^{n}(w_iq_i) +2t\sum_{i=1}^{n}(w_i)-2\sum_{i=1}^{n}(w_ip_i) \right)\\ &=\arg \min \left( 2R\bar{q}+2t-\bar{p} \right)\\ &=0 \end{aligned} ∂t∂F(t)​​=argmin2i=1∑n​wi​(Rqi​+t−pi​)=argmin(2Ri=1∑n​(wi​qi​)+2ti=1∑n​(w