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 #include "rtabmap/core/OptimizerG2O.h"
00061 #include <pcl/io/pcd_io.h>
00062 #include <pcl/common/common.h>
00063 #include <rtabmap/core/OccupancyGrid.h>
00064 #include <opencv2/imgproc/types_c.h>
00065
00066 namespace rtabmap {
00067
00068 const int Memory::kIdStart = 0;
00069 const int Memory::kIdVirtual = -1;
00070 const int Memory::kIdInvalid = 0;
00071
00072 Memory::Memory(const ParametersMap & parameters) :
00073 _dbDriver(0),
00074 _similarityThreshold(Parameters::defaultMemRehearsalSimilarity()),
00075 _binDataKept(Parameters::defaultMemBinDataKept()),
00076 _rawDescriptorsKept(Parameters::defaultMemRawDescriptorsKept()),
00077 _saveDepth16Format(Parameters::defaultMemSaveDepth16Format()),
00078 _notLinkedNodesKeptInDb(Parameters::defaultMemNotLinkedNodesKept()),
00079 _saveIntermediateNodeData(Parameters::defaultMemIntermediateNodeDataKept()),
00080 _incrementalMemory(Parameters::defaultMemIncrementalMemory()),
00081 _reduceGraph(Parameters::defaultMemReduceGraph()),
00082 _maxStMemSize(Parameters::defaultMemSTMSize()),
00083 _recentWmRatio(Parameters::defaultMemRecentWmRatio()),
00084 _transferSortingByWeightId(Parameters::defaultMemTransferSortingByWeightId()),
00085 _idUpdatedToNewOneRehearsal(Parameters::defaultMemRehearsalIdUpdatedToNewOne()),
00086 _generateIds(Parameters::defaultMemGenerateIds()),
00087 _badSignaturesIgnored(Parameters::defaultMemBadSignaturesIgnored()),
00088 _mapLabelsAdded(Parameters::defaultMemMapLabelsAdded()),
00089 _depthAsMask(Parameters::defaultMemDepthAsMask()),
00090 _imagePreDecimation(Parameters::defaultMemImagePreDecimation()),
00091 _imagePostDecimation(Parameters::defaultMemImagePostDecimation()),
00092 _compressionParallelized(Parameters::defaultMemCompressionParallelized()),
00093 _laserScanDownsampleStepSize(Parameters::defaultMemLaserScanDownsampleStepSize()),
00094 _laserScanVoxelSize(Parameters::defaultMemLaserScanVoxelSize()),
00095 _laserScanNormalK(Parameters::defaultMemLaserScanNormalK()),
00096 _laserScanNormalRadius(Parameters::defaultMemLaserScanNormalRadius()),
00097 _reextractLoopClosureFeatures(Parameters::defaultRGBDLoopClosureReextractFeatures()),
00098 _localBundleOnLoopClosure(Parameters::defaultRGBDLocalBundleOnLoopClosure()),
00099 _rehearsalMaxDistance(Parameters::defaultRGBDLinearUpdate()),
00100 _rehearsalMaxAngle(Parameters::defaultRGBDAngularUpdate()),
00101 _rehearsalWeightIgnoredWhileMoving(Parameters::defaultMemRehearsalWeightIgnoredWhileMoving()),
00102 _useOdometryFeatures(Parameters::defaultMemUseOdomFeatures()),
00103 _createOccupancyGrid(Parameters::defaultRGBDCreateOccupancyGrid()),
00104 _visMaxFeatures(Parameters::defaultVisMaxFeatures()),
00105 _visCorType(Parameters::defaultVisCorType()),
00106 _imagesAlreadyRectified(Parameters::defaultRtabmapImagesAlreadyRectified()),
00107 _rectifyOnlyFeatures(Parameters::defaultRtabmapRectifyOnlyFeatures()),
00108 _covOffDiagonalIgnored(Parameters::defaultMemCovOffDiagIgnored()),
00109 _idCount(kIdStart),
00110 _idMapCount(kIdStart),
00111 _lastSignature(0),
00112 _lastGlobalLoopClosureId(0),
00113 _memoryChanged(false),
00114 _linksChanged(false),
00115 _signaturesAdded(0),
00116
00117 _badSignRatio(Parameters::defaultKpBadSignRatio()),
00118 _tfIdfLikelihoodUsed(Parameters::defaultKpTfIdfLikelihoodUsed()),
00119 _parallelized(Parameters::defaultKpParallelized())
00120 {
00121 _feature2D = Feature2D::create(parameters);
00122 _vwd = new VWDictionary(parameters);
00123 _registrationPipeline = Registration::create(parameters);
00124
00125
00126 float corRatio = Parameters::defaultIcpCorrespondenceRatio();
00127 Parameters::parse(parameters, Parameters::kIcpCorrespondenceRatio(), corRatio);
00128 ParametersMap paramsMulti = parameters;
00129 paramsMulti.insert(ParametersPair(Parameters::kIcpCorrespondenceRatio(), uNumber2Str(corRatio>=0.5f?1.0f:corRatio*2.0f)));
00130 if(corRatio >= 0.5)
00131 {
00132 UWARN( "%s is >=0.5, which sets correspondence ratio for proximity detection using "
00133 "laser scans to 100% (2 x Ratio). You may lower the ratio to accept proximity "
00134 "detection with not full scans overlapping.", Parameters::kIcpCorrespondenceRatio().c_str());
00135 }
00136 _registrationIcpMulti = new RegistrationIcp(paramsMulti);
00137
00138 _occupancy = new OccupancyGrid(parameters);
00139 this->parseParameters(parameters);
00140 }
00141
00142 bool Memory::init(const std::string & dbUrl, bool dbOverwritten, const ParametersMap & parameters, bool postInitClosingEvents)
00143 {
00144 if(postInitClosingEvents) UEventsManager::post(new RtabmapEventInit(RtabmapEventInit::kInitializing));
00145
00146 UDEBUG("");
00147 this->parseParameters(parameters);
00148
00149 if(postInitClosingEvents) UEventsManager::post(new RtabmapEventInit("Clearing memory..."));
00150 DBDriver * tmpDriver = 0;
00151 if((!_memoryChanged && !_linksChanged) || dbOverwritten)
00152 {
00153 if(_dbDriver)
00154 {
00155 tmpDriver = _dbDriver;
00156 _dbDriver = 0;
00157 }
00158 }
00159 else if(!_memoryChanged && _linksChanged)
00160 {
00161 _dbDriver->setTimestampUpdateEnabled(false);
00162 }
00163 this->clear();
00164 if(postInitClosingEvents) UEventsManager::post(new RtabmapEventInit("Clearing memory, done!"));
00165
00166 if(tmpDriver)
00167 {
00168 _dbDriver = tmpDriver;
00169 }
00170
00171 if(_dbDriver)
00172 {
00173 if(postInitClosingEvents) UEventsManager::post(new RtabmapEventInit("Closing database connection..."));
00174 _dbDriver->closeConnection();
00175 if(postInitClosingEvents) UEventsManager::post(new RtabmapEventInit("Closing database connection, done!"));
00176 }
00177
00178 if(_dbDriver == 0)
00179 {
00180 _dbDriver = DBDriver::create(parameters);
00181 }
00182
00183 bool success = true;
00184 if(_dbDriver)
00185 {
00186 _dbDriver->setTimestampUpdateEnabled(true);
00187 success = false;
00188 if(postInitClosingEvents) UEventsManager::post(new RtabmapEventInit(std::string("Connecting to database \"") + dbUrl + "\"..."));
00189 if(_dbDriver->openConnection(dbUrl, dbOverwritten))
00190 {
00191 success = true;
00192 if(postInitClosingEvents) UEventsManager::post(new RtabmapEventInit(std::string("Connecting to database \"") + dbUrl + "\", done!"));
00193 }
00194 else
00195 {
00196 if(postInitClosingEvents) UEventsManager::post(new RtabmapEventInit(RtabmapEventInit::kError, std::string("Connecting to database ") + dbUrl + ", path is invalid!"));
00197 }
00198 }
00199
00200 loadDataFromDb(postInitClosingEvents);
00201
00202 if(postInitClosingEvents) UEventsManager::post(new RtabmapEventInit(RtabmapEventInit::kInitialized));
00203
00204 return success;
00205 }
00206
00207 void Memory::loadDataFromDb(bool postInitClosingEvents)
00208 {
00209 if(_dbDriver && _dbDriver->isConnected())
00210 {
00211 bool loadAllNodesInWM = Parameters::defaultMemInitWMWithAllNodes();
00212 Parameters::parse(parameters_, Parameters::kMemInitWMWithAllNodes(), loadAllNodesInWM);
00213
00214
00215 std::list<Signature*> dbSignatures;
00216
00217 if(loadAllNodesInWM)
00218 {
00219 if(postInitClosingEvents) UEventsManager::post(new RtabmapEventInit(std::string("Loading all nodes to WM...")));
00220 std::set<int> ids;
00221 _dbDriver->getAllNodeIds(ids, true);
00222 _dbDriver->loadSignatures(std::list<int>(ids.begin(), ids.end()), dbSignatures);
00223 }
00224 else
00225 {
00226
00227 if(postInitClosingEvents) UEventsManager::post(new RtabmapEventInit(std::string("Loading last nodes to WM...")));
00228 _dbDriver->loadLastNodes(dbSignatures);
00229 }
00230 for(std::list<Signature*>::reverse_iterator iter=dbSignatures.rbegin(); iter!=dbSignatures.rend(); ++iter)
00231 {
00232
00233 if(!((*iter)->isBadSignature() && _badSignaturesIgnored))
00234 {
00235
00236
00237
00238
00239
00240 _signatures.insert(std::pair<int, Signature *>((*iter)->id(), *iter));
00241 _workingMem.insert(std::make_pair((*iter)->id(), UTimer::now()));
00242 }
00243 else
00244 {
00245 delete *iter;
00246 }
00247 }
00248 if(postInitClosingEvents) UEventsManager::post(new RtabmapEventInit(std::string("Loading nodes to WM, done! (") + uNumber2Str(int(_workingMem.size() + _stMem.size())) + " loaded)"));
00249
00250
00251 if(_stMem.size()>0)
00252 {
00253 _lastSignature = uValue(_signatures, *_stMem.rbegin(), (Signature*)0);
00254 }
00255 else if(_workingMem.size()>0)
00256 {
00257 _lastSignature = uValue(_signatures, _workingMem.rbegin()->first, (Signature*)0);
00258 }
00259
00260
00261 _dbDriver->getLastNodeId(_idCount);
00262 _idMapCount = _lastSignature?_lastSignature->mapId()+1:kIdStart;
00263
00264
00265 if(postInitClosingEvents) UEventsManager::post(new RtabmapEventInit("Loading dictionary..."));
00266 UDEBUG("Loading dictionary...");
00267 if(loadAllNodesInWM)
00268 {
00269 UDEBUG("load all referenced words in working memory");
00270
00271 std::set<int> wordIds;
00272 const std::map<int, Signature *> & signatures = this->getSignatures();
00273 for(std::map<int, Signature *>::const_iterator i=signatures.begin(); i!=signatures.end(); ++i)
00274 {
00275 const std::multimap<int, cv::KeyPoint> & words = i->second->getWords();
00276 std::list<int> keys = uUniqueKeys(words);
00277 for(std::list<int>::iterator iter=keys.begin(); iter!=keys.end(); ++iter)
00278 {
00279 if(*iter > 0)
00280 {
00281 wordIds.insert(*iter);
00282 }
00283 }
00284 }
00285
00286 UDEBUG("load words %d", (int)wordIds.size());
00287 if(_vwd->isIncremental())
00288 {
00289 if(wordIds.size())
00290 {
00291 std::list<VisualWord*> words;
00292 _dbDriver->loadWords(wordIds, words);
00293 for(std::list<VisualWord*>::iterator iter = words.begin(); iter!=words.end(); ++iter)
00294 {
00295 _vwd->addWord(*iter);
00296 }
00297
00298 int id = 0;
00299 _dbDriver->getLastWordId(id);
00300 _vwd->setLastWordId(id);
00301 }
00302 }
00303 else
00304 {
00305 _dbDriver->load(_vwd, false);
00306 }
00307 }
00308 else
00309 {
00310 UDEBUG("load words");
00311
00312 _dbDriver->load(_vwd, _vwd->isIncremental());
00313 }
00314 UDEBUG("%d words loaded!", _vwd->getUnusedWordsSize());
00315 _vwd->update();
00316 if(postInitClosingEvents) UEventsManager::post(new RtabmapEventInit(uFormat("Loading dictionary, done! (%d words)", (int)_vwd->getUnusedWordsSize())));
00317
00318 if(postInitClosingEvents) UEventsManager::post(new RtabmapEventInit(std::string("Adding word references...")));
00319
00320 const std::map<int, Signature *> & signatures = this->getSignatures();
00321 for(std::map<int, Signature *>::const_iterator i=signatures.begin(); i!=signatures.end(); ++i)
00322 {
00323 Signature * s = this->_getSignature(i->first);
00324 UASSERT(s != 0);
00325
00326 const std::multimap<int, cv::KeyPoint> & words = s->getWords();
00327 if(words.size())
00328 {
00329 UDEBUG("node=%d, word references=%d", s->id(), words.size());
00330 for(std::multimap<int, cv::KeyPoint>::const_iterator iter = words.begin(); iter!=words.end(); ++iter)
00331 {
00332 if(iter->first > 0)
00333 {
00334 _vwd->addWordRef(iter->first, i->first);
00335 }
00336 }
00337 s->setEnabled(true);
00338 }
00339 }
00340 if(postInitClosingEvents) UEventsManager::post(new RtabmapEventInit(uFormat("Adding word references, done! (%d)", _vwd->getTotalActiveReferences())));
00341
00342 if(_vwd->getUnusedWordsSize() && _vwd->isIncremental())
00343 {
00344 UWARN("_vwd->getUnusedWordsSize() must be empty... size=%d", _vwd->getUnusedWordsSize());
00345 }
00346 UDEBUG("Total word references added = %d", _vwd->getTotalActiveReferences());
00347
00348 if(_lastSignature == 0)
00349 {
00350
00351 ParametersMap parameters = Parameters::getDefaultParameters();
00352 uInsert(parameters, parameters_);
00353 parameters.erase(Parameters::kRtabmapWorkingDirectory());
00354 UDEBUG("");
00355 _dbDriver->addInfoAfterRun(0, 0, 0, 0, 0, parameters);
00356 }
00357 }
00358 else
00359 {
00360 _idCount = kIdStart;
00361 _idMapCount = kIdStart;
00362 }
00363
00364 _workingMem.insert(std::make_pair(kIdVirtual, 0));
00365
00366 UDEBUG("ids start with %d", _idCount+1);
00367 UDEBUG("map ids start with %d", _idMapCount);
00368 }
00369
00370 void Memory::close(bool databaseSaved, bool postInitClosingEvents, const std::string & ouputDatabasePath)
00371 {
00372 UINFO("databaseSaved=%d, postInitClosingEvents=%d", databaseSaved?1:0, postInitClosingEvents?1:0);
00373 if(postInitClosingEvents) UEventsManager::post(new RtabmapEventInit(RtabmapEventInit::kClosing));
00374
00375 bool databaseNameChanged = false;
00376 if(databaseSaved && _dbDriver)
00377 {
00378 databaseNameChanged = ouputDatabasePath.size() && _dbDriver->getUrl().size() && _dbDriver->getUrl().compare(ouputDatabasePath) != 0?true:false;
00379 }
00380
00381 if(!databaseSaved || (!_memoryChanged && !_linksChanged && !databaseNameChanged))
00382 {
00383 if(postInitClosingEvents) UEventsManager::post(new RtabmapEventInit(uFormat("No changes added to database.")));
00384
00385 UINFO("No changes added to database.");
00386 if(_dbDriver)
00387 {
00388 if(postInitClosingEvents) UEventsManager::post(new RtabmapEventInit(uFormat("Closing database \"%s\"...", _dbDriver->getUrl().c_str())));
00389 _dbDriver->closeConnection(false, ouputDatabasePath);
00390 delete _dbDriver;
00391 _dbDriver = 0;
00392 if(postInitClosingEvents) UEventsManager::post(new RtabmapEventInit("Closing database, done!"));
00393 }
00394 if(postInitClosingEvents) UEventsManager::post(new RtabmapEventInit("Clearing memory..."));
00395 this->clear();
00396 if(postInitClosingEvents) UEventsManager::post(new RtabmapEventInit("Clearing memory, done!"));
00397 }
00398 else
00399 {
00400 UINFO("Saving memory...");
00401 if(postInitClosingEvents) UEventsManager::post(new RtabmapEventInit("Saving memory..."));
00402 if(!_memoryChanged && _linksChanged && _dbDriver)
00403 {
00404
00405 UDEBUG("");
00406 _dbDriver->setTimestampUpdateEnabled(false);
00407 }
00408 this->clear();
00409 if(_dbDriver)
00410 {
00411 _dbDriver->emptyTrashes();
00412 if(postInitClosingEvents) UEventsManager::post(new RtabmapEventInit("Saving memory, done!"));
00413 if(postInitClosingEvents) UEventsManager::post(new RtabmapEventInit(uFormat("Closing database \"%s\"...", _dbDriver->getUrl().c_str())));
00414 _dbDriver->closeConnection(true, ouputDatabasePath);
00415 delete _dbDriver;
00416 _dbDriver = 0;
00417 if(postInitClosingEvents) UEventsManager::post(new RtabmapEventInit("Closing database, done!"));
00418 }
00419 else
00420 {
00421 if(postInitClosingEvents) UEventsManager::post(new RtabmapEventInit("Saving memory, done!"));
00422 }
00423 }
00424 if(postInitClosingEvents) UEventsManager::post(new RtabmapEventInit(RtabmapEventInit::kClosed));
00425 }
00426
00427 Memory::~Memory()
00428 {
00429 this->close();
00430
00431 if(_dbDriver)
00432 {
00433 UWARN("Please call Memory::close() before");
00434 }
00435 delete _feature2D;
00436 delete _vwd;
00437 delete _registrationPipeline;
00438 delete _registrationIcpMulti;
00439 delete _occupancy;
00440 }
00441
00442 void Memory::parseParameters(const ParametersMap & parameters)
00443 {
00444 uInsert(parameters_, parameters);
00445 ParametersMap params = parameters;
00446
00447 UDEBUG("");
00448 ParametersMap::const_iterator iter;
00449
00450 Parameters::parse(params, Parameters::kMemBinDataKept(), _binDataKept);
00451 Parameters::parse(params, Parameters::kMemRawDescriptorsKept(), _rawDescriptorsKept);
00452 Parameters::parse(params, Parameters::kMemSaveDepth16Format(), _saveDepth16Format);
00453 Parameters::parse(params, Parameters::kMemReduceGraph(), _reduceGraph);
00454 Parameters::parse(params, Parameters::kMemNotLinkedNodesKept(), _notLinkedNodesKeptInDb);
00455 Parameters::parse(params, Parameters::kMemIntermediateNodeDataKept(), _saveIntermediateNodeData);
00456 Parameters::parse(params, Parameters::kMemRehearsalIdUpdatedToNewOne(), _idUpdatedToNewOneRehearsal);
00457 Parameters::parse(params, Parameters::kMemGenerateIds(), _generateIds);
00458 Parameters::parse(params, Parameters::kMemBadSignaturesIgnored(), _badSignaturesIgnored);
00459 Parameters::parse(params, Parameters::kMemMapLabelsAdded(), _mapLabelsAdded);
00460 Parameters::parse(params, Parameters::kMemRehearsalSimilarity(), _similarityThreshold);
00461 Parameters::parse(params, Parameters::kMemRecentWmRatio(), _recentWmRatio);
00462 Parameters::parse(params, Parameters::kMemTransferSortingByWeightId(), _transferSortingByWeightId);
00463 Parameters::parse(params, Parameters::kMemSTMSize(), _maxStMemSize);
00464 Parameters::parse(params, Parameters::kMemDepthAsMask(), _depthAsMask);
00465 Parameters::parse(params, Parameters::kMemImagePreDecimation(), _imagePreDecimation);
00466 Parameters::parse(params, Parameters::kMemImagePostDecimation(), _imagePostDecimation);
00467 Parameters::parse(params, Parameters::kMemCompressionParallelized(), _compressionParallelized);
00468 Parameters::parse(params, Parameters::kMemLaserScanDownsampleStepSize(), _laserScanDownsampleStepSize);
00469 Parameters::parse(params, Parameters::kMemLaserScanVoxelSize(), _laserScanVoxelSize);
00470 Parameters::parse(params, Parameters::kMemLaserScanNormalK(), _laserScanNormalK);
00471 Parameters::parse(params, Parameters::kMemLaserScanNormalRadius(), _laserScanNormalRadius);
00472 Parameters::parse(params, Parameters::kRGBDLoopClosureReextractFeatures(), _reextractLoopClosureFeatures);
00473 Parameters::parse(params, Parameters::kRGBDLocalBundleOnLoopClosure(), _localBundleOnLoopClosure);
00474 Parameters::parse(params, Parameters::kRGBDLinearUpdate(), _rehearsalMaxDistance);
00475 Parameters::parse(params, Parameters::kRGBDAngularUpdate(), _rehearsalMaxAngle);
00476 Parameters::parse(params, Parameters::kMemRehearsalWeightIgnoredWhileMoving(), _rehearsalWeightIgnoredWhileMoving);
00477 Parameters::parse(params, Parameters::kMemUseOdomFeatures(), _useOdometryFeatures);
00478 Parameters::parse(params, Parameters::kRGBDCreateOccupancyGrid(), _createOccupancyGrid);
00479 Parameters::parse(params, Parameters::kVisMaxFeatures(), _visMaxFeatures);
00480 Parameters::parse(params, Parameters::kVisCorType(), _visCorType);
00481 if(_visCorType != 0)
00482 {
00483 UWARN("%s is not 0 (Features Matching), the only approach supported for loop closure transformation estimation. Setting to 0...",
00484 Parameters::kVisCorType().c_str());
00485 _visCorType = 0;
00486 uInsert(parameters_, ParametersPair(Parameters::kVisCorType(), "0"));
00487 uInsert(params, ParametersPair(Parameters::kVisCorType(), "0"));
00488 }
00489 Parameters::parse(params, Parameters::kRtabmapImagesAlreadyRectified(), _imagesAlreadyRectified);
00490 Parameters::parse(params, Parameters::kRtabmapRectifyOnlyFeatures(), _rectifyOnlyFeatures);
00491 Parameters::parse(params, Parameters::kMemCovOffDiagIgnored(), _covOffDiagonalIgnored);
00492
00493
00494 UASSERT_MSG(_maxStMemSize >= 0, uFormat("value=%d", _maxStMemSize).c_str());
00495 UASSERT_MSG(_similarityThreshold >= 0.0f && _similarityThreshold <= 1.0f, uFormat("value=%f", _similarityThreshold).c_str());
00496 UASSERT_MSG(_recentWmRatio >= 0.0f && _recentWmRatio <= 1.0f, uFormat("value=%f", _recentWmRatio).c_str());
00497 if(_imagePreDecimation == 0)
00498 {
00499 _imagePreDecimation = 1;
00500 }
00501 if(_imagePostDecimation == 0)
00502 {
00503 _imagePostDecimation = 1;
00504 }
00505 UASSERT(_rehearsalMaxDistance >= 0.0f);
00506 UASSERT(_rehearsalMaxAngle >= 0.0f);
00507
00508 if(_dbDriver)
00509 {
00510 _dbDriver->parseParameters(params);
00511 }
00512
00513
00514 if(_vwd)
00515 {
00516 _vwd->parseParameters(params);
00517 }
00518
00519 Parameters::parse(params, Parameters::kKpTfIdfLikelihoodUsed(), _tfIdfLikelihoodUsed);
00520 Parameters::parse(params, Parameters::kKpParallelized(), _parallelized);
00521 Parameters::parse(params, Parameters::kKpBadSignRatio(), _badSignRatio);
00522
00523
00524 UASSERT(_feature2D != 0);
00525 Feature2D::Type detectorStrategy = Feature2D::kFeatureUndef;
00526 if((iter=params.find(Parameters::kKpDetectorStrategy())) != params.end())
00527 {
00528 detectorStrategy = (Feature2D::Type)std::atoi((*iter).second.c_str());
00529 }
00530 if(detectorStrategy!=Feature2D::kFeatureUndef && detectorStrategy!=_feature2D->getType())
00531 {
00532 if(_vwd->getVisualWords().size())
00533 {
00534 UWARN("new detector strategy %d while the vocabulary is already created. This may give problems if feature descriptors are not the same type than the one used in the current vocabulary (a memory reset would be required if so).", int(detectorStrategy));
00535 }
00536 else
00537 {
00538 UINFO("new detector strategy %d.", int(detectorStrategy));
00539 }
00540 if(_feature2D)
00541 {
00542 delete _feature2D;
00543 _feature2D = 0;
00544 }
00545
00546 _feature2D = Feature2D::create(detectorStrategy, parameters_);
00547 }
00548 else if(_feature2D)
00549 {
00550 _feature2D->parseParameters(params);
00551 }
00552
00553 Registration::Type regStrategy = Registration::kTypeUndef;
00554 if((iter=params.find(Parameters::kRegStrategy())) != params.end())
00555 {
00556 regStrategy = (Registration::Type)std::atoi((*iter).second.c_str());
00557 }
00558 if(regStrategy!=Registration::kTypeUndef)
00559 {
00560 UDEBUG("new registration strategy %d", int(regStrategy));
00561 if(_registrationPipeline)
00562 {
00563 delete _registrationPipeline;
00564 _registrationPipeline = 0;
00565 }
00566
00567 _registrationPipeline = Registration::create(regStrategy, parameters_);
00568 }
00569 else if(_registrationPipeline)
00570 {
00571 _registrationPipeline->parseParameters(params);
00572 }
00573
00574 if(_registrationIcpMulti)
00575 {
00576 if(uContains(params, Parameters::kIcpCorrespondenceRatio()))
00577 {
00578
00579 float corRatio = Parameters::defaultIcpCorrespondenceRatio();
00580 Parameters::parse(parameters, Parameters::kIcpCorrespondenceRatio(), corRatio);
00581 ParametersMap paramsMulti = params;
00582 paramsMulti.insert(ParametersPair(Parameters::kIcpCorrespondenceRatio(), uNumber2Str(corRatio>=0.5f?1.0f:corRatio*2.0f)));
00583 if(corRatio >= 0.5)
00584 {
00585 UWARN( "%s is >=0.5, which sets correspondence ratio for proximity detection using "
00586 "laser scans to 100% (2 x Ratio). You may lower the ratio to accept proximity "
00587 "detection with not full scans overlapping.", Parameters::kIcpCorrespondenceRatio().c_str());
00588 }
00589 _registrationIcpMulti->parseParameters(paramsMulti);
00590 }
00591 else
00592 {
00593 _registrationIcpMulti->parseParameters(params);
00594 }
00595 }
00596
00597 if(_occupancy)
00598 {
00599 _occupancy->parseParameters(params);
00600 }
00601
00602
00603
00604 iter = params.find(Parameters::kMemIncrementalMemory());
00605 if(iter != params.end())
00606 {
00607 bool value = uStr2Bool(iter->second.c_str());
00608 if(value == false && _incrementalMemory)
00609 {
00610
00611 this->incrementMapId();
00612
00613
00614
00615 if((_memoryChanged || _linksChanged) && _dbDriver)
00616 {
00617 UWARN("Switching from Mapping to Localization mode, the database will be saved and reloaded.");
00618 bool memoryChanged = _memoryChanged;
00619 bool linksChanged = _linksChanged;
00620 this->clear();
00621 _memoryChanged = memoryChanged;
00622 _linksChanged = linksChanged;
00623 this->loadDataFromDb(false);
00624 UWARN("Switching from Mapping to Localization mode, the database is reloaded!");
00625 }
00626 }
00627 _incrementalMemory = value;
00628 }
00629
00630 if(_useOdometryFeatures)
00631 {
00632 int visFeatureType = Parameters::defaultVisFeatureType();
00633 int kpDetectorStrategy = Parameters::defaultKpDetectorStrategy();
00634 Parameters::parse(parameters_, Parameters::kVisFeatureType(), visFeatureType);
00635 Parameters::parse(parameters_, Parameters::kKpDetectorStrategy(), kpDetectorStrategy);
00636 if(visFeatureType != kpDetectorStrategy)
00637 {
00638 UWARN("%s is enabled, but %s and %s parameters are not the same! Disabling %s...",
00639 Parameters::kMemUseOdomFeatures().c_str(),
00640 Parameters::kVisFeatureType().c_str(),
00641 Parameters::kKpDetectorStrategy().c_str(),
00642 Parameters::kMemUseOdomFeatures().c_str());
00643 _useOdometryFeatures = false;
00644 uInsert(parameters_, ParametersPair(Parameters::kMemUseOdomFeatures(), "false"));
00645 }
00646 }
00647 }
00648
00649 void Memory::preUpdate()
00650 {
00651 _signaturesAdded = 0;
00652 if(_vwd->isIncremental())
00653 {
00654 this->cleanUnusedWords();
00655 }
00656 if(_vwd && !_parallelized)
00657 {
00658
00659 _vwd->update();
00660 }
00661 }
00662
00663 bool Memory::update(
00664 const SensorData & data,
00665 Statistics * stats)
00666 {
00667 return update(data, Transform(), cv::Mat(), std::vector<float>(), stats);
00668 }
00669
00670 bool Memory::update(
00671 const SensorData & data,
00672 const Transform & pose,
00673 const cv::Mat & covariance,
00674 const std::vector<float> & velocity,
00675 Statistics * stats)
00676 {
00677 UDEBUG("");
00678 UTimer timer;
00679 UTimer totalTimer;
00680 timer.start();
00681 float t;
00682
00683
00684
00685
00686 UDEBUG("pre-updating...");
00687 this->preUpdate();
00688 t=timer.ticks()*1000;
00689 if(stats) stats->addStatistic(Statistics::kTimingMemPre_update(), t);
00690 UDEBUG("time preUpdate=%f ms", t);
00691
00692
00693
00694
00695 Signature * signature = this->createSignature(data, pose, stats);
00696 if (signature == 0)
00697 {
00698 UERROR("Failed to create a signature...");
00699 return false;
00700 }
00701 if(velocity.size()==6)
00702 {
00703 signature->setVelocity(velocity[0], velocity[1], velocity[2], velocity[3], velocity[4], velocity[5]);
00704 }
00705
00706 t=timer.ticks()*1000;
00707 if(stats) stats->addStatistic(Statistics::kTimingMemSignature_creation(), t);
00708 UDEBUG("time creating signature=%f ms", t);
00709
00710
00711 this->addSignatureToStm(signature, covariance);
00712
00713 _lastSignature = signature;
00714
00715
00716
00717
00718
00719
00720
00721 if(_incrementalMemory)
00722 {
00723 if(_similarityThreshold < 1.0f)
00724 {
00725 this->rehearsal(signature, stats);
00726 }
00727 t=timer.ticks()*1000;
00728 if(stats) stats->addStatistic(Statistics::kTimingMemRehearsal(), t);
00729 UDEBUG("time rehearsal=%f ms", t);
00730 }
00731 else
00732 {
00733 if(_workingMem.size() <= 1)
00734 {
00735 UWARN("The working memory is empty and the memory is not "
00736 "incremental (Mem/IncrementalMemory=False), no loop closure "
00737 "can be detected! Please set Mem/IncrementalMemory=true to increase "
00738 "the memory with new images or decrease the STM size (which is %d "
00739 "including the new one added).", (int)_stMem.size());
00740 }
00741 }
00742
00743
00744
00745
00746 int notIntermediateNodesCount = 0;
00747 for(std::set<int>::iterator iter=_stMem.begin(); iter!=_stMem.end(); ++iter)
00748 {
00749 const Signature * s = this->getSignature(*iter);
00750 UASSERT(s != 0);
00751 if(s->getWeight() >= 0)
00752 {
00753 ++notIntermediateNodesCount;
00754 }
00755 }
00756 std::map<int, int> reducedIds;
00757 while(_stMem.size() && _maxStMemSize>0 && notIntermediateNodesCount > _maxStMemSize)
00758 {
00759 int id = *_stMem.begin();
00760 Signature * s = this->_getSignature(id);
00761 UASSERT(s != 0);
00762 if(s->getWeight() >= 0)
00763 {
00764 --notIntermediateNodesCount;
00765 }
00766
00767 int reducedTo = 0;
00768 moveSignatureToWMFromSTM(id, &reducedTo);
00769
00770 if(reducedTo > 0)
00771 {
00772 reducedIds.insert(std::make_pair(id, reducedTo));
00773 }
00774 }
00775 if(stats) stats->setReducedIds(reducedIds);
00776
00777 if(!_memoryChanged && _incrementalMemory)
00778 {
00779 _memoryChanged = true;
00780 }
00781
00782 UDEBUG("totalTimer = %fs", totalTimer.ticks());
00783
00784 return true;
00785 }
00786
00787 void Memory::addSignatureToStm(Signature * signature, const cv::Mat & covariance)
00788 {
00789 UTimer timer;
00790
00791 if(signature)
00792 {
00793 UDEBUG("adding %d", signature->id());
00794
00795 if(_stMem.size())
00796 {
00797 if(_signatures.at(*_stMem.rbegin())->mapId() == signature->mapId())
00798 {
00799 Transform motionEstimate;
00800 if(!signature->getPose().isNull() &&
00801 !_signatures.at(*_stMem.rbegin())->getPose().isNull())
00802 {
00803 UASSERT(covariance.cols == 6 && covariance.rows == 6 && covariance.type() == CV_64FC1);
00804 double maxAngVar = uMax3(covariance.at<double>(3,3), covariance.at<double>(4,4), covariance.at<double>(5,5));
00805 if(maxAngVar != 1.0 && maxAngVar > 0.1)
00806 {
00807 UWARN("Very large angular variance (%f) detected! Please fix odometry "
00808 "twist covariance, otherwise poor graph optimizations are "
00809 "expected and wrong loop closure detections creating a lot "
00810 "of errors in the map could be accepted.", maxAngVar);
00811 }
00812
00813 cv::Mat infMatrix;
00814 if(_covOffDiagonalIgnored)
00815 {
00816 infMatrix = cv::Mat::zeros(6,6,CV_64FC1);
00817 infMatrix.at<double>(0,0) = 1.0 / covariance.at<double>(0,0);
00818 infMatrix.at<double>(1,1) = 1.0 / covariance.at<double>(1,1);
00819 infMatrix.at<double>(2,2) = 1.0 / covariance.at<double>(2,2);
00820 infMatrix.at<double>(3,3) = 1.0 / covariance.at<double>(3,3);
00821 infMatrix.at<double>(4,4) = 1.0 / covariance.at<double>(4,4);
00822 infMatrix.at<double>(5,5) = 1.0 / covariance.at<double>(5,5);
00823 }
00824 else
00825 {
00826 infMatrix = covariance.inv();
00827 }
00828 if((uIsFinite(covariance.at<double>(0,0)) && covariance.at<double>(0,0)>0.0) &&
00829 !(uIsFinite(infMatrix.at<double>(0,0)) && infMatrix.at<double>(0,0)>0.0))
00830 {
00831 UERROR("Failed to invert the covariance matrix! Covariance matrix should be invertible!");
00832 std::cout << "Covariance: " << covariance << std::endl;
00833 infMatrix = cv::Mat::eye(6,6,CV_64FC1);
00834 }
00835 motionEstimate = _signatures.at(*_stMem.rbegin())->getPose().inverse() * signature->getPose();
00836 _signatures.at(*_stMem.rbegin())->addLink(Link(*_stMem.rbegin(), signature->id(), Link::kNeighbor, motionEstimate, infMatrix));
00837 signature->addLink(Link(signature->id(), *_stMem.rbegin(), Link::kNeighbor, motionEstimate.inverse(), infMatrix));
00838 }
00839 else
00840 {
00841 _signatures.at(*_stMem.rbegin())->addLink(Link(*_stMem.rbegin(), signature->id(), Link::kNeighbor, Transform()));
00842 signature->addLink(Link(signature->id(), *_stMem.rbegin(), Link::kNeighbor, Transform()));
00843 }
00844 UDEBUG("Min STM id = %d", *_stMem.begin());
00845 }
00846 else
00847 {
00848 UDEBUG("Ignoring neighbor link between %d and %d because they are not in the same map! (%d vs %d)",
00849 *_stMem.rbegin(), signature->id(),
00850 _signatures.at(*_stMem.rbegin())->mapId(), signature->mapId());
00851
00852 if(_mapLabelsAdded && isIncremental())
00853 {
00854
00855 std::string tag = uFormat("map%d", signature->mapId());
00856 if(getSignatureIdByLabel(tag, false) == 0)
00857 {
00858 UINFO("Tagging node %d with label \"%s\"", signature->id(), tag.c_str());
00859 signature->setLabel(tag);
00860 }
00861 }
00862 }
00863 }
00864 else if(_mapLabelsAdded && isIncremental())
00865 {
00866
00867 std::string tag = uFormat("map%d", signature->mapId());
00868 if(getSignatureIdByLabel(tag, false) == 0)
00869 {
00870 UINFO("Tagging node %d with label \"%s\"", signature->id(), tag.c_str());
00871 signature->setLabel(tag);
00872 }
00873 }
00874
00875 _signatures.insert(_signatures.end(), std::pair<int, Signature *>(signature->id(), signature));
00876 _stMem.insert(_stMem.end(), signature->id());
00877 ++_signaturesAdded;
00878
00879 if(_vwd)
00880 {
00881 UDEBUG("%d words ref for the signature %d", signature->getWords().size(), signature->id());
00882 }
00883 if(signature->getWords().size())
00884 {
00885 signature->setEnabled(true);
00886 }
00887 }
00888
00889 UDEBUG("time = %fs", timer.ticks());
00890 }
00891
00892 void Memory::addSignatureToWmFromLTM(Signature * signature)
00893 {
00894 if(signature)
00895 {
00896 UDEBUG("Inserting node %d in WM...", signature->id());
00897 _workingMem.insert(std::make_pair(signature->id(), UTimer::now()));
00898 _signatures.insert(std::pair<int, Signature*>(signature->id(), signature));
00899 ++_signaturesAdded;
00900 }
00901 else
00902 {
00903 UERROR("Signature is null ?!?");
00904 }
00905 }
00906
00907 void Memory::moveSignatureToWMFromSTM(int id, int * reducedTo)
00908 {
00909 UDEBUG("Inserting node %d from STM in WM...", id);
00910 UASSERT(_stMem.find(id) != _stMem.end());
00911 Signature * s = this->_getSignature(id);
00912 UASSERT(s!=0);
00913
00914 if(_reduceGraph)
00915 {
00916 bool merge = false;
00917 const std::map<int, Link> & links = s->getLinks();
00918 std::map<int, Link> neighbors;
00919 for(std::map<int, Link>::const_iterator iter=links.begin(); iter!=links.end(); ++iter)
00920 {
00921 if(!merge)
00922 {
00923 merge = iter->second.to() < s->id() &&
00924 iter->second.to() != iter->second.from() &&
00925 iter->second.type() != Link::kNeighbor &&
00926 iter->second.type() != Link::kNeighborMerged &&
00927 iter->second.userDataCompressed().empty() &&
00928 iter->second.type() != Link::kUndef &&
00929 iter->second.type() != Link::kVirtualClosure;
00930 if(merge)
00931 {
00932 UDEBUG("Reduce %d to %d", s->id(), iter->second.to());
00933 if(reducedTo)
00934 {
00935 *reducedTo = iter->second.to();
00936 }
00937 }
00938
00939 }
00940 if(iter->second.type() == Link::kNeighbor)
00941 {
00942 neighbors.insert(*iter);
00943 }
00944 }
00945 if(merge)
00946 {
00947 if(s->getLabel().empty())
00948 {
00949 for(std::map<int, Link>::const_iterator iter=links.begin(); iter!=links.end(); ++iter)
00950 {
00951 merge = true;
00952 Signature * sTo = this->_getSignature(iter->first);
00953 UASSERT(sTo!=0);
00954 sTo->removeLink(s->id());
00955 if(iter->second.type() != Link::kNeighbor &&
00956 iter->second.type() != Link::kNeighborMerged &&
00957 iter->second.type() != Link::kUndef)
00958 {
00959
00960 for(std::map<int, Link>::iterator jter=neighbors.begin(); jter!=neighbors.end(); ++jter)
00961 {
00962 if(!sTo->hasLink(jter->second.to()))
00963 {
00964 Link l = iter->second.inverse().merge(
00965 jter->second,
00966 iter->second.userDataCompressed().empty() && iter->second.type() != Link::kVirtualClosure?Link::kNeighborMerged:iter->second.type());
00967 sTo->addLink(l);
00968 Signature * sB = this->_getSignature(l.to());
00969 UASSERT(sB!=0);
00970 UASSERT(!sB->hasLink(l.to()));
00971 sB->addLink(l.inverse());
00972 }
00973 }
00974 }
00975 }
00976
00977
00978 std::map<int, Link> linksCopy = links;
00979 for(std::map<int, Link>::iterator iter=linksCopy.begin(); iter!=linksCopy.end(); ++iter)
00980 {
00981 if(iter->second.type() == Link::kNeighbor ||
00982 iter->second.type() == Link::kNeighborMerged)
00983 {
00984 s->removeLink(iter->first);
00985 if(iter->second.type() == Link::kNeighbor)
00986 {
00987 if(_lastGlobalLoopClosureId == s->id())
00988 {
00989 _lastGlobalLoopClosureId = iter->first;
00990 }
00991 }
00992 }
00993 }
00994
00995 this->moveToTrash(s, _notLinkedNodesKeptInDb);
00996 s = 0;
00997 }
00998 }
00999 }
01000 if(s != 0)
01001 {
01002 _workingMem.insert(_workingMem.end(), std::make_pair(*_stMem.begin(), UTimer::now()));
01003 _stMem.erase(*_stMem.begin());
01004 }
01005
01006 }
01007
01008 const Signature * Memory::getSignature(int id) const
01009 {
01010 return _getSignature(id);
01011 }
01012
01013 Signature * Memory::_getSignature(int id) const
01014 {
01015 return uValue(_signatures, id, (Signature*)0);
01016 }
01017
01018 const VWDictionary * Memory::getVWDictionary() const
01019 {
01020 return _vwd;
01021 }
01022
01023 std::map<int, Link> Memory::getNeighborLinks(
01024 int signatureId,
01025 bool lookInDatabase) const
01026 {
01027 std::map<int, Link> links;
01028 Signature * s = uValue(_signatures, signatureId, (Signature*)0);
01029 if(s)
01030 {
01031 const std::map<int, Link> & allLinks = s->getLinks();
01032 for(std::map<int, Link>::const_iterator iter = allLinks.begin(); iter!=allLinks.end(); ++iter)
01033 {
01034 if(iter->second.type() == Link::kNeighbor ||
01035 iter->second.type() == Link::kNeighborMerged)
01036 {
01037 links.insert(*iter);
01038 }
01039 }
01040 }
01041 else if(lookInDatabase && _dbDriver)
01042 {
01043 std::map<int, Link> neighbors;
01044 _dbDriver->loadLinks(signatureId, neighbors);
01045 for(std::map<int, Link>::iterator iter=neighbors.begin(); iter!=neighbors.end();)
01046 {
01047 if(iter->second.type() != Link::kNeighbor &&
01048 iter->second.type() != Link::kNeighborMerged)
01049 {
01050 neighbors.erase(iter++);
01051 }
01052 else
01053 {
01054 ++iter;
01055 }
01056 }
01057 }
01058 else
01059 {
01060 UWARN("Cannot find signature %d in memory", signatureId);
01061 }
01062 return links;
01063 }
01064
01065 std::map<int, Link> Memory::getLoopClosureLinks(
01066 int signatureId,
01067 bool lookInDatabase) const
01068 {
01069 const Signature * s = this->getSignature(signatureId);
01070 std::map<int, Link> loopClosures;
01071 if(s)
01072 {
01073 const std::map<int, Link> & allLinks = s->getLinks();
01074 for(std::map<int, Link>::const_iterator iter = allLinks.begin(); iter!=allLinks.end(); ++iter)
01075 {
01076 if(iter->second.type() != Link::kNeighbor &&
01077 iter->second.type() != Link::kNeighborMerged &&
01078 iter->second.type() != Link::kPosePrior &&
01079 iter->second.type() != Link::kUndef)
01080 {
01081 loopClosures.insert(*iter);
01082 }
01083 }
01084 }
01085 else if(lookInDatabase && _dbDriver)
01086 {
01087 _dbDriver->loadLinks(signatureId, loopClosures);
01088 for(std::map<int, Link>::iterator iter=loopClosures.begin(); iter!=loopClosures.end();)
01089 {
01090 if(iter->second.type() == Link::kNeighbor ||
01091 iter->second.type() == Link::kNeighborMerged ||
01092 iter->second.type() == Link::kUndef )
01093 {
01094 loopClosures.erase(iter++);
01095 }
01096 else
01097 {
01098 ++iter;
01099 }
01100 }
01101 }
01102 return loopClosures;
01103 }
01104
01105 std::map<int, Link> Memory::getLinks(
01106 int signatureId,
01107 bool lookInDatabase) const
01108 {
01109 std::map<int, Link> links;
01110 Signature * s = uValue(_signatures, signatureId, (Signature*)0);
01111 if(s)
01112 {
01113 links = s->getLinks();
01114 }
01115 else if(lookInDatabase && _dbDriver)
01116 {
01117 _dbDriver->loadLinks(signatureId, links, Link::kUndef);
01118 }
01119 else
01120 {
01121 UWARN("Cannot find signature %d in memory", signatureId);
01122 }
01123 return links;
01124 }
01125
01126 std::multimap<int, Link> Memory::getAllLinks(bool lookInDatabase, bool ignoreNullLinks) const
01127 {
01128 std::multimap<int, Link> links;
01129
01130 if(lookInDatabase && _dbDriver)
01131 {
01132 _dbDriver->getAllLinks(links, ignoreNullLinks);
01133 }
01134
01135 for(std::map<int, Signature*>::const_iterator iter=_signatures.begin(); iter!=_signatures.end(); ++iter)
01136 {
01137 links.erase(iter->first);
01138 for(std::map<int, Link>::const_iterator jter=iter->second->getLinks().begin();
01139 jter!=iter->second->getLinks().end();
01140 ++jter)
01141 {
01142 if(!ignoreNullLinks || jter->second.isValid())
01143 {
01144 links.insert(std::make_pair(iter->first, jter->second));
01145 }
01146 }
01147 }
01148
01149 return links;
01150 }
01151
01152
01153
01154
01155
01156 std::map<int, int> Memory::getNeighborsId(
01157 int signatureId,
01158 int maxGraphDepth,
01159 int maxCheckedInDatabase,
01160 bool incrementMarginOnLoop,
01161 bool ignoreLoopIds,
01162 bool ignoreIntermediateNodes,
01163 bool ignoreLocalSpaceLoopIds,
01164 const std::set<int> & nodesSet,
01165 double * dbAccessTime
01166 ) const
01167 {
01168 UASSERT(maxGraphDepth >= 0);
01169
01170 if(dbAccessTime)
01171 {
01172 *dbAccessTime = 0;
01173 }
01174 std::map<int, int> ids;
01175 if(signatureId<=0)
01176 {
01177 return ids;
01178 }
01179 int nbLoadedFromDb = 0;
01180 std::list<int> curentMarginList;
01181 std::set<int> currentMargin;
01182 std::set<int> nextMargin;
01183 nextMargin.insert(signatureId);
01184 int m = 0;
01185 std::set<int> ignoredIds;
01186 while((maxGraphDepth == 0 || m < maxGraphDepth) && nextMargin.size())
01187 {
01188
01189 curentMarginList = std::list<int>(nextMargin.rbegin(), nextMargin.rend());
01190 nextMargin.clear();
01191
01192 for(std::list<int>::iterator jter = curentMarginList.begin(); jter!=curentMarginList.end(); ++jter)
01193 {
01194 if(ids.find(*jter) == ids.end() && (nodesSet.empty() || nodesSet.find(*jter) != nodesSet.end()))
01195 {
01196
01197
01198 const Signature * s = this->getSignature(*jter);
01199 std::map<int, Link> tmpLinks;
01200 const std::map<int, Link> * links = &tmpLinks;
01201 if(s)
01202 {
01203 if(!ignoreIntermediateNodes || s->getWeight() != -1)
01204 {
01205 ids.insert(std::pair<int, int>(*jter, m));
01206 }
01207 else
01208 {
01209 ignoredIds.insert(*jter);
01210 }
01211
01212 links = &s->getLinks();
01213 }
01214 else if(maxCheckedInDatabase == -1 || (maxCheckedInDatabase > 0 && _dbDriver && nbLoadedFromDb < maxCheckedInDatabase))
01215 {
01216 ++nbLoadedFromDb;
01217 ids.insert(std::pair<int, int>(*jter, m));
01218
01219 UTimer timer;
01220 _dbDriver->loadLinks(*jter, tmpLinks);
01221 if(dbAccessTime)
01222 {
01223 *dbAccessTime += timer.getElapsedTime();
01224 }
01225 }
01226
01227
01228 for(std::map<int, Link>::const_iterator iter=links->begin(); iter!=links->end(); ++iter)
01229 {
01230 if( !uContains(ids, iter->first) && ignoredIds.find(iter->first) == ignoredIds.end())
01231 {
01232 UASSERT(iter->second.type() != Link::kUndef);
01233 if(iter->second.type() == Link::kNeighbor ||
01234 iter->second.type() == Link::kNeighborMerged)
01235 {
01236 if(ignoreIntermediateNodes && s->getWeight()==-1)
01237 {
01238
01239 if(currentMargin.insert(iter->first).second)
01240 {
01241 curentMarginList.push_back(iter->first);
01242 }
01243 }
01244 else
01245 {
01246 nextMargin.insert(iter->first);
01247 }
01248 }
01249 else if(!ignoreLoopIds && (!ignoreLocalSpaceLoopIds || iter->second.type()!=Link::kLocalSpaceClosure))
01250 {
01251 if(incrementMarginOnLoop)
01252 {
01253 nextMargin.insert(iter->first);
01254 }
01255 else
01256 {
01257 if(currentMargin.insert(iter->first).second)
01258 {
01259 curentMarginList.push_back(iter->first);
01260 }
01261 }
01262 }
01263 }
01264 }
01265 }
01266 }
01267 ++m;
01268 }
01269 return ids;
01270 }
01271
01272
01273 std::map<int, float> Memory::getNeighborsIdRadius(
01274 int signatureId,
01275 float radius,
01276 const std::map<int, Transform> & optimizedPoses,
01277 int maxGraphDepth
01278 ) const
01279 {
01280 UASSERT(maxGraphDepth >= 0);
01281 UASSERT(uContains(optimizedPoses, signatureId));
01282 UASSERT(signatureId > 0);
01283 std::map<int, float> ids;
01284 std::map<int, float> checkedIds;
01285 std::list<int> curentMarginList;
01286 std::set<int> currentMargin;
01287 std::set<int> nextMargin;
01288 nextMargin.insert(signatureId);
01289 int m = 0;
01290 Transform referential = optimizedPoses.at(signatureId);
01291 UASSERT(!referential.isNull());
01292 float radiusSqrd = radius*radius;
01293 while((maxGraphDepth == 0 || m < maxGraphDepth) && nextMargin.size())
01294 {
01295 curentMarginList = std::list<int>(nextMargin.begin(), nextMargin.end());
01296 nextMargin.clear();
01297
01298 for(std::list<int>::iterator jter = curentMarginList.begin(); jter!=curentMarginList.end(); ++jter)
01299 {
01300 if(checkedIds.find(*jter) == checkedIds.end())
01301 {
01302
01303
01304 const Signature * s = this->getSignature(*jter);
01305 std::map<int, Link> tmpLinks;
01306 const std::map<int, Link> * links = &tmpLinks;
01307 if(s)
01308 {
01309 const Transform & t = optimizedPoses.at(*jter);
01310 UASSERT(!t.isNull());
01311 float distanceSqrd = referential.getDistanceSquared(t);
01312 if(radiusSqrd == 0 || distanceSqrd<radiusSqrd)
01313 {
01314 ids.insert(std::pair<int, float>(*jter,distanceSqrd));
01315 }
01316
01317 links = &s->getLinks();
01318 }
01319
01320
01321 for(std::map<int, Link>::const_iterator iter=links->begin(); iter!=links->end(); ++iter)
01322 {
01323 if(!uContains(ids, iter->first) &&
01324 uContains(optimizedPoses, iter->first) &&
01325 iter->second.type()!=Link::kVirtualClosure)
01326 {
01327 nextMargin.insert(iter->first);
01328 }
01329 }
01330 }
01331 }
01332 ++m;
01333 }
01334 return ids;
01335 }
01336
01337 int Memory::getNextId()
01338 {
01339 return ++_idCount;
01340 }
01341
01342 int Memory::incrementMapId(std::map<int, int> * reducedIds)
01343 {
01344
01345 const Signature * s = getLastWorkingSignature();
01346 if(s && s->mapId() == _idMapCount)
01347 {
01348
01349 while(_stMem.size())
01350 {
01351 int reducedId = 0;
01352 int id = *_stMem.begin();
01353 moveSignatureToWMFromSTM(id, &reducedId);
01354 if(reducedIds && reducedId > 0)
01355 {
01356 reducedIds->insert(std::make_pair(id, reducedId));
01357 }
01358 }
01359
01360 return ++_idMapCount;
01361 }
01362 return _idMapCount;
01363 }
01364
01365 void Memory::updateAge(int signatureId)
01366 {
01367 std::map<int, double>::iterator iter=_workingMem.find(signatureId);
01368 if(iter!=_workingMem.end())
01369 {
01370 iter->second = UTimer::now();
01371 }
01372 }
01373
01374 int Memory::getDatabaseMemoryUsed() const
01375 {
01376 int memoryUsed = 0;
01377 if(_dbDriver)
01378 {
01379 memoryUsed = _dbDriver->getMemoryUsed()/(1024*1024);
01380 }
01381
01382 return memoryUsed;
01383 }
01384
01385 std::string Memory::getDatabaseVersion() const
01386 {
01387 std::string version = "0.0.0";
01388 if(_dbDriver)
01389 {
01390 version = _dbDriver->getDatabaseVersion();
01391 }
01392 return version;
01393 }
01394
01395 std::string Memory::getDatabaseUrl() const
01396 {
01397 std::string url = "";
01398 if(_dbDriver)
01399 {
01400 url = _dbDriver->getUrl();
01401 }
01402 return url;
01403 }
01404
01405 double Memory::getDbSavingTime() const
01406 {
01407 return _dbDriver?_dbDriver->getEmptyTrashesTime():0;
01408 }
01409
01410 std::set<int> Memory::getAllSignatureIds() const
01411 {
01412 std::set<int> ids;
01413 if(_dbDriver)
01414 {
01415 _dbDriver->getAllNodeIds(ids);
01416 }
01417 for(std::map<int, Signature*>::const_iterator iter = _signatures.begin(); iter!=_signatures.end(); ++iter)
01418 {
01419 ids.insert(iter->first);
01420 }
01421 return ids;
01422 }
01423
01424 void Memory::clear()
01425 {
01426 UDEBUG("");
01427
01428
01429 while(_stMem.size())
01430 {
01431 moveSignatureToWMFromSTM(*_stMem.begin());
01432 }
01433 if(_stMem.size() != 0)
01434 {
01435 ULOGGER_ERROR("_stMem must be empty here, size=%d", _stMem.size());
01436 }
01437 _stMem.clear();
01438
01439 this->cleanUnusedWords();
01440
01441 if(_dbDriver)
01442 {
01443 _dbDriver->emptyTrashes();
01444 _dbDriver->join();
01445 }
01446 if(_dbDriver)
01447 {
01448
01449
01450 uSleep(1500);
01451 }
01452
01453
01454 if(_dbDriver && (_stMem.size() || _workingMem.size()))
01455 {
01456 unsigned int memSize = (unsigned int)(_workingMem.size() + _stMem.size());
01457 if(_workingMem.size() && _workingMem.begin()->first < 0)
01458 {
01459 --memSize;
01460 }
01461
01462
01463 UASSERT_MSG(memSize == _signatures.size(),
01464 uFormat("The number of signatures don't match! _workingMem=%d, _stMem=%d, _signatures=%d",
01465 _workingMem.size(), _stMem.size(), _signatures.size()).c_str());
01466
01467 UDEBUG("Adding statistics after run...");
01468 if(_memoryChanged)
01469 {
01470 ParametersMap parameters = Parameters::getDefaultParameters();
01471 uInsert(parameters, parameters_);
01472 parameters.erase(Parameters::kRtabmapWorkingDirectory());
01473 UDEBUG("");
01474 _dbDriver->addInfoAfterRun(memSize,
01475 _lastSignature?_lastSignature->id():0,
01476 UProcessInfo::getMemoryUsage(),
01477 _dbDriver->getMemoryUsed(),
01478 (int)_vwd->getVisualWords().size(),
01479 parameters);
01480 }
01481 }
01482 UDEBUG("");
01483
01484
01485 std::map<int, Signature*> mem = _signatures;
01486 for(std::map<int, Signature *>::iterator i=mem.begin(); i!=mem.end(); ++i)
01487 {
01488 if(i->second)
01489 {
01490 UDEBUG("deleting from the working and the short-term memory: %d", i->first);
01491 this->moveToTrash(i->second);
01492 }
01493 }
01494
01495 if(_workingMem.size() != 0 && !(_workingMem.size() == 1 && _workingMem.begin()->first == kIdVirtual))
01496 {
01497 ULOGGER_ERROR("_workingMem must be empty here, size=%d", _workingMem.size());
01498 }
01499 _workingMem.clear();
01500 if(_signatures.size()!=0)
01501 {
01502 ULOGGER_ERROR("_signatures must be empty here, size=%d", _signatures.size());
01503 }
01504 _signatures.clear();
01505
01506 UDEBUG("");
01507
01508 if(_dbDriver)
01509 {
01510 _dbDriver->emptyTrashes();
01511 }
01512 UDEBUG("");
01513 _lastSignature = 0;
01514 _lastGlobalLoopClosureId = 0;
01515 _idCount = kIdStart;
01516 _idMapCount = kIdStart;
01517 _memoryChanged = false;
01518 _linksChanged = false;
01519 _gpsOrigin = GPS();
01520 _rectCameraModels.clear();
01521 _rectStereoCameraModel = StereoCameraModel();
01522
01523 if(_dbDriver)
01524 {
01525 _dbDriver->join(true);
01526 cleanUnusedWords();
01527 _dbDriver->emptyTrashes();
01528 }
01529 else
01530 {
01531 cleanUnusedWords();
01532 }
01533 if(_vwd)
01534 {
01535 _vwd->clear();
01536 }
01537 UDEBUG("");
01538 }
01539
01545 std::map<int, float> Memory::computeLikelihood(const Signature * signature, const std::list<int> & ids)
01546 {
01547 if(!_tfIdfLikelihoodUsed)
01548 {
01549 UTimer timer;
01550 timer.start();
01551 std::map<int, float> likelihood;
01552
01553 if(!signature)
01554 {
01555 ULOGGER_ERROR("The signature is null");
01556 return likelihood;
01557 }
01558 else if(ids.empty())
01559 {
01560 UWARN("ids list is empty");
01561 return likelihood;
01562 }
01563
01564 for(std::list<int>::const_iterator iter = ids.begin(); iter!=ids.end(); ++iter)
01565 {
01566 float sim = 0.0f;
01567 if(*iter > 0)
01568 {
01569 const Signature * sB = this->getSignature(*iter);
01570 if(!sB)
01571 {
01572 UFATAL("Signature %d not found in WM ?!?", *iter);
01573 }
01574 sim = signature->compareTo(*sB);
01575 }
01576
01577 likelihood.insert(likelihood.end(), std::pair<int, float>(*iter, sim));
01578 }
01579
01580 UDEBUG("compute likelihood (similarity)... %f s", timer.ticks());
01581 return likelihood;
01582 }
01583 else
01584 {
01585 UTimer timer;
01586 timer.start();
01587 std::map<int, float> likelihood;
01588 std::map<int, float> calculatedWordsRatio;
01589
01590 if(!signature)
01591 {
01592 ULOGGER_ERROR("The signature is null");
01593 return likelihood;
01594 }
01595 else if(ids.empty())
01596 {
01597 UWARN("ids list is empty");
01598 return likelihood;
01599 }
01600
01601 for(std::list<int>::const_iterator iter = ids.begin(); iter!=ids.end(); ++iter)
01602 {
01603 likelihood.insert(likelihood.end(), std::pair<int, float>(*iter, 0.0f));
01604 }
01605
01606 const std::list<int> & wordIds = uUniqueKeys(signature->getWords());
01607
01608 float nwi;
01609 float ni;
01610 float nw;
01611 float N;
01612
01613 float logNnw;
01614 const VisualWord * vw;
01615
01616 N = this->getSignatures().size();
01617
01618 if(N)
01619 {
01620 UDEBUG("processing... ");
01621
01622 for(std::list<int>::const_iterator i=wordIds.begin(); i!=wordIds.end(); ++i)
01623 {
01624 if(*i>0)
01625 {
01626
01627 vw = _vwd->getWord(*i);
01628 UASSERT_MSG(vw!=0, uFormat("Word %d not found in dictionary!?", *i).c_str());
01629
01630 const std::map<int, int> & refs = vw->getReferences();
01631 nw = refs.size();
01632 if(nw)
01633 {
01634 logNnw = log10(N/nw);
01635 if(logNnw)
01636 {
01637 for(std::map<int, int>::const_iterator j=refs.begin(); j!=refs.end(); ++j)
01638 {
01639 std::map<int, float>::iterator iter = likelihood.find(j->first);
01640 if(iter != likelihood.end())
01641 {
01642 nwi = j->second;
01643 ni = this->getNi(j->first);
01644 if(ni != 0)
01645 {
01646
01647 iter->second += ( nwi * logNnw ) / ni;
01648 }
01649 }
01650 }
01651 }
01652 }
01653 }
01654 }
01655 }
01656
01657 UDEBUG("compute likelihood (tf-idf) %f s", timer.ticks());
01658 return likelihood;
01659 }
01660 }
01661
01662
01663 std::map<int, int> Memory::getWeights() const
01664 {
01665 std::map<int, int> weights;
01666 for(std::map<int, double>::const_iterator iter=_workingMem.begin(); iter!=_workingMem.end(); ++iter)
01667 {
01668 if(iter->first > 0)
01669 {
01670 const Signature * s = this->getSignature(iter->first);
01671 if(!s)
01672 {
01673 UFATAL("Location %d must exist in memory", iter->first);
01674 }
01675 weights.insert(weights.end(), std::make_pair(iter->first, s->getWeight()));
01676 }
01677 else
01678 {
01679 weights.insert(weights.end(), std::make_pair(iter->first, -1));
01680 }
01681 }
01682 return weights;
01683 }
01684
01685 std::list<int> Memory::forget(const std::set<int> & ignoredIds)
01686 {
01687 UDEBUG("");
01688 std::list<int> signaturesRemoved;
01689 if(this->isIncremental() &&
01690 _vwd->isIncremental() &&
01691 _vwd->getVisualWords().size() &&
01692 !_vwd->isIncrementalFlann())
01693 {
01694
01695
01696
01697
01698 int newWords = 0;
01699 int wordsRemoved = 0;
01700
01701
01702 newWords = _vwd->getNotIndexedWordsCount();
01703
01704
01705
01706 while(wordsRemoved < newWords)
01707 {
01708 std::list<Signature *> signatures = this->getRemovableSignatures(1, ignoredIds);
01709 if(signatures.size())
01710 {
01711 Signature * s = dynamic_cast<Signature *>(signatures.front());
01712 if(s)
01713 {
01714 signaturesRemoved.push_back(s->id());
01715 this->moveToTrash(s);
01716 wordsRemoved = _vwd->getUnusedWordsSize();
01717 }
01718 else
01719 {
01720 break;
01721 }
01722 }
01723 else
01724 {
01725 break;
01726 }
01727 }
01728 UDEBUG("newWords=%d, wordsRemoved=%d", newWords, wordsRemoved);
01729 }
01730 else
01731 {
01732 UDEBUG("");
01733
01734 int signaturesAdded = _signaturesAdded;
01735 std::list<Signature *> signatures = getRemovableSignatures(signaturesAdded+1, ignoredIds);
01736 for(std::list<Signature *>::iterator iter=signatures.begin(); iter!=signatures.end(); ++iter)
01737 {
01738 signaturesRemoved.push_back((*iter)->id());
01739
01740
01741 this->moveToTrash(*iter);
01742 }
01743 if((int)signatures.size() < signaturesAdded)
01744 {
01745 UWARN("Less signatures transferred (%d) than added (%d)! The working memory cannot decrease in size.",
01746 (int)signatures.size(), signaturesAdded);
01747 }
01748 else
01749 {
01750 UDEBUG("signaturesRemoved=%d, _signaturesAdded=%d", (int)signatures.size(), signaturesAdded);
01751 }
01752 }
01753 return signaturesRemoved;
01754 }
01755
01756
01757 int Memory::cleanup()
01758 {
01759 UDEBUG("");
01760 int signatureRemoved = 0;
01761
01762
01763 if(_lastSignature && ((_lastSignature->isBadSignature() && _badSignaturesIgnored) || !_incrementalMemory))
01764 {
01765 if(_lastSignature->isBadSignature())
01766 {
01767 UDEBUG("Bad signature! %d", _lastSignature->id());
01768 }
01769 signatureRemoved = _lastSignature->id();
01770 moveToTrash(_lastSignature, _incrementalMemory);
01771 }
01772
01773 return signatureRemoved;
01774 }
01775
01776 void Memory::saveStatistics(const Statistics & statistics)
01777 {
01778 if(_dbDriver)
01779 {
01780 _dbDriver->addStatistics(statistics);
01781 }
01782 }
01783
01784 void Memory::savePreviewImage(const cv::Mat & image) const
01785 {
01786 if(_dbDriver)
01787 {
01788 _dbDriver->savePreviewImage(image);
01789 }
01790 }
01791 cv::Mat Memory::loadPreviewImage() const
01792 {
01793 if(_dbDriver)
01794 {
01795 return _dbDriver->loadPreviewImage();
01796 }
01797 return cv::Mat();
01798 }
01799
01800 void Memory::saveOptimizedPoses(const std::map<int, Transform> & optimizedPoses, const Transform & lastlocalizationPose) const
01801 {
01802 if(_dbDriver)
01803 {
01804 _dbDriver->saveOptimizedPoses(optimizedPoses, lastlocalizationPose);
01805 }
01806 }
01807
01808 std::map<int, Transform> Memory::loadOptimizedPoses(Transform * lastlocalizationPose) const
01809 {
01810 if(_dbDriver)
01811 {
01812 return _dbDriver->loadOptimizedPoses(lastlocalizationPose);
01813 }
01814 return std::map<int, Transform>();
01815 }
01816
01817 void Memory::save2DMap(const cv::Mat & map, float xMin, float yMin, float cellSize) const
01818 {
01819 if(_dbDriver)
01820 {
01821 _dbDriver->save2DMap(map, xMin, yMin, cellSize);
01822 }
01823 }
01824
01825 cv::Mat Memory::load2DMap(float & xMin, float & yMin, float & cellSize) const
01826 {
01827 if(_dbDriver)
01828 {
01829 return _dbDriver->load2DMap(xMin, yMin, cellSize);
01830 }
01831 return cv::Mat();
01832 }
01833
01834 void Memory::saveOptimizedMesh(
01835 const cv::Mat & cloud,
01836 const std::vector<std::vector<std::vector<unsigned int> > > & polygons,
01837 #if PCL_VERSION_COMPARE(>=, 1, 8, 0)
01838 const std::vector<std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > > & texCoords,
01839 #else
01840 const std::vector<std::vector<Eigen::Vector2f> > & texCoords,
01841 #endif
01842 const cv::Mat & textures) const
01843 {
01844 if(_dbDriver)
01845 {
01846 _dbDriver->saveOptimizedMesh(cloud, polygons, texCoords, textures);
01847 }
01848 }
01849
01850 cv::Mat Memory::loadOptimizedMesh(
01851 std::vector<std::vector<std::vector<unsigned int> > > * polygons,
01852 #if PCL_VERSION_COMPARE(>=, 1, 8, 0)
01853 std::vector<std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > > * texCoords,
01854 #else
01855 std::vector<std::vector<Eigen::Vector2f> > * texCoords,
01856 #endif
01857 cv::Mat * textures) const
01858 {
01859 if(_dbDriver)
01860 {
01861 return _dbDriver->loadOptimizedMesh(polygons, texCoords, textures);
01862 }
01863 return cv::Mat();
01864 }
01865
01866 void Memory::emptyTrash()
01867 {
01868 if(_dbDriver)
01869 {
01870 _dbDriver->emptyTrashes(true);
01871 }
01872 }
01873
01874 void Memory::joinTrashThread()
01875 {
01876 if(_dbDriver)
01877 {
01878 UDEBUG("");
01879 _dbDriver->join();
01880 UDEBUG("");
01881 }
01882 }
01883
01884 class WeightAgeIdKey
01885 {
01886 public:
01887 WeightAgeIdKey(int w, double a, int i) :
01888 weight(w),
01889 age(a),
01890 id(i){}
01891 bool operator<(const WeightAgeIdKey & k) const
01892 {
01893 if(weight < k.weight)
01894 {
01895 return true;
01896 }
01897 else if(weight == k.weight)
01898 {
01899 if(age < k.age)
01900 {
01901 return true;
01902 }
01903 else if(age == k.age)
01904 {
01905 if(id < k.id)
01906 {
01907 return true;
01908 }
01909 }
01910 }
01911 return false;
01912 }
01913 int weight, age, id;
01914 };
01915 std::list<Signature *> Memory::getRemovableSignatures(int count, const std::set<int> & ignoredIds)
01916 {
01917
01918 std::list<Signature *> removableSignatures;
01919 std::map<WeightAgeIdKey, Signature *> weightAgeIdMap;
01920
01921
01922 UDEBUG("mem.size()=%d, ignoredIds.size()=%d", (int)_workingMem.size(), (int)ignoredIds.size());
01923
01924 if(_workingMem.size())
01925 {
01926 int recentWmMaxSize = _recentWmRatio * float(_workingMem.size());
01927 bool recentWmImmunized = false;
01928
01929 int currentRecentWmSize = 0;
01930 if(_lastGlobalLoopClosureId > 0 && _stMem.find(_lastGlobalLoopClosureId) == _stMem.end())
01931 {
01932
01933 std::map<int, double>::const_iterator iter = _workingMem.find(_lastGlobalLoopClosureId);
01934 while(iter != _workingMem.end())
01935 {
01936 ++currentRecentWmSize;
01937 ++iter;
01938 }
01939 if(currentRecentWmSize>1 && currentRecentWmSize < recentWmMaxSize)
01940 {
01941 recentWmImmunized = true;
01942 }
01943 else if(currentRecentWmSize == 0 && _workingMem.size() > 1)
01944 {
01945 UERROR("Last loop closure id not found in WM (%d)", _lastGlobalLoopClosureId);
01946 }
01947 UDEBUG("currentRecentWmSize=%d, recentWmMaxSize=%d, _recentWmRatio=%f, end recent wM = %d", currentRecentWmSize, recentWmMaxSize, _recentWmRatio, _lastGlobalLoopClosureId);
01948 }
01949
01950
01951 Signature * lastInSTM = 0;
01952 if(_stMem.size())
01953 {
01954 lastInSTM = _signatures.at(*_stMem.begin());
01955 }
01956
01957 for(std::map<int, double>::const_iterator memIter = _workingMem.begin(); memIter != _workingMem.end(); ++memIter)
01958 {
01959 if( (recentWmImmunized && memIter->first > _lastGlobalLoopClosureId) ||
01960 memIter->first == _lastGlobalLoopClosureId)
01961 {
01962
01963 }
01964 else if(memIter->first > 0 && ignoredIds.find(memIter->first) == ignoredIds.end() && (!lastInSTM || !lastInSTM->hasLink(memIter->first)))
01965 {
01966 Signature * s = this->_getSignature(memIter->first);
01967 if(s)
01968 {
01969
01970 bool foundInSTM = false;
01971 for(std::map<int, Link>::const_iterator iter = s->getLinks().begin(); iter!=s->getLinks().end(); ++iter)
01972 {
01973 if(_stMem.find(iter->first) != _stMem.end())
01974 {
01975 UDEBUG("Ignored %d because it has a link (%d) to STM", s->id(), iter->first);
01976 foundInSTM = true;
01977 break;
01978 }
01979 }
01980 if(!foundInSTM)
01981 {
01982
01983 weightAgeIdMap.insert(std::make_pair(WeightAgeIdKey(s->getWeight(), _transferSortingByWeightId?0.0:memIter->second, s->id()), s));
01984 }
01985 }
01986 else
01987 {
01988 ULOGGER_ERROR("Not supposed to occur!!!");
01989 }
01990 }
01991 else
01992 {
01993
01994 }
01995 }
01996
01997 int recentWmCount = 0;
01998
01999
02000 UDEBUG("signatureMap.size()=%d _lastGlobalLoopClosureId=%d currentRecentWmSize=%d recentWmMaxSize=%d",
02001 (int)weightAgeIdMap.size(), _lastGlobalLoopClosureId, currentRecentWmSize, recentWmMaxSize);
02002 for(std::map<WeightAgeIdKey, Signature*>::iterator iter=weightAgeIdMap.begin();
02003 iter!=weightAgeIdMap.end();
02004 ++iter)
02005 {
02006 if(!recentWmImmunized)
02007 {
02008 UDEBUG("weight=%d, id=%d",
02009 iter->second->getWeight(),
02010 iter->second->id());
02011 removableSignatures.push_back(iter->second);
02012
02013 if(_lastGlobalLoopClosureId && iter->second->id() > _lastGlobalLoopClosureId)
02014 {
02015 ++recentWmCount;
02016 if(currentRecentWmSize - recentWmCount < recentWmMaxSize)
02017 {
02018 UDEBUG("switched recentWmImmunized");
02019 recentWmImmunized = true;
02020 }
02021 }
02022 }
02023 else if(_lastGlobalLoopClosureId == 0 || iter->second->id() < _lastGlobalLoopClosureId)
02024 {
02025 UDEBUG("weight=%d, id=%d",
02026 iter->second->getWeight(),
02027 iter->second->id());
02028 removableSignatures.push_back(iter->second);
02029 }
02030 if(removableSignatures.size() >= (unsigned int)count)
02031 {
02032 break;
02033 }
02034 }
02035 }
02036 else
02037 {
02038 ULOGGER_WARN("not enough signatures to get an old one...");
02039 }
02040 return removableSignatures;
02041 }
02042
02046 void Memory::moveToTrash(Signature * s, bool keepLinkedToGraph, std::list<int> * deletedWords)
02047 {
02048 UDEBUG("id=%d", s?s->id():0);
02049 if(s)
02050 {
02051
02052 if(!keepLinkedToGraph || (!s->isSaved() && s->isBadSignature() && _badSignaturesIgnored))
02053 {
02054 UASSERT_MSG(this->isInSTM(s->id()),
02055 uFormat("Deleting location (%d) outside the "
02056 "STM is not implemented!", s->id()).c_str());
02057 const std::map<int, Link> & links = s->getLinks();
02058 for(std::map<int, Link>::const_iterator iter=links.begin(); iter!=links.end(); ++iter)
02059 {
02060 if(iter->second.from() != iter->second.to())
02061 {
02062 Signature * sTo = this->_getSignature(iter->first);
02063
02064 UASSERT_MSG(sTo!=0,
02065 uFormat("A neighbor (%d) of the deleted location %d is "
02066 "not found in WM/STM! Are you deleting a location "
02067 "outside the STM?", iter->first, s->id()).c_str());
02068
02069 if(iter->first > s->id() && links.size()>1 && sTo->hasLink(s->id()))
02070 {
02071 UWARN("Link %d of %d is newer, removing neighbor link "
02072 "may split the map!",
02073 iter->first, s->id());
02074 }
02075
02076
02077 if(iter->second.type() == Link::kGlobalClosure && s->id() > sTo->id())
02078 {
02079 sTo->setWeight(sTo->getWeight() + s->getWeight());
02080 }
02081
02082 sTo->removeLink(s->id());
02083 }
02084
02085 }
02086 s->removeLinks();
02087 s->setWeight(0);
02088 s->setLabel("");
02089 }
02090 else
02091 {
02092
02093
02094
02095 removeVirtualLinks(s->id());
02096 }
02097
02098 this->disableWordsRef(s->id());
02099 if(!keepLinkedToGraph && _vwd->isIncremental())
02100 {
02101 std::list<int> keys = uUniqueKeys(s->getWords());
02102 for(std::list<int>::const_iterator i=keys.begin(); i!=keys.end(); ++i)
02103 {
02104
02105 VisualWord * w = _vwd->getUnusedWord(*i);
02106 if(w)
02107 {
02108 std::vector<VisualWord*> wordToDelete;
02109 wordToDelete.push_back(w);
02110 _vwd->removeWords(wordToDelete);
02111 if(deletedWords)
02112 {
02113 deletedWords->push_back(w->id());
02114 }
02115 delete w;
02116 }
02117 }
02118 }
02119
02120 _workingMem.erase(s->id());
02121 _stMem.erase(s->id());
02122 _signatures.erase(s->id());
02123 if(_signaturesAdded>0)
02124 {
02125 --_signaturesAdded;
02126 }
02127
02128 if(_lastSignature == s)
02129 {
02130 _lastSignature = 0;
02131 if(_stMem.size())
02132 {
02133 _lastSignature = this->_getSignature(*_stMem.rbegin());
02134 }
02135 else if(_workingMem.size())
02136 {
02137 _lastSignature = this->_getSignature(_workingMem.rbegin()->first);
02138 }
02139 }
02140
02141 if(_lastGlobalLoopClosureId == s->id())
02142 {
02143 _lastGlobalLoopClosureId = 0;
02144 }
02145
02146 if( (_notLinkedNodesKeptInDb || keepLinkedToGraph || s->isSaved()) &&
02147 _dbDriver &&
02148 s->id()>0 &&
02149 (_incrementalMemory || s->isSaved()))
02150 {
02151 _dbDriver->asyncSave(s);
02152 }
02153 else
02154 {
02155 delete s;
02156 }
02157 }
02158 }
02159
02160 int Memory::getLastSignatureId() const
02161 {
02162 return _idCount;
02163 }
02164
02165 const Signature * Memory::getLastWorkingSignature() const
02166 {
02167 UDEBUG("");
02168 return _lastSignature;
02169 }
02170
02171 int Memory::getSignatureIdByLabel(const std::string & label, bool lookInDatabase) const
02172 {
02173 UDEBUG("label=%s", label.c_str());
02174 int id = 0;
02175 if(label.size())
02176 {
02177 for(std::map<int, Signature*>::const_iterator iter=_signatures.begin(); iter!=_signatures.end(); ++iter)
02178 {
02179 UASSERT(iter->second != 0);
02180 if(iter->second->getLabel().compare(label) == 0)
02181 {
02182 id = iter->second->id();
02183 break;
02184 }
02185 }
02186 if(id == 0 && _dbDriver && lookInDatabase)
02187 {
02188 _dbDriver->getNodeIdByLabel(label, id);
02189 }
02190 }
02191 return id;
02192 }
02193
02194 bool Memory::labelSignature(int id, const std::string & label)
02195 {
02196
02197 int idFound=getSignatureIdByLabel(label);
02198 if(idFound == 0 || idFound == id)
02199 {
02200 Signature * s = this->_getSignature(id);
02201 if(s)
02202 {
02203 s->setLabel(label);
02204 _linksChanged = s->isSaved();
02205 UWARN("Label \"%s\" set to node %d", label.c_str(), id);
02206 return true;
02207 }
02208 else if(_dbDriver)
02209 {
02210 std::list<int> ids;
02211 ids.push_back(id);
02212 std::list<Signature *> signatures;
02213 _dbDriver->loadSignatures(ids,signatures);
02214 if(signatures.size())
02215 {
02216 signatures.front()->setLabel(label);
02217 UWARN("Label \"%s\" set to node %d", label.c_str(), id);
02218 _dbDriver->asyncSave(signatures.front());
02219 return true;
02220 }
02221 }
02222 else
02223 {
02224 UERROR("Node %d not found, failed to set label \"%s\"!", id, label.c_str());
02225 }
02226 }
02227 else if(idFound)
02228 {
02229 UWARN("Node %d has already label \"%s\"", idFound, label.c_str());
02230 }
02231 return false;
02232 }
02233
02234 std::map<int, std::string> Memory::getAllLabels() const
02235 {
02236 std::map<int, std::string> labels;
02237 for(std::map<int, Signature*>::const_iterator iter = _signatures.begin(); iter!=_signatures.end(); ++iter)
02238 {
02239 if(!iter->second->getLabel().empty())
02240 {
02241 labels.insert(std::make_pair(iter->first, iter->second->getLabel()));
02242 }
02243 }
02244 if(_dbDriver)
02245 {
02246 _dbDriver->getAllLabels(labels);
02247 }
02248 return labels;
02249 }
02250
02251 bool Memory::setUserData(int id, const cv::Mat & data)
02252 {
02253 Signature * s = this->_getSignature(id);
02254 if(s)
02255 {
02256 s->sensorData().setUserData(data);
02257 return true;
02258 }
02259 else
02260 {
02261 UERROR("Node %d not found in RAM, failed to set user data (size=%d)!", id, data.total());
02262 }
02263 return false;
02264 }
02265
02266 void Memory::deleteLocation(int locationId, std::list<int> * deletedWords)
02267 {
02268 UDEBUG("Deleting location %d", locationId);
02269 Signature * location = _getSignature(locationId);
02270 if(location)
02271 {
02272 this->moveToTrash(location, false, deletedWords);
02273 }
02274 }
02275
02276 void Memory::saveLocationData(int locationId)
02277 {
02278 UDEBUG("Saving location data %d", locationId);
02279 Signature * location = _getSignature(locationId);
02280 if( location &&
02281 _dbDriver &&
02282 !_dbDriver->isInMemory() &&
02283 location->id()>0 &&
02284 (_incrementalMemory && !location->isSaved()))
02285 {
02286 Signature * cpy = new Signature();
02287 *cpy = *location;
02288 _dbDriver->asyncSave(cpy);
02289
02290 location->setSaved(true);
02291 location->sensorData().clearCompressedData();
02292 }
02293 }
02294
02295 void Memory::removeLink(int oldId, int newId)
02296 {
02297
02298 Signature * oldS = this->_getSignature(oldId<newId?oldId:newId);
02299 Signature * newS = this->_getSignature(oldId<newId?newId:oldId);
02300 if(oldS && newS)
02301 {
02302 UINFO("removing link between location %d and %d", oldS->id(), newS->id());
02303
02304 if(oldS->hasLink(newS->id()) && newS->hasLink(oldS->id()))
02305 {
02306 Link::Type type = oldS->getLinks().at(newS->id()).type();
02307 if(type == Link::kGlobalClosure && newS->getWeight() > 0)
02308 {
02309
02310 oldS->setWeight(oldS->getWeight()+1);
02311 newS->setWeight(newS->getWeight()>0?newS->getWeight()-1:0);
02312 }
02313
02314
02315 oldS->removeLink(newS->id());
02316 newS->removeLink(oldS->id());
02317
02318 if(type!=Link::kVirtualClosure)
02319 {
02320 _linksChanged = true;
02321 }
02322
02323 bool noChildrenAnymore = true;
02324 for(std::map<int, Link>::const_iterator iter=newS->getLinks().begin(); iter!=newS->getLinks().end(); ++iter)
02325 {
02326 if(iter->second.type() != Link::kNeighbor &&
02327 iter->second.type() != Link::kNeighborMerged &&
02328 iter->second.type() != Link::kPosePrior &&
02329 iter->first < newS->id())
02330 {
02331 noChildrenAnymore = false;
02332 break;
02333 }
02334 }
02335 if(noChildrenAnymore && newS->id() == _lastGlobalLoopClosureId)
02336 {
02337 _lastGlobalLoopClosureId = 0;
02338 }
02339 }
02340 else
02341 {
02342 UERROR("Signatures %d and %d don't have bidirectional link!", oldS->id(), newS->id());
02343 }
02344 }
02345 else
02346 {
02347 if(!newS)
02348 {
02349 UERROR("Signature %d is not in working memory... cannot remove link.", newS->id());
02350 }
02351 if(!oldS)
02352 {
02353 UERROR("Signature %d is not in working memory... cannot remove link.", oldS->id());
02354 }
02355 }
02356 }
02357
02358 void Memory::removeRawData(int id, bool image, bool scan, bool userData)
02359 {
02360 UDEBUG("id=%d image=%d scan=%d userData=%d", id, image?1:0, scan?1:0, userData?1:0);
02361 Signature * s = this->_getSignature(id);
02362 if(s)
02363 {
02364 if(image && (!_reextractLoopClosureFeatures || !_registrationPipeline->isImageRequired()))
02365 {
02366 s->sensorData().setImageRaw(cv::Mat());
02367 s->sensorData().setDepthOrRightRaw(cv::Mat());
02368 }
02369 if(scan && !_registrationPipeline->isScanRequired())
02370 {
02371 LaserScan scan = s->sensorData().laserScanRaw();
02372 scan.clear();
02373 s->sensorData().setLaserScanRaw(scan);
02374 }
02375 if(userData && !_registrationPipeline->isUserDataRequired())
02376 {
02377 s->sensorData().setUserDataRaw(cv::Mat());
02378 }
02379 }
02380 }
02381
02382
02383 Transform Memory::computeTransform(
02384 int fromId,
02385 int toId,
02386 Transform guess,
02387 RegistrationInfo * info,
02388 bool useKnownCorrespondencesIfPossible)
02389 {
02390 Signature * fromS = this->_getSignature(fromId);
02391 Signature * toS = this->_getSignature(toId);
02392
02393 Transform transform;
02394
02395 if(fromS && toS)
02396 {
02397 transform = computeTransform(*fromS, *toS, guess, info, useKnownCorrespondencesIfPossible);
02398 }
02399 else
02400 {
02401 std::string msg = uFormat("Did not find nodes %d and/or %d", fromId, toId);
02402 if(info)
02403 {
02404 info->rejectedMsg = msg;
02405 }
02406 UWARN(msg.c_str());
02407 }
02408 return transform;
02409 }
02410
02411
02412 Transform Memory::computeTransform(
02413 Signature & fromS,
02414 Signature & toS,
02415 Transform guess,
02416 RegistrationInfo * info,
02417 bool useKnownCorrespondencesIfPossible) const
02418 {
02419 UDEBUG("");
02420 Transform transform;
02421
02422
02423
02424 if((((_reextractLoopClosureFeatures || _visCorType==1) && _registrationPipeline->isImageRequired()) && fromS.sensorData().imageCompressed().empty()) ||
02425 (_registrationPipeline->isScanRequired() && fromS.sensorData().imageCompressed().empty() && fromS.sensorData().laserScanCompressed().isEmpty()) ||
02426 (_registrationPipeline->isUserDataRequired() && fromS.sensorData().imageCompressed().empty() && fromS.sensorData().userDataCompressed().empty()))
02427 {
02428 fromS.sensorData() = getNodeData(fromS.id());
02429 }
02430 if((((_reextractLoopClosureFeatures || _visCorType==1) && _registrationPipeline->isImageRequired()) && toS.sensorData().imageCompressed().empty()) ||
02431 (_registrationPipeline->isScanRequired() && toS.sensorData().imageCompressed().empty() && toS.sensorData().laserScanCompressed().isEmpty()) ||
02432 (_registrationPipeline->isUserDataRequired() && toS.sensorData().imageCompressed().empty() && toS.sensorData().userDataCompressed().empty()))
02433 {
02434 toS.sensorData() = getNodeData(toS.id());
02435 }
02436
02437 cv::Mat imgBuf, depthBuf, userBuf;
02438 LaserScan laserBuf;
02439 fromS.sensorData().uncompressData(
02440 ((_reextractLoopClosureFeatures || _visCorType==1) && _registrationPipeline->isImageRequired())?&imgBuf:0,
02441 ((_reextractLoopClosureFeatures || _visCorType==1) && _registrationPipeline->isImageRequired())?&depthBuf:0,
02442 _registrationPipeline->isScanRequired()?&laserBuf:0,
02443 _registrationPipeline->isUserDataRequired()?&userBuf:0);
02444 toS.sensorData().uncompressData(
02445 ((_reextractLoopClosureFeatures || _visCorType==1) && _registrationPipeline->isImageRequired())?&imgBuf:0,
02446 ((_reextractLoopClosureFeatures || _visCorType==1) && _registrationPipeline->isImageRequired())?&depthBuf:0,
02447 _registrationPipeline->isScanRequired()?&laserBuf:0,
02448 _registrationPipeline->isUserDataRequired()?&userBuf:0);
02449
02450
02451
02452 std::vector<int> inliersV;
02453 if((_reextractLoopClosureFeatures && _registrationPipeline->isImageRequired()) ||
02454 (fromS.getWords().size() && toS.getWords().size()) ||
02455 (!guess.isNull() && !_registrationPipeline->isImageRequired()))
02456 {
02457 Signature tmpFrom = fromS;
02458 Signature tmpTo = toS;
02459
02460 if(_reextractLoopClosureFeatures && _registrationPipeline->isImageRequired())
02461 {
02462 UDEBUG("");
02463 tmpFrom.setWords(std::multimap<int, cv::KeyPoint>());
02464 tmpFrom.setWords3(std::multimap<int, cv::Point3f>());
02465 tmpFrom.setWordsDescriptors(std::multimap<int, cv::Mat>());
02466 tmpFrom.sensorData().setFeatures(std::vector<cv::KeyPoint>(), std::vector<cv::Point3f>(), cv::Mat());
02467 tmpTo.setWords(std::multimap<int, cv::KeyPoint>());
02468 tmpTo.setWords3(std::multimap<int, cv::Point3f>());
02469 tmpTo.setWordsDescriptors(std::multimap<int, cv::Mat>());
02470 tmpTo.sensorData().setFeatures(std::vector<cv::KeyPoint>(), std::vector<cv::Point3f>(), cv::Mat());
02471 }
02472 else if(useKnownCorrespondencesIfPossible)
02473 {
02474
02475 tmpFrom.setWordsDescriptors(std::multimap<int, cv::Mat>());
02476 tmpTo.setWordsDescriptors(std::multimap<int, cv::Mat>());
02477 }
02478
02479 if(guess.isNull() && (!_registrationPipeline->isImageRequired() || _visCorType==1))
02480 {
02481 UDEBUG("");
02482
02483
02484 ParametersMap parameters = parameters_;
02485 uInsert(parameters, ParametersPair(Parameters::kVisCorType(), "0"));
02486 uInsert(parameters, ParametersPair(Parameters::kRegRepeatOnce(), "false"));
02487 RegistrationVis regVis(parameters);
02488 guess = regVis.computeTransformation(tmpFrom, tmpTo, guess, info);
02489 if(!guess.isNull())
02490 {
02491 transform = _registrationPipeline->computeTransformationMod(tmpFrom, tmpTo, guess, info);
02492 }
02493 }
02494 else if(_localBundleOnLoopClosure &&
02495 _registrationPipeline->isImageRequired() &&
02496 !_registrationPipeline->isScanRequired() &&
02497 !_registrationPipeline->isUserDataRequired() &&
02498 !tmpTo.getWordsDescriptors().empty() &&
02499 !tmpTo.getWords().empty() &&
02500 !tmpFrom.getWordsDescriptors().empty() &&
02501 !tmpFrom.getWords().empty() &&
02502 !tmpFrom.getWords3().empty())
02503 {
02504 std::multimap<int, cv::Point3f> words3DMap;
02505 std::multimap<int, cv::KeyPoint> wordsMap;
02506 std::multimap<int, cv::Mat> wordsDescriptorsMap;
02507
02508 const std::map<int, Link> & links = fromS.getLinks();
02509 {
02510 const std::map<int, cv::Point3f> & words3 = uMultimapToMapUnique(fromS.getWords3());
02511 UDEBUG("fromS.getWords3()=%d uniques=%d", (int)fromS.getWords3().size(), (int)words3.size());
02512 for(std::map<int, cv::Point3f>::const_iterator jter=words3.begin(); jter!=words3.end(); ++jter)
02513 {
02514 if(util3d::isFinite(jter->second))
02515 {
02516 words3DMap.insert(*jter);
02517 wordsMap.insert(*fromS.getWords().find(jter->first));
02518 wordsDescriptorsMap.insert(*fromS.getWordsDescriptors().find(jter->first));
02519 }
02520 }
02521 }
02522 UDEBUG("words3DMap=%d", (int)words3DMap.size());
02523
02524 for(std::map<int, Link>::const_iterator iter=links.begin(); iter!=links.end(); ++iter)
02525 {
02526 int id = iter->first;
02527 const Signature * s = this->getSignature(id);
02528 const std::map<int, cv::Point3f> & words3 = uMultimapToMapUnique(s->getWords3());
02529 for(std::map<int, cv::Point3f>::const_iterator jter=words3.begin(); jter!=words3.end(); ++jter)
02530 {
02531 if( jter->first > 0 &&
02532 util3d::isFinite(jter->second) &&
02533 words3DMap.find(jter->first) == words3DMap.end())
02534 {
02535 words3DMap.insert(std::make_pair(jter->first, util3d::transformPoint(jter->second, iter->second.transform())));
02536 wordsMap.insert(*s->getWords().find(jter->first));
02537 wordsDescriptorsMap.insert(*s->getWordsDescriptors().find(jter->first));
02538 }
02539 }
02540 }
02541 UDEBUG("words3DMap=%d", (int)words3DMap.size());
02542 Signature tmpFrom2(fromS.id());
02543 tmpFrom2.setWords3(words3DMap);
02544 tmpFrom2.setWords(wordsMap);
02545 tmpFrom2.setWordsDescriptors(wordsDescriptorsMap);
02546
02547 transform = _registrationPipeline->computeTransformationMod(tmpFrom2, tmpTo, guess, info);
02548
02549 if(!transform.isNull() && info)
02550 {
02551 std::map<int, cv::Point3f> points3DMap = uMultimapToMapUnique(tmpFrom2.getWords3());
02552 std::map<int, Transform> bundlePoses;
02553 std::multimap<int, Link> bundleLinks;
02554 std::map<int, CameraModel> bundleModels;
02555 std::map<int, std::map<int, cv::Point3f> > wordReferences;
02556
02557 std::map<int, Link> links = fromS.getLinks();
02558 links.insert(std::make_pair(toS.id(), Link(fromS.id(), toS.id(), Link::kGlobalClosure, transform, info->covariance.inv())));
02559 links.insert(std::make_pair(fromS.id(), Link()));
02560
02561 for(std::map<int, Link>::iterator iter=links.begin(); iter!=links.end(); ++iter)
02562 {
02563 int id = iter->first;
02564 const Signature * s;
02565 if(id == tmpTo.id())
02566 {
02567 s = &tmpTo;
02568 }
02569 else
02570 {
02571 s = this->getSignature(id);
02572 }
02573 CameraModel model;
02574 if(s->sensorData().cameraModels().size() == 1 && s->sensorData().cameraModels().at(0).isValidForProjection())
02575 {
02576 model = s->sensorData().cameraModels()[0];
02577 }
02578 else if(s->sensorData().stereoCameraModel().isValidForProjection())
02579 {
02580 model = s->sensorData().stereoCameraModel().left();
02581
02582 model = CameraModel(model.fx(),
02583 model.fy(),
02584 model.cx(),
02585 model.cy(),
02586 model.localTransform(),
02587 -s->sensorData().stereoCameraModel().baseline()*model.fx());
02588 }
02589 else
02590 {
02591 UFATAL("no valid camera model to use local bundle adjustment on loop closure!");
02592 }
02593 bundleModels.insert(std::make_pair(id, model));
02594 Transform invLocalTransform = model.localTransform().inverse();
02595 if(iter->second.isValid())
02596 {
02597 bundleLinks.insert(std::make_pair(iter->second.from(), iter->second));
02598 bundlePoses.insert(std::make_pair(id, iter->second.transform()));
02599 }
02600 else
02601 {
02602 bundlePoses.insert(std::make_pair(id, Transform::getIdentity()));
02603 }
02604 const std::map<int,cv::KeyPoint> & words = uMultimapToMapUnique(s->getWords());
02605 for(std::map<int, cv::KeyPoint>::const_iterator jter=words.begin(); jter!=words.end(); ++jter)
02606 {
02607 if(points3DMap.find(jter->first)!=points3DMap.end() &&
02608 (id == tmpTo.id() || jter->first > 0))
02609 {
02610 std::multimap<int, cv::Point3f>::const_iterator kter = s->getWords3().find(jter->first);
02611 cv::Point3f pt3d = util3d::transformPoint(kter->second, invLocalTransform);
02612 wordReferences.insert(std::make_pair(jter->first, std::map<int, cv::Point3f>()));
02613 wordReferences.at(jter->first).insert(std::make_pair(id, cv::Point3f(jter->second.pt.x, jter->second.pt.y, pt3d.z)));
02614 }
02615 }
02616 }
02617
02618 UDEBUG("sba...start");
02619
02620 std::set<int> sbaOutliers;
02621 UTimer bundleTimer;
02622 OptimizerG2O sba;
02623 UTimer bundleTime;
02624 bundlePoses = sba.optimizeBA(-toS.id(), bundlePoses, bundleLinks, bundleModels, points3DMap, wordReferences, &sbaOutliers);
02625 UDEBUG("sba...end");
02626
02627 UDEBUG("bundleTime=%fs (poses=%d wordRef=%d outliers=%d)", bundleTime.ticks(), (int)bundlePoses.size(), (int)wordReferences.size(), (int)sbaOutliers.size());
02628
02629 UDEBUG("Local Bundle Adjustment Before: %s", transform.prettyPrint().c_str());
02630 if(!bundlePoses.rbegin()->second.isNull())
02631 {
02632 if(sbaOutliers.size())
02633 {
02634 std::vector<int> newInliers(info->inliersIDs.size());
02635 int oi=0;
02636 for(unsigned int i=0; i<info->inliersIDs.size(); ++i)
02637 {
02638 if(sbaOutliers.find(info->inliersIDs[i]) == sbaOutliers.end())
02639 {
02640 newInliers[oi++] = info->inliersIDs[i];
02641 }
02642 }
02643 newInliers.resize(oi);
02644 UDEBUG("BA outliers ratio %f", float(sbaOutliers.size())/float(info->inliersIDs.size()));
02645 info->inliers = (int)newInliers.size();
02646 info->inliersIDs = newInliers;
02647 }
02648 if(info->inliers < _registrationPipeline->getMinVisualCorrespondences())
02649 {
02650 info->rejectedMsg = uFormat("Too low inliers after bundle adjustment: %d<%d", info->inliers, _registrationPipeline->getMinVisualCorrespondences());
02651 transform.setNull();
02652 }
02653 else
02654 {
02655 transform = bundlePoses.rbegin()->second;
02656 }
02657 }
02658 UDEBUG("Local Bundle Adjustment After : %s", transform.prettyPrint().c_str());
02659 }
02660 }
02661 else
02662 {
02663 transform = _registrationPipeline->computeTransformationMod(tmpFrom, tmpTo, guess, info);
02664 }
02665
02666 if(!transform.isNull())
02667 {
02668 UDEBUG("");
02669
02670 float x,y,z, roll,pitch,yaw;
02671 if(guess.isNull())
02672 {
02673 transform.getTranslationAndEulerAngles(x,y,z, roll,pitch,yaw);
02674 }
02675 else
02676 {
02677 Transform guessError = guess.inverse() * transform;
02678 guessError.getTranslationAndEulerAngles(x,y,z, roll,pitch,yaw);
02679 }
02680 if(fabs(pitch) > CV_PI/2 ||
02681 fabs(yaw) > CV_PI/2)
02682 {
02683 transform.setNull();
02684 std::string msg = uFormat("Too large rotation detected! (pitch=%f, yaw=%f) max is %f",
02685 roll, pitch, yaw, CV_PI/2);
02686 UINFO(msg.c_str());
02687 if(info)
02688 {
02689 info->rejectedMsg = msg;
02690 }
02691 }
02692 }
02693 }
02694 return transform;
02695 }
02696
02697
02698 Transform Memory::computeIcpTransformMulti(
02699 int fromId,
02700 int toId,
02701 const std::map<int, Transform> & poses,
02702 RegistrationInfo * info)
02703 {
02704 UASSERT(uContains(poses, fromId) && uContains(_signatures, fromId));
02705 UASSERT(uContains(poses, toId) && uContains(_signatures, toId));
02706
02707 UDEBUG("%d -> %d, Guess=%s", fromId, toId, (poses.at(fromId).inverse() * poses.at(toId)).prettyPrint().c_str());
02708 if(ULogger::level() == ULogger::kDebug)
02709 {
02710 std::string ids;
02711 for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
02712 {
02713 if(iter->first != fromId)
02714 {
02715 ids += uNumber2Str(iter->first) + " ";
02716 }
02717 }
02718 UDEBUG("%d vs %s", fromId, ids.c_str());
02719 }
02720
02721
02722 std::list<Signature*> depthToLoad;
02723 for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
02724 {
02725 Signature * s = _getSignature(iter->first);
02726 UASSERT(s != 0);
02727
02728 if(s->sensorData().imageCompressed().empty() &&
02729 s->sensorData().laserScanCompressed().isEmpty())
02730 {
02731 depthToLoad.push_back(s);
02732 }
02733 }
02734 if(depthToLoad.size() && _dbDriver)
02735 {
02736 _dbDriver->loadNodeData(depthToLoad, false, true, false, false);
02737 }
02738
02739 Signature * fromS = _getSignature(fromId);
02740 LaserScan fromScan;
02741 fromS->sensorData().uncompressData(0, 0, &fromScan);
02742
02743 Signature * toS = _getSignature(toId);
02744 LaserScan toScan;
02745 toS->sensorData().uncompressData(0, 0, &toScan);
02746
02747 Transform t;
02748 if(!fromScan.isEmpty() && !toScan.isEmpty())
02749 {
02750 Transform guess = poses.at(fromId).inverse() * poses.at(toId);
02751 float guessNorm = guess.getNorm();
02752 if(fromScan.maxRange() > 0.0f && toScan.maxRange() > 0.0f &&
02753 guessNorm > fromScan.maxRange() + toScan.maxRange())
02754 {
02755
02756 UINFO("Too far scans between %d and %d to compute transformation: guessNorm=%f, scan range from=%f to=%f", fromId, toId, guessNorm, fromScan.maxRange(), toScan.maxRange());
02757 return t;
02758 }
02759
02760
02761 SensorData assembledData;
02762 Transform toPoseInv = poses.at(toId).inverse();
02763 std::string msg;
02764 int maxPoints = fromScan.size();
02765 pcl::PointCloud<pcl::PointXYZ>::Ptr assembledToClouds(new pcl::PointCloud<pcl::PointXYZ>);
02766 pcl::PointCloud<pcl::PointNormal>::Ptr assembledToNormalClouds(new pcl::PointCloud<pcl::PointNormal>);
02767 pcl::PointCloud<pcl::PointXYZI>::Ptr assembledToIClouds(new pcl::PointCloud<pcl::PointXYZI>);
02768 pcl::PointCloud<pcl::PointXYZINormal>::Ptr assembledToNormalIClouds(new pcl::PointCloud<pcl::PointXYZINormal>);
02769 for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
02770 {
02771 if(iter->first != fromId)
02772 {
02773 Signature * s = this->_getSignature(iter->first);
02774 if(!s->sensorData().laserScanCompressed().isEmpty())
02775 {
02776 LaserScan scan;
02777 s->sensorData().uncompressData(0, 0, &scan);
02778 if(!scan.isEmpty() && scan.format() == fromScan.format())
02779 {
02780 if(scan.hasIntensity())
02781 {
02782 if(scan.hasNormals())
02783 {
02784 *assembledToNormalIClouds += *util3d::laserScanToPointCloudINormal(scan,
02785 toPoseInv * iter->second * scan.localTransform());
02786 }
02787 else
02788 {
02789 *assembledToIClouds += *util3d::laserScanToPointCloudI(scan,
02790 toPoseInv * iter->second * scan.localTransform());
02791 }
02792 }
02793 else
02794 {
02795 if(scan.hasNormals())
02796 {
02797 *assembledToNormalClouds += *util3d::laserScanToPointCloudNormal(scan,
02798 toPoseInv * iter->second * scan.localTransform());
02799 }
02800 else
02801 {
02802 *assembledToClouds += *util3d::laserScanToPointCloud(scan,
02803 toPoseInv * iter->second * scan.localTransform());
02804 }
02805 }
02806
02807 if(scan.size() > maxPoints)
02808 {
02809 maxPoints = scan.size();
02810 }
02811 }
02812 else if(!scan.isEmpty())
02813 {
02814 UWARN("Incompatible scan format %d vs %d", (int)fromScan.format(), (int)scan.format());
02815 }
02816 }
02817 else
02818 {
02819 UWARN("Depth2D not found for signature %d", iter->first);
02820 }
02821 }
02822 }
02823
02824 cv::Mat assembledScan;
02825 if(assembledToNormalClouds->size())
02826 {
02827 assembledScan = fromScan.is2d()?util3d::laserScan2dFromPointCloud(*assembledToNormalClouds):util3d::laserScanFromPointCloud(*assembledToNormalClouds);
02828 }
02829 else if(assembledToClouds->size())
02830 {
02831 assembledScan = fromScan.is2d()?util3d::laserScan2dFromPointCloud(*assembledToClouds):util3d::laserScanFromPointCloud(*assembledToClouds);
02832 }
02833 else if(assembledToNormalIClouds->size())
02834 {
02835 assembledScan = fromScan.is2d()?util3d::laserScan2dFromPointCloud(*assembledToNormalIClouds):util3d::laserScanFromPointCloud(*assembledToNormalIClouds);
02836 }
02837 else if(assembledToIClouds->size())
02838 {
02839 assembledScan = fromScan.is2d()?util3d::laserScan2dFromPointCloud(*assembledToIClouds):util3d::laserScanFromPointCloud(*assembledToIClouds);
02840 }
02841
02842
02843 assembledData.setLaserScanRaw(
02844 LaserScan(assembledScan,
02845 fromScan.maxPoints()?fromScan.maxPoints():maxPoints,
02846 fromScan.maxRange(),
02847 fromScan.format(),
02848 fromScan.is2d()?Transform(0,0,fromScan.localTransform().z(),0,0,0):Transform::getIdentity()));
02849
02850 t = _registrationIcpMulti->computeTransformation(fromS->sensorData(), assembledData, guess, info);
02851 }
02852
02853 return t;
02854 }
02855
02856 bool Memory::addLink(const Link & link, bool addInDatabase)
02857 {
02858 UASSERT(link.type() > Link::kNeighbor && link.type() != Link::kUndef);
02859
02860 ULOGGER_INFO("to=%d, from=%d transform: %s var=%f", link.to(), link.from(), link.transform().prettyPrint().c_str(), link.transVariance());
02861 Signature * toS = _getSignature(link.to());
02862 Signature * fromS = _getSignature(link.from());
02863 if(toS && fromS)
02864 {
02865 if(toS->hasLink(link.from()))
02866 {
02867
02868 UINFO("already linked! to=%d, from=%d", link.to(), link.from());
02869 return true;
02870 }
02871
02872 UDEBUG("Add link between %d and %d", toS->id(), fromS->id());
02873
02874 toS->addLink(link.inverse());
02875 fromS->addLink(link);
02876
02877 if(_incrementalMemory)
02878 {
02879 if(link.type()!=Link::kVirtualClosure)
02880 {
02881 _linksChanged = true;
02882
02883
02884
02885 if(link.type() != Link::kLocalSpaceClosure ||
02886 link.userDataCompressed().empty())
02887 {
02888 _lastGlobalLoopClosureId = fromS->id()>toS->id()?fromS->id():toS->id();
02889
02890
02891
02892 UASSERT(fromS->getWeight() >= 0 && toS->getWeight() >=0);
02893 if((_reduceGraph && fromS->id() < toS->id()) ||
02894 (!_reduceGraph && fromS->id() > toS->id()))
02895 {
02896 fromS->setWeight(fromS->getWeight() + toS->getWeight());
02897 toS->setWeight(0);
02898 }
02899 else
02900 {
02901 toS->setWeight(toS->getWeight() + fromS->getWeight());
02902 fromS->setWeight(0);
02903 }
02904 }
02905 }
02906 }
02907 }
02908 else if(!addInDatabase)
02909 {
02910 if(!fromS)
02911 {
02912 UERROR("from=%d, to=%d, Signature %d not found in working/st memories", link.from(), link.to(), link.from());
02913 }
02914 if(!toS)
02915 {
02916 UERROR("from=%d, to=%d, Signature %d not found in working/st memories", link.from(), link.to(), link.to());
02917 }
02918 return false;
02919 }
02920 else if(fromS)
02921 {
02922 UDEBUG("Add link between %d and %d (db)", link.from(), link.to());
02923 fromS->addLink(link);
02924 _dbDriver->addLink(link.inverse());
02925 }
02926 else if(toS)
02927 {
02928 UDEBUG("Add link between %d (db) and %d", link.from(), link.to());
02929 _dbDriver->addLink(link);
02930 toS->addLink(link.inverse());
02931 }
02932 else
02933 {
02934 UDEBUG("Add link between %d (db) and %d (db)", link.from(), link.to());
02935 _dbDriver->addLink(link);
02936 _dbDriver->addLink(link.inverse());
02937 }
02938 return true;
02939 }
02940
02941 void Memory::updateLink(const Link & link, bool updateInDatabase)
02942 {
02943 Signature * fromS = this->_getSignature(link.from());
02944 Signature * toS = this->_getSignature(link.to());
02945
02946 if(fromS && toS)
02947 {
02948 if(fromS->hasLink(link.to()) && toS->hasLink(link.from()))
02949 {
02950 Link::Type oldType = fromS->getLinks().at(link.to()).type();
02951
02952 fromS->removeLink(link.to());
02953 toS->removeLink(link.from());
02954
02955 fromS->addLink(link);
02956 toS->addLink(link.inverse());
02957
02958 if(oldType!=Link::kVirtualClosure || link.type()!=Link::kVirtualClosure)
02959 {
02960 _linksChanged = true;
02961 }
02962 }
02963 else
02964 {
02965 UERROR("fromId=%d and toId=%d are not linked!", link.from(), link.to());
02966 }
02967 }
02968 else if(!updateInDatabase)
02969 {
02970 if(!fromS)
02971 {
02972 UERROR("from=%d, to=%d, Signature %d not found in working/st memories", link.from(), link.to(), link.from());
02973 }
02974 if(!toS)
02975 {
02976 UERROR("from=%d, to=%d, Signature %d not found in working/st memories", link.from(), link.to(), link.to());
02977 }
02978 }
02979 else if(fromS)
02980 {
02981 UDEBUG("Update link between %d and %d (db)", link.from(), link.to());
02982 fromS->removeLink(link.to());
02983 fromS->addLink(link);
02984 _dbDriver->updateLink(link.inverse());
02985 }
02986 else if(toS)
02987 {
02988 UDEBUG("Update link between %d (db) and %d", link.from(), link.to());
02989 toS->removeLink(link.from());
02990 toS->addLink(link.inverse());
02991 _dbDriver->updateLink(link);
02992 }
02993 else
02994 {
02995 UDEBUG("Update link between %d (db) and %d (db)", link.from(), link.to());
02996 _dbDriver->updateLink(link);
02997 _dbDriver->updateLink(link.inverse());
02998 }
02999 }
03000
03001 void Memory::removeAllVirtualLinks()
03002 {
03003 UDEBUG("");
03004 for(std::map<int, Signature*>::iterator iter=_signatures.begin(); iter!=_signatures.end(); ++iter)
03005 {
03006 iter->second->removeVirtualLinks();
03007 }
03008 }
03009
03010 void Memory::removeVirtualLinks(int signatureId)
03011 {
03012 UDEBUG("");
03013 Signature * s = this->_getSignature(signatureId);
03014 if(s)
03015 {
03016 const std::map<int, Link> & links = s->getLinks();
03017 for(std::map<int, Link>::const_iterator iter=links.begin(); iter!=links.end(); ++iter)
03018 {
03019 if(iter->second.type() == Link::kVirtualClosure)
03020 {
03021 Signature * sTo = this->_getSignature(iter->first);
03022 if(sTo)
03023 {
03024 sTo->removeLink(s->id());
03025 }
03026 else
03027 {
03028 UERROR("Link %d of %d not in WM/STM?!?", iter->first, s->id());
03029 }
03030 }
03031 }
03032 s->removeVirtualLinks();
03033 }
03034 else
03035 {
03036 UERROR("Signature %d not in WM/STM?!?", signatureId);
03037 }
03038 }
03039
03040 void Memory::dumpMemory(std::string directory) const
03041 {
03042 UINFO("Dumping memory to directory \"%s\"", directory.c_str());
03043 this->dumpDictionary((directory+"/DumpMemoryWordRef.txt").c_str(), (directory+"/DumpMemoryWordDesc.txt").c_str());
03044 this->dumpSignatures((directory + "/DumpMemorySign.txt").c_str(), false);
03045 this->dumpSignatures((directory + "/DumpMemorySign3.txt").c_str(), true);
03046 this->dumpMemoryTree((directory + "/DumpMemoryTree.txt").c_str());
03047 }
03048
03049 void Memory::dumpDictionary(const char * fileNameRef, const char * fileNameDesc) const
03050 {
03051 if(_vwd)
03052 {
03053 _vwd->exportDictionary(fileNameRef, fileNameDesc);
03054 }
03055 }
03056
03057 void Memory::dumpSignatures(const char * fileNameSign, bool words3D) const
03058 {
03059 UDEBUG("");
03060 FILE* foutSign = 0;
03061 #ifdef _MSC_VER
03062 fopen_s(&foutSign, fileNameSign, "w");
03063 #else
03064 foutSign = fopen(fileNameSign, "w");
03065 #endif
03066
03067 if(foutSign)
03068 {
03069 fprintf(foutSign, "SignatureID WordsID...\n");
03070 const std::map<int, Signature *> & signatures = this->getSignatures();
03071 for(std::map<int, Signature *>::const_iterator iter=signatures.begin(); iter!=signatures.end(); ++iter)
03072 {
03073 fprintf(foutSign, "%d ", iter->first);
03074 const Signature * ss = dynamic_cast<const Signature *>(iter->second);
03075 if(ss)
03076 {
03077 if(words3D)
03078 {
03079 const std::multimap<int, cv::Point3f> & ref = ss->getWords3();
03080 for(std::multimap<int, cv::Point3f>::const_iterator jter=ref.begin(); jter!=ref.end(); ++jter)
03081 {
03082
03083 if(pcl::isFinite(jter->second) &&
03084 (jter->second.x != 0 || jter->second.y != 0 || jter->second.z != 0))
03085 {
03086 fprintf(foutSign, "%d ", (*jter).first);
03087 }
03088 }
03089 }
03090 else
03091 {
03092 const std::multimap<int, cv::KeyPoint> & ref = ss->getWords();
03093 for(std::multimap<int, cv::KeyPoint>::const_iterator jter=ref.begin(); jter!=ref.end(); ++jter)
03094 {
03095 fprintf(foutSign, "%d ", (*jter).first);
03096 }
03097 }
03098 }
03099 fprintf(foutSign, "\n");
03100 }
03101 fclose(foutSign);
03102 }
03103 }
03104
03105 void Memory::dumpMemoryTree(const char * fileNameTree) const
03106 {
03107 UDEBUG("");
03108 FILE* foutTree = 0;
03109 #ifdef _MSC_VER
03110 fopen_s(&foutTree, fileNameTree, "w");
03111 #else
03112 foutTree = fopen(fileNameTree, "w");
03113 #endif
03114
03115 if(foutTree)
03116 {
03117 fprintf(foutTree, "SignatureID Weight NbLoopClosureIds LoopClosureIds... NbChildLoopClosureIds ChildLoopClosureIds...\n");
03118
03119 for(std::map<int, Signature *>::const_iterator i=_signatures.begin(); i!=_signatures.end(); ++i)
03120 {
03121 fprintf(foutTree, "%d %d", i->first, i->second->getWeight());
03122
03123 std::map<int, Link> loopIds, childIds;
03124
03125 for(std::map<int, Link>::const_iterator iter = i->second->getLinks().begin();
03126 iter!=i->second->getLinks().end();
03127 ++iter)
03128 {
03129 if(iter->second.type() != Link::kNeighbor &&
03130 iter->second.type() != Link::kNeighborMerged)
03131 {
03132 if(iter->first < i->first)
03133 {
03134 childIds.insert(*iter);
03135 }
03136 else if(iter->second.from() != iter->second.to())
03137 {
03138 loopIds.insert(*iter);
03139 }
03140 }
03141 }
03142
03143 fprintf(foutTree, " %d", (int)loopIds.size());
03144 for(std::map<int, Link>::const_iterator j=loopIds.begin(); j!=loopIds.end(); ++j)
03145 {
03146 fprintf(foutTree, " %d", j->first);
03147 }
03148
03149 fprintf(foutTree, " %d", (int)childIds.size());
03150 for(std::map<int, Link>::const_iterator j=childIds.begin(); j!=childIds.end(); ++j)
03151 {
03152 fprintf(foutTree, " %d", j->first);
03153 }
03154
03155 fprintf(foutTree, "\n");
03156 }
03157
03158 fclose(foutTree);
03159 }
03160
03161 }
03162
03163 void Memory::rehearsal(Signature * signature, Statistics * stats)
03164 {
03165 UTimer timer;
03166 if(signature->isBadSignature())
03167 {
03168 return;
03169 }
03170
03171
03172
03173
03174 Signature * sB = 0;
03175 for(std::set<int>::reverse_iterator iter=_stMem.rbegin(); iter!=_stMem.rend(); ++iter)
03176 {
03177 Signature * s = this->_getSignature(*iter);
03178 UASSERT(s!=0);
03179 if(s->getWeight() >= 0 && s->id() != signature->id())
03180 {
03181 sB = s;
03182 break;
03183 }
03184 }
03185 if(sB)
03186 {
03187 int id = sB->id();
03188 UDEBUG("Comparing with signature (%d)...", id);
03189
03190 float sim = signature->compareTo(*sB);
03191
03192 int merged = 0;
03193 if(sim >= _similarityThreshold)
03194 {
03195 if(_incrementalMemory)
03196 {
03197 if(this->rehearsalMerge(id, signature->id()))
03198 {
03199 merged = id;
03200 }
03201 }
03202 else
03203 {
03204 signature->setWeight(signature->getWeight() + 1 + sB->getWeight());
03205 }
03206 }
03207
03208 if(stats) stats->addStatistic(Statistics::kMemoryRehearsal_merged(), merged);
03209 if(stats) stats->addStatistic(Statistics::kMemoryRehearsal_sim(), sim);
03210 if(stats) stats->addStatistic(Statistics::kMemoryRehearsal_id(), sim >= _similarityThreshold?id:0);
03211 UDEBUG("merged=%d, sim=%f t=%fs", merged, sim, timer.ticks());
03212 }
03213 else
03214 {
03215 if(stats) stats->addStatistic(Statistics::kMemoryRehearsal_merged(), 0);
03216 if(stats) stats->addStatistic(Statistics::kMemoryRehearsal_sim(), 0);
03217 }
03218 }
03219
03220 bool Memory::rehearsalMerge(int oldId, int newId)
03221 {
03222 ULOGGER_INFO("old=%d, new=%d", oldId, newId);
03223 Signature * oldS = _getSignature(oldId);
03224 Signature * newS = _getSignature(newId);
03225 if(oldS && newS && _incrementalMemory)
03226 {
03227 UASSERT_MSG(oldS->getWeight() >= 0 && newS->getWeight() >= 0, uFormat("%d %d", oldS->getWeight(), newS->getWeight()).c_str());
03228 std::map<int, Link>::const_iterator iter = oldS->getLinks().find(newS->id());
03229 if(iter != oldS->getLinks().end() &&
03230 iter->second.type() != Link::kNeighbor &&
03231 iter->second.type() != Link::kNeighborMerged &&
03232 iter->second.from() != iter->second.to())
03233 {
03234
03235 UWARN("already merged, old=%d, new=%d", oldId, newId);
03236 return false;
03237 }
03238 UASSERT(!newS->isSaved());
03239
03240 UINFO("Rehearsal merging %d (w=%d) and %d (w=%d)",
03241 oldS->id(), oldS->getWeight(),
03242 newS->id(), newS->getWeight());
03243
03244 bool fullMerge;
03245 bool intermediateMerge = false;
03246 if(!newS->getLinks().empty() && !newS->getLinks().begin()->second.transform().isNull())
03247 {
03248
03249
03250
03251 float x,y,z, roll,pitch,yaw;
03252 newS->getLinks().begin()->second.transform().getTranslationAndEulerAngles(x,y,z, roll,pitch,yaw);
03253 bool isMoving = fabs(x) > _rehearsalMaxDistance ||
03254 fabs(y) > _rehearsalMaxDistance ||
03255 fabs(z) > _rehearsalMaxDistance ||
03256 fabs(roll) > _rehearsalMaxAngle ||
03257 fabs(pitch) > _rehearsalMaxAngle ||
03258 fabs(yaw) > _rehearsalMaxAngle;
03259 if(isMoving && _rehearsalWeightIgnoredWhileMoving)
03260 {
03261 UINFO("Rehearsal ignored because the robot has moved more than %f m or %f rad (\"Mem/RehearsalWeightIgnoredWhileMoving\"=true)",
03262 _rehearsalMaxDistance, _rehearsalMaxAngle);
03263 return false;
03264 }
03265 fullMerge = !isMoving && newS->hasLink(oldS->id());
03266 intermediateMerge = !isMoving && !newS->hasLink(oldS->id());
03267 }
03268 else
03269 {
03270 fullMerge = newS->hasLink(oldS->id()) && newS->getLinks().begin()->second.transform().isNull();
03271 }
03272
03273 if(fullMerge)
03274 {
03275
03276 Link newToOldLink = newS->getLinks().at(oldS->id());
03277 oldS->removeLink(newId);
03278 newS->removeLink(oldId);
03279
03280 if(_idUpdatedToNewOneRehearsal)
03281 {
03282
03283 const std::map<int, Link> & links = oldS->getLinks();
03284 for(std::map<int, Link>::const_iterator iter = links.begin(); iter!=links.end(); ++iter)
03285 {
03286 if(iter->second.from() != iter->second.to())
03287 {
03288 Link link = iter->second;
03289 Link mergedLink = newToOldLink.merge(link, link.type());
03290 UASSERT(mergedLink.from() == newS->id() && mergedLink.to() == link.to());
03291
03292 Signature * s = this->_getSignature(link.to());
03293 if(s)
03294 {
03295
03296 s->removeLink(oldS->id());
03297 s->addLink(mergedLink.inverse());
03298
03299 newS->addLink(mergedLink);
03300 }
03301 else
03302 {
03303 UERROR("Didn't find neighbor %d of %d in RAM...", link.to(), oldS->id());
03304 }
03305 }
03306 }
03307 newS->setLabel(oldS->getLabel());
03308 oldS->setLabel("");
03309 oldS->removeLinks();
03310 oldS->addLink(Link(oldS->id(), newS->id(), Link::kGlobalClosure, Transform(), cv::Mat::eye(6,6,CV_64FC1)));
03311
03312
03313 this->copyData(oldS, newS);
03314
03315
03316 newS->setWeight(newS->getWeight() + 1 + oldS->getWeight());
03317
03318 if(_lastGlobalLoopClosureId == oldS->id())
03319 {
03320 _lastGlobalLoopClosureId = newS->id();
03321 }
03322 }
03323 else
03324 {
03325 newS->addLink(Link(newS->id(), oldS->id(), Link::kGlobalClosure, Transform() , cv::Mat::eye(6,6,CV_64FC1)));
03326
03327
03328 oldS->setWeight(newS->getWeight() + 1 + oldS->getWeight());
03329
03330 if(_lastSignature == newS)
03331 {
03332 _lastSignature = oldS;
03333 }
03334 }
03335
03336
03337 moveToTrash(_idUpdatedToNewOneRehearsal?oldS:newS, _notLinkedNodesKeptInDb);
03338
03339 return true;
03340 }
03341 else
03342 {
03343
03344 if(_idUpdatedToNewOneRehearsal)
03345 {
03346
03347 int w = oldS->getWeight()>=0?oldS->getWeight():0;
03348 newS->setWeight(w + newS->getWeight() + 1);
03349 oldS->setWeight(intermediateMerge?-1:0);
03350
03351 if(_lastGlobalLoopClosureId == oldS->id())
03352 {
03353 _lastGlobalLoopClosureId = newS->id();
03354 }
03355 }
03356 else
03357 {
03358 int w = newS->getWeight()>=0?newS->getWeight():0;
03359 oldS->setWeight(w + oldS->getWeight() + 1);
03360 newS->setWeight(intermediateMerge?-1:0);
03361 }
03362 }
03363 }
03364 else
03365 {
03366 if(!newS)
03367 {
03368 UERROR("newId=%d, oldId=%d, Signature %d not found in working/st memories", newId, oldId, newId);
03369 }
03370 if(!oldS)
03371 {
03372 UERROR("newId=%d, oldId=%d, Signature %d not found in working/st memories", newId, oldId, oldId);
03373 }
03374 }
03375 return false;
03376 }
03377
03378 Transform Memory::getOdomPose(int signatureId, bool lookInDatabase) const
03379 {
03380 Transform pose, groundTruth;
03381 int mapId, weight;
03382 std::string label;
03383 double stamp;
03384 std::vector<float> velocity;
03385 GPS gps;
03386 getNodeInfo(signatureId, pose, mapId, weight, label, stamp, groundTruth, velocity, gps, lookInDatabase);
03387 return pose;
03388 }
03389
03390 Transform Memory::getGroundTruthPose(int signatureId, bool lookInDatabase) const
03391 {
03392 Transform pose, groundTruth;
03393 int mapId, weight;
03394 std::string label;
03395 double stamp;
03396 std::vector<float> velocity;
03397 GPS gps;
03398 getNodeInfo(signatureId, pose, mapId, weight, label, stamp, groundTruth, velocity, gps, lookInDatabase);
03399 return groundTruth;
03400 }
03401
03402 bool Memory::getNodeInfo(int signatureId,
03403 Transform & odomPose,
03404 int & mapId,
03405 int & weight,
03406 std::string & label,
03407 double & stamp,
03408 Transform & groundTruth,
03409 std::vector<float> & velocity,
03410 GPS & gps,
03411 bool lookInDatabase) const
03412 {
03413 const Signature * s = this->getSignature(signatureId);
03414 if(s)
03415 {
03416 odomPose = s->getPose();
03417 mapId = s->mapId();
03418 weight = s->getWeight();
03419 label = s->getLabel();
03420 stamp = s->getStamp();
03421 groundTruth = s->getGroundTruthPose();
03422 velocity = s->getVelocity();
03423 gps = s->sensorData().gps();
03424 return true;
03425 }
03426 else if(lookInDatabase && _dbDriver)
03427 {
03428 return _dbDriver->getNodeInfo(signatureId, odomPose, mapId, weight, label, stamp, groundTruth, velocity, gps);
03429 }
03430 return false;
03431 }
03432
03433 cv::Mat Memory::getImageCompressed(int signatureId) const
03434 {
03435 cv::Mat image;
03436 const Signature * s = this->getSignature(signatureId);
03437 if(s)
03438 {
03439 image = s->sensorData().imageCompressed();
03440 }
03441 if(image.empty() && this->isBinDataKept() && _dbDriver)
03442 {
03443 SensorData data;
03444 _dbDriver->getNodeData(signatureId, data);
03445 image = data.imageCompressed();
03446 }
03447 return image;
03448 }
03449
03450 SensorData Memory::getNodeData(int nodeId, bool uncompressedData) const
03451 {
03452 UDEBUG("nodeId=%d", nodeId);
03453 SensorData r;
03454 Signature * s = this->_getSignature(nodeId);
03455 if(s && !s->sensorData().imageCompressed().empty())
03456 {
03457 r = s->sensorData();
03458 }
03459 else if(_dbDriver)
03460 {
03461
03462 _dbDriver->getNodeData(nodeId, r);
03463 }
03464
03465 if(uncompressedData)
03466 {
03467 r.uncompressData();
03468 }
03469
03470 return r;
03471 }
03472
03473 void Memory::getNodeWords(int nodeId,
03474 std::multimap<int, cv::KeyPoint> & words,
03475 std::multimap<int, cv::Point3f> & words3,
03476 std::multimap<int, cv::Mat> & wordsDescriptors)
03477 {
03478 UDEBUG("nodeId=%d", nodeId);
03479 Signature * s = this->_getSignature(nodeId);
03480 if(s)
03481 {
03482 words = s->getWords();
03483 words3 = s->getWords3();
03484 wordsDescriptors = s->getWordsDescriptors();
03485 }
03486 else if(_dbDriver)
03487 {
03488
03489 std::list<Signature*> signatures;
03490 std::list<int> ids;
03491 ids.push_back(nodeId);
03492 std::set<int> loadedFromTrash;
03493 _dbDriver->loadSignatures(ids, signatures, &loadedFromTrash);
03494 if(signatures.size())
03495 {
03496 words = signatures.front()->getWords();
03497 words3 = signatures.front()->getWords3();
03498 wordsDescriptors = signatures.front()->getWordsDescriptors();
03499 if(loadedFromTrash.size())
03500 {
03501
03502 _dbDriver->asyncSave(signatures.front());
03503 }
03504 else
03505 {
03506 delete signatures.front();
03507 }
03508 }
03509 }
03510 }
03511
03512 void Memory::getNodeCalibration(int nodeId,
03513 std::vector<CameraModel> & models,
03514 StereoCameraModel & stereoModel)
03515 {
03516 UDEBUG("nodeId=%d", nodeId);
03517 Signature * s = this->_getSignature(nodeId);
03518 if(s)
03519 {
03520 models = s->sensorData().cameraModels();
03521 stereoModel = s->sensorData().stereoCameraModel();
03522 }
03523 else if(_dbDriver)
03524 {
03525
03526 _dbDriver->getCalibration(nodeId, models, stereoModel);
03527 }
03528 }
03529
03530 SensorData Memory::getSignatureDataConst(int locationId,
03531 bool images, bool scan, bool userData, bool occupancyGrid) const
03532 {
03533 UDEBUG("");
03534 SensorData r;
03535 const Signature * s = this->getSignature(locationId);
03536 if(s && (!s->sensorData().imageCompressed().empty() ||
03537 !s->sensorData().laserScanCompressed().isEmpty() ||
03538 !s->sensorData().userDataCompressed().empty() ||
03539 s->sensorData().gridCellSize() != 0.0f))
03540 {
03541 r = s->sensorData();
03542 }
03543 else if(_dbDriver)
03544 {
03545
03546 _dbDriver->getNodeData(locationId, r, images, scan, userData, occupancyGrid);
03547 }
03548
03549 return r;
03550 }
03551
03552 void Memory::generateGraph(const std::string & fileName, const std::set<int> & ids)
03553 {
03554 if(!_dbDriver)
03555 {
03556 UERROR("A database must must loaded first...");
03557 return;
03558 }
03559
03560 _dbDriver->generateGraph(fileName, ids, _signatures);
03561 }
03562
03563 int Memory::getNi(int signatureId) const
03564 {
03565 int ni = 0;
03566 const Signature * s = this->getSignature(signatureId);
03567 if(s)
03568 {
03569 ni = (int)((Signature *)s)->getWords().size();
03570 }
03571 else
03572 {
03573 _dbDriver->getInvertedIndexNi(signatureId, ni);
03574 }
03575 return ni;
03576 }
03577
03578
03579 void Memory::copyData(const Signature * from, Signature * to)
03580 {
03581 UTimer timer;
03582 timer.start();
03583 if(from && to)
03584 {
03585
03586 this->disableWordsRef(to->id());
03587 to->setWords(from->getWords());
03588 std::list<int> id;
03589 id.push_back(to->id());
03590 this->enableWordsRef(id);
03591
03592 if(from->isSaved() && _dbDriver)
03593 {
03594 _dbDriver->getNodeData(from->id(), to->sensorData());
03595 UDEBUG("Loaded image data from database");
03596 }
03597 else
03598 {
03599 to->sensorData() = (SensorData)from->sensorData();
03600 }
03601 to->sensorData().setId(to->id());
03602
03603 to->setPose(from->getPose());
03604 to->setWords3(from->getWords3());
03605 to->setWordsDescriptors(from->getWordsDescriptors());
03606 }
03607 else
03608 {
03609 ULOGGER_ERROR("Can't merge the signatures because there are not same type.");
03610 }
03611 UDEBUG("Merging time = %fs", timer.ticks());
03612 }
03613
03614 class PreUpdateThread : public UThreadNode
03615 {
03616 public:
03617 PreUpdateThread(VWDictionary * vwp) : _vwp(vwp) {}
03618 virtual ~PreUpdateThread() {}
03619 private:
03620 void mainLoop() {
03621 if(_vwp)
03622 {
03623 _vwp->update();
03624 }
03625 this->kill();
03626 }
03627 VWDictionary * _vwp;
03628 };
03629
03630 Signature * Memory::createSignature(const SensorData & inputData, const Transform & pose, Statistics * stats)
03631 {
03632 UDEBUG("");
03633 SensorData data = inputData;
03634 UASSERT(data.imageRaw().empty() ||
03635 data.imageRaw().type() == CV_8UC1 ||
03636 data.imageRaw().type() == CV_8UC3);
03637 UASSERT_MSG(data.depthOrRightRaw().empty() ||
03638 ( ( data.depthOrRightRaw().type() == CV_16UC1 ||
03639 data.depthOrRightRaw().type() == CV_32FC1 ||
03640 data.depthOrRightRaw().type() == CV_8UC1)
03641 &&
03642 ( (data.imageRaw().empty() && data.depthOrRightRaw().type() != CV_8UC1) ||
03643 (data.imageRaw().rows % data.depthOrRightRaw().rows == 0 && data.imageRaw().cols % data.depthOrRightRaw().cols == 0))),
03644 uFormat("image=(%d/%d) depth=(%d/%d, type=%d [accepted=%d,%d,%d])",
03645 data.imageRaw().cols,
03646 data.imageRaw().rows,
03647 data.depthOrRightRaw().cols,
03648 data.depthOrRightRaw().rows,
03649 data.depthOrRightRaw().type(),
03650 CV_16UC1, CV_32FC1, CV_8UC1).c_str());
03651
03652 if(!data.depthOrRightRaw().empty() &&
03653 data.cameraModels().size() == 0 &&
03654 !data.stereoCameraModel().isValidForProjection() &&
03655 !pose.isNull())
03656 {
03657 UERROR("Camera calibration not valid, calibrate your camera!");
03658 return 0;
03659 }
03660 UASSERT(_feature2D != 0);
03661
03662 PreUpdateThread preUpdateThread(_vwd);
03663
03664 UTimer timer;
03665 timer.start();
03666 float t;
03667 std::vector<cv::KeyPoint> keypoints;
03668 cv::Mat descriptors;
03669 bool isIntermediateNode = data.id() < 0 || data.imageRaw().empty();
03670 int id = data.id();
03671 if(_generateIds)
03672 {
03673 id = this->getNextId();
03674 }
03675 else
03676 {
03677 if(id <= 0)
03678 {
03679 UERROR("Received image ID is null. "
03680 "Please set parameter Mem/GenerateIds to \"true\" or "
03681 "make sure the input source provides image ids (seq).");
03682 return 0;
03683 }
03684 else if(id > _idCount)
03685 {
03686 _idCount = id;
03687 }
03688 else
03689 {
03690 UERROR("Id of acquired image (%d) is smaller than the last in memory (%d). "
03691 "Please set parameter Mem/GenerateIds to \"true\" or "
03692 "make sure the input source provides image ids (seq) over the last in "
03693 "memory, which is %d.",
03694 id,
03695 _idCount,
03696 _idCount);
03697 return 0;
03698 }
03699 }
03700
03701 bool imagesRectified = _imagesAlreadyRectified;
03702
03703 if(!imagesRectified && !data.imageRaw().empty() && !(_rectifyOnlyFeatures && data.rightRaw().empty()))
03704 {
03705
03706 if(data.cameraModels().size())
03707 {
03708
03709 UASSERT(int((data.imageRaw().cols/data.cameraModels().size())*data.cameraModels().size()) == data.imageRaw().cols);
03710 int subImageWidth = data.imageRaw().cols/data.cameraModels().size();
03711 cv::Mat rectifiedImages(data.imageRaw().size(), data.imageRaw().type());
03712 bool initRectMaps = _rectCameraModels.empty();
03713 if(initRectMaps)
03714 {
03715 _rectCameraModels.resize(data.cameraModels().size());
03716 }
03717 for(unsigned int i=0; i<data.cameraModels().size(); ++i)
03718 {
03719 if(data.cameraModels()[i].isValidForRectification())
03720 {
03721 if(initRectMaps)
03722 {
03723 _rectCameraModels[i] = data.cameraModels()[i];
03724 if(!_rectCameraModels[i].isRectificationMapInitialized())
03725 {
03726 UWARN("Initializing rectification maps for camera %d (only done for the first image received)...", i);
03727 _rectCameraModels[i].initRectificationMap();
03728 UWARN("Initializing rectification maps for camera %d (only done for the first image received)... done!", i);
03729 }
03730 }
03731 UASSERT(_rectCameraModels[i].imageWidth() == data.cameraModels()[i].imageWidth() &&
03732 _rectCameraModels[i].imageHeight() == data.cameraModels()[i].imageHeight());
03733 cv::Mat rectifiedImage = _rectCameraModels[i].rectifyImage(cv::Mat(data.imageRaw(), cv::Rect(subImageWidth*i, 0, subImageWidth, data.imageRaw().rows)));
03734 rectifiedImage.copyTo(cv::Mat(rectifiedImages, cv::Rect(subImageWidth*i, 0, subImageWidth, data.imageRaw().rows)));
03735 imagesRectified = true;
03736 }
03737 else
03738 {
03739 UERROR("Calibration for camera %d cannot be used to rectify the image. Make sure to do a "
03740 "full calibration. If images are already rectified, set %s parameter back to true.",
03741 (int)i,
03742 Parameters::kRtabmapImagesAlreadyRectified().c_str());
03743 return 0;
03744 }
03745 }
03746 data.setImageRaw(rectifiedImages);
03747 }
03748 else if(data.stereoCameraModel().isValidForRectification())
03749 {
03750 if(!_rectStereoCameraModel.isValidForRectification())
03751 {
03752 _rectStereoCameraModel = data.stereoCameraModel();
03753 if(!_rectStereoCameraModel.isRectificationMapInitialized())
03754 {
03755 UWARN("Initializing rectification maps (only done for the first image received)...");
03756 _rectStereoCameraModel.initRectificationMap();
03757 UWARN("Initializing rectification maps (only done for the first image received)...done!");
03758 }
03759 }
03760 UASSERT(_rectStereoCameraModel.left().imageWidth() == data.stereoCameraModel().left().imageWidth());
03761 UASSERT(_rectStereoCameraModel.left().imageHeight() == data.stereoCameraModel().left().imageHeight());
03762 data.setImageRaw(_rectStereoCameraModel.left().rectifyImage(data.imageRaw()));
03763 data.setDepthOrRightRaw(_rectStereoCameraModel.right().rectifyImage(data.rightRaw()));
03764 imagesRectified = true;
03765 }
03766 else
03767 {
03768 UERROR("Stereo calibration cannot be used to rectify images. Make sure to do a "
03769 "full stereo calibration. If images are already rectified, set %s parameter back to true.",
03770 Parameters::kRtabmapImagesAlreadyRectified().c_str());
03771 return 0;
03772 }
03773 t = timer.ticks();
03774 if(stats) stats->addStatistic(Statistics::kTimingMemRectification(), t*1000.0f);
03775 UDEBUG("time rectification = %fs", t);
03776 }
03777
03778 int treeSize= int(_workingMem.size() + _stMem.size());
03779 int meanWordsPerLocation = _feature2D->getMaxFeatures()>0?_feature2D->getMaxFeatures():0;
03780 if(treeSize > 1)
03781 {
03782 meanWordsPerLocation = _vwd->getTotalActiveReferences() / (treeSize-1);
03783 }
03784
03785 if(_parallelized && !isIntermediateNode)
03786 {
03787 UDEBUG("Start dictionary update thread");
03788 preUpdateThread.start();
03789 }
03790
03791 int preDecimation = 1;
03792 std::vector<cv::Point3f> keypoints3D;
03793 SensorData decimatedData;
03794 if(!_useOdometryFeatures || data.keypoints().empty() || (int)data.keypoints().size() != data.descriptors().rows)
03795 {
03796 if(_feature2D->getMaxFeatures() >= 0 && !data.imageRaw().empty() && !isIntermediateNode)
03797 {
03798 decimatedData = data;
03799 if(_imagePreDecimation > 1)
03800 {
03801 preDecimation = _imagePreDecimation;
03802 if(!decimatedData.rightRaw().empty() ||
03803 (decimatedData.depthRaw().rows == decimatedData.imageRaw().rows && decimatedData.depthRaw().cols == decimatedData.imageRaw().cols))
03804 {
03805 decimatedData.setDepthOrRightRaw(util2d::decimate(decimatedData.depthOrRightRaw(), _imagePreDecimation));
03806 }
03807 decimatedData.setImageRaw(util2d::decimate(decimatedData.imageRaw(), _imagePreDecimation));
03808 std::vector<CameraModel> cameraModels = decimatedData.cameraModels();
03809 for(unsigned int i=0; i<cameraModels.size(); ++i)
03810 {
03811 cameraModels[i] = cameraModels[i].scaled(1.0/double(_imagePreDecimation));
03812 }
03813 decimatedData.setCameraModels(cameraModels);
03814 StereoCameraModel stereoModel = decimatedData.stereoCameraModel();
03815 if(stereoModel.isValidForProjection())
03816 {
03817 stereoModel.scale(1.0/double(_imagePreDecimation));
03818 }
03819 decimatedData.setStereoCameraModel(stereoModel);
03820 }
03821
03822 UINFO("Extract features");
03823 cv::Mat imageMono;
03824 if(decimatedData.imageRaw().channels() == 3)
03825 {
03826 cv::cvtColor(decimatedData.imageRaw(), imageMono, CV_BGR2GRAY);
03827 }
03828 else
03829 {
03830 imageMono = decimatedData.imageRaw();
03831 }
03832
03833 cv::Mat depthMask;
03834 if(imagesRectified && !decimatedData.depthRaw().empty() && _depthAsMask)
03835 {
03836 if(imageMono.rows % decimatedData.depthRaw().rows == 0 &&
03837 imageMono.cols % decimatedData.depthRaw().cols == 0 &&
03838 imageMono.rows/decimatedData.depthRaw().rows == imageMono.cols/decimatedData.depthRaw().cols)
03839 {
03840 depthMask = util2d::interpolate(decimatedData.depthRaw(), imageMono.rows/decimatedData.depthRaw().rows, 0.1f);
03841 }
03842 }
03843
03844 int oldMaxFeatures = _feature2D->getMaxFeatures();
03845 UDEBUG("rawDescriptorsKept=%d, pose=%d, maxFeatures=%d, visMaxFeatures=%d", _rawDescriptorsKept?1:0, pose.isNull()?0:1, _feature2D->getMaxFeatures(), _visMaxFeatures);
03846 ParametersMap tmpMaxFeatureParameter;
03847 if(_rawDescriptorsKept&&!pose.isNull()&&_feature2D->getMaxFeatures()>0&&_feature2D->getMaxFeatures()<_visMaxFeatures)
03848 {
03849
03850 UDEBUG("Changing temporary max features from %d to %d", _feature2D->getMaxFeatures(), _visMaxFeatures);
03851 tmpMaxFeatureParameter.insert(ParametersPair(Parameters::kKpMaxFeatures(), uNumber2Str(_visMaxFeatures)));
03852 _feature2D->parseParameters(tmpMaxFeatureParameter);
03853 }
03854
03855 keypoints = _feature2D->generateKeypoints(
03856 imageMono,
03857 depthMask);
03858
03859 if(tmpMaxFeatureParameter.size())
03860 {
03861 tmpMaxFeatureParameter.at(Parameters::kKpMaxFeatures()) = uNumber2Str(oldMaxFeatures);
03862 _feature2D->parseParameters(tmpMaxFeatureParameter);
03863 }
03864 t = timer.ticks();
03865 if(stats) stats->addStatistic(Statistics::kTimingMemKeypoints_detection(), t*1000.0f);
03866 UDEBUG("time keypoints (%d) = %fs", (int)keypoints.size(), t);
03867
03868 descriptors = _feature2D->generateDescriptors(imageMono, keypoints);
03869 t = timer.ticks();
03870 if(stats) stats->addStatistic(Statistics::kTimingMemDescriptors_extraction(), t*1000.0f);
03871 UDEBUG("time descriptors (%d) = %fs", descriptors.rows, t);
03872
03873 UDEBUG("ratio=%f, meanWordsPerLocation=%d", _badSignRatio, meanWordsPerLocation);
03874 if(descriptors.rows && descriptors.rows < _badSignRatio * float(meanWordsPerLocation))
03875 {
03876 descriptors = cv::Mat();
03877 }
03878 else
03879 {
03880 if(!imagesRectified && decimatedData.cameraModels().size())
03881 {
03882 std::vector<cv::KeyPoint> keypointsValid;
03883 keypointsValid.reserve(keypoints.size());
03884 cv::Mat descriptorsValid;
03885 descriptorsValid.reserve(descriptors.rows);
03886
03887
03888 if(decimatedData.cameraModels().size() == 1)
03889 {
03890 std::vector<cv::Point2f> pointsIn, pointsOut;
03891 cv::KeyPoint::convert(keypoints,pointsIn);
03892 if(decimatedData.cameraModels()[0].D_raw().cols == 6)
03893 {
03894 #if CV_MAJOR_VERSION > 2 or (CV_MAJOR_VERSION == 2 and (CV_MINOR_VERSION >4 or (CV_MINOR_VERSION == 4 and CV_SUBMINOR_VERSION >=10)))
03895
03896
03897 cv::Mat D(1, 4, CV_64FC1);
03898 D.at<double>(0,0) = decimatedData.cameraModels()[0].D_raw().at<double>(0,1);
03899 D.at<double>(0,1) = decimatedData.cameraModels()[0].D_raw().at<double>(0,1);
03900 D.at<double>(0,2) = decimatedData.cameraModels()[0].D_raw().at<double>(0,4);
03901 D.at<double>(0,3) = decimatedData.cameraModels()[0].D_raw().at<double>(0,5);
03902 cv::fisheye::undistortPoints(pointsIn, pointsOut,
03903 decimatedData.cameraModels()[0].K_raw(),
03904 D,
03905 decimatedData.cameraModels()[0].R(),
03906 decimatedData.cameraModels()[0].P());
03907 }
03908 else
03909 #else
03910 UWARN("Too old opencv version (%d,%d,%d) to support fisheye model (min 2.4.10 required)!",
03911 CV_MAJOR_VERSION, CV_MINOR_VERSION, CV_SUBMINOR_VERSION);
03912 }
03913 #endif
03914 {
03915
03916 cv::undistortPoints(pointsIn, pointsOut,
03917 decimatedData.cameraModels()[0].K_raw(),
03918 decimatedData.cameraModels()[0].D_raw(),
03919 decimatedData.cameraModels()[0].R(),
03920 decimatedData.cameraModels()[0].P());
03921 }
03922 UASSERT(pointsOut.size() == keypoints.size());
03923 for(unsigned int i=0; i<pointsOut.size(); ++i)
03924 {
03925 if(pointsOut.at(i).x>=0 && pointsOut.at(i).x<decimatedData.cameraModels()[0].imageWidth() &&
03926 pointsOut.at(i).y>=0 && pointsOut.at(i).y<decimatedData.cameraModels()[0].imageHeight())
03927 {
03928 keypointsValid.push_back(keypoints.at(i));
03929 keypointsValid.back().pt.x = pointsOut.at(i).x;
03930 keypointsValid.back().pt.y = pointsOut.at(i).y;
03931 descriptorsValid.push_back(descriptors.row(i));
03932 }
03933 }
03934 }
03935 else
03936 {
03937 UASSERT(int((decimatedData.imageRaw().cols/decimatedData.cameraModels().size())*decimatedData.cameraModels().size()) == decimatedData.imageRaw().cols);
03938 float subImageWidth = decimatedData.imageRaw().cols/decimatedData.cameraModels().size();
03939 for(unsigned int i=0; i<keypoints.size(); ++i)
03940 {
03941 int cameraIndex = int(keypoints.at(i).pt.x / subImageWidth);
03942 UASSERT_MSG(cameraIndex >= 0 && cameraIndex < (int)decimatedData.cameraModels().size(),
03943 uFormat("cameraIndex=%d, models=%d, kpt.x=%f, subImageWidth=%f (Camera model image width=%d)",
03944 cameraIndex, (int)decimatedData.cameraModels().size(), keypoints[i].pt.x, subImageWidth, decimatedData.cameraModels()[0].imageWidth()).c_str());
03945
03946 std::vector<cv::Point2f> pointsIn, pointsOut;
03947 pointsIn.push_back(cv::Point2f(keypoints.at(i).pt.x-subImageWidth*cameraIndex, keypoints.at(i).pt.y));
03948 if(decimatedData.cameraModels()[cameraIndex].D_raw().cols == 6)
03949 {
03950 #if CV_MAJOR_VERSION > 2 or (CV_MAJOR_VERSION == 2 and (CV_MINOR_VERSION >4 or (CV_MINOR_VERSION == 4 and CV_SUBMINOR_VERSION >=10)))
03951
03952
03953 cv::Mat D(1, 4, CV_64FC1);
03954 D.at<double>(0,0) = decimatedData.cameraModels()[cameraIndex].D_raw().at<double>(0,1);
03955 D.at<double>(0,1) = decimatedData.cameraModels()[cameraIndex].D_raw().at<double>(0,1);
03956 D.at<double>(0,2) = decimatedData.cameraModels()[cameraIndex].D_raw().at<double>(0,4);
03957 D.at<double>(0,3) = decimatedData.cameraModels()[cameraIndex].D_raw().at<double>(0,5);
03958 cv::fisheye::undistortPoints(pointsIn, pointsOut,
03959 decimatedData.cameraModels()[cameraIndex].K_raw(),
03960 D,
03961 decimatedData.cameraModels()[cameraIndex].R(),
03962 decimatedData.cameraModels()[cameraIndex].P());
03963 }
03964 else
03965 #else
03966 UWARN("Too old opencv version (%d,%d,%d) to support fisheye model (min 2.4.10 required)!",
03967 CV_MAJOR_VERSION, CV_MINOR_VERSION, CV_SUBMINOR_VERSION);
03968 }
03969 #endif
03970 {
03971
03972 cv::undistortPoints(pointsIn, pointsOut,
03973 decimatedData.cameraModels()[cameraIndex].K_raw(),
03974 decimatedData.cameraModels()[cameraIndex].D_raw(),
03975 decimatedData.cameraModels()[cameraIndex].R(),
03976 decimatedData.cameraModels()[cameraIndex].P());
03977 }
03978
03979 if(pointsOut[0].x>=0 && pointsOut[0].x<decimatedData.cameraModels()[cameraIndex].imageWidth() &&
03980 pointsOut[0].y>=0 && pointsOut[0].y<decimatedData.cameraModels()[cameraIndex].imageHeight())
03981 {
03982 keypointsValid.push_back(keypoints.at(i));
03983 keypointsValid.back().pt.x = pointsOut[0].x + subImageWidth*cameraIndex;
03984 keypointsValid.back().pt.y = pointsOut[0].y;
03985 descriptorsValid.push_back(descriptors.row(i));
03986 }
03987 }
03988 }
03989
03990 keypoints = keypointsValid;
03991 descriptors = descriptorsValid;
03992
03993 t = timer.ticks();
03994 if(stats) stats->addStatistic(Statistics::kTimingMemRectification(), t*1000.0f);
03995 UDEBUG("time rectification = %fs", t);
03996 }
03997
03998 if((!decimatedData.depthRaw().empty() && decimatedData.cameraModels().size() && decimatedData.cameraModels()[0].isValidForProjection()) ||
03999 (!decimatedData.rightRaw().empty() && decimatedData.stereoCameraModel().isValidForProjection()))
04000 {
04001 keypoints3D = _feature2D->generateKeypoints3D(decimatedData, keypoints);
04002 t = timer.ticks();
04003 if(stats) stats->addStatistic(Statistics::kTimingMemKeypoints_3D(), t*1000.0f);
04004 UDEBUG("time keypoints 3D (%d) = %fs", (int)keypoints3D.size(), t);
04005 }
04006 }
04007 }
04008 else if(data.imageRaw().empty())
04009 {
04010 UDEBUG("Empty image, cannot extract features...");
04011 }
04012 else if(_feature2D->getMaxFeatures() < 0)
04013 {
04014 UDEBUG("_feature2D->getMaxFeatures()(%d<0) so don't extract any features...", _feature2D->getMaxFeatures());
04015 }
04016 else
04017 {
04018 UDEBUG("Intermediate node detected, don't extract features!");
04019 }
04020 }
04021 else if(_feature2D->getMaxFeatures() >= 0 && !isIntermediateNode)
04022 {
04023 UINFO("Use odometry features: kpts=%d 3d=%d desc=%d (dim=%d, type=%d)",
04024 (int)data.keypoints().size(),
04025 (int)data.keypoints3D().size(),
04026 data.descriptors().rows,
04027 data.descriptors().cols,
04028 data.descriptors().type());
04029 keypoints = data.keypoints();
04030 keypoints3D = data.keypoints3D();
04031 descriptors = data.descriptors().clone();
04032
04033 UASSERT(descriptors.empty() || descriptors.rows == (int)keypoints.size());
04034 UASSERT(keypoints3D.empty() || keypoints3D.size() == keypoints.size());
04035
04036 int maxFeatures = _rawDescriptorsKept&&!pose.isNull()&&_feature2D->getMaxFeatures()>0&&_feature2D->getMaxFeatures()<_visMaxFeatures?_visMaxFeatures:_feature2D->getMaxFeatures();
04037 if((int)keypoints.size() > maxFeatures)
04038 {
04039 _feature2D->limitKeypoints(keypoints, keypoints3D, descriptors, maxFeatures);
04040 }
04041 t = timer.ticks();
04042 if(stats) stats->addStatistic(Statistics::kTimingMemKeypoints_detection(), t*1000.0f);
04043 UDEBUG("time keypoints (%d) = %fs", (int)keypoints.size(), t);
04044
04045 if(descriptors.empty())
04046 {
04047 cv::Mat imageMono;
04048 if(data.imageRaw().channels() == 3)
04049 {
04050 cv::cvtColor(data.imageRaw(), imageMono, CV_BGR2GRAY);
04051 }
04052 else
04053 {
04054 imageMono = data.imageRaw();
04055 }
04056
04057 UASSERT_MSG(imagesRectified, "Cannot extract descriptors on not rectified image from keypoints which assumed to be undistorted");
04058
04059 descriptors = _feature2D->generateDescriptors(imageMono, keypoints);
04060 }
04061 t = timer.ticks();
04062 if(stats) stats->addStatistic(Statistics::kTimingMemDescriptors_extraction(), t*1000.0f);
04063 UDEBUG("time descriptors (%d) = %fs", descriptors.rows, t);
04064
04065 if(keypoints3D.empty() &&
04066 ((!data.depthRaw().empty() && data.cameraModels().size() && data.cameraModels()[0].isValidForProjection()) ||
04067 (!data.rightRaw().empty() && data.stereoCameraModel().isValidForProjection())))
04068 {
04069 keypoints3D = _feature2D->generateKeypoints3D(data, keypoints);
04070 if(_feature2D->getMinDepth() > 0.0f || _feature2D->getMaxDepth() > 0.0f)
04071 {
04072 UDEBUG("");
04073
04074 UASSERT((int)keypoints.size() == descriptors.rows &&
04075 keypoints3D.size() == keypoints.size());
04076 std::vector<cv::KeyPoint> validKeypoints(keypoints.size());
04077 std::vector<cv::Point3f> validKeypoints3D(keypoints.size());
04078 cv::Mat validDescriptors(descriptors.size(), descriptors.type());
04079
04080 int oi=0;
04081 for(unsigned int i=0; i<keypoints3D.size(); ++i)
04082 {
04083 if(util3d::isFinite(keypoints3D[i]))
04084 {
04085 validKeypoints[oi] = keypoints[i];
04086 validKeypoints3D[oi] = keypoints3D[i];
04087 descriptors.row(i).copyTo(validDescriptors.row(oi));
04088 ++oi;
04089 }
04090 }
04091 UDEBUG("Removed %d invalid 3D points", (int)keypoints3D.size()-oi);
04092 validKeypoints.resize(oi);
04093 validKeypoints3D.resize(oi);
04094 keypoints = validKeypoints;
04095 keypoints3D = validKeypoints3D;
04096 descriptors = validDescriptors.rowRange(0, oi).clone();
04097 }
04098 }
04099 t = timer.ticks();
04100 if(stats) stats->addStatistic(Statistics::kTimingMemKeypoints_3D(), t*1000.0f);
04101 UDEBUG("time keypoints 3D (%d) = %fs", (int)keypoints3D.size(), t);
04102
04103 UDEBUG("ratio=%f, meanWordsPerLocation=%d", _badSignRatio, meanWordsPerLocation);
04104 if(descriptors.rows && descriptors.rows < _badSignRatio * float(meanWordsPerLocation))
04105 {
04106 descriptors = cv::Mat();
04107 }
04108 }
04109
04110 if(_parallelized)
04111 {
04112 UDEBUG("Joining dictionary update thread...");
04113 preUpdateThread.join();
04114 UDEBUG("Joining dictionary update thread... thread finished!");
04115 }
04116
04117 t = timer.ticks();
04118 if(stats) stats->addStatistic(Statistics::kTimingMemJoining_dictionary_update(), t*1000.0f);
04119 if(_parallelized)
04120 {
04121 UDEBUG("time descriptor and memory update (%d of size=%d) = %fs", descriptors.rows, descriptors.cols, t);
04122 }
04123 else
04124 {
04125 UDEBUG("time descriptor (%d of size=%d) = %fs", descriptors.rows, descriptors.cols, t);
04126 }
04127
04128 std::list<int> wordIds;
04129 if(descriptors.rows)
04130 {
04131
04132
04133 std::vector<bool> inliers;
04134 cv::Mat descriptorsForQuantization = descriptors;
04135 std::vector<int> quantizedToRawIndices;
04136 if(_feature2D->getMaxFeatures()>0 && descriptors.rows > _feature2D->getMaxFeatures())
04137 {
04138 UASSERT((int)keypoints.size() == descriptors.rows);
04139 Feature2D::limitKeypoints(keypoints, inliers, _feature2D->getMaxFeatures());
04140
04141 descriptorsForQuantization = cv::Mat(_feature2D->getMaxFeatures(), descriptors.cols, descriptors.type());
04142 quantizedToRawIndices.resize(_feature2D->getMaxFeatures());
04143 unsigned int oi=0;
04144 UASSERT((int)inliers.size() == descriptors.rows);
04145 for(int k=0; k < descriptors.rows; ++k)
04146 {
04147 if(inliers[k])
04148 {
04149 UASSERT(oi < quantizedToRawIndices.size());
04150 if(descriptors.type() == CV_32FC1)
04151 {
04152 memcpy(descriptorsForQuantization.ptr<float>(oi), descriptors.ptr<float>(k), descriptors.cols*sizeof(float));
04153 }
04154 else
04155 {
04156 memcpy(descriptorsForQuantization.ptr<char>(oi), descriptors.ptr<char>(k), descriptors.cols*sizeof(char));
04157 }
04158 quantizedToRawIndices[oi] = k;
04159 ++oi;
04160 }
04161 }
04162 UASSERT((int)oi == _feature2D->getMaxFeatures());
04163 }
04164
04165
04166 wordIds = _vwd->addNewWords(descriptorsForQuantization, id);
04167
04168
04169 if(wordIds.size() < keypoints.size())
04170 {
04171 std::vector<int> allWordIds;
04172 allWordIds.resize(keypoints.size(),-1);
04173 int i=0;
04174 for(std::list<int>::iterator iter=wordIds.begin(); iter!=wordIds.end(); ++iter)
04175 {
04176 allWordIds[quantizedToRawIndices[i]] = *iter;
04177 ++i;
04178 }
04179 int negIndex = -1;
04180 for(i=0; i<(int)allWordIds.size(); ++i)
04181 {
04182 if(allWordIds[i] < 0)
04183 {
04184 allWordIds[i] = negIndex--;
04185 }
04186 }
04187 wordIds = uVectorToList(allWordIds);
04188 }
04189
04190 t = timer.ticks();
04191 if(stats) stats->addStatistic(Statistics::kTimingMemAdd_new_words(), t*1000.0f);
04192 UDEBUG("time addNewWords %fs indexed=%d not=%d", t, _vwd->getIndexedWordsCount(), _vwd->getNotIndexedWordsCount());
04193 }
04194 else if(id>0)
04195 {
04196 UDEBUG("id %d is a bad signature", id);
04197 }
04198
04199 std::multimap<int, cv::KeyPoint> words;
04200 std::multimap<int, cv::Point3f> words3D;
04201 std::multimap<int, cv::Mat> wordsDescriptors;
04202 if(wordIds.size() > 0)
04203 {
04204 UASSERT(wordIds.size() == keypoints.size());
04205 UASSERT(keypoints3D.size() == 0 || keypoints3D.size() == wordIds.size());
04206 unsigned int i=0;
04207 float decimationRatio = float(preDecimation) / float(_imagePostDecimation);
04208 double log2value = log(double(preDecimation))/log(2.0);
04209 for(std::list<int>::iterator iter=wordIds.begin(); iter!=wordIds.end() && i < keypoints.size(); ++iter, ++i)
04210 {
04211 cv::KeyPoint kpt = keypoints[i];
04212 if(preDecimation != _imagePostDecimation)
04213 {
04214
04215 kpt.pt.x *= decimationRatio;
04216 kpt.pt.y *= decimationRatio;
04217 kpt.size *= decimationRatio;
04218 kpt.octave += log2value;
04219 }
04220 words.insert(std::pair<int, cv::KeyPoint>(*iter, kpt));
04221
04222 if(keypoints3D.size())
04223 {
04224 words3D.insert(std::pair<int, cv::Point3f>(*iter, keypoints3D.at(i)));
04225 }
04226 if(_rawDescriptorsKept)
04227 {
04228 wordsDescriptors.insert(std::pair<int, cv::Mat>(*iter, descriptors.row(i).clone()));
04229 }
04230 }
04231 }
04232
04233 cv::Mat image = data.imageRaw();
04234 cv::Mat depthOrRightImage = data.depthOrRightRaw();
04235 std::vector<CameraModel> cameraModels = data.cameraModels();
04236 StereoCameraModel stereoCameraModel = data.stereoCameraModel();
04237
04238
04239 if(_imagePostDecimation > 1 && !isIntermediateNode)
04240 {
04241 if(_imagePostDecimation == preDecimation && decimatedData.isValid())
04242 {
04243 image = decimatedData.imageRaw();
04244 depthOrRightImage = decimatedData.depthOrRightRaw();
04245 cameraModels = decimatedData.cameraModels();
04246 stereoCameraModel = decimatedData.stereoCameraModel();
04247 }
04248 else
04249 {
04250 if(!data.rightRaw().empty() ||
04251 (data.depthRaw().rows == image.rows && data.depthRaw().cols == image.cols))
04252 {
04253 depthOrRightImage = util2d::decimate(depthOrRightImage, _imagePostDecimation);
04254 }
04255 image = util2d::decimate(image, _imagePostDecimation);
04256 for(unsigned int i=0; i<cameraModels.size(); ++i)
04257 {
04258 cameraModels[i] = cameraModels[i].scaled(1.0/double(_imagePostDecimation));
04259 }
04260 if(stereoCameraModel.isValidForProjection())
04261 {
04262 stereoCameraModel.scale(1.0/double(_imagePostDecimation));
04263 }
04264 }
04265
04266 t = timer.ticks();
04267 if(stats) stats->addStatistic(Statistics::kTimingMemPost_decimation(), t*1000.0f);
04268 UDEBUG("time post-decimation = %fs", t);
04269 }
04270
04271 bool triangulateWordsWithoutDepth = !_depthAsMask;
04272 if(!pose.isNull() &&
04273 cameraModels.size() == 1 &&
04274 words.size() &&
04275 (words3D.size() == 0 || (triangulateWordsWithoutDepth && words.size() == words3D.size())) &&
04276 _registrationPipeline->isImageRequired() &&
04277 _signatures.size() &&
04278 _signatures.rbegin()->second->mapId() == _idMapCount)
04279 {
04280 UDEBUG("Generate 3D words using odometry");
04281 Signature * previousS = _signatures.rbegin()->second;
04282 if(previousS->getWords().size() > 8 && words.size() > 8 && !previousS->getPose().isNull())
04283 {
04284 UDEBUG("Previous pose(%d) = %s", previousS->id(), previousS->getPose().prettyPrint().c_str());
04285 UDEBUG("Current pose(%d) = %s", id, pose.prettyPrint().c_str());
04286 Transform cameraTransform = pose.inverse() * previousS->getPose();
04287
04288 Signature cpPrevious(2);
04289
04290 std::map<int, cv::KeyPoint> uniqueWords = uMultimapToMapUnique(previousS->getWords());
04291 std::map<int, cv::Mat> uniqueWordsDescriptors = uMultimapToMapUnique(previousS->getWordsDescriptors());
04292 cpPrevious.sensorData().setCameraModels(previousS->sensorData().cameraModels());
04293 cpPrevious.setWords(std::multimap<int, cv::KeyPoint>(uniqueWords.begin(), uniqueWords.end()));
04294 cpPrevious.setWordsDescriptors(std::multimap<int, cv::Mat>(uniqueWordsDescriptors.begin(), uniqueWordsDescriptors.end()));
04295 Signature cpCurrent(1);
04296 uniqueWords = uMultimapToMapUnique(words);
04297 uniqueWordsDescriptors = uMultimapToMapUnique(wordsDescriptors);
04298 cpCurrent.sensorData().setCameraModels(cameraModels);
04299 cpCurrent.setWords(std::multimap<int, cv::KeyPoint>(uniqueWords.begin(), uniqueWords.end()));
04300 cpCurrent.setWordsDescriptors(std::multimap<int, cv::Mat>(uniqueWordsDescriptors.begin(), uniqueWordsDescriptors.end()));
04301
04302 Transform tmpt = _registrationPipeline->computeTransformationMod(cpCurrent, cpPrevious, cameraTransform);
04303 UDEBUG("t=%s", tmpt.prettyPrint().c_str());
04304
04305
04306 std::map<int, cv::Point3f> inliers = util3d::generateWords3DMono(
04307 uMultimapToMapUnique(cpCurrent.getWords()),
04308 uMultimapToMapUnique(cpPrevious.getWords()),
04309 cameraModels[0],
04310 cameraTransform);
04311
04312 UDEBUG("inliers=%d", (int)inliers.size());
04313
04314
04315 float bad_point = std::numeric_limits<float>::quiet_NaN ();
04316 UASSERT(words3D.size() == 0 || words.size() == words3D.size());
04317 bool words3DWasEmpty = words3D.empty();
04318 int added3DPointsWithoutDepth = 0;
04319 for(std::multimap<int, cv::KeyPoint>::const_iterator iter=words.begin(); iter!=words.end(); ++iter)
04320 {
04321 std::map<int, cv::Point3f>::iterator jter=inliers.find(iter->first);
04322 std::multimap<int, cv::Point3f>::iterator iter3D = words3D.find(iter->first);
04323 if(iter3D == words3D.end())
04324 {
04325 if(jter != inliers.end())
04326 {
04327 words3D.insert(std::make_pair(iter->first, jter->second));
04328 ++added3DPointsWithoutDepth;
04329 }
04330 else
04331 {
04332 words3D.insert(std::make_pair(iter->first, cv::Point3f(bad_point,bad_point,bad_point)));
04333 }
04334 }
04335 else if(!util3d::isFinite(iter3D->second) && jter != inliers.end())
04336 {
04337 iter3D->second = jter->second;
04338 ++added3DPointsWithoutDepth;
04339 }
04340 else if(words3DWasEmpty && jter == inliers.end())
04341 {
04342
04343 words3D.insert(std::make_pair(iter->first, cv::Point3f(bad_point,bad_point,bad_point)));
04344 }
04345 }
04346 UDEBUG("added3DPointsWithoutDepth=%d", added3DPointsWithoutDepth);
04347 if(stats) stats->addStatistic(Statistics::kMemoryTriangulated_points(), (float)added3DPointsWithoutDepth);
04348
04349 t = timer.ticks();
04350 UASSERT(words3D.size() == words.size());
04351 if(stats) stats->addStatistic(Statistics::kTimingMemKeypoints_3D_motion(), t*1000.0f);
04352 UDEBUG("time keypoints 3D by motion (%d) = %fs", (int)words3D.size(), t);
04353 }
04354 }
04355
04356
04357 LaserScan laserScan = data.laserScanRaw();
04358 if(!isIntermediateNode && laserScan.size())
04359 {
04360 if(laserScan.maxRange() == 0.0f)
04361 {
04362 bool id2d = laserScan.is2d();
04363 float maxRange = 0.0f;
04364 for(int i=0; i<laserScan.size(); ++i)
04365 {
04366 const float * ptr = laserScan.data().ptr<float>(0, i);
04367 float r;
04368 if(id2d)
04369 {
04370 r = ptr[0]*ptr[0] + ptr[1]*ptr[1];
04371 }
04372 else
04373 {
04374 r = ptr[0]*ptr[0] + ptr[1]*ptr[1] + ptr[2]*ptr[2];
04375 }
04376 if(r>maxRange)
04377 {
04378 maxRange = r;
04379 }
04380 }
04381 if(maxRange > 0.0f)
04382 {
04383 laserScan=LaserScan(laserScan.data(), laserScan.maxPoints(), sqrt(maxRange), laserScan.format(), laserScan.localTransform());
04384 }
04385 }
04386
04387 laserScan = util3d::commonFiltering(laserScan,
04388 _laserScanDownsampleStepSize,
04389 0,
04390 0,
04391 _laserScanVoxelSize,
04392 _laserScanNormalK,
04393 _laserScanNormalRadius);
04394 t = timer.ticks();
04395 if(stats) stats->addStatistic(Statistics::kTimingMemScan_filtering(), t*1000.0f);
04396 UDEBUG("time normals scan = %fs", t);
04397 }
04398
04399 Signature * s;
04400 if(this->isBinDataKept() && (!isIntermediateNode || _saveIntermediateNodeData))
04401 {
04402 UDEBUG("Bin data kept: rgb=%d, depth=%d, scan=%d, userData=%d",
04403 image.empty()?0:1,
04404 depthOrRightImage.empty()?0:1,
04405 laserScan.isEmpty()?0:1,
04406 data.userDataRaw().empty()?0:1);
04407
04408 std::vector<unsigned char> imageBytes;
04409 std::vector<unsigned char> depthBytes;
04410
04411 if(_saveDepth16Format && !depthOrRightImage.empty() && depthOrRightImage.type() == CV_32FC1)
04412 {
04413 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).");
04414 depthOrRightImage = util2d::cvtDepthFromFloat(depthOrRightImage);
04415 }
04416
04417 cv::Mat compressedImage;
04418 cv::Mat compressedDepth;
04419 cv::Mat compressedScan;
04420 cv::Mat compressedUserData;
04421 if(_compressionParallelized)
04422 {
04423 rtabmap::CompressionThread ctImage(image, std::string(".jpg"));
04424 rtabmap::CompressionThread ctDepth(depthOrRightImage, std::string(".png"));
04425 rtabmap::CompressionThread ctLaserScan(laserScan.data());
04426 rtabmap::CompressionThread ctUserData(data.userDataRaw());
04427 if(!image.empty())
04428 {
04429 ctImage.start();
04430 }
04431 if(!depthOrRightImage.empty())
04432 {
04433 ctDepth.start();
04434 }
04435 if(!laserScan.isEmpty())
04436 {
04437 ctLaserScan.start();
04438 }
04439 if(!data.userDataRaw().empty())
04440 {
04441 ctUserData.start();
04442 }
04443 ctImage.join();
04444 ctDepth.join();
04445 ctLaserScan.join();
04446 ctUserData.join();
04447
04448 compressedImage = ctImage.getCompressedData();
04449 compressedDepth = ctDepth.getCompressedData();
04450 compressedScan = ctLaserScan.getCompressedData();
04451 compressedUserData = ctUserData.getCompressedData();
04452 }
04453 else
04454 {
04455 compressedImage = compressImage2(image, std::string(".jpg"));
04456 compressedDepth = compressImage2(depthOrRightImage, depthOrRightImage.type() == CV_32FC1 || depthOrRightImage.type() == CV_16UC1?std::string(".png"):std::string(".jpg"));
04457 compressedScan = compressData2(laserScan.data());
04458 compressedUserData = compressData2(data.userDataRaw());
04459 }
04460
04461 s = new Signature(id,
04462 _idMapCount,
04463 isIntermediateNode?-1:0,
04464 data.stamp(),
04465 "",
04466 pose,
04467 data.groundTruth(),
04468 stereoCameraModel.isValidForProjection()?
04469 SensorData(
04470 LaserScan(compressedScan, laserScan.maxPoints(), laserScan.maxRange(), laserScan.format(), laserScan.localTransform()),
04471 compressedImage,
04472 compressedDepth,
04473 stereoCameraModel,
04474 id,
04475 0,
04476 compressedUserData):
04477 SensorData(
04478 LaserScan(compressedScan, laserScan.maxPoints(), laserScan.maxRange(), laserScan.format(), laserScan.localTransform()),
04479 compressedImage,
04480 compressedDepth,
04481 cameraModels,
04482 id,
04483 0,
04484 compressedUserData));
04485 }
04486 else
04487 {
04488 UDEBUG("Bin data kept: scan=%d, userData=%d",
04489 laserScan.isEmpty()?0:1,
04490 data.userDataRaw().empty()?0:1);
04491
04492
04493 cv::Mat compressedScan;
04494 cv::Mat compressedUserData;
04495 if(_compressionParallelized)
04496 {
04497 rtabmap::CompressionThread ctUserData(data.userDataRaw());
04498 rtabmap::CompressionThread ctLaserScan(laserScan.data());
04499 if(!data.userDataRaw().empty() && !isIntermediateNode)
04500 {
04501 ctUserData.start();
04502 }
04503 if(!laserScan.isEmpty() && !isIntermediateNode)
04504 {
04505 ctLaserScan.start();
04506 }
04507 ctUserData.join();
04508 ctLaserScan.join();
04509
04510 compressedScan = ctLaserScan.getCompressedData();
04511 compressedUserData = ctUserData.getCompressedData();
04512 }
04513 else
04514 {
04515 compressedScan = compressData2(laserScan.data());
04516 compressedUserData = compressData2(data.userDataRaw());
04517 }
04518
04519 s = new Signature(id,
04520 _idMapCount,
04521 isIntermediateNode?-1:0,
04522 data.stamp(),
04523 "",
04524 pose,
04525 data.groundTruth(),
04526 stereoCameraModel.isValidForProjection()?
04527 SensorData(
04528 LaserScan(compressedScan, laserScan.maxPoints(), laserScan.maxRange(), laserScan.format(), laserScan.localTransform()),
04529 cv::Mat(),
04530 cv::Mat(),
04531 stereoCameraModel,
04532 id,
04533 0,
04534 compressedUserData):
04535 SensorData(
04536 LaserScan(compressedScan, laserScan.maxPoints(), laserScan.maxRange(), laserScan.format(), laserScan.localTransform()),
04537 cv::Mat(),
04538 cv::Mat(),
04539 cameraModels,
04540 id,
04541 0,
04542 compressedUserData));
04543 }
04544
04545 s->setWords(words);
04546 s->setWords3(words3D);
04547 s->setWordsDescriptors(wordsDescriptors);
04548
04549
04550 s->sensorData().setImageRaw(image);
04551 s->sensorData().setDepthOrRightRaw(depthOrRightImage);
04552 s->sensorData().setLaserScanRaw(laserScan);
04553 s->sensorData().setUserDataRaw(data.userDataRaw());
04554
04555 s->sensorData().setGroundTruth(data.groundTruth());
04556 s->sensorData().setGPS(data.gps());
04557
04558 t = timer.ticks();
04559 if(stats) stats->addStatistic(Statistics::kTimingMemCompressing_data(), t*1000.0f);
04560 UDEBUG("time compressing data (id=%d) %fs", id, t);
04561 if(words.size())
04562 {
04563 s->setEnabled(true);
04564 }
04565
04566
04567 cv::Mat ground, obstacles, empty;
04568 float cellSize = 0.0f;
04569 cv::Point3f viewPoint(0,0,0);
04570 if(_createOccupancyGrid && !data.depthOrRightRaw().empty() && !isIntermediateNode)
04571 {
04572 _occupancy->createLocalMap(*s, ground, obstacles, empty, viewPoint);
04573 cellSize = _occupancy->getCellSize();
04574
04575 t = timer.ticks();
04576 if(stats) stats->addStatistic(Statistics::kTimingMemOccupancy_grid(), t*1000.0f);
04577 UDEBUG("time grid map = %fs", t);
04578 }
04579 s->sensorData().setOccupancyGrid(ground, obstacles, empty, cellSize, viewPoint);
04580
04581
04582 if(!isIntermediateNode)
04583 {
04584 if(!data.globalPose().isNull() && data.globalPoseCovariance().cols==6 && data.globalPoseCovariance().rows==6 && data.globalPoseCovariance().cols==CV_64FC1)
04585 {
04586 s->addLink(Link(s->id(), s->id(), Link::kPosePrior, data.globalPose(), data.globalPoseCovariance().inv()));
04587
04588
04589
04590
04591
04592 }
04593 else if(data.gps().stamp() > 0.0)
04594 {
04595
04596
04597
04598
04599
04600
04601
04602
04603
04604
04605
04606
04607
04608
04609
04610
04611
04612
04613
04614 }
04615 }
04616
04617 return s;
04618 }
04619
04620 void Memory::disableWordsRef(int signatureId)
04621 {
04622 UDEBUG("id=%d", signatureId);
04623
04624 Signature * ss = this->_getSignature(signatureId);
04625 if(ss && ss->isEnabled())
04626 {
04627 const std::multimap<int, cv::KeyPoint> & words = ss->getWords();
04628 const std::list<int> & keys = uUniqueKeys(words);
04629 int count = _vwd->getTotalActiveReferences();
04630
04631 for(std::list<int>::const_iterator i=keys.begin(); i!=keys.end(); ++i)
04632 {
04633 _vwd->removeAllWordRef(*i, signatureId);
04634 }
04635
04636 count -= _vwd->getTotalActiveReferences();
04637 ss->setEnabled(false);
04638 UDEBUG("%d words total ref removed from signature %d... (total active ref = %d)", count, ss->id(), _vwd->getTotalActiveReferences());
04639 }
04640 }
04641
04642 void Memory::cleanUnusedWords()
04643 {
04644 std::vector<VisualWord*> removedWords = _vwd->getUnusedWords();
04645 UDEBUG("Removing %d words (dictionary size=%d)...", removedWords.size(), _vwd->getVisualWords().size());
04646 if(removedWords.size())
04647 {
04648
04649 _vwd->removeWords(removedWords);
04650
04651 for(unsigned int i=0; i<removedWords.size(); ++i)
04652 {
04653 if(_dbDriver)
04654 {
04655 _dbDriver->asyncSave(removedWords[i]);
04656 }
04657 else
04658 {
04659 delete removedWords[i];
04660 }
04661 }
04662 }
04663 }
04664
04665 void Memory::enableWordsRef(const std::list<int> & signatureIds)
04666 {
04667 UDEBUG("size=%d", signatureIds.size());
04668 UTimer timer;
04669 timer.start();
04670
04671 std::map<int, int> refsToChange;
04672
04673 std::set<int> oldWordIds;
04674 std::list<Signature *> surfSigns;
04675 for(std::list<int>::const_iterator i=signatureIds.begin(); i!=signatureIds.end(); ++i)
04676 {
04677 Signature * ss = dynamic_cast<Signature *>(this->_getSignature(*i));
04678 if(ss && !ss->isEnabled())
04679 {
04680 surfSigns.push_back(ss);
04681 std::list<int> uniqueKeys = uUniqueKeys(ss->getWords());
04682
04683
04684 for(std::list<int>::const_iterator k=uniqueKeys.begin(); k!=uniqueKeys.end(); ++k)
04685 {
04686 if(*k>0 && _vwd->getWord(*k) == 0 && _vwd->getUnusedWord(*k) == 0)
04687 {
04688 oldWordIds.insert(oldWordIds.end(), *k);
04689 }
04690 }
04691 }
04692 }
04693
04694 if(!_vwd->isIncremental() && oldWordIds.size())
04695 {
04696 UWARN("Dictionary is fixed, but some words retrieved have not been found!?");
04697 }
04698
04699 UDEBUG("oldWordIds.size()=%d, getOldIds time=%fs", oldWordIds.size(), timer.ticks());
04700
04701
04702 std::list<VisualWord *> vws;
04703 if(oldWordIds.size() && _dbDriver)
04704 {
04705
04706 _dbDriver->loadWords(oldWordIds, vws);
04707 }
04708 UDEBUG("loading words(%d) time=%fs", oldWordIds.size(), timer.ticks());
04709
04710
04711 if(vws.size())
04712 {
04713
04714 std::vector<int> vwActiveIds = _vwd->findNN(vws);
04715 UDEBUG("find active ids (number=%d) time=%fs", vws.size(), timer.ticks());
04716 int i=0;
04717 for(std::list<VisualWord *>::iterator iterVws=vws.begin(); iterVws!=vws.end(); ++iterVws)
04718 {
04719 if(vwActiveIds[i] > 0)
04720 {
04721
04722 refsToChange.insert(refsToChange.end(), std::pair<int, int>((*iterVws)->id(), vwActiveIds[i]));
04723 if((*iterVws)->isSaved())
04724 {
04725 delete (*iterVws);
04726 }
04727 else if(_dbDriver)
04728 {
04729 _dbDriver->asyncSave(*iterVws);
04730 }
04731 }
04732 else
04733 {
04734
04735 _vwd->addWord(*iterVws);
04736 }
04737 ++i;
04738 }
04739 UDEBUG("Added %d to dictionary, time=%fs", vws.size()-refsToChange.size(), timer.ticks());
04740
04741
04742 for(std::map<int, int>::const_iterator iter=refsToChange.begin(); iter != refsToChange.end(); ++iter)
04743 {
04744
04745 for(std::list<Signature *>::iterator j=surfSigns.begin(); j!=surfSigns.end(); ++j)
04746 {
04747 (*j)->changeWordsRef(iter->first, iter->second);
04748 }
04749 }
04750 UDEBUG("changing ref, total=%d, time=%fs", refsToChange.size(), timer.ticks());
04751 }
04752
04753 int count = _vwd->getTotalActiveReferences();
04754
04755
04756 for(std::list<Signature *>::iterator j=surfSigns.begin(); j!=surfSigns.end(); ++j)
04757 {
04758 const std::vector<int> & keys = uKeys((*j)->getWords());
04759 if(keys.size())
04760 {
04761
04762 for(unsigned int i=0; i<keys.size(); ++i)
04763 {
04764 if(keys.at(i)>0)
04765 {
04766 _vwd->addWordRef(keys.at(i), (*j)->id());
04767 }
04768 }
04769 (*j)->setEnabled(true);
04770 }
04771 }
04772
04773 count = _vwd->getTotalActiveReferences() - count;
04774 UDEBUG("%d words total ref added from %d signatures, time=%fs...", count, surfSigns.size(), timer.ticks());
04775 }
04776
04777 std::set<int> Memory::reactivateSignatures(const std::list<int> & ids, unsigned int maxLoaded, double & timeDbAccess)
04778 {
04779
04780
04781
04782
04783 UDEBUG("");
04784 UTimer timer;
04785 std::list<int> idsToLoad;
04786 std::map<int, int>::iterator wmIter;
04787 for(std::list<int>::const_iterator i=ids.begin(); i!=ids.end(); ++i)
04788 {
04789 if(!this->getSignature(*i) && !uContains(idsToLoad, *i))
04790 {
04791 if(!maxLoaded || idsToLoad.size() < maxLoaded)
04792 {
04793 idsToLoad.push_back(*i);
04794 UINFO("Loading location %d from database...", *i);
04795 }
04796 }
04797 }
04798
04799 UDEBUG("idsToLoad = %d", idsToLoad.size());
04800
04801 std::list<Signature *> reactivatedSigns;
04802 if(_dbDriver)
04803 {
04804 _dbDriver->loadSignatures(idsToLoad, reactivatedSigns);
04805 }
04806 timeDbAccess = timer.getElapsedTime();
04807 std::list<int> idsLoaded;
04808 for(std::list<Signature *>::iterator i=reactivatedSigns.begin(); i!=reactivatedSigns.end(); ++i)
04809 {
04810 idsLoaded.push_back((*i)->id());
04811
04812 this->addSignatureToWmFromLTM(*i);
04813 }
04814 this->enableWordsRef(idsLoaded);
04815 UDEBUG("time = %fs", timer.ticks());
04816 return std::set<int>(idsToLoad.begin(), idsToLoad.end());
04817 }
04818
04819
04820
04821 void Memory::getMetricConstraints(
04822 const std::set<int> & ids,
04823 std::map<int, Transform> & poses,
04824 std::multimap<int, Link> & links,
04825 bool lookInDatabase)
04826 {
04827 UDEBUG("");
04828 for(std::set<int>::const_iterator iter=ids.begin(); iter!=ids.end(); ++iter)
04829 {
04830 Transform pose = getOdomPose(*iter, lookInDatabase);
04831 if(!pose.isNull())
04832 {
04833 poses.insert(std::make_pair(*iter, pose));
04834 }
04835 }
04836
04837 for(std::set<int>::const_iterator iter=ids.begin(); iter!=ids.end(); ++iter)
04838 {
04839 if(uContains(poses, *iter))
04840 {
04841 std::map<int, Link> tmpLinks = getLinks(*iter, lookInDatabase);
04842 for(std::map<int, Link>::iterator jter=tmpLinks.begin(); jter!=tmpLinks.end(); ++jter)
04843 {
04844 if( jter->second.isValid() &&
04845 uContains(poses, jter->first) &&
04846 graph::findLink(links, *iter, jter->first) == links.end())
04847 {
04848 if(!lookInDatabase &&
04849 (jter->second.type() == Link::kNeighbor ||
04850 jter->second.type() == Link::kNeighborMerged))
04851 {
04852 Link link = jter->second;
04853 const Signature * s = this->getSignature(jter->first);
04854 UASSERT(s!=0);
04855 while(s && s->getWeight() == -1)
04856 {
04857
04858
04859 std::map<int, Link> n = this->getNeighborLinks(s->id(), false);
04860 UASSERT(n.size() <= 2);
04861 std::map<int, Link>::iterator uter = n.upper_bound(s->id());
04862 if(uter != n.end())
04863 {
04864 const Signature * s2 = this->getSignature(uter->first);
04865 if(s2)
04866 {
04867 link = link.merge(uter->second, uter->second.type());
04868 poses.erase(s->id());
04869 s = s2;
04870 }
04871
04872 }
04873 else
04874 {
04875 break;
04876 }
04877 }
04878
04879 links.insert(std::make_pair(*iter, link));
04880 }
04881 else
04882 {
04883 links.insert(std::make_pair(*iter, jter->second));
04884 }
04885 }
04886 }
04887 }
04888 }
04889 }
04890
04891 }