fixed colored loop closure detection rect when no images are in GUI cache. Modified some logged info.

corelib/include/rtabmap/core/Rtabmap.h

@@ -128,7 +128,8 @@ public:
 			std::map<int, std::string> & labels,
 			std::map<int, std::vector<unsigned char> > & userDatas,
 			bool optimized,
-			bool global);
+			bool global,
+			bool posesConstraintsOnly = false);
 	void clearPath();
 	bool computePath(int targetNode, bool global);
 	bool computePath(const Transform & targetPose, bool global);

corelib/include/rtabmap/core/RtabmapThread.h

@@ -95,7 +95,7 @@ private:
 	void pushNewState(State newState, const ParametersMap & parameters = ParametersMap());
 	void setDataBufferSize(int size);
 	void publishMap(bool optimized, bool full) const;
-	void publishTOROGraph(bool optimized, bool full) const;
+	void publishGraph(bool optimized, bool full) const;
 
 private:
 	UMutex _stateMutex;

corelib/src/DBReader.cpp

@@ -185,8 +185,6 @@ void DBReader::mainLoop()
 
 		if(goalId > 0)
 		{
-			this->post(new RtabmapEventCmd(RtabmapEventCmd::kCmdGoal, "", goalId));
-
 			if(!_ignoreGoalDelay && _currentId != _ids.end())
 			{
 				// get stamp for the next signature to compute the delay
@@ -203,12 +201,19 @@ void DBReader::mainLoop()
 					double delay = stamp - previousStamp;
 					UWARN("Goal %d detected, posting it! Waiting %f seconds before sending next data...",
 							goalId, delay);
+					this->post(new RtabmapEventCmd(RtabmapEventCmd::kCmdGoal, "", goalId));
 					uSleep(delay*1000);
 				}
+				else
+				{
+					UWARN("Goal %d detected, posting it!", goalId);
+					this->post(new RtabmapEventCmd(RtabmapEventCmd::kCmdGoal, "", goalId));
+				}
 			}
 			else
 			{
 				UWARN("Goal %d detected, posting it!", goalId);
+				this->post(new RtabmapEventCmd(RtabmapEventCmd::kCmdGoal, "", goalId));
 			}
 		}
 

corelib/src/Rtabmap.cpp

