ICP SVD 解法

整体流程

  1. 有两组点，已经配好对，分别为：p_i, p'_i，每个点是一个列向量
  2. 求中心，得到p_c, p'_c
  3. 去中心，得到q_i, q'_i
  4. 求旋转 R，依据优化公式：
    R^*=\arg \min \limits_{R} \frac{1}{2} \sum_i||q_i - Rq'_i||
  5. 求平移 t，依据公式：
    p_c = Rp'_c+t

优化结果

  1. 计算3x3 矩阵 H（如果是三维点云）
    H=\sum_i^n q'_i q_i^T

  2. SVD 分解 H
    H=U\Sigma V^T
    其中 U V 均为 3x3 正交矩阵

  3. 得到最优 R^*：
    R^* = VU^T

优化推导

  1. 展开
    \begin{align*} R^*&=\arg \min \limits_{R} \frac{1}{2} \sum_i ||q_i - Rq'_i|| \\ &=\arg \min \limits_{R} \frac{1}{2} \sum_i (q_i - Rq'_i)^T (q_i - Rq'_i) \\ &=\arg \min \limits_{R} \frac{1}{2} \sum_i q_i^Tq_i - 2q_i^TRq'_i+{q'_i}^TR^TRq'_i \\ &=\arg \min \limits_{R} \frac{1}{2} \sum_i q_i^Tq_i - 2q_i^TRq'_i+{q'_i}^Tq'_i\\ \end{align*}
    用到的理论：
  • R 为正交矩阵

  1. 去除与R^* 无关项，优化问题等价于：
    R^*=\arg \min \limits_{R} \sum_i -q_i^TRq'_i

  2. 使用迹
    \begin{align*} R^* &=\arg \min \limits_{R} \sum_i -q_i^TRq'_i \\ &=\arg \min \limits_{R} \sum_i -tr(q_i^TRq'_i) \\ &=\arg \min \limits_{R} \sum_i -tr(Rq'_iq_i^T) \\ &=\arg \max \limits_{R} \sum_i tr(Rq'_iq_i^T) \\ &=\arg \max \limits_{R} tr(\sum_i Rq'_iq_i^T) \\ &=\arg \max \limits_{R} tr(R\sum_i q'_iq_i^T) \\ \end{align*}
    用到的理论：

  • tr(AB)=tr(BA)
  • tr(A+B)=tr(A)+tr(B)

  1. 使用 3x3 H 矩阵化简：
    R^*=\arg \max \limits_{R} tr(RH)
    现在的问题就是，找到一个R，使RH的迹最大。

  2. 使用辅助旋转矩阵 B：
    上述问题等价于
    tr(R^*H) \geq tr(BR^*H)
    B 为一个正交矩阵。

  3. 有如下理论：
    tr(AA^T)≥tr(BAA^T),(BB^T=I)
    （可由柯西不等式证明）
    则问题转化为，求一个R^*，使R^*H可以表示为AA^T的形式

  4. SVD 分解H
    H=U\Sigma V^T
    当 R^*=VU^T 时，R^*H=VΣV^T
    令 A=VΣ^\frac{1}{2}
    有 R^*H=AA^T
    因此优化结果为 R^*=VU^T。

相关证明

https://blog.csdn.net/zhouyelihua/article/details/77807541

最后编辑于 ：
©著作权归作者所有,转载或内容合作请联系作者
平台声明：文章内容（如有图片或视频亦包括在内）由作者上传并发布，文章内容仅代表作者本人观点，简书系信息发布平台，仅提供信息存储服务。

推荐阅读更多精彩内容