i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-02 01:20:25 +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

74
corelib/src/Rtabmap.cpp
View File

@@ -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, " 8-Statistics creation time (s)\n");
fprintf(_foutFloat, " 9-Loop closure hypothesis value\n");
fprintf(_foutFloat, " 10-NAN\n");
fprintf(_foutFloat, " 11-Maximum likelihood\n");
fprintf(_foutFloat, " 12-Sum likelihood\n");
fprintf(_foutFloat, " 13-Mean likelihood\n");
fprintf(_foutFloat, " 14-Std dev likelihood\n");
fprintf(_foutFloat, " 15-Virtual place hypothesis\n");
fprintf(_foutFloat, " 16-Join trash time (s)\n");
fprintf(_foutFloat, " 17-Weight Update (rehearsal) similarity\n");
fprintf(_foutFloat, " 18-Empty trash time (s)\n");
fprintf(_foutFloat, " 19-Retrieval database access time (s)\n");
fprintf(_foutFloat, " 20-Add loop closure link time (s)\n");
fprintf(_foutFloat, " 7-Hypothesis validation time (s)\n");
fprintf(_foutFloat, " 8-Transfer time (s)\n");
fprintf(_foutFloat, " 9-Statistics creation time (s)\n");
fprintf(_foutFloat, " 10-Loop closure hypothesis value\n");
fprintf(_foutFloat, " 11-NAN\n");
fprintf(_foutFloat, " 12-NAN\n");
fprintf(_foutFloat, " 13-NAN\n");
fprintf(_foutFloat, " 14-NAN\n");
fprintf(_foutFloat, " 15-NAN\n");
fprintf(_foutFloat, " 16-Virtual place hypothesis\n");
fprintf(_foutFloat, " 17-Join trash time (s)\n");
fprintf(_foutFloat, " 18-Weight Update (rehearsal) similarity\n");
fprintf(_foutFloat, " 19-Empty trash 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());
@@ -1106,7 +1114,6 @@ bool Rtabmap::process(
weights = _memory->getWeights();
}
timer.start();
//============================================================
// 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
//============================================================
@@ -1965,6 +1969,9 @@ bool Rtabmap::process(
}
}
timeMapOptimization = timer.ticks();
ULOGGER_INFO("timeMapOptimization=%fs", timeMapOptimization);
//============================================================
// Prepare statistics
//============================================================
@@ -2077,11 +2084,15 @@ bool Rtabmap::process(
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
//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
@@ -2107,14 +2118,13 @@ bool Rtabmap::process(
_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...
signature = 0;
timeMemoryCleanup = timer.ticks();
ULOGGER_INFO("timeMemoryCleanup = %fs... %d signatures removed", timeMemoryCleanup, (int)signaturesRemoved.size());
//============================================================
// TRANSFER
@@ -2126,7 +2136,6 @@ bool Rtabmap::process(
//============================================================
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))
{
@@ -2251,7 +2260,7 @@ bool Rtabmap::process(
// 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,
@@ -2272,8 +2281,13 @@ bool Rtabmap::process(
rehearsalValue,
timeEmptyingTrash,
timeRetrievalDbAccess,
timeAddLoopClosureLink);
std::string logI = uFormat("%d %d %d %d %d %d %d %d %d %d %d %d %d %d %d %d %d %d %d\n",
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",
_loopClosureHypothesis.first,
_highestHypothesis.first,
(int)signaturesRemoved.size(),