问题引入
迭代最近点(iterative Closest Point, ICP)算法是一种点云匹配算法.
假设我们通过RGB-D相机得到了第一组点云$P=\left\{p_1,p_2,\cdots,p_n\right\}$, 相机经过位姿变换(旋转+平移)后又拍摄了第二组点云$Q=\left\{q_1,q_2,\cdots,q_n\right\}$. 注意这里的$P$和$Q$的坐标分别对应移动前和移动后的坐标系(即坐标原点始终为相机光心, 这里我们有移动前、后两个坐标系), 并且我们通过筛选和调整了点云储存的顺序, 使得$P, Q$中的点一一对应, 如$(p_{99},q_{99})$在三维空间中对应同一个点.
现在我们要解决的问题是: 计算相机的旋转$R$和平移$t$, 在没有误差的情况下, 从$P$坐标系转换到$Q$的公式为
$$
q_i=Rp_i+t
$$
但由于噪声及错误匹配(如$(p_{99}, q_{99})$其实并不对应空间中同一点, 但特征算法误认为二者是同一点)的存在, 上式不总是成立, 所以我们要最小化的目标函数为
$$
\frac{1}{2}\sum_{i=1}{n}\parallel q_i-Rp_i-t\parallel^2
$$
常用的求解$R$和$t$的方法有:
- SVD
- 非线性优化
下面介绍下SVD方法.
Solution: SVD
为了后面能用SVD算法, 对公式进行一些小小的变形.
首先定义两组点云的质心(center of mass)为$\mu=\frac{1}{n}\sum_{i=1}^{n}p_i,\;\mu_q\frac{1}{n}\sum_{i=1}^{n}q_i$, 并做如下处理
$$
\begin{aligned}
&\frac{1}{2}\sum_{i=1}^{n}\parallel q_i-Rp_i-t\parallel^2\\=&\frac{1}{2}\sum_{i=1}^{n}\parallel q_i-Rp_i-t-(\mu_q-R\mu_p)+(\mu_q-R\mu_p)\parallel^2\\=&\frac{1}{2}\sum_{i=1}^{n}\parallel (q_i-\mu-q-R(p_i-\mu_p))+(\mu_q-R\mu_p-t)\parallel^2\\=&\frac{1}{2}\sum_{i=1}^{n}(\parallel q_i-\mu_q-R(p_i-\mu_p)\parallel^2+\parallel u_q-R\mu_p-t\parallel^2+\\&2(q_i-\mu_q-R(p_i-\mu_p))^T(\mu_q-R\mu_p-t))
\end{aligned}
$$
注意最后一项中
$$
\begin{aligned}&\sum_{i=1}^{n}(q_i-\mu_q-R(p_i-\mu_p))^T(\mu_p-R\mu_p-t)\\=&(\mu_q -R\mu_p-t)^T\sum_{i=1}^n(q_i-\mu_q-R(p_i-\mu_p))\\=&(\mu_q-R\mu_p-t)^T(n\mu_q-n\mu_q-R(n\mu_p-n\mu_p))\\=&0\end{aligned}
$$
设$p_i'=p_i-\mu_p,\;q_i'=q_i-\mu_q$, 目标函数简化为：
$$
\frac{1}{2}\sum_{i=1}^{n}(\parallel q_i'-Rp_i'\parallel^2+\parallel \mu_q-R\mu_p-t\parallel^2)
$$
设$R^*,\;t^*$为最优解, 可以将优化问题两步:
- $R^*=\mathop{\arg\min}\limits_{R}\frac{1}{2}\sum_{i=1}^{n}\parallel q_i'-Rp_i'\parallel^2$
- $t^*=\mu_q-R\mu_p$
重点在于步骤1, 将他展开得
$$
\begin{aligned}R^*&=\mathop{\arg\min}\limits_{R}\frac{1}{2}\sum_{i=1}^{n}(q_i'^Tq_i+p_i'^TR^TRp_i'-2q_i'^TRp_i')\\&=\mathop{\arg\min}\limits_{R}\frac{1}{2}\sum_{i=1}^{n}(q_i'^Tq_i+I-2q_i'^TRp_i')\\&=\mathop{\arg\min}\limits_{R}\frac{1}{2}\sum_{i=1}^{n}-q_i'^TRp_i'\end{aligned}
$$
令$W=\sum_{i=1}^{n}q_i'p_i'^T$, 通过SVD分解有
$$
W=U\Sigma V^T
$$
当$W$满秩时有:
$$
\Sigma=\left[\begin{matrix}\sigma_1&0&0\\0&\sigma_2&0\\0&0&\sigma_3\end{matrix}\right]
$$
且对应唯一的$U,\; V$组合, 对应的
$$
\begin{aligned}R^*&=UV^T\\t^*&=\mu_q-R\mu_p\end{aligned}
$$
就是我们要找的最优解arun1987.pdf
0 条评论