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

DbViewer: fixed fatal error when adding new constraints on nodes not in the graph (now we can only add new constraints with nodes in the graph)

Browse Source
This commit is contained in:
matlabbe
2019-02-15 17:43:18 -05:00
parent caf165a635
commit dd5e09be37
1 changed files with 16 additions and 15 deletions
Download Patch File Download Diff File Expand all files Collapse all files
guilib/src/DatabaseViewer.cpp
+16 -15
View File
@@ -1536,9 +1536,16 @@ void DatabaseViewer::updateIds()
linksRemoved_.clear(); linksRemoved_.clear();
ui_->toolBox_statistics->clear(); ui_->toolBox_statistics->clear();
ui_->label_optimizeFrom->setText(tr("Root")); ui_->label_optimizeFrom->setText(tr("Root"));
std::multimap<int, Link> unilinks;
dbDriver_->getAllLinks(unilinks, true);
UDEBUG("%d total links loaded", (int)unilinks.size());
// add both direction links
std::multimap<int, Link> links; std::multimap<int, Link> links;
dbDriver_->getAllLinks(links, true); for(std::multimap<int, Link>::iterator iter=unilinks.begin(); iter!=unilinks.end(); ++iter)
UDEBUG("%d total links loaded", (int)links.size()); {
links.insert(*iter);
links.insert(std::make_pair(iter->second.to(), iter->second.inverse()));
}
double totalOdom = 0.0; double totalOdom = 0.0;
Transform previousPose; Transform previousPose;
@@ -1599,7 +1606,7 @@ void DatabaseViewer::updateIds()
previousPose=p; previousPose=p;
//links //links
bool addPose = links.find(ids_[i]) == links.end(); bool addPose = false;
for(std::multimap<int, Link>::iterator jter=links.find(ids_[i]); jter!=links.end() && jter->first == ids_[i]; ++jter) for(std::multimap<int, Link>::iterator jter=links.find(ids_[i]); jter!=links.end() && jter->first == ids_[i]; ++jter)
{ {
if(jter->second.type() == Link::kNeighborMerged) if(jter->second.type() == Link::kNeighborMerged)
@@ -3620,6 +3627,10 @@ void DatabaseViewer::update(int value,
float x,y,z,roll,pitch,yaw; float x,y,z,roll,pitch,yaw;
odomPose.getTranslationAndEulerAngles(x,y,z,roll, pitch,yaw); odomPose.getTranslationAndEulerAngles(x,y,z,roll, pitch,yaw);
labelPose->setText(QString("%1xyz=(%2,%3,%4)\nrpy=(%5,%6,%7)").arg(odomPose.isIdentity()?"* ":"").arg(x).arg(y).arg(z).arg(roll).arg(pitch).arg(yaw)); labelPose->setText(QString("%1xyz=(%2,%3,%4)\nrpy=(%5,%6,%7)").arg(odomPose.isIdentity()?"* ":"").arg(x).arg(y).arg(z).arg(roll).arg(pitch).arg(yaw));
if(odomPoses_.size() && odomPoses_.find(id) == odomPoses_.end())
{
labelPose->setText(labelPose->text() + "\n<Not in graph>");
}
if(s!=0.0) if(s!=0.0)
{ {
stamp->setText(QString::number(s, 'f')); stamp->setText(QString::number(s, 'f'));
@@ -6552,16 +6563,6 @@ bool DatabaseViewer::addConstraint(int from, int to, bool silent)
uStr2Int(ui_->parameters_toolbox->getParameters().at(Parameters::kOptimizerIterations())) > 0.0f) uStr2Int(ui_->parameters_toolbox->getParameters().at(Parameters::kOptimizerIterations())) > 0.0f)
{ {
int fromId = newLink.from(); int fromId = newLink.from();
int mapId = mapIds_.at(newLink.from());
// use first node of the map containing from
for(std::map<int, int>::iterator iter=mapIds_.begin(); iter!=mapIds_.end(); ++iter)
{
if(iter->second == mapId)
{
fromId = iter->first;
break;
}
}
std::multimap<int, Link> linksIn = updateLinksWithModifications(links_); std::multimap<int, Link> linksIn = updateLinksWithModifications(links_);
linksIn.insert(std::make_pair(newLink.from(), newLink)); linksIn.insert(std::make_pair(newLink.from(), newLink));
const Link * maxLinearLink = 0; const Link * maxLinearLink = 0;
@@ -6572,8 +6573,8 @@ bool DatabaseViewer::addConstraint(int from, int to, bool silent)
std::map<int, Transform> poses; std::map<int, Transform> poses;
std::multimap<int, Link> links; std::multimap<int, Link> links;
UASSERT(odomPoses_.find(fromId) != odomPoses_.end()); UASSERT(odomPoses_.find(fromId) != odomPoses_.end());
UASSERT_MSG(odomPoses_.find(newLink.from()) != odomPoses_.end(), uFormat("id=%d poses=%d links=%d", newLink.from(), (int)poses.size(), (int)links.size()).c_str()); UASSERT_MSG(odomPoses_.find(newLink.from()) != odomPoses_.end(), uFormat("id=%d poses=%d links=%d", newLink.from(), (int)odomPoses_.size(), (int)linksIn.size()).c_str());
UASSERT_MSG(odomPoses_.find(newLink.to()) != odomPoses_.end(), uFormat("id=%d poses=%d links=%d", newLink.to(), (int)poses.size(), (int)links.size()).c_str()); UASSERT_MSG(odomPoses_.find(newLink.to()) != odomPoses_.end(), uFormat("id=%d poses=%d links=%d", newLink.to(), (int)odomPoses_.size(), (int)linksIn.size()).c_str());
optimizer->getConnectedGraph(fromId, odomPoses_, linksIn, poses, links); optimizer->getConnectedGraph(fromId, odomPoses_, linksIn, poses, links);
// use already optimized poses // use already optimized poses
if(graphes_.size()) if(graphes_.size())