Updated logged statistics

Matlab/ShowLogs/showlogs.m

@@ -97,15 +97,21 @@ end
 % xlabel('Location indexes')
 
 figure
-plot((sum(LogF(:,2:7),2)+LogF(:,17))*1000);
+plot(LogF(:,1), 'g'); % to verify that we have all timings below
 hold on
-ylabel('Time (ms)')
+if size(LogF, 2) == 21
-xlabel('Location indexes')
+    plot((sum(LogF(:,2:7),2)+LogF(:,17)+LogF(:,21)));
+else
+    plot((sum(LogF(:,2:7),2)+LogF(:,17)+sum(LogF(:,21:26),2)));
+end
+ylabel('Time (s)')
+xlabel('Node indexes')
 meanTime = mean(LogF(:,1))*1000
-plot([1 length(LogF(:,1))], [700 700], 'r')
+plot([1 length(LogF(:,1))], [0.7 0.7], 'r')
+plot([1 length(LogF(:,1))], [1 1], 'k')
 %plot([1 length(LogF(:,1))], [350 350], 'r')
 %legend('Processing time', 'Time limit')%, 'Acquisition rate (1 Hz)')
-title('Processing time')
+%title('Processing time')
 
 maxTime = max(sum(LogF(:,2:7),2)+LogF(:,17))
 maxDict = max(LogI(:, 6))
@@ -183,6 +189,34 @@ ylabel('MB')
 xlabel('Location indexes')
 % -------------------------
 
+if size(LogI, 2) >= 18
+    LTMsize = zeros(1,length(LogI(:,16)));
+    for i=1:length(LogI(:,16))
+        LTMsize(i) = sum(LogI(1:i,16) == 0);
+    end
+    LTM = LTMsize(end)
+
+    figure
+    % subplot(211)
+    H2 = plot(LTMsize, 'r'); % global graph
+     hold on
+    H1 = plot(LogI(:,7)); % WM
+    H3 = plot(LogI(:,17), 'g'); % Local graph
+    % H2 = plot(1:length(LogI(:,7)), ones(length(LogI(:,7)),1).*mean(LogI(:,7)), 'r--')
+    %title('Graph size')
+    legend('Global graph', 'WM', 'Local graph')
+    ylabel('Nodes')
+    xlabel('Node indexes')
+    %set(H1,'color',[0.3 0.3 0.3])
+    %set(H2,'color',[0 0 0])
+    %set(H3,'color',[0 0 0])
+    % subplot(212)
+    % plot(LogI(:,6));
+    meanWM = mean(LogI(:,7))
+    meanDict = mean(LogI(:,6))
+    % ylabel('Dictionary size')
+    % xlabel('Location indexes')
+else
     figure
     % subplot(211)
     H1 = plot(LogI(:,7));
@@ -199,6 +233,7 @@ xlabel('Location indexes')
     meanDict = mean(LogI(:,6))
     % ylabel('Dictionary size')
     % xlabel('Location indexes')
+end
 
 meanWordsPerSign = mean(LogI(:,5))
 %% -------------------------

corelib/src/Rtabmap.cpp

@@ -184,20 +184,26 @@ void Rtabmap::setupLogFiles(bool overwrite)
 			fprintf(_foutFloat, " 4-Likelihood time (s)\n");
 			fprintf(_foutFloat, " 5-Posterior time (s)\n");
 			fprintf(_foutFloat, " 6-Hypothesis selection time (s)\n");
-			fprintf(_foutFloat, " 7-Transfer time (s)\n");
+			fprintf(_foutFloat, " 7-Hypothesis validation time (s)\n");
-			fprintf(_foutFloat, " 8-Statistics creation time (s)\n");
+			fprintf(_foutFloat, " 8-Transfer time (s)\n");
-			fprintf(_foutFloat, " 9-Loop closure hypothesis value\n");
+			fprintf(_foutFloat, " 9-Statistics creation time (s)\n");
-			fprintf(_foutFloat, " 10-NAN\n");
+			fprintf(_foutFloat, " 10-Loop closure hypothesis value\n");
-			fprintf(_foutFloat, " 11-Maximum likelihood\n");
+			fprintf(_foutFloat, " 11-NAN\n");
-			fprintf(_foutFloat, " 12-Sum likelihood\n");
+			fprintf(_foutFloat, " 12-NAN\n");
-			fprintf(_foutFloat, " 13-Mean likelihood\n");
+			fprintf(_foutFloat, " 13-NAN\n");
-			fprintf(_foutFloat, " 14-Std dev likelihood\n");
+			fprintf(_foutFloat, " 14-NAN\n");
-			fprintf(_foutFloat, " 15-Virtual place hypothesis\n");
+			fprintf(_foutFloat, " 15-NAN\n");
-			fprintf(_foutFloat, " 16-Join trash time (s)\n");
+			fprintf(_foutFloat, " 16-Virtual place hypothesis\n");
-			fprintf(_foutFloat, " 17-Weight Update (rehearsal) similarity\n");
+			fprintf(_foutFloat, " 17-Join trash time (s)\n");
-			fprintf(_foutFloat, " 18-Empty trash time (s)\n");
+			fprintf(_foutFloat, " 18-Weight Update (rehearsal) similarity\n");
-			fprintf(_foutFloat, " 19-Retrieval database access time (s)\n");
+			fprintf(_foutFloat, " 19-Empty trash time (s)\n");
-			fprintf(_foutFloat, " 20-Add loop closure link time (s)\n");
+			fprintf(_foutFloat, " 20-Retrieval database access time (s)\n");
+			fprintf(_foutFloat, " 21-Add loop closure link time (s)\n");
+			fprintf(_foutFloat, " 22-Memory cleanup time (s)\n");
+			fprintf(_foutFloat, " 23-Scan matching (odometry correction) time (s)\n");
+			fprintf(_foutFloat, " 24-Local time loop closure detection time (s)\n");
+			fprintf(_foutFloat, " 25-Local space loop closure detection time (s)\n");
+			fprintf(_foutFloat, " 26-Map optimization (s)\n");
 		}
 		if(_statisticLoggedHeaders && addLogIHeader && _foutInt)
 		{
@@ -219,6 +225,8 @@ void Rtabmap::setupLogFiles(bool overwrite)
 			fprintf(_foutInt, " 15-Non-null likelihood values\n");
 			fprintf(_foutInt, " 16-Weight Update ID\n");
 			fprintf(_foutInt, " 17-Is last location merged through Weight Update?\n");
+			fprintf(_foutInt, " 18-Local graph size\n");
+			fprintf(_foutInt, " 19-Sensor data id\n");
 		}
 
 		ULOGGER_DEBUG("Log file (int)=%s", (_wDir+"/"+LOG_I).c_str());
