Added rtabmap::computePath() using a pose goal instead of node Id. Graph methods moved in rtabmap::graph namespace

corelib/include/rtabmap/core/Graph.h

@@ -36,6 +36,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 namespace rtabmap {
 
+namespace graph {
+
 std::multimap<int, Link>::iterator RTABMAP_EXP findLink(
 		std::multimap<int, Link> & links,
 		int from,
@@ -107,5 +109,17 @@ std::vector<int> RTABMAP_EXP computePath(
 			int from,
 			int to);
 
+int RTABMAP_EXP findNearestNode(
+		const std::map<int, rtabmap::Transform> & nodes,
+		const rtabmap::Transform & targetPose);
+
+std::map<int, float> RTABMAP_EXP getNodesInRadius(
+		int nodeId,
+		const std::map<int, Transform> & nodes,
+		int maxNearestNeighbors,
+		float radius);
+
+} /* namespace graph */
+
 } /* namespace rtabmap */
 #endif /* GRAPH_H_ */

corelib/include/rtabmap/core/Rtabmap.h

@@ -118,11 +118,13 @@ public:
 			bool optimized,
 			bool global);
 	void clearPath();
-	std::list<std::pair<int, Transform> > computePath(int targetNode);
+	std::list<std::pair<int, Transform> > computePath(int targetNode, bool global);
+	std::list<std::pair<int, Transform> > computePath(const Transform & targetPose, bool global);
 	const std::vector<int> & getPath() const {return _path;}
 	std::list<std::pair<int, Transform> > getPathNextPoses() const;
 	std::vector<int> getPathNextNodes() const;
-	int getPathGoalId() const;
+	int getPathCurrentGoalId() const;
+	const Transform & getPathTransformToGoal() const {return _pathTransformToGoal;}
 
 	std::map<int, float> getNodesInRadius(int fromId, int maxNearestNeighbors, float radius) const;
 	std::map<int, Transform> getWMPosesInRadius(int fromId, int maxNearestNeighbors, float radius, int maxDiffID, int & nearestId) const;
@@ -136,6 +138,7 @@ private:
 			std::map<int, Transform> & optimizedPoses,
 			std::multimap<int, Link> * constraints = 0) const;
 	void updateGoalIndex();
+	bool computePath(int targetNode, const std::map<int, Transform> & nodes, const std::multimap<int, rtabmap::Link> & constraints);
 
 	void setupLogFiles(bool overwrite = false);
 	void flushStatisticLogs();
@@ -209,6 +212,7 @@ private:
 	std::vector<int> _path;
 	unsigned int _pathCurrentIndex;
 	unsigned int _pathGoalIndex;
+	Transform _pathTransformToGoal;
 
 };
 

corelib/src/Graph.cpp

