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

Fixed missing 3D keypoints when RGBD/LoopClosureReextractFeatures=true and Reg/Strategy=1 (https://github.com/introlab/rtabmap_ros/issues/668). DBViewer: fixed wrong poses optimization with GTSAM when showing scans of loop closures by proximity by space (multiscan) while there are GPS priors.

Browse Source
This commit is contained in:
matlabbe
2021-12-05 17:36:20 -05:00
parent 12906f4490
commit 580e35afb1
6 changed files with 43 additions and 43 deletions
Download Patch File Download Diff File Expand all files Collapse all files

33
guilib/src/DatabaseViewer.cpp
View File

@@ -1831,11 +1831,11 @@ void DatabaseViewer::updateIds()
infoReducedGraph_ = true;
}
std::multimap<int, Link>::iterator invertedLinkIter = graph::findLink(links, jter->second.to(), jter->second.from(), false);
std::multimap<int, Link>::iterator invertedLinkIter = graph::findLink(links, jter->second.to(), jter->second.from(), false, jter->second.type());
if( jter->second.isValid() && // null transform means a rehearsed location
ids.find(jter->second.from()) != ids.end() &&
(ids.find(jter->second.to()) != ids.end() || jter->second.to()<0) && // to add landmark links
graph::findLink(links_, jter->second.from(), jter->second.to()) == links_.end() &&
graph::findLink(links_, jter->second.from(), jter->second.to(), false, jter->second.type()) == links_.end() &&
invertedLinkIter != links.end() &&
w != -9)
{
@@ -6218,25 +6218,28 @@ void DatabaseViewer::updateConstraintView(
linksOut);
if(poses.size() != posesOut.size())
{
UWARN("Scan poses input and output are different! %d vs %d", (int)poses.size(), (int)posesOut.size());
UWARN("Input poses: ");
for(std::map<int, Transform>::iterator iter=poses.begin(); iter!=poses.end(); ++iter)
{
UWARN(" %d", iter->first);
}
UWARN("Input links: ");
std::multimap<int, Link> modifiedLinks = updateLinksWithModifications(links_);
for(std::multimap<int, Link>::iterator iter=modifiedLinks.begin(); iter!=modifiedLinks.end(); ++iter)
{
{
UWARN("Scan poses input and output are different! %d vs %d", (int)poses.size(), (int)posesOut.size());
}
UDEBUG("Input poses: ");
for(std::map<int, Transform>::iterator iter=posesOut.begin(); iter!=posesOut.end(); ++iter)
{
UDEBUG(" %d=%s", iter->first, iter->second.prettyPrint().c_str());
}
UDEBUG("Input links: ");
for(std::multimap<int, Link>::iterator iter=linksOut.begin(); iter!=linksOut.end(); ++iter)
{
UDEBUG(" %d->%d (type=%s) %s", iter->second.from(), iter->second.to(), iter->second.typeName().c_str(), iter->second.transform().prettyPrint().c_str());
}
std::map<int, rtabmap::Transform> finalPoses = optimizer->optimize(link.to(), posesOut, linksOut);
delete optimizer;
UDEBUG("Output poses: ");
for(std::map<int, Transform>::iterator iter=finalPoses.begin(); iter!=finalPoses.end(); ++iter)
{
UDEBUG(" %d=%s", iter->first, iter->second.prettyPrint().c_str());
}
// transform local poses in loop referential