# ORB-SLAM笔记——(6)LocalMapping

## ProcessNewKeyFrame()

1. 计算当前关键帧的Bag of Words：mpCurrentKeyFrame->ComputeBoW()

2. 在TrackLocalMap阶段，我们已经在局部地图中找出了一些与当前帧能匹配上的地图点。用当前帧对应特征去更新地图点的法向向量（normal vector）和深度，并选出一个最佳描述子。

### UpdateNormalAndDepth()

void MapPoint::UpdateNormalAndDepth()
{
map<KeyFrame*,size_t> observations;
KeyFrame* pRefKF;
cv::Mat Pos;
{
unique_lock<mutex> lock1(mMutexFeatures);
unique_lock<mutex> lock2(mMutexPos);
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;
}
}

### ComputeDistinctiveDescriptors()

void MapPoint::ComputeDistinctiveDescriptors()
{
// Retrieve all observed descriptors
vector<cv::Mat> vDescriptors;

map<KeyFrame*,size_t> observations;

{
unique_lock<mutex> lock1(mMutexFeatures);
return;
observations=mObservations;
}

if(observations.empty())
return;

vDescriptors.reserve(observations.size());

for(map<KeyFrame*,size_t>::iterator mit=observations.begin(), mend=observations.end(); mit!=mend; mit++)
{
KeyFrame* pKF = mit->first;

vDescriptors.push_back(pKF->mDescriptors.row(mit->second));
}

if(vDescriptors.empty())
return;

// Compute distances between them
const size_t N = vDescriptors.size();

float Distances[N][N];
for(size_t i=0;i<N;i++)
{
Distances[i][i]=0;
for(size_t j=i+1;j<N;j++)
{
int distij = ORBmatcher::DescriptorDistance(vDescriptors[i],vDescriptors[j]);
Distances[i][j]=distij;
Distances[j][i]=distij;
}
}

// Take the descriptor with least median distance to the rest
int BestMedian = INT_MAX;
int BestIdx = 0;
for(size_t i=0;i<N;i++)
{
vector<int> vDists(Distances[i],Distances[i]+N);
sort(vDists.begin(),vDists.end());
int median = vDists[0.5*(N-1)];

if(median<BestMedian)
{
BestMedian = median;
BestIdx = i;
}
}

{
unique_lock<mutex> lock(mMutexFeatures);
mDescriptor = vDescriptors[BestIdx].clone();
}
}

3. 在做完点的更新后，需要更新该帧在covisibility graph和essential graph中，和其他关键帧的连接关系。

## MapPointCulling()

void LocalMapping::MapPointCulling()
{
const unsigned long int nCurrentKFid = mpCurrentKeyFrame->mnId;

int nThObs;
if(mbMonocular)
nThObs = 2;
else
nThObs = 3;
const int cnThObs = nThObs;

{
MapPoint* pMP = *lit;
{
}
else if(pMP->GetFoundRatio()<0.25f )
{
}
else if(((int)nCurrentKFid-(int)pMP->mnFirstKFid)>=2 && pMP->Observations()<=cnThObs)
{
}
else if(((int)nCurrentKFid-(int)pMP->mnFirstKFid)>=3)
else
lit++;
}
}

1. 已经是坏点的MapPoints直接从检查链表中删除;

2. 通过GetFoundRatio()返回的(mnFound)/mnVisible的比值，也就是实际能够发生匹配的帧数与可以发生匹配的帧数的比值。如果该值<0.25，将该点置为坏点并从list中删去该点；

3. 如果连续该点从生成以后经历了两个以上的关键帧，但观测数却<=cnThObs，则将该点置为坏点并从list中删去该点；

4. 如果该点从生成以后经历了三个以上的关键帧，并且没有被当作坏点，则停止对它进行更多的判断，认为它是一个mappoint，并从list中删除索引。

## CreateNewMapPoints()

//Check scale consistency
cv::Mat normal1 = x3D-Ow1;
float dist1 = cv::norm(normal1);

cv::Mat normal2 = x3D-Ow2;
float dist2 = cv::norm(normal2);

if(dist1==0 || dist2==0)
continue;

const float ratioDist = dist2/dist1;
const float ratioOctave = mpCurrentKeyFrame->mvScaleFactors[kp1.octave]/pKF2->mvScaleFactors[kp2.octave];

/*if(fabs(ratioDist-ratioOctave)>ratioFactor)
continue;*/
if(ratioDist*ratioFactor<ratioOctave || ratioDist>ratioOctave*ratioFactor)
continue;

11-29 2万+
02-28 2097
08-08 588
11-14 209
12-26 709
12-12 3601
09-04 4367
04-10 2万+
01-12 2466
04-13 707
11-14 86