视觉里程计（二）

1.3D-2D：PnP

PnP(Perspective-n-Point)是一种求解3D到2D点对运动的方法。描述了当知道n个3D空间点及其投影位置时，如何估计相机的位姿。
对象：双目或RGB-D视觉里程计
原理：如果俩张图像中其中一张特征点已知，那么最少只需要3个点对（一个额外点验证结果）
优势：对比对极约束而言，PnP只需要3个点即可估计其相机的位姿

PnP求解问题的方法：3对点估计位姿的P3P、直线线性变换（DLT）。可以用非线性优化的方式，构建最小二乘问题并迭代求解，也就是Bundle Adjustment

1.1直线线性变换

空间点P=(X,Y,Z,1)T$P=(X,Y,Z,1{)}^T$, 在归一化平面上的坐标值为 x1=(u,v,1)T$x_1=(u,v,1{)}^T$，定义一个增广矩阵 [R|t]$[R|t]$，有以下方程式：

s⎛⎝⎜u1v11⎞⎠⎟=⎛⎝⎜t1t2t3⎞⎠⎟P$s\left(\begin{array}{c}{u}_1\\ v_1\\ 1\end{array}\right)=\left(\begin{array}{c}{\mathbf{t}}_1\\ {\mathbf{t}}_2\\ {\mathbf{t}}_3\end{array}\right)\mathbf{P}$

其中，t1=(t1,t2,t3,t4)${\mathbf{t}}_1=(t_1,t_2,t_3,t_4)$，t2=(t5,t6,t7,t8)${\mathbf{t}}_2=(t_5,t_6,t_7,t_8)$，t3=(t9,t10,t11,t12)${\mathbf{t}}_3=(t_9,t_{10},t_{11},t_{12})$

