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

Fixed gravity links not added for optimization when both prior and gravity links are added to same nodes

Browse Source
This commit is contained in:
matlabbe
2020-04-26 23:45:32 -04:00
parent 1a3c90e898
commit be57c2752d
1 changed files with 2 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files

3
corelib/src/Memory.cpp
View File

@@ -5476,8 +5476,9 @@ void Memory::getMetricConstraints(
std::multimap<int, Link> tmpLinks = getLinks(*iter, lookInDatabase, true); std::multimap<int, Link> tmpLinks = getLinks(*iter, lookInDatabase, true);
for(std::multimap<int, Link>::iterator jter=tmpLinks.begin(); jter!=tmpLinks.end(); ++jter) for(std::multimap<int, Link>::iterator jter=tmpLinks.begin(); jter!=tmpLinks.end(); ++jter)
{ {
std::multimap<int, Link>::iterator addedLinksIterator = graph::findLink(links, *iter, jter->first);
if( jter->second.isValid() && if( jter->second.isValid() &&
graph::findLink(links, *iter, jter->first) == links.end() && (addedLinksIterator == links.end() || addedLinksIterator->second.from()==addedLinksIterator->second.to()) &&
(uContains(poses, jter->first) || (landmarksAdded && jter->second.type() == Link::kLandmark))) (uContains(poses, jter->first) || (landmarksAdded && jter->second.type() == Link::kLandmark)))
{ {
if(!lookInDatabase && if(!lookInDatabase &&