ORB-SLAM2 ---- Initializer::CheckRT函数

目录

1 Initializer::CheckRT

1.1 函数作用

1.2 构造函数 

1.3 代码 

1.4 流程解析 

1.4.0 初始化参数

1.4.1 计算初始化两帧的投影矩阵 

1.4.2  三角化恢复三维点Initializer::Triangulate

1.4.3  遍历所有的特征点对检查三维点是否合适

1.4.4 最后处理 


1 Initializer::CheckRT

1.1 函数作用

        用位姿来对特征匹配点三角化,从中筛选中合格的三维点。

1.2 构造函数 

 * @brief 用位姿来对特征匹配点三角化,从中筛选中合格的三维点
 * 
 * @param[in] R                                     旋转矩阵R
 * @param[in] t                                     平移矩阵t
 * @param[in] vKeys1                                参考帧特征点  
 * @param[in] vKeys2                                当前帧特征点
 * @param[in] vMatches12                            两帧特征点的匹配关系
 * @param[in] vbMatchesInliers                      特征点对内点标记
 * @param[in] K                                     相机内参矩阵
 * @param[in & out] vP3D                            三角化测量之后的特征点的空间坐标
 * @param[in] th2                                   重投影误差的阈值
 * @param[in & out] vbGood                          标记成功三角化点?
 * @param[in & out] parallax                        计算出来的比较大的视差角(注意不是最大,具体看后面代码)
 * @return int 
 */
int Initializer::CheckRT(const cv::Mat &R, const cv::Mat &t, const vector &vKeys1, const vector &vKeys2,
                       const vector &vMatches12, vector &vbMatchesInliers,
                       const cv::Mat &K, vector &vP3D, float th2, vector &vbGood, float ¶llax)

        传入参数:

        ①参考帧到当前帧的旋转矩阵R和平移矩阵t

        ②参考帧(第一帧)和当前帧(第二帧)的特征点容器vKeys1、vKeys2

        ③两帧特征点的匹配标记vMatches12以及特征点对内点标记vbMatchesInliers

        ④相机内参K,重投影误差阈值th2

        传出参数:

        ①三角化测量之后的特征点的空间坐标v3D

        ②标记成功三角化点vGoog

        ③返回三角化点的数量

1.3 代码 

