SLAM源码分析（八）

2021SC@SDUSC

Initializer.cc，ComputeH21函数，用DLT方法求解单应矩阵H。

vP1：参考帧中归一化后的特征点；vP2：当前帧中归一化后的特征点；

基本原理：

|x'|         | h1 h2 h3 ||x|

|y'| = a   | h4 h5 h6 ||y|  简写: x' = a H x, a为一个尺度因子

1 |          | h7 h8 h9 ||1|

使用DLT(direct linear tranform)求解该模型

 x' = a H x

---> (x') 叉乘 (H x)  = 0  (因为方向相同) (取前两行就可以推导出下面的了)

---> Ah = 0

A = | 0  0  0 -x -y -1 xy' yy' y'|  h = | h1 h2 h3 h4 h5 h6 h7 h8 h9 |

      |-x -y -1  0  0  0 xx' yx' x'|

通过SVD求解Ah = 0，A^T*A最小特征值对应的特征向量即为解。

cv::Mat Initializer::ComputeH21(
    const vector &vP1, //归一化后的点, in reference frame
    const vector &vP2) //归一化后的点, in current frame
{
    cv::Mat A(2*N,				//行，注意每一个点的数据对应两行
			  9,				//列
			  CV_32F);      	//float数据类型

	// 构造矩阵A，将每个特征点添加到矩阵A中的元素
    for(int i=0; i(2*i,0) = 0.0;
        A.at(2*i,1) = 0.0;
        A.at(2*i,2) = 0.0;
        A.at(2*i,3) = -u1;
        A.at(2*i,4) = -v1;
        A.at(2*i,5) = -1;
        A.at(2*i,6) = v2*u1;
        A.at(2*i,7) = v2*v1;
        A.at(2*i,8) = v2;

		//生成这个点的第二行
        A.at(2*i+1,0) = u1;
        A.at(2*i+1,1) = v1;
        A.at(2*i+1,2) = 1;
        A.at(2*i+1,3) = 0.0;
        A.at(2*i+1,4) = 0.0;
        A.at(2*i+1,5) = 0.0;
        A.at(2*i+1,6) = -u2*u1;
        A.at(2*i+1,7) = -u2*v1;
        A.at(2*i+1,8) = -u2;

    }

    // 定义输出变量，u是左边的正交矩阵U， w为奇异矩阵，vt中的t表示是右正交矩阵V的转置
    cv::Mat u,w,vt;

	//使用opencv提供的进行奇异值分解的函数
    cv::SVDecomp(A,							//输入，待进行奇异值分解的矩阵
				 w,							//输出，奇异值矩阵
				 u,							//输出，矩阵U
				 vt,						//输出，矩阵V^T
				 cv::SVD::MODIFY_A | 		//输入，MODIFY_A是指允许计算函数可以修改待分解的矩阵，官方文档上说这样可以加快计算速度、节省内存
				     cv::SVD::FULL_UV);		//FULL_UV=把U和VT补充成单位正交方阵

	// 返回最小奇异值所对应的右奇异向量
    return vt.row(8).reshape(0, 			//转换后的通道数，这里设置为0表示是与前面相同
							 3); 			//转换后的行数,对应V的最后一列
}

ComputeF21函数，通过八点法，根据特征点匹配求fundamental matrix；因为F矩阵有秩为2的约束，所以需要两次SVD分解。

x'Fx = 0 整理可得：Af = 0

A = | x'x x'y x' y'x y'y y' x y 1 |, f = | f1 f2 f3 f4 f5 f6 f7 f8 f9 |

通过SVD求解Af = 0，A'A最小特征值对应的特征向量即为解

cv::Mat Initializer::ComputeF21(
    const vector &vP1, //归一化后的点, in reference frame
    const vector &vP2) //归一化后的点, in current frame
{
	//获取参与计算的特征点对数
    const int N = vP1.size();

	//初始化A矩阵
    cv::Mat A(N,9,CV_32F); // N*9维

    // 构造矩阵A，将每个特征点添加到矩阵A中的元素
    for(int i=0; i(i,0) = u2*u1;
        A.at(i,1) = u2*v1;
        A.at(i,2) = u2;
        A.at(i,3) = v2*u1;
        A.at(i,4) = v2*v1;
        A.at(i,5) = v2;
        A.at(i,6) = u1;
        A.at(i,7) = v1;
        A.at(i,8) = 1;
    }

    //存储奇异值分解结果的变量
    cv::Mat u,w,vt;

    // 定义输出变量，u是左边的正交矩阵U， w为奇异矩阵，vt中的t表示是右正交矩阵V的转置
    cv::SVDecomp(A,w,u,vt,cv::SVD::MODIFY_A | cv::SVD::FULL_UV);
	// 转换成基础矩阵的形式
    cv::Mat Fpre = vt.row(8).reshape(0, 3); // v的最后一列

    //基础矩阵的秩为2,而我们不敢保证计算得到的这个结果的秩为2,所以需要通过第二次奇异值分解,来强制使其秩为2
    // 对初步得来的基础矩阵进行第2次奇异值分解
    cv::SVDecomp(Fpre,w,u,vt,cv::SVD::MODIFY_A | cv::SVD::FULL_UV);

	// 秩2约束，强制将第3个奇异值设置为0
    w.at(2)=0; 
    
    // 重新组合好满足秩约束的基础矩阵，作为最终计算结果返回 
    return  u*cv::Mat::diag(w)*vt;
}

CheckHomography函数，对给定的homography matrix打分。

在已值n维观测数据误差服从N(0，sigma）的高斯分布时

其误差加权最小二乘结果为  sum_error = SUM(e(i)^T * Q^(-1) * e(i))

其中：e(i) = [e_x,e_y,...]^T, Q维观测数据协方差矩阵，即sigma * sigma组成的协方差矩阵

误差加权最小二次结果越小，说明观测数据精度越高

那么，score = SUM((th - e(i)^T * Q^(-1) * e(i)))的分数就越高

算法目标： 检查单应变换矩阵

检查方式：通过H矩阵，进行参考帧和当前帧之间的双向投影，并计算起加权最小二乘投影误差

算法流程

input: 单应性矩阵 H21, H12, 匹配点集 mvKeys1

do:

      for p1(i), p2(i) in mvKeys:

         error_i1 = ||p2(i) - H21 * p1(i)||2

         error_i2 = ||p1(i) - H12 * p2(i)||2     

         w1 = 1 / sigma / sigma

         w2 = 1 / sigma / sigma

        if error1 < th

           score +=   th - error_i1 * w1

       if error2 < th

            score +=   th - error_i2 * w2

       if error_1i > th or error_2i > th

            p1(i), p2(i) are inner points

            vbMatchesInliers(i) = true

        else

             p1(i), p2(i) are outliers

            vbMatchesInliers(i) = false

         end

  end

  output: score, inliers

float Initializer::CheckHomography(
    const cv::Mat &H21,                 //从参考帧到当前帧的单应矩阵
    const cv::Mat &H12,                 //从当前帧到参考帧的单应矩阵
    vector &vbMatchesInliers,     //匹配好的特征点对的Inliers标记
    float sigma)                        //估计误差
{   
	// 特点匹配个数
    const int N = mvMatches12.size();

    const float h11 = H21.at(0,0);
    const float h12 = H21.at(0,1);
    const float h13 = H21.at(0,2);
    const float h21 = H21.at(1,0);
    const float h22 = H21.at(1,1);
    const float h23 = H21.at(1,2);
    const float h31 = H21.at(2,0);
    const float h32 = H21.at(2,1);
    const float h33 = H21.at(2,2);

	// 获取从当前帧到参考帧的单应矩阵的各个元素
    const float h11inv = H12.at(0,0);
    const float h12inv = H12.at(0,1);
    const float h13inv = H12.at(0,2);
    const float h21inv = H12.at(1,0);
    const float h22inv = H12.at(1,1);
    const float h23inv = H12.at(1,2);
    const float h31inv = H12.at(2,0);
    const float h32inv = H12.at(2,1);
    const float h33inv = H12.at(2,2);

	// 给特征点对的Inliers标记预分配空间
    vbMatchesInliers.resize(N);

	// 初始化score值
    float score = 0;

    // 基于卡方检验计算出的阈值（假设测量有一个像素的偏差）
	// 自由度为2的卡方分布，显著性水平为0.05，对应的临界阈值
    const float th = 5.991;

    //信息矩阵，方差平方的倒数
    const float invSigmaSquare = 1.0/(sigma*sigma);

    // Step 2 通过H矩阵，进行参考帧和当前帧之间的双向投影，并计算起加权最小二乘投影误差
    // H21 表示从img1 到 img2的变换矩阵
    // H12 表示从img2 到 img1的变换矩阵 
    for(int i=0; ith)
            bIn = false;
        else
            // 误差越大，得分越低
            score += th - chiSquare1;

        // 计算从img1 到 img2 的投影变换误差
        // x1in2 = H21*x1
        // 将图像2中的特征点单应到图像1中
        // |u2|   |h11 h12 h13||u1|   |u1in2|
        // |v2| = |h21 h22 h23||v1| = |v1in2| * w1in2inv
        // |1 |   |h31 h32 h33||1 |   |  1  |
		// 计算投影归一化坐标
        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;   

        // Step 2.4 如果从img2 到 img1 和 从img1 到img2的重投影误差均满足要求，则说明是Inlier point
        if(bIn)
            vbMatchesInliers[i]=true;
        else
            vbMatchesInliers[i]=false;
    }

    return score;
}

ReconstructF函数，从基础矩阵F中求解位姿R，t及三维点。

Step 1 统计有效匹配点个数，并用 N 表示

Step 2 根据基础矩阵和相机的内参数矩阵计算本质矩阵

Step 3 从本质矩阵求解两个R解和两个t解，共四组解

Step 4 分别验证求解的4种R和t的组合，选出最佳组合

bool Initializer::ReconstructF(vector &vbMatchesInliers, cv::Mat &F21, cv::Mat &K,
                            cv::Mat &R21, cv::Mat &t21, vector &vP3D, vector &vbTriangulated, float minParallax, int minTriangulated)
{
    // vbMatchesInliers 中存储匹配点对是否是有效
    int N=0;
    for(size_t i=0, iend = vbMatchesInliers.size() ; i vP3D1, vP3D2, vP3D3, vP3D4;

	// 定义四组解分别对同一匹配点集的有效三角化结果，True or False
    vector vbTriangulated1,vbTriangulated2,vbTriangulated3, vbTriangulated4;

	// 定义四种解对应的比较大的特征点对视差角
    float parallax1,parallax2, parallax3, parallax4;

	// Step 4.1 使用同样的匹配点分别检查四组解，记录当前计算的3D点在摄像头前方且投影误差小于阈值的个数，记为有效3D点个数
    int nGood1 = CheckRT(R1,t1,							//当前组解
						 mvKeys1,mvKeys2,				//参考帧和当前帧中的特征点
						 mvMatches12, vbMatchesInliers,	//特征点的匹配关系和Inliers标记
						 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);

    // Step 4.2 选取最大可三角化测量的点的数目
    int maxGood = max(nGood1,max(nGood2,max(nGood3,nGood4)));

	// 重置变量，并在后面赋值为最佳R和T
    R21 = cv::Mat();
    t21 = cv::Mat();

    // Step 4.3 确定最小的可以三角化的点数 
    // 在0.9倍的内点数 和 指定值minTriangulated =50 中取最大的，也就是说至少50个
    int nMinGood = max(static_cast(0.9*N), minTriangulated);

	// 统计四组解中重建的有效3D点个数 > 0.7 * maxGood 的解的数目
    // 如果有多个解同时满足该条件，认为结果太接近，nsimilar++，nsimilar>1就认为有问题了，后面返回false
    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++;

    // Step 4.4 四个结果中如果没有明显的最优结果，或者没有足够数量的三角化点，则返回失败
    // 条件1: 如果四组解能够重建的最多3D点个数小于所要求的最少3D点个数（mMinGood），失败
    // 条件2: 如果存在两组及以上的解能三角化出 >0.7*maxGood的点，说明没有明显最优结果，失败
    if(maxGood1)	
    {
        return false;
    }


    //  Step 4.5 选择最佳解记录结果
    // 条件1: 有效重建最多的3D点，即maxGood == nGoodx，也即是位于相机前方的3D点个数最多
    // 条件2: 三角化视差角 parallax 必须大于最小视差角 minParallax，角度越大3D点越稳定

    //看看最好的good点是在哪种解的条件下发生的
    if(maxGood==nGood1)
    {
		//如果该种解下的parallax大于函数参数中给定的最小值
        if(parallax1>minParallax)
        {
            // 存储3D坐标
            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;
        }
    }

    // 如果有最优解但是不满足对应的parallax>minParallax，那么返回false表示求解失败
    return false;
}