当前位置：首页 > news >正文

ORB_SLAM2原理及代码解析：MapPoint::UpdateNormalAndDepth() 函数

news 2025/10/3 9:08:50

目录

1 作用

2 位置及代码

3 参数及含义

4 代码解析

4.1 加锁及拷贝数据

4.2 检测是否观测

4.3 计算平均法向量

4.3.1 GetCameraCenter() 函数

4.3.2 normali / cv::norm(normali)

4.4 计算平均可观测距离

4.5 更新成员变量

1 作用

更新地图点的法向量 (mNormalVector)，更新地图点的可观测距离范围 (mfMinDistance / mfMaxDistance)以便后续在跟踪或重建中判断该点是否可见。即把地图点的方向和深度信息重新计算一遍。

2 位置及代码

（1）声明：MapPoint.h

    void UpdateNormalAndDepth();

（2）定义：MapPoint.cc

void MapPoint::UpdateNormalAndDepth()
{map<KeyFrame*,size_t> observations;KeyFrame* pRefKF;cv::Mat Pos;{unique_lock<mutex> lock1(mMutexFeatures);unique_lock<mutex> lock2(mMutexPos);if(mbBad)return;observations=mObservations;pRefKF=mpRefKF;Pos = mWorldPos.clone();}if(observations.empty())return;cv::Mat normal = cv::Mat::zeros(3,1,CV_32F);int n=0;for(map<KeyFrame*,size_t>::iterator mit=observations.begin(), mend=observations.end(); mit!=mend; mit++){KeyFrame* pKF = mit->first;cv::Mat Owi = pKF->GetCameraCenter();cv::Mat normali = mWorldPos - Owi;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];const int nLevels = pRefKF->mnScaleLevels;{unique_lock<mutex> lock3(mMutexPos);mfMaxDistance = dist*levelScaleFactor;mfMinDistance = mfMaxDistance/pRefKF->mvScaleFactors[nLevels-1];mNormalVector = normal/n;}
}

3 参数及含义

4 代码解析

4.1 加锁及拷贝数据

    map<KeyFrame*,size_t> observations;KeyFrame* pRefKF;cv::Mat Pos;{unique_lock<mutex> lock1(mMutexFeatures);unique_lock<mutex> lock2(mMutexPos);if(mbBad)return;observations=mObservations;pRefKF=mpRefKF;Pos = mWorldPos.clone();}

4.2 检测是否观测

    if(observations.empty())return;

4.3 计算平均法向量

    cv::Mat normal = cv::Mat::zeros(3,1,CV_32F);int n=0;for(map<KeyFrame*,size_t>::iterator mit=observations.begin(), mend=observations.end(); mit!=mend; mit++){KeyFrame* pKF = mit->first;cv::Mat Owi = pKF->GetCameraCenter();cv::Mat normali = mWorldPos - Owi;normal = normal + normali/cv::norm(normali);n++;}

4.3.1 GetCameraCenter() 函数

（1）声明：frame.h
    // Returns the camera center.inline cv::Mat GetCameraCenter(){return mOw.clone();}
（2）定义：KeyFrame.cc
cv::Mat KeyFrame::GetCameraCenter()
{unique_lock<mutex> lock(mMutexPose);return Ow.clone();
}
相机中心Ow运算见SetPose() 函数-2.2：

https://blog.csdn.net/weixin_45728280/article/details/152333209?spm=1011.2124.3001.6209

4.3.2 normali / cv::norm(normali)

将向量单位化，只考虑方向，忽略大小。

4.4 计算平均可观测距离

    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];//特征点在金字塔层数，后续调整尺度const int nLevels = pRefKF->mnScaleLevels;//获取金字塔总层数

4.5 更新成员变量

    {unique_lock<mutex> lock3(mMutexPos);mfMaxDistance = dist*levelScaleFactor;mfMinDistance = mfMaxDistance/pRefKF->mvScaleFactors[nLevels-1];//最小距离 = 最大距离 / 最小尺度因子mNormalVector = normal/n;//地图点被关键帧测得的平均方向}

查看全文

http://www.dtcms.com/a/435297.html