int Initializer::CheckRT(const cv::Mat &R, const cv::Mat &t, const vector &vKeys1, const vector &vKeys2,
                       const vector &vMatches12, vector &vbMatchesInliers,
                       const cv::Mat &K, vector &vP3D, float th2, vector &vbGood, float ¶llax)
{   
    // 对给出的特征点对及其R t , 通过三角化检查解的有效性,也称为 cheirality check

    // Calibration parameters
	//从相机内参数矩阵获取相机的校正参数
    const float fx = K.at(0,0);
    const float fy = K.at(1,1);
    const float cx = K.at(0,2);
    const float cy = K.at(1,2);

	//特征点是否是good点的标记,这里的特征点指的是参考帧中的特征点
    vbGood = vector(vKeys1.size(),false);
	//重设存储空间坐标的点的大小
    vP3D.resize(vKeys1.size());

	//存储计算出来的每对特征点的视差
    vector vCosParallax;
    vCosParallax.reserve(vKeys1.size());

    // Camera 1 Projection Matrix K[I|0]
    // Step 1:计算相机的投影矩阵  
    // 投影矩阵P是一个 3x4 的矩阵,可以将空间中的一个点投影到平面上,获得其平面坐标,这里均指的是齐次坐标。
    // 对于第一个相机是 P1=K*[I|0]
 
    // 以第一个相机的光心作为世界坐标系, 定义相机的投影矩阵
    cv::Mat P1(3,4,				//矩阵的大小是3x4
			   CV_32F,			//数据类型是浮点数
			   cv::Scalar(0));	//初始的数值是0
	//将整个K矩阵拷贝到P1矩阵的左侧3x3矩阵,因为 K*I = K
    K.copyTo(P1.rowRange(0,3).colRange(0,3));
    // 第一个相机的光心设置为世界坐标系下的原点
    cv::Mat O1 = cv::Mat::zeros(3,1,CV_32F);

    // Camera 2 Projection Matrix K[R|t]
    // 计算第二个相机的投影矩阵 P2=K*[R|t]
    cv::Mat P2(3,4,CV_32F);
    R.copyTo(P2.rowRange(0,3).colRange(0,3));
    t.copyTo(P2.rowRange(0,3).col(3));
	//最终结果是K*[R|t]
    P2 = K*P2;
    // 第二个相机的光心在世界坐标系下的坐标
    cv::Mat O2 = -R.t()*t;

	//在遍历开始前,先将good点计数设置为0
    int nGood=0;

	// 开始遍历所有的特征点对
    for(size_t i=0, iend=vMatches12.size();i(0)) || !isfinite(p3dC1.at(1)) || !isfinite(p3dC1.at(2)))
        {
			//其实这里就算是不这样写也没问题,因为默认的匹配点对就不是good点
            vbGood[vMatches12[i].first]=false;
			//继续对下一对匹配点的处理
            continue;
        }

        // Check parallax
        // Step 4 第二关:通过三维点深度值正负、两相机光心视差角大小来检查是否合法 

        //得到向量PO1
        cv::Mat normal1 = p3dC1 - O1;
		//求取模长,其实就是距离
        float dist1 = cv::norm(normal1);

		//同理构造向量PO2
        cv::Mat normal2 = p3dC1 - O2;
		//求模长
        float dist2 = cv::norm(normal2);

		//根据公式:a.*b=|a||b|cos_theta 可以推导出来下面的式子
        float cosParallax = normal1.dot(normal2)/(dist1*dist2);

        // Check depth in front of first camera (only if enough parallax, as "infinite" points can easily go to negative depth)
        // 如果深度值为负值,为非法三维点跳过该匹配点对
        // ?视差比较小时,重投影误差比较大。这里0.99998 对应的角度为0.36°,这里不应该是 cosParallax>0.99998 吗?
        // ?因为后面判断vbGood 点时的条件也是 cosParallax<0.99998 
        // !可能导致初始化不稳定
        if(p3dC1.at(2)<=0 && cosParallax<0.99998)
            continue;

        // Check depth in front of second camera (only if enough parallax, as "infinite" points can easily go to negative depth)
        // 讲空间点p3dC1变换到第2个相机坐标系下变为p3dC2
        cv::Mat p3dC2 = R*p3dC1+t;	
		//判断过程和上面的相同
        if(p3dC2.at(2)<=0 && cosParallax<0.99998)
            continue;

        // Step 5 第三关:计算空间点在参考帧和当前帧上的重投影误差,如果大于阈值则舍弃
        // Check reprojection error in first image
        // 计算3D点在第一个图像上的投影误差
		//投影到参考帧图像上的点的坐标x,y
        float im1x, im1y;
		//这个使能空间点的z坐标的倒数
        float invZ1 = 1.0/p3dC1.at(2);
		//投影到参考帧图像上。因为参考帧下的相机坐标系和世界坐标系重合,因此这里就直接进行投影就可以了
        im1x = fx*p3dC1.at(0)*invZ1+cx;
        im1y = fy*p3dC1.at(1)*invZ1+cy;

		//参考帧上的重投影误差,这个的确就是按照定义来的
        float squareError1 = (im1x-kp1.pt.x)*(im1x-kp1.pt.x)+(im1y-kp1.pt.y)*(im1y-kp1.pt.y);

        // 重投影误差太大,跳过淘汰
        if(squareError1>th2)
            continue;

        // Check reprojection error in second image
        // 计算3D点在第二个图像上的投影误差,计算过程和第一个图像类似
        float im2x, im2y;
        // 注意这里的p3dC2已经是第二个相机坐标系下的三维点了
        float invZ2 = 1.0/p3dC2.at(2);
        im2x = fx*p3dC2.at(0)*invZ2+cx;
        im2y = fy*p3dC2.at(1)*invZ2+cy;

		// 计算重投影误差
        float squareError2 = (im2x-kp2.pt.x)*(im2x-kp2.pt.x)+(im2y-kp2.pt.y)*(im2y-kp2.pt.y);

        // 重投影误差太大,跳过淘汰
        if(squareError2>th2)
            continue;

        // Step 6 统计经过检验的3D点个数,记录3D点视差角 
        // 如果运行到这里就说明当前遍历的这个特征点对靠谱,经过了重重检验,说明是一个合格的点,称之为good点 
        vCosParallax.push_back(cosParallax);
		//存储这个三角化测量后的3D点在世界坐标系下的坐标
        vP3D[vMatches12[i].first] = cv::Point3f(p3dC1.at(0),p3dC1.at(1),p3dC1.at(2));
		//good点计数++
        nGood++;

		//判断视差角,只有视差角稍稍大一丢丢的才会给打good点标记
		//? bug 我觉得这个写的位置不太对。你的good点计数都++了然后才判断,不是会让good点标志和good点计数不一样吗
        if(cosParallax<0.99998)
            vbGood[vMatches12[i].first]=true;
    }

    // Step 7 得到3D点中较小的视差角,并且转换成为角度制表示
    if(nGood>0)
    {
        // 从小到大排序,注意vCosParallax值越大,视差越小
        sort(vCosParallax.begin(),vCosParallax.end());

        // !排序后并没有取最小的视差角,而是取一个较小的视差角
		// 作者的做法:如果经过检验过后的有效3D点小于50个,那么就取最后那个最小的视差角(cos值最大)
		// 如果大于50个,就取排名第50个的较小的视差角即可,为了避免3D点太多时出现太小的视差角 
        size_t idx = min(50,int(vCosParallax.size()-1));
		//将这个选中的角弧度制转换为角度制
        parallax = acos(vCosParallax[idx])*180/CV_PI;
    }
    else
		//如果没有good点那么这个就直接设置为0了
        parallax=0;

	//返回good点计数
    return nGood;
}