将s消去，于是有 {tT1P−tT3Pu1=0tT2P−tT3Pv1=0$\{\begin{array}{c}{\mathbf{t}}_1^{T}P-{\mathbf{t}}_3^{T}P{u}_1=0\\ {\mathbf{t}}_2^{T}P-{\mathbf{t}}_3^{T}P{v}_1=0\end{array}$

整理得：⎛⎝⎜⎜⎜⎜⎜⎜⎜⎜PT10⋮PTN00PT1⋮0PTN−u1PT1−v1PT1⋮−uNPTN−vNPTN⎞⎠⎟⎟⎟⎟⎟⎟⎟⎟⎛⎝⎜t1t2t3⎞⎠⎟=0$\left(\begin{array}{ccc}{P}_1^T& 0& -u_1{P}_1^T\\ 0& P_1^T& -v_1{P}_1^T\\ ⋮& ⋮& ⋮\\ P_N^T& 0& -u_N{P}_N^T\\ 0& P_N^T& -v_N{P}_N^T\end{array}\right)\left(\begin{array}{c}{t}_1\\ t_2\\ t_3\end{array}\right)=0$

把 t3=(t9,t10,t11,t12)${\mathbf{t}}_3=(t_9,t_{10},t_{11},t_{12})$ 看成变量，进行求解，由于t有12维，因此需要6对点求解，这种方法称为直接线性变换。当匹配点大于6对时，也可以使用SVD求解。但是，当我们在求解时，直接将t看做分开的12个变量，实际上，旋转矩阵R有其自身的约束。对于该问题，必须针对DLT估计的T寻找一个最好的旋转矩阵与其对应。然后再把矩阵空间投影到SE(3)流上，转化成旋转平移。

1.2 P3P

仅使用3对配对点，对数据要求比较少，它的输入数据为3对3D-2D匹配点，如下图所示：

三角形的对应关系为：ΔOab−ΔOAB,ΔObc−ΔOBC,ΔOac−ΔOAC$\mathrm{\Delta }Oab-\mathrm{\Delta }OAB,\mathrm{\Delta }Obc-\mathrm{\Delta }OBC,\mathrm{\Delta }Oac-\mathrm{\Delta }OAC$

通过推导，可知：{(1−u)y2−ux2−cos<b,c>y+2uxycos<a,b>+1=0(1−w)y2−wy2−cos<a,c>x+2wxycos<a,b>+1=0$\{\begin{array}{c}(1-u)y^2-u{x}^2-cos<b,c>y+2uxycos<a,b>+1=0\\ (1-w)y^2-w{y}^2-cos<a,c>x+2wxycos<a,b>+1=0\end{array}$

其中，在上述公式中，x=OA/OC$x=OA/OC$,y=OB/OC$y=OB/OC$ ,u=BC2/AB2,w=AC2/AB2$u=B{C}^2/A{B}^2,w=A{C}^2/A{B}^2$。

通过以上公式，我们必须知道未知量和已知量，已知量有3个余弦角,u,w是已知的，因此，公式中只有x，y是未知的，因为随着相机的运动会发生改变。类似于求解本质矩阵，该方程最多得到4个解，用验证点来计算最可能的解，得到A，B，C在相机坐标系的3D坐标。

从PnP原理看出，为了求解PnP，我们利用了三角形相似性质，求解投影点a,b,c在相机坐标系的3D坐标（求出x，y比例之后，怎么算出摄像机三维空间点的？暂时还想不通），最后把问题转换成一个3D到3D的过程。

存在的问题： 1.P3P 只利用3个点的信息，无法利用更多的点。2.如果3D点或者2D点受噪声影响，存在误匹配，算法失效。

——-在SLAM中，通常做法是先使用P3P/EPnP等方式估计相机位姿，然后构建最小二乘问题对估计值进行调整。——-

—–1.3 Bundle Ajustment （BA）——

除了使用线性方法之外，我们可以把PnP问题构建成一个定义于李代数上的非线性最小二乘问题。前面的线性方法，除DLT、P3P之外，都是先求相机位姿，再求空间点P，对PnP或ICP给出的结果进行优化。在PnP中的BA问题，是一个最小化重投影问题。

用李代数se(3)来代表T，则像素位置与空间点位置的关系如下：

siui=Kexp(ξ∧)Pi$s_i{\mathbf{u}}_i=Kexp({\xi }^{\wedge })P_i$

由于相机位姿未知且观测点有噪声，构成最小二乘问题，寻找最好的位姿问题

ξ∗=argminξ12∑ni=1||ui−1siKexp(ξ∧)Pi||22${\xi }^{\ast }=arg\underset{\xi }{min}\frac{1}{2}\sum _{i=1}^n||u_i-\frac{1}{s_i}Kexp({\xi }^{\wedge })P_i|{|}_2^2$

该问题的误差项，是将像素坐标(观测到的投影位置)与3D点按照当前估计的位姿进行投影得到的位置相比较得到的误差，所以称为重投影误差。

—–上图所示，p2ˆ$\widehat{p_2}$ 是通过在求出 exp(ξ∧)$exp({\xi }^{\wedge })$ 后然后对P这个空间点进行投影得到的，实际上这俩个具有一定的距离误差**。BA要做的就是将这个距离缩小到最小，但是不一定等于0。——

使用李代数，可以建立无约束的优化问题，可以通过高斯牛顿法、列文伯格-马夸克特方法，但是，我们需要知道其雅克比矩阵。

泰勒展开得到：e(x+Δx)≈e(x)+JΔx$e(x+\mathrm{\Delta }x)\approx e(x)+J\mathrm{\Delta }x$，因此该雅克比矩阵必须为2x6矩阵，将空间点转换到摄像机坐标系里面有：

P′=(exp(ξ∧P)1:3=[X′Y′Z′]T$P^{{}^{\prime }}=(exp({\xi }^{\wedge }P{)}_{1:3}={\left[\begin{array}{ccc}{X}^{\prime }& Y^{\prime }& Z^{\prime }\end{array}\right]}^T$

我们对 ξ∧${\xi }^{\wedge }$ 左乘扰动量 δξ∧$\delta {\xi }^{\wedge }$，然后求e$e$关于扰动量的导数。

{∂e∂δξ=limδξ−>0e(δξ⨁ξ)δξ=∂e∂P′∂P′∂δξ$\{\begin{array}{c}\frac{\mathrm{\partial }e}{\mathrm{\partial }\delta \xi }=\underset{\delta \xi ->0}{lim}\frac{e(\delta \xi ⨁\xi )}{\delta \xi }=\frac{\mathrm{\partial }e}{\mathrm{\partial }{P}^{\prime }}\frac{\mathrm{\partial }{P}^{\prime }}{\mathrm{\partial }\delta \xi }\end{array}$

通过推导，利用李代数的性质，可得2x6雅克比矩阵为：

∂e∂δξ=−⎡⎣⎢fxZ′00fyZ′−fxX′Z′2−fyX′Z′2−fxX′Y′Z′2−fy−fyY′2Z′2fx+fxX2Z′2fyX′Y′Z′2−fxY′Z′fxX′Z′2⎤⎦⎥$\frac{\mathrm{\partial }e}{\mathrm{\partial }\delta \xi }=-\left[\begin{array}{cccccc}\frac{f_x}{Z^{\prime }}& 0& -\frac{f_x{X}^{\prime }}{Z^{\prime 2}}& -\frac{f_x{X}^{\prime }{Y}^{\prime }}{Z^{\prime 2}}& f_x+\frac{f_x{X}^2}{Z^{\prime 2}}& -\frac{f_x{Y}^{\prime }}{Z^{\prime }}\\ 0& \frac{f_y}{Z^{\prime }}& -\frac{f_y{X}^{\prime }}{Z^{\prime 2}}& -f_y-\frac{f_y{Y}^{\prime 2}}{Z^{\prime 2}}& \frac{f_y{X}^{\prime }{Y}^{\prime }}{Z^{\prime 2}}& \frac{f_x{X}^{\prime }}{Z^{\prime 2}}\end{array}\right]$

这个雅克比矩阵描述了重投影误差关于相机位姿李代数的一阶变化关系，前面的负号是因为这公式是观测值减预测值。

另一方面，除了优化位姿，我们还希望优化特征点的空间位置。e关于空间点P的导数，则有

∂e∂P=−⎡⎣⎢fxZ′00fyZ′−fxX′Z′2−fyX′Z′2⎤⎦⎥R$\frac{\mathrm{\partial }e}{\mathrm{\partial }P}=-\left[\begin{array}{ccc}\frac{f_x}{Z^{\prime }}& 0& -\frac{f_x{X}^{\prime }}{Z^{\prime 2}}\\ 0& \frac{f_y}{Z^{\prime }}& -\frac{f_y{X}^{\prime }}{Z^{\prime 2}}\end{array}\right]R$

下面的程序中，首先通过1.png和2.png图片算出配对的特征点keypoints_1和keypoints_2,通过1_depth.png得到keypoints_1在相机1坐标的三维坐标P，同时该坐标相对于相机2(即第二个位姿)为空间坐标系点，通过几个P的坐标值,利用EPnP求出相机1和相机2的R,t值，最后用R，t将P点映射到相机2像素坐标系，与配对时的keypoint_2进行优化(BA优化)，利用了图优化工具。

int main ( int argc, char** argv )
{
    if ( argc != 5 )
    {
        cout<<"usage: pose_estimation_3d2d img1 img2 depth1 depth2"<<endl;
        return 1;
    }
    //-- 读取图像,先找到俩张图片的特征匹配点
    Mat img_1 = imread ( "/home/hansry/Slam_Book/src/Test_trian/1.png/", CV_LOAD_IMAGE_COLOR );
    Mat img_2 = imread ( "/home/hansry/Slam_Book/src/Test_trian/2.png/", CV_LOAD_IMAGE_COLOR );

    vector<KeyPoint> keypoints_1, keypoints_2;
    vector<DMatch> matches;
    find_feature_matches ( img_1, img_2, keypoints_1, keypoints_2, matches );
    cout<<"一共找到了"<<matches.size() <<"组匹配点"<<endl;

    // 建立3D点
    Mat d1 = imread ( "/home/hansry/Slam_Book/src/Test_trian/1_depth.png", CV_LOAD_IMAGE_UNCHANGED );       // 深度图为16位无符号数，单通道图像
    Mat K = ( Mat_<double> ( 3,3 ) << 520.9, 0, 325.1, 0, 521.0, 249.7, 0, 0, 1 );
    vector<Point3f> pts_3d;
    vector<Point2f> pts_2d;
    for ( DMatch m:matches )
    {
        ushort d = d1.ptr<unsigned short> (int ( keypoints_1[m.queryIdx].pt.y )) [ int ( keypoints_1[m.queryIdx].pt.x ) ];//历遍像素，第几行第几列
        if ( d == 0 )   // bad depth
            continue;
        float dd = d/1000.0;//mm->m 
        Point2d p1 = pixel2cam ( keypoints_1[m.queryIdx].pt, K );  //像素点转换成归一化平面上的点
        pts_3d.push_back ( Point3f ( p1.x*dd, p1.y*dd, dd ) );  //基于相机1坐标系的空间坐标,但是对于相机2即为空间坐标系的点
        pts_2d.push_back ( keypoints_2[m.trainIdx].pt );  //第二张图片上的像素值坐标
    }

    cout<<"3d-2d pairs: "<<pts_3d.size() <<endl;

    Mat r, t;
    solvePnP ( pts_3d, pts_2d, K, Mat(), r, t, false ); // 调用OpenCV 的 PnP 求解，可选择EPNP，DLS等方法
    Mat R;
    cv::Rodrigues ( r, R ); // r为旋转向量形式，用Rodrigues公式转换为矩阵

    cout<<"R="<<endl<<R<<endl;
    cout<<"t="<<endl<<t<<endl;

    cout<<"calling bundle adjustment"<<endl;

    bundleAdjustment ( pts_3d, pts_2d, K, R, t );//利用了图优化，g2o
}

在上述程序中，我们使用了BA优化，这里我们将介绍一下，在使用图优化之前，先把问题建模成一个最小二乘问题，如下图所示：

在这个图优化中，节点和边的选择如下：
1.节点： 第二个相机的位姿 ξ∈se(3)$\xi \in se(3)$ ，所有特征点的空间位置P
2.边：每个3D点在第二个相机中的投影，以观测方程来描述：zj=h(ξ,Pj)$z_j=h(\xi ,P_j)$

其中，g2o提供了许多关于BA的节点和边，例如常用到的有VertexSE3Expmap(李代数位姿)、VertexSBAPointXYZ(空间点位置)、EdgeProjectXYZ2UV(投影方程边）等几个类。

—- 2. 3D-3D：ICP (Iterative Closest Point)

3D与3D的位姿估计问题。假如我们有一组配对好的3D点（请注意，这里我们是已经配对好的，比如我们对俩幅RGB-D图像进行了匹配）：

P={p1,⋯,pn}$P=\left\{\begin{array}{c}{p}_1,\cdots ,p_n\end{array}\right\}$，P′={p′1,⋯,p′n}$P^{\prime }=\left\{\begin{array}{c}{p}_1^{\prime },\cdots ,p_n^{\prime }\end{array}\right\}$

现在找到一个欧式变换R，t$R，t$，使得 ∀i,pi=Rp′i+t$\mathrm{\forall }i,p_i=R{p}_i^{{}^{\prime }}+t$

这个问题可以用迭代最近点求解(Iterative Closet Point,ICP)求解，由于我们不知道俩个点集之间的匹配关系，只能认为最近的俩个点为同一个，这个方法称为迭代最近法。在RGB-D SLAM中，可以用这种方式估计相机位姿。

ICP求解方式：利用线性代数的求解（SVD）以及利用非线性优化方式的求解(类似与BA)

2.1 SVD方法

构造最小二乘问题，求使误差达到最小的R，t： minR,tJ=12∑ni=1||pi−(Rp′i+t)||2$\underset{R,t}{min}J=\frac{1}{2}\sum _{i=1}^n||p_i-(R{p}_i^{\prime }+t)|{|}^2$

定义俩组点的质心：p=1n∑ni=1(pi),p′=1n∑ni=1(p′i)$p=\frac{1}{n}\sum _{i=1}^n(p_i),p^{\prime }=\frac{1}{n}\sum _{i=1}^n(p_i^{\prime })$ (质心没有下标)

则通过以下公式得到：

其中 (pi−p−R(p′i−p′))$(p_i-p-R(p_i^{\prime }-p^{\prime }))$该项和为0，那么可以得到优化项为：

12∑ni=1||pi−p−R(p′i−p′)||2+||p−Rp′−t||2$\frac{1}{2}\sum _{i=1}^n||p_i-p-R(p_i^{\prime }-p^{\prime })|{|}^2+||p-R{p}^{\prime }-t|{|}^2$

对于以上优化公式，会发现：右边只有R，而左边既有R也有t，但只与质心有关。所以，只要我们得到了R，另第二项为为0就能得到t。

ICP求解方法可以分为以下三个步骤：
1.求出俩个配对好集合的特征点的质心p和p’，计算每个点的去质心坐标：qi=pi−p,q′i=p′i−p$q_i=p_i-p,q_i^{\prime }=p_i^{\prime }-p$
2.通过 frac12∑ni=1||pi−p−R(p′i−p′)||2+||p−Rp′−t||2$frac12\sum _{i=1}^n||p_i-p-R(p_i^{\prime }-p^{\prime })|{|}^2+||p-R{p}^{\prime }-t|{|}^2$ 左边项求出 R*。
3.根据第2步的R计算t。

从以上看出，只要求出俩组点的R值，要求出t就比较简单了，展开上式，我们有：12∑ni=1||qi−Rq′i||2=12∑ni=1⋯−2qTiRqi$\frac{1}{2}\sum _{i=1}^n||q_i-R{q}_i^{\prime }|{|}^2=\frac{1}{2}\sum _{i=1}^n\cdots -2q_i^{T}R{q}_i$ (这里我们只列出了有关R的项)。

实际上优化目标函数变为 ：∑ni=1−qTiRq′i=∑ni=1−tr(Rq′iqTi)=−tr(R∑ni=1q′iqTi)$\sum _{i=1}^n-q_i^{T}R{q}_i^{\prime }=\sum _{i=1}^n-tr(R{q}_i^{\prime }{q}_i^T)=-tr(R\sum _{i=1}^n{q}_i^{\prime }{q}_i^T)$ (这里不是很懂？？？)

为了解R，先定义矩阵：∑ni=1q′iqTi$\sum _{i=1}^n{q}_i^{\prime }{q}_i^T$，紧接着对W进行SVD分解，得到Σ$\mathrm{\Sigma }$为奇异值组成的对角矩阵，对角线元素从大到小排列，而U$U$和V$V$为对角矩阵。当W满秩时，R=UVT$R=U{V}^T$，紧接着求出t

2.2 非线性优化的方法

求解ICP的另一种方式是使用非线性优化，以迭代的方式去找最小值，用李代数表示位姿时，目标函数可以写成：
undersetεmin=12∑ni=1||pi−exp(ε∧p′i||2$underset\epsilon min=\frac{1}{2}\sum _{i=1}^n||p_i-exp({\epsilon }^{\wedge }{p}_i^{\prime }|{|}^2$，对于单个误差项的增量方程的雅克比矩阵，可通过左乘一个扰动模型，并对改模型进行求导。

ICP非线性优化迭代的过程中，在唯一解的情况下(当然可能出现多解)，只要能找到极小值解，那么这个极小值就是全局最优值，因此不会遇到局部极小而非全局最小的情况。

—————————–我们可以混合着使用PnP和ICP优化：对于深度已知的特征点，建模它们的3D-3D误差；对于深度未知的特征点，则建模3D-2D的重投影误差。于是，可以将误差放到同一个问题中考虑。————————

int main ( int argc, char** argv )
{
    if ( argc != 5 )
    {
        cout<<"usage: pose_estimation_3d3d img1 img2 depth1 depth2"<<endl;
        return 1;
    }
    //-- 读取图像
 Mat img_1 = imread ("/home/hansry/Slam_Book/src/Test_trian/data/1.png", CV_LOAD_IMAGE_COLOR );
 Mat img_2 = imread ( "/home/hansry/Slam_Book/src/Test_trian/data/2.png", CV_LOAD_IMAGE_COLOR );
    vector<KeyPoint> keypoints_1, keypoints_2;
    vector<DMatch> matches;
    find_feature_matches ( img_1, img_2, keypoints_1, keypoints_2, matches );
    cout<<"一共找到了"<<matches.size() <<"组匹配点"<<endl;

    // 建立3D点
    Mat depth1 = imread ( argv[3], CV_LOAD_IMAGE_UNCHANGED );       // 深度图为16位无符号数，单通道图像
    Mat depth2 = imread ( argv[4], CV_LOAD_IMAGE_UNCHANGED );       // 深度图为16位无符号数，单通道图像
    Mat K = ( Mat_<double> ( 3,3 ) << 520.9, 0, 325.1, 0, 521.0, 249.7, 0, 0, 1 );
    vector<Point3f> pts1, pts2;//储存俩个匹配的三维特征点

    for ( DMatch m:matches )
    {
        ushort d1 = depth1.ptr<unsigned short> ( int ( keypoints_1[m.queryIdx].pt.y ) ) [ int ( keypoints_1[m.queryIdx].pt.x ) ];
        ushort d2 = depth2.ptr<unsigned short> ( int ( keypoints_2[m.trainIdx].pt.y ) ) [ int ( keypoints_2[m.trainIdx].pt.x ) ];
        if ( d1==0 || d2==0 )   // bad depth
            continue;
        Point2d p1 = pixel2cam ( keypoints_1[m.queryIdx].pt, K );
        Point2d p2 = pixel2cam ( keypoints_2[m.trainIdx].pt, K );
        float dd1 = float ( d1 ) /1000.0;
        float dd2 = float ( d2 ) /1000.0;//cm->mm
        pts1.push_back ( Point3f ( p1.x*dd1, p1.y*dd1, dd1 ) );
        pts2.push_back ( Point3f ( p2.x*dd2, p2.y*dd2, dd2 ) );
    }

    cout<<"3d-3d pairs: "<<pts1.size() <<endl;
    Mat R, t;
    pose_estimation_3d3d ( pts1, pts2, R, t ); //该函数通过俩组匹配的空间点，经过奇异值分解求出R和t
    cout<<"ICP via SVD results: "<<endl;
    cout<<"R = "<<R<<endl;
    cout<<"t = "<<t<<endl;
    cout<<"R_inv = "<<R.t() <<endl;
    cout<<"t_inv = "<<-R.t() *t<<endl;

    cout<<"calling bundle adjustment"<<endl;

    bundleAdjustment( pts1, pts2, R, t );//进行优化，通过比较俩组空间点的差来对位姿进行优化，利用的是李代数，雅克比矩阵

    // verify p1 = R*p2 + t
    for ( int i=0; i<5; i++ )
    {
        cout<<"p1 = "<<pts1[i]<<endl;
        cout<<"p2 = "<<pts2[i]<<endl;
        cout<<"(R*p2+t) = "<< 
            R * (Mat_<double>(3,1)<<pts2[i].x, pts2[i].y, pts2[i].z) + t
            <<endl;
        cout<<endl;
    }
}

——-至此，我们讨论了orb特征匹配(FAST关键点和BREIF描述子)，通过汉明距离来进行判断匹配，然后讨论了2D-2D(通过匹配像素值求出本质矩阵，进而求出R和t，同时也可通过三角测量来进行深度估计，这个针对单目），3D-2D(通过直接线性变换或者PnP利用相机一坐标系的点求出相机二坐标系的点，然后相机二的点投影到相机然后进行BA优化)，3D-3D(通过ICP：SVD(线性优化)，非线性优化，求出R,t,然后对俩组空间点进行BA优化)——–