@@ -39,6 +39,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 namespace rtabmap {
 
+namespace graph {
+
 std::multimap<int, Link>::iterator findLink(
 		std::multimap<int, Link> & links,
 		int from,
@@ -753,5 +755,85 @@ std::vector<int> computePath(
 	return uListToVector(path);
 }
 
+int findNearestNode(
+		const std::map<int, rtabmap::Transform> & nodes,
+		const rtabmap::Transform & targetPose)
+{
+	int id = 0;
+	if(nodes.size() && !targetPose.isNull())
+	{
+		pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
+		cloud->resize(nodes.size());
+		std::vector<int> ids(nodes.size());
+		int oi = 0;
+		for(std::map<int, Transform>::const_iterator iter = nodes.begin(); iter!=nodes.end(); ++iter)
+		{
+			(*cloud)[oi] = pcl::PointXYZ(iter->second.x(), iter->second.y(), iter->second.z());
+			ids[oi++] = iter->first;
+		}
+
+		std::map<int, float> foundNodes;
+		if(cloud->size())
+		{
+			pcl::search::KdTree<pcl::PointXYZ>::Ptr kdTree(new pcl::search::KdTree<pcl::PointXYZ>);
+			kdTree->setInputCloud(cloud);
+			std::vector<int> ind;
+			std::vector<float> dist;
+			pcl::PointXYZ pt(targetPose.x(), targetPose.y(), targetPose.z());
+			kdTree->nearestKSearch(pt, 1, ind, dist);
+			if(ind.size() && dist.size() && ind[0] >= 0)
+			{
+				UDEBUG("Nearest node = %d: %f", ids[ind[0]], dist[0]);
+				id = ids[ind[0]];
+			}
+		}
+	}
+	return id;
+}
+
+// return <id, distance>, including query
+std::map<int, float> getNodesInRadius(
+		int nodeId,
+		const std::map<int, Transform> & nodes,
+		int maxNearestNeighbors,
+		float radius)
+{
+	UASSERT(uContains(nodes, nodeId));
+
+	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
+	cloud->resize(nodes.size());
+	std::vector<int> ids(nodes.size());
+	int oi = 0;
+	for(std::map<int, Transform>::const_iterator iter = nodes.begin(); iter!=nodes.end(); ++iter)
+	{
+		(*cloud)[oi] = pcl::PointXYZ(iter->second.x(), iter->second.y(), iter->second.z());
+		ids[oi++] = iter->first;
+	}
+
+	Transform fromT = nodes.at(nodeId);
+
+	std::map<int, float> foundNodes;
+	if(cloud->size())
+	{
+		pcl::search::KdTree<pcl::PointXYZ>::Ptr kdTree(new pcl::search::KdTree<pcl::PointXYZ>);
+		kdTree->setInputCloud(cloud);
+		std::vector<int> ind;
+		std::vector<float> dist;
+		pcl::PointXYZ pt(fromT.x(), fromT.y(), fromT.z());
+		kdTree->radiusSearch(pt, radius, ind, dist, maxNearestNeighbors);
+		for(unsigned int i=0; i<ind.size(); ++i)
+		{
+			if(ind[i] >=0)
+			{
+				UDEBUG("Inlier %d: %f", ids[ind[i]], dist[i]);
+				foundNodes.insert(std::make_pair(ids[ind[i]], dist[i]));
+			}
+		}
+	}
+	UDEBUG("found nodes=%d", (int)foundNodes.size());
+	return foundNodes;
+}
+
+} /* namespace graph */
 
 } /* namespace rtabmap */

corelib/src/Rtabmap.cpp

@@ -121,7 +121,8 @@ Rtabmap::Rtabmap() :
 	_mapCorrection(Transform::getIdentity()),
 	_mapTransform(Transform::getIdentity()),
 	_pathCurrentIndex(0),
-	_pathGoalIndex(0)
+	_pathGoalIndex(0),
+	_pathTransformToGoal(Transform::getIdentity())
 {
 }
 
@@ -643,7 +644,7 @@ void Rtabmap::generateTOROGraph(const std::string & path, bool optimized, bool g
 			_memory->getMetricConstraints(uKeys(ids), poses, constraints, global);
 		}
 
-		rtabmap::saveTOROGraph(path, poses, constraints);
+		rtabmap::graph::saveTOROGraph(path, poses, constraints);
 	}
 }
 
@@ -1895,55 +1896,6 @@ void Rtabmap::dumpData() const
 	}
 }
 
