increased graph::getConnectedGraph() performance

corelib/src/Graph.cpp

@@ -236,6 +236,13 @@ void Optimizer::getConnectedGraph(
 	std::set<int> nextDepth;
 	nextDepth.insert(fromId);
 	int d = 0;
+	std::multimap<int, int> biLinks;
+	for(std::multimap<int, Link>::const_iterator iter=linksIn.begin(); iter!=linksIn.end(); ++iter)
+	{
+		biLinks.insert(std::make_pair(iter->second.from(), iter->second.to()));
+		biLinks.insert(std::make_pair(iter->second.to(), iter->second.from()));
+	}
+
 	while((depth == 0 || d < depth) && nextDepth.size())
 	{
 		curentDepth = nextDepth;
@@ -248,39 +255,22 @@ void Optimizer::getConnectedGraph(
 				ids.insert(*jter);
 				posesOut.insert(*posesIn.find(*jter));
 
-				for(std::multimap<int, Link>::const_iterator iter=linksIn.begin(); iter!=linksIn.end(); ++iter)
+				for(std::multimap<int, int>::const_iterator iter=biLinks.find(*jter); iter!=biLinks.end() && iter->first==*jter; ++iter)
 				{
-					if(iter->second.from() == *jter)
+					int nextId = iter->second;
+					if(ids.find(nextId) == ids.end() && uContains(posesIn, nextId))
 					{
-						if(ids.find(iter->second.to()) == ids.end() && uContains(posesIn, iter->second.to()))
+						nextDepth.insert(nextId);
-						{
-							nextDepth.insert(iter->second.to());
-							if(depth == 0 || d < depth-1)
-							{
-								linksOut.insert(*iter);
-							}
-							else if(curentDepth.find(iter->second.to()) != curentDepth.end() ||
-									ids.find(iter->second.to()) != ids.end())
-							{
-								linksOut.insert(*iter);
-							}
-						}
-					}
-					else if(iter->second.to() == *jter)
-					{
-						if(ids.find(iter->second.from()) == ids.end() && uContains(posesIn, iter->second.from()))
-						{
-							nextDepth.insert(iter->second.from());
 
+						std::map<int, Link>::const_iterator kter = graph::findLink(linksIn, *jter, nextId);
 						if(depth == 0 || d < depth-1)
 						{
-								linksOut.insert(*iter);
+							linksOut.insert(*kter);
 						}
-							else if(curentDepth.find(iter->second.from()) != curentDepth.end() ||
+						else if(curentDepth.find(nextId) != curentDepth.end() ||
-									ids.find(iter->second.from()) != ids.end())
+								ids.find(nextId) != ids.end())
 						{
-								linksOut.insert(*iter);
+							linksOut.insert(*kter);
-							}
 						}
 					}
 				}

guilib/src/DatabaseViewer.cpp

@@ -2679,13 +2679,21 @@ void DatabaseViewer::sliderIterationsValueChanged(int value)
 	if(memory_ && value >=0 && value < (int)graphes_.size())
 	{
 		std::map<int, rtabmap::Transform> & graph = uValueAt(graphes_, value);
-		if(ui_->dockWidget_graphView->isVisible() && localMaps_.size() == 0)
+		std::map<int, rtabmap::Transform> graphFiltered = graph;
+		if(ui_->groupBox_posefiltering->isChecked())
+		{
+			graphFiltered = graph::radiusPosesFiltering(graph,
+					ui_->doubleSpinBox_posefilteringRadius->value(),
+					ui_->doubleSpinBox_posefilteringAngle->value()*CV_PI/180.0);
+		}
+		if(ui_->dockWidget_graphView->isVisible())
 		{
 			//update scans
 			UINFO("Update local maps list...");
-
+			std::vector<int> ids = uKeys(graphFiltered);
-			std::vector<int> ids = uKeys(graph);
 			for(unsigned int i=0; i<ids.size(); ++i)
+			{
+				if(localMaps_.find(ids[i]) == localMaps_.end())
 				{
 					UTimer time;
 					bool added = false;
@@ -2748,6 +2756,7 @@ void DatabaseViewer::sliderIterationsValueChanged(int value)
 						UINFO("Processed grid map %d/%d (%fs)", i+1, (int)ids.size(), time.ticks());
 					}
 				}
+			}
 			UINFO("Update local maps list... done");
 		}
 
@@ -2759,17 +2768,7 @@ void DatabaseViewer::sliderIterationsValueChanged(int value)
 			cv::Mat map;
 			QTime time;
 			time.start();
-			if(ui_->groupBox_posefiltering->isChecked())
-			{
-				std::map<int, rtabmap::Transform> graphFiltered = graph::radiusPosesFiltering(graph,
-						ui_->doubleSpinBox_posefilteringRadius->value(),
-						ui_->doubleSpinBox_posefilteringAngle->value()*CV_PI/180.0);
 			map = rtabmap::util3d::create2DMapFromOccupancyLocalMaps(graphFiltered, localMaps_, cell, xMin, yMin, 0, ui_->checkBox_gridErode->isChecked());
-			}
-			else
-			{
-				map = rtabmap::util3d::create2DMapFromOccupancyLocalMaps(graph, localMaps_, cell, xMin, yMin, 0, ui_->checkBox_gridErode->isChecked());
-			}
 			if(!map.empty())
 			{
 				ui_->graphViewer->updateMap(rtabmap::util3d::convertMap2Image8U(map), cell, xMin, yMin);
@@ -2966,6 +2965,7 @@ void DatabaseViewer::updateGraphView()
 		}
 		std::map<int, rtabmap::Transform> posesOut;
 		std::multimap<int, rtabmap::Link> linksOut;
+		UINFO("Get connected graph (%d poses, %d links)", (int)poses.size(), (int)links.size());
 		optimizer->getConnectedGraph(
 				fromId,
 				poses,
@@ -2973,7 +2973,7 @@ void DatabaseViewer::updateGraphView()
 				posesOut,
 				linksOut,
 				ui_->spinBox_optimizationDepth->value());
-
+		UINFO("Connected graph of %d poses and %d links", (int)posesOut.size(), (int)linksOut.size());
 		QTime time;
 		time.start();
 		std::map<int, rtabmap::Transform> finalPoses = optimizer->optimize(fromId, posesOut, linksOut, &graphes_);