当前位置：首页 > news >正文

ORB-SLAM2源码学习：Initializer.cc:Initializer::ComputeF21地图初始化——计算基础矩阵

news 2025/6/24 16:35:55

前言

在平面场景我们通过求解单应矩阵H来求解位姿，但是我们在实际中常见的都是非平面场景，此时需要用基础矩阵F求解位姿。

1.函数声明

cv::Mat Initializer::ComputeF21(const vector<cv::Point2f> &vP1, const vector<cv::Point2f> &vP2)

2.函数定义

用基础矩阵描述特征点之间的变换关系。

矩阵形式：

方程形式：

矩阵方程形式：

对矩阵进行两次SVD分解，右奇异矩阵的最后一列就是最优解。

使用两次 SVD 的原因：

1.第一次 SVD 用于从特征点对中解线性方程，得到初步估计的基础矩阵Fpre。

2.第二次 SVD 用于对初步估计进行优化，施加基础矩阵的秩约束，确保结果符合理论要求。

/*根据特征点匹配求fundamental matrix（normalized 8点法）注意F矩阵有秩为2的约束，所以需要两次SVD分解vP1           参考帧中归一化后的特征点vP2           当前帧中归一化后的特征点return cv::Mat          最后计算得到的基础矩阵F*/
cv::Mat Initializer::ComputeF21(const vector<cv::Point2f> &vP1, //归一化后的点, in reference frameconst vector<cv::Point2f> &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<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;// 定义输出变量，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个奇异值设置为0w.at<float>(2)=0; // 重新组合好满足秩约束的基础矩阵，作为最终计算结果返回 return  u*cv::Mat::diag(w)*vt;
}