-// fromId must be in _memory and in _optimizedPoses
-// return <id, distance>
-std::map<int, float> Rtabmap::getNodesInRadius(
-		int fromId,
-		int maxNearestNeighbors,
-		float radius) const
-{
-	UDEBUG("");
-	const Signature * fromS = _memory->getSignature(fromId);
-	UASSERT(fromS != 0);
-
-	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
-	cloud->resize(_optimizedPoses.size());
-	std::vector<int> ids(_optimizedPoses.size());
-	int oi = 0;
-	for(std::map<int, Transform>::const_iterator iter = _optimizedPoses.begin(); iter!=_optimizedPoses.end(); ++iter)
-	{
-		(*cloud)[oi] = pcl::PointXYZ(iter->second.x(), iter->second.y(), iter->second.z());
-		ids[oi++] = iter->first;
-	}
-
-	cloud->resize(oi);
-	ids.resize(oi);
-
-	UASSERT(_optimizedPoses.find(fromId) != _optimizedPoses.end());
-	Transform fromT = _optimizedPoses.at(fromId);
-
-	std::map<int, float> nodes;
-	if(cloud->size())
-	{
-		pcl::search::KdTree<pcl::PointXYZ>::Ptr kdTree(new pcl::search::KdTree<pcl::PointXYZ>);
-		kdTree->setInputCloud(cloud);
-		std::vector<int> ind;
-		std::vector<float> dist;
-		pcl::PointXYZ pt(fromT.x(), fromT.y(), fromT.z());
-		kdTree->radiusSearch(pt, radius, ind, dist, maxNearestNeighbors);
-		for(unsigned int i=0; i<ind.size(); ++i)
-		{
-			if(ind[i] >=0)
-			{
-				UDEBUG("Inlier %d: %f", ids[ind[i]], dist[i]);
-				nodes.insert(std::make_pair(ids[ind[i]], dist[i]));
-			}
-		}
-	}
-	UDEBUG("nodes=%d", (int)nodes.size());
-	return nodes;
-}
-
 // fromId must be in _memory and in _optimizedPoses
 // Get poses in front of the robot
 std::map<int, Transform> Rtabmap::getWMPosesInRadius(
@@ -2107,7 +2059,7 @@ void Rtabmap::optimizeCurrentMap(
 		}
 		else
 		{
-			rtabmap::optimizeTOROGraph(ids, poses, edgeConstraints, optimizedPoses, _toroIterations, true, _toroIgnoreVariance);
+			rtabmap::graph::optimizeTOROGraph(ids, poses, edgeConstraints, optimizedPoses, _toroIterations, true, _toroIgnoreVariance);
 		}
 	}
 }
@@ -2331,50 +2283,39 @@ void Rtabmap::clearPath()
 	_path.clear();
 	_pathCurrentIndex=0;
 	_pathGoalIndex = 0;
+	_pathTransformToGoal.setIdentity();
 	if(_memory)
 	{
 		_memory->removeAllVirtualLinks();
 	}
 }
 
