ORB-SLAM2地图保存、加载和用于定位
ORB-SLAM2在跟踪建图过程中得到的是稀疏地图，目前看来只能用于定位，源代码里没有地图保存和加载的模块，在此参考网上的多篇博客实现ORB-SLAM2稀疏点云地图的保存、加载以及用于定位（注意只能用于定位不能用于导航）。网上关于地图保存和加载的代码有很多，根源上好像都是这一篇：https://www.cnblogs.com/mafuqiang/p/6972841.html我主要参考的是这...
·
ORB-SLAM2在跟踪建图过程中得到的是稀疏地图，目前看来只能用于定位，源代码里没有地图保存和加载的模块，在此参考网上的多篇博客实现ORB-SLAM2稀疏点云地图的保存、加载以及用于定位（注意只能用于定位不能用于导航）。
网上关于地图保存和加载的代码有很多，根源上好像都是这一篇：https://www.cnblogs.com/mafuqiang/p/6972841.html
我主要参考的是这一篇：https://blog.csdn.net/u014709760/article/details/86319090
在上面这篇博客的基础上，我实现了地图保存、加载并用于定位，但这篇不介绍任何原理，只是填补一些操作细节。（尽管原博已经讲得够细了）
目录
1.2 Converter.h和Conerter.cc的修改
2.4 SystemSetting.h和SystemSetting.cc的创建
2.5 InitKeyFrame.h和InitKeyFrame.cc的创建
1.地图保存
1.1 修改Map.h和Map.cc
Map.h的修改：
添加如下头文件、成员变量、函数：
#include "Converter.h"
public:
//保存地图信息函数
void Save(const string &filename);
//获取地图点ID
void GetMapPointsIdx();
//添加成员变量
std::map<MapPoint*,unsigned long int> mmpnMapPointsIdx;
protected:
//保存地图点和关键帧
void SaveMapPoint(ofstream &f,MapPoint* mp);
void SaveKeyFrame(ofstream &f,KeyFrame* kf);
Map.cc的修改：
添加如下成员函数：
//保存地图信息
void Map::Save ( const string& filename )
{
//Print the information of the saving map
cerr<<"Map.cc :: Map Saving to "<<filename <<endl;
ofstream f;
f.open(filename.c_str(), ios_base::out|ios::binary);
//Number of MapPoints
unsigned long int nMapPoints = mspMapPoints.size();
f.write((char*)&nMapPoints, sizeof(nMapPoints) );
//Save MapPoint sequentially
for ( auto mp: mspMapPoints ){
//Save MapPoint
SaveMapPoint( f, mp );
// cerr << "Map.cc :: Saving map point number: " << mp->mnId << endl;
}
//Print The number of MapPoints
cerr << "Map.cc :: The number of MapPoints is :"<<mspMapPoints.size()<<endl;
//Grab the index of each MapPoint, count from 0, in which we initialized mmpnMapPointsIdx
GetMapPointsIdx();
//Print the number of KeyFrames
cerr <<"Map.cc :: The number of KeyFrames:"<<mspKeyFrames.size()<<endl;
//Number of KeyFrames
unsigned long int nKeyFrames = mspKeyFrames.size();
f.write((char*)&nKeyFrames, sizeof(nKeyFrames));
//Save KeyFrames sequentially
for ( auto kf: mspKeyFrames )
SaveKeyFrame( f, kf );
for (auto kf:mspKeyFrames )
{
//Get parent of current KeyFrame and save the ID of this parent
KeyFrame* parent = kf->GetParent();
unsigned long int parent_id = ULONG_MAX;
if ( parent )
parent_id = parent->mnId;
f.write((char*)&parent_id, sizeof(parent_id));
//Get the size of the Connected KeyFrames of the current KeyFrames
//and then save the ID and weight of the Connected KeyFrames
unsigned long int nb_con = kf->GetConnectedKeyFrames().size();
f.write((char*)&nb_con, sizeof(nb_con));
for ( auto ckf: kf->GetConnectedKeyFrames())
{
int weight = kf->GetWeight(ckf);
f.write((char*)&ckf->mnId, sizeof(ckf->mnId));
f.write((char*)&weight, sizeof(weight));
}
}
// Save last Frame ID
// SaveFrameID(f);
f.close();
cerr<<"Map.cc :: Map Saving Finished!"<<endl;
}
//保存地图点
void Map::SaveMapPoint( ofstream& f, MapPoint* mp)
{
//Save ID and the x,y,z coordinates of the current MapPoint
f.write((char*)&mp->mnId, sizeof(mp->mnId));
cv::Mat mpWorldPos = mp->GetWorldPos();
f.write((char*)& mpWorldPos.at<float>(0),sizeof(float));
f.write((char*)& mpWorldPos.at<float>(1),sizeof(float));
f.write((char*)& mpWorldPos.at<float>(2),sizeof(float));
}
//保存关键帧
void Map::SaveKeyFrame( ofstream &f, KeyFrame* kf )
{
//Save the ID and timesteps of current KeyFrame
f.write((char*)&kf->mnId, sizeof(kf->mnId));
// cout << "saving kf->mnId = " << kf->mnId <<endl;
f.write((char*)&kf->mTimeStamp, sizeof(kf->mTimeStamp));
//Save the Pose Matrix of current KeyFrame
cv::Mat Tcw = kf->GetPose();
Save the rotation matrix
// for ( int i = 0; i < Tcw.rows; i ++ )
// {
// for ( int j = 0; j < Tcw.cols; j ++ )
// {
// f.write((char*)&Tcw.at<float>(i,j), sizeof(float));
// //cerr<<"Tcw.at<float>("<<i<<","<<j<<"):"<<Tcw.at<float>(i,j)<<endl;
// }
// }
//Save the rotation matrix in Quaternion
std::vector<float> Quat = Converter::toQuaternion(Tcw);
for ( int i = 0; i < 4; i ++ )
f.write((char*)&Quat[i],sizeof(float));
//Save the translation matrix
for ( int i = 0; i < 3; i ++ )
f.write((char*)&Tcw.at<float>(i,3),sizeof(float));
//Save the size of the ORB features current KeyFrame
//cerr<<"kf->N:"<<kf->N<<endl;
f.write((char*)&kf->N, sizeof(kf->N));
//Save each ORB features
for( int i = 0; i < kf->N; i ++ )
{
cv::KeyPoint kp = kf->mvKeys[i];
f.write((char*)&kp.pt.x, sizeof(kp.pt.x));
f.write((char*)&kp.pt.y, sizeof(kp.pt.y));
f.write((char*)&kp.size, sizeof(kp.size));
f.write((char*)&kp.angle,sizeof(kp.angle));
f.write((char*)&kp.response, sizeof(kp.response));
f.write((char*)&kp.octave, sizeof(kp.octave));
//Save the Descriptors of current ORB features
f.write((char*)&kf->mDescriptors.cols, sizeof(kf->mDescriptors.cols)); //kf->mDescriptors.cols is always 32 here.
for (int j = 0; j < kf->mDescriptors.cols; j ++ )
f.write((char*)&kf->mDescriptors.at<unsigned char>(i,j), sizeof(char));
//Save the index of MapPoints that corresponds to current ORB features
unsigned long int mnIdx;
MapPoint* mp = kf->GetMapPoint(i);
if (mp == NULL )
mnIdx = ULONG_MAX;
else
mnIdx = mmpnMapPointsIdx[mp];
f.write((char*)&mnIdx, sizeof(mnIdx));
}
// Save BoW for relocalization.
// f.write((char*)&kf->mBowVec, sizeof(kf->mBowVec));
}
//获取地图点ID
void Map::GetMapPointsIdx()
{
unique_lock<mutex> lock(mMutexMap);
unsigned long int i = 0;
for ( auto mp: mspMapPoints )
{
mmpnMapPointsIdx[mp] = i;
i += 1;
}
}
1.2 Converter.h和Conerter.cc的修改
Converter.h的修改：
添加函数原型：
public:
static cv::Mat Converter::toCvMat(const std::vector<float>& v);Converter.cc的修改：
添加函数定义：
cv::Mat toCvMat(const std::vector<float>& v)
{
Eigen::Quaterniond q;
q.x() = v[0];
q.y() = v[1];
q.z() = v[2];
q.w() = v[3];
Eigen::Matrix<double,3,3> eigMat(q);
cv::Mat M = toCvMat(eigMat);
return M;
}
1.3 System.c和System.h的修改
System.h的修改：
添加函数原型：
public:
void SaveMap(const string &filename); System.cc的修改：
//地图保存
void System::SaveMap(const string &filename)
{
mpMap->Save(filename);
}
1.4 入口程序处的修改：
在此以mono_tum.cc为例，调用SaveMap函数的位置可以在SLAM.Shutdown之前，也可在之后，其中参数是保存地图路径，需要根据自己的路径修改。
SLAM.SaveMap("/home/greenhand/ORB_SLAM2-Mapsave/Examples/Monocular/map.bin");
SLAM.Shutdown();1.5 编译并测试
编译步骤与普通ORB-SLAM2的编译一样，执行./build.sh就可以了，之后按一般执行ORB-SLAM2的指令来执行即可，可看到如下结果：
然后发现路径里有一个map.bin文件
2.地图加载
先说明一点，这个地图加载步骤仅仅就是可视化一下而已，如果需要进行定位需要在地图加载的基础上进行微小的修改，读者如果想用于定位可以直接结合第三部分一起看。
2.1 Map.cc和Map.h的修改
Map.h的修改：
添加头文件、地图信息加载函数、地图点加载函数、关键帧加载函数的原型：
#include "SystemSetting.h"
//加载地图信息
public：
void Load(const string &filename,SystemSetting* mySystemSetting);
MapPoint* LoadMapPoint(ifstream &f);
KeyFrame* LoadKeyFrame(ifstream &f,SystemSetting* mySystemSetting);
Map.cc的修改：
添加如下函数
//地图加载函数
void Map::Load ( const string &filename, SystemSetting* mySystemSetting)
{
cerr << "Map.cc :: Map reading from:"<<filename<<endl;
ifstream f;
f.open( filename.c_str() );
// Same as the sequence that we save the file, we first read the number of MapPoints.
unsigned long int nMapPoints;
f.read((char*)&nMapPoints, sizeof(nMapPoints));
// Then read MapPoints one after another, and add them into the map
cerr<<"Map.cc :: The number of MapPoints:"<<nMapPoints<<endl;
for ( unsigned int i = 0; i < nMapPoints; i ++ )
{
MapPoint* mp = LoadMapPoint(f);
AddMapPoint(mp);
}
// Get all MapPoints
std::vector<MapPoint*> vmp = GetAllMapPoints();
// Read the number of KeyFrames
unsigned long int nKeyFrames;
f.read((char*)&nKeyFrames, sizeof(nKeyFrames));
cerr<<"Map.cc :: The number of KeyFrames:"<<nKeyFrames<<endl;
// Then read KeyFrames one after another, and add them into the map
vector<KeyFrame*>kf_by_order;
for( unsigned int i = 0; i < nKeyFrames; i ++ )
{
KeyFrame* kf = LoadKeyFrame(f, mySystemSetting);
AddKeyFrame(kf);
kf_by_order.push_back(kf);
}
cerr<<"Map.cc :: Max KeyFrame ID is: " << mnMaxKFid << ", and I set mnId to this number" <<endl;
cerr<<"Map.cc :: KeyFrame Load OVER!"<<endl;
// Read Spanning Tree(open loop trajectory)
map<unsigned long int, KeyFrame*> kf_by_id;
for ( auto kf: mspKeyFrames )
kf_by_id[kf->mnId] = kf;
cerr<<"Map.cc :: Start Load The Parent!"<<endl;
for( auto kf: kf_by_order )
{
// Read parent_id of current KeyFrame.
unsigned long int parent_id;
f.read((char*)&parent_id, sizeof(parent_id));
// Add parent KeyFrame to current KeyFrame.
// cout<<"Map::Load : Add parent KeyFrame to current KeyFrame"<<endl;
if ( parent_id != ULONG_MAX )
kf->ChangeParent(kf_by_id[parent_id]);
// Read covisibility graphs.
// Read the number of Connected KeyFrames of current KeyFrame.
unsigned long int nb_con;
f.read((char*)&nb_con, sizeof(nb_con));
// Read id and weight of Connected KeyFrames of current KeyFrame,
// and add Connected KeyFrames into covisibility graph.
// cout<<"Map::Load : Read id and weight of Connected KeyFrames"<<endl;
for ( unsigned long int i = 0; i < nb_con; i ++ )
{
unsigned long int id;
int weight;
f.read((char*)&id, sizeof(id));
f.read((char*)&weight, sizeof(weight));
kf->AddConnection(kf_by_id[id],weight);
}
}
cerr<<"Map.cc :: Parent Load OVER!"<<endl;
for ( auto mp: vmp )
{
// cout << "Now mp = "<< mp << endl;
if(mp)
{
// cout << "compute for mp = "<< mp << endl;
mp->ComputeDistinctiveDescriptors();
// cout << "Computed Distinctive Descriptors." << endl;
mp->UpdateNormalAndDepth();
// cout << "Updated Normal And Depth." << endl;
}
}
f.close();
cerr<<"Map.cc :: Load IS OVER!"<<endl;
return;
}
//地图点加载函数
MapPoint* Map::LoadMapPoint( ifstream &f )
{
// Position and Orientation of the MapPoints.
cv::Mat Position(3,1,CV_32F);
long unsigned int id;
f.read((char*)&id, sizeof(id));
f.read((char*)&Position.at<float>(0), sizeof(float));
f.read((char*)&Position.at<float>(1), sizeof(float));
f.read((char*)&Position.at<float>(2), sizeof(float));
// Initialize a MapPoint, and set its id and Position.
MapPoint* mp = new MapPoint(Position, this );
mp->mnId = id;
mp->SetWorldPos( Position );
return mp;
}
//关键帧加载函数
KeyFrame* Map::LoadKeyFrame( ifstream &f, SystemSetting* mySystemSetting )
{
InitKeyFrame initkf(*mySystemSetting);
// Read ID and TimeStamp of each KeyFrame.
f.read((char*)&initkf.nId, sizeof(initkf.nId));
f.read((char*)&initkf.TimeStamp, sizeof(double));
// Read position and quaternion
cv::Mat T = cv::Mat::zeros(4,4,CV_32F);
std::vector<float> Quat(4);
//Quat.reserve(4);
for ( int i = 0; i < 4; i ++ )
f.read((char*)&Quat[i],sizeof(float));
cv::Mat R = Converter::toCvMat(Quat);
for ( int i = 0; i < 3; i ++ )
f.read((char*)&T.at<float>(i,3),sizeof(float));
for ( int i = 0; i < 3; i ++ )
for ( int j = 0; j < 3; j ++ )
T.at<float>(i,j) = R.at<float>(i,j);
T.at<float>(3,3) = 1;
// Read feature point number of current Key Frame
f.read((char*)&initkf.N, sizeof(initkf.N));
initkf.vKps.reserve(initkf.N);
initkf.Descriptors.create(initkf.N, 32, CV_8UC1);
vector<float>KeypointDepth;
std::vector<MapPoint*> vpMapPoints;
vpMapPoints = vector<MapPoint*>(initkf.N,static_cast<MapPoint*>(NULL));
// Read Keypoints and descriptors of current KeyFrame
std::vector<MapPoint*> vmp = GetAllMapPoints();
for(int i = 0; i < initkf.N; i ++ )
{
cv::KeyPoint kp;
f.read((char*)&kp.pt.x, sizeof(kp.pt.x));
f.read((char*)&kp.pt.y, sizeof(kp.pt.y));
f.read((char*)&kp.size, sizeof(kp.size));
f.read((char*)&kp.angle,sizeof(kp.angle));
f.read((char*)&kp.response, sizeof(kp.response));
f.read((char*)&kp.octave, sizeof(kp.octave));
initkf.vKps.push_back(kp);
// Read descriptors of keypoints
f.read((char*)&initkf.Descriptors.cols, sizeof(initkf.Descriptors.cols));
// for ( int j = 0; j < 32; j ++ ) // Since initkf.Descriptors.cols is always 32, for loop may also write like this.
for ( int j = 0; j < initkf.Descriptors.cols; j ++ )
f.read((char*)&initkf.Descriptors.at<unsigned char>(i,j),sizeof(char));
// Read the mapping from keypoints to MapPoints.
unsigned long int mpidx;
f.read((char*)&mpidx, sizeof(mpidx));
// Look up from vmp, which contains all MapPoints, MapPoint of current KeyFrame, and then insert in vpMapPoints.
if( mpidx == ULONG_MAX )
vpMapPoints[i] = NULL;
else
vpMapPoints[i] = vmp[mpidx];
}
initkf.vRight = vector<float>(initkf.N,-1);
initkf.vDepth = vector<float>(initkf.N,-1);
//initkf.vDepth = KeypointDepth;
initkf.UndistortKeyPoints();
initkf.AssignFeaturesToGrid();
// Use initkf to initialize a KeyFrame and set parameters
KeyFrame* kf = new KeyFrame( initkf, this, NULL, vpMapPoints );
kf->mnId = initkf.nId;
kf->SetPose(T);
kf->ComputeBoW();
for ( int i = 0; i < initkf.N; i ++ )
{
if ( vpMapPoints[i] )
{
vpMapPoints[i]->AddObservation(kf,i);
if( !vpMapPoints[i]->GetReferenceKeyFrame())
vpMapPoints[i]->SetReferenceKeyFrame(kf);
}
}
return kf;
}
2.2 MapPoint.cc和MapPoint.h的修改
MapPoint.h的修改：
添加构造函数、成员函数的原型：
public：
MapPoint(const cv::Mat &Pos,Map* pMap);
KeyFrame* SetReferenceKeyFrame(KeyFrame* RFKF);
MapPoint.cc的修改：
添加以上函数的定义：
MapPoint::MapPoint(const cv::Mat &Pos,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)
{
Pos.copyTo(mWorldPos);
mNormalVector = cv::Mat::zeros(3,1,CV_32F);
unique_lock<mutex> lock(mpMap->mMutexPointCreation);
mnId = nNextId++;
}
KeyFrame* MapPoint::SetReferenceKeyFrame(KeyFrame* RFKF)
{
return mpRefKF = RFKF;
}2.3 KeyFrame.h和KeyFrame.cc的修改
KeyFrame.h的修改：
添加头文件、构造函数原型、类声明：
#include "InitKeyFrame.h"
KeyFrame(InitKeyFrame &initkf,Map* pMap,KeyFrameDatabase* pKFDB,vector<MapPoint*>& vpMapPoints);
class InitKeyFrame;KeyFrame.cc的修改：
添加构造函数定义：
KeyFrame::KeyFrame(InitKeyFrame &initkf, Map *pMap, KeyFrameDatabase *pKFDB, vector<MapPoint*> &vpMapPoints):
mnFrameId(0), mTimeStamp(initkf.TimeStamp), mnGridCols(FRAME_GRID_COLS), mnGridRows(FRAME_GRID_ROWS),
mfGridElementWidthInv(initkf.fGridElementWidthInv), mfGridElementHeightInv(initkf.fGridElementHeightInv),
mnTrackReferenceForFrame(0), mnFuseTargetForKF(0), mnBALocalForKF(0), mnBAFixedForKF(0),
mnLoopQuery(0), mnLoopWords(0), mnRelocQuery(0), mnRelocWords(0), mnBAGlobalForKF(0),
fx(initkf.fx), fy(initkf.fy), cx(initkf.cx), cy(initkf.cy), invfx(initkf.invfx),
invfy(initkf.invfy), mbf(initkf.bf), mb(initkf.b), mThDepth(initkf.ThDepth), N(initkf.N),
mvKeys(initkf.vKps), mvKeysUn(initkf.vKpsUn), mvuRight(initkf.vRight), mvDepth(initkf.vDepth),
mDescriptors(initkf.Descriptors.clone()), mBowVec(initkf.BowVec), mFeatVec(initkf.FeatVec),
mnScaleLevels(initkf.nScaleLevels), mfScaleFactor(initkf.fScaleFactor), mfLogScaleFactor(initkf.fLogScaleFactor),
mvScaleFactors(initkf.vScaleFactors), mvLevelSigma2(initkf.vLevelSigma2),mvInvLevelSigma2(initkf.vInvLevelSigma2),
mnMinX(initkf.nMinX), mnMinY(initkf.nMinY), mnMaxX(initkf.nMaxX), mnMaxY(initkf.nMaxY), mK(initkf.K),
mvpMapPoints(vpMapPoints), mpKeyFrameDB(pKFDB), mpORBvocabulary(initkf.pVocabulary),
mbFirstConnection(true), mpParent(NULL), mbNotErase(false), mbToBeErased(false), mbBad(false),
mHalfBaseline(initkf.b/2), mpMap(pMap)
{
mnId = nNextId ++;
}
2.4 SystemSetting.h和SystemSetting.cc的创建
SystemSetting.h的创建：
#define SYSTEMSETTING_H
#include<string>
#include"ORBVocabulary.h"
#include<opencv2/opencv.hpp>
namespace ORB_SLAM2 {
class SystemSetting{
public:
SystemSetting(ORBVocabulary* pVoc);
bool LoadSystemSetting(const std::string strSettingPath);
public:
ORBVocabulary* pVocavulary;
//相机参数
float width;
float height;
float fx;
float fy;
float cx;
float cy;
float invfx;
float invfy;
float bf;
float b;
float fps;
cv::Mat K;
cv::Mat DistCoef;
bool initialized;
//相机 RGB 参数
int nRGB;
//ORB特征参数
int nFeatures;
float fScaleFactor;
int nLevels;
float fIniThFAST;
float fMinThFAST;
//其他参数
float ThDepth = -1;
float DepthMapFactor = -1;
};
}//namespace ORB_SLAM2
#endif //SystemSetting
SystemSetting.cc的创建：
#include<iostream>
#include"SystemSetting.h"
using namespace std;
namespace ORB_SLAM2 {
SystemSetting::SystemSetting(ORBVocabulary* pVoc):pVocavulary(pVoc)
{
}
bool SystemSetting::LoadSystemSetting(const std::string strSettingPath)
{
cout<<endl<<"Loading System Parameters form:"<<strSettingPath<<endl;
cv::FileStorage fSettings(strSettingPath, cv::FileStorage::READ);
width = fSettings["Camera.width"];
height = fSettings["Camera.height"];
fx = fSettings["Camera.fx"];
fy = fSettings["Camera.fy"];
cx = fSettings["Camera.cx"];
cy = fSettings["Camera.cy"];
cv::Mat tmpK = cv::Mat::eye(3,3,CV_32F);
tmpK.at<float>(0,0) = fx;
tmpK.at<float>(1,1) = fy;
tmpK.at<float>(0,2) = cx;
tmpK.at<float>(1,2) = cy;
tmpK.copyTo(K);
cv::Mat tmpDistCoef(4,1,CV_32F);
tmpDistCoef.at<float>(0) = fSettings["Camera.k1"];
tmpDistCoef.at<float>(1) = fSettings["Camera.k2"];
tmpDistCoef.at<float>(2) = fSettings["Camera.p1"];
tmpDistCoef.at<float>(3) = fSettings["Camera.p2"];
const float k3 = fSettings["Camera.k3"];
if( k3!=0 )
{
tmpDistCoef.resize(5);
tmpDistCoef.at<float>(4) = k3;
}
tmpDistCoef.copyTo( DistCoef );
bf = fSettings["Camera.bf"];
fps= fSettings["Camera.fps"];
invfx = 1.0f/fx;
invfy = 1.0f/fy;
b = bf /fx;
initialized = true;
cout<<"- size:"<<width<<"x"<<height<<endl;
cout<<"- fx:" <<fx<<endl;
cout << "- fy: " << fy << endl;
cout << "- cx: " << cx << endl;
cout << "- cy: " << cy << endl;
cout << "- k1: " << DistCoef.at<float>(0) << endl;
cout << "- k2: " << DistCoef.at<float>(1) << endl;
if(DistCoef.rows==5)
cout << "- k3: " << DistCoef.at<float>(4) << endl;
cout << "- p1: " << DistCoef.at<float>(2) << endl;
cout << "- p2: " << DistCoef.at<float>(3) << endl;
cout << "- bf: " << bf << endl;
//Load RGB parameter
nRGB = fSettings["Camera.RGB"];
//Load ORB feature parameters
nFeatures = fSettings["ORBextractor.nFeatures"];
fScaleFactor = fSettings["ORBextractor.scaleFactor"];
nLevels = fSettings["ORBextractor.nLevels"];
fIniThFAST = fSettings["ORBextractor.iniThFAST"];
fMinThFAST = fSettings["ORBextractor.minThFAST"];
cout << endl << "ORB Extractor Parameters: " << endl;
cout << "- Number of Features: " << nFeatures << endl;
cout << "- Scale Levels: " << nLevels << endl;
cout << "- Scale Factor: " << fScaleFactor << endl;
cout << "- Initial Fast Threshold: " << fIniThFAST << endl;
cout << "- Minimum Fast Threshold: " << fMinThFAST << endl;
//Load others parameters, if the sensor is MONOCULAR, the parameters is zero;
//ThDepth = fSettings["ThDepth"];
//DepthMapFactor = fSettings["DepthMapFactor"];
fSettings.release();
return true;
}
}
2.5 InitKeyFrame.h和InitKeyFrame.cc的创建
InitKeyFrame.h的创建：
#ifndef INITKEYFRAME_H
#define INITKEYFRAME_H
#include "Thirdparty/DBoW2/DBoW2/BowVector.h"
#include "Thirdparty/DBoW2/DBoW2/FeatureVector.h"
#include "SystemSetting.h"
#include <opencv2/opencv.hpp>
#include "ORBVocabulary.h"
#include "KeyFrameDatabase.h"
//#include "MapPoints.h"
namespace ORB_SLAM2
{
#define FRAME_GRID_ROWS 48
#define FRAME_GRID_COLS 64
class SystemSetting;
class KeyFrameDatabase;
//class ORBVocabulary;
class InitKeyFrame
{
public:
InitKeyFrame(SystemSetting &SS);
void UndistortKeyPoints();
bool PosInGrid(const cv::KeyPoint& kp, int &posX, int &posY);
void AssignFeaturesToGrid();
public:
ORBVocabulary* pVocabulary;
//KeyFrameDatabase* pKeyFrameDatabase;
long unsigned int nId;
double TimeStamp;
float fGridElementWidthInv;
float fGridElementHeightInv;
std::vector<std::size_t> vGrid[FRAME_GRID_COLS][FRAME_GRID_ROWS];
float fx;
float fy;
float cx;
float cy;
float invfx;
float invfy;
float bf;
float b;
float ThDepth;
int N;
std::vector<cv::KeyPoint> vKps;
std::vector<cv::KeyPoint> vKpsUn;
cv::Mat Descriptors;
//it's zero for mono
std::vector<float> vRight;
std::vector<float> vDepth;
DBoW2::BowVector BowVec;
DBoW2::FeatureVector FeatVec;
int nScaleLevels;
float fScaleFactor;
float fLogScaleFactor;
std::vector<float> vScaleFactors;
std::vector<float> vLevelSigma2;
std::vector<float> vInvLevelSigma2;
std::vector<float> vInvScaleFactors;
int nMinX;
int nMinY;
int nMaxX;
int nMaxY;
cv::Mat K;
cv::Mat DistCoef;
};
} //namespace ORB_SLAM2
#endif //INITKEYFRAME_H
InitKeyFrame.cc的创建：
#include "InitKeyFrame.h"
#include <opencv2/opencv.hpp>
#include "SystemSetting.h"
namespace ORB_SLAM2
{
InitKeyFrame::InitKeyFrame(SystemSetting &SS):pVocabulary(SS.pVocavulary)//, pKeyFrameDatabase(SS.pKeyFrameDatabase)
{
fx = SS.fx;
fy = SS.fy;
cx = SS.cx;
cy = SS.cy;
invfx = SS.invfx;
invfy = SS.invfy;
bf = SS.bf;
b = SS.b;
ThDepth = SS.ThDepth;
nScaleLevels = SS.nLevels;
fScaleFactor = SS.fScaleFactor;
fLogScaleFactor = log(SS.fScaleFactor);
vScaleFactors.resize(nScaleLevels);
vLevelSigma2.resize(nScaleLevels);
vScaleFactors[0] = 1.0f;
vLevelSigma2[0] = 1.0f;
for ( int i = 1; i < nScaleLevels; i ++ )
{
vScaleFactors[i] = vScaleFactors[i-1]*fScaleFactor;
vLevelSigma2[i] = vScaleFactors[i]*vScaleFactors[i];
}
vInvScaleFactors.resize(nScaleLevels);
vInvLevelSigma2.resize(nScaleLevels);
for ( int i = 0; i < nScaleLevels; i ++ )
{
vInvScaleFactors[i] = 1.0f/vScaleFactors[i];
vInvLevelSigma2[i] = 1.0f/vLevelSigma2[i];
}
K = SS.K;
DistCoef = SS.DistCoef;
if( SS.DistCoef.at<float>(0)!=0.0)
{
cv::Mat mat(4,2,CV_32F);
mat.at<float>(0,0) = 0.0;
mat.at<float>(0,1) = 0.0;
mat.at<float>(1,0) = SS.width;
mat.at<float>(1,1) = 0.0;
mat.at<float>(2,0) = 0.0;
mat.at<float>(2,1) = SS.height;
mat.at<float>(3,0) = SS.width;
mat.at<float>(3,1) = SS.height;
mat = mat.reshape(2);
cv::undistortPoints(mat, mat, SS.K, SS.DistCoef, cv::Mat(), SS.K);
mat = mat.reshape(1);
nMinX = min(mat.at<float>(0,0), mat.at<float>(2,0));
nMaxX = max(mat.at<float>(1,0), mat.at<float>(3,0));
nMinY = min(mat.at<float>(0,1), mat.at<float>(1,1));
nMaxY = max(mat.at<float>(2,1), mat.at<float>(3,1));
}
else
{
nMinX = 0.0f;
nMaxX = SS.width;
nMinY = 0.0f;
nMaxY = SS.height;
}
fGridElementWidthInv=static_cast<float>(FRAME_GRID_COLS)/(nMaxX-nMinX);
fGridElementHeightInv=static_cast<float>(FRAME_GRID_ROWS)/(nMaxY-nMinY);
}
void InitKeyFrame::UndistortKeyPoints()
{
if( DistCoef.at<float>(0) == 0.0)
{
vKpsUn = vKps;
return;
}
cv::Mat mat(N,2,CV_32F);
for ( int i = 0; i < N; i ++ )
{
mat.at<float>(i,0) = vKps[i].pt.x;
mat.at<float>(i,1) = vKps[i].pt.y;
}
mat = mat.reshape(2);
cv::undistortPoints(mat, mat, K, DistCoef, cv::Mat(), K );
mat = mat.reshape(1);
vKpsUn.resize(N);
for( int i = 0; i < N; i ++ )
{
cv::KeyPoint kp = vKps[i];
kp.pt.x = mat.at<float>(i,0);
kp.pt.y = mat.at<float>(i,1);
vKpsUn[i] = kp;
}
}
void InitKeyFrame::AssignFeaturesToGrid()
{
int nReserve = 0.5f*N/(FRAME_GRID_COLS*FRAME_GRID_ROWS);
for ( unsigned int i = 0; i < FRAME_GRID_COLS; i ++ )
{
for ( unsigned int j = 0; j < FRAME_GRID_ROWS; j ++)
vGrid[i][j].reserve(nReserve);
}
for ( int i = 0; i < N; i ++ )
{
const cv::KeyPoint& kp = vKpsUn[i];
int nGridPosX, nGridPosY;
if( PosInGrid(kp, nGridPosX, nGridPosY))
vGrid[nGridPosX][nGridPosY].push_back(i);
}
}
bool InitKeyFrame::PosInGrid(const cv::KeyPoint &kp, int &posX, int &posY)
{
posX = round((kp.pt.x-nMinX)*fGridElementWidthInv);
posY = round((kp.pt.y-nMinY)*fGridElementHeightInv);
if(posX<0 || posX>=FRAME_GRID_COLS ||posY<0 || posY>=FRAME_GRID_ROWS)
return false;
return true;
}
}
2.6 System.h和System.cc的修改：
System.h的修改：
添加成员函数原型和成员变量。
public:
//添加成员函数
void LoadMap(const string &filename);
//添加成员变量
std::string mySettingFile;
System.cc的修改：
添加函数定义，并在System的构造函数中加入以下那一句。
void System::LoadMap(const string &filename)
{
SystemSetting* mySystemSetting = new SystemSetting(mpVocabulary);
mySystemSetting->LoadSystemSetting(mySettingFile);
mpMap->Load(filename,mySystemSetting);
}
//在System构造函数里加入这一句
mySettingFile = strSettingsFile;
2.7 入口程序的修改
这一句添加在SLAM对象声明之后，循环读入图片之前，路径为第一部分测试保存的地图路径。
SLAM.LoadMap("/home/greenhand/ORB_SLAM2-Mapsave/Examples/Monocular/map.bin");2.8 CMakeList.txt的修改：
在生成库时添加SystemSetting.CC和InitKeyFrame.cc文件到列表里：
add_library(${PROJECT_NAME} SHARED
src/System.cc
src/Tracking.cc
src/LocalMapping.cc
src/LoopClosing.cc
src/ORBextractor.cc
src/ORBmatcher.cc
src/FrameDrawer.cc
src/Converter.cc
src/MapPoint.cc
src/KeyFrame.cc
src/Map.cc
src/MapDrawer.cc
src/Optimizer.cc
src/PnPsolver.cc
src/Frame.cc
src/KeyFrameDatabase.cc
src/Sim3Solver.cc
src/Initializer.cc
src/Viewer.cc
src/InitKeyFrame.cc
src/SystemSetting.cc
)2.9 编译并测试
像往常一样执行，这一步测试的前提是在上一步测试后，已经保存了一张地图。
我在这想讲一下一些可能出现的问题：在第一部分我们在入口文件里加入了保存地图代码，而这一次我们在入口文件里加入了加载地图代码，这一次测试是装载上一次保存后的地图。
那么系统先装载地图，然后按照正常流程执行，所以这一次测试之后，再测试时就会用加载地图后的地图，可能出现崩溃的情况。读者在这一步可以根据实际调试需要，灵活注释掉mono_tum.cc里的保存和加载语句，也可以自己写一些条件语句使自己可以选择。我个人的做法会在后面一部分讲。
3.地图加载后用于定位，补充一些选择机制
这一部分我和原博做的略有不同。
3.1修改Map.h和Map.cc
Map.h中添加头文件并修改Load函数原型：
#include "KeyFrameDatabase.h"
void Load(const string &filename,SystemSetting* mySystemSetting, KeyFrameDatabase* mpKeyFrameDatabase);Map.cc修改Load函数定义，只需在原函数的一个循环结构末尾添加一句，就可将读取到的保存的关键帧加入KeyFrameDatabase
//函数头修改为和原型一致
void Map::Load ( const string &filename, SystemSetting* mySystemSetting,KeyFrameDatabase* mpKeyFrameDatabase)
//找到下面这个循环结构，添加一句
for( unsigned int i = 0; i < nKeyFrames; i ++ )
{
KeyFrame* kf = LoadKeyFrame(f, mySystemSetting);
AddKeyFrame(kf);
kf_by_order.push_back(kf);
//将关键帧添加到关键帧数据库中
mpKeyFrameDatabase->add(kf);
}
3.2修改System.cc
将LoadMap函数修改为如下形式：
void System::LoadMap(const string &filename)
{
//设置定位模式
char IsPureLocalization;
cout << "是否开启纯定位模式？(y/n)"<<endl;
cin >> IsPureLocalization;
if(IsPureLocalization == 'Y' || IsPureLocalization == 'y')
ActivateLocalizationMode();
//导入地图
string strPathMap = filename;
mpMap->Load(strPathMap,mySystemSetting,mpKeyFrameDatabase);
SystemSetting* mySystemSetting = new SystemSetting(mpVocabulary);
mySystemSetting->LoadSystemSetting(mySettingFile);
}将cv::Mat System::TrackMonocular(const cv::Mat &im, const double ×tamp)这个函数中的控制仅定位模式总是置False的语句注释掉（关于注释哪个函数根据自己的需求来，我是跑单目就注释掉单目里的）
mpTracker->InformOnlyTracking(true);
//mbActivateLocalizationMode = false;把这句像这样注释掉3.3修改Mono_tum.cc做一个选择的机制
其实我就是加了两个条件语句来让用户自己选择是否要保存地图、是否加载地图——>是否选择纯定位模式。这个代码也没什么好贴的，大家自己用个字符输入来做条件吧！
4.问题
说一下我遇到的问题。
1.有时会出现定位失败后一直卡在重启系统“SystemReseting"这一步骤，但有时又会很顺滑地Resetting成功。
2.在加载地图时卡在Start Load the parent这一步，有时又不会。
起初我以为是我与原博在定位方面做法不同导致的，但两种做法或多或少都会出现一些系统卡死问题，我猜测时因为虚拟机配置太低的问题。目前我也处于初学阶段，所以并没有什么解决办法，希望如果有人在实践并解决了类似的问题以后可以告诉我。
瓜分20万奖金 获得内推名额 丰厚实物奖励 易参与易上手
更多推荐
9
0- 0
已为社区贡献1条内容
所有评论(0)