)
第一阶段设置“参考帧”if(!mpInitializer)当系统启动或上一次初始化失败时mpInitializer为空。此阶段的目标是找到一张质量足够好的图像作为“参考帧”。if(mCurrentFrame.mvKeys.size() 100) { mInitialFrame Frame(mCurrentFrame); // 保存参考帧 mLastFrame Frame(mCurrentFrame); // 更新上一帧用于运动模型 // 记录参考帧所有去畸变后的特征点坐标用于后续匹配 mvbPrevMatched.resize(...); ... mpInitializer new Initializer(mCurrentFrame, 1.0, 200); // 创建初始化器 fill(mvIniMatches.begin(), mvIniMatches.end(), -1); // 清空匹配关系 return; }阈值 100与双目初始化要求N500不同单目要求较低100个点即可。因为单目初始化依赖帧间匹配过高的门槛会导致在弱纹理场景中永远无法启动Initializer创建传入参考帧设定 RANSAC 置信度1.0和迭代次数200。注意此时只是创建了求解器还没有进行计算。作用存下第一帧后函数返回。系统继续处理下一帧。第二阶段尝试初始化else分支当mpInitializer已存在即已经有了参考帧系统会用当前帧来尝试完成初始化。Step 2.1检查当前帧质量if((int)mCurrentFrame.mvKeys.size() 100) { delete mpInitializer; // 当前帧太差销毁初始化器重新选参考帧 mpInitializer nullptr; return; }如果当前帧特征点也不足 100 个说明连续两帧质量都不行直接放弃本次初始化重置状态机。Step 2.2特征匹配寻找两帧之间的关系关键步骤ORBmatcher matcher(0.9, true);宽松阈值0.9单目初始化时两帧之间可能运动较大使用较宽松的匹配阈值相比跟踪时的0.7可以提高鲁棒性。int nmatches matcher.SearchForInitialization( mInitialFrame, mCurrentFrame, mvbPrevMatched, // 输入参考帧的特征点坐标引导匹配 mvIniMatches, // 输出参考帧索引 - 当前帧索引 的匹配关系 100); // 搜索窗口大小输入参考帧的去畸变坐标 mvbPrevMatched作为当前帧搜索时的位置先验。输出匹配点对索引mvIniMatchesmvIniMatches[i] j表示参考帧第i个点匹配当前帧第j个点if(nmatches100) { delete mpInitializer; mpInitializer static_castInitializer*(NULL); return; }匹配数量检查if(nmatches 100)—— 匹配点太少意味着两帧重合度低或运动过大无法可靠求解重置初始化器。Step 2.3几何求解H 矩阵 或 F 矩阵if(mpInitializer-Initialize( mCurrentFrame, mvIniMatches, Rcw, tcw, // 计算出的旋转和平移当前帧相对于参考帧 mvIniP3D, // 三角化出的临时 3D 点世界坐标系 vbTriangulated) // 哪些匹配点成功三角化了 )这是算法的最核心部分。Initializer::Initialize内部会同时计算单应矩阵H和基础矩阵F根据评分自动选择更合适的模型平面场景选 H非平面场景选 F并从模型中分解出Rcw旋转和tcw平移。特别注意单目初始化的尺度是任意的tcw的模长没有物理单位后续通过CreateInitialMapMonocular会进行尺度归一化。Step 2.4清理未三角化的点for(size_t i0; imvIniMatches.size(); i) { if(mvIniMatches[i]0 !vbTriangulated[i]) { mvIniMatches[i] -1; // 删除匹配关系 nmatches--; } }有些点虽然特征匹配上了但三角化角度太小视差不足导致三维坐标不稳定这些点会被标记为vbTriangulatedfalse在此步骤被剔除防止引入噪声点。第三阶段设置位姿与创建地图CreateInitialMapMonocularmInitialFrame.SetPose(cv::Mat::eye(4,4,CV_32F)); // 参考帧位姿 世界原点 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); // 当前帧位姿 相对于参考帧的相对运动 CreateInitialMapMonocular(); // 正式构建关键帧、地图点、共视图参考帧设为世界坐标原点单目 SLAM 的世界坐标系原点就定义在初始参考帧的相机光心处。CreateInitialMapMonocular这是一个独立函数负责将临时变量mvIniP3D中的点包装成MapPoint对象创建两个初始关键帧建立共视关系并启动局部建图线程。这是初始化成功的最后一步。