-// return true if path is updated
+bool Rtabmap::computePath(
-std::list<std::pair<int, Transform> > Rtabmap::computePath(int targetNode)
+		int targetNode,
+		const std::map<int, Transform> & nodes,
+		const std::multimap<int, rtabmap::Link> & constraints)
 {
-	this->clearPath();
-	std::list<std::pair<int, Transform> > pathPoses;
-
-	if(!_rgbdSlamMode)
-	{
-		UWARN("A path can only be computed in RGBD-SLAM mode");
-		return pathPoses;
-	}
-
 	if(_memory->getWorkingMem().size() <= 1 || !_memory->getLastWorkingSignature()) // ignore virtual place
 	{
 		UWARN("Working memory is empty... cannot compute a path");
-		return pathPoses;
+		return false;
 	}
 	int currentNode = _memory->getLastWorkingSignature()->id();
 
-	UTimer timer;
+	if(!uContains(nodes, currentNode))
-	std::map<int, Transform> globalGraph;
-	std::multimap<int, Link> constraints;
-	std::map<int, int> mapIds;
-	this->getGraph(globalGraph, constraints, mapIds, true, true);
-
-	if(!uContains(globalGraph, currentNode))
 	{
-		UWARN("Last signature %d not found in the global graph! Cannot compute a path", currentNode);
+		UWARN("Last signature %d not found in the graph! Cannot compute a path", currentNode);
-		return pathPoses;
+		return false;
 	}
-	if(!uContains(globalGraph, targetNode))
+
+	if(!uContains(nodes, targetNode))
 	{
-		UWARN("Goal %d not found in the global graph! Cannot compute a path", targetNode);
+		UWARN("Goal %d not found in the graph! Cannot compute a path", targetNode);
-		return pathPoses;
+		return false;
 	}
 
 	std::multimap<int, int> links;
-	for(std::multimap<int, rtabmap::Link>::iterator iter=constraints.begin(); iter!=constraints.end(); ++iter)
+	for(std::multimap<int, rtabmap::Link>::const_iterator iter=constraints.begin(); iter!=constraints.end(); ++iter)
 	{
 		links.insert(std::make_pair(iter->first, iter->second.to()));
 		links.insert(std::make_pair(iter->second.to(), iter->first)); // <->
@@ -2383,11 +2324,8 @@ std::list<std::pair<int, Transform> > Rtabmap::computePath(int targetNode)
 	//std::multimap<int, int> clusters = rtabmap::radiusPosesClustering(globalGraph, _goalMetricError, CV_PI);
 	//links.insert(clusters.begin(), clusters.end());
 
-	UINFO("Time creating global graph = %fs", timer.ticks());
-
 	UINFO("Computing path from location %d to %d", currentNode, targetNode);
-	_path = rtabmap::computePath(globalGraph, links, currentNode, targetNode);
+	_path = rtabmap::graph::computePath(nodes, links, currentNode, targetNode);
-	UINFO("Time computing path = %fs", timer.ticks());
 
 	if(_path.size()<2)
 	{
@@ -2412,11 +2350,83 @@ std::list<std::pair<int, Transform> > Rtabmap::computePath(int targetNode)
 		}
 	}
 
-	updateGoalIndex();
+	return _path.size()>0;
+}
 
-	for(unsigned int i = 0; i<_path.size(); ++i)
+// return true if path is updated
+std::list<std::pair<int, Transform> > Rtabmap::computePath(int targetNode, bool global)
+{
+	this->clearPath();
+	std::list<std::pair<int, Transform> > pathPoses;
+
+	if(!_rgbdSlamMode)
 	{
-		pathPoses.push_back(std::make_pair(_path[i], globalGraph.at(_path[i])));
+		UWARN("A path can only be computed in RGBD-SLAM mode");
+		return pathPoses;
+	}
+
+	UTimer timer;
+	std::map<int, Transform> nodes;
+	std::multimap<int, Link> constraints;
+	std::map<int, int> mapIds;
+	this->getGraph(nodes, constraints, mapIds, true, global);
+	UINFO("Time creating graph (global=%s) = %fs", global?"true":"false", timer.ticks());
+
+	if(computePath(targetNode, nodes, constraints))
+	{
+		updateGoalIndex();
+
+		for(unsigned int i = 0; i<_path.size(); ++i)
+		{
+			pathPoses.push_back(std::make_pair(_path[i], nodes.at(_path[i])));
+		}
+	}
+	UINFO("Time computing path = %fs", timer.ticks());
+
+	return pathPoses;
+}
+
+std::list<std::pair<int, Transform> > Rtabmap::computePath(const Transform & targetPose, bool global)
+{
+	this->clearPath();
+	std::list<std::pair<int, Transform> > pathPoses;
+
+	if(!_rgbdSlamMode)
+	{
+		UWARN("This method can only be used in RGBD-SLAM mode");
+		return pathPoses;
+	}
+
+	//Find the nearest node
+	UTimer timer;
+	std::map<int, Transform> nodes;
+	std::multimap<int, Link> constraints;
+	std::map<int, int> mapIds;
+	this->getGraph(nodes, constraints, mapIds, true, global);
+	UINFO("Time creating graph (global=%s) = %fs", global?"true":"false", timer.ticks());
+
+	int nearestId = rtabmap::graph::findNearestNode(nodes, targetPose);
+	UINFO("Nearest node found=%d ,%fs", nearestId, timer.ticks());
+	if(nearestId > 0)
+	{
+		if(computePath(nearestId, nodes, constraints))
+		{
+			UASSERT(_path.size() > 0);
+			UASSERT(uContains(nodes, _path.back()));
+			_pathTransformToGoal = nodes.at(_path.back()).inverse() * targetPose;
+
+			updateGoalIndex();
+			for(unsigned int i = 0; i<_path.size(); ++i)
+			{
+				pathPoses.push_back(std::make_pair(_path[i], nodes.at(_path[i])));
+			}
+			pathPoses.back().second *= _pathTransformToGoal;
+		}
+		UINFO("Time computing path = %fs", timer.ticks());
+	}
+	else
+	{
+		UWARN("Nearest node not found in graph (size=%d) for pose %s", (int)nodes.size(), targetPose.prettyPrint().c_str());
 	}
 
 	return pathPoses;
@@ -2469,7 +2479,7 @@ std::vector<int> Rtabmap::getPathNextNodes() const
 	return ids;
 }
 
