ORBSLAM中的地圖儲存與載入原版程式碼中沒有給,需要自己來實現.看了達達大神等一系列的博文,終於有了一個大的改動框架,尤其這篇博文算是集大成者,這裡我厚著臉皮進行了轉載~

我在此基礎上我解決了addVertex：FATAL的錯誤,這個錯誤來源於進行G2O設定的時候新增節點時的編號衝突,這個錯誤看似不會影響執行,實際上是比較致命的.由於這個錯誤的存在會導致無法新增新的優化節點,造成優化部分資訊缺失,在我使用TUM的資料集進行測試的時候,如果不修正這個錯誤,常常會發生載入地圖後第二次的軌跡與第一次差很多的嚴重錯誤!!!所以有必要對這個問題進行修改.

具體改動如下:

錯誤產生原因:載入離線地圖之後在開始跟蹤時,Frame,KeyFrame,MapPoint的編號與載入的離線地圖中的編號衝突,導致新的資訊無法新增入優化.

解決辦法:記錄離線地圖的最大編號,在進行載入後,更新Frame,KeyFrame,MapPoint的起始編號(最大編號+1),避免衝突.

1. MapPoint:

Map.cc:

    // Then read MapPoints one after another, and add them into the map
    //讀取MapPoint,同時將其新增入map中
    cerr << "Map.cc :: The number of MapPoints:" << nMapPoints << endl;
    for (unsigned int i = 0; i < nMapPoints; i++) {
        MapPoint *mp = LoadMapPoint(f);
        AddMapPoint(mp);
    }

    //更新一下MapPoint那邊的id,使得後續插入的MapPoint編號從N+1開始.
    //重點!!!!!!!!!!!!!!!!!!!
    if (mnMaxMPid > 0) {
        MapPoint *temp_MapPoint = new MapPoint(mnMaxMPid,this);  //使用MapPoint的建構函式來解決id更新的問題
    }

    cerr<<"Map.cc :: Max MapPoint ID is: " << mnMaxMPid << ", update MapPoint-mnId to this number" <<endl;

    cerr<<"Map.cc :: MapPoint Load OVER!"<<endl;
 

MapPoint.cc

    MapPoint::MapPoint(long unsigned int i, Map* pMap):
            mnFirstKFid(0), mnFirstFrame(0), nObs(0), mnTrackReferenceForFrame(0), mnLastFrameSeen(0), mnBALocalForKF(0), mnFuseCandidateForKF(0), mnLoopPointForKF(0), mnCorrectedByKF(0),
            mnCorrectedReference(0), mnBAGlobalForKF(0), mpRefKF(static_cast<KeyFrame*>(NULL)), mnVisible(1), mnFound(1), mbBad(false),
            mpReplaced(static_cast<MapPoint*>(NULL)), mfMinDistance(0), mfMaxDistance(0), mpMap(pMap)
    {
        // MapPoints can be created from Tracking and Local Mapping. This mutex avoid conflicts with id.
        unique_lock<mutex> lock(mpMap->mMutexPointCreation);
        mnId = i;
        nNextId = i+1;
    }
 

map.cc

    //更新一下Frame那邊的id,把這前N個從離線特徵地圖讀入的KF記作前N個Frame,那麼後續插入的Frame編號從N+1開始記錄
    if (mnMaxKFid > 0) {
        Frame temp_frame = Frame(mnMaxKFid);  //使用Frame的建構函式來解決id更新的問題
        kf_by_order.front()->updateID(mnMaxKFid);  //更新kf的id
    }
    cerr<<"Map.cc :: Max KeyFrame ID is: " << mnMaxKFid << ", update KF(num+2) and Frame mnId to this number" <<endl;
    cerr<<"Map.cc :: Load IS OVER!"<<endl;

Frame.cc

    //After Load Map need to Update frame id
    Frame::Frame( long unsigned int i )
    {
        mnId = i;
        nNextId = i+1;
    }

KeyFrame.cc

//After Load Map need to Update Keyframe id
void KeyFrame::updateID(long unsigned int i)
    {
        mnId=i+1;
        nNextId = i+2;   //為了保險,我這邊直接設定其下一個id是最大id+2
    }

---------------------------------------------------------------我是很醜的分界線-----------------------------------------------------------------

ORB SLAM2 真是個神奇的演算法。他開源了，但是各種不實用。這大概就是學術和工作的區別吧。人家畢竟花了一整個博士階段去開發這個演算法，我們還是應該心存感激。

