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