rtabmap::getGraph() returning mapIds too

git-svn-id: http://rtabmap.googlecode.com/svn/trunk/rtabmap@1681 f169173b-cf89-36c8-b27e-44dbe73f0c83

corelib/include/rtabmap/core/Rtabmap.h

@@ -121,6 +121,7 @@ public:
 			bool global) const;
 	void getGraph(std::map<int, Transform> & poses,
 			std::multimap<int, Link> & constraints,
+			std::map<int, int> & mapIds,
 			bool optimized,
 			bool global);
 

corelib/src/Rtabmap.cpp

@@ -2326,6 +2326,7 @@ void Rtabmap::get3DMap(std::map<int, std::vector<unsigned char> > & images,
 void Rtabmap::getGraph(
 		std::map<int, Transform> & poses,
 		std::multimap<int, Link> & constraints,
+		std::map<int, int> & mapIds,
 		bool optimized,
 		bool global)
 {
@@ -2340,6 +2341,18 @@ void Rtabmap::getGraph(
 			std::map<int, int> ids = _memory->getNeighborsId(_memory->getLastWorkingSignature()->id(), 0, global?-1:0, true);
 			_memory->getMetricConstraints(uKeys(ids), poses, constraints, global);
 		}
+
+		std::set<int> ids = _memory->getWorkingMem(); // STM + WM
+		ids.insert(_memory->getStMem().begin(), _memory->getStMem().end());
+		if(global)
+		{
+			ids = _memory->getAllSignatureIds(); // STM + WM + LTM
+		}
+
+		for(std::set<int>::iterator iter = ids.begin(); iter!=ids.end(); ++iter)
+		{
+			mapIds.insert(std::make_pair(*iter, _memory->getMapId(*iter)));
+		}
 	}
 }
 

corelib/src/RtabmapThread.cpp

@@ -155,6 +155,7 @@ void RtabmapThread::publishTOROGraph(bool optimized, bool full) const
 
 	_rtabmap->getGraph(poses,
 			constraints,
+			mapIds,
 			optimized,
 			full);