# MapDrawer.cpp

### MapDrawer::GetCurrentOpenGLCameraMatrix()
將相機姿態mCameraPose由Mat類型轉化為OpenGlMatrix類型
在[Viewer::Run()](#Viewer::Run())中引用

#### 函式流程
1. 取得相機的姿態
2. 將姿態變成轉換矩陣

#### 流程詳解

1. 取得相機的姿態

if(!mCameraPose.empty())
{
    cv::Mat Rwc(3,3,CV_32F);
    cv::Mat twc(3,1,CV_32F);
    {
        unique_lock<mutex> lock(mMutexCamera);
        Rwc = mCameraPose.rowRange(0,3).colRange(0,3).t();
        twc = -Rwc*mCameraPose.rowRange(0,3).col(3);
    }

2. 將姿態變成轉換矩陣

    M.m[0] = Rwc.at<float>(0,0);
    M.m[1] = Rwc.at<float>(1,0);
    M.m[2] = Rwc.at<float>(2,0);
    M.m[3]  = 0.0;

    M.m[4] = Rwc.at<float>(0,1);
    M.m[5] = Rwc.at<float>(1,1);
    M.m[6] = Rwc.at<float>(2,1);
    M.m[7]  = 0.0;

    M.m[8] = Rwc.at<float>(0,2);
    M.m[9] = Rwc.at<float>(1,2);
    M.m[10] = Rwc.at<float>(2,2);
    M.m[11]  = 0.0;

    M.m[12] = twc.at<float>(0);
    M.m[13] = twc.at<float>(1);
    M.m[14] = twc.at<float>(2);
    M.m[15]  = 1.0;
}
else
    M.SetIdentity();

### MapDrawer::DrawCurrentCamera()
繪製當前相機
在[Viewer::Run()](#Viewer::Run())
#### 定義

void MapDrawer::DrawCurrentCamera(pangolin::OpenGlMatrix &Twc) //輸入轉換矩陣

#### 函式流程
1. 計算相機在世界座標的位置
2. 根據相機姿態畫出當前相機姿態對應的綠色方框

#### 流程詳解

1. 計算相機在世界座標的位置

//相機模型大小：寬度佔總寬度比例為0.08
const float &w = mCameraSize;
const float h = w*0.75;
const float z = w*0.6;

//儲存當前姿態
glPushMatrix();

//將4*4的矩陣Twc.m右乘一個當前矩陣
//（由於使用了glPushMatrix函數，因此當前幀矩陣為世界坐標系下的單位矩陣）
//因為OpenGL中的矩陣為列優先存儲，因此實際為Tcw，即相機在世界坐標下的位姿
#ifdef HAVE_GLES
    glMultMatrixf(Twc.m);
#else
    glMultMatrixd(Twc.m);
#endif

2. 根據相機姿態畫出當前相機姿態對應的綠色方框

//設置繪製圖形時線的寬度
glLineWidth(mCameraLineWidth);
//設置當前顏色為綠色(相機圖標顯示為綠色)
glColor3f(0.0f,1.0f,0.0f);
//用線將下面的頂點兩兩相連
glBegin(GL_LINES);
glVertex3f(0,0,0);
glVertex3f(w,h,z);
glVertex3f(0,0,0);
glVertex3f(w,-h,z);
glVertex3f(0,0,0);
glVertex3f(-w,-h,z);
glVertex3f(0,0,0);
glVertex3f(-w,h,z);

glVertex3f(w,h,z);
glVertex3f(w,-h,z);

glVertex3f(-w,h,z);
glVertex3f(-w,-h,z);

glVertex3f(-w,h,z);
glVertex3f(w,h,z);

glVertex3f(-w,-h,z);
glVertex3f(w,-h,z);
glEnd();

glPopMatrix();

### MapDrawer::DrawKeyFrames()
繪製當前關鍵幀
在[Viewer::Run()](#Viewer::Run())

#### 函式流程
1. 取出所有的關鍵幀
2. 顯示所有關鍵幀
3. 顯示本質圖(Essential Graph)

#### 流程詳解

1. 取出所有的關鍵幀

//歷史關鍵幀圖標：寬度佔總寬度比例為0.05
const float &w = mKeyFrameSize;
const float h = w*0.75;
const float z = w*0.6;

//取出 vector<KeyFrame*>(mspKeyFrames.begin(),mspKeyFrames.end());
const vector<KeyFrame*> vpKFs = mpMap->GetAllKeyFrames();

2. 顯示所有關鍵幀

//通過顯示界面選擇是否顯示歷史關鍵幀圖標
if(bDrawKF)
{
    for(size_t i=0; i<vpKFs.size(); i++)
    {
        KeyFrame* pKF = vpKFs[i];
        //轉置, OpenGL中的矩陣為列優先存儲
        cv::Mat Twc = pKF->GetPoseInverse().t();

        glPushMatrix();

        //（由於使用了glPushMatrix函數，因此當前幀矩陣為世界坐標系下的單位矩陣）
        //因為OpenGL中的矩陣為列優先存儲，因此實際為Tcw，即相機在世界坐標下的位姿
        glMultMatrixf(Twc.ptr<GLfloat>(0));

        //設置繪製圖形時線的寬度
        glLineWidth(mKeyFrameLineWidth);
        //設置當前顏色為藍色(關鍵幀圖標顯示為藍色)
        glColor3f(0.0f,0.0f,1.0f);
        //用線將下面的頂點兩兩相連
        glBegin(GL_LINES);
        glVertex3f(0,0,0);
        glVertex3f(w,h,z);
        glVertex3f(0,0,0);
        glVertex3f(w,-h,z);
        glVertex3f(0,0,0);
        glVertex3f(-w,-h,z);
        glVertex3f(0,0,0);
        glVertex3f(-w,h,z);

        glVertex3f(w,h,z);
        glVertex3f(w,-h,z);

        glVertex3f(-w,h,z);
        glVertex3f(-w,-h,z);

        glVertex3f(-w,h,z);
        glVertex3f(w,h,z);

        glVertex3f(-w,-h,z);
        glVertex3f(w,-h,z);
        glEnd();

        glPopMatrix();
    }
}

3. 顯示本質圖(Essential Graph)

/**已知共視圖中存儲了所有關鍵幀的共視關係，本徵圖中對邊進行了優化,
保存了所有節點,只存儲了具有較多共視點的邊,用於進行優化，而生成樹則進一步進行了優化,
保存了所有節點,但是值保存具有最多共視地圖點的關鍵幀的邊**/

//通過顯示界面選擇是否顯示關鍵幀連接關係
if(bDrawGraph)
{
    //設置繪製圖形時線的寬度
    glLineWidth(mGraphLineWidth);
    //設置共視圖連接線為綠色，透明度為0.6f
    glColor4f(0.0f,1.0f,0.0f,0.6f);
    glBegin(GL_LINES);

    //遍歷關鍵幀
    for(size_t i=0; i<vpKFs.size(); i++)
    {
        // Covisibility Graph
//3.1. 共視程度比較高的共視關鍵幀用線連接
        //遍歷每一個關鍵幀，得到它們共視程度比較高的關鍵幀
        const vector<KeyFrame*> vCovKFs = vpKFs[i]->GetCovisiblesByWeight(100);
        //遍歷每一個關鍵幀，得到它在世界坐標系下的相機坐標
        cv::Mat Ow = vpKFs[i]->GetCameraCenter();
        if(!vCovKFs.empty())
        {
            for(vector<KeyFrame*>::const_iterator vit=vCovKFs.begin(), vend=vCovKFs.end(); vit!=vend; vit++)
            {
                if((*vit)->mnId<vpKFs[i]->mnId)
                    continue;
                cv::Mat Ow2 = (*vit)->GetCameraCenter();
                glVertex3f(Ow.at<float>(0),Ow.at<float>(1),Ow.at<float>(2));
                glVertex3f(Ow2.at<float>(0),Ow2.at<float>(1),Ow2.at<float>(2));
            }
        }

        // Spanning tree
//3.2. 連接最小生成樹
        KeyFrame* pParent = vpKFs[i]->GetParent();
        if(pParent)
        {
            cv::Mat Owp = pParent->GetCameraCenter();
            glVertex3f(Ow.at<float>(0),Ow.at<float>(1),Ow.at<float>(2));
            glVertex3f(Owp.at<float>(0),Owp.at<float>(1),Owp.at<float>(2));
        }

        // Loops
//3.3 連接閉環時形成的連接關係
        set<KeyFrame*> sLoopKFs = vpKFs[i]->GetLoopEdges();
        for(set<KeyFrame*>::iterator sit=sLoopKFs.begin(), send=sLoopKFs.end(); sit!=send; sit++)
        {
            if((*sit)->mnId<vpKFs[i]->mnId)
                continue;
            cv::Mat Owl = (*sit)->GetCameraCenter();
            glVertex3f(Ow.at<float>(0),Ow.at<float>(1),Ow.at<float>(2));
            glVertex3f(Owl.at<float>(0),Owl.at<float>(1),Owl.at<float>(2));
        }
    }

    glEnd();
}

### MapDrawer::DrawMapPoints()
繪製當前地圖點
在[Viewer::Run()](#Viewer::Run())

#### 函式流程
1. 取出所有的地圖點
2. 繪製不包含局部地圖點地圖點(黑色)
3. 繪製局部地圖點(紅色)

#### 流程詳解
1. 取出所有的地圖點

const vector<MapPoint*> &vpMPs = mpMap->GetAllMapPoints();
//取出mvpReferenceMapPoints，也即局部地圖d點
const vector<MapPoint*> &vpRefMPs = mpMap->GetReferenceMapPoints();

mpMap->GetAllMapPoints(): 跳到[Map::GetAllMapPoints()](#Map::GetAllMapPoints())

mpMap->GetReferenceMapPoints(): 跳到[Map::GetReferenceMapPoints()](#Map::GetReferenceMapPoints())

2. 繪製不包含局部地圖點地圖點(黑色)

//將vpRefMPs從vector容器類型轉化為set容器類型，便於使用set::count快速統計
set<MapPoint*> spRefMPs(vpRefMPs.begin(), vpRefMPs.end());

if(vpMPs.empty())
    return;

// for AllMapPoints
//顯示所有的地圖點（不包括局部地圖點），大小為2個像素，黑色
glPointSize(mPointSize);
glBegin(GL_POINTS);
glColor3f(0.0,0.0,0.0);

for(size_t i=0, iend=vpMPs.size(); i<iend;i++)
{
    // 不包括ReferenceMapPoints（局部地圖點）
    if(vpMPs[i]->isBad() || spRefMPs.count(vpMPs[i]))
        continue;
    cv::Mat pos = vpMPs[i]->GetWorldPos();
    glVertex3f(pos.at<float>(0),pos.at<float>(1),pos.at<float>(2));
}
glEnd();

3. 繪製局部地圖點(紅色)

// for ReferenceMapPoints
//顯示局部地圖點，大小為2個像素，紅色
glPointSize(mPointSize);
glBegin(GL_POINTS);
glColor3f(1.0,0.0,0.0);

for(set<MapPoint*>::iterator sit=spRefMPs.begin(), send=spRefMPs.end(); sit!=send; sit++)
{
    if((*sit)->isBad())
        continue;
    cv::Mat pos = (*sit)->GetWorldPos();
    glVertex3f(pos.at<float>(0),pos.at<float>(1),pos.at<float>(2));

}
glEnd();