i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-02 17:40:23 +08:00
Code Issues Packages Projects Releases Wiki Activity

merged master to multicamera branch

Browse Source
This commit is contained in:
Mathieu Labbe
2015-06-16 13:37:33 -04:00
parent 7d72aa83bc feba562c1b
commit 039ae45d57
2 changed files with 100 additions and 51 deletions
Download Patch File Download Diff File Expand all files Collapse all files

77
Matlab/ShowLogs/showlogs.m
View File

@@ -97,15 +97,21 @@ end
% xlabel('Location indexes') % xlabel('Location indexes')
figure figure
plot((sum(LogF(:,2:7),2)+LogF(:,17))*1000); plot(LogF(:,1), 'g'); % to verify that we have all timings below
hold on 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 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') %plot([1 length(LogF(:,1))], [350 350], 'r')
%legend('Processing time', 'Time limit')%, 'Acquisition rate (1 Hz)') %legend('Processing time', 'Time limit')%, 'Acquisition rate (1 Hz)')
title('Processing time') %title('Processing time')
maxTime = max(sum(LogF(:,2:7),2)+LogF(:,17)) maxTime = max(sum(LogF(:,2:7),2)+LogF(:,17))
maxDict = max(LogI(:, 6)) maxDict = max(LogI(:, 6))
@@ -183,22 +189,51 @@ ylabel('MB')
xlabel('Location indexes') xlabel('Location indexes')
% ------------------------- % -------------------------
figure if size(LogI, 2) >= 18
% subplot(211) LTMsize = zeros(1,length(LogI(:,16)));
H1 = plot(LogI(:,7)); for i=1:length(LogI(:,16))
% hold on LTMsize(i) = sum(LogI(1:i,16) == 0);
% H2 = plot(1:length(LogI(:,7)), ones(length(LogI(:,7)),1).*mean(LogI(:,7)), 'r--') end
title('Working memory size') LTM = LTMsize(end)
meanWM = mean(LogI(:,7))
ylabel('WM size (locations)') figure
xlabel('Location indexes') % subplot(211)
% set(H1,'color',[0.3 0.3 0.3]) H2 = plot(LTMsize, 'r'); % global graph
% set(H2,'color',[0 0 0]) hold on
% subplot(212) H1 = plot(LogI(:,7)); % WM
% plot(LogI(:,6)); H3 = plot(LogI(:,17), 'g'); % Local graph
meanDict = mean(LogI(:,6)) % H2 = plot(1:length(LogI(:,7)), ones(length(LogI(:,7)),1).*mean(LogI(:,7)), 'r--')
% ylabel('Dictionary size') %title('Graph size')
% xlabel('Location indexes') 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));
% hold on
% H2 = plot(1:length(LogI(:,7)), ones(length(LogI(:,7)),1).*mean(LogI(:,7)), 'r--')
title('Working memory size')
meanWM = mean(LogI(:,7))
ylabel('WM size (locations)')
xlabel('Location indexes')
% set(H1,'color',[0.3 0.3 0.3])
% set(H2,'color',[0 0 0])
% subplot(212)
% plot(LogI(:,6));
meanDict = mean(LogI(:,6))
% ylabel('Dictionary size')
% xlabel('Location indexes')
end
meanWordsPerSign = mean(LogI(:,5)) meanWordsPerSign = mean(LogI(:,5))
%% ------------------------- %% -------------------------

72
corelib/src/Rtabmap.cpp
View File

