00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028 #include <rtabmap/utilite/UEventsManager.h>
00029 #include <rtabmap/utilite/ULogger.h>
00030 #include <rtabmap/utilite/UTimer.h>
00031 #include <rtabmap/utilite/UConversion.h>
00032 #include <rtabmap/utilite/UProcessInfo.h>
00033 #include <rtabmap/utilite/UMath.h>
00034
00035 #include "rtabmap/core/Memory.h"
00036 #include "rtabmap/core/Signature.h"
00037 #include "rtabmap/core/Parameters.h"
00038 #include "rtabmap/core/RtabmapEvent.h"
00039 #include "rtabmap/core/VWDictionary.h"
00040 #include <rtabmap/core/EpipolarGeometry.h>
00041 #include "rtabmap/core/VisualWord.h"
00042 #include "rtabmap/core/Features2d.h"
00043 #include "rtabmap/core/RegistrationIcp.h"
00044 #include "rtabmap/core/Registration.h"
00045 #include "rtabmap/core/RegistrationVis.h"
00046 #include "rtabmap/core/DBDriver.h"
00047 #include "rtabmap/core/util3d_features.h"
00048 #include "rtabmap/core/util3d_filtering.h"
00049 #include "rtabmap/core/util3d_correspondences.h"
00050 #include "rtabmap/core/util3d_registration.h"
00051 #include "rtabmap/core/util3d_surface.h"
00052 #include "rtabmap/core/util3d_transforms.h"
00053 #include "rtabmap/core/util3d_motion_estimation.h"
00054 #include "rtabmap/core/util3d.h"
00055 #include "rtabmap/core/util2d.h"
00056 #include "rtabmap/core/Statistics.h"
00057 #include "rtabmap/core/Compression.h"
00058 #include "rtabmap/core/Graph.h"
00059 #include "rtabmap/core/Stereo.h"
00060
00061 #include <pcl/io/pcd_io.h>
00062 #include <pcl/common/common.h>
00063
00064 namespace rtabmap {
00065
00066 const int Memory::kIdStart = 0;
00067 const int Memory::kIdVirtual = -1;
00068 const int Memory::kIdInvalid = 0;
00069
00070 Memory::Memory(const ParametersMap & parameters) :
00071 _dbDriver(0),
00072 _similarityThreshold(Parameters::defaultMemRehearsalSimilarity()),
00073 _binDataKept(Parameters::defaultMemBinDataKept()),
00074 _rawDescriptorsKept(Parameters::defaultMemRawDescriptorsKept()),
00075 _saveDepth16Format(Parameters::defaultMemSaveDepth16Format()),
00076 _notLinkedNodesKeptInDb(Parameters::defaultMemNotLinkedNodesKept()),
00077 _incrementalMemory(Parameters::defaultMemIncrementalMemory()),
00078 _reduceGraph(Parameters::defaultMemReduceGraph()),
00079 _maxStMemSize(Parameters::defaultMemSTMSize()),
00080 _recentWmRatio(Parameters::defaultMemRecentWmRatio()),
00081 _transferSortingByWeightId(Parameters::defaultMemTransferSortingByWeightId()),
00082 _idUpdatedToNewOneRehearsal(Parameters::defaultMemRehearsalIdUpdatedToNewOne()),
00083 _generateIds(Parameters::defaultMemGenerateIds()),
00084 _badSignaturesIgnored(Parameters::defaultMemBadSignaturesIgnored()),
00085 _mapLabelsAdded(Parameters::defaultMemMapLabelsAdded()),
00086 _imagePreDecimation(Parameters::defaultMemImagePreDecimation()),
00087 _imagePostDecimation(Parameters::defaultMemImagePostDecimation()),
00088 _laserScanDownsampleStepSize(Parameters::defaultMemLaserScanDownsampleStepSize()),
00089 _reextractLoopClosureFeatures(Parameters::defaultRGBDLoopClosureReextractFeatures()),
00090 _rehearsalMaxDistance(Parameters::defaultRGBDLinearUpdate()),
00091 _rehearsalMaxAngle(Parameters::defaultRGBDAngularUpdate()),
00092 _rehearsalWeightIgnoredWhileMoving(Parameters::defaultMemRehearsalWeightIgnoredWhileMoving()),
00093 _useOdometryFeatures(Parameters::defaultMemUseOdomFeatures()),
00094 _idCount(kIdStart),
00095 _idMapCount(kIdStart),
00096 _lastSignature(0),
00097 _lastGlobalLoopClosureId(0),
00098 _memoryChanged(false),
00099 _linksChanged(false),
00100 _signaturesAdded(0),
00101
00102 _badSignRatio(Parameters::defaultKpBadSignRatio()),
00103 _tfIdfLikelihoodUsed(Parameters::defaultKpTfIdfLikelihoodUsed()),
00104 _parallelized(Parameters::defaultKpParallelized())
00105 {
00106 _feature2D = Feature2D::create(parameters);
00107 _vwd = new VWDictionary(parameters);
00108 _registrationPipeline = Registration::create(parameters);
00109 _registrationIcp = new RegistrationIcp(parameters);
00110 this->parseParameters(parameters);
00111 }
00112
00113 bool Memory::init(const std::string & dbUrl, bool dbOverwritten, const ParametersMap & parameters, bool postInitClosingEvents)
00114 {
00115 if(postInitClosingEvents) UEventsManager::post(new RtabmapEventInit(RtabmapEventInit::kInitializing));
00116
00117 UDEBUG("");
00118 this->parseParameters(parameters);
00119 bool loadAllNodesInWM = Parameters::defaultMemInitWMWithAllNodes();
00120 Parameters::parse(parameters, Parameters::kMemInitWMWithAllNodes(), loadAllNodesInWM);
00121
00122 if(postInitClosingEvents) UEventsManager::post(new RtabmapEventInit("Clearing memory..."));
00123 DBDriver * tmpDriver = 0;
00124 if((!_memoryChanged && !_linksChanged) || dbOverwritten)
00125 {
00126 if(_dbDriver)
00127 {
00128 tmpDriver = _dbDriver;
00129 _dbDriver = 0;
00130 }
00131 }
00132 else if(!_memoryChanged && _linksChanged)
00133 {
00134 _dbDriver->setTimestampUpdateEnabled(false);
00135 }
00136 this->clear();
00137 if(postInitClosingEvents) UEventsManager::post(new RtabmapEventInit("Clearing memory, done!"));
00138
00139 if(tmpDriver)
00140 {
00141 _dbDriver = tmpDriver;
00142 }
00143
00144 if(_dbDriver)
00145 {
00146 if(postInitClosingEvents) UEventsManager::post(new RtabmapEventInit("Closing database connection..."));
00147 _dbDriver->closeConnection();
00148 if(postInitClosingEvents) UEventsManager::post(new RtabmapEventInit("Closing database connection, done!"));
00149 }
00150
00151 if(_dbDriver == 0 && !dbUrl.empty())
00152 {
00153 _dbDriver = DBDriver::create(parameters);
00154 }
00155
00156 bool success = true;
00157 if(_dbDriver)
00158 {
00159 _dbDriver->setTimestampUpdateEnabled(true);
00160 success = false;
00161 if(postInitClosingEvents) UEventsManager::post(new RtabmapEventInit(std::string("Connecting to database ") + dbUrl + "..."));
00162 if(_dbDriver->openConnection(dbUrl, dbOverwritten))
00163 {
00164 success = true;
00165 if(postInitClosingEvents) UEventsManager::post(new RtabmapEventInit(std::string("Connecting to database ") + dbUrl + ", done!"));
00166
00167
00168 std::list<Signature*> dbSignatures;
00169
00170 if(loadAllNodesInWM)
00171 {
00172 if(postInitClosingEvents) UEventsManager::post(new RtabmapEventInit(std::string("Loading all nodes to WM...")));
00173 std::set<int> ids;
00174 _dbDriver->getAllNodeIds(ids, true);
00175 _dbDriver->loadSignatures(std::list<int>(ids.begin(), ids.end()), dbSignatures);
00176 }
00177 else
00178 {
00179
00180 if(postInitClosingEvents) UEventsManager::post(new RtabmapEventInit(std::string("Loading last nodes to WM...")));
00181 _dbDriver->loadLastNodes(dbSignatures);
00182 }
00183 for(std::list<Signature*>::reverse_iterator iter=dbSignatures.rbegin(); iter!=dbSignatures.rend(); ++iter)
00184 {
00185
00186 if(!((*iter)->isBadSignature() && _badSignaturesIgnored))
00187 {
00188
00189
00190
00191
00192
00193 _signatures.insert(std::pair<int, Signature *>((*iter)->id(), *iter));
00194 _workingMem.insert(std::make_pair((*iter)->id(), UTimer::now()));
00195 }
00196 else
00197 {
00198 delete *iter;
00199 }
00200 }
00201 if(postInitClosingEvents) UEventsManager::post(new RtabmapEventInit(std::string("Loading nodes to WM, done! (") + uNumber2Str(int(_workingMem.size() + _stMem.size())) + " loaded)"));
00202
00203
00204 if(_stMem.size()>0)
00205 {
00206 _lastSignature = uValue(_signatures, *_stMem.rbegin(), (Signature*)0);
00207 }
00208 else if(_workingMem.size()>0)
00209 {
00210 _lastSignature = uValue(_signatures, _workingMem.rbegin()->first, (Signature*)0);
00211 }
00212
00213
00214 _dbDriver->getLastNodeId(_idCount);
00215 _idMapCount = _lastSignature?_lastSignature->mapId()+1:kIdStart;
00216 }
00217 else
00218 {
00219 if(postInitClosingEvents) UEventsManager::post(new RtabmapEventInit(RtabmapEventInit::kError, std::string("Connecting to database ") + dbUrl + ", path is invalid!"));
00220 }
00221 }
00222 else
00223 {
00224 _idCount = kIdStart;
00225 _idMapCount = kIdStart;
00226 }
00227
00228 _workingMem.insert(std::make_pair(kIdVirtual, 0));
00229
00230 UDEBUG("ids start with %d", _idCount+1);
00231 UDEBUG("map ids start with %d", _idMapCount);
00232
00233
00234
00235 if(_dbDriver && _dbDriver->isConnected())
00236 {
00237 if(postInitClosingEvents) UEventsManager::post(new RtabmapEventInit("Loading dictionary..."));
00238 if(loadAllNodesInWM)
00239 {
00240
00241 std::set<int> wordIds;
00242 const std::map<int, Signature *> & signatures = this->getSignatures();
00243 for(std::map<int, Signature *>::const_iterator i=signatures.begin(); i!=signatures.end(); ++i)
00244 {
00245 const std::multimap<int, cv::KeyPoint> & words = i->second->getWords();
00246 std::list<int> keys = uUniqueKeys(words);
00247 wordIds.insert(keys.begin(), keys.end());
00248 }
00249 if(wordIds.size())
00250 {
00251 std::list<VisualWord*> words;
00252 _dbDriver->loadWords(wordIds, words);
00253 for(std::list<VisualWord*>::iterator iter = words.begin(); iter!=words.end(); ++iter)
00254 {
00255 _vwd->addWord(*iter);
00256 }
00257
00258 int id = 0;
00259 _dbDriver->getLastWordId(id);
00260 _vwd->setLastWordId(id);
00261 }
00262 }
00263 else
00264 {
00265
00266 _dbDriver->load(_vwd);
00267 }
00268 UDEBUG("%d words loaded!", _vwd->getUnusedWordsSize());
00269 _vwd->update();
00270 if(postInitClosingEvents) UEventsManager::post(new RtabmapEventInit(uFormat("Loading dictionary, done! (%d words)", (int)_vwd->getUnusedWordsSize())));
00271 }
00272
00273 if(postInitClosingEvents) UEventsManager::post(new RtabmapEventInit(std::string("Adding word references...")));
00274
00275 const std::map<int, Signature *> & signatures = this->getSignatures();
00276 for(std::map<int, Signature *>::const_iterator i=signatures.begin(); i!=signatures.end(); ++i)
00277 {
00278 Signature * s = this->_getSignature(i->first);
00279 UASSERT(s != 0);
00280
00281 const std::multimap<int, cv::KeyPoint> & words = s->getWords();
00282 if(words.size())
00283 {
00284 UDEBUG("node=%d, word references=%d", s->id(), words.size());
00285 for(std::multimap<int, cv::KeyPoint>::const_iterator iter = words.begin(); iter!=words.end(); ++iter)
00286 {
00287 _vwd->addWordRef(iter->first, i->first);
00288 }
00289 s->setEnabled(true);
00290 }
00291 }
00292 if(postInitClosingEvents) UEventsManager::post(new RtabmapEventInit(uFormat("Adding word references, done! (%d)", _vwd->getTotalActiveReferences())));
00293
00294 if(_vwd->getUnusedWordsSize())
00295 {
00296 UWARN("_vwd->getUnusedWordsSize() must be empty... size=%d", _vwd->getUnusedWordsSize());
00297 }
00298 UDEBUG("Total word references added = %d", _vwd->getTotalActiveReferences());
00299
00300 if(postInitClosingEvents) UEventsManager::post(new RtabmapEventInit(RtabmapEventInit::kInitialized));
00301 return success;
00302 }
00303
00304 void Memory::close(bool databaseSaved, bool postInitClosingEvents)
00305 {
00306 UINFO("databaseSaved=%d, postInitClosingEvents=%d", databaseSaved?1:0, postInitClosingEvents?1:0);
00307 if(postInitClosingEvents) UEventsManager::post(new RtabmapEventInit(RtabmapEventInit::kClosing));
00308
00309 if(!databaseSaved || (!_memoryChanged && !_linksChanged))
00310 {
00311 if(postInitClosingEvents) UEventsManager::post(new RtabmapEventInit(uFormat("No changes added to database.")));
00312
00313 UINFO("No changes added to database.");
00314 if(_dbDriver)
00315 {
00316 if(postInitClosingEvents) UEventsManager::post(new RtabmapEventInit(uFormat("Closing database \"%s\"...", _dbDriver->getUrl().c_str())));
00317 _dbDriver->closeConnection(false);
00318 delete _dbDriver;
00319 _dbDriver = 0;
00320 if(postInitClosingEvents) UEventsManager::post(new RtabmapEventInit("Closing database, done!"));
00321 }
00322 if(postInitClosingEvents) UEventsManager::post(new RtabmapEventInit("Clearing memory..."));
00323 this->clear();
00324 if(postInitClosingEvents) UEventsManager::post(new RtabmapEventInit("Clearing memory, done!"));
00325 }
00326 else
00327 {
00328 UINFO("Saving memory...");
00329 if(postInitClosingEvents) UEventsManager::post(new RtabmapEventInit("Saving memory..."));
00330 if(!_memoryChanged && _linksChanged && _dbDriver)
00331 {
00332
00333 UDEBUG("");
00334 _dbDriver->setTimestampUpdateEnabled(false);
00335 }
00336 this->clear();
00337 if(_dbDriver)
00338 {
00339 _dbDriver->emptyTrashes();
00340 if(postInitClosingEvents) UEventsManager::post(new RtabmapEventInit("Saving memory, done!"));
00341 if(postInitClosingEvents) UEventsManager::post(new RtabmapEventInit(uFormat("Closing database \"%s\"...", _dbDriver->getUrl().c_str())));
00342 _dbDriver->closeConnection();
00343 delete _dbDriver;
00344 _dbDriver = 0;
00345 if(postInitClosingEvents) UEventsManager::post(new RtabmapEventInit("Closing database, done!"));
00346 }
00347 else
00348 {
00349 if(postInitClosingEvents) UEventsManager::post(new RtabmapEventInit("Saving memory, done!"));
00350 }
00351 }
00352 if(postInitClosingEvents) UEventsManager::post(new RtabmapEventInit(RtabmapEventInit::kClosed));
00353 }
00354
00355 Memory::~Memory()
00356 {
00357 this->close();
00358
00359 if(_dbDriver)
00360 {
00361 UWARN("Please call Memory::close() before");
00362 }
00363 if(_feature2D)
00364 {
00365 delete _feature2D;
00366 }
00367 if(_vwd)
00368 {
00369 delete _vwd;
00370 }
00371 if(_registrationPipeline)
00372 {
00373 delete _registrationPipeline;
00374 }
00375 if(_registrationIcp)
00376 {
00377 delete _registrationIcp;
00378 }
00379 }
00380
00381 void Memory::parseParameters(const ParametersMap & parameters)
00382 {
00383 uInsert(parameters_, parameters);
00384
00385 UDEBUG("");
00386 ParametersMap::const_iterator iter;
00387
00388 Parameters::parse(parameters, Parameters::kMemBinDataKept(), _binDataKept);
00389 Parameters::parse(parameters, Parameters::kMemRawDescriptorsKept(), _rawDescriptorsKept);
00390 Parameters::parse(parameters, Parameters::kMemSaveDepth16Format(), _saveDepth16Format);
00391 Parameters::parse(parameters, Parameters::kMemReduceGraph(), _reduceGraph);
00392 Parameters::parse(parameters, Parameters::kMemNotLinkedNodesKept(), _notLinkedNodesKeptInDb);
00393 Parameters::parse(parameters, Parameters::kMemRehearsalIdUpdatedToNewOne(), _idUpdatedToNewOneRehearsal);
00394 Parameters::parse(parameters, Parameters::kMemGenerateIds(), _generateIds);
00395 Parameters::parse(parameters, Parameters::kMemBadSignaturesIgnored(), _badSignaturesIgnored);
00396 Parameters::parse(parameters, Parameters::kMemMapLabelsAdded(), _mapLabelsAdded);
00397 Parameters::parse(parameters, Parameters::kMemRehearsalSimilarity(), _similarityThreshold);
00398 Parameters::parse(parameters, Parameters::kMemRecentWmRatio(), _recentWmRatio);
00399 Parameters::parse(parameters, Parameters::kMemTransferSortingByWeightId(), _transferSortingByWeightId);
00400 Parameters::parse(parameters, Parameters::kMemSTMSize(), _maxStMemSize);
00401 Parameters::parse(parameters, Parameters::kMemImagePreDecimation(), _imagePreDecimation);
00402 Parameters::parse(parameters, Parameters::kMemImagePostDecimation(), _imagePostDecimation);
00403 Parameters::parse(parameters, Parameters::kMemLaserScanDownsampleStepSize(), _laserScanDownsampleStepSize);
00404 Parameters::parse(parameters, Parameters::kRGBDLoopClosureReextractFeatures(), _reextractLoopClosureFeatures);
00405 Parameters::parse(parameters, Parameters::kRGBDLinearUpdate(), _rehearsalMaxDistance);
00406 Parameters::parse(parameters, Parameters::kRGBDAngularUpdate(), _rehearsalMaxAngle);
00407 Parameters::parse(parameters, Parameters::kMemRehearsalWeightIgnoredWhileMoving(), _rehearsalWeightIgnoredWhileMoving);
00408 Parameters::parse(parameters, Parameters::kMemUseOdomFeatures(), _useOdometryFeatures);
00409
00410 UASSERT_MSG(_maxStMemSize >= 0, uFormat("value=%d", _maxStMemSize).c_str());
00411 UASSERT_MSG(_similarityThreshold >= 0.0f && _similarityThreshold <= 1.0f, uFormat("value=%f", _similarityThreshold).c_str());
00412 UASSERT_MSG(_recentWmRatio >= 0.0f && _recentWmRatio <= 1.0f, uFormat("value=%f", _recentWmRatio).c_str());
00413 UASSERT(_imagePreDecimation >= 1);
00414 UASSERT(_imagePostDecimation >= 1);
00415 UASSERT(_rehearsalMaxDistance >= 0.0f);
00416 UASSERT(_rehearsalMaxAngle >= 0.0f);
00417
00418 if(_dbDriver)
00419 {
00420 _dbDriver->parseParameters(parameters);
00421 }
00422
00423
00424 if(_vwd)
00425 {
00426 _vwd->parseParameters(parameters);
00427 }
00428
00429 Parameters::parse(parameters, Parameters::kKpTfIdfLikelihoodUsed(), _tfIdfLikelihoodUsed);
00430 Parameters::parse(parameters, Parameters::kKpParallelized(), _parallelized);
00431 Parameters::parse(parameters, Parameters::kKpBadSignRatio(), _badSignRatio);
00432
00433
00434 UASSERT(_feature2D != 0);
00435 Feature2D::Type detectorStrategy = Feature2D::kFeatureUndef;
00436 if((iter=parameters.find(Parameters::kKpDetectorStrategy())) != parameters.end())
00437 {
00438 detectorStrategy = (Feature2D::Type)std::atoi((*iter).second.c_str());
00439 }
00440 if(detectorStrategy!=Feature2D::kFeatureUndef)
00441 {
00442 UDEBUG("new detector strategy %d", int(detectorStrategy));
00443 if(_feature2D)
00444 {
00445 delete _feature2D;
00446 _feature2D = 0;
00447 }
00448
00449 _feature2D = Feature2D::create(detectorStrategy, parameters_);
00450 }
00451 else if(_feature2D)
00452 {
00453 _feature2D->parseParameters(parameters);
00454 }
00455
00456 Registration::Type regStrategy = Registration::kTypeUndef;
00457 if((iter=parameters.find(Parameters::kRegStrategy())) != parameters.end())
00458 {
00459 regStrategy = (Registration::Type)std::atoi((*iter).second.c_str());
00460 }
00461 if(regStrategy!=Registration::kTypeUndef)
00462 {
00463 UDEBUG("new registration strategy %d", int(regStrategy));
00464 if(_registrationPipeline)
00465 {
00466 delete _registrationPipeline;
00467 _registrationPipeline = 0;
00468 }
00469
00470 _registrationPipeline = Registration::create(regStrategy, parameters_);
00471 }
00472 else if(_registrationPipeline)
00473 {
00474 _registrationPipeline->parseParameters(parameters);
00475 }
00476
00477 if(_registrationIcp)
00478 {
00479 _registrationIcp->parseParameters(parameters);
00480 }
00481
00482
00483
00484 iter = parameters.find(Parameters::kMemIncrementalMemory());
00485 if(iter != parameters.end())
00486 {
00487 bool value = uStr2Bool(iter->second.c_str());
00488 if(value == false && _incrementalMemory)
00489 {
00490
00491 this->incrementMapId();
00492
00493
00494
00495 if((_memoryChanged || _linksChanged) && _dbDriver)
00496 {
00497 UWARN("Switching from Mapping to Localization mode, the database will be saved and reloaded.");
00498 this->init(_dbDriver->getUrl());
00499 UWARN("Switching from Mapping to Localization mode, the database is reloaded!");
00500 }
00501 }
00502 _incrementalMemory = value;
00503 }
00504 }
00505
00506 void Memory::preUpdate()
00507 {
00508 _signaturesAdded = 0;
00509 this->cleanUnusedWords();
00510 if(_vwd && !_parallelized)
00511 {
00512
00513 _vwd->update();
00514 }
00515 }
00516
00517 bool Memory::update(
00518 const SensorData & data,
00519 Statistics * stats)
00520 {
00521 return update(data, Transform(), cv::Mat(), stats);
00522 }
00523
00524 bool Memory::update(
00525 const SensorData & data,
00526 const Transform & pose,
00527 const cv::Mat & covariance,
00528 Statistics * stats)
00529 {
00530 UDEBUG("");
00531 UTimer timer;
00532 UTimer totalTimer;
00533 timer.start();
00534 float t;
00535
00536
00537
00538
00539 UDEBUG("pre-updating...");
00540 this->preUpdate();
00541 t=timer.ticks()*1000;
00542 if(stats) stats->addStatistic(Statistics::kTimingMemPre_update(), t);
00543 UDEBUG("time preUpdate=%f ms", t);
00544
00545
00546
00547
00548 Signature * signature = this->createSignature(data, pose, stats);
00549 if (signature == 0)
00550 {
00551 UERROR("Failed to create a signature...");
00552 return false;
00553 }
00554
00555 t=timer.ticks()*1000;
00556 if(stats) stats->addStatistic(Statistics::kTimingMemSignature_creation(), t);
00557 UDEBUG("time creating signature=%f ms", t);
00558
00559
00560 this->addSignatureToStm(signature, covariance);
00561
00562 _lastSignature = signature;
00563
00564
00565
00566
00567
00568
00569
00570 if(_incrementalMemory)
00571 {
00572 if(_similarityThreshold < 1.0f)
00573 {
00574 this->rehearsal(signature, stats);
00575 }
00576 t=timer.ticks()*1000;
00577 if(stats) stats->addStatistic(Statistics::kTimingMemRehearsal(), t);
00578 UDEBUG("time rehearsal=%f ms", t);
00579 }
00580 else
00581 {
00582 if(_workingMem.size() <= 1)
00583 {
00584 UWARN("The working memory is empty and the memory is not "
00585 "incremental (Mem/IncrementalMemory=False), no loop closure "
00586 "can be detected! Please set Mem/IncrementalMemory=true to increase "
00587 "the memory with new images or decrease the STM size (which is %d "
00588 "including the new one added).", (int)_stMem.size());
00589 }
00590 }
00591
00592
00593
00594
00595 int notIntermediateNodesCount = 0;
00596 for(std::set<int>::iterator iter=_stMem.begin(); iter!=_stMem.end(); ++iter)
00597 {
00598 const Signature * s = this->getSignature(*iter);
00599 UASSERT(s != 0);
00600 if(s->getWeight() >= 0)
00601 {
00602 ++notIntermediateNodesCount;
00603 }
00604 }
00605 std::map<int, int> reducedIds;
00606 while(_stMem.size() && _maxStMemSize>0 && notIntermediateNodesCount > _maxStMemSize)
00607 {
00608 int id = *_stMem.begin();
00609 Signature * s = this->_getSignature(id);
00610 UASSERT(s != 0);
00611 if(s->getWeight() >= 0)
00612 {
00613 --notIntermediateNodesCount;
00614 }
00615
00616 int reducedTo = 0;
00617 moveSignatureToWMFromSTM(id, &reducedTo);
00618
00619 if(reducedTo > 0)
00620 {
00621 reducedIds.insert(std::make_pair(id, reducedTo));
00622 }
00623 }
00624 if(stats) stats->setReducedIds(reducedIds);
00625
00626 if(!_memoryChanged && _incrementalMemory)
00627 {
00628 _memoryChanged = true;
00629 }
00630
00631 UDEBUG("totalTimer = %fs", totalTimer.ticks());
00632
00633 return true;
00634 }
00635
00636 void Memory::addSignatureToStm(Signature * signature, const cv::Mat & covariance)
00637 {
00638 UTimer timer;
00639
00640 if(signature)
00641 {
00642 UDEBUG("adding %d", signature->id());
00643
00644 if(_stMem.size())
00645 {
00646 if(_signatures.at(*_stMem.rbegin())->mapId() == signature->mapId())
00647 {
00648 Transform motionEstimate;
00649 if(!signature->getPose().isNull() &&
00650 !_signatures.at(*_stMem.rbegin())->getPose().isNull())
00651 {
00652 cv::Mat infMatrix = covariance.inv();
00653 motionEstimate = _signatures.at(*_stMem.rbegin())->getPose().inverse() * signature->getPose();
00654 _signatures.at(*_stMem.rbegin())->addLink(Link(*_stMem.rbegin(), signature->id(), Link::kNeighbor, motionEstimate, infMatrix));
00655 signature->addLink(Link(signature->id(), *_stMem.rbegin(), Link::kNeighbor, motionEstimate.inverse(), infMatrix));
00656 }
00657 else
00658 {
00659 _signatures.at(*_stMem.rbegin())->addLink(Link(*_stMem.rbegin(), signature->id(), Link::kNeighbor, Transform()));
00660 signature->addLink(Link(signature->id(), *_stMem.rbegin(), Link::kNeighbor, Transform()));
00661 }
00662 UDEBUG("Min STM id = %d", *_stMem.begin());
00663 }
00664 else
00665 {
00666 UDEBUG("Ignoring neighbor link between %d and %d because they are not in the same map! (%d vs %d)",
00667 *_stMem.rbegin(), signature->id(),
00668 _signatures.at(*_stMem.rbegin())->mapId(), signature->mapId());
00669
00670
00671 std::string tag = uFormat("map%d", signature->mapId());
00672 if(getSignatureIdByLabel(tag, false) == 0)
00673 {
00674 UINFO("Tagging node %d with label \"%s\"", signature->id(), tag.c_str());
00675 signature->setLabel(tag);
00676 }
00677 }
00678 }
00679 else if(_mapLabelsAdded)
00680 {
00681
00682 std::string tag = uFormat("map%d", signature->mapId());
00683 if(getSignatureIdByLabel(tag, false) == 0)
00684 {
00685 UINFO("Tagging node %d with label \"%s\"", signature->id(), tag.c_str());
00686 signature->setLabel(tag);
00687 }
00688 }
00689
00690 _signatures.insert(_signatures.end(), std::pair<int, Signature *>(signature->id(), signature));
00691 _stMem.insert(_stMem.end(), signature->id());
00692 ++_signaturesAdded;
00693
00694 if(_vwd)
00695 {
00696 UDEBUG("%d words ref for the signature %d", signature->getWords().size(), signature->id());
00697 }
00698 if(signature->getWords().size())
00699 {
00700 signature->setEnabled(true);
00701 }
00702 }
00703
00704 UDEBUG("time = %fs", timer.ticks());
00705 }
00706
00707 void Memory::addSignatureToWmFromLTM(Signature * signature)
00708 {
00709 if(signature)
00710 {
00711 UDEBUG("Inserting node %d in WM...", signature->id());
00712 _workingMem.insert(std::make_pair(signature->id(), UTimer::now()));
00713 _signatures.insert(std::pair<int, Signature*>(signature->id(), signature));
00714 ++_signaturesAdded;
00715 }
00716 else
00717 {
00718 UERROR("Signature is null ?!?");
00719 }
00720 }
00721
00722 void Memory::moveSignatureToWMFromSTM(int id, int * reducedTo)
00723 {
00724 UDEBUG("Inserting node %d from STM in WM...", id);
00725 UASSERT(_stMem.find(id) != _stMem.end());
00726 Signature * s = this->_getSignature(id);
00727 UASSERT(s!=0);
00728
00729 if(_reduceGraph)
00730 {
00731 bool merge = false;
00732 const std::map<int, Link> & links = s->getLinks();
00733 std::map<int, Link> neighbors;
00734 for(std::map<int, Link>::const_iterator iter=links.begin(); iter!=links.end(); ++iter)
00735 {
00736 if(!merge)
00737 {
00738 merge = iter->second.to() < s->id() &&
00739 iter->second.type() != Link::kNeighbor &&
00740 iter->second.type() != Link::kNeighborMerged &&
00741 iter->second.userDataCompressed().empty() &&
00742 iter->second.type() != Link::kUndef &&
00743 iter->second.type() != Link::kVirtualClosure;
00744 if(merge)
00745 {
00746 UDEBUG("Reduce %d to %d", s->id(), iter->second.to());
00747 if(reducedTo)
00748 {
00749 *reducedTo = iter->second.to();
00750 }
00751 }
00752
00753 }
00754 if(iter->second.type() == Link::kNeighbor)
00755 {
00756 neighbors.insert(*iter);
00757 }
00758 }
00759 if(merge)
00760 {
00761 if(s->getLabel().empty())
00762 {
00763 for(std::map<int, Link>::const_iterator iter=links.begin(); iter!=links.end(); ++iter)
00764 {
00765 merge = true;
00766 Signature * sTo = this->_getSignature(iter->first);
00767 UASSERT(sTo!=0);
00768 sTo->removeLink(s->id());
00769 if(iter->second.type() != Link::kNeighbor &&
00770 iter->second.type() != Link::kNeighborMerged &&
00771 iter->second.type() != Link::kUndef)
00772 {
00773
00774 for(std::map<int, Link>::iterator jter=neighbors.begin(); jter!=neighbors.end(); ++jter)
00775 {
00776 if(!sTo->hasLink(jter->second.to()))
00777 {
00778 Link l = iter->second.inverse().merge(
00779 jter->second,
00780 iter->second.userDataCompressed().empty() && iter->second.type() != Link::kVirtualClosure?Link::kNeighborMerged:iter->second.type());
00781 sTo->addLink(l);
00782 Signature * sB = this->_getSignature(l.to());
00783 UASSERT(sB!=0);
00784 UASSERT(!sB->hasLink(l.to()));
00785 sB->addLink(l.inverse());
00786 }
00787 }
00788 }
00789 }
00790
00791
00792 std::map<int, Link> linksCopy = links;
00793 for(std::map<int, Link>::iterator iter=linksCopy.begin(); iter!=linksCopy.end(); ++iter)
00794 {
00795 if(iter->second.type() == Link::kNeighbor ||
00796 iter->second.type() == Link::kNeighborMerged)
00797 {
00798 s->removeLink(iter->first);
00799 if(iter->second.type() == Link::kNeighbor)
00800 {
00801 if(_lastGlobalLoopClosureId == s->id())
00802 {
00803 _lastGlobalLoopClosureId = iter->first;
00804 }
00805 }
00806 }
00807 }
00808
00809 this->moveToTrash(s, _notLinkedNodesKeptInDb);
00810 s = 0;
00811 }
00812 }
00813 }
00814 if(s != 0)
00815 {
00816 _workingMem.insert(_workingMem.end(), std::make_pair(*_stMem.begin(), UTimer::now()));
00817 _stMem.erase(*_stMem.begin());
00818 }
00819
00820 }
00821
00822 const Signature * Memory::getSignature(int id) const
00823 {
00824 return _getSignature(id);
00825 }
00826
00827 Signature * Memory::_getSignature(int id) const
00828 {
00829 return uValue(_signatures, id, (Signature*)0);
00830 }
00831
00832 const VWDictionary * Memory::getVWDictionary() const
00833 {
00834 return _vwd;
00835 }
00836
00837 std::map<int, Link> Memory::getNeighborLinks(
00838 int signatureId,
00839 bool lookInDatabase) const
00840 {
00841 std::map<int, Link> links;
00842 Signature * s = uValue(_signatures, signatureId, (Signature*)0);
00843 if(s)
00844 {
00845 const std::map<int, Link> & allLinks = s->getLinks();
00846 for(std::map<int, Link>::const_iterator iter = allLinks.begin(); iter!=allLinks.end(); ++iter)
00847 {
00848 if(iter->second.type() == Link::kNeighbor ||
00849 iter->second.type() == Link::kNeighborMerged)
00850 {
00851 links.insert(*iter);
00852 }
00853 }
00854 }
00855 else if(lookInDatabase && _dbDriver)
00856 {
00857 std::map<int, Link> neighbors;
00858 _dbDriver->loadLinks(signatureId, neighbors);
00859 for(std::map<int, Link>::iterator iter=neighbors.begin(); iter!=neighbors.end();)
00860 {
00861 if(iter->second.type() != Link::kNeighbor &&
00862 iter->second.type() != Link::kNeighborMerged)
00863 {
00864 neighbors.erase(iter++);
00865 }
00866 else
00867 {
00868 ++iter;
00869 }
00870 }
00871 }
00872 else
00873 {
00874 UWARN("Cannot find signature %d in memory", signatureId);
00875 }
00876 return links;
00877 }
00878
00879 std::map<int, Link> Memory::getLoopClosureLinks(
00880 int signatureId,
00881 bool lookInDatabase) const
00882 {
00883 const Signature * s = this->getSignature(signatureId);
00884 std::map<int, Link> loopClosures;
00885 if(s)
00886 {
00887 const std::map<int, Link> & allLinks = s->getLinks();
00888 for(std::map<int, Link>::const_iterator iter = allLinks.begin(); iter!=allLinks.end(); ++iter)
00889 {
00890 if(iter->second.type() != Link::kNeighbor &&
00891 iter->second.type() != Link::kNeighborMerged &&
00892 iter->second.type() != Link::kUndef)
00893 {
00894 loopClosures.insert(*iter);
00895 }
00896 }
00897 }
00898 else if(lookInDatabase && _dbDriver)
00899 {
00900 _dbDriver->loadLinks(signatureId, loopClosures);
00901 for(std::map<int, Link>::iterator iter=loopClosures.begin(); iter!=loopClosures.end();)
00902 {
00903 if(iter->second.type() == Link::kNeighbor ||
00904 iter->second.type() == Link::kNeighborMerged ||
00905 iter->second.type() == Link::kUndef )
00906 {
00907 loopClosures.erase(iter++);
00908 }
00909 else
00910 {
00911 ++iter;
00912 }
00913 }
00914 }
00915 return loopClosures;
00916 }
00917
00918 std::map<int, Link> Memory::getLinks(
00919 int signatureId,
00920 bool lookInDatabase) const
00921 {
00922 std::map<int, Link> links;
00923 Signature * s = uValue(_signatures, signatureId, (Signature*)0);
00924 if(s)
00925 {
00926 links = s->getLinks();
00927 }
00928 else if(lookInDatabase && _dbDriver)
00929 {
00930 _dbDriver->loadLinks(signatureId, links, Link::kUndef);
00931 }
00932 else
00933 {
00934 UWARN("Cannot find signature %d in memory", signatureId);
00935 }
00936 return links;
00937 }
00938
00939 std::multimap<int, Link> Memory::getAllLinks(bool lookInDatabase, bool ignoreNullLinks) const
00940 {
00941 std::multimap<int, Link> links;
00942
00943 if(lookInDatabase && _dbDriver)
00944 {
00945 _dbDriver->getAllLinks(links, ignoreNullLinks);
00946 }
00947
00948 for(std::map<int, Signature*>::const_iterator iter=_signatures.begin(); iter!=_signatures.end(); ++iter)
00949 {
00950 links.erase(iter->first);
00951 for(std::map<int, Link>::const_iterator jter=iter->second->getLinks().begin();
00952 jter!=iter->second->getLinks().end();
00953 ++jter)
00954 {
00955 if(!ignoreNullLinks || jter->second.isValid())
00956 {
00957 links.insert(std::make_pair(iter->first, jter->second));
00958 }
00959 }
00960 }
00961
00962 return links;
00963 }
00964
00965
00966
00967
00968
00969 std::map<int, int> Memory::getNeighborsId(
00970 int signatureId,
00971 int maxGraphDepth,
00972 int maxCheckedInDatabase,
00973 bool incrementMarginOnLoop,
00974 bool ignoreLoopIds,
00975 bool ignoreIntermediateNodes,
00976 double * dbAccessTime
00977 ) const
00978 {
00979 UASSERT(maxGraphDepth >= 0);
00980
00981 if(dbAccessTime)
00982 {
00983 *dbAccessTime = 0;
00984 }
00985 std::map<int, int> ids;
00986 if(signatureId<=0)
00987 {
00988 return ids;
00989 }
00990 int nbLoadedFromDb = 0;
00991 std::list<int> curentMarginList;
00992 std::set<int> currentMargin;
00993 std::set<int> nextMargin;
00994 nextMargin.insert(signatureId);
00995 int m = 0;
00996 std::set<int> ignoredIds;
00997 while((maxGraphDepth == 0 || m < maxGraphDepth) && nextMargin.size())
00998 {
00999
01000 curentMarginList = std::list<int>(nextMargin.rbegin(), nextMargin.rend());
01001 nextMargin.clear();
01002
01003 for(std::list<int>::iterator jter = curentMarginList.begin(); jter!=curentMarginList.end(); ++jter)
01004 {
01005 if(ids.find(*jter) == ids.end())
01006 {
01007
01008
01009 const Signature * s = this->getSignature(*jter);
01010 std::map<int, Link> tmpLinks;
01011 const std::map<int, Link> * links = &tmpLinks;
01012 if(s)
01013 {
01014 if(!ignoreIntermediateNodes || s->getWeight() != -1)
01015 {
01016 ids.insert(std::pair<int, int>(*jter, m));
01017 }
01018 else
01019 {
01020 ignoredIds.insert(*jter);
01021 }
01022
01023 links = &s->getLinks();
01024 }
01025 else if(maxCheckedInDatabase == -1 || (maxCheckedInDatabase > 0 && _dbDriver && nbLoadedFromDb < maxCheckedInDatabase))
01026 {
01027 ++nbLoadedFromDb;
01028 ids.insert(std::pair<int, int>(*jter, m));
01029
01030 UTimer timer;
01031 _dbDriver->loadLinks(*jter, tmpLinks);
01032 if(dbAccessTime)
01033 {
01034 *dbAccessTime += timer.getElapsedTime();
01035 }
01036 }
01037
01038
01039 for(std::map<int, Link>::const_iterator iter=links->begin(); iter!=links->end(); ++iter)
01040 {
01041 if( !uContains(ids, iter->first) && ignoredIds.find(iter->first) == ignoredIds.end())
01042 {
01043 UASSERT(iter->second.type() != Link::kUndef);
01044 if(iter->second.type() == Link::kNeighbor ||
01045 iter->second.type() == Link::kNeighborMerged)
01046 {
01047 if(ignoreIntermediateNodes && s->getWeight()==-1)
01048 {
01049
01050 if(currentMargin.insert(iter->first).second)
01051 {
01052 curentMarginList.push_back(iter->first);
01053 }
01054 }
01055 else
01056 {
01057 nextMargin.insert(iter->first);
01058 }
01059 }
01060 else if(!ignoreLoopIds)
01061 {
01062 if(incrementMarginOnLoop)
01063 {
01064 nextMargin.insert(iter->first);
01065 }
01066 else
01067 {
01068 if(currentMargin.insert(iter->first).second)
01069 {
01070 curentMarginList.push_back(iter->first);
01071 }
01072 }
01073 }
01074 }
01075 }
01076 }
01077 }
01078 ++m;
01079 }
01080 return ids;
01081 }
01082
01083
01084 std::map<int, float> Memory::getNeighborsIdRadius(
01085 int signatureId,
01086 float radius,
01087 const std::map<int, Transform> & optimizedPoses,
01088 int maxGraphDepth
01089 ) const
01090 {
01091 UASSERT(maxGraphDepth >= 0);
01092 UASSERT(uContains(optimizedPoses, signatureId));
01093 UASSERT(signatureId > 0);
01094 std::map<int, float> ids;
01095 std::list<int> curentMarginList;
01096 std::set<int> currentMargin;
01097 std::set<int> nextMargin;
01098 nextMargin.insert(signatureId);
01099 int m = 0;
01100 Transform referential = optimizedPoses.at(signatureId);
01101 UASSERT(!referential.isNull());
01102 float radiusSqrd = radius*radius;
01103 std::map<int, float> savedRadius;
01104 savedRadius.insert(std::make_pair(signatureId, 0));
01105 while((maxGraphDepth == 0 || m < maxGraphDepth) && nextMargin.size())
01106 {
01107 curentMarginList = std::list<int>(nextMargin.begin(), nextMargin.end());
01108 nextMargin.clear();
01109
01110 for(std::list<int>::iterator jter = curentMarginList.begin(); jter!=curentMarginList.end(); ++jter)
01111 {
01112 if(ids.find(*jter) == ids.end())
01113 {
01114
01115
01116 const Signature * s = this->getSignature(*jter);
01117 std::map<int, Link> tmpLinks;
01118 const std::map<int, Link> * links = &tmpLinks;
01119 if(s)
01120 {
01121 ids.insert(std::pair<int, float>(*jter, savedRadius.at(*jter)));
01122
01123 links = &s->getLinks();
01124 }
01125
01126
01127 for(std::map<int, Link>::const_iterator iter=links->begin(); iter!=links->end(); ++iter)
01128 {
01129 if(!uContains(ids, iter->first) &&
01130 uContains(optimizedPoses, iter->first) &&
01131 iter->second.type()!=Link::kVirtualClosure)
01132 {
01133 const Transform & t = optimizedPoses.at(iter->first);
01134 UASSERT(!t.isNull());
01135 float distanceSqrd = referential.getDistanceSquared(t);
01136 if(radiusSqrd == 0 || distanceSqrd<radiusSqrd)
01137 {
01138 savedRadius.insert(std::make_pair(iter->first, distanceSqrd));
01139 nextMargin.insert(iter->first);
01140 }
01141
01142 }
01143 }
01144 }
01145 }
01146 ++m;
01147 }
01148 return ids;
01149 }
01150
01151 int Memory::getNextId()
01152 {
01153 return ++_idCount;
01154 }
01155
01156 int Memory::incrementMapId(std::map<int, int> * reducedIds)
01157 {
01158
01159 const Signature * s = getLastWorkingSignature();
01160 if(s && s->mapId() == _idMapCount)
01161 {
01162
01163 while(_stMem.size())
01164 {
01165 int reducedId = 0;
01166 int id = *_stMem.begin();
01167 moveSignatureToWMFromSTM(id, &reducedId);
01168 if(reducedIds && reducedId > 0)
01169 {
01170 reducedIds->insert(std::make_pair(id, reducedId));
01171 }
01172 }
01173
01174 return ++_idMapCount;
01175 }
01176 return _idMapCount;
01177 }
01178
01179 void Memory::updateAge(int signatureId)
01180 {
01181 std::map<int, double>::iterator iter=_workingMem.find(signatureId);
01182 if(iter!=_workingMem.end())
01183 {
01184 iter->second = UTimer::now();
01185 }
01186 }
01187
01188 int Memory::getDatabaseMemoryUsed() const
01189 {
01190 int memoryUsed = 0;
01191 if(_dbDriver)
01192 {
01193 memoryUsed = _dbDriver->getMemoryUsed()/(1024*1024);
01194 }
01195 return memoryUsed;
01196 }
01197
01198 std::string Memory::getDatabaseVersion() const
01199 {
01200 std::string version = "0.0.0";
01201 if(_dbDriver)
01202 {
01203 version = _dbDriver->getDatabaseVersion();
01204 }
01205 return version;
01206 }
01207
01208 double Memory::getDbSavingTime() const
01209 {
01210 return _dbDriver?_dbDriver->getEmptyTrashesTime():0;
01211 }
01212
01213 std::set<int> Memory::getAllSignatureIds() const
01214 {
01215 std::set<int> ids;
01216 if(_dbDriver)
01217 {
01218 _dbDriver->getAllNodeIds(ids);
01219 }
01220 for(std::map<int, Signature*>::const_iterator iter = _signatures.begin(); iter!=_signatures.end(); ++iter)
01221 {
01222 ids.insert(iter->first);
01223 }
01224 return ids;
01225 }
01226
01227 void Memory::clear()
01228 {
01229 UDEBUG("");
01230
01231
01232 while(_stMem.size())
01233 {
01234 moveSignatureToWMFromSTM(*_stMem.begin());
01235 }
01236 if(_stMem.size() != 0)
01237 {
01238 ULOGGER_ERROR("_stMem must be empty here, size=%d", _stMem.size());
01239 }
01240 _stMem.clear();
01241
01242 this->cleanUnusedWords();
01243
01244 if(_dbDriver)
01245 {
01246 _dbDriver->emptyTrashes();
01247 _dbDriver->join();
01248 }
01249 if(_dbDriver)
01250 {
01251
01252
01253 uSleep(1500);
01254 }
01255
01256
01257 if(_dbDriver && (_stMem.size() || _workingMem.size()))
01258 {
01259 unsigned int memSize = (unsigned int)(_workingMem.size() + _stMem.size());
01260 if(_workingMem.size() && _workingMem.begin()->first < 0)
01261 {
01262 --memSize;
01263 }
01264
01265
01266 UASSERT_MSG(memSize == _signatures.size(),
01267 uFormat("The number of signatures don't match! _workingMem=%d, _stMem=%d, _signatures=%d",
01268 _workingMem.size(), _stMem.size(), _signatures.size()).c_str());
01269
01270 UDEBUG("Adding statistics after run...");
01271 if(_memoryChanged)
01272 {
01273 UDEBUG("");
01274 _dbDriver->addStatisticsAfterRun(memSize,
01275 _lastSignature?_lastSignature->id():0,
01276 UProcessInfo::getMemoryUsage(),
01277 _dbDriver->getMemoryUsed(),
01278 (int)_vwd->getVisualWords().size(),
01279 parameters_);
01280 }
01281 }
01282 UDEBUG("");
01283
01284
01285 std::map<int, Signature*> mem = _signatures;
01286 for(std::map<int, Signature *>::iterator i=mem.begin(); i!=mem.end(); ++i)
01287 {
01288 if(i->second)
01289 {
01290 UDEBUG("deleting from the working and the short-term memory: %d", i->first);
01291 this->moveToTrash(i->second);
01292 }
01293 }
01294
01295 if(_workingMem.size() != 0 && !(_workingMem.size() == 1 && _workingMem.begin()->first == kIdVirtual))
01296 {
01297 ULOGGER_ERROR("_workingMem must be empty here, size=%d", _workingMem.size());
01298 }
01299 _workingMem.clear();
01300 if(_signatures.size()!=0)
01301 {
01302 ULOGGER_ERROR("_signatures must be empty here, size=%d", _signatures.size());
01303 }
01304 _signatures.clear();
01305
01306 UDEBUG("");
01307
01308 if(_dbDriver)
01309 {
01310 _dbDriver->emptyTrashes();
01311 }
01312 UDEBUG("");
01313 _lastSignature = 0;
01314 _lastGlobalLoopClosureId = 0;
01315 _idCount = kIdStart;
01316 _idMapCount = kIdStart;
01317 _memoryChanged = false;
01318 _linksChanged = false;
01319
01320 if(_dbDriver)
01321 {
01322 _dbDriver->join(true);
01323 cleanUnusedWords();
01324 _dbDriver->emptyTrashes();
01325 }
01326 else
01327 {
01328 cleanUnusedWords();
01329 }
01330 if(_vwd)
01331 {
01332 _vwd->clear();
01333 }
01334 UDEBUG("");
01335 }
01336
01342 std::map<int, float> Memory::computeLikelihood(const Signature * signature, const std::list<int> & ids)
01343 {
01344 if(!_tfIdfLikelihoodUsed)
01345 {
01346 UTimer timer;
01347 timer.start();
01348 std::map<int, float> likelihood;
01349
01350 if(!signature)
01351 {
01352 ULOGGER_ERROR("The signature is null");
01353 return likelihood;
01354 }
01355 else if(ids.empty())
01356 {
01357 UWARN("ids list is empty");
01358 return likelihood;
01359 }
01360
01361 for(std::list<int>::const_iterator iter = ids.begin(); iter!=ids.end(); ++iter)
01362 {
01363 float sim = 0.0f;
01364 if(*iter > 0)
01365 {
01366 const Signature * sB = this->getSignature(*iter);
01367 if(!sB)
01368 {
01369 UFATAL("Signature %d not found in WM ?!?", *iter);
01370 }
01371 sim = signature->compareTo(*sB);
01372 }
01373
01374 likelihood.insert(likelihood.end(), std::pair<int, float>(*iter, sim));
01375 }
01376
01377 UDEBUG("compute likelihood (similarity)... %f s", timer.ticks());
01378 return likelihood;
01379 }
01380 else
01381 {
01382 UTimer timer;
01383 timer.start();
01384 std::map<int, float> likelihood;
01385 std::map<int, float> calculatedWordsRatio;
01386
01387 if(!signature)
01388 {
01389 ULOGGER_ERROR("The signature is null");
01390 return likelihood;
01391 }
01392 else if(ids.empty())
01393 {
01394 UWARN("ids list is empty");
01395 return likelihood;
01396 }
01397
01398 for(std::list<int>::const_iterator iter = ids.begin(); iter!=ids.end(); ++iter)
01399 {
01400 likelihood.insert(likelihood.end(), std::pair<int, float>(*iter, 0.0f));
01401 }
01402
01403 const std::list<int> & wordIds = uUniqueKeys(signature->getWords());
01404
01405 float nwi;
01406 float ni;
01407 float nw;
01408 float N;
01409
01410 float logNnw;
01411 const VisualWord * vw;
01412
01413 N = this->getSignatures().size();
01414
01415 if(N)
01416 {
01417 UDEBUG("processing... ");
01418
01419 for(std::list<int>::const_iterator i=wordIds.begin(); i!=wordIds.end(); ++i)
01420 {
01421
01422 vw = _vwd->getWord(*i);
01423 if(vw)
01424 {
01425 const std::map<int, int> & refs = vw->getReferences();
01426 nw = refs.size();
01427 if(nw)
01428 {
01429 logNnw = log10(N/nw);
01430 if(logNnw)
01431 {
01432 for(std::map<int, int>::const_iterator j=refs.begin(); j!=refs.end(); ++j)
01433 {
01434 std::map<int, float>::iterator iter = likelihood.find(j->first);
01435 if(iter != likelihood.end())
01436 {
01437 nwi = j->second;
01438 ni = this->getNi(j->first);
01439 if(ni != 0)
01440 {
01441
01442 iter->second += ( nwi * logNnw ) / ni;
01443 }
01444 }
01445 }
01446 }
01447 }
01448 }
01449 }
01450 }
01451
01452 UDEBUG("compute likelihood (tf-idf) %f s", timer.ticks());
01453 return likelihood;
01454 }
01455 }
01456
01457
01458 std::map<int, int> Memory::getWeights() const
01459 {
01460 std::map<int, int> weights;
01461 for(std::map<int, double>::const_iterator iter=_workingMem.begin(); iter!=_workingMem.end(); ++iter)
01462 {
01463 if(iter->first > 0)
01464 {
01465 const Signature * s = this->getSignature(iter->first);
01466 if(!s)
01467 {
01468 UFATAL("Location %d must exist in memory", iter->first);
01469 }
01470 weights.insert(weights.end(), std::make_pair(iter->first, s->getWeight()));
01471 }
01472 else
01473 {
01474 weights.insert(weights.end(), std::make_pair(iter->first, -1));
01475 }
01476 }
01477 return weights;
01478 }
01479
01480 std::list<int> Memory::forget(const std::set<int> & ignoredIds)
01481 {
01482 UDEBUG("");
01483 std::list<int> signaturesRemoved;
01484 if(this->isIncremental() &&
01485 _vwd->isIncremental() &&
01486 _vwd->getVisualWords().size() &&
01487 !_vwd->isIncrementalFlann())
01488 {
01489
01490
01491
01492
01493 int newWords = 0;
01494 int wordsRemoved = 0;
01495
01496
01497 newWords = _vwd->getNotIndexedWordsCount();
01498
01499
01500
01501 while(wordsRemoved < newWords)
01502 {
01503 std::list<Signature *> signatures = this->getRemovableSignatures(1, ignoredIds);
01504 if(signatures.size())
01505 {
01506 Signature * s = dynamic_cast<Signature *>(signatures.front());
01507 if(s)
01508 {
01509 signaturesRemoved.push_back(s->id());
01510 this->moveToTrash(s);
01511 wordsRemoved = _vwd->getUnusedWordsSize();
01512 }
01513 else
01514 {
01515 break;
01516 }
01517 }
01518 else
01519 {
01520 break;
01521 }
01522 }
01523 UDEBUG("newWords=%d, wordsRemoved=%d", newWords, wordsRemoved);
01524 }
01525 else
01526 {
01527 UDEBUG("");
01528
01529 int signaturesAdded = _signaturesAdded;
01530 std::list<Signature *> signatures = getRemovableSignatures(signaturesAdded+1, ignoredIds);
01531 for(std::list<Signature *>::iterator iter=signatures.begin(); iter!=signatures.end(); ++iter)
01532 {
01533 signaturesRemoved.push_back((*iter)->id());
01534
01535
01536 this->moveToTrash(*iter);
01537 }
01538 if((int)signatures.size() < signaturesAdded)
01539 {
01540 UWARN("Less signatures transferred (%d) than added (%d)! The working memory cannot decrease in size.",
01541 (int)signatures.size(), signaturesAdded);
01542 }
01543 else
01544 {
01545 UDEBUG("signaturesRemoved=%d, _signaturesAdded=%d", (int)signatures.size(), signaturesAdded);
01546 }
01547 }
01548 return signaturesRemoved;
01549 }
01550
01551
01552 int Memory::cleanup()
01553 {
01554 UDEBUG("");
01555 int signatureRemoved = 0;
01556
01557
01558 if(_lastSignature && ((_lastSignature->isBadSignature() && _badSignaturesIgnored) || !_incrementalMemory))
01559 {
01560 if(_lastSignature->isBadSignature())
01561 {
01562 UDEBUG("Bad signature! %d", _lastSignature->id());
01563 }
01564 signatureRemoved = _lastSignature->id();
01565 moveToTrash(_lastSignature, _incrementalMemory);
01566 }
01567
01568 return signatureRemoved;
01569 }
01570
01571 void Memory::emptyTrash()
01572 {
01573 if(_dbDriver)
01574 {
01575 _dbDriver->emptyTrashes(true);
01576 }
01577 }
01578
01579 void Memory::joinTrashThread()
01580 {
01581 if(_dbDriver)
01582 {
01583 UDEBUG("");
01584 _dbDriver->join();
01585 UDEBUG("");
01586 }
01587 }
01588
01589 class WeightAgeIdKey
01590 {
01591 public:
01592 WeightAgeIdKey(int w, double a, int i) :
01593 weight(w),
01594 age(a),
01595 id(i){}
01596 bool operator<(const WeightAgeIdKey & k) const
01597 {
01598 if(weight < k.weight)
01599 {
01600 return true;
01601 }
01602 else if(weight == k.weight)
01603 {
01604 if(age < k.age)
01605 {
01606 return true;
01607 }
01608 else if(age == k.age)
01609 {
01610 if(id < k.id)
01611 {
01612 return true;
01613 }
01614 }
01615 }
01616 return false;
01617 }
01618 int weight, age, id;
01619 };
01620 std::list<Signature *> Memory::getRemovableSignatures(int count, const std::set<int> & ignoredIds)
01621 {
01622
01623 std::list<Signature *> removableSignatures;
01624 std::map<WeightAgeIdKey, Signature *> weightAgeIdMap;
01625
01626
01627 UDEBUG("mem.size()=%d, ignoredIds.size()=%d", (int)_workingMem.size(), (int)ignoredIds.size());
01628
01629 if(_workingMem.size())
01630 {
01631 int recentWmMaxSize = _recentWmRatio * float(_workingMem.size());
01632 bool recentWmImmunized = false;
01633
01634 int currentRecentWmSize = 0;
01635 if(_lastGlobalLoopClosureId > 0 && _stMem.find(_lastGlobalLoopClosureId) == _stMem.end())
01636 {
01637
01638 std::map<int, double>::const_iterator iter = _workingMem.find(_lastGlobalLoopClosureId);
01639 while(iter != _workingMem.end())
01640 {
01641 ++currentRecentWmSize;
01642 ++iter;
01643 }
01644 if(currentRecentWmSize>1 && currentRecentWmSize < recentWmMaxSize)
01645 {
01646 recentWmImmunized = true;
01647 }
01648 else if(currentRecentWmSize == 0 && _workingMem.size() > 1)
01649 {
01650 UERROR("Last loop closure id not found in WM (%d)", _lastGlobalLoopClosureId);
01651 }
01652 UDEBUG("currentRecentWmSize=%d, recentWmMaxSize=%d, _recentWmRatio=%f, end recent wM = %d", currentRecentWmSize, recentWmMaxSize, _recentWmRatio, _lastGlobalLoopClosureId);
01653 }
01654
01655
01656 Signature * lastInSTM = 0;
01657 if(_stMem.size())
01658 {
01659 lastInSTM = _signatures.at(*_stMem.begin());
01660 }
01661
01662 for(std::map<int, double>::const_iterator memIter = _workingMem.begin(); memIter != _workingMem.end(); ++memIter)
01663 {
01664 if( (recentWmImmunized && memIter->first > _lastGlobalLoopClosureId) ||
01665 memIter->first == _lastGlobalLoopClosureId)
01666 {
01667
01668 }
01669 else if(memIter->first > 0 && ignoredIds.find(memIter->first) == ignoredIds.end() && (!lastInSTM || !lastInSTM->hasLink(memIter->first)))
01670 {
01671 Signature * s = this->_getSignature(memIter->first);
01672 if(s)
01673 {
01674
01675 bool foundInSTM = false;
01676 for(std::map<int, Link>::const_iterator iter = s->getLinks().begin(); iter!=s->getLinks().end(); ++iter)
01677 {
01678 if(_stMem.find(iter->first) != _stMem.end())
01679 {
01680 UDEBUG("Ignored %d because it has a link (%d) to STM", s->id(), iter->first);
01681 foundInSTM = true;
01682 break;
01683 }
01684 }
01685 if(!foundInSTM)
01686 {
01687
01688 weightAgeIdMap.insert(std::make_pair(WeightAgeIdKey(s->getWeight(), _transferSortingByWeightId?0.0:memIter->second, s->id()), s));
01689 }
01690 }
01691 else
01692 {
01693 ULOGGER_ERROR("Not supposed to occur!!!");
01694 }
01695 }
01696 else
01697 {
01698
01699 }
01700 }
01701
01702 int recentWmCount = 0;
01703
01704
01705 UDEBUG("signatureMap.size()=%d _lastGlobalLoopClosureId=%d currentRecentWmSize=%d recentWmMaxSize=%d",
01706 (int)weightAgeIdMap.size(), _lastGlobalLoopClosureId, currentRecentWmSize, recentWmMaxSize);
01707 for(std::map<WeightAgeIdKey, Signature*>::iterator iter=weightAgeIdMap.begin();
01708 iter!=weightAgeIdMap.end();
01709 ++iter)
01710 {
01711 if(!recentWmImmunized)
01712 {
01713 UDEBUG("weight=%d, id=%d",
01714 iter->second->getWeight(),
01715 iter->second->id());
01716 removableSignatures.push_back(iter->second);
01717
01718 if(_lastGlobalLoopClosureId && iter->second->id() > _lastGlobalLoopClosureId)
01719 {
01720 ++recentWmCount;
01721 if(currentRecentWmSize - recentWmCount < recentWmMaxSize)
01722 {
01723 UDEBUG("switched recentWmImmunized");
01724 recentWmImmunized = true;
01725 }
01726 }
01727 }
01728 else if(_lastGlobalLoopClosureId == 0 || iter->second->id() < _lastGlobalLoopClosureId)
01729 {
01730 UDEBUG("weight=%d, id=%d",
01731 iter->second->getWeight(),
01732 iter->second->id());
01733 removableSignatures.push_back(iter->second);
01734 }
01735 if(removableSignatures.size() >= (unsigned int)count)
01736 {
01737 break;
01738 }
01739 }
01740 }
01741 else
01742 {
01743 ULOGGER_WARN("not enough signatures to get an old one...");
01744 }
01745 return removableSignatures;
01746 }
01747
01751 void Memory::moveToTrash(Signature * s, bool keepLinkedToGraph, std::list<int> * deletedWords)
01752 {
01753 UDEBUG("id=%d", s?s->id():0);
01754 if(s)
01755 {
01756
01757 if(!keepLinkedToGraph || (!s->isSaved() && s->isBadSignature() && _badSignaturesIgnored))
01758 {
01759 UASSERT_MSG(this->isInSTM(s->id()),
01760 uFormat("Deleting location (%d) outside the "
01761 "STM is not implemented!", s->id()).c_str());
01762 const std::map<int, Link> & links = s->getLinks();
01763 for(std::map<int, Link>::const_iterator iter=links.begin(); iter!=links.end(); ++iter)
01764 {
01765 Signature * sTo = this->_getSignature(iter->first);
01766
01767 UASSERT_MSG(sTo!=0,
01768 uFormat("A neighbor (%d) of the deleted location %d is "
01769 "not found in WM/STM! Are you deleting a location "
01770 "outside the STM?", iter->first, s->id()).c_str());
01771
01772 if(iter->first > s->id() && links.size()>1 && sTo->hasLink(s->id()))
01773 {
01774 UWARN("Link %d of %d is newer, removing neighbor link "
01775 "may split the map!",
01776 iter->first, s->id());
01777 }
01778
01779
01780 if(iter->second.type() == Link::kGlobalClosure && s->id() > sTo->id())
01781 {
01782 sTo->setWeight(sTo->getWeight() + s->getWeight());
01783 }
01784
01785 sTo->removeLink(s->id());
01786
01787 }
01788 s->removeLinks();
01789 s->setWeight(0);
01790 s->setLabel("");
01791 }
01792 else
01793 {
01794
01795
01796
01797 removeVirtualLinks(s->id());
01798 }
01799
01800 this->disableWordsRef(s->id());
01801 if(!keepLinkedToGraph)
01802 {
01803 std::list<int> keys = uUniqueKeys(s->getWords());
01804 for(std::list<int>::const_iterator i=keys.begin(); i!=keys.end(); ++i)
01805 {
01806
01807 VisualWord * w = _vwd->getUnusedWord(*i);
01808 if(w)
01809 {
01810 std::vector<VisualWord*> wordToDelete;
01811 wordToDelete.push_back(w);
01812 _vwd->removeWords(wordToDelete);
01813 if(deletedWords)
01814 {
01815 deletedWords->push_back(w->id());
01816 }
01817 delete w;
01818 }
01819 }
01820 }
01821
01822 _workingMem.erase(s->id());
01823 _stMem.erase(s->id());
01824 _signatures.erase(s->id());
01825 if(_signaturesAdded>0)
01826 {
01827 --_signaturesAdded;
01828 }
01829
01830 if(_lastSignature == s)
01831 {
01832 _lastSignature = 0;
01833 if(_stMem.size())
01834 {
01835 _lastSignature = this->_getSignature(*_stMem.rbegin());
01836 }
01837 else if(_workingMem.size())
01838 {
01839 _lastSignature = this->_getSignature(_workingMem.rbegin()->first);
01840 }
01841 }
01842
01843 if(_lastGlobalLoopClosureId == s->id())
01844 {
01845 _lastGlobalLoopClosureId = 0;
01846 }
01847
01848 if( (_notLinkedNodesKeptInDb || keepLinkedToGraph) &&
01849 _dbDriver &&
01850 s->id()>0 &&
01851 (_incrementalMemory || s->isSaved()))
01852 {
01853 _dbDriver->asyncSave(s);
01854 }
01855 else
01856 {
01857 delete s;
01858 }
01859 }
01860 }
01861
01862 int Memory::getLastSignatureId() const
01863 {
01864 return _idCount;
01865 }
01866
01867 const Signature * Memory::getLastWorkingSignature() const
01868 {
01869 UDEBUG("");
01870 return _lastSignature;
01871 }
01872
01873 int Memory::getSignatureIdByLabel(const std::string & label, bool lookInDatabase) const
01874 {
01875 UDEBUG("label=%s", label.c_str());
01876 int id = 0;
01877 if(label.size())
01878 {
01879 for(std::map<int, Signature*>::const_iterator iter=_signatures.begin(); iter!=_signatures.end(); ++iter)
01880 {
01881 UASSERT(iter->second != 0);
01882 if(iter->second->getLabel().compare(label) == 0)
01883 {
01884 id = iter->second->id();
01885 break;
01886 }
01887 }
01888 if(id == 0 && _dbDriver && lookInDatabase)
01889 {
01890 _dbDriver->getNodeIdByLabel(label, id);
01891 }
01892 }
01893 return id;
01894 }
01895
01896 bool Memory::labelSignature(int id, const std::string & label)
01897 {
01898
01899 int idFound=getSignatureIdByLabel(label);
01900 if(idFound == 0 || idFound == id)
01901 {
01902 Signature * s = this->_getSignature(id);
01903 if(s)
01904 {
01905 s->setLabel(label);
01906 _linksChanged = s->isSaved();
01907 UWARN("Label \"%s\" set to node %d", label.c_str(), id);
01908 return true;
01909 }
01910 else if(_dbDriver)
01911 {
01912 std::list<int> ids;
01913 ids.push_back(id);
01914 std::list<Signature *> signatures;
01915 _dbDriver->loadSignatures(ids,signatures);
01916 if(signatures.size())
01917 {
01918 signatures.front()->setLabel(label);
01919 UWARN("Label \"%s\" set to node %d", label.c_str(), id);
01920 _dbDriver->asyncSave(signatures.front());
01921 return true;
01922 }
01923 }
01924 else
01925 {
01926 UERROR("Node %d not found, failed to set label \"%s\"!", id, label.c_str());
01927 }
01928 }
01929 else if(idFound)
01930 {
01931 UWARN("Node %d has already label \"%s\"", idFound, label.c_str());
01932 }
01933 return false;
01934 }
01935
01936 std::map<int, std::string> Memory::getAllLabels() const
01937 {
01938 std::map<int, std::string> labels;
01939 for(std::map<int, Signature*>::const_iterator iter = _signatures.begin(); iter!=_signatures.end(); ++iter)
01940 {
01941 if(!iter->second->getLabel().empty())
01942 {
01943 labels.insert(std::make_pair(iter->first, iter->second->getLabel()));
01944 }
01945 }
01946 if(_dbDriver)
01947 {
01948 _dbDriver->getAllLabels(labels);
01949 }
01950 return labels;
01951 }
01952
01953 bool Memory::setUserData(int id, const cv::Mat & data)
01954 {
01955 Signature * s = this->_getSignature(id);
01956 if(s)
01957 {
01958 s->sensorData().setUserData(data);
01959 return true;
01960 }
01961 else
01962 {
01963 UERROR("Node %d not found in RAM, failed to set user data (size=%d)!", id, data.total());
01964 }
01965 return false;
01966 }
01967
01968 void Memory::deleteLocation(int locationId, std::list<int> * deletedWords)
01969 {
01970 UDEBUG("Deleting location %d", locationId);
01971 Signature * location = _getSignature(locationId);
01972 if(location)
01973 {
01974 this->moveToTrash(location, false, deletedWords);
01975 }
01976 }
01977
01978 void Memory::removeLink(int oldId, int newId)
01979 {
01980
01981 Signature * oldS = this->_getSignature(oldId<newId?oldId:newId);
01982 Signature * newS = this->_getSignature(oldId<newId?newId:oldId);
01983 if(oldS && newS)
01984 {
01985 UINFO("removing link between location %d and %d", oldS->id(), newS->id());
01986
01987 if(oldS->hasLink(newS->id()) && newS->hasLink(oldS->id()))
01988 {
01989 Link::Type type = oldS->getLinks().at(newS->id()).type();
01990 if(type == Link::kGlobalClosure && newS->getWeight() > 0)
01991 {
01992
01993 oldS->setWeight(oldS->getWeight()+1);
01994 newS->setWeight(newS->getWeight()>0?newS->getWeight()-1:0);
01995 }
01996
01997
01998 oldS->removeLink(newS->id());
01999 newS->removeLink(oldS->id());
02000
02001 if(type!=Link::kVirtualClosure)
02002 {
02003 _linksChanged = true;
02004 }
02005
02006 bool noChildrenAnymore = true;
02007 for(std::map<int, Link>::const_iterator iter=newS->getLinks().begin(); iter!=newS->getLinks().end(); ++iter)
02008 {
02009 if(iter->second.type() != Link::kNeighbor &&
02010 iter->second.type() != Link::kNeighborMerged &&
02011 iter->first < newS->id())
02012 {
02013 noChildrenAnymore = false;
02014 break;
02015 }
02016 }
02017 if(noChildrenAnymore && newS->id() == _lastGlobalLoopClosureId)
02018 {
02019 _lastGlobalLoopClosureId = 0;
02020 }
02021 }
02022 else
02023 {
02024 UERROR("Signatures %d and %d don't have bidirectional link!", oldS->id(), newS->id());
02025 }
02026 }
02027 else
02028 {
02029 if(!newS)
02030 {
02031 UERROR("Signature %d is not in working memory... cannot remove link.", newS->id());
02032 }
02033 if(!oldS)
02034 {
02035 UERROR("Signature %d is not in working memory... cannot remove link.", oldS->id());
02036 }
02037 }
02038 }
02039
02040 void Memory::removeRawData(int id, bool image, bool scan, bool userData)
02041 {
02042 Signature * s = this->_getSignature(id);
02043 if(s)
02044 {
02045 if(image && (!_reextractLoopClosureFeatures || !_registrationPipeline->isImageRequired()))
02046 {
02047 s->sensorData().setImageRaw(cv::Mat());
02048 s->sensorData().setDepthOrRightRaw(cv::Mat());
02049 }
02050 if(scan && !_registrationPipeline->isScanRequired())
02051 {
02052 s->sensorData().setLaserScanRaw(cv::Mat(), s->sensorData().laserScanMaxPts(), s->sensorData().laserScanMaxRange());
02053 }
02054 if(userData && !_registrationPipeline->isUserDataRequired())
02055 {
02056 s->sensorData().setUserDataRaw(cv::Mat());
02057 }
02058 }
02059 }
02060
02061
02062 Transform Memory::computeTransform(
02063 int fromId,
02064 int toId,
02065 Transform guess,
02066 RegistrationInfo * info)
02067 {
02068 Signature * fromS = this->_getSignature(fromId);
02069 Signature * toS = this->_getSignature(toId);
02070
02071 Transform transform;
02072
02073 if(fromS && toS)
02074 {
02075 return computeTransform(*fromS, *toS, guess, info);
02076 }
02077 else
02078 {
02079 std::string msg = uFormat("Did not find nodes %d and/or %d", fromId, toId);
02080 if(info)
02081 {
02082 info->rejectedMsg = msg;
02083 }
02084 UWARN(msg.c_str());
02085 }
02086 return transform;
02087 }
02088
02089
02090 Transform Memory::computeTransform(
02091 Signature & fromS,
02092 Signature & toS,
02093 Transform guess,
02094 RegistrationInfo * info) const
02095 {
02096 Transform transform;
02097
02098
02099
02100 if((_reextractLoopClosureFeatures && _registrationPipeline->isImageRequired() && fromS.sensorData().imageCompressed().empty()) ||
02101 (_registrationPipeline->isScanRequired() && fromS.sensorData().imageCompressed().empty() && fromS.sensorData().laserScanCompressed().empty()) ||
02102 (_registrationPipeline->isUserDataRequired() && fromS.sensorData().imageCompressed().empty() && fromS.sensorData().userDataCompressed().empty()))
02103 {
02104 fromS.sensorData() = getNodeData(fromS.id());
02105 }
02106 if((_reextractLoopClosureFeatures && _registrationPipeline->isImageRequired() && toS.sensorData().imageCompressed().empty()) ||
02107 (_registrationPipeline->isScanRequired() && toS.sensorData().imageCompressed().empty() && toS.sensorData().laserScanCompressed().empty()) ||
02108 (_registrationPipeline->isUserDataRequired() && toS.sensorData().imageCompressed().empty() && toS.sensorData().userDataCompressed().empty()))
02109 {
02110 toS.sensorData() = getNodeData(toS.id());
02111 }
02112
02113 cv::Mat imgBuf, depthBuf, laserBuf, userBuf;
02114 fromS.sensorData().uncompressData(
02115 (_reextractLoopClosureFeatures && _registrationPipeline->isImageRequired())?&imgBuf:0,
02116 (_reextractLoopClosureFeatures && _registrationPipeline->isImageRequired())?&depthBuf:0,
02117 _registrationPipeline->isScanRequired()?&laserBuf:0,
02118 _registrationPipeline->isUserDataRequired()?&userBuf:0);
02119 toS.sensorData().uncompressData(
02120 (_reextractLoopClosureFeatures && _registrationPipeline->isImageRequired())?&imgBuf:0,
02121 (_reextractLoopClosureFeatures && _registrationPipeline->isImageRequired())?&depthBuf:0,
02122 _registrationPipeline->isScanRequired()?&laserBuf:0,
02123 _registrationPipeline->isUserDataRequired()?&userBuf:0);
02124
02125
02126
02127 std::vector<int> inliersV;
02128 if(_reextractLoopClosureFeatures ||
02129 (fromS.getWords().size() && toS.getWords().size()) ||
02130 (!guess.isNull() && !_registrationPipeline->isImageRequired()))
02131 {
02132 Signature tmpFrom = fromS;
02133 Signature tmpTo = toS;
02134
02135 if(_reextractLoopClosureFeatures)
02136 {
02137 UDEBUG("");
02138 tmpFrom.setWords(std::multimap<int, cv::KeyPoint>());
02139 tmpFrom.setWords3(std::multimap<int, cv::Point3f>());
02140 tmpFrom.setWordsDescriptors(std::multimap<int, cv::Mat>());
02141 tmpFrom.sensorData().setFeatures(std::vector<cv::KeyPoint>(), cv::Mat());
02142 tmpTo.setWords(std::multimap<int, cv::KeyPoint>());
02143 tmpTo.setWords3(std::multimap<int, cv::Point3f>());
02144 tmpTo.setWordsDescriptors(std::multimap<int, cv::Mat>());
02145 tmpTo.sensorData().setFeatures(std::vector<cv::KeyPoint>(), cv::Mat());
02146 }
02147
02148 if(guess.isNull() && !_registrationPipeline->isImageRequired())
02149 {
02150 UDEBUG("");
02151
02152 RegistrationVis regVis(parameters_);
02153 guess = regVis.computeTransformation(tmpFrom, tmpTo, guess, info);
02154 if(!guess.isNull())
02155 {
02156 transform = _registrationPipeline->computeTransformation(tmpFrom, tmpTo, guess, info);
02157 }
02158 }
02159 else
02160 {
02161 transform = _registrationPipeline->computeTransformation(tmpFrom, tmpTo, guess, info);
02162 }
02163
02164 if(!transform.isNull())
02165 {
02166 UDEBUG("");
02167
02168 float x,y,z, roll,pitch,yaw;
02169 transform.getTranslationAndEulerAngles(x,y,z, roll,pitch,yaw);
02170 if(fabs(roll) > CV_PI/2 ||
02171 fabs(pitch) > CV_PI/2 ||
02172 fabs(yaw) > CV_PI/2)
02173 {
02174 transform.setNull();
02175 std::string msg = uFormat("Too large rotation detected! (roll=%f, pitch=%f, yaw=%f)",
02176 roll, pitch, yaw);
02177 UINFO(msg.c_str());
02178 if(info)
02179 {
02180 info->rejectedMsg = msg;
02181 }
02182 }
02183 }
02184 }
02185 return transform;
02186 }
02187
02188
02189 Transform Memory::computeIcpTransform(
02190 int fromId,
02191 int toId,
02192 Transform guess,
02193 RegistrationInfo * info)
02194 {
02195 Signature * fromS = this->_getSignature(fromId);
02196 Signature * toS = this->_getSignature(toId);
02197
02198 if(fromS && toS && _dbDriver)
02199 {
02200 std::list<Signature*> depthsToLoad;
02201
02202 if(fromS->sensorData().imageCompressed().empty() &&
02203 fromS->sensorData().laserScanCompressed().empty())
02204 {
02205 depthsToLoad.push_back(fromS);
02206 }
02207 if(toS->sensorData().imageCompressed().empty() &&
02208 toS->sensorData().laserScanCompressed().empty())
02209 {
02210 depthsToLoad.push_back(toS);
02211 }
02212
02213 if(depthsToLoad.size())
02214 {
02215 _dbDriver->loadNodeData(depthsToLoad);
02216 }
02217 }
02218
02219 Transform t;
02220 if(fromS && toS)
02221 {
02222
02223 cv::Mat tmp1, tmp2;
02224 fromS->sensorData().uncompressData(0, 0, &tmp1);
02225 toS->sensorData().uncompressData(0, 0, &tmp2);
02226
02227
02228 std::vector<int> inliersV;
02229 t = _registrationIcp->computeTransformation(fromS->sensorData(), toS->sensorData(), guess, info);
02230 }
02231 else
02232 {
02233 std::string msg = uFormat("Did not find nodes %d and/or %d", fromId, toId);
02234 if(info)
02235 {
02236 info->rejectedMsg = msg;
02237 }
02238 UWARN(msg.c_str());
02239 }
02240 return t;
02241 }
02242
02243
02244 Transform Memory::computeIcpTransformMulti(
02245 int fromId,
02246 int toId,
02247 const std::map<int, Transform> & poses,
02248 RegistrationInfo * info)
02249 {
02250 UASSERT(uContains(poses, fromId) && uContains(_signatures, fromId));
02251 UASSERT(uContains(poses, toId) && uContains(_signatures, toId));
02252
02253 UDEBUG("Guess=%s", (poses.at(fromId).inverse() * poses.at(toId)).prettyPrint().c_str());
02254
02255
02256 std::list<Signature*> depthToLoad;
02257 for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
02258 {
02259 Signature * s = _getSignature(iter->first);
02260 UASSERT(s != 0);
02261
02262 if(s->sensorData().imageCompressed().empty() &&
02263 s->sensorData().laserScanCompressed().empty())
02264 {
02265 depthToLoad.push_back(s);
02266 }
02267 }
02268 if(depthToLoad.size() && _dbDriver)
02269 {
02270 _dbDriver->loadNodeData(depthToLoad);
02271 }
02272
02273 Signature * fromS = _getSignature(fromId);
02274 cv::Mat fromScan;
02275 fromS->sensorData().uncompressData(0, 0, &fromScan);
02276
02277 Transform t;
02278 if(!fromScan.empty())
02279 {
02280
02281 SensorData assembledData;
02282 Transform toPose = poses.at(toId);
02283 std::string msg;
02284 int maxPoints = fromScan.cols;
02285 pcl::PointCloud<pcl::PointXYZ>::Ptr assembledToClouds(new pcl::PointCloud<pcl::PointXYZ>);
02286 for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
02287 {
02288 if(iter->first != fromId)
02289 {
02290 Signature * s = this->_getSignature(iter->first);
02291 if(!s->sensorData().laserScanCompressed().empty())
02292 {
02293 cv::Mat scan;
02294 s->sensorData().uncompressData(0, 0, &scan);
02295 pcl::PointCloud<pcl::PointXYZ>::Ptr cloud = util3d::laserScanToPointCloud(scan, toPose.inverse() * iter->second);
02296 if(scan.cols > maxPoints)
02297 {
02298 maxPoints = scan.cols;
02299 }
02300 *assembledToClouds += *cloud;
02301 }
02302 else
02303 {
02304 UWARN("Depth2D not found for signature %d", iter->first);
02305 }
02306 }
02307 }
02308 if(assembledToClouds->size())
02309 {
02310 assembledData.setLaserScanRaw(util3d::laserScanFromPointCloud(*assembledToClouds, Transform()), fromS->sensorData().laserScanMaxPts()?fromS->sensorData().laserScanMaxPts():maxPoints, fromS->sensorData().laserScanMaxRange());
02311 }
02312
02313 Transform guess = poses.at(fromId).inverse() * poses.at(toId);
02314 std::vector<int> inliersV;
02315 t = _registrationIcp->computeTransformation(fromS->sensorData(), assembledData, guess, info);
02316 }
02317
02318 return t;
02319 }
02320
02321 bool Memory::addLink(const Link & link, bool addInDatabase)
02322 {
02323 UASSERT(link.type() > Link::kNeighbor && link.type() != Link::kUndef);
02324
02325 ULOGGER_INFO("to=%d, from=%d transform: %s var=%f", link.to(), link.from(), link.transform().prettyPrint().c_str(), link.transVariance());
02326 Signature * toS = _getSignature(link.to());
02327 Signature * fromS = _getSignature(link.from());
02328 if(toS && fromS)
02329 {
02330 if(toS->hasLink(link.from()))
02331 {
02332
02333 UINFO("already linked! to=%d, from=%d", link.to(), link.from());
02334 return true;
02335 }
02336
02337 UDEBUG("Add link between %d and %d", toS->id(), fromS->id());
02338
02339 toS->addLink(link.inverse());
02340 fromS->addLink(link);
02341
02342 if(_incrementalMemory)
02343 {
02344 if(link.type()!=Link::kVirtualClosure)
02345 {
02346 _linksChanged = true;
02347
02348
02349
02350 if(link.type() != Link::kLocalSpaceClosure ||
02351 link.userDataCompressed().empty())
02352 {
02353 _lastGlobalLoopClosureId = fromS->id()>toS->id()?fromS->id():toS->id();
02354
02355
02356
02357 UASSERT(fromS->getWeight() >= 0 && toS->getWeight() >=0);
02358 if((_reduceGraph && fromS->id() < toS->id()) ||
02359 (!_reduceGraph && fromS->id() > toS->id()))
02360 {
02361 fromS->setWeight(fromS->getWeight() + toS->getWeight());
02362 toS->setWeight(0);
02363 }
02364 else
02365 {
02366 toS->setWeight(toS->getWeight() + fromS->getWeight());
02367 fromS->setWeight(0);
02368 }
02369 }
02370 }
02371 }
02372 }
02373 else if(!addInDatabase)
02374 {
02375 if(!fromS)
02376 {
02377 UERROR("from=%d, to=%d, Signature %d not found in working/st memories", link.from(), link.to(), link.from());
02378 }
02379 if(!toS)
02380 {
02381 UERROR("from=%d, to=%d, Signature %d not found in working/st memories", link.from(), link.to(), link.to());
02382 }
02383 return false;
02384 }
02385 else if(fromS)
02386 {
02387 UDEBUG("Add link between %d and %d (db)", link.from(), link.to());
02388 fromS->addLink(link);
02389 _dbDriver->addLink(link.inverse());
02390 }
02391 else if(toS)
02392 {
02393 UDEBUG("Add link between %d (db) and %d", link.from(), link.to());
02394 _dbDriver->addLink(link);
02395 toS->addLink(link.inverse());
02396 }
02397 else
02398 {
02399 UDEBUG("Add link between %d (db) and %d (db)", link.from(), link.to());
02400 _dbDriver->addLink(link);
02401 _dbDriver->addLink(link.inverse());
02402 }
02403 return true;
02404 }
02405
02406 void Memory::updateLink(int fromId, int toId, const Transform & transform, float rotVariance, float transVariance)
02407 {
02408 Signature * fromS = this->_getSignature(fromId);
02409 Signature * toS = this->_getSignature(toId);
02410
02411 if(fromS->hasLink(toId) && toS->hasLink(fromId))
02412 {
02413 Link::Type type = fromS->getLinks().at(toId).type();
02414 fromS->removeLink(toId);
02415 toS->removeLink(fromId);
02416
02417 fromS->addLink(Link(fromId, toId, type, transform, rotVariance, transVariance));
02418 toS->addLink(Link(toId, fromId, type, transform.inverse(), rotVariance, transVariance));
02419
02420 if(type!=Link::kVirtualClosure)
02421 {
02422 _linksChanged = true;
02423 }
02424 }
02425 else
02426 {
02427 UERROR("fromId=%d and toId=%d are not linked!", fromId, toId);
02428 }
02429 }
02430
02431 void Memory::updateLink(int fromId, int toId, const Transform & transform, const cv::Mat & covariance)
02432 {
02433 Signature * fromS = this->_getSignature(fromId);
02434 Signature * toS = this->_getSignature(toId);
02435
02436 if(fromS->hasLink(toId) && toS->hasLink(fromId))
02437 {
02438 Link::Type type = fromS->getLinks().at(toId).type();
02439 fromS->removeLink(toId);
02440 toS->removeLink(fromId);
02441
02442 cv::Mat infMatrix = covariance.inv();
02443 fromS->addLink(Link(fromId, toId, type, transform, infMatrix));
02444 toS->addLink(Link(toId, fromId, type, transform.inverse(), infMatrix));
02445
02446 if(type!=Link::kVirtualClosure)
02447 {
02448 _linksChanged = true;
02449 }
02450 }
02451 else
02452 {
02453 UERROR("fromId=%d and toId=%d are not linked!", fromId, toId);
02454 }
02455 }
02456
02457 void Memory::removeAllVirtualLinks()
02458 {
02459 UDEBUG("");
02460 for(std::map<int, Signature*>::iterator iter=_signatures.begin(); iter!=_signatures.end(); ++iter)
02461 {
02462 iter->second->removeVirtualLinks();
02463 }
02464 }
02465
02466 void Memory::removeVirtualLinks(int signatureId)
02467 {
02468 UDEBUG("");
02469 Signature * s = this->_getSignature(signatureId);
02470 if(s)
02471 {
02472 const std::map<int, Link> & links = s->getLinks();
02473 for(std::map<int, Link>::const_iterator iter=links.begin(); iter!=links.end(); ++iter)
02474 {
02475 if(iter->second.type() == Link::kVirtualClosure)
02476 {
02477 Signature * sTo = this->_getSignature(iter->first);
02478 if(sTo)
02479 {
02480 sTo->removeLink(s->id());
02481 }
02482 else
02483 {
02484 UERROR("Link %d of %d not in WM/STM?!?", iter->first, s->id());
02485 }
02486 }
02487 }
02488 s->removeVirtualLinks();
02489 }
02490 else
02491 {
02492 UERROR("Signature %d not in WM/STM?!?", signatureId);
02493 }
02494 }
02495
02496 void Memory::dumpMemory(std::string directory) const
02497 {
02498 UINFO("Dumping memory to directory \"%s\"", directory.c_str());
02499 this->dumpDictionary((directory+"/DumpMemoryWordRef.txt").c_str(), (directory+"/DumpMemoryWordDesc.txt").c_str());
02500 this->dumpSignatures((directory + "/DumpMemorySign.txt").c_str(), false);
02501 this->dumpSignatures((directory + "/DumpMemorySign3.txt").c_str(), true);
02502 this->dumpMemoryTree((directory + "/DumpMemoryTree.txt").c_str());
02503 }
02504
02505 void Memory::dumpDictionary(const char * fileNameRef, const char * fileNameDesc) const
02506 {
02507 if(_vwd)
02508 {
02509 _vwd->exportDictionary(fileNameRef, fileNameDesc);
02510 }
02511 }
02512
02513 void Memory::dumpSignatures(const char * fileNameSign, bool words3D) const
02514 {
02515 UDEBUG("");
02516 FILE* foutSign = 0;
02517 #ifdef _MSC_VER
02518 fopen_s(&foutSign, fileNameSign, "w");
02519 #else
02520 foutSign = fopen(fileNameSign, "w");
02521 #endif
02522
02523 if(foutSign)
02524 {
02525 fprintf(foutSign, "SignatureID WordsID...\n");
02526 const std::map<int, Signature *> & signatures = this->getSignatures();
02527 for(std::map<int, Signature *>::const_iterator iter=signatures.begin(); iter!=signatures.end(); ++iter)
02528 {
02529 fprintf(foutSign, "%d ", iter->first);
02530 const Signature * ss = dynamic_cast<const Signature *>(iter->second);
02531 if(ss)
02532 {
02533 if(words3D)
02534 {
02535 const std::multimap<int, cv::Point3f> & ref = ss->getWords3();
02536 for(std::multimap<int, cv::Point3f>::const_iterator jter=ref.begin(); jter!=ref.end(); ++jter)
02537 {
02538
02539 if(pcl::isFinite(jter->second) &&
02540 (jter->second.x != 0 || jter->second.y != 0 || jter->second.z != 0))
02541 {
02542 fprintf(foutSign, "%d ", (*jter).first);
02543 }
02544 }
02545 }
02546 else
02547 {
02548 const std::multimap<int, cv::KeyPoint> & ref = ss->getWords();
02549 for(std::multimap<int, cv::KeyPoint>::const_iterator jter=ref.begin(); jter!=ref.end(); ++jter)
02550 {
02551 fprintf(foutSign, "%d ", (*jter).first);
02552 }
02553 }
02554 }
02555 fprintf(foutSign, "\n");
02556 }
02557 fclose(foutSign);
02558 }
02559 }
02560
02561 void Memory::dumpMemoryTree(const char * fileNameTree) const
02562 {
02563 UDEBUG("");
02564 FILE* foutTree = 0;
02565 #ifdef _MSC_VER
02566 fopen_s(&foutTree, fileNameTree, "w");
02567 #else
02568 foutTree = fopen(fileNameTree, "w");
02569 #endif
02570
02571 if(foutTree)
02572 {
02573 fprintf(foutTree, "SignatureID Weight NbLoopClosureIds LoopClosureIds... NbChildLoopClosureIds ChildLoopClosureIds...\n");
02574
02575 for(std::map<int, Signature *>::const_iterator i=_signatures.begin(); i!=_signatures.end(); ++i)
02576 {
02577 fprintf(foutTree, "%d %d", i->first, i->second->getWeight());
02578
02579 std::map<int, Link> loopIds, childIds;
02580
02581 for(std::map<int, Link>::const_iterator iter = i->second->getLinks().begin();
02582 iter!=i->second->getLinks().end();
02583 ++iter)
02584 {
02585 if(iter->second.type() != Link::kNeighbor &&
02586 iter->second.type() != Link::kNeighborMerged)
02587 {
02588 if(iter->first < i->first)
02589 {
02590 childIds.insert(*iter);
02591 }
02592 else
02593 {
02594 loopIds.insert(*iter);
02595 }
02596 }
02597 }
02598
02599 fprintf(foutTree, " %d", (int)loopIds.size());
02600 for(std::map<int, Link>::const_iterator j=loopIds.begin(); j!=loopIds.end(); ++j)
02601 {
02602 fprintf(foutTree, " %d", j->first);
02603 }
02604
02605 fprintf(foutTree, " %d", (int)childIds.size());
02606 for(std::map<int, Link>::const_iterator j=childIds.begin(); j!=childIds.end(); ++j)
02607 {
02608 fprintf(foutTree, " %d", j->first);
02609 }
02610
02611 fprintf(foutTree, "\n");
02612 }
02613
02614 fclose(foutTree);
02615 }
02616
02617 }
02618
02619 void Memory::rehearsal(Signature * signature, Statistics * stats)
02620 {
02621 UTimer timer;
02622 if(signature->getLinks().size() != 1 ||
02623 signature->isBadSignature())
02624 {
02625 return;
02626 }
02627
02628
02629
02630
02631 Signature * sB = 0;
02632 for(std::set<int>::reverse_iterator iter=_stMem.rbegin(); iter!=_stMem.rend(); ++iter)
02633 {
02634 Signature * s = this->_getSignature(*iter);
02635 UASSERT(s!=0);
02636 if(s->getWeight() >= 0 && s->id() != signature->id())
02637 {
02638 sB = s;
02639 break;
02640 }
02641 }
02642 if(sB)
02643 {
02644 int id = sB->id();
02645 UDEBUG("Comparing with signature (%d)...", id);
02646
02647 float sim = signature->compareTo(*sB);
02648
02649 int merged = 0;
02650 if(sim >= _similarityThreshold)
02651 {
02652 if(_incrementalMemory)
02653 {
02654 if(this->rehearsalMerge(id, signature->id()))
02655 {
02656 merged = id;
02657 }
02658 }
02659 else
02660 {
02661 signature->setWeight(signature->getWeight() + 1 + sB->getWeight());
02662 }
02663 }
02664
02665 if(stats) stats->addStatistic(Statistics::kMemoryRehearsal_merged(), merged);
02666 if(stats) stats->addStatistic(Statistics::kMemoryRehearsal_sim(), sim);
02667 if(stats) stats->addStatistic(Statistics::kMemoryRehearsal_id(), sim >= _similarityThreshold?id:0);
02668 UDEBUG("merged=%d, sim=%f t=%fs", merged, sim, timer.ticks());
02669 }
02670 else
02671 {
02672 if(stats) stats->addStatistic(Statistics::kMemoryRehearsal_merged(), 0);
02673 if(stats) stats->addStatistic(Statistics::kMemoryRehearsal_sim(), 0);
02674 }
02675 }
02676
02677 bool Memory::rehearsalMerge(int oldId, int newId)
02678 {
02679 ULOGGER_INFO("old=%d, new=%d", oldId, newId);
02680 Signature * oldS = _getSignature(oldId);
02681 Signature * newS = _getSignature(newId);
02682 if(oldS && newS && _incrementalMemory)
02683 {
02684 UASSERT_MSG(oldS->getWeight() >= 0 && newS->getWeight() >= 0, uFormat("%d %d", oldS->getWeight(), newS->getWeight()).c_str());
02685 std::map<int, Link>::const_iterator iter = oldS->getLinks().find(newS->id());
02686 if(iter != oldS->getLinks().end() &&
02687 iter->second.type() != Link::kNeighbor &&
02688 iter->second.type() != Link::kNeighborMerged)
02689 {
02690
02691 UWARN("already merged, old=%d, new=%d", oldId, newId);
02692 return false;
02693 }
02694 UASSERT(!newS->isSaved());
02695
02696 UINFO("Rehearsal merging %d (w=%d) and %d (w=%d)",
02697 oldS->id(), oldS->getWeight(),
02698 newS->id(), newS->getWeight());
02699
02700 bool fullMerge;
02701 bool intermediateMerge = false;
02702 if(!newS->getLinks().begin()->second.transform().isNull())
02703 {
02704
02705
02706
02707 float x,y,z, roll,pitch,yaw;
02708 newS->getLinks().begin()->second.transform().getTranslationAndEulerAngles(x,y,z, roll,pitch,yaw);
02709 bool isMoving = fabs(x) > _rehearsalMaxDistance ||
02710 fabs(y) > _rehearsalMaxDistance ||
02711 fabs(z) > _rehearsalMaxDistance ||
02712 fabs(roll) > _rehearsalMaxAngle ||
02713 fabs(pitch) > _rehearsalMaxAngle ||
02714 fabs(yaw) > _rehearsalMaxAngle;
02715 if(isMoving && _rehearsalWeightIgnoredWhileMoving)
02716 {
02717 UINFO("Rehearsal ignored because the robot has moved more than %f m or %f rad (\"Mem/RehearsalWeightIgnoredWhileMoving\"=true)",
02718 _rehearsalMaxDistance, _rehearsalMaxAngle);
02719 return false;
02720 }
02721 fullMerge = !isMoving && newS->hasLink(oldS->id());
02722 intermediateMerge = !isMoving && !newS->hasLink(oldS->id());
02723 }
02724 else
02725 {
02726 fullMerge = newS->hasLink(oldS->id()) && newS->getLinks().begin()->second.transform().isNull();
02727 }
02728
02729 if(fullMerge)
02730 {
02731
02732 Link newToOldLink = newS->getLinks().at(oldS->id());
02733 oldS->removeLink(newId);
02734 newS->removeLink(oldId);
02735
02736 if(_idUpdatedToNewOneRehearsal)
02737 {
02738
02739 const std::map<int, Link> & links = oldS->getLinks();
02740 for(std::map<int, Link>::const_iterator iter = links.begin(); iter!=links.end(); ++iter)
02741 {
02742 Link link = iter->second;
02743 Link mergedLink = newToOldLink.merge(link, link.type());
02744 UASSERT(mergedLink.from() == newS->id() && mergedLink.to() == link.to());
02745
02746 Signature * s = this->_getSignature(link.to());
02747 if(s)
02748 {
02749
02750 s->removeLink(oldS->id());
02751 s->addLink(mergedLink.inverse());
02752
02753 newS->addLink(mergedLink);
02754 }
02755 else
02756 {
02757 UERROR("Didn't find neighbor %d of %d in RAM...", link.to(), oldS->id());
02758 }
02759 }
02760 newS->setLabel(oldS->getLabel());
02761 oldS->setLabel("");
02762 oldS->removeLinks();
02763 oldS->addLink(Link(oldS->id(), newS->id(), Link::kGlobalClosure, Transform(), 1, 1));
02764
02765
02766 this->copyData(oldS, newS);
02767
02768
02769 newS->setWeight(newS->getWeight() + 1 + oldS->getWeight());
02770
02771 if(_lastGlobalLoopClosureId == oldS->id())
02772 {
02773 _lastGlobalLoopClosureId = newS->id();
02774 }
02775 }
02776 else
02777 {
02778 newS->addLink(Link(newS->id(), oldS->id(), Link::kGlobalClosure, Transform() , 1, 1));
02779
02780
02781 oldS->setWeight(newS->getWeight() + 1 + oldS->getWeight());
02782
02783 if(_lastSignature == newS)
02784 {
02785 _lastSignature = oldS;
02786 }
02787 }
02788
02789
02790 moveToTrash(_idUpdatedToNewOneRehearsal?oldS:newS, _notLinkedNodesKeptInDb);
02791
02792 return true;
02793 }
02794 else
02795 {
02796
02797 if(_idUpdatedToNewOneRehearsal)
02798 {
02799
02800 int w = oldS->getWeight()>=0?oldS->getWeight():0;
02801 newS->setWeight(w + newS->getWeight() + 1);
02802 oldS->setWeight(intermediateMerge?-1:0);
02803
02804 if(_lastGlobalLoopClosureId == oldS->id())
02805 {
02806 _lastGlobalLoopClosureId = newS->id();
02807 }
02808 }
02809 else
02810 {
02811 int w = newS->getWeight()>=0?newS->getWeight():0;
02812 oldS->setWeight(w + oldS->getWeight() + 1);
02813 newS->setWeight(intermediateMerge?-1:0);
02814 }
02815 }
02816 }
02817 else
02818 {
02819 if(!newS)
02820 {
02821 UERROR("newId=%d, oldId=%d, Signature %d not found in working/st memories", newId, oldId, newId);
02822 }
02823 if(!oldS)
02824 {
02825 UERROR("newId=%d, oldId=%d, Signature %d not found in working/st memories", newId, oldId, oldId);
02826 }
02827 }
02828 return false;
02829 }
02830
02831 Transform Memory::getOdomPose(int signatureId, bool lookInDatabase) const
02832 {
02833 Transform pose, groundTruth;
02834 int mapId, weight;
02835 std::string label;
02836 double stamp;
02837 getNodeInfo(signatureId, pose, mapId, weight, label, stamp, groundTruth, lookInDatabase);
02838 return pose;
02839 }
02840
02841 Transform Memory::getGroundTruthPose(int signatureId, bool lookInDatabase) const
02842 {
02843 Transform pose, groundTruth;
02844 int mapId, weight;
02845 std::string label;
02846 double stamp;
02847 getNodeInfo(signatureId, pose, mapId, weight, label, stamp, groundTruth, lookInDatabase);
02848 return groundTruth;
02849 }
02850
02851 bool Memory::getNodeInfo(int signatureId,
02852 Transform & odomPose,
02853 int & mapId,
02854 int & weight,
02855 std::string & label,
02856 double & stamp,
02857 Transform & groundTruth,
02858 bool lookInDatabase) const
02859 {
02860 const Signature * s = this->getSignature(signatureId);
02861 if(s)
02862 {
02863 odomPose = s->getPose();
02864 mapId = s->mapId();
02865 weight = s->getWeight();
02866 label = s->getLabel();
02867 stamp = s->getStamp();
02868 groundTruth = s->getGroundTruthPose();
02869 return true;
02870 }
02871 else if(lookInDatabase && _dbDriver)
02872 {
02873 return _dbDriver->getNodeInfo(signatureId, odomPose, mapId, weight, label, stamp, groundTruth);
02874 }
02875 return false;
02876 }
02877
02878 cv::Mat Memory::getImageCompressed(int signatureId) const
02879 {
02880 cv::Mat image;
02881 const Signature * s = this->getSignature(signatureId);
02882 if(s)
02883 {
02884 image = s->sensorData().imageCompressed();
02885 }
02886 if(image.empty() && this->isBinDataKept() && _dbDriver)
02887 {
02888 SensorData data;
02889 _dbDriver->getNodeData(signatureId, data);
02890 image = data.imageCompressed();
02891 }
02892 return image;
02893 }
02894
02895 SensorData Memory::getNodeData(int nodeId, bool uncompressedData) const
02896 {
02897 UDEBUG("nodeId=%d", nodeId);
02898 SensorData r;
02899 Signature * s = this->_getSignature(nodeId);
02900 if(s && !s->sensorData().imageCompressed().empty())
02901 {
02902 r = s->sensorData();
02903 }
02904 else if(_dbDriver)
02905 {
02906
02907 _dbDriver->getNodeData(nodeId, r);
02908 }
02909
02910 if(uncompressedData)
02911 {
02912 r.uncompressData();
02913 }
02914
02915 return r;
02916 }
02917
02918 void Memory::getNodeWords(int nodeId,
02919 std::multimap<int, cv::KeyPoint> & words,
02920 std::multimap<int, cv::Point3f> & words3,
02921 std::multimap<int, cv::Mat> & wordsDescriptors)
02922 {
02923 UDEBUG("nodeId=%d", nodeId);
02924 Signature * s = this->_getSignature(nodeId);
02925 if(s)
02926 {
02927 words = s->getWords();
02928 words3 = s->getWords3();
02929 wordsDescriptors = s->getWordsDescriptors();
02930 }
02931 else if(_dbDriver)
02932 {
02933
02934 std::list<Signature*> signatures;
02935 std::list<int> ids;
02936 ids.push_back(nodeId);
02937 std::set<int> loadedFromTrash;
02938 _dbDriver->loadSignatures(ids, signatures, &loadedFromTrash);
02939 if(signatures.size())
02940 {
02941 words = signatures.front()->getWords();
02942 words3 = signatures.front()->getWords3();
02943 wordsDescriptors = signatures.front()->getWordsDescriptors();
02944 if(loadedFromTrash.size())
02945 {
02946
02947 _dbDriver->asyncSave(signatures.front());
02948 }
02949 else
02950 {
02951 delete signatures.front();
02952 }
02953 }
02954 }
02955 }
02956
02957 void Memory::getNodeCalibration(int nodeId,
02958 std::vector<CameraModel> & models,
02959 StereoCameraModel & stereoModel)
02960 {
02961 UDEBUG("nodeId=%d", nodeId);
02962 Signature * s = this->_getSignature(nodeId);
02963 if(s)
02964 {
02965 models = s->sensorData().cameraModels();
02966 stereoModel = s->sensorData().stereoCameraModel();
02967 }
02968 else if(_dbDriver)
02969 {
02970
02971 _dbDriver->getCalibration(nodeId, models, stereoModel);
02972 }
02973 }
02974
02975 SensorData Memory::getSignatureDataConst(int locationId) const
02976 {
02977 UDEBUG("");
02978 SensorData r;
02979 const Signature * s = this->getSignature(locationId);
02980 if(s && !s->sensorData().imageCompressed().empty())
02981 {
02982 r = s->sensorData();
02983 }
02984 else if(_dbDriver)
02985 {
02986
02987 if(s)
02988 {
02989 std::list<Signature*> signatures;
02990 Signature tmp = *s;
02991 signatures.push_back(&tmp);
02992 _dbDriver->loadNodeData(signatures);
02993 r = tmp.sensorData();
02994 }
02995 else
02996 {
02997 std::list<int> ids;
02998 ids.push_back(locationId);
02999 std::list<Signature*> signatures;
03000 std::set<int> loadedFromTrash;
03001 _dbDriver->loadSignatures(ids, signatures, &loadedFromTrash);
03002 if(signatures.size())
03003 {
03004 Signature * sTmp = signatures.front();
03005 if(sTmp->sensorData().imageCompressed().empty())
03006 {
03007 _dbDriver->loadNodeData(signatures);
03008 }
03009 r = sTmp->sensorData();
03010 if(loadedFromTrash.size())
03011 {
03012
03013 _dbDriver->asyncSave(sTmp);
03014 }
03015 else
03016 {
03017 delete sTmp;
03018 }
03019 }
03020 }
03021 }
03022
03023 return r;
03024 }
03025
03026 void Memory::generateGraph(const std::string & fileName, const std::set<int> & ids)
03027 {
03028 if(!_dbDriver)
03029 {
03030 UERROR("A database must must loaded first...");
03031 return;
03032 }
03033
03034 _dbDriver->generateGraph(fileName, ids, _signatures);
03035 }
03036
03037 int Memory::getNi(int signatureId) const
03038 {
03039 int ni = 0;
03040 const Signature * s = this->getSignature(signatureId);
03041 if(s)
03042 {
03043 ni = (int)((Signature *)s)->getWords().size();
03044 }
03045 else
03046 {
03047 _dbDriver->getInvertedIndexNi(signatureId, ni);
03048 }
03049 return ni;
03050 }
03051
03052
03053 void Memory::copyData(const Signature * from, Signature * to)
03054 {
03055 UTimer timer;
03056 timer.start();
03057 if(from && to)
03058 {
03059
03060 this->disableWordsRef(to->id());
03061 to->setWords(from->getWords());
03062 std::list<int> id;
03063 id.push_back(to->id());
03064 this->enableWordsRef(id);
03065
03066 if(from->isSaved() && _dbDriver)
03067 {
03068 _dbDriver->getNodeData(from->id(), to->sensorData());
03069 UDEBUG("Loaded image data from database");
03070 }
03071 else
03072 {
03073 to->sensorData() = (SensorData)from->sensorData();
03074 }
03075 to->sensorData().setId(to->id());
03076
03077 to->setPose(from->getPose());
03078 to->setWords3(from->getWords3());
03079 to->setWordsDescriptors(from->getWordsDescriptors());
03080 }
03081 else
03082 {
03083 ULOGGER_ERROR("Can't merge the signatures because there are not same type.");
03084 }
03085 UDEBUG("Merging time = %fs", timer.ticks());
03086 }
03087
03088 class PreUpdateThread : public UThreadNode
03089 {
03090 public:
03091 PreUpdateThread(VWDictionary * vwp) : _vwp(vwp) {}
03092 virtual ~PreUpdateThread() {}
03093 private:
03094 void mainLoop() {
03095 if(_vwp)
03096 {
03097 _vwp->update();
03098 }
03099 this->kill();
03100 }
03101 VWDictionary * _vwp;
03102 };
03103
03104 Signature * Memory::createSignature(const SensorData & data, const Transform & pose, Statistics * stats)
03105 {
03106 UDEBUG("");
03107 UASSERT(data.imageRaw().empty() ||
03108 data.imageRaw().type() == CV_8UC1 ||
03109 data.imageRaw().type() == CV_8UC3);
03110 UASSERT_MSG(data.depthOrRightRaw().empty() ||
03111 ( ( data.depthOrRightRaw().type() == CV_16UC1 ||
03112 data.depthOrRightRaw().type() == CV_32FC1 ||
03113 data.depthOrRightRaw().type() == CV_8UC1)
03114 &&
03115 ( (data.imageRaw().empty() && data.depthOrRightRaw().type() != CV_8UC1) ||
03116 (data.imageRaw().rows % data.depthOrRightRaw().rows == 0 && data.imageRaw().cols % data.depthOrRightRaw().cols == 0))),
03117 uFormat("image=(%d/%d) depth=(%d/%d, type=%d [accepted=%d,%d,%d])",
03118 data.imageRaw().cols,
03119 data.imageRaw().rows,
03120 data.depthOrRightRaw().cols,
03121 data.depthOrRightRaw().rows,
03122 data.depthOrRightRaw().type(),
03123 CV_16UC1, CV_32FC1, CV_8UC1).c_str());
03124 UASSERT(data.laserScanRaw().empty() || data.laserScanRaw().type() == CV_32FC2 || data.laserScanRaw().type() == CV_32FC3 || data.laserScanRaw().type() == CV_32FC(6));
03125
03126 if(!data.depthOrRightRaw().empty() &&
03127 data.cameraModels().size() == 0 &&
03128 !data.stereoCameraModel().isValidForProjection())
03129 {
03130 UERROR("Rectified images required! Calibrate your camera.");
03131 return 0;
03132 }
03133 UASSERT(_feature2D != 0);
03134
03135 PreUpdateThread preUpdateThread(_vwd);
03136
03137 UTimer timer;
03138 timer.start();
03139 float t;
03140 std::vector<cv::KeyPoint> keypoints;
03141 cv::Mat descriptors;
03142 bool isIntermediateNode = data.id() < 0 || data.imageRaw().empty();
03143 int id = data.id();
03144 if(_generateIds)
03145 {
03146 id = this->getNextId();
03147 }
03148 else
03149 {
03150 if(id <= 0)
03151 {
03152 UERROR("Received image ID is null. "
03153 "Please set parameter Mem/GenerateIds to \"true\" or "
03154 "make sure the input source provides image ids (seq).");
03155 return 0;
03156 }
03157 else if(id > _idCount)
03158 {
03159 _idCount = id;
03160 }
03161 else
03162 {
03163 UERROR("Id of acquired image (%d) is smaller than the last in memory (%d). "
03164 "Please set parameter Mem/GenerateIds to \"true\" or "
03165 "make sure the input source provides image ids (seq) over the last in "
03166 "memory, which is %d.",
03167 id,
03168 _idCount,
03169 _idCount);
03170 return 0;
03171 }
03172 }
03173
03174 int treeSize= int(_workingMem.size() + _stMem.size());
03175 int meanWordsPerLocation = 0;
03176 if(treeSize > 0)
03177 {
03178 meanWordsPerLocation = _vwd->getTotalActiveReferences() / treeSize;
03179 }
03180
03181 if(_parallelized)
03182 {
03183 UDEBUG("Start dictionary update thread");
03184 preUpdateThread.start();
03185 }
03186
03187 int preDecimation = 1;
03188 std::vector<cv::Point3f> keypoints3D;
03189 if(!_useOdometryFeatures || data.keypoints().empty() || (int)data.keypoints().size() != data.descriptors().rows)
03190 {
03191 if(_feature2D->getMaxFeatures() >= 0 && !data.imageRaw().empty() && !isIntermediateNode)
03192 {
03193 SensorData decimatedData = data;
03194 if(_imagePreDecimation > 1)
03195 {
03196 preDecimation = _imagePreDecimation;
03197 decimatedData.setImageRaw(util2d::decimate(decimatedData.imageRaw(), _imagePreDecimation));
03198 decimatedData.setDepthOrRightRaw(util2d::decimate(decimatedData.depthOrRightRaw(), _imagePreDecimation));
03199 std::vector<CameraModel> cameraModels = decimatedData.cameraModels();
03200 for(unsigned int i=0; i<cameraModels.size(); ++i)
03201 {
03202 cameraModels[i] = cameraModels[i].scaled(1.0/double(_imagePreDecimation));
03203 }
03204 decimatedData.setCameraModels(cameraModels);
03205 StereoCameraModel stereoModel = decimatedData.stereoCameraModel();
03206 if(stereoModel.isValidForProjection())
03207 {
03208 stereoModel.scale(1.0/double(_imagePreDecimation));
03209 }
03210 decimatedData.setStereoCameraModel(stereoModel);
03211 }
03212
03213 UINFO("Extract features");
03214 cv::Mat imageMono;
03215 if(decimatedData.imageRaw().channels() == 3)
03216 {
03217 cv::cvtColor(decimatedData.imageRaw(), imageMono, CV_BGR2GRAY);
03218 }
03219 else
03220 {
03221 imageMono = decimatedData.imageRaw();
03222 }
03223
03224 cv::Mat depthMask;
03225 if(!decimatedData.depthRaw().empty())
03226 {
03227 if(imageMono.rows % decimatedData.depthRaw().rows == 0 &&
03228 imageMono.cols % decimatedData.depthRaw().cols == 0 &&
03229 imageMono.rows/decimatedData.depthRaw().rows == imageMono.cols/decimatedData.depthRaw().cols)
03230 {
03231 depthMask = util2d::interpolate(decimatedData.depthRaw(), imageMono.rows/decimatedData.depthRaw().rows, 0.1f);
03232 }
03233 }
03234
03235 keypoints = _feature2D->generateKeypoints(
03236 imageMono,
03237 depthMask);
03238 t = timer.ticks();
03239 if(stats) stats->addStatistic(Statistics::kTimingMemKeypoints_detection(), t*1000.0f);
03240 UDEBUG("time keypoints (%d) = %fs", (int)keypoints.size(), t);
03241
03242 descriptors = _feature2D->generateDescriptors(imageMono, keypoints);
03243 t = timer.ticks();
03244 if(stats) stats->addStatistic(Statistics::kTimingMemDescriptors_extraction(), t*1000.0f);
03245 UDEBUG("time descriptors (%d) = %fs", descriptors.rows, t);
03246
03247 UDEBUG("ratio=%f, meanWordsPerLocation=%d", _badSignRatio, meanWordsPerLocation);
03248 if(descriptors.rows && descriptors.rows < _badSignRatio * float(meanWordsPerLocation))
03249 {
03250 descriptors = cv::Mat();
03251 }
03252 else if((!decimatedData.depthRaw().empty() && decimatedData.cameraModels().size() && decimatedData.cameraModels()[0].isValidForProjection()) ||
03253 (!decimatedData.rightRaw().empty() && decimatedData.stereoCameraModel().isValidForProjection()))
03254 {
03255 keypoints3D = _feature2D->generateKeypoints3D(decimatedData, keypoints);
03256 t = timer.ticks();
03257 if(stats) stats->addStatistic(Statistics::kTimingMemKeypoints_3D(), t*1000.0f);
03258 UDEBUG("time keypoints 3D (%d) = %fs", (int)keypoints3D.size(), t);
03259 }
03260 }
03261 else if(data.imageRaw().empty())
03262 {
03263 UDEBUG("Empty image, cannot extract features...");
03264 }
03265 else if(_feature2D->getMaxFeatures() < 0)
03266 {
03267 UDEBUG("_feature2D->getMaxFeatures()(%d<0) so don't extract any features...", _feature2D->getMaxFeatures());
03268 }
03269 else
03270 {
03271 UDEBUG("Intermediate node detected, don't extract features!");
03272 }
03273 }
03274 else if(_feature2D->getMaxFeatures() >= 0 && !isIntermediateNode)
03275 {
03276 UINFO("Use odometry features");
03277 keypoints = data.keypoints();
03278 descriptors = data.descriptors().clone();
03279
03280 UASSERT(descriptors.empty() || descriptors.rows == (int)keypoints.size());
03281
03282 if((int)keypoints.size() > _feature2D->getMaxFeatures())
03283 {
03284 _feature2D->limitKeypoints(keypoints, descriptors, _feature2D->getMaxFeatures());
03285 }
03286
03287 if(descriptors.empty())
03288 {
03289 cv::Mat imageMono;
03290 if(data.imageRaw().channels() == 3)
03291 {
03292 cv::cvtColor(data.imageRaw(), imageMono, CV_BGR2GRAY);
03293 }
03294 else
03295 {
03296 imageMono = data.imageRaw();
03297 }
03298
03299 descriptors = _feature2D->generateDescriptors(imageMono, keypoints);
03300 t = timer.ticks();
03301 if(stats) stats->addStatistic(Statistics::kTimingMemDescriptors_extraction(), t*1000.0f);
03302 UDEBUG("time descriptors (%d) = %fs", descriptors.rows, t);
03303 }
03304
03305 if((!data.depthRaw().empty() && data.cameraModels().size() && data.cameraModels()[0].isValidForProjection()) ||
03306 (!data.rightRaw().empty() && data.stereoCameraModel().isValidForProjection()))
03307 {
03308 keypoints3D = _feature2D->generateKeypoints3D(data, keypoints);
03309 if(_feature2D->getMinDepth() > 0.0f || _feature2D->getMaxDepth() > 0.0f)
03310 {
03311 UDEBUG("");
03312
03313 UASSERT((int)keypoints.size() == descriptors.rows &&
03314 keypoints3D.size() == keypoints.size());
03315 std::vector<cv::KeyPoint> validKeypoints(keypoints.size());
03316 std::vector<cv::Point3f> validKeypoints3D(keypoints.size());
03317 cv::Mat validDescriptors(descriptors.size(), descriptors.type());
03318
03319 int oi=0;
03320 for(unsigned int i=0; i<keypoints3D.size(); ++i)
03321 {
03322 if(util3d::isFinite(keypoints3D[i]))
03323 {
03324 validKeypoints[oi] = keypoints[i];
03325 validKeypoints3D[oi] = keypoints3D[i];
03326 descriptors.row(i).copyTo(validDescriptors.row(oi));
03327 ++oi;
03328 }
03329 }
03330 UDEBUG("Removed %d invalid 3D points", (int)keypoints3D.size()-oi);
03331 validKeypoints.resize(oi);
03332 validKeypoints3D.resize(oi);
03333 keypoints = validKeypoints;
03334 keypoints3D = validKeypoints3D;
03335 descriptors = validDescriptors.rowRange(0, oi).clone();
03336 }
03337 t = timer.ticks();
03338 if(stats) stats->addStatistic(Statistics::kTimingMemKeypoints_3D(), t*1000.0f);
03339 UDEBUG("time keypoints 3D (%d) = %fs", (int)keypoints3D.size(), t);
03340 }
03341
03342 UDEBUG("ratio=%f, meanWordsPerLocation=%d", _badSignRatio, meanWordsPerLocation);
03343 if(descriptors.rows && descriptors.rows < _badSignRatio * float(meanWordsPerLocation))
03344 {
03345 descriptors = cv::Mat();
03346 }
03347 }
03348
03349 if(_parallelized)
03350 {
03351 UDEBUG("Joining dictionary update thread...");
03352 preUpdateThread.join();
03353 UDEBUG("Joining dictionary update thread... thread finished!");
03354 }
03355
03356 std::list<int> wordIds;
03357 if(descriptors.rows)
03358 {
03359 t = timer.ticks();
03360 if(stats) stats->addStatistic(Statistics::kTimingMemJoining_dictionary_update(), t*1000.0f);
03361 if(_parallelized)
03362 {
03363 UDEBUG("time descriptor and memory update (%d of size=%d) = %fs", descriptors.rows, descriptors.cols, t);
03364 }
03365 else
03366 {
03367 UDEBUG("time descriptor (%d of size=%d) = %fs", descriptors.rows, descriptors.cols, t);
03368 }
03369
03370 wordIds = _vwd->addNewWords(descriptors, id);
03371 t = timer.ticks();
03372 if(stats) stats->addStatistic(Statistics::kTimingMemAdd_new_words(), t*1000.0f);
03373 UDEBUG("time addNewWords %fs", t);
03374 }
03375 else if(id>0)
03376 {
03377 UDEBUG("id %d is a bad signature", id);
03378 }
03379
03380 std::multimap<int, cv::KeyPoint> words;
03381 std::multimap<int, cv::Point3f> words3D;
03382 std::multimap<int, cv::Mat> wordsDescriptors;
03383 if(wordIds.size() > 0)
03384 {
03385 UASSERT(wordIds.size() == keypoints.size());
03386 UASSERT(keypoints3D.size() == 0 || keypoints3D.size() == wordIds.size());
03387 unsigned int i=0;
03388 float decimationRatio = preDecimation / _imagePostDecimation;
03389 double log2value = log(double(preDecimation))/log(2.0);
03390 for(std::list<int>::iterator iter=wordIds.begin(); iter!=wordIds.end() && i < keypoints.size(); ++iter, ++i)
03391 {
03392 cv::KeyPoint kpt = keypoints[i];
03393 if(preDecimation != _imagePostDecimation)
03394 {
03395
03396 kpt.pt.x *= decimationRatio;
03397 kpt.pt.y *= decimationRatio;
03398 kpt.size *= decimationRatio;
03399 kpt.octave += log2value;
03400 }
03401 words.insert(std::pair<int, cv::KeyPoint>(*iter, kpt));
03402
03403 if(keypoints3D.size())
03404 {
03405 words3D.insert(std::pair<int, cv::Point3f>(*iter, keypoints3D.at(i)));
03406 }
03407 if(_rawDescriptorsKept)
03408 {
03409 wordsDescriptors.insert(std::pair<int, cv::Mat>(*iter, descriptors.row(i).clone()));
03410 }
03411 }
03412 }
03413
03414 if(!pose.isNull() &&
03415 data.cameraModels().size() == 1 &&
03416 words.size() &&
03417 words3D.size() == 0)
03418 {
03419 bool fillWithNaN = true;
03420 if(_signatures.size())
03421 {
03422 UDEBUG("Generate 3D words using odometry");
03423 Signature * previousS = _signatures.rbegin()->second;
03424 if(previousS->getWords().size() > 8 && words.size() > 8 && !previousS->getPose().isNull())
03425 {
03426 Transform cameraTransform = pose.inverse() * previousS->getPose();
03427
03428 std::map<int, cv::Point3f> inliers = util3d::generateWords3DMono(
03429 uMultimapToMapUnique(words),
03430 uMultimapToMapUnique(previousS->getWords()),
03431 data.cameraModels()[0],
03432 cameraTransform);
03433
03434
03435 float bad_point = std::numeric_limits<float>::quiet_NaN ();
03436 for(std::multimap<int, cv::KeyPoint>::const_iterator iter=words.begin(); iter!=words.end(); ++iter)
03437 {
03438 std::map<int, cv::Point3f>::iterator jter=inliers.find(iter->first);
03439 if(jter != inliers.end())
03440 {
03441 words3D.insert(std::make_pair(iter->first, jter->second));
03442 }
03443 else
03444 {
03445 words3D.insert(std::make_pair(iter->first, cv::Point3f(bad_point,bad_point,bad_point)));
03446 }
03447 }
03448
03449 t = timer.ticks();
03450 UASSERT(words3D.size() == words.size());
03451 if(stats) stats->addStatistic(Statistics::kTimingMemKeypoints_3D(), t*1000.0f);
03452 UDEBUG("time keypoints 3D (%d) = %fs", (int)words3D.size(), t);
03453 fillWithNaN = false;
03454 }
03455 }
03456 if(fillWithNaN)
03457 {
03458 float bad_point = std::numeric_limits<float>::quiet_NaN ();
03459 for(std::multimap<int, cv::KeyPoint>::const_iterator iter=words.begin(); iter!=words.end(); ++iter)
03460 {
03461 words3D.insert(std::make_pair(iter->first, cv::Point3f(bad_point,bad_point,bad_point)));
03462 }
03463 }
03464 }
03465
03466 cv::Mat image = data.imageRaw();
03467 cv::Mat depthOrRightImage = data.depthOrRightRaw();
03468 std::vector<CameraModel> cameraModels = data.cameraModels();
03469 StereoCameraModel stereoCameraModel = data.stereoCameraModel();
03470
03471
03472 if(_imagePostDecimation > 1)
03473 {
03474 image = util2d::decimate(image, _imagePostDecimation);
03475 depthOrRightImage = util2d::decimate(depthOrRightImage, _imagePostDecimation);
03476 for(unsigned int i=0; i<cameraModels.size(); ++i)
03477 {
03478 cameraModels[i] = cameraModels[i].scaled(1.0/double(_imagePostDecimation));
03479 }
03480 if(stereoCameraModel.isValidForProjection())
03481 {
03482 stereoCameraModel.scale(1.0/double(_imagePostDecimation));
03483 }
03484 }
03485
03486
03487 cv::Mat laserScan = data.laserScanRaw();
03488 int maxLaserScanMaxPts = data.laserScanMaxPts();
03489 if(!laserScan.empty() && _laserScanDownsampleStepSize > 1)
03490 {
03491 laserScan = util3d::downsample(laserScan, _laserScanDownsampleStepSize);
03492 maxLaserScanMaxPts /= _laserScanDownsampleStepSize;
03493 }
03494
03495 Signature * s;
03496 if(this->isBinDataKept())
03497 {
03498 UDEBUG("Bin data kept: rgb=%d, depth=%d, scan=%d, userData=%d",
03499 image.empty()?0:1,
03500 depthOrRightImage.empty()?0:1,
03501 laserScan.empty()?0:1,
03502 data.userDataRaw().empty()?0:1);
03503
03504 std::vector<unsigned char> imageBytes;
03505 std::vector<unsigned char> depthBytes;
03506
03507 if(_saveDepth16Format && !depthOrRightImage.empty() && depthOrRightImage.type() == CV_32FC1)
03508 {
03509 UWARN("Save depth data to 16 bits format: depth type detected is 32FC1, use 16UC1 depth format to avoid this conversion (or set parameter \"Mem/SaveDepth16Format\"=false to use 32bits format).");
03510 depthOrRightImage = util2d::cvtDepthFromFloat(depthOrRightImage);
03511 }
03512
03513 rtabmap::CompressionThread ctImage(image, std::string(".jpg"));
03514 rtabmap::CompressionThread ctDepth(depthOrRightImage, std::string(".png"));
03515 rtabmap::CompressionThread ctLaserScan(laserScan);
03516 rtabmap::CompressionThread ctUserData(data.userDataRaw());
03517 ctImage.start();
03518 ctDepth.start();
03519 ctLaserScan.start();
03520 ctUserData.start();
03521 ctImage.join();
03522 ctDepth.join();
03523 ctLaserScan.join();
03524 ctUserData.join();
03525
03526 s = new Signature(id,
03527 _idMapCount,
03528 isIntermediateNode?-1:0,
03529 data.stamp(),
03530 "",
03531 pose,
03532 data.groundTruth(),
03533 stereoCameraModel.isValidForProjection()?
03534 SensorData(
03535 ctLaserScan.getCompressedData(),
03536 maxLaserScanMaxPts,
03537 data.laserScanMaxRange(),
03538 ctImage.getCompressedData(),
03539 ctDepth.getCompressedData(),
03540 stereoCameraModel,
03541 id,
03542 0,
03543 ctUserData.getCompressedData()):
03544 SensorData(
03545 ctLaserScan.getCompressedData(),
03546 maxLaserScanMaxPts,
03547 data.laserScanMaxRange(),
03548 ctImage.getCompressedData(),
03549 ctDepth.getCompressedData(),
03550 cameraModels,
03551 id,
03552 0,
03553 ctUserData.getCompressedData()));
03554 }
03555 else
03556 {
03557
03558 rtabmap::CompressionThread ctLaserScan(laserScan);
03559 rtabmap::CompressionThread ctUserData(data.userDataRaw());
03560 ctLaserScan.start();
03561 ctUserData.start();
03562 ctLaserScan.join();
03563 ctUserData.join();
03564
03565 s = new Signature(id,
03566 _idMapCount,
03567 isIntermediateNode?-1:0,
03568 data.stamp(),
03569 "",
03570 pose,
03571 data.groundTruth(),
03572 stereoCameraModel.isValidForProjection()?
03573 SensorData(
03574 ctLaserScan.getCompressedData(),
03575 maxLaserScanMaxPts,
03576 data.laserScanMaxRange(),
03577 cv::Mat(),
03578 cv::Mat(),
03579 stereoCameraModel,
03580 id,
03581 0,
03582 ctUserData.getCompressedData()):
03583 SensorData(
03584 ctLaserScan.getCompressedData(),
03585 maxLaserScanMaxPts,
03586 data.laserScanMaxRange(),
03587 cv::Mat(),
03588 cv::Mat(),
03589 cameraModels,
03590 id,
03591 0,
03592 ctUserData.getCompressedData()));
03593 }
03594
03595 s->setWords(words);
03596 s->setWords3(words3D);
03597 s->setWordsDescriptors(wordsDescriptors);
03598
03599
03600 s->sensorData().setImageRaw(image);
03601 s->sensorData().setDepthOrRightRaw(depthOrRightImage);
03602 s->sensorData().setLaserScanRaw(laserScan, maxLaserScanMaxPts, data.laserScanMaxRange());
03603 s->sensorData().setUserDataRaw(data.userDataRaw());
03604
03605 s->sensorData().setGroundTruth(data.groundTruth());
03606
03607 t = timer.ticks();
03608 if(stats) stats->addStatistic(Statistics::kTimingMemCompressing_data(), t*1000.0f);
03609 UDEBUG("time compressing data (id=%d) %fs", id, t);
03610 if(words.size())
03611 {
03612 s->setEnabled(true);
03613 }
03614 return s;
03615 }
03616
03617 void Memory::disableWordsRef(int signatureId)
03618 {
03619 UDEBUG("id=%d", signatureId);
03620
03621 Signature * ss = this->_getSignature(signatureId);
03622 if(ss && ss->isEnabled())
03623 {
03624 const std::multimap<int, cv::KeyPoint> & words = ss->getWords();
03625 const std::list<int> & keys = uUniqueKeys(words);
03626 int count = _vwd->getTotalActiveReferences();
03627
03628 for(std::list<int>::const_iterator i=keys.begin(); i!=keys.end(); ++i)
03629 {
03630 _vwd->removeAllWordRef(*i, signatureId);
03631 }
03632
03633 count -= _vwd->getTotalActiveReferences();
03634 ss->setEnabled(false);
03635 UDEBUG("%d words total ref removed from signature %d... (total active ref = %d)", count, ss->id(), _vwd->getTotalActiveReferences());
03636 }
03637 }
03638
03639 void Memory::cleanUnusedWords()
03640 {
03641 if(_vwd->isIncremental())
03642 {
03643 std::vector<VisualWord*> removedWords = _vwd->getUnusedWords();
03644 UDEBUG("Removing %d words (dictionary size=%d)...", removedWords.size(), _vwd->getVisualWords().size());
03645 if(removedWords.size())
03646 {
03647
03648 _vwd->removeWords(removedWords);
03649
03650 for(unsigned int i=0; i<removedWords.size(); ++i)
03651 {
03652 if(_dbDriver)
03653 {
03654 _dbDriver->asyncSave(removedWords[i]);
03655 }
03656 else
03657 {
03658 delete removedWords[i];
03659 }
03660 }
03661 }
03662 }
03663 }
03664
03665 void Memory::enableWordsRef(const std::list<int> & signatureIds)
03666 {
03667 UDEBUG("size=%d", signatureIds.size());
03668 UTimer timer;
03669 timer.start();
03670
03671 std::map<int, int> refsToChange;
03672
03673 std::set<int> oldWordIds;
03674 std::list<Signature *> surfSigns;
03675 for(std::list<int>::const_iterator i=signatureIds.begin(); i!=signatureIds.end(); ++i)
03676 {
03677 Signature * ss = dynamic_cast<Signature *>(this->_getSignature(*i));
03678 if(ss && !ss->isEnabled())
03679 {
03680 surfSigns.push_back(ss);
03681 std::list<int> uniqueKeys = uUniqueKeys(ss->getWords());
03682
03683
03684 for(std::list<int>::const_iterator k=uniqueKeys.begin(); k!=uniqueKeys.end(); ++k)
03685 {
03686 if(_vwd->getWord(*k) == 0 && _vwd->getUnusedWord(*k) == 0)
03687 {
03688 oldWordIds.insert(oldWordIds.end(), *k);
03689 }
03690 }
03691 }
03692 }
03693
03694 UDEBUG("oldWordIds.size()=%d, getOldIds time=%fs", oldWordIds.size(), timer.ticks());
03695
03696
03697 std::list<VisualWord *> vws;
03698 if(oldWordIds.size() && _dbDriver)
03699 {
03700
03701 _dbDriver->loadWords(oldWordIds, vws);
03702 }
03703 UDEBUG("loading words(%d) time=%fs", oldWordIds.size(), timer.ticks());
03704
03705
03706 if(vws.size())
03707 {
03708
03709 std::vector<int> vwActiveIds = _vwd->findNN(vws);
03710 UDEBUG("find active ids (number=%d) time=%fs", vws.size(), timer.ticks());
03711 int i=0;
03712 for(std::list<VisualWord *>::iterator iterVws=vws.begin(); iterVws!=vws.end(); ++iterVws)
03713 {
03714 if(vwActiveIds[i] > 0)
03715 {
03716
03717 refsToChange.insert(refsToChange.end(), std::pair<int, int>((*iterVws)->id(), vwActiveIds[i]));
03718 if((*iterVws)->isSaved())
03719 {
03720 delete (*iterVws);
03721 }
03722 else if(_dbDriver)
03723 {
03724 _dbDriver->asyncSave(*iterVws);
03725 }
03726 }
03727 else
03728 {
03729
03730 _vwd->addWord(*iterVws);
03731 }
03732 ++i;
03733 }
03734 UDEBUG("Added %d to dictionary, time=%fs", vws.size()-refsToChange.size(), timer.ticks());
03735
03736
03737 for(std::map<int, int>::const_iterator iter=refsToChange.begin(); iter != refsToChange.end(); ++iter)
03738 {
03739
03740 for(std::list<Signature *>::iterator j=surfSigns.begin(); j!=surfSigns.end(); ++j)
03741 {
03742 (*j)->changeWordsRef(iter->first, iter->second);
03743 }
03744 }
03745 UDEBUG("changing ref, total=%d, time=%fs", refsToChange.size(), timer.ticks());
03746 }
03747
03748 int count = _vwd->getTotalActiveReferences();
03749
03750
03751 for(std::list<Signature *>::iterator j=surfSigns.begin(); j!=surfSigns.end(); ++j)
03752 {
03753 const std::vector<int> & keys = uKeys((*j)->getWords());
03754 if(keys.size())
03755 {
03756 const VisualWord * wordFirst = _vwd->getWord(keys.front());
03757 UASSERT(wordFirst!=0);
03758
03759 cv::Mat descriptors(keys.size(), wordFirst->getDescriptor().cols, wordFirst->getDescriptor().type());
03760
03761 for(unsigned int i=0; i<keys.size(); ++i)
03762 {
03763 _vwd->addWordRef(keys.at(i), (*j)->id());
03764 const VisualWord * word = _vwd->getWord(keys.at(i));
03765 UASSERT(word != 0);
03766
03767 word->getDescriptor().copyTo(descriptors.row(i));
03768
03769 }
03770 (*j)->sensorData().setFeatures(std::vector<cv::KeyPoint>(), descriptors);
03771 (*j)->setEnabled(true);
03772 }
03773 }
03774
03775 count = _vwd->getTotalActiveReferences() - count;
03776 UDEBUG("%d words total ref added from %d signatures, time=%fs...", count, surfSigns.size(), timer.ticks());
03777 }
03778
03779 std::set<int> Memory::reactivateSignatures(const std::list<int> & ids, unsigned int maxLoaded, double & timeDbAccess)
03780 {
03781
03782
03783
03784
03785 UDEBUG("");
03786 UTimer timer;
03787 std::list<int> idsToLoad;
03788 std::map<int, int>::iterator wmIter;
03789 for(std::list<int>::const_iterator i=ids.begin(); i!=ids.end(); ++i)
03790 {
03791 if(!this->getSignature(*i) && !uContains(idsToLoad, *i))
03792 {
03793 if(!maxLoaded || idsToLoad.size() < maxLoaded)
03794 {
03795 idsToLoad.push_back(*i);
03796 UINFO("Loading location %d from database...", *i);
03797 }
03798 }
03799 }
03800
03801 UDEBUG("idsToLoad = %d", idsToLoad.size());
03802
03803 std::list<Signature *> reactivatedSigns;
03804 if(_dbDriver)
03805 {
03806 _dbDriver->loadSignatures(idsToLoad, reactivatedSigns);
03807 }
03808 timeDbAccess = timer.getElapsedTime();
03809 std::list<int> idsLoaded;
03810 for(std::list<Signature *>::iterator i=reactivatedSigns.begin(); i!=reactivatedSigns.end(); ++i)
03811 {
03812 idsLoaded.push_back((*i)->id());
03813
03814 this->addSignatureToWmFromLTM(*i);
03815 }
03816 this->enableWordsRef(idsLoaded);
03817 UDEBUG("time = %fs", timer.ticks());
03818 return std::set<int>(idsToLoad.begin(), idsToLoad.end());
03819 }
03820
03821
03822
03823 void Memory::getMetricConstraints(
03824 const std::set<int> & ids,
03825 std::map<int, Transform> & poses,
03826 std::multimap<int, Link> & links,
03827 bool lookInDatabase)
03828 {
03829 UDEBUG("");
03830 for(std::set<int>::const_iterator iter=ids.begin(); iter!=ids.end(); ++iter)
03831 {
03832 Transform pose = getOdomPose(*iter, lookInDatabase);
03833 if(!pose.isNull())
03834 {
03835 poses.insert(std::make_pair(*iter, pose));
03836 }
03837 }
03838
03839 for(std::set<int>::const_iterator iter=ids.begin(); iter!=ids.end(); ++iter)
03840 {
03841 if(uContains(poses, *iter))
03842 {
03843 std::map<int, Link> tmpLinks = getLinks(*iter, lookInDatabase);
03844 for(std::map<int, Link>::iterator jter=tmpLinks.begin(); jter!=tmpLinks.end(); ++jter)
03845 {
03846 if( jter->second.isValid() &&
03847 uContains(poses, jter->first) &&
03848 graph::findLink(links, *iter, jter->first) == links.end())
03849 {
03850 if(!lookInDatabase &&
03851 (jter->second.type() == Link::kNeighbor ||
03852 jter->second.type() == Link::kNeighborMerged))
03853 {
03854 Link link = jter->second;
03855 const Signature * s = this->getSignature(jter->first);
03856 UASSERT(s!=0);
03857 while(s && s->getWeight() == -1)
03858 {
03859
03860
03861 std::map<int, Link> n = this->getNeighborLinks(s->id(), false);
03862 UASSERT(n.size() <= 2);
03863 std::map<int, Link>::iterator uter = n.upper_bound(s->id());
03864 if(uter != n.end())
03865 {
03866 const Signature * s2 = this->getSignature(uter->first);
03867 if(s2)
03868 {
03869 link = link.merge(uter->second, uter->second.type());
03870 poses.erase(s->id());
03871 s = s2;
03872 }
03873
03874 }
03875 else
03876 {
03877 break;
03878 }
03879 }
03880
03881 links.insert(std::make_pair(*iter, link));
03882 }
03883 else
03884 {
03885 links.insert(std::make_pair(*iter, jter->second));
03886 }
03887 }
03888 }
03889 }
03890 }
03891 }
03892
03893 }