@@ -1109,7 +1117,6 @@ bool Rtabmap::process(const SensorData & data)
 				weights = _memory->getWeights();
 			}
 
-			timer.start();
 			//============================================================
 			// Select the highest hypothesis
 			//============================================================
@@ -1948,9 +1955,6 @@ bool Rtabmap::process(const SensorData & data)
 		}
 	}
 
-	timeMapOptimization = timer.ticks();
-	ULOGGER_INFO("timeMapOptimization=%fs", timeMapOptimization);
-
 	//============================================================
 	// Add virtual links if a path is activated
 	//============================================================
@@ -1969,6 +1973,9 @@ bool Rtabmap::process(const SensorData & data)
 		}
 	}
 
+	timeMapOptimization = timer.ticks();
+	ULOGGER_INFO("timeMapOptimization=%fs", timeMapOptimization);
+
 	//============================================================
 	// Prepare statistics
 	//============================================================
@@ -2127,8 +2134,6 @@ bool Rtabmap::process(const SensorData & data)
 	//By default, remove all signatures with a loop closure link if they are not in reactivateIds
 	//This will also remove rehearsed signatures
 	std::list<int> signaturesRemoved = _memory->cleanup();
-	timeMemoryCleanup = timer.ticks();
-	ULOGGER_INFO("timeMemoryCleanup = %fs... %d signatures removed", timeMemoryCleanup, (int)signaturesRemoved.size());
 
 	// If this option activated, add new nodes only if there are linked with a previous map.
 	// Used when rtabmap is first started, it will wait a
@@ -2154,6 +2159,9 @@ bool Rtabmap::process(const SensorData & data)
 		_memory->deleteLocation(signature->id());
 	}
 
+	timeMemoryCleanup = timer.ticks();
+	ULOGGER_INFO("timeMemoryCleanup = %fs... %d signatures removed", timeMemoryCleanup, (int)signaturesRemoved.size());
+
 	// Pass this point signature should not be used, since it could have been transferred...
 	signature = 0;
 
@@ -2167,7 +2175,6 @@ bool Rtabmap::process(const SensorData & data)
 	//============================================================
 	double totalTime = timerTotal.ticks();
 	ULOGGER_INFO("Total time processing = %fs...", totalTime);
-	timer.start();
 	if((_maxTimeAllowed != 0 && totalTime*1000>_maxTimeAllowed) ||
 		(_maxMemoryAllowed != 0 && _memory->getWorkingMem().size() > _maxMemoryAllowed))
 	{
@@ -2278,7 +2285,7 @@ bool Rtabmap::process(const SensorData & data)
 	// TODO : use a specific class which will handle the RtabmapEvent
 	if(_foutFloat && _foutInt)
 	{
-		std::string logF = uFormat("%f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f\n",
+		std::string logF = uFormat("%f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f\n",
 									totalTime,
 									timeMemoryUpdate,
 									timeReactivations,
@@ -2299,7 +2306,12 @@ bool Rtabmap::process(const SensorData & data)
 									rehearsalValue,
 									timeEmptyingTrash,
 									timeRetrievalDbAccess,
-									timeAddLoopClosureLink);
+									timeAddLoopClosureLink,
+									timeMemoryCleanup,
+									timeScanMatching,
+									timeLocalTimeDetection,
+									timeLocalSpaceDetection,
+									timeMapOptimization);
 		std::string logI = uFormat("%d %d %d %d %d %d %d %d %d %d %d %d %d %d %d %d %d\n",
 									_loopClosureHypothesis.first,
 									_highestHypothesis.first,