@@ -184,20 +184,26 @@ void Rtabmap::setupLogFiles(bool overwrite)
fprintf(_foutFloat, " 4-Likelihood time (s)\n"); fprintf(_foutFloat, " 4-Likelihood time (s)\n");
fprintf(_foutFloat, " 5-Posterior time (s)\n"); fprintf(_foutFloat, " 5-Posterior time (s)\n");
fprintf(_foutFloat, " 6-Hypothesis selection 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) if(_statisticLoggedHeaders && addLogIHeader && _foutInt)
{ {
@@ -219,6 +225,8 @@ void Rtabmap::setupLogFiles(bool overwrite)
fprintf(_foutInt, " 15-Non-null likelihood values\n"); fprintf(_foutInt, " 15-Non-null likelihood values\n");
fprintf(_foutInt, " 16-Weight Update ID\n"); fprintf(_foutInt, " 16-Weight Update ID\n");
fprintf(_foutInt, " 17-Is last location merged through Weight Update?\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()); ULOGGER_DEBUG("Log file (int)=%s", (_wDir+"/"+LOG_I).c_str());
@@ -1106,7 +1114,6 @@ bool Rtabmap::process(
weights = _memory->getWeights(); weights = _memory->getWeights();
} }
timer.start();
//============================================================ //============================================================
// Select the highest hypothesis // Select the highest hypothesis
//============================================================ //============================================================
@@ -1944,9 +1951,6 @@ bool Rtabmap::process(
} }
} }
timeMapOptimization = timer.ticks();
ULOGGER_INFO("timeMapOptimization=%fs", timeMapOptimization);
//============================================================ //============================================================
// Add virtual links if a path is activated // Add virtual links if a path is activated
//============================================================ //============================================================
@@ -1965,6 +1969,9 @@ bool Rtabmap::process(
} }
} }
timeMapOptimization = timer.ticks();
ULOGGER_INFO("timeMapOptimization=%fs", timeMapOptimization);
//============================================================ //============================================================
// Prepare statistics // Prepare statistics
//============================================================ //============================================================
@@ -2077,11 +2084,15 @@ bool Rtabmap::process(
ULOGGER_INFO("Time creating stats = %f...", timeStatsCreation); ULOGGER_INFO("Time creating stats = %f...", timeStatsCreation);
} }
Signature lastSignatureData(signature->id());
if(_publishLastSignatureData)
{
lastSignatureData = *signature;
}
//By default, remove all signatures with a loop closure link if they are not in reactivateIds //By default, remove all signatures with a loop closure link if they are not in reactivateIds
//This will also remove rehearsed signatures //This will also remove rehearsed signatures
std::list<int> signaturesRemoved = _memory->cleanup(); 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. // 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 // Used when rtabmap is first started, it will wait a
@@ -2107,15 +2118,14 @@ bool Rtabmap::process(
_memory->deleteLocation(signature->id()); _memory->deleteLocation(signature->id());
} }
Signature lastSignatureData(signature->id());
if(_publishLastSignatureData)
{
lastSignatureData = *signature;
}
// Pass this point signature should not be used, since it could have been transferred... // Pass this point signature should not be used, since it could have been transferred...
signature = 0; signature = 0;
timeMemoryCleanup = timer.ticks();
ULOGGER_INFO("timeMemoryCleanup = %fs... %d signatures removed", timeMemoryCleanup, (int)signaturesRemoved.size());
//============================================================ //============================================================
// TRANSFER // TRANSFER
//============================================================ //============================================================
@@ -2126,7 +2136,6 @@ bool Rtabmap::process(
//============================================================ //============================================================
double totalTime = timerTotal.ticks(); double totalTime = timerTotal.ticks();
ULOGGER_INFO("Total time processing = %fs...", totalTime); ULOGGER_INFO("Total time processing = %fs...", totalTime);
timer.start();
if((_maxTimeAllowed != 0 && totalTime*1000>_maxTimeAllowed) || if((_maxTimeAllowed != 0 && totalTime*1000>_maxTimeAllowed) ||
(_maxMemoryAllowed != 0 && _memory->getWorkingMem().size() > _maxMemoryAllowed)) (_maxMemoryAllowed != 0 && _memory->getWorkingMem().size() > _maxMemoryAllowed))
{ {
@@ -2251,7 +2260,7 @@ bool Rtabmap::process(
// TODO : use a specific class which will handle the RtabmapEvent // TODO : use a specific class which will handle the RtabmapEvent
if(_foutFloat && _foutInt) 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, totalTime,
timeMemoryUpdate, timeMemoryUpdate,
timeReactivations, timeReactivations,
@@ -2272,7 +2281,12 @@ bool Rtabmap::process(
rehearsalValue, rehearsalValue,
timeEmptyingTrash, timeEmptyingTrash,
timeRetrievalDbAccess, 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 %d %d\n", std::string logI = uFormat("%d %d %d %d %d %d %d %d %d %d %d %d %d %d %d %d %d %d %d\n",
_loopClosureHypothesis.first, _loopClosureHypothesis.first,
_highestHypothesis.first, _highestHypothesis.first,