Skip to content

Commit

Permalink
Fixed duplicates in neighbor and loop closure sliders, fixed WM check…
Browse files Browse the repository at this point in the history 
…box not visible.
  • Loading branch information
@matlabbe
matlabbe committed Sep 29, 2024
1 parent 745f70e commit 30f3fb2
Show file tree
Hide file tree
Showing 2 changed files with 112 additions and 64 deletions.
1 change: 1 addition & 0 deletions guilib/include/rtabmap/gui/DatabaseViewer.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -198,6 +198,7 @@ private Q_SLOTS:
int to);
std::multimap<int, rtabmap::Link> updateLinksWithModifications(
const std::multimap<int, rtabmap::Link> & edgeConstraints);
void updateNeighborsSlider(int from = 0, int to = 0);
void updateLoopClosuresSlider(int from = 0, int to = 0);
void updateCovariances(const QList<Link> & links);
void refineLinks(const QList<Link> & links);
Expand Down
175 changes: 111 additions & 64 deletions guilib/src/DatabaseViewer.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -1874,7 +1874,6 @@ void DatabaseViewer::updateIds()
if(wmStates.find(ids_[i]) != wmStates.end())
{
wmStates_.insert(std::make_pair(ids_[i], wmStates.at(ids_[i])));
ui_->checkBox_wmState->setVisible(true);
}
if(w < 0)
{
Expand Down Expand Up @@ -1953,6 +1952,8 @@ void DatabaseViewer::updateIds()
}
}

ui_->checkBox_wmState->setVisible(!wmStates_.empty() || !lastWmIds_.empty());

progressDialog->appendText("Loading info for all nodes... done!");
progressDialog->incrementStep();
progressDialog->appendText("Loading optimized poses and maps...");
Expand Down Expand Up @@ -2107,27 +2108,6 @@ void DatabaseViewer::updateIds()
ui_->menuExport_poses->setEnabled(!odomPoses_.empty());
graphes_.clear();
graphLinks_.clear();
neighborLinks_.clear();
loopLinks_.clear();
for(std::multimap<int, rtabmap::Link>::iterator iter = links_.begin(); iter!=links_.end(); ++iter)
{
if(!iter->second.transform().isNull())
{
if(iter->second.type() == rtabmap::Link::kNeighbor ||
iter->second.type() == rtabmap::Link::kNeighborMerged)
{
neighborLinks_.append(iter->second);
}
else if(iter->second.from()!=iter->second.to())
{
loopLinks_.append(iter->second);
}
}
else
{
UERROR("Transform null for link from %d to %d", iter->first, iter->second.to());
}
}

if(ids_.size())
{
Expand Down Expand Up @@ -2161,20 +2141,9 @@ void DatabaseViewer::updateIds()
ui_->label_idB->setText("NaN");
}

if(neighborLinks_.size())
{
ui_->horizontalSlider_neighbors->setMinimum(0);
ui_->horizontalSlider_neighbors->setMaximum(neighborLinks_.size()-1);
ui_->horizontalSlider_neighbors->setEnabled(true);
ui_->horizontalSlider_neighbors->setSliderPosition(0);
}
else
{
ui_->horizontalSlider_neighbors->setEnabled(false);
}