-int Rtabmap::getPathGoalId() const
+int Rtabmap::getPathCurrentGoalId() const
 {
 	if(_path.size())
 	{
@@ -2501,7 +2511,7 @@ void Rtabmap::updateGoalIndex()
 		if(uContains(_optimizedPoses, goalId))
 		{
 			//use local position to know if the goal is reached
-			float d = _optimizedPoses.at(_memory->getLastWorkingSignature()->id()).getDistance(_optimizedPoses.at(goalId));
+			float d = _optimizedPoses.at(_memory->getLastWorkingSignature()->id()).getDistance(_optimizedPoses.at(goalId)*_pathTransformToGoal);
 			if(d < _goalReachedRadius)
 			{
 				UINFO("Goal %d reached!", goalId);
@@ -2521,7 +2531,7 @@ void Rtabmap::updateGoalIndex()
 					break;
 				}
 			}
-			UASSERT(_pathGoalIndex <= _path.size() && goalIndex >= 0 && goalIndex <= (int)_path.size());
+			UASSERT(_pathGoalIndex < _path.size() && goalIndex >= 0 && goalIndex < (int)_path.size());
 			if((int)_pathGoalIndex != goalIndex)
 			{
 				UINFO("Updated current goal from %d to %d (%d/%d)",

guilib/src/DatabaseViewer.cpp

@@ -241,7 +241,7 @@ void DatabaseViewer::closeEvent(QCloseEvent* event)
 			// Added links
 			for(std::multimap<int, rtabmap::Link>::iterator iter=linksAdded_.begin(); iter!=linksAdded_.end(); ++iter)
 			{
-				std::multimap<int, rtabmap::Link>::iterator refinedIter = rtabmap::findLink(linksRefined_, iter->second.from(), iter->second.to());
+				std::multimap<int, rtabmap::Link>::iterator refinedIter = rtabmap::graph::findLink(linksRefined_, iter->second.from(), iter->second.to());
 				if(refinedIter != linksRefined_.end())
 				{
 					memory_->addLink(refinedIter->second.to(), refinedIter->second.from(), refinedIter->second.transform(), refinedIter->second.type(), refinedIter->second.variance());
@@ -574,7 +574,7 @@ void DatabaseViewer::generateTOROGraph()
 		QString path = QFileDialog::getSaveFileName(this, tr("Save File"), pathDatabase_+"/constraints" + QString::number(id) + ".graph", tr("TORO file (*.graph)"));
 		if(!path.isEmpty())
 		{
-			rtabmap::saveTOROGraph(path.toStdString(), uValueAt(graphes_, id), links);
+			rtabmap::graph::saveTOROGraph(path.toStdString(), uValueAt(graphes_, id), links);
 		}
 	}
 }
@@ -806,7 +806,7 @@ void DatabaseViewer::detectMoreLoopClosures()
 	for(int n=0; n<iterations; ++n)
 	{
 		UINFO("iteration %d/%d", n+1, iterations);
-		std::multimap<int, int> clusters = rtabmap::radiusPosesClustering(
+		std::multimap<int, int> clusters = rtabmap::graph::radiusPosesClustering(
 				optimizedPoses,
 				ui_->doubleSpinBox_detectMore_radius->value(),
 				ui_->doubleSpinBox_detectMore_angle->value()*CV_PI/180.0);
@@ -1418,7 +1418,7 @@ void DatabaseViewer::updateConstraintView(const rtabmap::Link & linkIn,
 		const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloudTo,
 		bool updateImageSliders)
 {
-	std::multimap<int, Link>::iterator iter = rtabmap::findLink(linksRefined_, linkIn.from(), linkIn.to());
+	std::multimap<int, Link>::iterator iter = rtabmap::graph::findLink(linksRefined_, linkIn.from(), linkIn.to());
 	rtabmap::Link link = linkIn;
 	if(iter != linksRefined_.end())
 	{
@@ -1701,7 +1701,7 @@ void DatabaseViewer::updateConstraintButtons()
 
 		//check for modified link
 		bool modified = false;
-		std::multimap<int, Link>::iterator iter = rtabmap::findLink(linksRefined_, currentLink.from(), currentLink.to());
+		std::multimap<int, Link>::iterator iter = rtabmap::graph::findLink(linksRefined_, currentLink.from(), currentLink.to());
 		if(iter != linksRefined_.end())
 		{
 			currentLink = iter->second;
@@ -1791,8 +1791,8 @@ void DatabaseViewer::updateGraphView()
 		graphes_.push_back(poses_);
 		ui_->actionGenerate_TORO_graph_graph->setEnabled(true);
 		std::multimap<int, rtabmap::Link> links = updateLinksWithModifications(links_);
-		std::map<int, int> depthGraph = rtabmap::generateDepthGraph(links, ui_->spinBox_optimizationsFrom->value(), 0);
+		std::map<int, int> depthGraph = rtabmap::graph::generateDepthGraph(links, ui_->spinBox_optimizationsFrom->value(), 0);
-		rtabmap::optimizeTOROGraph(
+		rtabmap::graph::optimizeTOROGraph(
 				depthGraph,
 				poses_,
 				links, finalPoses,
@@ -1818,21 +1818,21 @@ void DatabaseViewer::updateGraphView()
 Link DatabaseViewer::findActiveLink(int from, int to)
 {
 	Link link;
-	std::multimap<int, Link>::iterator findIter = rtabmap::findLink(linksRefined_, from ,to);
+	std::multimap<int, Link>::iterator findIter = rtabmap::graph::findLink(linksRefined_, from ,to);
 	if(findIter != linksRefined_.end())
 	{
 		link = findIter->second;
 	}
 	else
 	{
-		findIter = rtabmap::findLink(linksAdded_, from ,to);
+		findIter = rtabmap::graph::findLink(linksAdded_, from ,to);
 		if(findIter != linksAdded_.end())
 		{
 			link = findIter->second;
 		}
 		else if(!containsLink(linksRemoved_, from ,to))
 		{
-			findIter = rtabmap::findLink(links_, from ,to);
+			findIter = rtabmap::graph::findLink(links_, from ,to);
 			if(findIter != links_.end())
 			{
 				link = findIter->second;
@@ -1844,7 +1844,7 @@ Link DatabaseViewer::findActiveLink(int from, int to)
 
 bool DatabaseViewer::containsLink(std::multimap<int, Link> & links, int from, int to)
 {
-	return rtabmap::findLink(links, from, to) != links.end();
+	return rtabmap::graph::findLink(links, from, to) != links.end();
 }
 
 void DatabaseViewer::refineConstraint()
@@ -2282,7 +2282,7 @@ bool DatabaseViewer::addConstraint(int from, int to, bool silent, bool updateGra
 	else if(containsLink(linksRemoved_, from, to))
 	{
 		//simply remove from linksRemoved
-		linksRemoved_.erase(rtabmap::findLink(linksRemoved_, from, to));
+		linksRemoved_.erase(rtabmap::graph::findLink(linksRemoved_, from, to));
 		updateSlider = true;
 	}
 
@@ -2315,19 +2315,19 @@ void DatabaseViewer::resetConstraint()
 	}
 
 
-	std::multimap<int, Link>::iterator iter = rtabmap::findLink(linksRefined_, from, to);
+	std::multimap<int, Link>::iterator iter = rtabmap::graph::findLink(linksRefined_, from, to);
 	if(iter != linksRefined_.end())
 	{
 		linksRefined_.erase(iter);
 		this->updateGraphView();
 	}
 
-	iter = rtabmap::findLink(links_, from, to);
+	iter = rtabmap::graph::findLink(links_, from, to);
 	if(iter != links_.end())
 	{
 		this->updateConstraintView(iter->second);
 	}
-	iter = rtabmap::findLink(linksAdded_, from, to);
+	iter = rtabmap::graph::findLink(linksAdded_, from, to);
 	if(iter != linksAdded_.end())
 	{
 		this->updateConstraintView(iter->second);
@@ -2355,7 +2355,7 @@ void DatabaseViewer::rejectConstraint()
 
 	// find the original one
 	std::multimap<int, Link>::iterator iter;
-	iter = rtabmap::findLink(links_, from, to);
+	iter = rtabmap::graph::findLink(links_, from, to);
 	if(iter != links_.end())
 	{
 		if(iter->second.type() == Link::kNeighbor)
@@ -2368,13 +2368,13 @@ void DatabaseViewer::rejectConstraint()
 	}
 
 	// remove from refined and added
-	iter = rtabmap::findLink(linksRefined_, from, to);
+	iter = rtabmap::graph::findLink(linksRefined_, from, to);
 	if(iter != linksRefined_.end())
 	{
 		linksRefined_.erase(iter);
 		removed = true;
 	}
-	iter = rtabmap::findLink(linksAdded_, from, to);
+	iter = rtabmap::graph::findLink(linksAdded_, from, to);
 	if(iter != linksAdded_.end())
 	{
 		linksAdded_.erase(iter);
@@ -2397,7 +2397,7 @@ std::multimap<int, rtabmap::Link> DatabaseViewer::updateLinksWithModifications(
 	{
 		std::multimap<int, rtabmap::Link>::iterator findIter;
 
-		findIter = rtabmap::findLink(linksRemoved_, iter->second.from(), iter->second.to());
+		findIter = rtabmap::graph::findLink(linksRemoved_, iter->second.from(), iter->second.to());
 		if(findIter != linksRemoved_.end())
 		{
 			if(!(iter->second.from() == findIter->second.from() &&
@@ -2415,7 +2415,7 @@ std::multimap<int, rtabmap::Link> DatabaseViewer::updateLinksWithModifications(
 			}
 		}
 
-		findIter = rtabmap::findLink(linksRefined_, iter->second.from(), iter->second.to());
+		findIter = rtabmap::graph::findLink(linksRefined_, iter->second.from(), iter->second.to());
 		if(findIter!=linksRefined_.end())
 		{
 			if(iter->second.from() == findIter->second.from() &&

guilib/src/MainWindow.cpp

@@ -1207,7 +1207,7 @@ void MainWindow::updateMapCloud(
 	{
 		float radius = _preferencesDialog->getCloudFilteringRadius();
 		float angle = _preferencesDialog->getCloudFilteringAngle()*CV_PI/180.0; // convert to rad
-		poses = rtabmap::radiusPosesFiltering(posesIn, radius, angle);
+		poses = rtabmap::graph::radiusPosesFiltering(posesIn, radius, angle);
 		// make sure the last is here
 		poses.insert(*posesIn.rbegin());
 		for(std::map<int, Transform>::iterator iter= poses.begin(); iter!=poses.end(); ++iter)
@@ -2996,7 +2996,7 @@ void MainWindow::postProcessing()
 			_initProgressDialog->appendText(tr("Looking for more loop closures, clustering poses... (iteration=%1/%2, radius=%3 m angle=%4 degrees)")
 					.arg(n+1).arg(detectLoopClosureIterations).arg(clusterRadius).arg(clusterAngle));
 
-			std::multimap<int, int> clusters = rtabmap::radiusPosesClustering(
+			std::multimap<int, int> clusters = rtabmap::graph::radiusPosesClustering(
 					_currentPosesMap,
 					clusterRadius,
 					clusterAngle*CV_PI/180.0);
@@ -3018,7 +3018,7 @@ void MainWindow::postProcessing()
 
 				// only add new links and one per cluster per iteration
 				if(addedLinks.find(from) == addedLinks.end() && addedLinks.find(to) == addedLinks.end() &&
-				   rtabmap::findLink(_currentLinksMap, from, to) == _currentLinksMap.end())
+				   rtabmap::graph::findLink(_currentLinksMap, from, to) == _currentLinksMap.end())
 				{
 					if(!_cachedSignatures.contains(from))
 					{
@@ -3101,8 +3101,8 @@ void MainWindow::postProcessing()
 				_initProgressDialog->appendText(tr("Optimizing graph with new links (%1 nodes, %2 constraints)...")
 						.arg(odomPoses.size()).arg(_currentLinksMap.size()));
 				std::map<int, rtabmap::Transform> optimizedPoses;
-				std::map<int, int> depthGraph = rtabmap::generateDepthGraph(_currentLinksMap, toroOptimizeFromGraphEnd?odomPoses.rbegin()->first:odomPoses.begin()->first);
+				std::map<int, int> depthGraph = rtabmap::graph::generateDepthGraph(_currentLinksMap, toroOptimizeFromGraphEnd?odomPoses.rbegin()->first:odomPoses.begin()->first);
-				rtabmap::optimizeTOROGraph(depthGraph, odomPoses, _currentLinksMap, optimizedPoses, toroIterations, true, ignoreVariance);
+				rtabmap::graph::optimizeTOROGraph(depthGraph, odomPoses, _currentLinksMap, optimizedPoses, toroIterations, true, ignoreVariance);
 				_currentPosesMap = optimizedPoses;
 				_initProgressDialog->appendText(tr("Optimizing graph with new links... done!"));
 			}
@@ -3256,8 +3256,8 @@ void MainWindow::postProcessing()
 	_initProgressDialog->appendText(tr("Optimizing graph with updated links (%1 nodes, %2 constraints)...")
 			.arg(odomPoses.size()).arg(_currentLinksMap.size()));
 	std::map<int, rtabmap::Transform> optimizedPoses;
-	std::map<int, int> depthGraph = rtabmap::generateDepthGraph(_currentLinksMap, toroOptimizeFromGraphEnd?odomPoses.rbegin()->first:odomPoses.begin()->first);
+	std::map<int, int> depthGraph = rtabmap::graph::generateDepthGraph(_currentLinksMap, toroOptimizeFromGraphEnd?odomPoses.rbegin()->first:odomPoses.begin()->first);
-	rtabmap::optimizeTOROGraph(depthGraph, odomPoses, _currentLinksMap, optimizedPoses, toroIterations, true, ignoreVariance);
+	rtabmap::graph::optimizeTOROGraph(depthGraph, odomPoses, _currentLinksMap, optimizedPoses, toroIterations, true, ignoreVariance);
 	_initProgressDialog->appendText(tr("Optimizing graph with updated links... done!"));
 	_initProgressDialog->incrementStep();