實驗室裡用的相機是ZED的雙目相機，所以為了這個相機，我做了自己的雙目相機資料包和介面，發現實時SLAM速度在i7處理器的條件下也就只能跑5~6Hz左右，這還不考慮做閉環的時間。顯然，對於一個正在快速運動的機器人來說，實時的SLAM並不實用。為了能夠真正地實時定位，我能想到合理的解決方案是，事先建好地圖，然後儲存地圖（關鍵點、關鍵幀及各種所需連線）。等我們需要定位時，直接載入做純定位即可。

當然，做起來永遠沒有那麼簡單。讀演算法就需要很久，找到問題又要花很久。網上有很多教程，但可惜的是大家都沒能把自己的演算法徹底開源。所以我嘗試提供一個可以直接從github抓下來用的帶有儲存和載入地圖功能的版本，供學習使用。（主要是沒人交流，痛苦啊，乾脆開一個倉庫，求大神幫忙給個pull request啊！）

喜大普奔，現在終於流暢的實現過載功能了（2018/05/02）。一開始是雙目ZED相機的功能已經除錯好。後續更新我會寫在下一段裡面。

感謝前輩大神們的博文：

這是直接從ORB SLAM2原版當中fork出來的。既然開源了，改動的地方大家也能看見，我就不多說了。程式碼持續更新中。。。

（2018/04/17 更新）現在暫時能夠載入地圖了但重定位時還是有問題。感覺是新讀進來的一幀並沒有和之前的最後一幀產生任何關係。我除錯好了再回來寫具體是怎麼回事吧。

（2018/05/01更新）現在載入地圖後的重定位除錯成功，但是插入新的關鍵幀出了些小問題。

（2018/05/02 更新）現在插入新的關鍵幀的問題解決了，這個版本終於可以用了。我現在只調試好了Example/Stereo資料夾裡的stereo_zed.cc檔案。至於stereo_kitti，和stereo_euroc，甚至是單目相機的部分，我暫時先放一會。

（2018/05/03 更新）stereo_kitti.cc 現在也可以使用了。理論上stereo_euroc.cc應該也能用了，我只是懶得下載他們的dataset罷了，所以沒有親自測試。除非有人報Issues, 不然我應該也不會去管啦。接下來會嘗試完成單目部分的除錯。

（2018/05/05 更新）現在mono_kitti.cc 也可以使用了，至少重複播完全相同的dataset是通過了我的測試的。所以理論上單目相機的程式應該都能正常使用啦。此處debug時其實買下了一個隱患，但是我先用一個愚蠢的辦法強行避開了。如果有麻煩，以後再說吧。接下來我會把時間放在機器人身上了，演算法本身的事就先放一放。

具體的執行命令大概是：

cd /XXXXXX/ORB_SLAM2

./Examples/Stereo/stereo_zed Vocabulary/ORBvoc.txt Examples/Stereo/zed0000012643.yaml /XXXXX/ROSbag/XXXXX

反正是這麼個格式，具體的大家可以自己改。我後面把bug除錯完了再想辦法寫個好用的接口出來。

給一個我除錯成功的KITTI datasaet的雙目命令作為樣例吧，我自己的程式碼，Ubuntu 16.04的系統，按照原作者的要求安裝的Library, 是可以跑通的：

cd ~/XXXX/ORB_SLAM2

./Examples/Stereo/stereo_kitti Vocabulary/ORBvoc.txt Examples/Stereo/KITTI04-12.yaml /XXXX/KITTI/dataset/sequences/08

或者單目：

/Examples/Monocular/mono_kitti Vocabulary/ORBvoc.txt Examples/Monocular/KITTI04-12.yaml /XXXX/KITTI/dataset/08

########################### Debug 過程 ###############################

2018.04.17 update

現階段大致的猜測是，這樣的: ORB SLAM當中的關鍵幀和mappoint都不是完全不變化的。這意味著，有的時候某些特定index的mappoint或者KeyFrame會被刪掉。而我們在儲存的時候，是按照剩下的C++的指標來儲存的，重新載入地圖之後，可能所有的指標都出現了問題。唉。。。感覺並不像一個很容易解決的問題啊。。。加油加油。。。

2018.05.01 update

唉，真所謂求人不如求己。然而求己很耽誤時間啊。真的是不得已硬著頭皮啃這塊骨頭。感覺時間很容易就耽誤了，因為程式碼無論怎麼讀，不自己動手逐行debug是沒有辦法真正記住裡面究竟發生了什麼的。

