ORB-SLAM2 ---- Initializer::ComputeH21函数

目录

1.函数作用

2.数学基础 

3.代码

---

1.函数作用

        用DLT方法求解单应矩阵H。

2.数学基础 

        我们通过八对归一化的点求单应矩阵H，步骤如下：

        特征点对用单应矩阵来描述特征点对之间的变换关系：。

        我们写成矩阵形式如下：

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

         一对点提供两个约束等式，单应矩阵H总共有9个元素，8个自由度（尺度等价性），所以需要4对点提供8个约束方程就可以求解。

         我们求矩阵可以用SVD分解的方法，第九个向量就是最优解。

3.代码

/**
 * @brief 用DLT方法求解单应矩阵H
 * 这里最少用4对点就能够求出来，不过这里为了统一还是使用了8对点求最小二乘解
 * 
 * @param[in] vP1               参考帧中归一化后的特征点
 * @param[in] vP2               当前帧中归一化后的特征点
 * @return cv::Mat              计算的单应矩阵H
 */
cv::Mat Initializer::ComputeH21(
    const vector &vP1, //归一化后的点, in reference frame
    const vector &vP2) //归一化后的点, in current frame
{
    const int N = vP1.size();
    cv::Mat A(2*N,				
			  9,				
			  CV_32F);      	
    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补充成单位正交方阵

	// 返回最小奇异值所对应的右奇异向量
    // 注意前面说的是右奇异值矩阵的最后一列，但是在这里因为是vt，转置后了，所以是行；由于A有9列数据，故最后一列的下标为8
    return vt.row(8).reshape(0, 			//转换后的通道数，这里设置为0表示是与前面相同
							 3); 			//转换后的行数,对应V的最后一列
}

        和数学公式一样，我们返回给调用函数单应矩阵H。