)
StereoInitialization函数主要的作用是双目/深度相机模式的初始化涵盖了系统初始化、相机位姿管理、地图点创建与维护、以及关键帧数据库。明确双目初始化到底初始化了什么1. 初始化了“世界坐标系”的原点和尺度物理意义原点在哪代码中将第一帧的相机位姿直接设为单位矩阵ISetPose(eye(4,4))。这意味着世界坐标系的原点(0,0,0)就被定义在了第一帧相机光心的位置。后续所有地图点、所有关键帧的位姿都是相对于这个原点来进行描述的。尺度是什么在单目视觉中我们无法知道真实世界的物理尺寸比如不知道“1单位”是1厘米还是1米存在尺度模糊性。但双目/RGB-D相机不同因为它有固定的物理基线b比如两个摄像头间距10cm通过公式 Zf⋅bdZdf⋅b 计算出的深度是真实的物理距离如米。因此这次初始化锁死了地图的绝对尺度后续建图大小就是真实世界的大小。2. 初始化了“稀疏地图点云”内容填充仅仅知道坐标系是空的没用。初始化过程中遍历了当前帧所有有效的特征点利用反投影UnprojectStereo公式将图像上的2D像素点转换成了3D空间点MapPoint。这些点构成了系统启动时的初始稀疏点云。没有这些点下一帧到来时就找不到任何东西去匹配跟踪就无从谈起。3. 初始化了“地图的第一个节点”参考锚点SLAM中的地图是一个不断增长的图结构关键帧 地图点。初始化创建了第一个KeyFrame关键帧并将其插入地图。这个关键帧充当了后续所有优化和建图的锚点。例如第2帧来临时它会以这个初始关键帧为参考进行位姿估计TrackReferenceKeyFrame。模块一立体初始化——尺度确定与单帧建图if(mCurrentFrame.N500) { mCurrentFrame.SetPose(cv::Mat::eye(4,4,CV_32F)); }为什么要500个点双目初始化非常依赖数量充足、分布均匀的稳定特征点。少于500个点意味着场景纹理极弱或图像质量较差此时强行初始化会产生极其不稳定的初始化地图导致系统瞬间丢失。这是一个鲁棒性门槛。为什么将初始位姿矩阵设置为单位阵双目/RGB-D相机的最核心优势是直接提供绝对尺度。在单目SLAM中初始化需要阶段本质矩阵Essential Matrix并面临尺度模糊性而在此处将第一个关键帧的相机位姿设为 TcwI。这意味着地图的原点就在相机光心处。后续所有的地图点、关键帧位姿都将相对于这个原点进行表达。模块二位姿矩阵更新/*SetPose - UpdatePoseMatrices*/ mRcw mTcw.rowRange(0,3).colRange(0,3); mtcw mTcw.rowRange(0,3).col(3); mOw -mRcw.t() * mtcw;模块三创建初始关键帧并加入地图KeyFrame* pKFini new KeyFrame(mCurrentFrame,mpMap,mpKeyFrameDB); mpMap-AddKeyFrame(pKFini);作用KeyFrame不仅存储了图像特征点还存储了词袋向量BoW用于后续的快速匹配和回环检测。将其加入地图的mspKeyFrames集合中标志地图生命周期的开始。模块四遍历特征点进行反投影生成地图点for(int i0; imCurrentFrame.N;i) { float z mCurrentFrame.mvDepth[i]; if(z0) { cv::Mat x3D mCurrentFrame.UnprojectStereo(i); // 核心反投影 MapPoint* pNewMP new MapPoint(x3D,pKFini,mpMap); ... } }立体反投影/*v::Mat x3D mCurrentFrame.UnprojectStereo(i);*/ const float x (u - cx) * z * invfx; const float y (v - cy) * z * invfy; cv::Mat x3Dc (cv::Mat_float(3,1) x, y, z); return mRwc * x3Dc mOw;模块五构建地图点的观测关系MapPoint* pNewMP new MapPoint(x3D,pKFini,mpMap); pNewMP-AddObservation(pKFini,i); pKFini-AddMapPoint(pNewMP,i);MapPoint记录它被哪个关键帧的哪个特征点观测到。KeyFrame记录它的哪个索引对应哪个地图点。为什么这么做这是为了后续的局部建图Local Mapping和优化BA做准备。在优化时系统需要通过这些关联快速找到重投影误差 eu−π(K⋅Tcw⋅Pw) 中的对应关系1. AddObservation观测计数加权/*AddObservation*/ if(pKF-mvuRight[idx]0) nObs 2; else nObs;权重公式对于立体相机的右目匹配因为左右目同时看到了该点提供了更强的极线约束所以观测计数加 2。这决定了在局部调整Local BA中该点的残差权重更高优化更稳健。模块六地图点MapPoint的核心维护 —— 描述子、法线与深度ComputeDistinctiveDescriptors代表性描述子选取一个 MapPoint 可能被多个 KeyFrame 观测到因此会对应多个 ORB 描述子。ComputeDistinctiveDescriptors()会从这些描述子中选出一个与其他描述子“整体最接近”的描述子作为该 MapPoint 的代表描述子mDescriptor。先理解为什么一个MapPoint会有很多个描述子假设地图中有一个三维点P那么这个三维点可能被多个关键帧观测到由于观测角度不同、相机距离不同、关照不同、图像模糊程度不同、尺度不同每个关键中提取出来的ORB描述子可能不同。例如KeyFrame 1: MapPoint P ↓ Descriptor D1 KeyFrame 2: MapPoint P ↓ Descriptor D2 KeyFrame 3: MapPoint P ↓ Descriptor D3因此一个 MapPoint 可能有D1D2D3......。但是后续进行特征匹配时不可能每次都拿 MapPoint 的所有描述子去匹配。所以 ORB-SLAM2 要选出一个最能够代表这个 MapPoint 的描述子。函数整体流程MapPoint │ │ 1. 获取所有观测该点的 KeyFrame ▼ mObservations │ │ 2. 从每个 KeyFrame 中取出对应 ORB Descriptor ▼ vDescriptors │ │ 3. 计算所有 Descriptor 两两之间的 Hamming Distance ▼ Distances[N][N] │ │ 4. 对每个 Descriptor │ 计算它到其他所有 Descriptor 距离的中值 ▼ Median Distance │ │ 5. 找到中值最小的 Descriptor ▼ mDescriptorDescriptorDistance() 计算两个描述子之间的汉明距离for (size_t i 0; iN; i) { // 和自己的距离当然是0 Distances[i][i]0; // 计算并记录不同描述子汉明距离 for(size_t ji1;jN;j) { int distij ORBmatcher::DescriptorDistance(vDescriptors[i],vDescriptors[j]); Distances[i][j]distij; Distances[j][i]distij; } }本质计算两个二进制描述子中有几个bit位不一样假设有5个描述子计算结果为D0 D1 D2 D3 D4 D0 0 20 25 80 22 D1 20 0 18 75 19 D2 25 18 0 78 21 D3 80 75 78 0 76 D4 22 19 21 76 0选取最具有代表性的描述子int BestMedian INT_MAX; // 记录最小的中值 int BestIdx 0; // 最小中值对应的索引 for(size_t i0;iN;i) { // 第i个描述子到其它所有描述子之间的距离 // vectorint vDists(Distances[i],Distances[i]N); vectorint vDists(Distances[i].begin(), Distances[i].end()); sort(vDists.begin(), vDists.end()); // 获得中值 int median vDists[0.5*(N-1)]; // 寻找最小的中值 if(medianBestMedian) { BestMedian median; BestIdx i; } }按行进行第一行[ 0 20 25 80 22]先排序 [0,20,22,25,80]再计算中值 median 22...计算每一行Descriptor Median D0 22 D1 19 ← 最小 D2 21 D3 76 D4 21最终选择D1。UpdateNormalAndDepth法向量与观测距离区间之前文章中写过主要计算当前地图点的三个属性平均观测方向mNormalVector最大有效观测距离mfMaxDistance最小有效观测距离mfMinDistance模块七地图与线程交互Map / LocalMapping—— 后端优化的强制同步将刚刚创建的初始关键帧放入局部建图线程Local Mapping的等待处理队列中。局部建图线程会随后对其进行处理如创建更复杂的共视关系、剔除冗余点等。mpLocalMapper-InsertKeyFrame(pKFini); mLastFrame Frame(mCurrentFrame); mnLastKeyFrameId mCurrentFrame.mnId; mpLastKeyFrame pKFini; mvpLocalKeyFrames.push_back(pKFini); mvpLocalMapPoints mpMap-GetAllMapPoints(); mpReferenceKF pKFini; mCurrentFrame.mpReferenceKF pKFini; mpMap-SetReferenceMapPoints(mvpLocalMapPoints); mpMapDrawer-SetCurrentCameraPose(mCurrentFrame.mTcw); mState OK;更新上一帧数据将当前帧拷贝给 mLastFrame用于下一帧的匀速运动模型预测。设置参考关键帧将初始关键帧设为当前帧的参考关键帧mpReferenceKF用于后续的局部地图跟踪。填充局部地图点将所有地图点就是刚刚生成的这些点设置为“局部地图点”供下一帧进行特征匹配和投影搜索。更新可视化通知绘图器更新相机位姿。设置状态为 OK这是最重要的标志。从此系统跳出初始化阶段进入正常的 OK 跟踪状态。下一帧到来时将直接执行 Track() 函数 ORB_SLAM2 ：双目相机初始化 StereoInitialization()
StereoInitialization函数主要的作用是双目/深度相机模式的初始化涵盖了系统初始化、相机位姿管理、地图点创建与维护、以及关键帧数据库。明确双目初始化到底初始化了什么1. 初始化了“世界坐标系”的原点和尺度物理意义原点在哪代码中将第一帧的相机位姿直接设为单位矩阵ISetPose(eye(4,4))。这意味着世界坐标系的原点(0,0,0)就被定义在了第一帧相机光心的位置。后续所有地图点、所有关键帧的位姿都是相对于这个原点来进行描述的。尺度是什么在单目视觉中我们无法知道真实世界的物理尺寸比如不知道“1单位”是1厘米还是1米存在尺度模糊性。但双目/RGB-D相机不同因为它有固定的物理基线b比如两个摄像头间距10cm通过公式 Zf⋅bdZdf⋅b 计算出的深度是真实的物理距离如米。因此这次初始化锁死了地图的绝对尺度后续建图大小就是真实世界的大小。2. 初始化了“稀疏地图点云”内容填充仅仅知道坐标系是空的没用。初始化过程中遍历了当前帧所有有效的特征点利用反投影UnprojectStereo公式将图像上的2D像素点转换成了3D空间点MapPoint。这些点构成了系统启动时的初始稀疏点云。没有这些点下一帧到来时就找不到任何东西去匹配跟踪就无从谈起。3. 初始化了“地图的第一个节点”参考锚点SLAM中的地图是一个不断增长的图结构关键帧 地图点。初始化创建了第一个KeyFrame关键帧并将其插入地图。这个关键帧充当了后续所有优化和建图的锚点。例如第2帧来临时它会以这个初始关键帧为参考进行位姿估计TrackReferenceKeyFrame。模块一立体初始化——尺度确定与单帧建图if(mCurrentFrame.N500) { mCurrentFrame.SetPose(cv::Mat::eye(4,4,CV_32F)); }为什么要500个点双目初始化非常依赖数量充足、分布均匀的稳定特征点。少于500个点意味着场景纹理极弱或图像质量较差此时强行初始化会产生极其不稳定的初始化地图导致系统瞬间丢失。这是一个鲁棒性门槛。为什么将初始位姿矩阵设置为单位阵双目/RGB-D相机的最核心优势是直接提供绝对尺度。在单目SLAM中初始化需要阶段本质矩阵Essential Matrix并面临尺度模糊性而在此处将第一个关键帧的相机位姿设为 TcwI。这意味着地图的原点就在相机光心处。后续所有的地图点、关键帧位姿都将相对于这个原点进行表达。模块二位姿矩阵更新/*SetPose - UpdatePoseMatrices*/ mRcw mTcw.rowRange(0,3).colRange(0,3); mtcw mTcw.rowRange(0,3).col(3); mOw -mRcw.t() * mtcw;模块三创建初始关键帧并加入地图KeyFrame* pKFini new KeyFrame(mCurrentFrame,mpMap,mpKeyFrameDB); mpMap-AddKeyFrame(pKFini);作用KeyFrame不仅存储了图像特征点还存储了词袋向量BoW用于后续的快速匹配和回环检测。将其加入地图的mspKeyFrames集合中标志地图生命周期的开始。模块四遍历特征点进行反投影生成地图点for(int i0; imCurrentFrame.N;i) { float z mCurrentFrame.mvDepth[i]; if(z0) { cv::Mat x3D mCurrentFrame.UnprojectStereo(i); // 核心反投影 MapPoint* pNewMP new MapPoint(x3D,pKFini,mpMap); ... } }立体反投影/*v::Mat x3D mCurrentFrame.UnprojectStereo(i);*/ const float x (u - cx) * z * invfx; const float y (v - cy) * z * invfy; cv::Mat x3Dc (cv::Mat_float(3,1) x, y, z); return mRwc * x3Dc mOw;模块五构建地图点的观测关系MapPoint* pNewMP new MapPoint(x3D,pKFini,mpMap); pNewMP-AddObservation(pKFini,i); pKFini-AddMapPoint(pNewMP,i);MapPoint记录它被哪个关键帧的哪个特征点观测到。KeyFrame记录它的哪个索引对应哪个地图点。为什么这么做这是为了后续的局部建图Local Mapping和优化BA做准备。在优化时系统需要通过这些关联快速找到重投影误差 eu−π(K⋅Tcw⋅Pw) 中的对应关系1. AddObservation观测计数加权/*AddObservation*/ if(pKF-mvuRight[idx]0) nObs 2; else nObs;权重公式对于立体相机的右目匹配因为左右目同时看到了该点提供了更强的极线约束所以观测计数加 2。这决定了在局部调整Local BA中该点的残差权重更高优化更稳健。模块六地图点MapPoint的核心维护 —— 描述子、法线与深度ComputeDistinctiveDescriptors代表性描述子选取一个 MapPoint 可能被多个 KeyFrame 观测到因此会对应多个 ORB 描述子。ComputeDistinctiveDescriptors()会从这些描述子中选出一个与其他描述子“整体最接近”的描述子作为该 MapPoint 的代表描述子mDescriptor。先理解为什么一个MapPoint会有很多个描述子假设地图中有一个三维点P那么这个三维点可能被多个关键帧观测到由于观测角度不同、相机距离不同、关照不同、图像模糊程度不同、尺度不同每个关键中提取出来的ORB描述子可能不同。例如KeyFrame 1: MapPoint P ↓ Descriptor D1 KeyFrame 2: MapPoint P ↓ Descriptor D2 KeyFrame 3: MapPoint P ↓ Descriptor D3因此一个 MapPoint 可能有D1D2D3......。但是后续进行特征匹配时不可能每次都拿 MapPoint 的所有描述子去匹配。所以 ORB-SLAM2 要选出一个最能够代表这个 MapPoint 的描述子。函数整体流程MapPoint │ │ 1. 获取所有观测该点的 KeyFrame ▼ mObservations │ │ 2. 从每个 KeyFrame 中取出对应 ORB Descriptor ▼ vDescriptors │ │ 3. 计算所有 Descriptor 两两之间的 Hamming Distance ▼ Distances[N][N] │ │ 4. 对每个 Descriptor │ 计算它到其他所有 Descriptor 距离的中值 ▼ Median Distance │ │ 5. 找到中值最小的 Descriptor ▼ mDescriptorDescriptorDistance() 计算两个描述子之间的汉明距离for (size_t i 0; iN; i) { // 和自己的距离当然是0 Distances[i][i]0; // 计算并记录不同描述子汉明距离 for(size_t ji1;jN;j) { int distij ORBmatcher::DescriptorDistance(vDescriptors[i],vDescriptors[j]); Distances[i][j]distij; Distances[j][i]distij; } }本质计算两个二进制描述子中有几个bit位不一样假设有5个描述子计算结果为D0 D1 D2 D3 D4 D0 0 20 25 80 22 D1 20 0 18 75 19 D2 25 18 0 78 21 D3 80 75 78 0 76 D4 22 19 21 76 0选取最具有代表性的描述子int BestMedian INT_MAX; // 记录最小的中值 int BestIdx 0; // 最小中值对应的索引 for(size_t i0;iN;i) { // 第i个描述子到其它所有描述子之间的距离 // vectorint vDists(Distances[i],Distances[i]N); vectorint vDists(Distances[i].begin(), Distances[i].end()); sort(vDists.begin(), vDists.end()); // 获得中值 int median vDists[0.5*(N-1)]; // 寻找最小的中值 if(medianBestMedian) { BestMedian median; BestIdx i; } }按行进行第一行[ 0 20 25 80 22]先排序 [0,20,22,25,80]再计算中值 median 22...计算每一行Descriptor Median D0 22 D1 19 ← 最小 D2 21 D3 76 D4 21最终选择D1。UpdateNormalAndDepth法向量与观测距离区间之前文章中写过主要计算当前地图点的三个属性平均观测方向mNormalVector最大有效观测距离mfMaxDistance最小有效观测距离mfMinDistance模块七地图与线程交互Map / LocalMapping—— 后端优化的强制同步将刚刚创建的初始关键帧放入局部建图线程Local Mapping的等待处理队列中。局部建图线程会随后对其进行处理如创建更复杂的共视关系、剔除冗余点等。mpLocalMapper-InsertKeyFrame(pKFini); mLastFrame Frame(mCurrentFrame); mnLastKeyFrameId mCurrentFrame.mnId; mpLastKeyFrame pKFini; mvpLocalKeyFrames.push_back(pKFini); mvpLocalMapPoints mpMap-GetAllMapPoints(); mpReferenceKF pKFini; mCurrentFrame.mpReferenceKF pKFini; mpMap-SetReferenceMapPoints(mvpLocalMapPoints); mpMapDrawer-SetCurrentCameraPose(mCurrentFrame.mTcw); mState OK;更新上一帧数据将当前帧拷贝给 mLastFrame用于下一帧的匀速运动模型预测。设置参考关键帧将初始关键帧设为当前帧的参考关键帧mpReferenceKF用于后续的局部地图跟踪。填充局部地图点将所有地图点就是刚刚生成的这些点设置为“局部地图点”供下一帧进行特征匹配和投影搜索。更新可视化通知绘图器更新相机位姿。设置状态为 OK这是最重要的标志。从此系统跳出初始化阶段进入正常的 OK 跟踪状态。下一帧到来时将直接执行 Track() 函数