MapPoint::UpdateNormalAndDepth()函数是ORB_SLAM2里MapPoint几何属性维护的核心函数。它主要做两件事更新MapPoint的平均观测方向、更新MapPoint的合理观测距离范围。最终更新三个非常重要的核心成员。mNormalVector // 平均观测方向 mfMaxDistance // 最大有效观测距离 mfMinDistance // 最小有效观测距离一、先理解MapPoint中存储的是什么一个MapPoint可以理解为世界坐标系中一个稳定的3D点。例如世界坐标系 Z ↑ │ │ MapPoint │ ● │ / \ │ / \ │ / \ │ / \ Camera1 Camera2 Camera3这个点可能被很多关键帧观测到所以MapPoint不只是一个位置还要包括哪些关键帧能看到我从什么方向看到我多远距离还能正常看到我我的最佳ORB描述子是什么void MapPoint::UpdateNormalAndDepth() { // Step 1 获得观测到该地图点的所有关键帧、坐标等信息 // observations表示 哪些关键帧观测到了这个地图点以及这个地图点对应关键帧中的那个特征点 //例如 KF1 → feature 125 mapKeyFrame*,size_t observations; KeyFrame* pRefKF; //表示这个地图点的参考关键帧 cv::Mat Pos; //MapPoint 在世界坐标系中的 3D 坐标。 { unique_lockmutex lock1(mMutexFeatures); unique_lockmutex lock2(mMutexPos); if(mbBad) return; observationsmObservations; // 获得观测到该地图点的所有关键帧 pRefKFmpRefKF; // 观测到该点的参考关键帧第一次创建时的关键帧 Pos mWorldPos.clone(); // 地图点在世界坐标系中的位置 } if(observations.empty()) return; // Step 2 计算该地图点的平均观测方向 // 能观测到该地图点的所有关键帧对该点的观测方向归一化为单位向量然后进行求和得到该地图点的朝向 // 初始值为0向量累加为归一化向量最后除以总数n cv::Mat normal cv::Mat::zeros(3,1,CV_32F); //创建一个三行1列的零向量 int n0; //遍历所有能观测到这个点的关键帧 for(mapKeyFrame*,size_t::iterator mitobservations.begin(), mendobservations.end(); mit!mend; mit) { KeyFrame* pKF mit-first; cv::Mat Owi pKF-GetCameraCenter(); //获取相机中心坐标 // 获得地图点和观测到它关键帧的向量并归一化 cv::Mat normali mWorldPos - Owi; //从相机中心指向 MapPoint 的向量。 normal normal normali/cv::norm(normali); // 归一化向量并累加归一化的原因是只保留方向 n; } cv::Mat PC Pos - pRefKF-GetCameraCenter(); // 参考关键帧相机指向地图点的向量在世界坐标系下的表示 const float dist cv::norm(PC); // 该点到参考关键帧相机的距离 const int level pRefKF-mvKeysUn[observations[pRefKF]].octave; // 观测到该地图点的当前帧的特征点在金字塔的第几层 const float levelScaleFactor pRefKF-mvScaleFactors[level]; // 当前金字塔层对应的尺度因子scale^nscale1.2n为层数 const int nLevels pRefKF-mnScaleLevels; // 金字塔总层数默认为8 { unique_lockmutex lock3(mMutexPos); // 使用方法见PredictScale函数前的注释 mfMaxDistance dist*levelScaleFactor; // 观测到该点的距离上限 mfMinDistance mfMaxDistance/pRefKF-mvScaleFactors[nLevels-1]; // 观测到该点的距离下限 mNormalVector normal/n; // 获得地图点平均的观测方向 } }ORB-SLAM2 不只在原始图像上检测特征而是建立Level 0 原图↓Level 1 缩小↓Level 2 更小↓Level 3↓...Level 7例如缩放因子scaleFactor1.2那么Level 0 → 1 Level 1 → 1.2 Level 2 → 1.2² Level 3 → 1.2³ ...计算 mfMaxDistance给MapPoint 建立最大有效观测距离因为一个特征点在参考 KeyFrame 中是在某一个尺度层被检测出来的。假设MapPoint距离相机 dist 5m 特征点 level 2那么dmax5×1.22 约7.2m意思可以粗略理解成如果相机距离这个 MapPoint 太远超过这个尺度范围就不太适合用当前的 ORB 特征尺度来描述它。计算mfMinDistance建立最小有效观测距离假设层数为8计算完最大和最小就可以得到一个有效距离区间这就是UpdateNormalAndDepth()最重要的实际意义。假设一个 MapPointmfMinDistance 2m mfMaxDistance 10m那么相机距离 MapPoint 1m → 太近 3m → 合理 5m → 合理 8m → 合理 12m → 太远后面 ORB-SLAM2 就可以根据当前相机距离判断这个这个 MapPoint 应该使用哪个 ORB 金字塔尺度。平均观测方向