1.4 流程解析 

1.4.0 初始化参数

        ①特征点是否是good点的标记,这里的特征点指的是参考帧中的特征点,将vGood初始化为第一帧特征点的数量,其为bool类型。

        ②三角化测量之后的特征点的空间坐标v3D初始化大小为第一帧中特征点的数量。

1.4.1 计算初始化两帧的投影矩阵 

        投影矩阵P是一个 3x4 的矩阵,可以将空间中的一个点投影到平面上,获得其平面坐标,这里均指的是齐次坐标。

        由于以第一个相机的光心作为世界坐标系。 其投影矩阵计算推导如下:

ZP_{uv} = K(RP_{w}+t)=KTP_{w}

        我们默认第一个相机的R,t矩阵为[E|0],因此第一个相机的投影矩阵为K。第一个相机的光心坐标为(0,0,0)^{T}

        我们从传入参数可知第一个相机到第二个相机的R,t变换T,因此再左乘相机内参矩阵K就能得到像素坐标,即世界坐标向像素坐标的投影矩阵为KT

        同时计算第二帧的光心坐标在原点(第一帧)的坐标,计算如下图所示:

        第二个相机的光心在世界坐标系下的坐标,即我们要求第二个相机的光心在第一个相机坐标系下的坐标。

1.4.2  三角化恢复三维点Initializer::Triangulate

1.数学原理

        我们将投影方程进行如下描述:

\begin{bmatrix} x\\y \\1 \end{bmatrix}=\lambda \begin{bmatrix} p_{1} &p_{2} &p_{3} &p_{4} \\ p_{5} & p_{6} &p_{7} & p_{8} \\ p_{9} &p_{10} &p_{11} & p_{12} \end{bmatrix} \begin{bmatrix} X\\Y \\Z \\1 \end{bmatrix}

        为方便推导,简单记为:

        为了化为齐次方程,左右两边同时叉乘,得到:

        利用两对匹配点,得到:

        SVD求解,右奇异矩阵的最后一行就是最终的解。

2.代码

/** 给定投影矩阵P1,P2和图像上的匹配特征点点kp1,kp2,从而计算三维点坐标
 * @brief 
 * 
 * @param[in] kp1               特征点, in reference frame
 * @param[in] kp2               特征点, in current frame
 * @param[in] P1                投影矩阵P1
 * @param[in] P2                投影矩阵P2
 * @param[in & out] x3D         计算的三维点
 */
