SLAM代码课程ORB_SLAM2

2020-02-17

Source

Edit

History

第一讲-ORB_SLAM2_预备知识_1

Catalogue

1. 1. ORB_SLAM2
2. 2. 预备知识【1】

1. ORB_SLAM2

2. 预备知识【1】

2.1. ICP SE3/Sim3求解

2.1.1. 已知n对匹配的3D点----3点法求R

这3个点实际上是世界坐标系W中的点，即世界坐标系中的点$p_1,p_2,p_3$构造出一个新的坐标系O，其中$p_1$的意义就相当于相机的坐标
根据这3个点$p_1,p_2,p_3$，可以得到坐标系O的三个轴的方向在世界坐标系W的表示
根据坐标系O的三个轴的方向在世界坐标系W的表示，可以写出从世界坐标系W到坐标系O的旋转变换$R_{w2o}=[\bar{x},\bar{y},\bar{z}]$，(我自己而言还是比较习惯写成$R_{o\leftarrow w}=[\bar{x},\bar{y},\bar{z}]$的形式)
如果知道两组这样的的点，并且这两组点一一匹配
那么就可以写出这从上面坐标系O到下面坐标系O'的旋转变换$R_{o2o'}=R_{w2o'}R_{w2o}^T$，(我自己而言还是比较习惯写成$R_{o'\leftarrow o}=R_{o'\leftarrow w}R_{o\leftarrow w}^T$的形式)

当匹配对数n>3，则可以采用最小二乘的方法，最小化误差

2.1.2. 求Sim3尺度因子

上图中，$p_i'=p_i-p^-$，$q_i'=q_i-q^-$

2.1.2.1. 尺度因子的正解：

2.1.3. 求旋转R的四元数方法

2.2. 3D-2D PnP

2.2.1. EPnP

需要4个点是因为原点+3个方向

得到4个控制点分别是c0, c1, c2, c3 模长均为1

世界坐标系下的每个点$x_i^w$都可以用4个控制点以及映射关系$[\alpha_1,\alpha_2,\alpha_3,\alpha_4]^T$来表示

EPNP 省略一部分

2.2.2. DLT算法

2.3. 2D-3D 三角化

2.3.1. 单目

2.3.2. 双目立体

2.4. 2D-2D Homography/Fundamental 对极几何

2.4.1. 基础矩阵与本质矩阵

极线$l'=e' \times u$ 的意思是, 这里的极线 $l'$ 指的是这条极线在相机平面上的法向量
换句话说, 相机平面上极线上的两个点叉乘, 即向量叉乘$e' \times u$ , 得到极线所在平面上的法向量$(a,b,c)$
这个法向量可以构成极线点法式方程, 设极点$e(e_x,e_y,1)$ , 那么极线方程为 $ae_x+be_y+c=0$
利用这个法向量, 可以出点$p_{proj}=[p_x,p_y,1]^T$到这条直线的距离 $d=\frac{|ae_x+be_y+c|}{\sqrt{a^2+b^2}}=\frac{l'*p_{proj}}{\sqrt{a^2+b^2}}$
因此, 这个距离也就是极线约束, 利用这个距离可以判断基础矩阵F的好坏 (ORB_SLAM2 Initializer::CheckFundamental()函数就是这么计算初始化的基础矩阵F得分 )

e'和u不是指二维像素点坐标(u,v)，二维没法做叉乘， e'应该指的是相机坐标系O'到像素点e'的向量，u类比，这样才能做叉乘，叉乘完之后得到的是极线l'在平面上的法向量，这个法向量用来计算投影点到极线的距离。(点法式方程)

注意 8点法的8个点不能在同一个平面上, 如墙壁
原因是3D点共面时, 就满足单应性$\bar{x}'=H\bar{x}$
后话:　所以, 一般操作时都会计算F和H, 两种情况, 看哪种情况下更好
另外: H矩阵的筛选点能力要强一些( 去除outlier ? )
另外: 纯旋转不能用求F

由基础矩阵约束 $u_2^T*F*u_1=0$
可以得到,使用归一化的点代入,有: $\bar{u}_2^T \bar{F} \bar{u}_1'=0$
因此,使用归一化的点代入约束方程,可以求出$\bar{F}$
又因为$\bar{u}_2'=T_2 u_2 , \bar{u}_1'=T_1 u_1$
$\bar{u}_2^T \bar{F} \bar{u}_1' = (T_2 u_2)^T \bar{F} (T_1 u_1) = u_2^T(T_2^T \bar{F} * T_1)* u_1 = 0$
所以,最终要求归一化之前的F
$u_2^T(T_2^T \bar{F} * T_1)* u_1=u_2^T*F*u_1=0$
$F=T_2^T \bar{F} * T_1$

ORB_SLAM2中归一化变换代码

void Initializer::Normalize(const vector<cv::KeyPoint> &vKeys, vector<cv::Point2f> &vNormalizedPoints, cv::Mat &T)
{
    ///归一化变换
    // 见:https://epsilonjohn.gitee.io/2020/02/17/%E7%AC%AC%E4%B8%80%E8%AE%B2-ORB-SLAM2-%E9%A2%84%E5%A4%87%E7%9F%A5%E8%AF%86-1/#d-2d-homographyfundamental-%E5%AF%B9%E6%9E%81%E5%87%A0%E4%BD%95
    float meanX = 0;
    float meanY = 0;
    const int N = vKeys.size();

    // 这是要输出的归一化之后的点
    vNormalizedPoints.resize(N);

    //求平均值
    for(int i=0; i<N; i++)
    {
        meanX += vKeys[i].pt.x;
        meanY += vKeys[i].pt.y;
    }

    meanX = meanX/N;
    meanY = meanY/N;

    float meanDevX = 0;
    float meanDevY = 0;

    for(int i=0; i<N; i++)
    {
        vNormalizedPoints[i].x = vKeys[i].pt.x - meanX;
        vNormalizedPoints[i].y = vKeys[i].pt.y - meanY;

        meanDevX += fabs(vNormalizedPoints[i].x);
        meanDevY += fabs(vNormalizedPoints[i].y);
    }

    // 尺度缩放
    meanDevX = meanDevX/N;
    meanDevY = meanDevY/N;

    float sX = 1.0/meanDevX;
    float sY = 1.0/meanDevY;

    for(int i=0; i<N; i++)
    {
        vNormalizedPoints[i].x = vNormalizedPoints[i].x * sX;
        vNormalizedPoints[i].y = vNormalizedPoints[i].y * sY;
    }

    // 构建归一化矩阵
    T = cv::Mat::eye(3,3,CV_32F);
    T.at<float>(0,0) = sX;
    T.at<float>(1,1) = sY;
    T.at<float>(0,2) = -meanX*sX;
    T.at<float>(1,2) = -meanY*sY;
}

ORB_SLAM2中计算基础矩阵F的代码(8点法)

void Initializer::FindFundamental(vector<bool> &vbMatchesInliers, float &score, cv::Mat &F21)
{
    // Number of putative matches
    const int N = vbMatchesInliers.size();

    // Normalize coordinates
    // 归一化点
    vector<cv::Point2f> vPn1, vPn2;
    cv::Mat T1, T2;
    Normalize(mvKeys1,vPn1, T1);
    Normalize(mvKeys2,vPn2, T2);
    cv::Mat T2t = T2.t();

    // Best Results variables
    // 输出
    score = 0.0;
    vbMatchesInliers = vector<bool>(N,false);

    // Iteration variables
    vector<cv::Point2f> vPn1i(8);
    vector<cv::Point2f> vPn2i(8);
    cv::Mat F21i;
    vector<bool> vbCurrentInliers(N,false);
    float currentScore;

    // Perform all RANSAC iterations and save the solution with highest score
    for(int it=0; it<mMaxIterations; it++)
    {
        // Select a minimum set
        for(int j=0; j<8; j++)
        {
            int idx = mvSets[it][j];

            //mvMatches12[i]=pair(参考帧特征点idx,当前帧特征点idx)
            vPn1i[j] = vPn1[mvMatches12[idx].first];
            vPn2i[j] = vPn2[mvMatches12[idx].second];
        }

        //计算出归一化特征点对应的基础矩阵
        //由基础矩阵约束 u2'*F*u1=0
        //可以得到,使用归一化的点代入,有:  _u2' * _F * _u1 =0
        //下面求出来的Fn就是 上式子的 _F
        cv::Mat Fn = ComputeF21(vPn1i,vPn2i);

        //又根据 _u2=T2*u2 , _u1=T1*u1
        //可得: _u2' * _F * _u1 = (T2*u2)' * _F *(T1*u1) = u2'*(T2' * _F * T1)* u1 = 0
        //即  :u2'*(T2' * _F * T1)* u1 = u2' * F * u1=0
        //所以: 当我们求解[u2'*F*u1=0]中的F的时候, 实际上这个F应该等于(T2' * _F * T1)

        //转换成归一化前特征点对应的基础矩阵
        //也就是求出没有归一化的点代入方程[u2'*F*u1=0]得到的F
        F21i = T2t*Fn*T1;

        //在参数 mSigma下，能够通过F21li，
        //重投影成功的点有哪些，并返回分数
        currentScore = CheckFundamental(F21i, vbCurrentInliers, mSigma);

        //储存最高分的情况下的基础矩阵F
        if(currentScore>score)
        {
            F21 = F21i.clone(); //F
            vbMatchesInliers = vbCurrentInliers;    //内点判断(std::vector)(判断哪些特征点对是内点)
            score = currentScore;
        }
    }
}
    
//8点法求基础矩阵F,并根据约束进行调整F
cv::Mat Initializer::ComputeF21(const vector<cv::Point2f> &vP1,const vector<cv::Point2f> &vP2)
{
    const int N = vP1.size();

    cv::Mat A(N,9,CV_32F);

    //八点法计算F

    //经过线性变换 DLT
    //构造矩阵A=u2^T F
    for(int i=0; i<N; i++)
    {
        const float u1 = vP1[i].x;
        const float v1 = vP1[i].y;
        const float u2 = vP2[i].x;
        const float v2 = vP2[i].y;

        A.at<float>(i,0) = u2*u1;
        A.at<float>(i,1) = u2*v1;
        A.at<float>(i,2) = u2;
        A.at<float>(i,3) = v2*u1;
        A.at<float>(i,4) = v2*v1;
        A.at<float>(i,5) = v2;
        A.at<float>(i,6) = u1;
        A.at<float>(i,7) = v1;
        A.at<float>(i,8) = 1;
    }

    cv::Mat u,w,vt;
    cv::SVDecomp(A,w,u,vt,cv::SVD::MODIFY_A | cv::SVD::FULL_UV);

    //用SVD算出基础矩阵
    //vt [8x8] : 由特征向量组成的矩阵, 每个特征向量都是9维的
    //vt.row(8):矩阵A经过SVD分解之后最小奇异值对应的特征向量9 维
    //重新reshape成3x3矩阵,得到基础矩阵F
    cv::Mat Fpre = vt.row(8).reshape(0, 3);

    ///下面进行对基础矩阵F进行约束调整
    //将基础矩阵svd分解
    cv::SVDecomp(Fpre,w,u,vt,cv::SVD::MODIFY_A | cv::SVD::FULL_UV);

    //根据基础矩阵的性质分解出来的w第三个元素应该为0
    w.at<float>(2)=0;

    //返回复合要求的基础矩阵
    return  u*cv::Mat::diag(w)*vt;
}

ORB_SLAM2中利用基础矩阵F约束进行重投影的代码

//利用基础矩阵F进行重投影,计算得分 [理解对极约束]
float Initializer::CheckFundamental(const cv::Mat &F21, vector<bool> &vbMatchesInliers, float sigma)
{
    const int N = mvMatches12.size();

    const float f11 = F21.at<float>(0,0);
    const float f12 = F21.at<float>(0,1);
    const float f13 = F21.at<float>(0,2);
    const float f21 = F21.at<float>(1,0);
    const float f22 = F21.at<float>(1,1);
    const float f23 = F21.at<float>(1,2);
    const float f31 = F21.at<float>(2,0);
    const float f32 = F21.at<float>(2,1);
    const float f33 = F21.at<float>(2,2);
    
    //std::vector<bool> vbMatchesInliers是输出,输出哪些匹配点对是内点
    vbMatchesInliers.resize(N);
    
    float score = 0;
    
    const float th = 3.841;
    const float thScore = 5.991;
    
    const float invSigmaSquare = 1.0/(sigma*sigma);
    
    for(int i=0; i<N; i++)  //遍历每一个特征点对
    {
        bool bIn = true;
    
        //mvMatches12[i]=pair(参考帧特征点idx,当前帧特征点idx) ,这两个特征点认为相互匹配, 现在取出来看是否真的匹配
        const cv::KeyPoint &kp1 = mvKeys1[mvMatches12[i].first];
        const cv::KeyPoint &kp2 = mvKeys2[mvMatches12[i].second];
    
        const float u1 = kp1.pt.x;
        const float v1 = kp1.pt.y;
        const float u2 = kp2.pt.x;
        const float v2 = kp2.pt.y;
    
        /****************************************************
         * 利用F重投影
         * (0)由极线约束的示意图可知: 极线l'在图像平面上的法向量可以由
         *    极线上的两点叉乘得到
         * (1)根据公式l'=F*u ==> 极线法向量 l2=F21 * x1  ===>极线法向量(a,b,c)
         * (2)于是可以得到极线点法式方程: a(X-u1)+b(Y-v1)+c=0
         * (3)假设在第一帧的点 x1 =[u1,v1,1]^T
         * (4)根据点到直线距离公式 dist= |a*u1+b*u2+c|/ sqrt(a^2+b^2)
         ****************************************************/
    
        ///1. 将第一帧的特征点重投影到第二帧,计算误差
        // l2=F21x1=[a2,b2,c2]^T (3x1)列向量
        // F= [ f11 f12 f13
        //      f21 f22 f23
        //      f31 f32 f33 ]
        //
        // f1=[f11 f12 f13] U1=[u1 v1 1]^T  ===> a2=f1*U1
        // 这里计算得到极线向量l2=[a2,b2,c2]
        const float a2 = f11*u1+f12*v1+f13;     // (Fu)_x
        const float b2 = f21*u1+f22*v1+f23;     // (Fu)_y
        const float c2 = f31*u1+f32*v1+f33;
    
        // 计算点x1到直线l2距离
        const float num2 = a2*u2+b2*v2+c2;
    
        // 距离的平方
        const float squareDist1 = num2*num2/(a2*a2+b2*b2);
    
        const float chiSquare1 = squareDist1*invSigmaSquare;
    
        //判断距离是否超过阈值,并计算得分,距离越大,得分越小
        if(chiSquare1>th)
            bIn = false;
        else
            score += thScore - chiSquare1;
    
        ///2. 将第二帧的特征点重投影到第一帧,计算误差(思路基本同上,计算点到直线距离)
        // l1 =x2tF21=[a1,b1,c1] (1x3) 行向量
        const float a1 = f11*u2+f21*v2+f31;
        const float b1 = f12*u2+f22*v2+f32;
        const float c1 = f13*u2+f23*v2+f33;
    
        const float num1 = a1*u1+b1*v1+c1;
    
        const float squareDist2 = num1*num1/(a1*a1+b1*b1);
    
        const float chiSquare2 = squareDist2*invSigmaSquare;
    
        //判断距离是否超过阈值,并计算得分,距离越大,得分越小
        if(chiSquare2>th)
            bIn = false;
        else
            score += thScore - chiSquare2;
    
        //设置标志: 描述这对特征点是否是inlier
        if(bIn)
            vbMatchesInliers[i]=true;
        else
            vbMatchesInliers[i]=false;
    }
    return score;
}

ORB_SLAM2中分解基础矩阵F得到R,t 代码

bool Initializer::ReconstructF(vector<bool> &vbMatchesInliers, cv::Mat &F21, cv::Mat &K,
                            cv::Mat &R21, cv::Mat &t21, vector<cv::Point3f> &vP3D, vector<bool> &vbTriangulated, float minParallax, int minTriangulated)
{
    int N=0;
    //匹配点中
    for(size_t i=0, iend = vbMatchesInliers.size() ; i<iend; i++)
        if(vbMatchesInliers[i])
            N++;

    // Compute Essential Matrix from Fundamental Matrix
    // 从基础矩阵F计算出本质矩阵E (利用内参K)
    cv::Mat E21 = K.t()*F21*K;

    cv::Mat R1, R2, t;

    // Recover the 4 motion hypotheses (从E分解出4种可能的情况)
    // 从矩阵E分解出R,t ,其中分解出两个R
    DecomposeE(E21,R1,R2,t);  

    // t又可以分为+t和-t
    cv::Mat t1=t;
    cv::Mat t2=-t;

    // Reconstruct with the 4 hyphoteses and check
    vector<cv::Point3f> vP3D1, vP3D2, vP3D3, vP3D4;
    vector<bool> vbTriangulated1,vbTriangulated2,vbTriangulated3, vbTriangulated4;
    float parallax1,parallax2, parallax3, parallax4;

    //mvKeys1:第一帧特征点 mvKeys2:第二帧特征点
    //mvMatches12:储存着匹配点对在参考帧F1特征点数组和当前帧F2特征点数组中的序号
    //vbMatchesInliers:有效的匹配点对
    int nGood1 = CheckRT(R1,t1,mvKeys1,mvKeys2,mvMatches12,vbMatchesInliers,K, vP3D1, 4.0*mSigma2, vbTriangulated1, parallax1);
    int nGood2 = CheckRT(R2,t1,mvKeys1,mvKeys2,mvMatches12,vbMatchesInliers,K, vP3D2, 4.0*mSigma2, vbTriangulated2, parallax2);
    int nGood3 = CheckRT(R1,t2,mvKeys1,mvKeys2,mvMatches12,vbMatchesInliers,K, vP3D3, 4.0*mSigma2, vbTriangulated3, parallax3);
    int nGood4 = CheckRT(R2,t2,mvKeys1,mvKeys2,mvMatches12,vbMatchesInliers,K, vP3D4, 4.0*mSigma2, vbTriangulated4, parallax4);

    //取得分最高的
    int maxGood = max(nGood1,max(nGood2,max(nGood3,nGood4)));

    R21 = cv::Mat();
    t21 = cv::Mat();

    //minTriangulated=50
    int nMinGood = max(static_cast<int>(0.9*N),minTriangulated);

    //如果有两种情况的R,t得分都差不多,没有明显差别,nsimilar将>1,这次初始化失败
    int nsimilar = 0;
    if(nGood1>0.7*maxGood)
        nsimilar++;
    if(nGood2>0.7*maxGood)
        nsimilar++;
    if(nGood3>0.7*maxGood)
        nsimilar++;
    if(nGood4>0.7*maxGood)
        nsimilar++;

    // If there is not a clear winner or not enough triangulated points reject initialization
    //nsimilar>1表明没有哪个模型明显胜出
    //匹配点三角化重投影成功数过少
    if(maxGood<nMinGood || nsimilar>1)
    {
        return false;
    }

    // If best reconstruction has enough parallax initialize
    // 如果某种情况的R,t得分比较高,则取该情况的R,t
    // 并将三角化的点拷贝,R,t拷贝,三角化标志位拷贝
    if(maxGood==nGood1)
    {
	//如果模型一对应的视差角大于最小值
        if(parallax1>minParallax)
        {
            vP3D = vP3D1;
            vbTriangulated = vbTriangulated1;

            R1.copyTo(R21);
            t1.copyTo(t21);
            return true;
        }
    }else if(maxGood==nGood2)
    {
        if(parallax2>minParallax)
        {
            vP3D = vP3D2;
            vbTriangulated = vbTriangulated2;

            R2.copyTo(R21);
            t1.copyTo(t21);
            return true;
        }
    }else if(maxGood==nGood3)
    {
        if(parallax3>minParallax)
        {
            vP3D = vP3D3;
            vbTriangulated = vbTriangulated3;

            R1.copyTo(R21);
            t2.copyTo(t21);
            return true;
        }
    }else if(maxGood==nGood4)
    {
        if(parallax4>minParallax)
        {
            vP3D = vP3D4;
            vbTriangulated = vbTriangulated4;

            R2.copyTo(R21);
            t2.copyTo(t21);
            return true;
        }
    }
    return false;
}

2.4.2. Homography模型

$d$表示的是平面到相机坐标系原点O的距离, 上面的"其中d表示~"说法有误。

下面给出"白巧克力亦违心"的博客内容:

注意: 上面的单应矩阵$\hat{H}$是包含了尺度项$\lambda$, 所以$\hat{H}=\lambda H$ , 可以把h9看做是包含了$\lambda$的项, 所以, 这里(ORB_SLAM2)的 H 矩阵参数为9个
这里 $Ah=0$ 与<视觉SLAM14讲-第二版 P171 公式7.20>的两个约束是对应的, 不同的地方是这里没有把h9看做是1, 这里的h9包含$\lambda$项, 把它看做参数一起求了.

ORB_SLAM2计算单应矩阵H代码

void Initializer::FindHomography(vector<bool> &vbMatchesInliers, float &score, cv::Mat &H21)
{
    // Number of putative matches
    //假定匹配的数量
    const int N = mvMatches12.size();

    // Normalize coordinates
    // 坐标归一化
    vector<cv::Point2f> vPn1, vPn2;
    cv::Mat T1, T2;
    Normalize(mvKeys1,vPn1, T1);
    Normalize(mvKeys2,vPn2, T2);
    cv::Mat T2inv = T2.inv();   //求出T2^{-1}
    
    // Best Results variables
    // 这是要输出的,描述某个特征点是否内点
    score = 0.0;
    vbMatchesInliers = vector<bool>(N,false);
    
    // Iteration variables
    vector<cv::Point2f> vPn1i(8);
    vector<cv::Point2f> vPn2i(8);
    cv::Mat H21i, H12i;
    vector<bool> vbCurrentInliers(N,false);
    float currentScore;
    
    // Perform all RANSAC iterations and save the solution with highest score
    for(int it=0; it<mMaxIterations; it++)
    {
        // RANSAC迭代200次,取得分最高情况下算出来的单应矩阵H
        // Select a minimum set 每个集合8对点(8点法)
        // mvSets[当前迭代次数][0~7]= 某个最小集合随机索引idx
        for(size_t j=0; j<8; j++)
        {
            int idx = mvSets[it][j];    //idx用来取某一堆匹配点对
            //mvMatches12[i]=pair(参考帧特征点idx,当前帧特征点idx), 这是匹配的特征点
            vPn1i[j] = vPn1[mvMatches12[idx].first];
            vPn2i[j] = vPn2[mvMatches12[idx].second];
        }
    
        //计算H
        cv::Mat Hn = ComputeH21(vPn1i,vPn2i);
        H21i = T2inv*Hn*T1; //注意: 这里是T2的逆,不是转置, 原因是约束方程里是叉乘关系, u2 X H * u1 = 0
        H12i = H21i.inv();
    
        //在参数 mSigma下，能够通过H21，H12重投影成功的点有哪些，并返回分数
        currentScore = CheckHomography(H21i, H12i, vbCurrentInliers, mSigma);
    
        //只取最高得分情况下的H以及内点判断vbMatchesInliers
        if(currentScore>score)
        {
            H21 = H21i.clone();
            vbMatchesInliers = vbCurrentInliers;
            score = currentScore;
        }
    }
}

//SVD分解求H21矩阵
cv::Mat Initializer::ComputeH21(const vector<cv::Point2f> &vP1, const vector<cv::Point2f> &vP2)
{
    const int N = vP1.size();

    cv::Mat A(2*N,9,CV_32F);
    
    //虽然书上说最少用4个点对就可以解出单应矩阵，但是这里依然用的是8个点对
    for(int i=0; i<N; i++)
    {
        //构造A矩阵
        const float u1 = vP1[i].x;
        const float v1 = vP1[i].y;
        const float u2 = vP2[i].x;
        const float v2 = vP2[i].y;
    
        A.at<float>(2*i,0) = 0.0;
        A.at<float>(2*i,1) = 0.0;
        A.at<float>(2*i,2) = 0.0;
        A.at<float>(2*i,3) = -u1;
        A.at<float>(2*i,4) = -v1;
        A.at<float>(2*i,5) = -1;
        A.at<float>(2*i,6) = v2*u1;
        A.at<float>(2*i,7) = v2*v1;
        A.at<float>(2*i,8) = v2;
    
        A.at<float>(2*i+1,0) = u1;
        A.at<float>(2*i+1,1) = v1;
        A.at<float>(2*i+1,2) = 1;
        A.at<float>(2*i+1,3) = 0.0;
        A.at<float>(2*i+1,4) = 0.0;
        A.at<float>(2*i+1,5) = 0.0;
        A.at<float>(2*i+1,6) = -u2*u1;
        A.at<float>(2*i+1,7) = -u2*v1;
        A.at<float>(2*i+1,8) = -u2;
    }
    
    cv::Mat u,w,vt;
    
    //SVD分解A=u*w*vt
    cv::SVDecomp(A,w,u,vt,cv::SVD::MODIFY_A | cv::SVD::FULL_UV);
    
    //返回了vt最后一个行向量 9维的向量
    //reshape成3x3
    return vt.row(8).reshape(0, 3);
}

ORB_SLAM2利用单应矩阵H进行重投影约束代码

//通过单应矩阵H21以及H21_inv 反投影,计算误差,得到当前单应矩阵H的分数
float Initializer::CheckHomography(const cv::Mat &H21, const cv::Mat &H12, vector<bool> &vbMatchesInliers, float sigma)
{   
    const int N = mvMatches12.size();

    const float h11 = H21.at<float>(0,0);
    const float h12 = H21.at<float>(0,1);
    const float h13 = H21.at<float>(0,2);
    const float h21 = H21.at<float>(1,0);
    const float h22 = H21.at<float>(1,1);
    const float h23 = H21.at<float>(1,2);
    const float h31 = H21.at<float>(2,0);
    const float h32 = H21.at<float>(2,1);
    const float h33 = H21.at<float>(2,2);
    
    const float h11inv = H12.at<float>(0,0);
    const float h12inv = H12.at<float>(0,1);
    const float h13inv = H12.at<float>(0,2);
    const float h21inv = H12.at<float>(1,0);
    const float h22inv = H12.at<float>(1,1);
    const float h23inv = H12.at<float>(1,2);
    const float h31inv = H12.at<float>(2,0);
    const float h32inv = H12.at<float>(2,1);
    const float h33inv = H12.at<float>(2,2);
    
    vbMatchesInliers.resize(N);
    
    float score = 0;
    
    //判断通过单应矩阵重投影是否成功的阈值
    const float th = 5.991;
    
    const float invSigmaSquare = 1.0/(sigma*sigma);
    
    //遍历所有的匹配点
    for(int i=0; i<N; i++)
    {
        bool bIn = true;
    
        const cv::KeyPoint &kp1 = mvKeys1[mvMatches12[i].first];
        const cv::KeyPoint &kp2 = mvKeys2[mvMatches12[i].second];
    
        const float u1 = kp1.pt.x;
        const float v1 = kp1.pt.y;
        const float u2 = kp2.pt.x;
        const float v2 = kp2.pt.y;
    
        // Reprojection error in first image
        // x2=[u2,v2,1]^T  x1=[u1,v1,1]^T
        // 这里的 H_21_inv = [h11_inv h12_inv h12_inv
        //                   h21_inv h22_inv h23_inv
        //                   h31_inv h32_inv h33_inv ]
        // H模型: 原模型: x2 = \hat{H_21} * x1   逆模型(左乘\hat{H_21}): H_21^{-1} * x2 = x1
        // 逆模型就是将第2帧的特征点利用H21_inv矩阵反投影到第一帧
        // 理解的时候,可以结合<视觉SLAM14讲-第二版 P171 公式7.20>来看
        const float w2in1inv = 1.0/(h31inv*u2+h32inv*v2+h33inv);    //把H矩阵第3行的约束嵌入到第一第二行中,相当于归一化?
        const float u2in1 = (h11inv*u2+h12inv*v2+h13inv)*w2in1inv;  
        const float v2in1 = (h21inv*u2+h22inv*v2+h23inv)*w2in1inv;
    
        //计算u2，v2投影到F1后与u1,v1的距离的平方，也就是重投影误差
        //H模型几何距离,使用对称转移误差,见<计算机视觉中的多视图几何> P58 公式3.7
        const float squareDist1 = (u1-u2in1)*(u1-u2in1)+(v1-v2in1)*(v1-v2in1);
    
        const float chiSquare1 = squareDist1*invSigmaSquare;
    
        //chiSquare1>th说明匹配的点对F1投影到F2，重投影失败
        if(chiSquare1>th)
            bIn = false;
        else
            score += th - chiSquare1;
    
        // Reprojection error in second image
        // x1in2 = H21*x1
        // H模型: 原模型: x2 = \hat{H_21} * x1
        // 步骤基本同上
        const float w1in2inv = 1.0/(h31*u1+h32*v1+h33);
        const float u1in2 = (h11*u1+h12*v1+h13)*w1in2inv;
        const float v1in2 = (h21*u1+h22*v1+h23)*w1in2inv;
    
        const float squareDist2 = (u2-u1in2)*(u2-u1in2)+(v2-v1in2)*(v2-v1in2);
    
        const float chiSquare2 = squareDist2*invSigmaSquare;
    
        if(chiSquare2>th)
            bIn = false;
        else
            score += th - chiSquare2;
    
    //bIn标志着此对匹配点是否重投影成功
        if(bIn)
            vbMatchesInliers[i]=true;
        else
            vbMatchesInliers[i]=false;
    }
    return score;
}

2.4.3. 2D-2D模型约束与外点剔除

下面是分解的方法推导, 可暂时不看

Faugeras SVD 解法求R,t

对右侧得到的3个方程(2) (3) (4) 两两之间乘以$t^{'}$的系数, 例如将方程(2)乘以$x_2$ , 将方程(3)乘以$x_1$, 两式相减, 可以得到一个消去$t^{'}$的方程, 两两操作, 一共得到三个新的方程如下

技巧1: 利用旋转变换不改变向量模长的性质, 可以对等式两边同时取范数, 即可消去旋转矩阵$R$
技巧2: 根据克莱姆法则，系数行列式delta不等于0时, 线性方程组只有唯一零解。因此, 要使齐次线性方程组有非零解, 则系数矩阵的行列式要等于0 . 性质用于上面的方程(6)
证明中: 由于$[x_1,x_2,x_3]^T$是法向量$n^{'}$, 所以模长不可能为0
最后得到距离$d^{'}$等于特征值$d_2$