Node highlighting addition to previous pull request (#1348)

* Clicking Nodes\Links brings them up in viewer * Removed duplicated feature, kept spin box and single click node event. Removed unrelated border option crashing. * added code to highlight selected nodes * Refactored: merged A-B functions into single one, fixed highlighted node color not reset after selecting another node, highlighting also when changing constraint sliders, change node color instead of border, added menu options to change color (added to settings). * cleanup * Fixed duplicates in neighbor and loop closure sliders, fixed WM checkbox not visible. * Fixed forward neighbors slider scrolling showing only one color. --------- Co-authored-by: Chuck Ellison <charles.c.ellison@erdc.dren.mil> Co-authored-by: matlabbe <matlabbe@gmail.com> Co-authored-by: Alex Perez <alex.s.perez@erdc.dren.mil>
2026-09-12 22:40:19 +08:00 · 2024-09-28 18:40:46 -07:00
parent 8c0e57bc52
commit 538bc681d4
4 changed files with 219 additions and 64 deletions
@@ -587,6 +587,7 @@ void DatabaseViewer::readSettings()

 	// GraphViewer settings
 	ui_->graphViewer->loadSettings(settings, "GraphView");
+	ui_->graphViewer->setReferentialVisible(false);

 	settings.beginGroup("optimization");
 	ui_->doubleSpinBox_gainCompensationRadius->setValue(settings.value("gainCompensationRadius", ui_->doubleSpinBox_gainCompensationRadius->value()).toDouble());
@@ -1873,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)
 		{
@@ -1952,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...");
@@ -2106,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())
 	{
@@ -2160,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())
 		{
@@ -4347,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);
@@ -4378,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)
@@ -4575,6 +4554,7 @@ void DatabaseViewer::resetAllChanges()
 		linksRemoved_.clear();
 		generatedLocalMaps_.clear();
 		modifiedLaserScans_.clear();
+		updateNeighborsSlider();
 		updateLoopClosuresSlider();
 		this->updateGraphView();
 	}
@@ -4711,6 +4691,10 @@ void DatabaseViewer::update(int value,
 		labelId->setText(QString::number(id));
 		if(id>0)
 		{
+			if(ui_->dockWidget_graphView->isVisible()) {
+				ui_->graphViewer->highlightNode(id, spinBoxIndex==ui_->spinBox_indexB?1:0);
+			}
+
 			//image
 			QImage img;
 			cv::Mat imgDepth;
@@ -7075,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;
@@ -7380,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);
@@ -7421,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)
@@ -7544,7 +7531,6 @@ void DatabaseViewer::updateGraphView()
 					links.erase(iter++);
 					continue;
 				}
-				loopLinks_.push_back(iter->second);
 				if(isUnique)
 					++totalGlobal;
 			}
@@ -7555,7 +7541,6 @@ void DatabaseViewer::updateGraphView()
 					links.erase(iter++);
 					continue;
 				}
-				loopLinks_.push_back(iter->second);
 				if(isUnique)
 					++totalLocalSpace;
 			}
@@ -7566,7 +7551,6 @@ void DatabaseViewer::updateGraphView()
 					links.erase(iter++);
 					continue;
 				}
-				loopLinks_.push_back(iter->second);
 				if(isUnique)
 					++totalLocalTime;
 			}
@@ -7577,7 +7561,6 @@ void DatabaseViewer::updateGraphView()
 					links.erase(iter++);
 					continue;
 				}
-				loopLinks_.push_back(iter->second);
 				if(isUnique)
 					++totalUser;
 			}
@@ -7593,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;

@@ -7621,10 +7603,6 @@ void DatabaseViewer::updateGraphView()
 				if(isUnique)
 					++totalGravity;
 			}
-			else
-			{
-				loopLinks_.push_back(iter->second);
-			}
 			++iter;
 		}
 		updateLoopClosuresSlider();
@@ -9113,7 +9091,6 @@ void DatabaseViewer::rejectConstraint()
 		if(priorId==0)
 		{
 			this->updateGraphView();
-			updateLoopClosuresSlider();
 		}
 		else
 		{
@@ -9159,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;
 		}
@@ -9176,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);
@@ -9196,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);
 		}