• 3D-2D:PnP


    PnP(Perspective-n-Point):当知道n个3D空间点及其投影位置时,估计相机位姿。

    2D-2D的对极几何方法需要八个或八个以上的点对(以八点法为例),且存在着初始化、纯旋转和尺度的问题。然而,如果两张图像中,其中一张特征点的 3D 位置已知(特征点的 3D 位置可以由三角化,或者由 RGB-D 相机的深度图确定)。那么最少只需三个点对(需要至少一个额外点验证结果)就可以估计相机运动。

    因此,在双目或 RGB-D 的视觉里程计中,我们可以直接使用 PnP 估计相机运动。

    而在单目视觉里程计中,必须先进行初始化,然后才能使用 PnP。

    3D-2D 方法不需要使用对极约束,又可以在很少的匹配点中获得较好的运动估计,是最重要的一种姿态估计方法。

    PnP有多种方法求解:直线线性变换(DLT)、P3P还有非线性方法,构建最小二乘问题并迭代求解(Bundle Adjustment)。

    直线线性变换(DLT)

    空间点P,齐次坐标为$P=left(X,Y,Z,1 ight)^{T}$

    在图像$I_1$中,投影到特征点$x_1=left(u_1,v_1,1 ight)^{T}$(归一化平面齐次坐标表示),并没有用到相机内参K,所以是归一化坐标而不是像素坐标。

    定义增广矩阵$left[R|t ight]$为一个3X4的矩阵,包含旋转与平移信息。

    $segin{bmatrix} u_1 \ v_1 \ 1 end{bmatrix} = egin{bmatrix} t_1 & t_2 & t_3 & t_4 \ t_5 & t_6 & t_7 & t_8 \ t_9 & t_{10} & t_{11} & t_{12} end{bmatrix}egin{bmatrix} X \ Y \ Z \1 end{bmatrix}$

    $su_1 = t_1X + t_2Y + t_3Z + t_4$

    $sv_1 = t_5X + t_6Y + t_7Z + t_8$

    $s = t_9X + t_{10}Y + t_{11}Z + t_{12}$

    把s消去:

    $u_1 = frac{t_1X + t_2Y + t_3Z + t_4}{t_9X + t_{10}Y + t_{11}Z + t_{12}}$,$v_1 = frac{t_5X + t_6Y + t_7Z + t_8}{t_9X + t_{10}Y + t_{11}Z + t_{12}}$

    $u_1 =frac{ extbf{t}_{1}^{T}P}{ extbf{t}_{3}^{T}P}$,$v_1 =frac{ extbf{t}_{2}^{T}P}{ extbf{t}_{3}^{T}P}$

    一个特征点提供了两个关于t的约束。

    t有12维,最少需要6对匹配点求$left[R|t ight]$,为直接线性变换。

    可由QR分解完成。

    P3P

    见书

    Bundle Adjustment

    除了以上DLT线性方法外,还可以构建非线性最小二乘问题。

    线性方法先求相机位姿,再求空间点位置。

    非线性优化则把相机位姿和空间点位置都当做优化变量,一起优化。

    最小化重投影误差(Reprojection error)

    n个三维空间点P及其投影p,位姿为R,t。李代数表示为$zeta$。

    考虑n个三维空间点P及其投影p。

    假设某空间点坐标为$P_i = left[X_i,Y_i,Z_i ight]^{T}$,投影坐标为$u_i=left[u_i,v_i ight]^{T}$

    $expleft(zeta^{wedge} ight)P_i$结果是 4 × 1 的,而它左侧的 K 是 3 × 3 的,所以必须把$expleft(zeta^{wedge} ight)P_i$的前三维取出来,变成三维的非齐次坐标。

    将像素坐标(观测到的投影位置)与3D点按照当前估计的位姿进行投影得到的位置相比较得到的误差,即重投影误差。

    使用牛顿高斯法需要知道每个误差关于优化变量的导数,也是线性化:

    $eleft(x+Delta ight) hickapprox eleft(x ight) + JDelta{x}$,其中e为像素坐标误差。

    优化相机位姿$zeta$

    相机坐标系下的空间点坐标$P^{'}$

    其中$delta{zeta}$是位姿增量相当于牛顿法中的$Delta{x}$,$eleft(deltazetaopluszeta ight)$相于与$fleft(x+Delta{x} ight)$,f为误差函数,$oplus$为李代数上的左扰动。

    设观测值$left[u^{'},v^{'} ight]^{T}$。

    $e = left[u^{'}-left(f_xfrac{X^{'}}{Z^{'}}+c_x ight), v^{'}-left(f_yfrac{Y^{'}}{Z^{'}}+c_y ight) ight]^{T}$

    $P^{'}=left[X^{'},Y^{'},Z^{'} ight]^{T}$

     

    $frac{partial{P}^{'}}{partialdeltazeta}=frac{partialleft({TP} ight)}{partialdeltazeta}$

    以下推导过程$p$表示$P$,即空间点。

    其中$deltazeta = left[delta ho,deltaphi ight]^{T}$,$a^{wedge}b = -b^{wedge}a$

    优化空间点$P$

    其中$frac{partial{P{'}}}{partial{deltazeta}}$雅克比的推导方法也可参考:https://blog.csdn.net/zhubaohua_bupt/article/details/74011005

    $delta{zeta}=left[delta ho,deltaphi ight]$,$P^{'}=left[X^{'},Y^{'},Z^{'} ight]$

    $delta{zeta}=left[delta ho_1,delta ho_2,delta ho_3,deltaphi_1,deltaphi_2,deltaphi_3 ight]$

    $delta{phi}^{wedge} = egin{bmatrix} 0 & -deltaphi_3 & deltaphi_2 \ deltaphi_3 & 0 & -deltaphi_1 \ -deltaphi_2 & deltaphi_1 & 0 end {bmatrix}$

    $delta{phi}^{wedge}P^{'}+delta ho = egin{bmatrix} -deltaphi_3x_2+deltaphi_2x_3 + delta ho_1 \ deltaphi_3x_1- deltaphi_1x_3 + delta ho_2 \ -deltaphi_2x_1+deltaphi_1x_2 + delta ho_3 end{bmatrix}$

    对$delta{zeta}=left[delta ho_1,delta ho_2,delta ho_3,deltaphi_1,deltaphi_2,deltaphi_3 ight]$求导,得$egin {bmatrix} 1 & 0 & 0 & 0 & -Z^{'} & Y^{'} \ 0 & 1 & 0 & Z^{'} &  0 & -X^{'} \ 0 & 0 & 1 & -Y^{'} & X^{'} & 0 end{bmatrix}$,即$left[I_{3x3},-P_{3x3}^{'wedge} ight]$

  • 相关阅读:
    temp
    JAVA 存储空间 寄存器 堆栈 堆…
    数据类型、变量、数组类
    ubuntu ARP 防御
    详解 JAVA 创建对象 NEW
    Eclipse常见问题集锦
    解决error:2014 Commands out of sync; you can't run this command now
    关于MySql5“data too long for column”问题的探解
    Notepad++编辑Pyhton文件的自动缩进的问题(图文)
    mysql 'latin1' codec can't encode characters的问题
  • 原文地址:https://www.cnblogs.com/112358nizhipeng/p/9718467.html
Copyright © 2020-2023  润新知