if(ids_.size())
{
updateNeighborsSlider();
updateLoopClosuresSlider();
if(ui_->graphViewer->isVisible() || ui_->dockWidget_occupancyGridView->isVisible())
{
Expand Down Expand Up @@ -4348,28 +4317,43 @@ void DatabaseViewer::detectMoreLoopClosures()

void DatabaseViewer::updateAllNeighborCovariances()
{
updateCovariances(neighborLinks_);
std::multimap<int, Link> allLinks = updateLinksWithModifications(links_);
QList<rtabmap::Link> links;
for(std::multimap<int, Link>::iterator iter=allLinks.begin(); iter!=allLinks.end(); ++iter)
{
if(iter->second.type() == Link::kNeighbor ||
iter->second.type() == Link::kNeighborMerged)
{
links.push_back(iter->second);
}
}
updateCovariances(links);
}
void DatabaseViewer::updateAllLoopClosureCovariances()
{
std::multimap<int, Link> allLinks = updateLinksWithModifications(links_);
QList<rtabmap::Link> links;
for(int i=0; i<loopLinks_.size(); ++i)
for(std::multimap<int, Link>::iterator iter=allLinks.begin(); iter!=allLinks.end(); ++iter)
{
if(loopLinks_.at(i).type() != Link::kLandmark)
if(iter->second.type() != Link::kNeighbor &&
iter->second.type() != Link::kNeighborMerged &&
iter->second.type() != Link::kLandmark &&
iter->second.from() != iter->second.to())
{
links.push_back(loopLinks_.at(i));
links.push_back(iter->second);
}
}
updateCovariances(links);
}
void DatabaseViewer::updateAllLandmarkCovariances()
{
std::multimap<int, Link> allLinks = updateLinksWithModifications(links_);
QList<rtabmap::Link> links;
for(int i=0; i<loopLinks_.size(); ++i)
for(std::multimap<int, Link>::iterator iter=allLinks.begin(); iter!=allLinks.end(); ++iter)
{
if(loopLinks_.at(i).type() == Link::kLandmark)
if(iter->second.type() == Link::kLandmark)
{
links.push_back(loopLinks_.at(i));
links.push_back(iter->second);
}
}
updateCovariances(links);
Expand All @@ -4379,13 +4363,7 @@ void DatabaseViewer::updateCovariances(const QList<Link> & links)
{
if(links.size())
{
cv::Mat infMatrix = links.first().infMatrix();
std::multimap<int, Link>::iterator findIter = rtabmap::graph::findLink(linksRefined_, links.first().from() ,links.first().to(), false, links.first().type());
if(findIter != linksRefined_.end())
{
infMatrix = findIter->second.infMatrix();
}

cv::Mat infMatrix = links.first().infMatrix();
EditConstraintDialog dialog(Transform::getIdentity(), infMatrix.inv());
dialog.setPoseGroupVisible(false);
if(dialog.exec() != QDialog::Accepted)
Expand Down Expand Up @@ -4576,6 +4554,7 @@ void DatabaseViewer::resetAllChanges()
linksRemoved_.clear();
generatedLocalMaps_.clear();
modifiedLaserScans_.clear();
updateNeighborsSlider();
updateLoopClosuresSlider();
this->updateGraphView();
}
Expand Down Expand Up @@ -7080,7 +7059,9 @@ void DatabaseViewer::sliderIterationsValueChanged(int value)
ui_->graphViewer->updateGTGraph(groundTruthPoses_);
ui_->graphViewer->updateGPSGraph(gpsPoses_, gpsValues_);
ui_->graphViewer->updateGraph(graph, graphLinks_, mapIds_, weights_);
if(!ui_->checkBox_wmState->isChecked())
if(ui_->checkBox_wmState->isEnabled() &&
ui_->checkBox_wmState->isChecked() &&
!lastWmIds_.empty())
{
bool allNodesAreInWM = true;
std::map<int, float> colors;
Expand Down Expand Up @@ -7385,7 +7366,9 @@ void DatabaseViewer::updateGraphView()
graphLinks_.clear();

std::map<int, rtabmap::Transform> poses = odomPoses_;
if(ui_->checkBox_wmState->isChecked() && uContains(wmStates_, fromId))
if(ui_->checkBox_wmState->isEnabled() &&
ui_->checkBox_wmState->isChecked() &&
uContains(wmStates_, fromId))
{
std::map<int, rtabmap::Transform> wmPoses;
std::vector<int> & wmState = wmStates_.at(fromId);
Expand Down Expand Up @@ -7426,7 +7409,6 @@ void DatabaseViewer::updateGraphView()

ui_->menuExport_poses->setEnabled(true);
std::multimap<int, rtabmap::Link> links = links_;
loopLinks_.clear();

// filter current map if not spanning to all maps
if(!ui_->checkBox_spanAllMaps->isChecked() && uContains(mapIds_, fromId) && mapIds_.at(fromId) >= 0)
Expand Down Expand Up @@ -7549,7 +7531,6 @@ void DatabaseViewer::updateGraphView()
links.erase(iter++);
continue;
}
loopLinks_.push_back(iter->second);
if(isUnique)
++totalGlobal;
}
Expand All @@ -7560,7 +7541,6 @@ void DatabaseViewer::updateGraphView()
links.erase(iter++);
continue;
}
loopLinks_.push_back(iter->second);
if(isUnique)
++totalLocalSpace;
}
Expand All @@ -7571,7 +7551,6 @@ void DatabaseViewer::updateGraphView()
links.erase(iter++);
continue;
}
loopLinks_.push_back(iter->second);
if(isUnique)
++totalLocalTime;
}
Expand All @@ -7582,7 +7561,6 @@ void DatabaseViewer::updateGraphView()
links.erase(iter++);
continue;
}
loopLinks_.push_back(iter->second);
if(isUnique)
++totalUser;
}
Expand All @@ -7598,7 +7576,6 @@ void DatabaseViewer::updateGraphView()
{
poses.insert(std::make_pair(iter->second.to(), poses.at(iter->second.from())*iter->second.transform()));
}
loopLinks_.push_back(iter->second);
if(isUnique)
++totalLandmarks;