@@ -2570,17 +2570,16 @@ void Rtabmap::optimizeCurrentMap(
 {
 	//Optimize the map
 	optimizedPoses.clear();
-	UDEBUG("Optimize map: around location %d", id);
+	UINFO("Optimize map: around location %d", id);
 	if(_memory && id > 0)
 	{
 		UTimer timer;
 		std::map<int, int> ids = _memory->getNeighborsId(id, 0, lookInDatabase?-1:0, true);
-		UDEBUG("get ids=%d", (int)ids.size());
 		if(!_optimizeFromGraphEnd && ids.size() > 1)
 		{
 			id = ids.begin()->first;
 		}
-		UINFO("get ids time %f s", timer.ticks());
+		UINFO("get %d ids time %f s", (int)ids.size(), timer.ticks());
 
 		optimizedPoses = Rtabmap::optimizeGraph(id, uKeysSet(ids), lookInDatabase, constraints);
 
@@ -2605,7 +2604,7 @@ std::map<int, Transform> Rtabmap::optimizeGraph(
 	std::multimap<int, Link> edgeConstraints;
 	UDEBUG("ids=%d", (int)ids.size());
 	_memory->getMetricConstraints(ids, poses, edgeConstraints, lookInDatabase);
-	UDEBUG("get constraints (%d poses, %d edges) time %f s", (int)poses.size(), (int)edgeConstraints.size(), timer.ticks());
+	UINFO("get constraints (%d poses, %d edges) time %f s", (int)poses.size(), (int)edgeConstraints.size(), timer.ticks());
 
 	if(constraints)
 	{
@@ -2622,6 +2621,7 @@ std::map<int, Transform> Rtabmap::optimizeGraph(
 	{
 		optimizedPoses = _graphOptimizer->optimize(fromId, poses, edgeConstraints);
 	}
+	UINFO("Optimization time %f s", timer.ticks());
 
 	return optimizedPoses;
 }
@@ -2817,7 +2817,8 @@ void Rtabmap::getGraph(
 		std::map<int, std::string> & labels,
 		std::map<int, std::vector<unsigned char> > & userDatas,
 		bool optimized,
-		bool global)
+		bool global,
+		bool posesConstraintsOnly)
 {
 	if(_memory && _memory->getLastWorkingSignature())
 	{
@@ -2840,19 +2841,22 @@ void Rtabmap::getGraph(
 			_memory->getMetricConstraints(uKeysSet(ids), poses, constraints, global);
 		}
 
-		for(std::map<int, Transform>::iterator iter=poses.begin(); iter!=poses.end(); ++iter)
+		if(!posesConstraintsOnly)
 		{
-			Transform odomPose;
+			for(std::map<int, Transform>::iterator iter=poses.begin(); iter!=poses.end(); ++iter)
-			int weight = -1;
+			{
-			int mapId = -1;
+				Transform odomPose;
-			std::string label;
+				int weight = -1;
-			double stamp = 0;
+				int mapId = -1;
-			std::vector<unsigned char> userData;
+				std::string label;
-			_memory->getNodeInfo(iter->first, odomPose, mapId, weight, label, stamp, userData, true);
+				double stamp = 0;
-			mapIds.insert(std::make_pair(iter->first, mapId));
+				std::vector<unsigned char> userData;
-			stamps.insert(std::make_pair(iter->first, stamp));
+				_memory->getNodeInfo(iter->first, odomPose, mapId, weight, label, stamp, userData, global);
-			labels.insert(std::make_pair(iter->first, label));
+				mapIds.insert(std::make_pair(iter->first, mapId));
-			userDatas.insert(std::make_pair(iter->first, userData));
+				stamps.insert(std::make_pair(iter->first, stamp));
+				labels.insert(std::make_pair(iter->first, label));
+				userDatas.insert(std::make_pair(iter->first, userData));
+			}
 		}
 	}
 	else if(_memory && (_memory->getStMem().size() || _memory->getWorkingMem().size()))
@@ -2991,6 +2995,7 @@ bool Rtabmap::computePath(
 // return true if path is updated
 bool Rtabmap::computePath(int targetNode, bool global)
 {
+	UINFO("Planning a path to node %d (global=%d)", targetNode, global?1:0);
 	this->clearPath();
 
 	if(!_rgbdSlamMode)
@@ -2999,6 +3004,7 @@ bool Rtabmap::computePath(int targetNode, bool global)
 		return false;
 	}
 
+	UTimer totalTimer;
 	UTimer timer;
 	std::map<int, Transform> nodes;
 	std::multimap<int, Link> constraints;
@@ -3013,13 +3019,16 @@ bool Rtabmap::computePath(int targetNode, bool global)
 	{
 		updateGoalIndex();
 	}
-	UINFO("Time computing path = %fs", timer.ticks());
+	UINFO("Time computing path (A*) = %fs", timer.ticks());
+	UINFO("Total planning time = %fs (%d nodes, %f m long)", totalTimer.ticks(), (int)_path.size(), graph::computePathLength(_path));
 
 	return _path.size()>0;
 }
 
 bool Rtabmap::computePath(const Transform & targetPose, bool global)
 {
+	UINFO("Planning a path to pose %s (global=%d)", targetPose.prettyPrint().c_str(), global?1:0);
+
 	this->clearPath();
 	std::list<std::pair<int, Transform> > pathPoses;
 

corelib/src/RtabmapThread.cpp

@@ -141,7 +141,7 @@ void RtabmapThread::publishMap(bool optimized, bool full) const
 			userDatas));
 }
 
-void RtabmapThread::publishTOROGraph(bool optimized, bool full) const
+void RtabmapThread::publishGraph(bool optimized, bool full) const
 {
 	std::map<int, Signature> signatures;
 	std::map<int, Transform> poses;
@@ -257,10 +257,10 @@ void RtabmapThread::mainLoop()
 		this->publishMap(atoi(parameters.at("optimized").c_str())!=0, true);
 		break;
 	case kStatePublishingTOROGraphLocal:
-		this->publishTOROGraph(atoi(parameters.at("optimized").c_str())!=0, false);
+		this->publishGraph(atoi(parameters.at("optimized").c_str())!=0, false);
 		break;
 	case kStatePublishingTOROGraphGlobal:
-		this->publishTOROGraph(atoi(parameters.at("optimized").c_str())!=0, true);
+		this->publishGraph(atoi(parameters.at("optimized").c_str())!=0, true);
 		break;
 	case kStateTriggeringMap:
 		_rtabmap->triggerNewMap();

guilib/src/MainWindow.cpp

@@ -1099,6 +1099,10 @@ void MainWindow::processStats(const rtabmap::Statistics & stat)
 			{
 				_ui->imageView_loopClosure->setImageDepth(lcDepth);
 			}
+			if(_ui->imageView_loopClosure->sceneRect().isNull())
+			{
+				_ui->imageView_loopClosure->setSceneRect(_ui->imageView_source->sceneRect());
+			}
 		}
 
 		UDEBUG("time= %d ms", time.restart());