2021SC@SDUSC
Map分析
Map类管理整个地图,因此其具有的属性是所有的地图点和关建针,将主要的方法梳理一下,做成一个图如下:
Map类总体较为简单明晰,因此会较为简略的分析。
AddKeyFrame作为Map类主要方法之一,用来在地图中插入关键帧,同时更新关键帧的最大id;
void Map::AddKeyFrame(KeyFrame *pKF)
{
unique_lock<mutex> lock(mMutexMap);
if(mspKeyFrames.empty()){
cout << "First KF:" << pKF->mnId << "; Map init KF:" << mnInitKFid << endl;
mnInitKFid = pKF->mnId;
mpKFinitial = pKF;
mpKFlowerID = pKF;
}
mspKeyFrames.insert(pKF);
if(pKF->mnId>mnMaxKFid)
{
mnMaxKFid=pKF->mnId;
}
if(pKF->mnId<mpKFlowerID->mnId)
{
mpKFlowerID = pKF;
}
}
AddMapPoint函数顾名思义,向地图中插入地图点;EraseMapPoint则是从地图中删除地图点,但是只是删除了Pointer,其实这个地图点所占用的内存空间并没有被释放。
void Map::AddMapPoint(MapPoint *pMP)
{
unique_lock<mutex> lock(mMutexMap);
mspMapPoints.insert(pMP);
}
void Map::EraseMapPoint(MapPoint *pMP)
{
unique_lock<mutex> lock(mMutexMap);
mspMapPoints.erase(pMP);
}
SetReferenceMapPoints方法是为了设置参考地图点用于绘图显示局部地图点(红色);
GetAllKeyFrames会获取地图中的所有关键帧;
GetAllMapPoints会获取地图中的所有地图点;
MapPointsInMap则是获取地图点数目;
KeyFramesInMap会获取地图中的关键帧数目;
GetReferenceMapPoints将会获取获取参考地图点。
void Map::SetReferenceMapPoints(const vector<MapPoint *> &vpMPs)
{
unique_lock<mutex> lock(mMutexMap);
mvpReferenceMapPoints = vpMPs;
}
vector<KeyFrame*> Map::GetAllKeyFrames()
{
unique_lock<mutex> lock(mMutexMap);
return vector<KeyFrame*>(mspKeyFrames.begin(),mspKeyFrames.end());
}
vector<MapPoint*> Map::GetAllMapPoints()
{
unique_lock<mutex> lock(mMutexMap);
return vector<MapPoint*>(mspMapPoints.begin(),mspMapPoints.end());
}
long unsigned int Map::MapPointsInMap()
{
unique_lock<mutex> lock(mMutexMap);
return mspMapPoints.size();
}
long unsigned int Map::KeyFramesInMap()
{
unique_lock<mutex> lock(mMutexMap);
return mspKeyFrames.size();
}
vector<MapPoint*> Map::GetReferenceMapPoints()
{
unique_lock<mutex> lock(mMutexMap);
return mvpReferenceMapPoints;
}
RotateMap的作用是对关键帧、MapPoint施加旋转;而ApplyScaledRotation则是对施加的旋转进行应用。
void Map::RotateMap(const cv::Mat &R)
{
unique_lock<mutex> lock(mMutexMap);
cv::Mat Txw = cv::Mat::eye(4,4,CV_32F);
R.copyTo(Txw.rowRange(0,3).colRange(0,3));
KeyFrame* pKFini = mvpKeyFrameOrigins[0];
cv::Mat Twc_0 = pKFini->GetPoseInverse();
cv::Mat Txc_0 = Txw*Twc_0;
cv::Mat Txb_0 = Txc_0*pKFini->mImuCalib.Tcb;
cv::Mat Tyx = cv::Mat::eye(4,4,CV_32F);
Tyx.rowRange(0,3).col(3) = -Txb_0.rowRange(0,3).col(3);
cv::Mat Tyw = Tyx*Txw;
cv::Mat Ryw = Tyw.rowRange(0,3).colRange(0,3);
cv::Mat tyw = Tyw.rowRange(0,3).col(3);
for(set<KeyFrame*>::iterator sit=mspKeyFrames.begin(); sit!=mspKeyFrames.end(); sit++)
{
KeyFrame* pKF = *sit;
cv::Mat Twc = pKF->GetPoseInverse();
cv::Mat Tyc = Tyw*Twc;
cv::Mat Tcy = cv::Mat::eye(4,4,CV_32F);
Tcy.rowRange(0,3).colRange(0,3) = Tyc.rowRange(0,3).colRange(0,3).t();
Tcy.rowRange(0,3).col(3) = -Tcy.rowRange(0,3).colRange(0,3)*Tyc.rowRange(0,3).col(3);
pKF->SetPose(Tcy);
cv::Mat Vw = pKF->GetVelocity();
pKF->SetVelocity(Ryw*Vw);
}
for(set<MapPoint*>::iterator sit=mspMapPoints.begin(); sit!=mspMapPoints.end(); sit++)
{
MapPoint* pMP = *sit;
pMP->SetWorldPos(Ryw*pMP->GetWorldPos()+tyw);
pMP->UpdateNormalAndDepth();
}
}
同时,ORB-SLAM3相对于2还增加不少方法,CheckEssentialGraph也是其中之一。通过两个for循环来检测第一个关键帧是否具有父代和子代。
bool Map::CheckEssentialGraph(){
vector<KeyFrame*> vpOriginKFs = mvpKeyFrameOrigins;
int count=0;
cout << "Number of origin KFs: " << vpOriginKFs.size() << endl;
KeyFrame* pFirstKF;
for(KeyFrame* pKFi : vpOriginKFs)
{
if(!pFirstKF)
pFirstKF = pKFi;
else if(!pKFi->GetParent())
pFirstKF = pKFi;
}
cout << "Checking if the first KF has parent" << endl;
if(pFirstKF->GetParent())
{
cout << "First KF in the essential graph has a parent, which is not possible" << endl;
}
set<KeyFrame*> spChilds = pFirstKF->GetChilds();
vector<KeyFrame*> vpChilds;
vpChilds.reserve(mspKeyFrames.size());
for(KeyFrame* pKFi : spChilds)
vpChilds.push_back(pKFi);
for(int i=0; i<vpChilds.size() && count <= (mspKeyFrames.size()+10); ++i)
{
count++;
KeyFrame* pKFi = vpChilds[i];
set<KeyFrame*> spKFiChilds = pKFi->GetChilds();
for(KeyFrame* pKFj : spKFiChilds)
vpChilds.push_back(pKFj);
}
cout << "count/tot" << count << "/" << mspKeyFrames.size() << endl;
if (count != (mspKeyFrames.size()-1))
return false;
else
return true;
}
对于PreSave方法,首先通过一个for循环来保存原始关键帧的id,接下来是将集合的容器备份到向量,来达到一个预存的目的。
void Map::PreSave(std::set<GeometricCamera*> &spCams)
{
int nMPWithoutObs = 0;
for(MapPoint* pMPi : mspMapPoints)
{
if(pMPi->GetObservations().size() == 0)
{
nMPWithoutObs++;
}
map<KeyFrame*, std::tuple<int,int>> mpObs = pMPi->GetObservations();
for(map<KeyFrame*, std::tuple<int,int>>::iterator it= mpObs.begin(), end=mpObs.end(); it!=end; ++it)
{
if(it->first->GetMap() != this)
{
pMPi->EraseObservation(it->first); //We need to find where the KF is set as Bad but the observation is not removed
}
}
}
cout << " Bad MapPoints removed" << endl;
// Saves the id of KF origins
mvBackupKeyFrameOriginsId.reserve(mvpKeyFrameOrigins.size());
for(int i = 0, numEl = mvpKeyFrameOrigins.size(); i < numEl; ++i)
{
mvBackupKeyFrameOriginsId.push_back(mvpKeyFrameOrigins[i]->mnId);
}
mvpBackupMapPoints.clear();
// Backup of set container to vector
//std::copy(mspMapPoints.begin(), mspMapPoints.end(), std::back_inserter(mvpBackupMapPoints));
for(MapPoint* pMPi : mspMapPoints)
{
//cout << "Pre-save of mappoint " << pMPi->mnId << endl;
mvpBackupMapPoints.push_back(pMPi);
pMPi->PreSave(mspKeyFrames,mspMapPoints);
}
cout << " MapPoints back up done!!" << endl;
mvpBackupKeyFrames.clear();
//std::copy(mspKeyFrames.begin(), mspKeyFrames.end(), std::back_inserter(mvpBackupKeyFrames));
for(KeyFrame* pKFi : mspKeyFrames)
{
mvpBackupKeyFrames.push_back(pKFi);
pKFi->PreSave(mspKeyFrames,mspMapPoints, spCams);
}
cout << " KeyFrames back up done!!" << endl;
mnBackupKFinitialID = -1;
if(mpKFinitial)
{
mnBackupKFinitialID = mpKFinitial->mnId;
}
mnBackupKFlowerID = -1;
if(mpKFlowerID)
{
mnBackupKFlowerID = mpKFlowerID->mnId;
}
}