Expand Down Expand Up @@ -7626,10 +7603,6 @@ void DatabaseViewer::updateGraphView()
if(isUnique)
++totalGravity;
}
else
{
loopLinks_.push_back(iter->second);
}
++iter;
}
updateLoopClosuresSlider();
Expand Down Expand Up @@ -9118,7 +9091,6 @@ void DatabaseViewer::rejectConstraint()
if(priorId==0)
{
this->updateGraphView();
updateLoopClosuresSlider();
}
else
{
Expand Down Expand Up @@ -9164,7 +9136,16 @@ std::multimap<int, rtabmap::Link> DatabaseViewer::updateLinksWithModifications(
findIter = rtabmap::graph::findLink(linksRefined_, iter->second.from(), iter->second.to());
if(findIter!=linksRefined_.end())
{
links.insert(*findIter); // add the refined link
// add the refined link
if(iter->second.from() == findIter->second.to() &&
iter->second.from() != iter->second.to())
{
links.insert(std::make_pair(iter->second.from(), findIter->second.inverse()));
}
else
{
links.insert(*findIter);
}
UDEBUG("Updated link (%d->%d, %d)", iter->second.from(), iter->second.to(), iter->second.type());
continue;
}
Expand All @@ -9181,17 +9162,83 @@ std::multimap<int, rtabmap::Link> DatabaseViewer::updateLinksWithModifications(
if(findIter!=linksRefined_.end())
{
links.insert(*findIter); // add the refined link
links.insert(std::make_pair(findIter->second.to(), findIter->second.inverse())); // return both ways
UDEBUG("Added refined link (%d->%d, %d)", findIter->second.from(), findIter->second.to(), findIter->second.type());
continue;
}

UDEBUG("Added link (%d->%d, %d)", iter->second.from(), iter->second.to(), iter->second.type());
links.insert(*iter);
links.insert(std::make_pair(iter->second.to(), iter->second.inverse())); // return both ways
}

return links;
}

void DatabaseViewer::updateNeighborsSlider(int from, int to)
{
UDEBUG("%d %d", from, to);
neighborLinks_.clear();
std::multimap<int, Link> links = updateLinksWithModifications(links_);
int position = ui_->horizontalSlider_neighbors->value();
std::multimap<int, Link> linksSortedByChildren;
for(std::multimap<int, rtabmap::Link>::iterator iter = links.begin(); iter!=links.end(); ++iter)
{
if(iter->second.from() < iter->second.to())
{
linksSortedByChildren.insert(*iter);
}
}

for(std::multimap<int, rtabmap::Link>::iterator iter = linksSortedByChildren.begin(); iter!=linksSortedByChildren.end(); ++iter)
{
if(!iter->second.transform().isNull())
{
if(iter->second.type() == rtabmap::Link::kNeighbor ||
iter->second.type() == rtabmap::Link::kNeighborMerged)
{
if((iter->second.from() == from && iter->second.to() == to) ||
(iter->second.to() == from && iter->second.from() == to))
{
position = neighborLinks_.size();
}
neighborLinks_.append(iter->second);
}
}
else
{
UERROR("Transform null for link from %d to %d", iter->first, iter->second.to());
}
}

if(neighborLinks_.size())
{
if(neighborLinks_.size() == 1)
{
// just to be able to move the cursor of the neighbor slider
neighborLinks_.push_back(neighborLinks_.front());
}
ui_->horizontalSlider_neighbors->setMinimum(0);
ui_->horizontalSlider_neighbors->setMaximum(neighborLinks_.size()-1);
ui_->horizontalSlider_neighbors->setEnabled(true);
if(position != ui_->horizontalSlider_neighbors->value())
{
ui_->horizontalSlider_neighbors->setValue(position);
}
else
{
this->updateConstraintView(neighborLinks_.at(position));
}
}
else
{
ui_->horizontalSlider_neighbors->setEnabled(false);
constraintsViewer_->removeAllClouds();
constraintsViewer_->refreshView();
updateConstraintButtons();
}
}

void DatabaseViewer::updateLoopClosuresSlider(int from, int to)
{
UDEBUG("%d %d", from, to);
Expand All @@ -9201,11 +9248,11 @@ void DatabaseViewer::updateLoopClosuresSlider(int from, int to)
std::multimap<int, Link> linksSortedByParents;
for(std::multimap<int, rtabmap::Link>::iterator iter = links.begin(); iter!=links.end(); ++iter)
{
if(iter->second.to() > iter->second.from())
if(iter->second.to() > iter->second.from() && iter->second.from() < 0) // landmark
{
linksSortedByParents.insert(std::make_pair(iter->second.to(), iter->second.inverse()));
}
else if(iter->second.to() != iter->second.from())
else if(iter->second.to() < iter->second.from())
{
linksSortedByParents.insert(*iter);
}
Expand Down

0 comments on commit 30f3fb2

Please sign in to comment.