void Tracking::MonocularInitialization()
{
// Step 1 如果单目初始器还没有被创建,则创建。后面如果重新初始化时会清掉这个
if(!mpInitializer)
{
// Set Reference Frame
// 追踪线程的当前帧mCurrentFrame mvKeys是特征点数量
if(mCurrentFrame.mvKeys.size()>100)
{
// 初始化需要两帧,分别是mInitialFrame,mCurrentFrame
mInitialFrame = Frame(mCurrentFrame);
// 用当前帧更新上一帧
mLastFrame = Frame(mCurrentFrame);
// mvbPrevMatched 记录"上一帧"所有特征点
mvbPrevMatched.resize(mCurrentFrame.mvKeysUn.size());
for(size_t i=0; i(NULL);
fill(mvIniMatches.begin(),mvIniMatches.end(),-1);
return;
}
// Find correspondences
// Step 3 在mInitialFrame与mCurrentFrame中找匹配的特征点对
ORBmatcher matcher(
0.9, //最佳的和次佳特征点评分的比值阈值,这里是比较宽松的,跟踪时一般是0.7,值越大匹配越宽松
true); //检查特征点的方向
// 对 mInitialFrame,mCurrentFrame 进行特征点匹配
// mvbPrevMatched为参考帧的特征SearchForInitialization点坐标,初始化存储的是mInitialFrame中特征点坐标,匹配后存储的是匹配好的当前帧的特征点坐标
// mvIniMatches 保存参考帧F1中特征点是否匹配上,index保存是F1对应特征点索引,值保存的是匹配好的F2特征点索引
int nmatches = matcher.SearchForInitialization(
mInitialFrame,mCurrentFrame, //初始化时的参考帧和当前帧
mvbPrevMatched, //在初始化参考帧中提取得到的特征点
mvIniMatches, //保存匹配关系
100); //搜索窗口大小
// Check if there are enough correspondences
// Step 4 验证匹配结果,如果初始化的两帧之间的匹配点太少,重新初始化
if(nmatches(NULL);
return;
}
cv::Mat Rcw; // Current Camera Rotation
cv::Mat tcw; // Current Camera Translation
vector vbTriangulated; // Triangulated Correspondences (mvIniMatches)
// Step 5 通过H模型或F模型进行单目初始化,得到两帧间相对运动、初始MapPoints
if(mpInitializer->Initialize(
mCurrentFrame, //当前帧
mvIniMatches, //当前帧和参考帧的特征点的匹配关系
Rcw, tcw, //初始化得到的相机的位姿
mvIniP3D, //进行三角化得到的空间点集合
vbTriangulated)) //以及对应于mvIniMatches来讲,其中哪些点被三角化了
{
// Step 6 初始化成功后,删除那些无法进行三角化的匹配点
for(size_t i=0, iend=mvIniMatches.size(); i=0 && !vbTriangulated[i])
{
mvIniMatches[i]=-1;
nmatches--;
}
}
// Set Frame Poses
// Step 7 将初始化的第一帧作为世界坐标系,因此第一帧变换矩阵为单位矩阵
mInitialFrame.SetPose(cv::Mat::eye(4,4,CV_32F));
// 由Rcw和tcw构造Tcw,并赋值给mTcw,mTcw为世界坐标系到相机坐标系的变换矩阵
cv::Mat Tcw = cv::Mat::eye(4,4,CV_32F);
Rcw.copyTo(Tcw.rowRange(0,3).colRange(0,3));
tcw.copyTo(Tcw.rowRange(0,3).col(3));
mCurrentFrame.SetPose(Tcw);
// Step 8 创建初始化地图点MapPoints
// Initialize函数会得到mvIniP3D,
// mvIniP3D是cv::Point3f类型的一个容器,是个存放3D点的临时变量,
// CreateInitialMapMonocular将3D点包装成MapPoint类型存入KeyFrame和Map中
CreateInitialMapMonocular();
}//当初始化成功的时候进行
}//如果单目初始化器已经被创建
}
首先我们构造一个 单目初始化器用于单目初始化,如果跟踪线程的当前帧的特征点(我们传入slam系统的图片是一帧一帧的,当输入第一帧图片时,我们对其进行单目初始化)
我们判断第一帧图片的特征点数量 mCurrentFrame.mvKeys.size是否满足初始化要求(大于100特征点),如果满足,由于初始化要求两帧我们将初始化的第一帧第二帧都初始化为第一帧图像。用 mvbPrevMatched保存参考帧(第一帧)中的特征点。
std::vector mvIniMatches;// 跟踪初始化时前两帧之间的匹配
用第一帧初始化单目初始化器,并将初始化时前两帧之间的匹配设为-1表示无匹配, mvIniMatches容器的意思是如果第一帧的第一个特征点和第二帧的第五个特征点有匹配,则记mvIniMatches[0] = 4,然后退出初始化,等待下一帧的到来。
当第二帧到来,检测到了单目初始化器的存在,检查第二帧的特征点数量是否满足slam系统的要求,若不满足则重新构造初始器(将初始化器的内存释放掉,用第 三第四帧….去尝试初始化),若满足要求初始化ORB匹配器,对这两帧的特征点进行匹配,由于单目初始化要求的帧质量比较高(特征点要求多,特征匹配要求高,因为约束越多位姿越准确),因此如果特征点匹配的数量不能够达到验证匹配结果,如果初始化的两帧之间的匹配点太少,重新初始化。
如果数量达到验证匹配结果,通过H模型或F模型进行单目初始化,得到两帧间相对运动、初始MapPoint,初始化成功后,删除那些无法进行三角化的匹配点。
将初始化的第一帧作为世界坐标系,因此第一帧变换矩阵为单位矩阵,随后创建初始化地图点MapPoints。
这些函数我都会在后文解释清楚的!
Original: https://blog.csdn.net/qq_41694024/article/details/127816993
Author: Courage2022
Title: ORB-SLAM2 —- Tracking::MonocularInitialization函数
原创文章受到原创版权保护。转载请注明出处:https://www.johngo689.com/655012/
转载文章受原作者版权保护。转载请注明原作者出处!