本文记录了ORB_SLAM2中地图保存与加载的过程。
参考博客:
https://www.cnblogs.com/mafuqiang/p/6972841.html
https://www.cnblogs.com/mafuqiang/p/7002568.html
1.1前面说了ORB-SLAM地图的保存部分,经过测试可以使用。本文我们继续说地图如何加载,因为加载部分相比保存要稍微复杂一些,所以要多说一点。
这个代码可以实现地图加载,加载之后的重定位工作正在摸索中。
1.2修改源码实现地图加载
地图加载是地图保存的逆过程,实际操作要相对麻烦一点。地图加载部分需要修改的较多,所以按所需修改的文件来进行说明。
修改代码:Map.h Map.cc MapPoint.h MapPoint.cc KeyFrame.h KeyFrame.cc System.h System.cc CMakeLists.txt
新建代码:SystemSetting.h SystemSetting.cc InitKeyFrame.h InitKeyFrame.cc
第一步:修改 Map.h Map.cc
修改Map.h
Map.h/class Map/public:
//加载地图信息
void Load(const string &filename,SystemSetting* mySystemSetting);//wuhoup20190924
MapPoint* LoadMapPoint(ifstream &f);//wuhoup20190924
KeyFrame* LoadKeyFrame(ifstream &f,SystemSetting* mySystemSetting);//wuhoup20190924
修改Map.cc
地图加载函数
//地图加载函数
void Map::Load ( const string &filename, SystemSetting* mySystemSetting)//wuhoup20190924
{
cerr << "Map.cc :: Map reading from:"< vmp = GetAllMapPoints();
// Read the number of KeyFrames
unsigned long int nKeyFrames;
f.read((char*)&nKeyFrames, sizeof(nKeyFrames));
cerr<<"Map.cc :: The number of KeyFrames:"<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" < kf_by_id;
for ( auto kf: mspKeyFrames )
kf_by_id[kf->mnId] = kf;
cerr<<"Map.cc :: Start Load The Parent!"<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"<AddConnection(kf_by_id[id],weight);
}
}
cerr<<"Map.cc :: Parent Load OVER!"<UpdateNormalAndDepth();
// cout << "Updated Normal And Depth." << endl;
}
}
f.close();
cerr<<"Map.cc :: Load IS OVER!"<
地图点加载函数
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(0), sizeof(float));
f.read((char*)&Position.at(1), sizeof(float));
f.read((char*)&Position.at(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 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(i,3),sizeof(float));
for ( int i = 0; i < 3; i ++ )
for ( int j = 0; j < 3; j ++ )
T.at(i,j) = R.at(i,j);
T.at(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);
vectorKeypointDepth;
std::vector vpMapPoints;
vpMapPoints = vector(initkf.N,static_cast(NULL));
// Read Keypoints and descriptors of current KeyFrame
std::vector 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(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(initkf.N,-1);
initkf.vDepth = vector(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;
}
第二步:修改MapPoint文件
MapPoint.h
MapPoint/public:
//重新定义一种MapPoint类的构造函数
MapPoint(const cv::Mat &Pos,Map* pMap);//wuhoup20190924
KeyFrame* SetReferenceKeyFrame(KeyFrame* RFKF);
MapPoint.cc
//由于在加载地图时我们只有Position以及当前的Map,所以需要重新定义一种MapPoint类的构造函数以满足要求。
MapPoint::MapPoint(const cv::Mat &Pos,Map* pMap)://wuhoup20190924
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(NULL)), mnVisible(1), mnFound(1), mbBad(false),
mpReplaced(static_cast(NULL)), mfMinDistance(0), mfMaxDistance(0), mpMap(pMap)
{
Pos.copyTo(mWorldPos);
mNormalVector = cv::Mat::zeros(3,1,CV_32F);
unique_lock lock(mpMap->mMutexPointCreation);
mnId = nNextId++;
}
KeyFrame* MapPoint::SetReferenceKeyFrame(KeyFrame* RFKF)//wuhoup20190924
{
return mpRefKF = RFKF;
}
第三步:修改KeyFrame相关文件
KeyFrame.h文件中添加如下构造函数
头文件:
#include "InitKeyFrame.h"
namespace ORB_SLAM2目录下
class InitKeyFrame;
class KeyFrame/public目录下
//构造函数
//从文件中读取的内容同保存的一致,同时由于是通过InitKeyFrame初始化的关键帧类KeyFrame,
//因此这里同样需要添加构造函数以及初始化方式:
KeyFrame(InitKeyFrame &initkf,Map* pMap,KeyFrameDatabase* pKFDB,vector& vpMapPoints);
在KeyFrame.cc文件中实现该构造函数:
//从文件中读取的内容同保存的一致,同时由于是通过InitKeyFrame初始化的关键帧类KeyFrame,
//因此这里同样需要添加构造函数以及初始化方式:
//加载了一个关键帧之后还需要计算其BoW向量等操作,同时指定关键帧对地图点的观测。
KeyFrame::KeyFrame(InitKeyFrame &initkf, Map *pMap, KeyFrameDatabase *pKFDB, vector &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 ++;
}
第四步:新建SystemSetting和InitKeyFrame相关文件
在上面的函数中我们用到了SystemSetting类和InitKeyFrame类。其中SystemSetting类用于读取参数文件中的相关参数(参数配置文件.yaml),InitKeyFrame类用于进行关键帧初始化。其实现过程如下:
SystemSetting.h
#ifndef SYSTEMSETTING_H
#define SYSTEMSETTING_H
#include
#include"ORBVocabulary.h"
#include
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
#include"SystemSetting.h"
using namespace std;
namespace ORB_SLAM2 {
SystemSetting::SystemSetting(ORBVocabulary* pVoc):pVocavulary(pVoc)
{
}
bool SystemSetting::LoadSystemSetting(const std::string strSettingPath)
{
cout<(0,0) = fx;
tmpK.at(1,1) = fy;
tmpK.at(0,2) = cx;
tmpK.at(1,2) = cy;
tmpK.copyTo(K);
cv::Mat tmpDistCoef(4,1,CV_32F);
tmpDistCoef.at(0) = fSettings["Camera.k1"];
tmpDistCoef.at(1) = fSettings["Camera.k2"];
tmpDistCoef.at(2) = fSettings["Camera.p1"];
tmpDistCoef.at(3) = fSettings["Camera.p2"];
const float k3 = fSettings["Camera.k3"];
if( k3!=0 )
{
tmpDistCoef.resize(5);
tmpDistCoef.at(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:"<(0) << endl;
cout << "- k2: " << DistCoef.at(1) << endl;
if(DistCoef.rows==5)
cout << "- k3: " << DistCoef.at(4) << endl;
cout << "- p1: " << DistCoef.at(2) << endl;
cout << "- p2: " << DistCoef.at(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;
}
}
InitKeyFrame.h
#ifndef INITKEYFRAME_H
#define INITKEYFRAME_H
#include "Thirdparty/DBoW2/DBoW2/BowVector.h"
#include "Thirdparty/DBoW2/DBoW2/FeatureVector.h"
#include "SystemSetting.h"
#include
#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 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 vKps;
std::vector vKpsUn;
cv::Mat Descriptors;
//it's zero for mono
std::vector vRight;
std::vector vDepth;
DBoW2::BowVector BowVec;
DBoW2::FeatureVector FeatVec;
int nScaleLevels;
float fScaleFactor;
float fLogScaleFactor;
std::vector vScaleFactors;
std::vector vLevelSigma2;
std::vector vInvLevelSigma2;
std::vector 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
#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(0)!=0.0)
{
cv::Mat mat(4,2,CV_32F);
mat.at(0,0) = 0.0;
mat.at(0,1) = 0.0;
mat.at(1,0) = SS.width;
mat.at(1,1) = 0.0;
mat.at(2,0) = 0.0;
mat.at(2,1) = SS.height;
mat.at(3,0) = SS.width;
mat.at(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(0,0), mat.at(2,0));
nMaxX = max(mat.at(1,0), mat.at(3,0));
nMinY = min(mat.at(0,1), mat.at(1,1));
nMaxY = max(mat.at(2,1), mat.at(3,1));
}
else
{
nMinX = 0.0f;
nMaxX = SS.width;
nMinY = 0.0f;
nMaxY = SS.height;
}
fGridElementWidthInv=static_cast(FRAME_GRID_COLS)/(nMaxX-nMinX);
fGridElementHeightInv=static_cast(FRAME_GRID_ROWS)/(nMaxY-nMinY);
}
void InitKeyFrame::UndistortKeyPoints()
{
if( DistCoef.at(0) == 0.0)
{
vKpsUn = vKps;
return;
}
cv::Mat mat(N,2,CV_32F);
for ( int i = 0; i < N; i ++ )
{
mat.at(i,0) = vKps[i].pt.x;
mat.at(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(i,0);
kp.pt.y = mat.at(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;
}
}
第五步:修改System相关文件
System.h
system/public:
//声明LoadMap
void LoadMap(const string &filename, const string &strSettingsFile);
std::string mySettingFile;
//std::string strSettingsFile;
这里需要注意,加载地图时需要响函数传递相机参数配置文件.yaml,需要给出.yaml的路径。
System.cc
/地图加载
void System::LoadMap(const string &filename, const string &strSettingsFile)
{
//std::string strSettingsFile = "/home/whp/whp_ws/orbslam2/src/ORB_SLAM2-master/Examples/Monocular/TUM1.yaml";
mySettingFile = strSettingsFile;
SystemSetting* mySystemSetting = new SystemSetting(mpVocabulary);
mySystemSetting->LoadSystemSetting(mySettingFile);
mpMap->Load(filename,mySystemSetting);
}
注意:LoadMap函数中的”mySettingFile"是在System.h,文件中声明的传递yaml文件的变量。在System.cc中,yaml文件的传递都是靠路径,也就是string类型的变量,所以这里”mySettingFile"的声明如下:
std::string mySettingsFile;
但是同时还要在System.cc的构造函数中,将yaml文件的路径传递给他:
mySettingsFile = strSettingsFile;
直接把路径写在那,也是完全OK的。
第六步:修改CMakeList.txt
#在add_library 中加入 src/InitkeyFrame.cc src/SystemSetting.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
)
第七步:测试
改写完成后,在Examples文件中对应的示例程序中加入地图加载代码即可实现地图加载功能。
如在mono_tum.cc文件中改写如下:
注释掉建图和存图部分(不注释掉会引起程序崩溃),写入加载地图部分,代码如下:
//加载地图
// Create SLAM system. 创建ORB系统对象.
//It initializes all system threads and gets ready to process frames.
ORB_SLAM2::System SLAM(argv[1],argv[2],ORB_SLAM2::System::MONOCULAR,true);
//SLAM.LoadMap("/home/whp/whp_ws/whpslam/ORB_SLAM2_whp/version_0.2/Examples/Monocular/map.bin");
SLAM.LoadMap("map.bin",argv[2]);
cv::waitKey();
// Stop all threads关闭SLAM系统
SLAM.Shutdown();