void Initializer::Triangulate(
    const cv::KeyPoint &kp1,    //特征点, in reference frame
    const cv::KeyPoint &kp2,    //特征点, in current frame
    const cv::Mat &P1,          //投影矩阵P1
    const cv::Mat &P2,          //投影矩阵P2
    cv::Mat &x3D)               //三维点
{
    // 原理
    // Trianularization: 已知匹配特征点对{x x'} 和 各自相机矩阵{P P'}, 估计三维点 X
    // x' = P'X  x = PX
    // 它们都属于 x = aPX模型
    //                         |X|
    // |x|     |p1 p2  p3  p4 ||Y|     |x|    |--p0--||.|
    // |y| = a |p5 p6  p7  p8 ||Z| ===>|y| = a|--p1--||X|
    // |z|     |p9 p10 p11 p12||1|     |z|    |--p2--||.|
    // 采用DLT的方法:x叉乘PX = 0
    // |yp2 -  p1|     |0|
    // |p0 -  xp2| X = |0|
    // |xp1 - yp0|     |0|
    // 两个点:
    // |yp2   -  p1  |     |0|
    // |p0    -  xp2 | X = |0| ===> AX = 0
    // |y'p2' -  p1' |     |0|
    // |p0'   - x'p2'|     |0|
    // 变成程序中的形式:
    // |xp2  - p0 |     |0|
    // |yp2  - p1 | X = |0| ===> AX = 0
    // |x'p2'- p0'|     |0|
    // |y'p2'- p1'|     |0|
    // 然后就组成了一个四元一次正定方程组,SVD求解,右奇异矩阵的最后一行就是最终的解.

	//这个就是上面注释中的矩阵A
    cv::Mat A(4,4,CV_32F);

	//构造参数矩阵A
    A.row(0) = kp1.pt.x*P1.row(2)-P1.row(0);   
    A.row(1) = kp1.pt.y*P1.row(2)-P1.row(1);
    A.row(2) = kp2.pt.x*P2.row(2)-P2.row(0);
    A.row(3) = kp2.pt.y*P2.row(2)-P2.row(1);

	//奇异值分解的结果
    cv::Mat u,w,vt;
	//对系数矩阵A进行奇异值分解
    cv::SVD::compute(A,w,u,vt,cv::SVD::MODIFY_A| cv::SVD::FULL_UV);
	//根据前面的结论,奇异值分解右矩阵的最后一行其实就是解,原理类似于前面的求最小二乘解,四个未知数四个方程正好正定
	//别忘了我们更习惯用列向量来表示一个点的空间坐标
    x3D = vt.row(3).t();
	//为了符合其次坐标的形式,使最后一维为1
    x3D = x3D.rowRange(0,3)/x3D.at(3);
}

        至此我们恢复一对匹配特征点的3D点。

1.4.3  遍历所有的特征点对检查三维点是否合适

        我们遍历所有匹配的特征点对,如果不是外点,则将匹配好的特征点对传入Initializer::Triangulate函数内得到三维点。我们对三维点进行判断:

        ①检查三角化的三维点坐标是否合法:防止三角化出的点有一维坐标是无穷。

        ②通过三维点深度值正负、两相机光心视差角大小来检查是否合法 。即判断如果深度值为负值,为非法三维点跳过该匹配点对。计算视差角,当视差角比较小时,重投影误差比较大。

        ③计算空间点在参考帧和当前帧上的重投影误差,如果大于阈值则舍弃。计算3D点在第一、二个图像上的投影误差,看是否超过阈值选择是否抛弃。

        如果如上满足了,这个3D点可以被留下来,用vCosParallax向量存储合格3D点生成时计算出来的视差,存储这个三角化测量后的3D点在世界坐标系下的坐标。并将这两帧累计成功初始化的3D点nGood累加。

vP3D[vMatches12[i].first] = cv::Point3f(p3dC1.at(0),p3dC1.at(1),p3dC1.at(2));

1.4.4 最后处理 

        如果我们的变量nGood大于0(成功三角化得到的3D点的数目),我们将视差角从小到大排序,排序后并没有取最小的视差角,而是取一个较小的视差角。如果经过检验过后的有效3D点小于50个,那么就取最后那个最小的视差角(cos值最大),如果大于50个,就取排名第50个的较小的视差角即可,为了避免3D点太多时出现太小的视差角。将这个选中的角弧度制转换为角度制输出。

你可能感兴趣的:(orb-slam2,算法)