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

最后编辑于 ：
©著作权归作者所有,转载或内容合作请联系作者
  • 人面猴
    序言：七十年代末，一起剥皮案震惊了整个滨河市，随后出现的几起案子，更是在滨河造成了极大的恐慌，老刑警刘岩，带你破解...
    沈念sama阅读 199,393评论 5赞 467
  • 死咒
    序言：滨河连续发生了三起死亡事件，死亡现场离奇诡异，居然都是意外死亡，警方通过查阅死者的电脑和手机，发现死者居然都...
    沈念sama阅读 83,790评论 2赞 376
  • 救了他两次的神仙让他今天三更去死
    文/潘晓璐 我一进店门，熙熙楼的掌柜王于贵愁眉苦脸地迎上来，“玉大人，你说我怎么就摊上这事。” “怎么了？”我有些...
    开封第一讲书人阅读 146,391评论 0赞 330
  • 道士缉凶录：失踪的卖姜人
    文/不坏的土叔 我叫张陵，是天一观的道长。 经常有香客问我，道长，这世上最难降的妖魔是什么？ 我笑而不...
    开封第一讲书人阅读 53,703评论 1赞 270
  • 港岛之恋（遗憾婚礼）
    正文 为了忘掉前任，我火速办了婚礼，结果婚礼上，老公的妹妹穿的比我还像新娘。我一直安慰自己，他们只是感情好，可当我...
    茶点故事阅读 62,613评论 5赞 359
  • 恶毒庶女顶嫁案：这布局不是一般人想出来的
    文/花漫 我一把揭开白布。 她就那样静静地躺着，像睡着了一般。 火红的嫁衣衬着肌肤如雪。 梳的纹丝不乱的头发上，一...
    开封第一讲书人阅读 48,003评论 1赞 275
  • 城市分裂传说
    那天，我揣着相机与录音，去河边找鬼。 笑死，一个胖子当着我的面吹牛，可吹牛的内容都是我干的。 我是一名探鬼主播，决...
    沈念sama阅读 37,507评论 3赞 390
  • 双鸳鸯连环套：你想象不到人心有多黑
    文/苍兰香墨 我猛地睁开眼，长吁一口气：“原来是场噩梦啊……” “哼！你这毒妇竟也来了？” 一声冷哼从身侧响起，我...
    开封第一讲书人阅读 36,158评论 0赞 254
  • 万荣杀人案实录
    序言：老挝万荣一对情侣失踪，失踪者是张志新（化名）和其女友刘颖，没想到半个月后，有当地人在树林里发现了一具尸体，经...
    沈念sama阅读 40,300评论 1赞 294
  • 护林员之死
    正文 独居荒郊野岭守林人离奇死亡，尸身上长有42处带血的脓包…… 初始之章·张勋 以下内容为张勋视角 年9月15日...
    茶点故事阅读 35,256评论 2赞 317
  • 白月光启示录
    正文 我和宋清朗相恋三年，在试婚纱的时候发现自己被绿了。 大学时的朋友给我发了我未婚夫和他白月光在一起吃饭的照片。...
    茶点故事阅读 37,274评论 1赞 328
  • 活死人
    序言：一个原本活蹦乱跳的男人离奇死亡，死状恐怖，灵堂内的尸体忽然破棺而出，到底是诈尸还是另有隐情，我是刑警宁泽，带...
    沈念sama阅读 32,984评论 3赞 316
  • 日本核电站爆炸内幕
    正文 年R本政府宣布，位于F岛的核电站，受9级特大地震影响，放射性物质发生泄漏。R本人自食恶果不足惜，却给世界环境...
    茶点故事阅读 38,569评论 3赞 303
  • 男人毒药：我在死后第九天来索命
    文/蒙蒙 一、第九天 我趴在偏房一处隐蔽的房顶上张望。 院中可真热闹，春花似锦、人声如沸。这庄子的主人今日做“春日...
    开封第一讲书人阅读 29,662评论 0赞 19
  • 一桩弑父案，背后竟有这般阴谋
    文/苍兰香墨 我抬头看了看天上的太阳。三九已至，却和暖如春，着一层夹袄步出监牢的瞬间，已是汗流浃背。 一阵脚步声响...
    开封第一讲书人阅读 30,899评论 1赞 255
  • 情欲美人皮
    我被黑心中介骗来泰国打工， 没想到刚下飞机就差点儿被人妖公主榨干…… 1. 我叫王不留，地道东北人。 一个月前我还...
    沈念sama阅读 42,268评论 2赞 345
  • 代替公主和亲
    正文 我出身青楼，却偏偏与公主长得像，于是被迫代替她去往敌国和亲。 传闻我的和亲对象是个残疾皇子，可洞房花烛夜当晚...
    茶点故事阅读 41,840评论 2赞 339

推荐阅读更多精彩内容