12345qiupeng
/
orb_slam3_details
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

fix: save map break

master
laobai 2023-02-27 19:53:37 +08:00
parent 33f7ef16ed
commit 6c3b52564b
3 changed files with 15 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
include/MapPoint.h
View File

@ -123,12 +123,12 @@ public:
int Observations();
void AddObservation(KeyFrame* pKF,int idx);
void EraseObservation(KeyFrame* pKF);
void EraseObservation(KeyFrame* pKF, bool erase = true);
std::tuple<int,int> GetIndexInKeyFrame(KeyFrame* pKF);
bool IsInKeyFrame(KeyFrame* pKF);
void SetBadFlag();
void SetBadFlag(bool erase = true);
bool isBad();
void Replace(MapPoint* pMP);

6
src/Map.cc
View File

@ -311,6 +311,8 @@ void Map::ApplyScaledRotation(const Sophus::SE3f &T, const float s, const bool b
{
// 更新每一个mp在世界坐标系下的坐标
MapPoint *pMP = *sit;
if (!pMP || pMP->isBad())
continue;
pMP->SetWorldPos(s * Ryw * pMP->GetWorldPos() + tyw);
pMP->UpdateNormalAndDepth();
}
@ -412,9 +414,9 @@ void Map::PreSave(std::set<GeometricCamera *> &spCams)
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 || it->first->isBad())
if (!it->first || it->first->GetMap() != this || it->first->isBad())
{
pMPi->EraseObservation(it->first);
pMPi->EraseObservation(it->first, false);
}
}
}

15
src/MapPoint.cc
View File

@ -201,7 +201,7 @@ void MapPoint::AddObservation(KeyFrame* pKF, int idx)
}
// 删除某个关键帧对当前地图点的观测
void MapPoint::EraseObservation(KeyFrame* pKF)
void MapPoint::EraseObservation(KeyFrame* pKF, bool erase)
{
bool bBad=false;
{
@ -236,7 +236,7 @@ void MapPoint::EraseObservation(KeyFrame* pKF)
}
// 告知可以观测到该MapPoint的Frame，该MapPoint已被删除
if(bBad)
SetBadFlag();
SetBadFlag(erase);
}
// 能够观测到当前地图点的所有关键帧及该地图点在KF中的索引
@ -260,7 +260,7 @@ int MapPoint::Observations()
* @brief 告知可以观测到该MapPoint的Frame，该MapPoint已被删除
*
*/
void MapPoint::SetBadFlag()
void MapPoint::SetBadFlag(bool erase)
{
map<KeyFrame*, tuple<int,int>> obs;
{
@ -285,7 +285,8 @@ void MapPoint::SetBadFlag()
}
// 擦除该MapPoint申请的内存
mpMap->EraseMapPoint(this);
if (erase)
mpMap->EraseMapPoint(this);
}
/**
@ -769,6 +770,7 @@ void MapPoint::PreSave(set<KeyFrame*>& spKF,set<MapPoint*>& spMP)
mBackupObservationsId1.clear();
mBackupObservationsId2.clear();
// Save the id and position in each KF who view it
std::vector<KeyFrame*> erase_kfs;
for(std::map<KeyFrame*,std::tuple<int,int> >::const_iterator it = mObservations.begin(),
end = mObservations.end(); it != end; ++it)
{
@ -780,10 +782,11 @@ void MapPoint::PreSave(set<KeyFrame*>& spKF,set<MapPoint*>& spMP)
}
else
{
EraseObservation(pKFi);
erase_kfs.push_back(pKFi);
}
}
for (auto pKFi : erase_kfs)
EraseObservation(pKFi, false);
// Save the id of the reference KF
// 3. 备份参考关键帧ID
if(spKF.find(mpRefKF) != spKF.end())