上次的問題終於被我解決了一部分。載入地圖時，如果地圖很大，用float Distances[N][N]這種格式來儲存二維陣列是會出現記憶體錯誤的，所以我用vector替換了一下。然後，上次之所以讀進來不能定位，是因為現有的網上能找到的程式碼當中，LoadMap時只過載了Mappoints，KeyFrame和Covisibilitygraph的邊。但是並沒有將KeyFrame放到KeyFrameDatabase裡面去。也就是說，當我們選擇載入已有的地圖時，即System::System（）函式裡，在mpMap->Load(strPathMap, mySystemSetting, mpKeyFrameDatabase)的時候是需要把mpKeyFrameDatabase這個指標也傳進去的。所以相關的一套程式碼全都要修改。

真是心累啊。於是我又在Tracking::Track()函式裡增加了一些判斷條件：

if(mState==NOT_INITIALIZED && mpMap->GetMaxKFid() == 0)//這就是完全新建地圖

else if(mState==NOT_INITIALIZED && mpMap->GetMaxKFid() > 0)// 這意味著我們過載了地圖

else // 最後才是正常的跟蹤執行緒

大概是下面這個思維導圖裡的意思（整個程式確實太龐大，我還沒寫明白，暫時先不公開這張思維導圖的全圖啦）：

然後我又添加了StereoInitializationWithMap()函式，用以區分StereoInitialization()函式。MonocularInitializationWithMap()我暫時還沒寫，先留白，後面把雙目除錯明白了再回來寫這個單目的。

另外，由於我不小心稍稍修改了一下savemap的二進位制儲存順序，所以可能只能讀取我這個版本自己存的二進位制檔案，不能用之前幾位大神的二進位制檔案啦！

可以看到：地圖已經能過載啦！這是一個我用5000多張圖片製作的大地圖。

如下圖所示KeyFrame 有1847幀，Mappoints有71900個

手動輸入幾張新的圖片之後：

可以看到，左邊紅色的點和綠色的當前幀已經成功定位啦！

可惜的是，如下圖命令列所示，多輸入幾張還是會有記憶體問題，可能是新的閉環被檢測出來，哪裡又出了錯誤吧：

程式碼仍然是同步到了我的github裡面，當前版本畢竟還不完善，我還在debug，所以有些cout就沒有抹掉，導致程式看起來還特別亂，等我後面除錯好了再回來刪吧。

現階段是過載了之後仍然繼續在原來的基礎上建立地圖，等把所有這些調好了之後我再調純定位的部分。

2018.05.02 update

今天又耐著性子找了半天，發現是在載入地圖的關鍵幀時，mGrid這個變數沒有求，導致後面ORBmatcher::Fuse需要GetFeatureInArea的時候後記憶體出錯。所以我在KeyFrame::KeyFrame裡面把mGrid這一段給加上了。成果如圖，終於能從左邊的起點流暢執行到上方的綠色幀啦：

當然，我貌似還遇到過addVertex：FATAL的錯誤出現。但貌似並不影響程式的正常工作，我姑且稱之為warning,先不去管它，至少程式還沒停。等後面找到原因了我再回來改吧。

2018.05.03 update

修復了一些載入地圖之後的bug，比如前幾幀質量都不好的情況下，我們需要重新定位，不能直接走入正常模式。更新完成後，我們會忽略掉最開始質量不高的幾幀，從特徵點足夠多的那一幀開始。

2018.05.05 update

在我debug的時候其實有一件很奇怪的事情發生。過載地圖後，程式運行了幾秒鐘就報Segmentation fault,仔細檢查發現是KeyFrame::SetBadFlag()函式裡面，正當中的這一行

(vpConnected[i]->mnId == (*spcit)->mnId)  

會出現錯誤。

原因是vpConnected[i]會等於0。

很奇怪，一KF的指標，怎麼會指向0而不指向一個地址呢？深挖一下，原因來自KeyFrame::AddConnection函式和KeyFrame::UpdateBestCovisibles()函式，他們裡面的mConnectedKeyFrameWeights可能本身就不對。也就是說，可能KeyFrame::UpdateConnections()這裡就已經出了錯。

向來應該是一件很難說清楚原因的事，也許改動的地方實在是太多，所以我就乾脆在KeyFrame::SetBadFlag()裡面使了點小伎倆，加了個if ... continue強行忽略了這個錯誤。唉，也許是立了個flag?畢竟這是我認為最小幅度的改動了，應該不會影響到其他功能。