两种常见的点云配准方法ICP&NDT

迭代最近点算法ICP(Iterative Closest Point)

问题描述

  假设我们有两组点集，注意这里的$\mathbf{P}$和$\mathbf{Q}$分别相对于变换前和变换后的相机参考系。我们要解决的问题是找一组$\mathbf{R}$和$\mathbf{T}$，使得$\mathbf{P}$中的每一个点经过变化后同$\mathbf{Q}$中的最近点的误差之和最小。用数学的话描述就是最小化如下一个目标函数：
$$\frac{1}{2}\sum _{i=1}^n\Vert {\mathbf{q}}_i-\mathbf{R}{\mathbf{p}}_i-\mathbf{T}{\Vert }^2\text{\hspace{1em}(1.1)}$$
求解的方法有很多，这里只介绍SVD方法

目标函数简化

我们定义两组点集的中心为
$${\mu }_p=\sum _{i=1}^n{\mathbf{p}}_i\text{\hspace{1em}(1.2)}$$
$${\mu }_q=\sum _{i=1}^n{\mathbf{q}}_i\text{\hspace{1em}(1.3)}$$
$$\begin{gathered} \Vert {\mathbf{q}}_i-\mathbf{R}{\mathbf{p}}_i-\mathbf{T}{\Vert }^2=\Vert {\mathbf{q}}_i-\mathbf{R}{\mathbf{p}}_i-\mathbf{T}-({\mu }_q-\mathbf{R}{\mu }_p)+({\mu }_q-\mathbf{R}{\mu }_p){\Vert }^2 \\ =\Vert {\mathbf{q}}_i-{\mu }_q-\mathbf{R}({\mathbf{p}}_i-{\mu }_p)+({\mu }_q-\mathbf{R}{\mu }_p-\mathbf{T}){\Vert }^2 \\ =\Vert {\mathbf{q}}_i-{\mu }_q-\mathbf{R}({\mathbf{p}}_i-{\mu }_p){\Vert }^2+\Vert {\mu }_q-\mathbf{R}{\mu }_p-\mathbf{T}{\Vert }^2+2({\mathbf{q}}_i-{\mu }_q-\mathbf{R}({\mathbf{p}}_i-{\mu }_p){)}^T({\mu }_q-\mathbf{R}{\mu }_p-\mathbf{T}) \end{gathered}$$
注意到最后一项
$$\begin{gathered} \sum _{i=1}^n({\mathbf{q}}_i-{\mu }_q-\mathbf{R}({\mathbf{p}}_i-{\mu }_p){)}^T({\mu }_q-\mathbf{R}{\mu }_p-\mathbf{T}) \\ =({\mu }_q-\mathbf{R}{\mu }_p \end{gathered}$$