文章目录
一、函数意义
二、LocalMapping::ComputeF12()
1.函数讲解
2.函数代码
// 计算两个关键帧之间的基准矩阵
cv::Mat LocalMapping::ComputeF12(KeyFrame *&pKF1, KeyFrame *&pKF2)
{
// 获取第一个关键帧的旋转矩阵和平移向量
cv::Mat R1w = pKF1->GetRotation();
cv::Mat t1w = pKF1->GetTranslation();
// 获取第二个关键帧的旋转矩阵和平移向量
cv::Mat R2w = pKF2->GetRotation();
cv::Mat t2w = pKF2->GetTranslation();
// 获取从相机2坐标系到相机1坐标系的旋转矩阵
cv::Mat R12 = R1w*R2w.t();
// 相机2坐标系到相机1坐标系的平移向量
cv::Mat t12 = -R1w*R2w.t()*t2w+t1w;
// 获取t12的反对称矩阵
cv::Mat t12x = SkewSymmetricMatrix(t12);
// 获取第一帧相机内参K1和第二帧内参K2
const cv::Mat &K1 = pKF1->mK;
const cv::Mat &K2 = pKF2->mK;
// 返回两个关键帧之间的基础矩阵 F
return K1.t().inv()*t12x*R12*K2.inv();
}
三、ORBmatcher::CheckDistEpipolarLine()
1.函数讲解
2.函数代码
// 实现了一个用于检测两个特征点(关键点)是否满足对极几何约束的函数 CheckDistEpipolarLine
bool ORBmatcher::CheckDistEpipolarLine(const cv::KeyPoint &kp1,const cv::KeyPoint &kp2,const cv::Mat &F12,const KeyFrame* pKF2)
{
// Epipolar line in second image l = x1'F12 = [a b c]
const float a = kp1.pt.x*F12.at<float>(0,0)+kp1.pt.y*F12.at<float>(1,0)+F12.at<float>(2,0);
const float b = kp1.pt.x*F12.at<float>(0,1)+kp1.pt.y*F12.at<float>(1,1)+F12.at<float>(2,1);
const float c = kp1.pt.x*F12.at<float>(0,2)+kp1.pt.y*F12.at<float>(1,2)+F12.at<float>(2,2);
const float num = a*kp2.pt.x+b*kp2.pt.y+c;
const float den = a*a+b*b;
if(den==0)
return false;
const float dsqr = num*num/den;
return dsqr<3.84*pKF2->mvLevelSigma2[kp2.octave];
}