# MapPoint.cpp

### MapPoint::MapPoint()
用三維點構造地圖點，構建地圖點
在[Tracking::CreateInitialMapMonocular()](#Tracking::CreateInitialMapMonocular()),[Tracking::CreateNewKeyFrame()](#Tracking::CreateNewKeyFrame()),
[Tracking::UpdateLastFrame()](#Tracking::UpdateLastFrame())中引用

#### 定義

MapPoint::MapPoint(const cv::Mat &Pos,    //3維點
                   KeyFrame *pRefKF,      //關鍵幀
                   Map* pMap):            //地圖

#### 函式流程
1. 構建地圖點

#### 函式詳解
1. 構建地圖點

Pos.copyTo(mWorldPos);

//平均觀測方向初始化為0
mNormalVector = cv::Mat::zeros(3,1,CV_32F);

// MapPoints can be created from Tracking and Local Mapping. This mutex avoid conflicts with id.
unique_lock<mutex> lock(mpMap->mMutexPointCreation);
mnId=nNextId++;

### MapPoint::AddObservation()
在地圖點上添加與關鍵幀的觀測關係
在[Tracking::CreateInitialMapMonocular()](#Tracking::CreateInitialMapMonocular())中引用

#### 定義

void MapPoint::AddObservation(KeyFrame* pKF, //關鍵幀
                              size_t idx)    //關鍵幀的地圖點ID

#### 函式流程
1. 確認是否有觀測關係
2. 將關鍵幀相對應索引添加到地圖點上

#### 函式詳解
1. 確認是否有觀測關係

unique_lock<mutex> lock(mMutexFeatures);
//如果已經有觀測關係，跳過
if(mObservations.count(pKF))
    return;

2. 將關鍵幀相對應索引添加到地圖點上

// 記錄下能觀測到該MapPoint的KF和該MapPoint在KF中的索引
mObservations[pKF]=idx;

if(pKF->mvuRight[idx]>=0)
    nObs+=2; // 雙目或者grbd
else
    nObs++; // 單目

### MapPoint::ComputeDistinctiveDescriptors()
從眾多觀測到該地圖點的特徵點中挑選最具有代表性的描述子
先獲得當前點的所有描述子，然後計算描述子之間的兩兩距離，最好的描述子與其他描述子應該具有最小的距離中值。
在[Tracking::CreateInitialMapMonocular()](#Tracking::CreateInitialMapMonocular()),[Tracking::CreateNewKeyFrame()](#Tracking::CreateNewKeyFrame())，[LocalMapping::SearchInNeighbors()](#LocalMapping::SearchInNeighbors())中引用

#### 函式流程
1. 獲取所有的觀測，跳過壞點
2. 遍歷觀測到3d點的所有關鍵幀，獲得orb描述子
3. 獲得這些描述子兩兩之間的距離
4. 選擇最具有代表性的描述子

#### 函式詳解
1. 獲取所有的觀測，跳過壞點

vector<cv::Mat> vDescriptors;

map<KeyFrame*,size_t> observations;

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

if(observations.empty())
    return;

2. 遍歷觀測到3d點的所有關鍵幀，獲得orb描述子

vDescriptors.reserve(observations.size());

// 遍歷觀測到3d點的所有關鍵幀，獲得orb描述子，並插入到vDescriptors中
for(map<KeyFrame*,size_t>::iterator mit=observations.begin(), mend=observations.end(); mit!=mend; mit++)
{
    KeyFrame* pKF = mit->first;

    if(!pKF->isBad())
        vDescriptors.push_back(pKF->mDescriptors.row(mit->second)); //取得對應的描述子
}

if(vDescriptors.empty())
    return;

3. 獲得這些描述子兩兩之間的距離

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

//float Distances[N][N];
//距離
std::vector<std::vector<float> > Distances;
Distances.resize(N, vector<float>(N, 0));
//紀錄距離對稱矩陣
for (size_t i = 0; i<N; i++)
{
    //與自己的距離為0
    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;
    }
}

4. 選擇最具有代表性的描述子

// 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++)
{
    // 第i個描述子到其它所有所有描述子之間的距離
    //vector<int> vDists(Distances[i],Distances[i]+N);
    vector<int> vDists(Distances[i].begin(), Distances[i].end());
    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();       
}

### MapPoint::UpdateNormalAndDepth()
更新平均觀測方向
在[Tracking::CreateNewKeyFrame()](#Tracking::CreateNewKeyFrame())，[Tracking::CreateInitialMapMonocular()](#Tracking::CreateInitialMapMonocular())，[LocalMapping::SearchInNeighbors()](#LocalMapping::SearchInNeighbors()) 中引用

#### 函式流程
1. 取得觀測數值
2. 計算法線方向

#### 函式詳解

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();    // 地圖點在世界坐標系中的位置
}

if(observations.empty())
    return;

2. 計算該地圖點的平均觀測方向(法線方向)

// 能觀測到該地圖點的所有關鍵幀，對該點的觀測方向歸一化為單位向量，然後進行求和得到該地圖點的朝向
// 初始值為0向量，累加為歸一化向量，最後除以總數n
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];          // 當前金字塔層對應的尺度因子，scale^n，scale=1.2，n為層數
const int nLevels = pRefKF->mnScaleLevels;                              // 金字塔總層數，默認為8

{
    unique_lock<mutex> lock3(mMutexPos);
    // 使用方法見PredictScale函數前的註釋
    mfMaxDistance = dist*levelScaleFactor;                              // 觀測到該點的距離上限
    mfMinDistance = mfMaxDistance/pRefKF->mvScaleFactors[nLevels-1];    // 觀測到該點的距離下限
    mNormalVector = normal/n;                                           // 獲得地圖點平均的觀測方向
}

### MapPoint::PredictScale()
預測地圖點對應特徵點的金字塔尺度
在[ORBmatcher::Fuse()](ORBmatcher::Fuse())中引用

#### 定義

int MapPoint::PredictScale(const float &currentDist,     //相機光心離地圖點的距離
                           KeyFrame* pKF)                //關鍵幀

#### 函式流程
1. 預測地圖點對應特徵點的金字塔尺度

#### 函式詳解

1. 預測地圖點對應特徵點的金字塔尺度

float ratio;
{
    unique_lock<mutex> lock(mMutexPos);
    // mfMaxDistance = ref_dist*levelScaleFactor為參考幀考慮上尺度後的距離
    // ratio = mfMaxDistance/currentDist = ref_dist/cur_dist
    ratio = mfMaxDistance/currentDist;
}

// 同時取log線性化
int nScale = ceil(log(ratio)/pKF->mfLogScaleFactor);
if(nScale<0)
    nScale = 0;
else if(nScale>=pKF->mnScaleLevels)
    nScale = pKF->mnScaleLevels-1;

return nScale;

### MapPoint::GetMaxDistanceInvariance()
最大的尺度變化範圍
在[Frame::isInFrustum()](#Frame::isInFrustum)中引用

#### 函式流程
1. 回傳1.2倍的最大範圍

#### 函式詳解
1. 回傳1.2倍的最大範圍

unique_lock<mutex> lock(mMutexPos);
return 1.2f*mfMaxDistance;

### MapPoint::GetMinDistanceInvariance()
最小的尺度變化範圍
在[Frame::isInFrustum()](#Frame::isInFrustum)中引用

#### 函式流程
1. 回傳0.8倍的最小範圍

#### 函式詳解
1. 回傳0.8倍的最小範圍

unique_lock<mutex> lock(mMutexPos);
return 0.8f*mfMinDistance;

### MapPoint::GetNormal()
平均觀測方向
mNormalVector值在[MapPoint::UpdateNormalAndDepth()](#MapPoint::UpdateNormalAndDepth())中更新
在[Frame::isInFrustum()](#Frame::isInFrustum)中引用

#### 函式流程
1. 回傳平均觀測方向

#### 函式詳解
1. 回傳平均觀測方向

unique_lock<mutex> lock(mMutexPos);
return mNormalVector.clone();

### MapPoint::GetReplaced()
地圖點是否需要替換
mpReplaced 在[MapPoint::Replace](#MapPoint::Replace)中更新
在[Tracking::CheckReplacedInLastFrame()](#Tracking::CheckReplacedInLastFrame())中引用

#### 函式流程
1. 地圖點是否需要替換

#### 函式詳解
1. 地圖點是否需要替換

unique_lock<mutex> lock1(mMutexFeatures);
unique_lock<mutex> lock2(mMutexPos);
return mpReplaced;

### MapPoint::IsInKeyFrame()
地圖點是否在關鍵幀中，
在[LocalMapping::ProcessNewKeyFrame()](#LocalMapping::ProcessNewKeyFrame())中引用

#### 定義

bool MapPoint::IsInKeyFrame(KeyFrame *pKF)    //輸入關鍵幀

#### 函式流程
1. 地圖點是否在關鍵幀中

#### 函式詳解
1. 地圖點是否在關鍵幀中

unique_lock<mutex> lock(mMutexFeatures);
return (mObservations.count(pKF));

### MapPoint::GetFoundRatio()
計算被找到的比例
mnFound ：地圖點被多少幀（包括普通幀）看到，次數越多越好
mnVisible：地圖點應該被看到的次數
(mnFound/mnVisible）：對於大FOV鏡頭這個比例會高，對於窄FOV鏡頭這個比例會低
在[LocalMapping::MapPointCulling()](#LocalMapping::MapPointCulling())中引用

#### 函式流程
1. 計算被找到的比例

#### 函式詳解
1. 計算被找到的比例

unique_lock<mutex> lock(mMutexFeatures);

return static_cast<float>(mnFound)/mnVisible;

### MapPoint::SetBadFlag()
告知可以觀測到該MapPoint的Frame，該MapPoint已被刪除
在[LocalMapping::MapPointCulling()](#LocalMapping::MapPointCulling())中引用

#### 函式流程
1. 清除該地圖點的觀測關鍵幀
2. 將觀測到該地圖點的關鍵幀的匹配關係刪除

#### 函式詳解
1. 清除該地圖點的觀測關鍵幀

map<KeyFrame*,size_t> obs;
{
    unique_lock<mutex> lock1(mMutexFeatures);
    unique_lock<mutex> lock2(mMutexPos);
    mbBad=true;
    // 把mObservations轉存到obs，obs和mObservations裡存的是指針，賦值過程為淺拷貝
    obs = mObservations;
    // 把mObservations指向的內存釋放，obs作為局部變量之後自動刪除
    mObservations.clear();
}

2. 將觀測到該地圖點的關鍵幀的匹配關係刪除

//遍歷觀測到該地圖點的關鍵幀
for(map<KeyFrame*,size_t>::iterator mit=obs.begin(), mend=obs.end(); mit!=mend; mit++)
{
    KeyFrame* pKF = mit->first;
    // 告訴可以觀測到該MapPoint的KeyFrame，該MapPoint被刪了
    pKF->EraseMapPointMatch(mit->second);
}
// 擦除該MapPoint申請的內存
mpMap->EraseMapPoint(this);
}

pKF->EraseMapPointMatch(): 跳到[KeyFrame::EraseMapPointMatch()ID](#KeyFrame::EraseMapPointMatchID)