EPnP笔记

问题描述

已知n个空间点在世界坐标系下的坐标 $P_i^w$ 及其图像坐标系下的坐标 $u_i$ ，所要求解的问题是如何根据空间点和图像点之间的对应关系求解旋转矩阵R和平移向量t。

EPnP基本思路

-EPnP算法将世界坐标系中的3D坐标表示为一组虚拟的控制点的加权和。对于一般情形，EPnP算法要求控制点的数目为4，且这4个控制点不能共面。通过求解4个控制点在相机坐标系下的坐标，就可以进一步求解出相机位姿

求解虚拟控制点图像坐标

在世界坐标系中设定4个非共面虚拟控制点，齐次坐标记为 $\{C_i^w|i=1,2,3,4\}$

注意这里是在世界坐标系中设定控制点

四个点通常选为：
$C_1^w=[0,0,0,1]^T$$ $$C_2^w=[0,0,0,1]^T$$ $$C_3^w=[0,0,0,1]^T$$ $$C_4^w=[0,0,0,1]^T$

image
空间点的世界坐标和控制点有如下关系
$p_i^w=\sum^4_{j=1}\alpha_{ij}C_j^w,with\sum^4_{j=1}=\alpha_{ij}=1$
向量 $[\alpha_{i1},\alpha_{i2},\alpha_{i3},\alpha_{i4}]^T$ 可以看作空间点 $P_i^W$ 在以控制点 $\{C_i^w|i=1,2,3,4\}$ 为基的欧氏空间的坐标。
同样的由于线性关系在欧式变换下的不变形有：
$p_i^c=\sum^4_{j=1}\alpha_{ij}C_j^c$
空间点 $P_i$ 和图像点 $u_i$ 的投影成像关系为：
$\lambda_iu_i=K[R,t]P_i^w=KP^c_i=K\sum_{j=1}^4\alpha C_j^c$
设控制点相机坐标系下 $C_j^c=[x_j^c,y_j^c,z_j^c]^T$ ，则
$\lambda_i=\sum_{j=1}^4\alpha_{ij}z_j^c$
把上面两个式子结合可得：
$\sum_{j=1}^4\alpha_{ij}fx_j^c-\alpha_{ij}u_iz_j^c=0$
$\sum_{j=1}^4\alpha_{ij}fy_j^c-\alpha_{ij}v_iz_j^c=0$

令 $Z=[Z_1^{cT},Z_2^{cT},Z_4^{cT},Z_4^{cT}]^T$ , $Z^c_j=[fx_j^c,fy^c_j,z_j^c]^T,j=1,2,3,4$ ，所以Z是1*12的向量

n个空间点可以得到线性方程组:
$MX=0$
其中 $X=[c_1^{cT},C_2^{cT},C_3^{cT},C_4^{cT}]^T$ ,每个元素是控制点在摄像头坐标系下的坐标。
$X=\sum^N_{i=1}\beta_iv_i$
可以计算 $M^TM$ 的特征向量得到 $v_i$ ，但还需要求出 $\beta_i$
对于第i个控制点：
$c_i^c=\sum^N_{k=1}\beta_kv_k^{[i]}$
$v_k^{[i]}$ 是特征向量 $v_k$ 的第i个3×1 sub-vector

高斯牛顿优化：
摄像头的外参描述的只是坐标变换，不会改变控制点之间的距离
$||\sum^N_{k=1}\beta_kv_k^{[i]}-\sum^N_{k=1}\beta_kv_k^{[j]}||^2=||c_i^w-c^w_j||^2$
优化目标函数：
$Error(\beta)=\sum_{(i,j)s.t.i<j}(||\sum^N_{k=1}\beta_kv_k^{[i]}-\sum^N_{k=1}\beta_kv_k^{[j]}||^2-||c_i^w-c^w_j||^2)^2$

计算相机位姿

控制点在相机坐标系下的坐标
$c_i^c=\sum^N_{k=1}\beta_kv_k^{[i]},i=1,...,4$
计算3D参考点在摄像头参考坐标系下的坐标：
$p_i^c=\sum^4_{j=1}\alpha_{ij}C_j^c,i=1,...,n$
计算 $\{P_i^w\}_{i=1,...,n}$ 的重心 $P^w_0$ 和矩阵A:
$A= \left[ \begin{matrix} p_1^{w^T}-p_0^{w^T}\\ ....\\ p_n^{w^T}-p_0^{w^T} \end{matrix} \right]$
计算 $\{P_i^c\}_{i=1,...,n}$ 的重心 $P^c_0$ 和矩阵B:
$B= \left[ \begin{matrix} p_1^{c^T}-p_0^{c^T}\\ ....\\ p_n^{c^T}-p_0^{c^T} \end{matrix} \right]$
计算H：
$H=B^TA$
计算H的SVD分解：
$H=U\Sigma V^T$
计算旋转R
$R=UV^T$
如果 $|R|<0$ ,那么 $R(2,:)=-R(2,:)$
计算平移t：
$t=p^c_0-Rp^w_0$

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

EPnP笔记

问题描述

EPnP基本思路

求解虚拟控制点图像坐标

计算相机位姿

推荐阅读更多精彩内容