精华内容
下载资源
问答
  • 更多相关内容
  • 相机位姿估计的实际应用中,参考点的坐标数据不可避免地包含了测量误差,其量值大小通常不会完全一致,如果不区别测量误差直接进行相机位姿估计,将可能导致估计结果与真值相差甚远。为此,在广泛应用的正交迭代...
  • 单目位姿估计是计算机视觉中一个基础而重要的问题,在机器人定位、虚拟现实、图像精密测量等领域应用广泛。在实际应用中,参考点坐标不可避免地含有粗差点,导致估计结果偏离真值,为此,提出自适应加权的稳健正交迭代...
  • 相机位姿估计1:根据四个特征点估计相机姿态 随文Demo 使用opencv基于特征点估计位姿
  • 调研深度学习方法解决相机位姿估计,解读相关论文 Kendall A, Grimes M, Cipolla R. Convolutional networks for real-time 6-DOF camera relocalization[J]. Education for Information, 2015, 31:125-141.
  • 针对未知特征点匹配关系下的单目相机位姿估计问题, 提出了一种基于软指派算法的相机位姿估计算法。结合了正交迭代算法和软指派算法, 以三维(3D)和二维(2D)特征点的物方空间共线性误差作为确定特征点之间匹配关系的...
  • 针对相机位姿估计的稳定性与实时性, 基于正交迭代算法, 提出了一种考虑空间参考点不确定性的相机位姿估计算法。该算法的关键思想是在考虑摄像头畸变的情况下, 根据参考点的位置特征获得相应权值, 并利用加速正交迭代...
  • 单应估计homograph estimation、相机位姿估计pose estimation 学习笔记单应性矩阵定义版本一定义版本二定义版本三单应阵求法相机位姿 在学习图像匹配的过程中涉及到了两个相关的任务,即单应估计和相机位姿估计,在...

    单应估计homograph estimation、相机位姿估计pose estimation 学习笔记

    引言

    在学习图像匹配的过程中涉及到了两个相关的任务,即单应估计和相机位姿估计,在学习这两块的时候觉得网上的理论挺混乱的,首先就是公式不统一,光单应阵的定义都有好几种,而且两者都涉及到了相机成像的原理,所以很多公式都类似,但是有的地方有所不同,网上也没有说明两者之间区别和联系的内容,在这里记录一下网上的几种说法以及自己的一些想法,帮自己和大家理一下思路

    单应性矩阵

    定义版本一

    在这里插入图片描述
    在这里插入图片描述
    这里出现了内参阵,那么就是相机模型,并且默认拍摄的场景位于同一平面上,所以设zw=0并把r3化简掉了。这里让我一开始很迷惑,因为他没有做任何解释就把世界坐标的zw给去掉了。
    式子中的s也是比较迷惑的地方,因为好多个版本它的符号都不一样,而且都没有明确给出定义,根据看得好几篇文章目前我对它的理解有几个:深度、相机坐标系中空间点的z坐标、尺度因子

    定义版本二

    在这里插入图片描述
    这里也是假设场景位于同一平面上,公式包含了相机内参Ka、Kb;相机外参R、t;平面参数n、d,其中n为场景平面法向量、d为相机原点与平面的距离。这个公式引入了场景平面的参数进行代换、化简,由于我没有用到,所以没有深入了解。

    定义版本三

    在这里插入图片描述
    我觉得这个式子有点问题,内参33和外参44的维度不能相乘,所以外参最下面一行应该去掉,改成这个式子:
    在这里插入图片描述
    然后我简单的推推了一下,想总结一下单应阵和相机位姿之间的关系,不知道对不对,大家帮忙看看,原式子中的Z分之一我换成了d分之一,对应版本一中的s,但是它具体代表什么以及背后的原理我还是有点模糊。
    在这里插入图片描述
    如果推的是对的话,这个是不是就是当场景为平面时,相机位姿变化和单应阵之间的关系呢

    单应阵求法

    由于单应阵只有八个自由度,所以理论上求出两张图片的四对匹配点就可以解出单应阵,也可以求出大量的匹配点然后用ransac方法解出。但是,我想知道怎么通过其他方法求出单应阵,因为要本来就是要训练图像匹配的算法,不能通过匹配点去生成groundtruth homograph。我用的数据集是scannet,数据里包括了深度图、相机内外参,所以想利用这些groundtruth的数据计算出来单应阵,后来发现只有场景为平面的时候才有单应阵,而且自己推出来的单应阵和位姿的关系式不知道对不对,所以就放弃了求单应的想法。

    相机位姿

    (未完待续)

    展开全文
  • 面对需要实时计算的相机位姿估计问题,针对经典的广泛应用的正交迭代算法,提出了一种加速正交迭代算法。其关键思想是将每一次迭代过程规整化,从而提炼出每一次迭代的重复计算,若将此重复计算在迭代开始前提前计算...
  • 相机在不同位姿拍到世界中同一个路标,会有不同的成像位置,如下图所示 世界路标P\boldsymbol PP,在相机两个不同位姿所拍到的帧I1,I2\boldsymbol I_1,\boldsymbol I_2I1​,I2​中,分别投影在了p1,p2\boldsymbol p...

    目录
    VSLAM学习(一) 三维运动、相机模型、SLAM模型
    VSLAM学习(二) 非线性优化
    VSLAM学习(三) 单目相机位姿估计
    VSLAM学习(四) Bundle Adjustment

    一、对极几何约束

    相机在不同位姿拍到世界中同一个路标,会有不同的成像位置,如下图所示

    世界路标 P \boldsymbol P P,在相机两个不同位姿所拍到的帧 I 1 , I 2 \boldsymbol I_1,\boldsymbol I_2 I1,I2中,分别投影在了 p 1 , p 2 \boldsymbol p_1,\boldsymbol p_2 p1,p2两点。
    这两帧时刻,相机的坐标中心点分别为 C 1 , C 2 \boldsymbol C_1,\boldsymbol C_2 C1,C2
    e 1 , e 2 \boldsymbol e_1,\boldsymbol e_2 e1,e2分别是这两个时刻中心点在另一帧上的投影

    那么很容易看出, p 1 e 1 / / P C 2 \boldsymbol p_1\boldsymbol e_1 // \boldsymbol P\boldsymbol C_2 p1e1//PC2 p 2 e 2 / / P C 1 \boldsymbol p_2\boldsymbol e_2 // \boldsymbol P\boldsymbol C_1 p2e2//PC1

    1.1 本质矩阵

    P \boldsymbol P P点在 I 1 \boldsymbol I_1 I1帧和 I 2 \boldsymbol I_2 I2帧的坐标系下的向量坐标分别为 P 1 \boldsymbol P_1 P1 P 2 \boldsymbol P_2 P2
    那么根据三维运动我们知道:

    P 2 = R 12 P 1 + t \boldsymbol P_2=\boldsymbol R_{12}\boldsymbol P_1+\boldsymbol t P2=R12P1+t

    两边同时左叉乘 t \boldsymbol t t

    t ∧ P 2 = t ∧ R 12 P 1 \boldsymbol t^{\land}\boldsymbol P_2=\boldsymbol t^{\land}\boldsymbol R_{12}\boldsymbol P_1 tP2=tR12P1
    两边再同时与 P 2 \boldsymbol P_2 P2作内积

    0 = P 2 T t ∧ R 12 P 1 0=\boldsymbol P_2^T\boldsymbol t^{\land}\boldsymbol R_{12}\boldsymbol P_1 0=P2TtR12P1

    我们将 t ∧ R 12 = d e f E \displaystyle{\boldsymbol t^{\land}\boldsymbol R_{12}\overset{\mathrm{def}}{=}\boldsymbol E} tR12=defE称为本质矩阵(Essential Matrix)

    P 2 T E P 1 = 0 \boldsymbol P_2^T\boldsymbol E\boldsymbol P_1=0 P2TEP1=0

    1.2 基础矩阵

    根据相机模型我们知道

    { s 1 p 1 = K P 1 s 2 p 2 = K P 2 \left\{ \begin{aligned} s_1\boldsymbol p_1&=\boldsymbol K\boldsymbol P_1 \\ s_2\boldsymbol p_2&=\boldsymbol K\boldsymbol P_2 \end{aligned} \right. {s1p1s2p2=KP1=KP2

    其中 s 1 , s 2 s_1,s_2 s1,s2分别为P点在相机两帧里的深度
       p 1 , p 2 \boldsymbol p_1,\boldsymbol p_2 p1,p2分别为P点在相机图片像素坐标的齐次坐标

    K \boldsymbol K K移到左边:

    { s 1 K − 1 p 1 = P 1 s 2 K − 1 p 2 = P 2 \left\{ \begin{aligned} s_1\boldsymbol K^{-1}\boldsymbol p_1&=\boldsymbol P_1 \\ s_2\boldsymbol K^{-1}\boldsymbol p_2&=\boldsymbol P_2 \end{aligned} \right. {s1K1p1s2K1p2=P1=P2

    带入之前本质矩阵式中,得

    0 = p 2 T ( K − 1 ) T E K − 1 p 1 0=\boldsymbol p_2^T(\boldsymbol K^{-1})^T\boldsymbol E\boldsymbol K^{-1}\boldsymbol p_1 0=p2T(K1)TEK1p1

    我们将 ( K − 1 ) T E K − 1 = d e f F \displaystyle{(\boldsymbol K^{-1})^T\boldsymbol E\boldsymbol K^{-1}\overset{\mathrm{def}}{=}\boldsymbol F} (K1)TEK1=defF称为基础矩阵(Fundamental Matrix)

    p 2 T F p 1 = 0 \boldsymbol p_2^T\boldsymbol F\boldsymbol p_1=0 p2TFp1=0

    以上两个关于本质矩阵和基础矩阵的等式称为对极几何约束

    二、求解旋转与平移

    2.1 八点法

    将本质矩阵约束中的坐标归一化

    x 2 T E x 1 = 0 \boldsymbol x_2^T\boldsymbol E\boldsymbol x_1=0 x2TEx1=0

    记归一化坐标 x 1 = ( u 1 , v 1 , 1 ) T \boldsymbol x_1=(u_1,v_1,1)^\mathrm{T} x1=(u1,v1,1)T, x 2 = ( u 2 , v 2 , 1 ) T \boldsymbol x_2=(u_2,v_2,1)^\mathrm{T} x2=(u2,v2,1)T
    记本质矩阵 E = ( e 1 e 2 e 3 e 4 e 5 e 6 e 7 e 8 e 9 ) \boldsymbol E=\begin{pmatrix}e_1 & e_2 & e_3 \\e_4 & e_5 & e_6 \\e_7 & e_8 & e_9\end{pmatrix} E=e1e4e7e2e5e8e3e6e9

    那么

    ( u 2 , v 2 , 1 ) ( e 1 e 2 e 3 e 4 e 5 e 6 e 7 e 8 e 9 ) ( u 1 v 1 1 ) = ( u 2 u 1 , u 2 v 1 , u 2 , v 2 u 1 , v 2 v 1 , v 2 , u 1 , v 1 , 1 ) ( e 1 e 2 e 3 e 4 e 5 e 6 e 7 e 8 e 9 ) = 0 (u_2,v_2,1) \begin{pmatrix} e_1 & e_2 & e_3 \\ e_4 & e_5 & e_6 \\ e_7 & e_8 & e_9 \end{pmatrix} \begin{pmatrix} u_1 \\ v_1 \\ 1 \end{pmatrix}= (u_2u_1,u_2v_1,u_2,v_2u_1,v_2v_1,v_2,u_1,v_1,1) \begin{pmatrix} e_1 \\ e_2 \\ e_3 \\ e_4 \\ e_5 \\ e_6 \\ e_7 \\ e_8 \\ e_9 \end{pmatrix} =0 (u2,v2,1)e1e4e7e2e5e8e3e6e9u1v11=(u2u1,u2v1,u2,v2u1,v2v1,v2,u1,v1,1)e1e2e3e4e5e6e7e8e9=0

    现在要求解这9个值,由于等式等于0,可以去掉一个尺度自由度,那么只需8个方程即可求解。而 E = t ∧ R \boldsymbol E=\boldsymbol t^\mathrm{\land}\boldsymbol R E=tR,平移和旋转各有3个自由度,去掉尺度就只有5个自由度了,所以理论上最少只需要5对点就能求解,但是比较麻烦。

    取8对点的方法称为八点法,将Essential矩阵展开成向量格式
    a 1 = ( u 2 u 1 , u 2 v 1 , u 2 , v 2 u 1 , v 2 v 1 , v 2 , u 1 , v 1 , 1 ) \boldsymbol a_1=(u_2u_1,u_2v_1,u_2,v_2u_1,v_2v_1,v_2,u_1,v_1,1) a1=(u2u1,u2v1,u2,v2u1,v2v1,v2,u1,v1,1)
    e = ( v e c T E ) T = ( e 1 , e 2 , ⋅ ⋅ ⋅ , e 9 ) T \quad\boldsymbol e=(\mathrm{vec}^\mathrm{T}\boldsymbol E)^\mathrm{T}=(e_1,e_2,···,e_9)^\mathrm{T} e=(vecTE)T=(e1,e2,,e9)T

    a 1 T e = 0 \boldsymbol a_1^\mathrm{T}\boldsymbol e=0 a1Te=0
    同样方法再找7对点,将对应系数记为 a 2 ⋅ ⋅ ⋅ a 8 \boldsymbol a_2···\boldsymbol a_8 a2a8
    可以得到方程组

    ( a 1 a 2 a 3 a 4 a 5 a 6 a 7 a 8 ) e = 0 \begin{pmatrix} \boldsymbol a_1 \\ \boldsymbol a_2 \\ \boldsymbol a_3 \\ \boldsymbol a_4 \\ \boldsymbol a_5 \\ \boldsymbol a_6 \\ \boldsymbol a_7 \\ \boldsymbol a_8 \end{pmatrix} \boldsymbol e =0 a1a2a3a4a5a6a7a8e=0

    即可解出 e \boldsymbol e e的值
    也就是求出了本质矩阵 E \boldsymbol E E的值

    然后将矩阵 E \boldsymbol E E进行奇异值分解,便可得到 R \boldsymbol R R t \boldsymbol t t
    (暂略)

    如果多于8对点求解,可以使用以下两种方法

    • 最小二乘求解 min ⁡ e ∥ A e ∥ 2 2 = min ⁡ e e T A T A e \min\limits_{\boldsymbol e}\|\boldsymbol A\boldsymbol e\|_2^2=\min\limits_{\boldsymbol e}\boldsymbol e^\mathrm{T}\boldsymbol A^\mathrm{T}\boldsymbol A\boldsymbol e eminAe22=emineTATAe
    • 使用RANSAC方法:
      ① 有一个模型适应于假设的局内点,即所有的未知参数都能从假设的局内点计算得出。
      ② 用1中得到的模型去测试所有的其它数据,如果某个点适用于估计的模型,认为它也是局内点。
      ③ 如果有足够多的点被归类为假设的局内点,那么估计的模型就足够合理。
      ④ 然后,用所有假设的局内点去重新估计模型,因为它仅仅被初始的假设局内点估计过。
      ⑤ 最后,通过估计局内点与模型的错误率来评估模型。

    2.2 单应矩阵

    所有取出的点共面时,八点法退化,无法求解。
    当所有点共面时,设该平面 π = ( n T , d ) \boldsymbol\pi=(\boldsymbol n^{\mathrm T},d) π=(nT,d),即单位法向量为 n \boldsymbol n n,截距为 d d d
    则有

    n T P + d = 0 o r − n T P d = 1 \boldsymbol n^{\mathrm T}\boldsymbol P+d=0 \quad\quad or \quad\quad -\frac{\boldsymbol n^{\mathrm T}\boldsymbol P}{d}=1 nTP+d=0ordnTP=1

    可将1带入下式:

    p 2 = 1 s 1 K ( R 12 P 1 + t ) = 1 s 1 K ( R 12 P 1 + t ⋅ ( − n T P d ) ) = 1 s 1 K ( R 12 − t n T d ) P 1 = s 2 s 1 K ( R 12 − t n T d ) K − 1 p 1 = d e f H p 1 \begin{aligned} \boldsymbol p_2&=\frac{1}{s_1}\boldsymbol K(\boldsymbol R_{12}\boldsymbol P_1+\boldsymbol t) \\ &=\frac{1}{s_1}\boldsymbol K\Big(\boldsymbol R_{12}\boldsymbol P_1+\boldsymbol t·(-\frac{\boldsymbol n^{\mathrm T}\boldsymbol P}{d})\Big) \\ &=\frac{1}{s_1}\boldsymbol K(\boldsymbol R_{12}-\frac{\boldsymbol t\boldsymbol n^{\mathrm T}}{d})\boldsymbol P_1 \\ &=\frac{s_2}{s_1}\boldsymbol K(\boldsymbol R_{12}-\frac{\boldsymbol t\boldsymbol n^{\mathrm T}}{d})\boldsymbol K^{-1}\boldsymbol p_1 \\ &\overset{\mathrm{def}}{=}\boldsymbol H\boldsymbol p_1 \end{aligned} p2=s11K(R12P1+t)=s11K(R12P1+t(dnTP))=s11K(R12dtnT)P1=s1s2K(R12dtnT)K1p1=defHp1

    ( u 2 v 2 1 ) = ( h 1 h 2 h 3 h 4 h 5 h 6 h 7 h 8 h 9 ) ( u 1 v 1 1 ) \begin{pmatrix} u_2 \\ v_2 \\ 1 \end{pmatrix}= \begin{pmatrix} h_1 & h_2 & h_3 \\ h_4 & h_5 & h_6 \\ h_7 & h_8 & h_9 \end{pmatrix} \begin{pmatrix} u_1 \\ v_1 \\ 1 \end{pmatrix} u2v21=h1h4h7h2h5h8h3h6h9u1v11

    { u 2 = u 1 h 1 + v 1 h 2 + h 3 v 2 = u 1 h 4 + v 1 h 5 + h 6 1 = u 1 h 7 + v 1 h 8 + h 9 \left\{ \begin{aligned} u_2&=u_1h_1+v_1h_2+h_3 \\ v_2&=u_1h_4+v_1h_5+h_6 \\ 1&=u_1h_7+v_1h_8+h_9 \end{aligned} \right. u2v21=u1h1+v1h2+h3=u1h4+v1h5+h6=u1h7+v1h8+h9

    归一化 H \boldsymbol H H(令 h 9 = 1 h_9=1 h9=1),③式带入①、②并移项得

    { u 2 = u 1 h 1 + v 1 h 2 + h 3 − u 2 u 1 h 7 − u 2 v 1 h 8 v 2 = u 1 h 4 + v 1 h 5 + h 6 − v 2 u 1 h 7 − v 2 v 1 h 8 \left\{ \begin{aligned} u_2&=u_1h_1+v_1h_2+h_3-u_2u_1h_7-u_2v_1h_8 \\ v_2&=u_1h_4+v_1h_5+h_6-v_2u_1h_7-v_2v_1h_8 \end{aligned} \right. {u2v2=u1h1+v1h2+h3u2u1h7u2v1h8=u1h4+v1h5+h6v2u1h7v2v1h8


    ( u 1 v 1 1 0 0 0 − u 2 u 1 − u 2 v 1 0 0 0 u 1 v 1 1 − v 2 u 1 − v 2 v 1 ) ( h 1 h 2 h 3 h 4 h 5 h 6 h 7 h 8 ) = ( u 2 v 2 ) \begin{pmatrix} u_1 & v_1 & 1 & 0 & 0 & 0 & -u_2u_1 & -u_2v_1 \\ 0 & 0 & 0 & u_1 & v_1 & 1 & -v_2u_1 & -v_2v_1 \end{pmatrix} \begin{pmatrix} h_1 \\ h_2 \\ h_3 \\ h_4 \\ h_5 \\ h_6 \\ h_7 \\ h_8 \end{pmatrix}= \begin{pmatrix} u_2 \\ v_2 \end{pmatrix} (u10v10100u10v101u2u1v2u1u2v1v2v1)h1h2h3h4h5h6h7h8=(u2v2)

    一对点两个方程,所以只需要4对点即可求解
    算出 H \boldsymbol H H,继而求解 R , t , n , K , d \boldsymbol R,\boldsymbol t,\boldsymbol n,\boldsymbol K,d R,t,n,K,d
    这一步贼麻烦,暂略…

    三、三角测量深度估计

    求出平移和旋转之后,就可以来求某一个路标点在相机坐标下的位置了,确定深度了。

    3.1 直接求解

    由三维运动方程知(归一化坐标):
    s 2 x 2 = s 1 R 12 x 1 + t s_2\boldsymbol x_2=s_1\boldsymbol R_{12}\boldsymbol x_1+\boldsymbol t s2x2=s1R12x1+t

    那么现在的任务就是求出 s 1 , s 2 s_1,s_2 s1,s2,即是深度值
    两边同时左叉乘 x 2 \boldsymbol x_2 x2
    0 = s 1 x 2 ∧ R 12 x 1 + x 2 ∧ t 0=s_1\boldsymbol x_2^{\land}\boldsymbol R_{12}\boldsymbol x_1+\boldsymbol x_2^{\land}\boldsymbol t 0=s1x2R12x1+x2t
    便可求出 s 1 s_1 s1的值,继而求出 s 2 s_2 s2

    然而,由于噪声的存在,我们之前确定的 R , t \boldsymbol R,\boldsymbol t R,t并不完全准确,导致 s 1 , s 2 s_1,s_2 s1,s2也不准
    故而通过

    s 1 x 1 = P 1 s_1\boldsymbol x_1=\boldsymbol P_1 s1x1=P1

    求出的路标P的坐标肯定也是个歪的

    所以我们一般是对P点做最小二乘解

    3.2 最小二乘解

    以第一帧相机坐标系为准
    对于第k帧中的路标P的坐标 x k \boldsymbol x_k xk

    s k x k = R 1 k P + t k = ( R 1 k ∣ t k ) P s_k\boldsymbol x_k=\boldsymbol R_{1k}\boldsymbol P+\boldsymbol t_k=(\boldsymbol R_{1k}|\boldsymbol t_k)\boldsymbol P skxk=R1kP+tk=(R1ktk)P

    展开

    s k ( u k v k 1 ) = ( r k , 1 r k , 2 r k , 3 r k , 4 r k , 5 r k , 6 r k , 7 r k , 8 r k , 9 t k , 1 t k , 2 t k , 3 ) ( X Y Z 1 ) s_k \begin{pmatrix} u_k \\ v_k \\ 1 \end{pmatrix}= \left( \begin{array}{c:c} \begin{matrix} r_{k,1} & r_{k,2} & r_{k,3} \\ r_{k,4} & r_{k,5} & r_{k,6} \\ r_{k,7} & r_{k,8} & r_{k,9} \end{matrix}& \begin{matrix} t_{k,1} \\ t_{k,2} \\ t_{k,3} \end{matrix} \end{array} \right) \begin{pmatrix} X \\ Y \\ Z \\ 1 \end{pmatrix} skukvk1=rk,1rk,4rk,7rk,2rk,5rk,8rk,3rk,6rk,9tk,1tk,2tk,3XYZ1

    { s k u k = ( R 1 k ∣ t k ) 1 P s k v k = ( R 1 k ∣ t k ) 2 P s k = ( R 1 k ∣ t k ) 3 P \left\{ \begin{aligned} s_ku_k&=(\boldsymbol R_{1k}|\boldsymbol t_k)_{_1}\boldsymbol P \\ s_kv_k&=(\boldsymbol R_{1k}|\boldsymbol t_k)_{_2}\boldsymbol P \\ s_k&=(\boldsymbol R_{1k}|\boldsymbol t_k)_{_3}\boldsymbol P \end{aligned} \right. skukskvksk=(R1ktk)1P=(R1ktk)2P=(R1ktk)3P

    其中 ( R 1 k ∣ t k ) n (\boldsymbol R_{1k}|\boldsymbol t_k)_{_n} (R1ktk)n表示增广矩阵 ( R 1 k ∣ t k ) (\boldsymbol R_{1k}|\boldsymbol t_k) (R1ktk)的第n行

    消去第三行
    { u k ( R 1 k ∣ t k ) 3 P − ( R 1 k ∣ t k ) 1 P = 0 v k ( R 1 k ∣ t k ) 3 P − ( R 1 k ∣ t k ) 2 P = 0 \left\{ \begin{aligned} u_k(\boldsymbol R_{1k}|\boldsymbol t_k)_{_3}\boldsymbol P-(\boldsymbol R_{1k}|\boldsymbol t_k)_{_1}\boldsymbol P&=0 \\ v_k(\boldsymbol R_{1k}|\boldsymbol t_k)_{_3}\boldsymbol P-(\boldsymbol R_{1k}|\boldsymbol t_k)_{_2}\boldsymbol P&=0 \end{aligned} \right. {uk(R1ktk)3P(R1ktk)1Pvk(R1ktk)3P(R1ktk)2P=0=0


    ( u k ( R 1 k ∣ t k ) 3 − ( R 1 k ∣ t k ) 1 v k ( R 1 k ∣ t k ) 3 − ( R 1 k ∣ t k ) 2 ) P = 0 \begin{pmatrix} u_k(\boldsymbol R_{1k}|\boldsymbol t_k)_{_3}-(\boldsymbol R_{1k}|\boldsymbol t_k)_{_1} \\ v_k(\boldsymbol R_{1k}|\boldsymbol t_k)_{_3}-(\boldsymbol R_{1k}|\boldsymbol t_k)_{_2} \end{pmatrix}\boldsymbol P =0 (uk(R1ktk)3(R1ktk)1vk(R1ktk)3(R1ktk)2)P=0

    将所有的帧都整合起来,记

    A = d e f ( u 1 ( R 11 ∣ t 1 ) 3 − ( R 11 ∣ t 1 ) 1 v 1 ( R 11 ∣ t 1 ) 3 − ( R 11 ∣ t 1 ) 2 u 2 ( R 12 ∣ t 2 ) 3 − ( R 12 ∣ t 2 ) 1 v 2 ( R 12 ∣ t 2 ) 3 − ( R 12 ∣ t 2 ) 2 ⋮ u n ( R 1 n ∣ t n ) 3 − ( R 1 n ∣ t n ) 1 v n ( R 1 n ∣ t n ) 3 − ( R 1 n ∣ t n ) 2 ) \boldsymbol A\overset{\mathrm{def}}{=} \begin{pmatrix} u_1(\boldsymbol R_{11}|\boldsymbol t_1)_{_3}-(\boldsymbol R_{11}|\boldsymbol t_1)_{_1} \\ v_1(\boldsymbol R_{11}|\boldsymbol t_1)_{_3}-(\boldsymbol R_{11}|\boldsymbol t_1)_{_2} \\ u_2(\boldsymbol R_{12}|\boldsymbol t_2)_{_3}-(\boldsymbol R_{12}|\boldsymbol t_2)_{_1} \\ v_2(\boldsymbol R_{12}|\boldsymbol t_2)_{_3}-(\boldsymbol R_{12}|\boldsymbol t_2)_{_2} \\ \vdots \\ u_n(\boldsymbol R_{1n}|\boldsymbol t_n)_{_3}-(\boldsymbol R_{1n}|\boldsymbol t_n)_{_1} \\ v_n(\boldsymbol R_{1n}|\boldsymbol t_n)_{_3}-(\boldsymbol R_{1n}|\boldsymbol t_n)_{_2} \end{pmatrix} A=defu1(R11t1)3(R11t1)1v1(R11t1)3(R11t1)2u2(R12t2)3(R12t2)1v2(R12t2)3(R12t2)2un(R1ntn)3(R1ntn)1vn(R1ntn)3(R1ntn)2

    A P = 0 \boldsymbol A\boldsymbol P=0 AP=0

    那么,可对P求最小二乘解

    min ⁡ P 1 2 ∥ A P ∥ 2 2 = min ⁡ P 1 2 P T A T A P \min\limits_{\boldsymbol P}\frac{1}{2}\|\boldsymbol A\boldsymbol P\|_2^2= \min\limits_{\boldsymbol P}\frac{1}{2}\boldsymbol P^{\mathrm T}\boldsymbol A^{\mathrm T}\boldsymbol A\boldsymbol P \\ Pmin21AP22=Pmin21PTATAP

    展开全文
  • 关键词:位姿估计 OpenCV::solvePnP用途:各种位姿估计文章类型:原理、流程、Demo示例@Author:VShawn(singlex@foxmail.com)@Date:2016-11-18@Lab: CvLab202@CSU目录前言本文通过迭代法解PNP问题,得到相机坐标系...

    关键词:位姿估计  OpenCV::solvePnP

    用途:各种位姿估计

    文章类型:原理、流程、Demo示例

    @Author:VShawn(singlex@foxmail.com)

    @Date:2016-11-18

    @Lab: CvLab202@CSU

    目录

    前言

    本文通过迭代法解PNP问题,得到相机坐标系关于世界坐标系的旋转矩阵R与平移矩阵T后,根据之前的文章《根据相机旋转矩阵求解三个轴的旋转角》获得相机坐标系的三轴旋转角,实现了对相机位姿的估计。知道相机在哪后,我们就可以通过两张照片,计算出照片中某个点的高度,实现对环境的测量。

    先看演示视频:

    原理简介

    相机位姿估计就是通过几个已知坐标的特征点,以及他们在相机照片中的成像,求解出相机位于坐标系内的坐标与旋转角度,其核心问题就在于对PNP问题的求解,这部分本文不再啰嗦,参见本人之前的博客文章《相机位姿估计0:基本原理之如何解PNP问题》。本文中对pnp问题的求解直接调用了OpenCV的库函数"solvePnP",其函数原型为:

    bool solvePnP(InputArray objectPoints, InputArray imagePoints, InputArray cameraMatrix, InputArray distCoeffs, OutputArray rvec, OutputArray tvec, bool useExtrinsicGuess=false, int flags=ITERATIVE )

    第一个输入objectPoints为特征点的世界坐标,坐标值需为float型,不能为double型,可以输入mat类型,也可以直接输入vector 。

    第二个输入imagePoints为特征点在图像中的坐标,需要与前面的输入一一对应。同样可以输入mat类型,也可以直接输入vector 。

    第三个输入cameraMatrix为相机内参数矩阵,大小为3×3,形式为:

    第四个输入distCoeffs输入为相机的畸变参数,为1×5的矩阵。

    第五个rvec为输出矩阵,输出解得的旋转向量。

    第六个tvec为输出平移向量。

    第七个设置为true后似乎会对输出进行优化。

    最后的输入参数有三个可选项:

    CV_ITERATIVE,默认值,它通过迭代求出重投影误差最小的解作为问题的最优解。

    CV_P3P则是使用非常经典的Gao的P3P问题求解算法。

    CV_EPNP使用文章《EPnP: Efficient Perspective-n-Point Camera Pose Estimation》中的方法求解。

    流程

    1.从函数的原型看出函数需要相机的内参数与畸变参数,于是相机标定是必不可少的,通过OpenCV自带例程或者Matlab的相机标定工具箱都可以很方便地求出相机标定参数。

    2.准备好四个特征点的世界坐标,存入Mat矩阵

    vector<:point3f> Points3D;

    Points3D.push_back(cv::Point3f(0, 0, 0)); //P1 三维坐标的单位是毫米

    Points3D.push_back(cv::Point3f(0, 200, 0)); //P2

    Points3D.push_back(cv::Point3f(150, 0, 0)); //P3

    Points3D.push_back(cv::Point3f(150, 200, 0)); //P4

    3.准备好四个特征点在图像上的对应点坐标,这个坐标在实验中我是通过PhotoShop数出来的。注意,输入坐标的顺序一定要与之前输入世界坐标的顺序一致,就是说点与点要对应上,OpenCV的函数无法解决点与点匹配的问题(对应搜索问题)。

    vector<:point2f> Points2D;

    Points2D.push_back(cv::Point2f(3062, 3073)); //P1 单位是像素

    Points2D.push_back(cv::Point2f(3809, 3089)); //P2

    Points2D.push_back(cv::Point2f(3035, 3208)); //P3

    Points2D.push_back(cv::Point2f(3838, 3217)); //P4

    4.创建输出变量,即旋转矩阵跟平移矩阵的变量。最后调用函数。

    //初始化输出矩阵

    cv::Mat rvec = cv::Mat::zeros(3, 1, CV_64FC1);

    cv::Mat tvec = cv::Mat::zeros(3, 1, CV_64FC1);

    //三种方法求解

    solvePnP(Points3D, Points2D, camera_matrix, distortion_coefficients, rvec, tvec, false, CV_ITERATIVE); //实测迭代法似乎只能用共面特征点求位置

    //solvePnP(Points3D, Points2D, camera_matrix, distortion_coefficients, rvec, tvec, false, CV_P3P); //Gao的方法可以使用任意四个特征点

    //solvePnP(Points3D, Points2D, camera_matrix, distortion_coefficients, rvec, tvec, false, CV_EPNP);

    5将输出的旋转向量转变为旋转矩阵

    //旋转向量变旋转矩阵

    double rm[9];

    cv::Mat rotM(3, 3, CV_64FC1, rm);

    Rodrigues(rvec, rotM);

    6.最后根据《根据相机旋转矩阵求解三个轴的旋转角》一文求出相机的三个旋转角,根据《子坐标系C在父坐标系W中的旋转》求出相机在世界坐标系中的位置。

    至此,我们就求出了相机的位姿。

    实验

    本人在实验中先后使用了两台相机做测试,一台是畸变较小的sony a6000微单+35mm定焦镜头,另一台是畸变较重的130w的工业相机+6mm定焦广角镜头,实验中两台相机都得到了正确的位姿结果,此处为了方便只用α6000微单做演示说明。

    如上图所示,四个特征点P1-P4的世界坐标与像素坐标都已在图中标明,P5用于重投影验证位姿解是否正确。

    相机实际位姿大约为:

    粗略读出卷尺读数,得到相机的世界坐标大约为(520,0,330)。细心的读者应该发现了,上面几张图的特征点不一样了,其实是我中途重新做了一张特征点图,重新安放实验装置的时候已经尽量按照(520,0,330)这个坐标去安放了,但误差肯定是不可避免的。

    把参数输入例程中,得到结果,计算出相机的世界坐标:

    也就是(528.6,-2.89,358.6),跟实际情况还是差不多的。

    同时还得到了x y z轴的三个旋转角

    自己动手转一转相机,发现也是对的。

    对P5点重投影,投影公式为:

    结果为:

    误差在10pix以内,结果也是正确的,于是验证完毕。

    P.S.经本人测试发现,solvePnP提供的三种算法都能对相机位姿进行估计,虽然三者直接解出的结果略有不同,但都在误差范围之内。其中solvePnP的默认方法迭代法,似乎只能使用共面的四个特征点求位姿,一旦有一个点不共面,解出的结果就会不对。

    例程

    最后给出例程,例程基于VS2013开发,使用的是OpenCV2.4.X,大家运行前需要将opencv的路径重新配置成自己电脑上的,不懂的话参考我的博客《OpenCV2+入门系列(一):OpenCV2.4.9的安装与测试》。例程中提供两张照片,其中DSC03323就是"实验"中所用图片,例程在计算完成后,会在D盘根目录下生成两个txt,分别存储:相机在世界坐标系的坐标、相机的三个旋转角。

    下载地址:

    CSDN:http://download.csdn.net/detail/wx2650/9688155

    GIT:https://github.com/vshawn/Shawn_pose_estimation_by_opencv

    最后

    我现在在外地出差,演示视频里的东西就下一篇文章中再说了,敬请期待。

    展开全文
  • 相机标定源代码校准相机相对位姿估计的算法集 介绍 该存储库包含以下相对姿态估计求解器,采用 C++ 和 Matlab API: 传统的 5 点算法 (5P),包装自 Hartley 的著名实现。 已知旋转角度的 4 点算法 (4P-RA) 平面运动...
  • 为了实现高精度的基于直线段对应的相机位姿估计,提出一种直接最小二乘法。通过提出并利用一种直线段之间的距离测度,将原问题转化为最小化一个姿态旋转矩阵的二次目标函数,该距离测度综合考虑了线段的端点距离、...
  • 由于该数据库给出的数据是每幅图像和其对应相机的内外参数和光心投影方向,所以我需要先求出其6DOF预估姿态。再利用PoseNet网络对其实现基于图像的定位估计。好了,问题就很明确了:(1)根据图像和激光雷达参数的3D...
  • 在高精度快速位姿估计算法(EPnP)的基础上,加入改进的自适应加权物方残差目标函数迭代算法来优化结果,并以迭代后的平均像方残差作为最小误差判决阈值来修正误差较大的参数。实验结果表明,所提方法可以极大地提高...
  • PnP 单目相机位姿估计(二):solvePnP利用二维码求解相机世界坐标 cocoaqin 2017-09-05 10:40:15 29341 收藏 114 分类专栏: 机器视觉 文章标签: pnp 位姿 单目相机 世界坐标 相机坐标 版权 前言 ...
  • 关键词:相机位姿估计,单目尺寸测量,环境探知用途:基于相机的环境测量,SLAM,单目尺寸测量文章类型:原理说明、Demo展示@Author:VShawn@Date:2016-11-28@Lab: CvLab202@CSU目录前言早就写好了....不过doc放在...
  • PnP 单目相机位姿估计(一):初识PnP问题

    万次阅读 多人点赞 2017-09-04 20:06:53
    他描述了当知道n个三维空间点坐标及其二维投影位置时,如何估计相机位姿。我们可以想象,在一幅图像中,最少只要知道三个点的空间坐标即3D坐标,就可以用于估计相机的运动以及相机的姿态。 而特征点的空间坐标
  • 单目相机位姿估计流程整体流程1. 坐标系变换2. 对极约束3. 单目初始化4. 三角测量5. 求解空间3D坐标6. 最小化重投影误差 整体流程 1. 坐标系变换 已知:匹配的像素点对(u,v),(u′,v′)(u,v),(u^{'},v^{'})(u,v)...
  • 人工智能-深度学习-基于深度学习的相机位姿估计方法研究.pdf
  • 关于PNP问题就是指通过世界中的N个特征点与图像成像中的N个像点,计算出其投影关系,从而获得相机或物体位姿的问题。 以下讨论中设相机位于点Oc,P1、P2、P3……为特征点。 Case1: 当N=1时 当只有一个特征点...
  • 二维码具有方向性(只要设计的不为中心对称),可以更好的辅助求解位姿 二维码具有可识别性,即可解析二维码内部黑白块的排列顺序,从而确定该二维码是否为我们所要用的二维码 二维码识别的流程 该流程以该二维码为...
  • // 像素坐标转相机归一化坐标 Point2d pixel2cam(const Point2d& p, const Mat& K); // BA void bundleAdjustment( const vector<Point3f> points_3d, const vector<Point2f> points_2d, const Mat& K, Mat& R, ...
  • 介绍了基于单目摄像机采集到的多帧图像,实现相机位姿估计。介绍了対级几何,三角测量,八点法,P3P,ICPSVD分解。对于上述方法的理论推导完善且详尽,对于求解问题用到的算法进行了简明的整理。是学习SLAM前端...
  • 如图,一个完整的SfM系统一般包括特征匹配、初始化、相机位姿和特征点三维位置的求解、集束调整(bundle adjustment,BA)和自标定(self-calibration)等模块。 早期的SfM系统一般是离线计算的,后来发展出了实时的...

空空如也

空空如也

1 2 3 4 5 ... 20
收藏数 6,128
精华内容 2,451
关键字:

相机位姿估计

友情链接: c++中文手册.zip