/**
* This file is part of ORB-SLAM3
*
* Copyright (C) 2017-2021 Carlos Campos, Richard Elvira, Juan J. Gómez Rodríguez, José M.M. Montiel and Juan D. Tardós, University of Zaragoza.
* Copyright (C) 2014-2016 Raúl Mur-Artal, José M.M. Montiel and Juan D. Tardós, University of Zaragoza.
*
* ORB-SLAM3 is free software: you can redistribute it and/or modify it under the terms of the GNU General Public
* License as published by the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* ORB-SLAM3 is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even
* the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License along with ORB-SLAM3.
* If not, see .
*/
#include "Map.h"
#include
namespace ORB_SLAM3
{
long unsigned int Map::nNextId = 0;
Map::Map()
: mnMaxKFid(0), mnBigChangeIdx(0), mbImuInitialized(false), mnMapChange(0), mpFirstRegionKF(static_cast(NULL)),
mbFail(false), mIsInUse(false), mHasTumbnail(false), mbBad(false), mnMapChangeNotified(0), mbIsInertial(false), mbIMU_BA1(false), mbIMU_BA2(false)
{
mnId = nNextId++;
mThumbnail = static_cast(NULL);
}
Map::Map(int initKFid)
: mnInitKFid(initKFid), mnMaxKFid(initKFid), /*mnLastLoopKFid(initKFid),*/ mnBigChangeIdx(0), mIsInUse(false),
mHasTumbnail(false), mbBad(false), mbImuInitialized(false), mpFirstRegionKF(static_cast(NULL)),
mnMapChange(0), mbFail(false), mnMapChangeNotified(0), mbIsInertial(false), mbIMU_BA1(false), mbIMU_BA2(false)
{
mnId = nNextId++;
mThumbnail = static_cast(NULL);
}
Map::~Map()
{
// TODO: erase all points from memory
mspMapPoints.clear();
// TODO: erase all keyframes from memory
mspKeyFrames.clear();
if (mThumbnail)
delete mThumbnail;
mThumbnail = static_cast(NULL);
mvpReferenceMapPoints.clear();
mvpKeyFrameOrigins.clear();
}
// 在地图中插入关键帧,同时更新关键帧的最大id
void Map::AddKeyFrame(KeyFrame *pKF)
{
unique_lock 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;
}
}
void Map::AddMapPoint(MapPoint *pMP)
{
unique_lock lock(mMutexMap);
mspMapPoints.insert(pMP);
}
void Map::SetImuInitialized()
{
unique_lock lock(mMutexMap);
mbImuInitialized = true;
}
bool Map::isImuInitialized()
{
unique_lock lock(mMutexMap);
return mbImuInitialized;
}
void Map::EraseMapPoint(MapPoint *pMP)
{
unique_lock lock(mMutexMap);
mspMapPoints.erase(pMP);
// 下面是作者加入的注释. 实际上只是从std::set中删除了地图点的指针, 原先地图点
// 占用的内存区域并没有得到释放
// TODO: This only erase the pointer.
// Delete the MapPoint
}
void Map::EraseKeyFrame(KeyFrame *pKF)
{
unique_lock lock(mMutexMap);
mspKeyFrames.erase(pKF);
if (mspKeyFrames.size() > 0)
{
if (pKF->mnId == mpKFlowerID->mnId)
{
vector vpKFs = vector(mspKeyFrames.begin(), mspKeyFrames.end());
sort(vpKFs.begin(), vpKFs.end(), KeyFrame::lId);
mpKFlowerID = vpKFs[0];
}
}
else
{
mpKFlowerID = 0;
}
// TODO: This only erase the pointer.
// Delete the MapPoint
}
/*
* @brief 设置参考MapPoints,将用于DrawMapPoints函数画图
* 设置参考地图点用于绘图显示局部地图点(红色)
* @param vpMPs Local MapPoints
*/
void Map::SetReferenceMapPoints(const vector &vpMPs)
{
unique_lock lock(mMutexMap);
mvpReferenceMapPoints = vpMPs;
}
void Map::InformNewBigChange()
{
unique_lock lock(mMutexMap);
mnBigChangeIdx++;
}
int Map::GetLastBigChangeIdx()
{
unique_lock lock(mMutexMap);
return mnBigChangeIdx;
}
// 获取地图中的所有关键帧
vector Map::GetAllKeyFrames()
{
unique_lock lock(mMutexMap);
return vector(mspKeyFrames.begin(), mspKeyFrames.end());
}
// 获取地图中的所有地图点
vector Map::GetAllMapPoints()
{
unique_lock lock(mMutexMap);
return vector(mspMapPoints.begin(), mspMapPoints.end());
}
// 获取地图点数目
long unsigned int Map::MapPointsInMap()
{
unique_lock lock(mMutexMap);
return mspMapPoints.size();
}
// 获取地图中的关键帧数目
long unsigned int Map::KeyFramesInMap()
{
unique_lock lock(mMutexMap);
return mspKeyFrames.size();
}
// 获取参考地图点
vector Map::GetReferenceMapPoints()
{
unique_lock lock(mMutexMap);
return mvpReferenceMapPoints;
}
long unsigned int Map::GetId()
{
return mnId;
}
long unsigned int Map::GetInitKFid()
{
unique_lock lock(mMutexMap);
return mnInitKFid;
}
void Map::SetInitKFid(long unsigned int initKFif)
{
unique_lock lock(mMutexMap);
mnInitKFid = initKFif;
}
long unsigned int Map::GetMaxKFid()
{
unique_lock lock(mMutexMap);
return mnMaxKFid;
}
KeyFrame *Map::GetOriginKF()
{
return mpKFinitial;
}
void Map::SetCurrentMap()
{
mIsInUse = true;
}
void Map::SetStoredMap()
{
mIsInUse = false;
}
void Map::clear()
{
// for(set::iterator sit=mspMapPoints.begin(), send=mspMapPoints.end(); sit!=send; sit++)
// delete *sit;
for (set::iterator sit = mspKeyFrames.begin(), send = mspKeyFrames.end(); sit != send; sit++)
{
KeyFrame *pKF = *sit;
pKF->UpdateMap(static_cast