i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-02 09:30:25 +08:00
Code Issues Packages Projects Releases Wiki Activity

Fixed assert when loading database having optimized poses different from the working memory (force re-update graph in this case).

Browse Source
This commit is contained in:
matlabbe
2021-02-06 15:10:18 -05:00
parent c1a22609f3
commit c42a4e3d7e
1 changed files with 30 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files

31
corelib/src/Memory.cpp
View File

@@ -2059,7 +2059,36 @@ std::map<int, Transform> Memory::loadOptimizedPoses(Transform * lastlocalization
{
if(_dbDriver)
{
return _dbDriver->loadOptimizedPoses(lastlocalizationPose);
bool ok = true;
std::map<int, Transform> poses = _dbDriver->loadOptimizedPoses(lastlocalizationPose);
// Make sure optimized poses match the working directory! Otherwise return nothing.
if(poses.size() == _workingMem.size())
{
for(std::map<int, Transform>::iterator iter=poses.begin(); iter!=poses.end() && ok; ++iter)
{
if(_workingMem.find(iter->first)==_workingMem.end())
{
ok = false;
}
}
}
else
{
ok = false;
}
if(!ok)
{
UWARN("Optimized poses (%d) and working memory "
"size (%d) don't match. Returning empty optimized "
"poses to force re-update. If you want to use the "
"saved optimized poses, set %s to true",
(int)poses.size(),
(int)_workingMem.size(),
Parameters::kMemInitWMWithAllNodes().c_str());
return std::map<int, Transform>();
}
return poses;
}
return std::map<int, Transform>();
}