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(); int Observations();
void AddObservation(KeyFrame* pKF,int idx); void AddObservation(KeyFrame* pKF,int idx);
void EraseObservation(KeyFrame* pKF); void EraseObservation(KeyFrame* pKF, bool erase = true);
std::tuple<int,int> GetIndexInKeyFrame(KeyFrame* pKF); std::tuple<int,int> GetIndexInKeyFrame(KeyFrame* pKF);
bool IsInKeyFrame(KeyFrame* pKF); bool IsInKeyFrame(KeyFrame* pKF);
void SetBadFlag(); void SetBadFlag(bool erase = true);
bool isBad(); bool isBad();
void Replace(MapPoint* pMP); 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在世界坐标系下的坐标 // 更新每一个mp在世界坐标系下的坐标
MapPoint *pMP = *sit; MapPoint *pMP = *sit;
if (!pMP || pMP->isBad())
continue;
pMP->SetWorldPos(s * Ryw * pMP->GetWorldPos() + tyw); pMP->SetWorldPos(s * Ryw * pMP->GetWorldPos() + tyw);
pMP->UpdateNormalAndDepth(); pMP->UpdateNormalAndDepth();
} }
@ -412,9 +414,9 @@ void Map::PreSave(std::set<GeometricCamera *> &spCams)
map<KeyFrame *, std::tuple<int, int>> mpObs = pMPi->GetObservations(); 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) 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);
} }
} }
} }

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