/* Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke All rights reserved. Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met: * Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. * Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the documentation and/or other materials provided with the distribution. * Neither the name of the Universite de Sherbrooke nor the names of its contributors may be used to endorse or promote products derived from this software without specific prior written permission. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ #include #include #include #include #include #include #include "rtabmap/core/Memory.h" #include "rtabmap/core/Signature.h" #include "rtabmap/core/Parameters.h" #include "rtabmap/core/RtabmapEvent.h" #include "rtabmap/core/VWDictionary.h" #include #include "VisualWord.h" #include "rtabmap/core/Features2d.h" #include "DBDriverSqlite3.h" #include "rtabmap/core/util3d_features.h" #include "rtabmap/core/util3d_filtering.h" #include "rtabmap/core/util3d_correspondences.h" #include "rtabmap/core/util3d_registration.h" #include "rtabmap/core/util3d_surface.h" #include "rtabmap/core/util3d_transforms.h" #include "rtabmap/core/util3d_motion_estimation.h" #include "rtabmap/core/util3d.h" #include "rtabmap/core/util2d.h" #include "rtabmap/core/Statistics.h" #include "rtabmap/core/Compression.h" #include "rtabmap/core/Graph.h" #include #include namespace rtabmap { const int Memory::kIdStart = 0; const int Memory::kIdVirtual = -1; const int Memory::kIdInvalid = 0; Memory::Memory(const ParametersMap & parameters) : _dbDriver(0), _similarityThreshold(Parameters::defaultMemRehearsalSimilarity()), _rawDataKept(Parameters::defaultMemImageKept()), _binDataKept(Parameters::defaultMemBinDataKept()), _notLinkedNodesKeptInDb(Parameters::defaultMemNotLinkedNodesKept()), _incrementalMemory(Parameters::defaultMemIncrementalMemory()), _maxStMemSize(Parameters::defaultMemSTMSize()), _recentWmRatio(Parameters::defaultMemRecentWmRatio()), _transferSortingByWeightId(Parameters::defaultMemTransferSortingByWeightId()), _idUpdatedToNewOneRehearsal(Parameters::defaultMemRehearsalIdUpdatedToNewOne()), _generateIds(Parameters::defaultMemGenerateIds()), _badSignaturesIgnored(Parameters::defaultMemBadSignaturesIgnored()), _imageDecimation(Parameters::defaultMemImageDecimation()), _laserScanVoxelSize(Parameters::defaultMemLaserScanVoxelSize()), _localSpaceLinksKeptInWM(Parameters::defaultMemLocalSpaceLinksKeptInWM()), _rehearsalMaxDistance(Parameters::defaultRGBDLinearUpdate()), _rehearsalMaxAngle(Parameters::defaultRGBDAngularUpdate()), _rehearsalWeightIgnoredWhileMoving(Parameters::defaultMemRehearsalWeightIgnoredWhileMoving()), _idCount(kIdStart), _idMapCount(kIdStart), _lastSignature(0), _lastGlobalLoopClosureId(0), _memoryChanged(false), _linksChanged(false), _signaturesAdded(0), _postInitClosingEvents(false), _featureType((Feature2D::Type)Parameters::defaultKpDetectorStrategy()), _badSignRatio(Parameters::defaultKpBadSignRatio()), _tfIdfLikelihoodUsed(Parameters::defaultKpTfIdfLikelihoodUsed()), _parallelized(Parameters::defaultKpParallelized()), _wordsMaxDepth(Parameters::defaultKpMaxDepth()), _roiRatios(std::vector(4, 0.0f)), _bowMinInliers(Parameters::defaultLccBowMinInliers()), _bowInlierDistance(Parameters::defaultLccBowInlierDistance()), _bowIterations(Parameters::defaultLccBowIterations()), _bowRefineIterations(Parameters::defaultLccBowRefineIterations()), _bowForce2D(Parameters::defaultLccBowForce2D()), _bowEpipolarGeometryVar(Parameters::defaultLccBowEpipolarGeometryVar()), _bowEstimationType(Parameters::defaultLccBowEstimationType()), _bowPnPReprojError(Parameters::defaultLccBowPnPReprojError()), _bowPnPFlags(Parameters::defaultLccBowPnPFlags()), _icpMaxTranslation(Parameters::defaultLccIcpMaxTranslation()), _icpMaxRotation(Parameters::defaultLccIcpMaxRotation()), _icpDecimation(Parameters::defaultLccIcp3Decimation()), _icpMaxDepth(Parameters::defaultLccIcp3MaxDepth()), _icpVoxelSize(Parameters::defaultLccIcp3VoxelSize()), _icpSamples(Parameters::defaultLccIcp3Samples()), _icpMaxCorrespondenceDistance(Parameters::defaultLccIcp3MaxCorrespondenceDistance()), _icpMaxIterations(Parameters::defaultLccIcp3Iterations()), _icpCorrespondenceRatio(Parameters::defaultLccIcp3CorrespondenceRatio()), _icpPointToPlane(Parameters::defaultLccIcp3PointToPlane()), _icpPointToPlaneNormalNeighbors(Parameters::defaultLccIcp3PointToPlaneNormalNeighbors()), _icp2MaxCorrespondenceDistance(Parameters::defaultLccIcp2MaxCorrespondenceDistance()), _icp2MaxIterations(Parameters::defaultLccIcp2Iterations()), _icp2CorrespondenceRatio(Parameters::defaultLccIcp2CorrespondenceRatio()), _icp2VoxelSize(Parameters::defaultLccIcp2VoxelSize()), _stereoFlowWinSize(Parameters::defaultStereoWinSize()), _stereoFlowIterations(Parameters::defaultStereoIterations()), _stereoFlowEpsilon(Parameters::defaultStereoEps()), _stereoFlowMaxLevel(Parameters::defaultStereoMaxLevel()), _stereoMaxSlope(Parameters::defaultStereoMaxSlope()), _subPixWinSize(Parameters::defaultKpSubPixWinSize()), _subPixIterations(Parameters::defaultKpSubPixIterations()), _subPixEps(Parameters::defaultKpSubPixEps()) { _feature2D = Feature2D::create(_featureType, parameters); _vwd = new VWDictionary(parameters); this->parseParameters(parameters); } bool Memory::init(const std::string & dbUrl, bool dbOverwritten, const ParametersMap & parameters, bool postInitClosingEvents) { _postInitClosingEvents = postInitClosingEvents; if(_postInitClosingEvents) UEventsManager::post(new RtabmapEventInit(RtabmapEventInit::kInitializing)); UDEBUG(""); this->parseParameters(parameters); bool loadAllNodesInWM = Parameters::defaultMemInitWMWithAllNodes(); Parameters::parse(parameters, Parameters::kMemInitWMWithAllNodes(), loadAllNodesInWM); if(_postInitClosingEvents) UEventsManager::post(new RtabmapEventInit("Clearing memory...")); DBDriver * tmpDriver = 0; if(!_memoryChanged && !_linksChanged) { if(_dbDriver) { tmpDriver = _dbDriver; _dbDriver = 0; // HACK for the clear() below to think that there is no db } } else if(!_memoryChanged && _linksChanged) { _dbDriver->setTimestampUpdateEnabled(false); // update links only } this->clear(); if(_postInitClosingEvents) UEventsManager::post(new RtabmapEventInit("Clearing memory, done!")); if(tmpDriver) { _dbDriver = tmpDriver; } if(_dbDriver) { if(_postInitClosingEvents) UEventsManager::post(new RtabmapEventInit("Closing database connection...")); _dbDriver->closeConnection(); if(_postInitClosingEvents) UEventsManager::post(new RtabmapEventInit("Closing database connection, done!")); } if(_dbDriver == 0 && !dbUrl.empty()) { _dbDriver = new DBDriverSqlite3(parameters); } bool success = true; if(_dbDriver) { _dbDriver->setTimestampUpdateEnabled(true); // make sure that timestamp update is enabled (may be disabled above) success = false; if(_postInitClosingEvents) UEventsManager::post(new RtabmapEventInit(std::string("Connecting to database ") + dbUrl + "...")); if(_dbDriver->openConnection(dbUrl, dbOverwritten)) { success = true; if(_postInitClosingEvents) UEventsManager::post(new RtabmapEventInit(std::string("Connecting to database ") + dbUrl + ", done!")); // Load the last working memory... std::list dbSignatures; if(loadAllNodesInWM) { if(_postInitClosingEvents) UEventsManager::post(new RtabmapEventInit(std::string("Loading all nodes to WM..."))); std::set ids; _dbDriver->getAllNodeIds(ids, true); _dbDriver->loadSignatures(std::list(ids.begin(), ids.end()), dbSignatures); } else { // load previous session working memory if(_postInitClosingEvents) UEventsManager::post(new RtabmapEventInit(std::string("Loading last nodes to WM..."))); _dbDriver->loadLastNodes(dbSignatures); } for(std::list::reverse_iterator iter=dbSignatures.rbegin(); iter!=dbSignatures.rend(); ++iter) { // ignore bad signatures if(!((*iter)->isBadSignature() && _badSignaturesIgnored)) { // insert all in WM // Note: it doesn't make sense to keep last STM images // of the last session in the new STM because they can be // only linked with the ones of the current session by // global loop closures. _signatures.insert(std::pair((*iter)->id(), *iter)); _workingMem.insert(std::make_pair((*iter)->id(), UTimer::now())); } else { delete *iter; } } if(_postInitClosingEvents) UEventsManager::post(new RtabmapEventInit(std::string("Loading nodes to WM, done! (") + uNumber2Str(int(_workingMem.size() + _stMem.size())) + " loaded)")); // Assign the last signature if(_stMem.size()>0) { _lastSignature = uValue(_signatures, *_stMem.rbegin(), (Signature*)0); } else if(_workingMem.size()>0) { _lastSignature = uValue(_signatures, _workingMem.rbegin()->first, (Signature*)0); } // Last id _dbDriver->getLastNodeId(_idCount); _idMapCount = _lastSignature?_lastSignature->mapId()+1:kIdStart; } else { if(_postInitClosingEvents) UEventsManager::post(new RtabmapEventInit(RtabmapEventInit::kError, std::string("Connecting to database ") + dbUrl + ", path is invalid!")); } } else { _idCount = kIdStart; _idMapCount = kIdStart; } _workingMem.insert(std::make_pair(kIdVirtual, 0)); UDEBUG("ids start with %d", _idCount+1); UDEBUG("map ids start with %d", _idMapCount); // Now load the dictionary if we have a connection if(_dbDriver && _dbDriver->isConnected()) { if(_postInitClosingEvents) UEventsManager::post(new RtabmapEventInit("Loading dictionary...")); if(loadAllNodesInWM) { // load all referenced words in working memory std::set wordIds; const std::map & signatures = this->getSignatures(); for(std::map::const_iterator i=signatures.begin(); i!=signatures.end(); ++i) { const std::multimap & words = i->second->getWords(); std::list keys = uUniqueKeys(words); wordIds.insert(keys.begin(), keys.end()); } if(wordIds.size()) { std::list words; _dbDriver->loadWords(wordIds, words); for(std::list::iterator iter = words.begin(); iter!=words.end(); ++iter) { _vwd->addWord(*iter); } // Get Last word id int id = 0; _dbDriver->getLastWordId(id); _vwd->setLastWordId(id); } } else { // load the last dictionary _dbDriver->load(_vwd); } UDEBUG("%d words loaded!", _vwd->getUnusedWordsSize()); _vwd->update(); if(_postInitClosingEvents) UEventsManager::post(new RtabmapEventInit(uFormat("Loading dictionary, done! (%d words)", (int)_vwd->getUnusedWordsSize()))); } if(_postInitClosingEvents) UEventsManager::post(new RtabmapEventInit(std::string("Adding word references..."))); // Enable loaded signatures const std::map & signatures = this->getSignatures(); for(std::map::const_iterator i=signatures.begin(); i!=signatures.end(); ++i) { Signature * s = this->_getSignature(i->first); UASSERT(s != 0); const std::multimap & words = s->getWords(); if(words.size()) { UDEBUG("node=%d, word references=%d", s->id(), words.size()); for(std::multimap::const_iterator iter = words.begin(); iter!=words.end(); ++iter) { _vwd->addWordRef(iter->first, i->first); } s->setEnabled(true); } } if(_postInitClosingEvents) UEventsManager::post(new RtabmapEventInit(uFormat("Adding word references, done! (%d)", _vwd->getTotalActiveReferences()))); if(_vwd->getUnusedWordsSize()) { UWARN("_vwd->getUnusedWordsSize() must be empty... size=%d", _vwd->getUnusedWordsSize()); } UDEBUG("Total word references added = %d", _vwd->getTotalActiveReferences()); if(_postInitClosingEvents) UEventsManager::post(new RtabmapEventInit(RtabmapEventInit::kInitialized)); return success; } Memory::~Memory() { if(_postInitClosingEvents) UEventsManager::post(new RtabmapEventInit(RtabmapEventInit::kClosing)); if(!_memoryChanged && !_linksChanged) { if(_postInitClosingEvents) UEventsManager::post(new RtabmapEventInit(uFormat("No changes added to database."))); UDEBUG(""); if(_dbDriver) { if(_postInitClosingEvents) UEventsManager::post(new RtabmapEventInit(uFormat("Closing database \"%s\"...", _dbDriver->getUrl().c_str()))); _dbDriver->closeConnection(); delete _dbDriver; _dbDriver = 0; if(_postInitClosingEvents) UEventsManager::post(new RtabmapEventInit("Closing database, done!")); } if(_postInitClosingEvents) UEventsManager::post(new RtabmapEventInit("Clearing memory...")); this->clear(); if(_postInitClosingEvents) UEventsManager::post(new RtabmapEventInit("Clearing memory, done!")); } else { UDEBUG(""); if(_postInitClosingEvents) UEventsManager::post(new RtabmapEventInit("Saving memory...")); if(!_memoryChanged && _linksChanged && _dbDriver) { // don't update the time stamps! UDEBUG(""); _dbDriver->setTimestampUpdateEnabled(false); } this->clear(); if(_dbDriver) { _dbDriver->emptyTrashes(); if(_postInitClosingEvents) UEventsManager::post(new RtabmapEventInit("Saving memory, done!")); if(_postInitClosingEvents) UEventsManager::post(new RtabmapEventInit(uFormat("Closing database \"%s\"...", _dbDriver->getUrl().c_str()))); _dbDriver->closeConnection(); delete _dbDriver; _dbDriver = 0; if(_postInitClosingEvents) UEventsManager::post(new RtabmapEventInit("Closing database, done!")); } else { if(_postInitClosingEvents) UEventsManager::post(new RtabmapEventInit("Saving memory, done!")); } } if(_feature2D) { delete _feature2D; } if(_vwd) { delete _vwd; } if(_postInitClosingEvents) UEventsManager::post(new RtabmapEventInit(RtabmapEventInit::kClosed)); } void Memory::parseParameters(const ParametersMap & parameters) { UDEBUG(""); ParametersMap::const_iterator iter; Parameters::parse(parameters, Parameters::kMemImageKept(), _rawDataKept); Parameters::parse(parameters, Parameters::kMemBinDataKept(), _binDataKept); Parameters::parse(parameters, Parameters::kMemNotLinkedNodesKept(), _notLinkedNodesKeptInDb); Parameters::parse(parameters, Parameters::kMemRehearsalIdUpdatedToNewOne(), _idUpdatedToNewOneRehearsal); Parameters::parse(parameters, Parameters::kMemGenerateIds(), _generateIds); Parameters::parse(parameters, Parameters::kMemBadSignaturesIgnored(), _badSignaturesIgnored); Parameters::parse(parameters, Parameters::kMemRehearsalSimilarity(), _similarityThreshold); Parameters::parse(parameters, Parameters::kMemRecentWmRatio(), _recentWmRatio); Parameters::parse(parameters, Parameters::kMemTransferSortingByWeightId(), _transferSortingByWeightId); Parameters::parse(parameters, Parameters::kMemSTMSize(), _maxStMemSize); Parameters::parse(parameters, Parameters::kMemImageDecimation(), _imageDecimation); Parameters::parse(parameters, Parameters::kMemLaserScanVoxelSize(), _laserScanVoxelSize); Parameters::parse(parameters, Parameters::kMemLocalSpaceLinksKeptInWM(), _localSpaceLinksKeptInWM); Parameters::parse(parameters, Parameters::kRGBDLinearUpdate(), _rehearsalMaxDistance); Parameters::parse(parameters, Parameters::kRGBDAngularUpdate(), _rehearsalMaxAngle); Parameters::parse(parameters, Parameters::kMemRehearsalWeightIgnoredWhileMoving(), _rehearsalWeightIgnoredWhileMoving); UASSERT_MSG(_maxStMemSize >= 0, uFormat("value=%d", _maxStMemSize).c_str()); UASSERT_MSG(_similarityThreshold >= 0.0f && _similarityThreshold <= 1.0f, uFormat("value=%f", _similarityThreshold).c_str()); UASSERT_MSG(_recentWmRatio >= 0.0f && _recentWmRatio <= 1.0f, uFormat("value=%f", _recentWmRatio).c_str()); UASSERT(_imageDecimation >= 1); // SLAM mode vs Localization mode iter = parameters.find(Parameters::kMemIncrementalMemory()); if(iter != parameters.end()) { bool value = uStr2Bool(iter->second.c_str()); if(value == false && _incrementalMemory) { // From SLAM to localization, change map id this->incrementMapId(); } _incrementalMemory = value; } if(_dbDriver) { _dbDriver->parseParameters(parameters); } Parameters::parse(parameters, Parameters::kLccBowMinInliers(), _bowMinInliers); Parameters::parse(parameters, Parameters::kLccBowInlierDistance(), _bowInlierDistance); Parameters::parse(parameters, Parameters::kLccBowIterations(), _bowIterations); Parameters::parse(parameters, Parameters::kLccBowRefineIterations(), _bowRefineIterations); Parameters::parse(parameters, Parameters::kLccBowForce2D(), _bowForce2D); Parameters::parse(parameters, Parameters::kLccBowEstimationType(), _bowEstimationType); Parameters::parse(parameters, Parameters::kLccBowEpipolarGeometryVar(), _bowEpipolarGeometryVar); Parameters::parse(parameters, Parameters::kLccBowPnPReprojError(), _bowPnPReprojError); Parameters::parse(parameters, Parameters::kLccBowPnPFlags(), _bowPnPFlags); Parameters::parse(parameters, Parameters::kLccIcpMaxTranslation(), _icpMaxTranslation); Parameters::parse(parameters, Parameters::kLccIcpMaxRotation(), _icpMaxRotation); Parameters::parse(parameters, Parameters::kLccIcp3Decimation(), _icpDecimation); Parameters::parse(parameters, Parameters::kLccIcp3MaxDepth(), _icpMaxDepth); Parameters::parse(parameters, Parameters::kLccIcp3VoxelSize(), _icpVoxelSize); Parameters::parse(parameters, Parameters::kLccIcp3Samples(), _icpSamples); Parameters::parse(parameters, Parameters::kLccIcp3MaxCorrespondenceDistance(), _icpMaxCorrespondenceDistance); Parameters::parse(parameters, Parameters::kLccIcp3Iterations(), _icpMaxIterations); Parameters::parse(parameters, Parameters::kLccIcp3CorrespondenceRatio(), _icpCorrespondenceRatio); Parameters::parse(parameters, Parameters::kLccIcp3PointToPlane(), _icpPointToPlane); Parameters::parse(parameters, Parameters::kLccIcp3PointToPlaneNormalNeighbors(), _icpPointToPlaneNormalNeighbors); Parameters::parse(parameters, Parameters::kLccIcp2MaxCorrespondenceDistance(), _icp2MaxCorrespondenceDistance); Parameters::parse(parameters, Parameters::kLccIcp2Iterations(), _icp2MaxIterations); Parameters::parse(parameters, Parameters::kLccIcp2CorrespondenceRatio(), _icp2CorrespondenceRatio); Parameters::parse(parameters, Parameters::kLccIcp2VoxelSize(), _icp2VoxelSize); //stereo Parameters::parse(parameters, Parameters::kStereoWinSize(), _stereoFlowWinSize); Parameters::parse(parameters, Parameters::kStereoIterations(), _stereoFlowIterations); Parameters::parse(parameters, Parameters::kStereoEps(), _stereoFlowEpsilon); Parameters::parse(parameters, Parameters::kStereoMaxLevel(), _stereoFlowMaxLevel); Parameters::parse(parameters, Parameters::kStereoMaxSlope(), _stereoMaxSlope); UASSERT_MSG(_bowMinInliers >= 1, uFormat("value=%d", _bowMinInliers).c_str()); UASSERT_MSG(_bowInlierDistance > 0.0f, uFormat("value=%f", _bowInlierDistance).c_str()); UASSERT_MSG(_bowIterations > 0, uFormat("value=%d", _bowIterations).c_str()); UASSERT_MSG(_icpDecimation > 0, uFormat("value=%d", _icpDecimation).c_str()); UASSERT_MSG(_icpMaxDepth >= 0.0f, uFormat("value=%f", _icpMaxDepth).c_str()); UASSERT_MSG(_icpVoxelSize >= 0, uFormat("value=%d", _icpVoxelSize).c_str()); UASSERT_MSG(_icpSamples >= 0, uFormat("value=%d", _icpSamples).c_str()); UASSERT_MSG(_icpMaxCorrespondenceDistance > 0.0f, uFormat("value=%f", _icpMaxCorrespondenceDistance).c_str()); UASSERT_MSG(_icpMaxIterations > 0, uFormat("value=%d", _icpMaxIterations).c_str()); UASSERT_MSG(_icpCorrespondenceRatio >=0.0f && _icpCorrespondenceRatio <=1.0f, uFormat("value=%f", _icpCorrespondenceRatio).c_str()); UASSERT_MSG(_icpPointToPlaneNormalNeighbors > 0, uFormat("value=%d", _icpPointToPlaneNormalNeighbors).c_str()); UASSERT_MSG(_icp2MaxCorrespondenceDistance > 0.0f, uFormat("value=%f", _icp2MaxCorrespondenceDistance).c_str()); UASSERT_MSG(_icp2MaxIterations > 0, uFormat("value=%d", _icp2MaxIterations).c_str()); UASSERT_MSG(_icp2CorrespondenceRatio >=0.0f && _icp2CorrespondenceRatio <=1.0f, uFormat("value=%f", _icp2CorrespondenceRatio).c_str()); UASSERT_MSG(_icp2VoxelSize >= 0, uFormat("value=%d", _icp2VoxelSize).c_str()); // Keypoint stuff if(_vwd) { _vwd->parseParameters(parameters); } Parameters::parse(parameters, Parameters::kKpTfIdfLikelihoodUsed(), _tfIdfLikelihoodUsed); Parameters::parse(parameters, Parameters::kKpParallelized(), _parallelized); Parameters::parse(parameters, Parameters::kKpBadSignRatio(), _badSignRatio); Parameters::parse(parameters, Parameters::kKpMaxDepth(), _wordsMaxDepth); Parameters::parse(parameters, Parameters::kKpSubPixWinSize(), _subPixWinSize); Parameters::parse(parameters, Parameters::kKpSubPixIterations(), _subPixIterations); Parameters::parse(parameters, Parameters::kKpSubPixEps(), _subPixEps); if((iter=parameters.find(Parameters::kKpRoiRatios())) != parameters.end()) { this->setRoi((*iter).second); } //Keypoint detector UASSERT(_feature2D != 0); Feature2D::Type detectorStrategy = Feature2D::kFeatureUndef; if((iter=parameters.find(Parameters::kKpDetectorStrategy())) != parameters.end()) { detectorStrategy = (Feature2D::Type)std::atoi((*iter).second.c_str()); } if(detectorStrategy!=Feature2D::kFeatureUndef) { UDEBUG("new detector strategy %d", int(detectorStrategy)); if(_feature2D) { delete _feature2D; _feature2D = 0; _featureType = Feature2D::kFeatureUndef; } _feature2D = Feature2D::create(detectorStrategy, parameters); _featureType = detectorStrategy; } else if(_feature2D) { _feature2D->parseParameters(parameters); } } void Memory::preUpdate() { _signaturesAdded = 0; this->cleanUnusedWords(); if(_vwd && !_parallelized) { //When parallelized, it is done in CreateSignature _vwd->update(); } } bool Memory::update( const SensorData & data, Statistics * stats) { return update(data, Transform(), cv::Mat(), stats); } bool Memory::update( const SensorData & data, const Transform & pose, const cv::Mat & covariance, Statistics * stats) { UDEBUG(""); UTimer timer; UTimer totalTimer; timer.start(); float t; //============================================================ // Pre update... //============================================================ UDEBUG("pre-updating..."); this->preUpdate(); t=timer.ticks()*1000; if(stats) stats->addStatistic(Statistics::kTimingMemPre_update(), t); UDEBUG("time preUpdate=%f ms", t); //============================================================ // Create a signature with the image received. //============================================================ Signature * signature = this->createSignature(data, pose, stats); if (signature == 0) { UERROR("Failed to create a signature..."); return false; } t=timer.ticks()*1000; if(stats) stats->addStatistic(Statistics::kTimingMemSignature_creation(), t); UDEBUG("time creating signature=%f ms", t); // It will be added to the short-term memory, no need to delete it... this->addSignatureToStm(signature, covariance); _lastSignature = signature; //============================================================ // Rehearsal step... // Compare with the X last signatures. If different, add this // signature like a parent to the memory tree, otherwise add // it as a child to the similar signature. //============================================================ if(_incrementalMemory) { if(_similarityThreshold < 1.0f) { this->rehearsal(signature, stats); } t=timer.ticks()*1000; if(stats) stats->addStatistic(Statistics::kTimingMemRehearsal(), t); UDEBUG("time rehearsal=%f ms", t); } else { if(_workingMem.size() <= 1) { UWARN("The working memory is empty and the memory is not " "incremental (Mem/IncrementalMemory=False), no loop closure " "can be detected! Please set Mem/IncrementalMemory=true to increase " "the memory with new images or decrease the STM size (which is %d " "including the new one added).", (int)_stMem.size()); } } //============================================================ // Transfer the oldest signature of the short-term memory to the working memory //============================================================ int validSignaturesCount = 0; for(std::set::iterator iter=_stMem.begin(); iter!=_stMem.end(); ++iter) { const Signature * s = this->getSignature(*iter); UASSERT(s != 0); if(!s->isBadSignature()) { ++validSignaturesCount; } } while(_stMem.size() && _maxStMemSize>0 && validSignaturesCount > _maxStMemSize) { UDEBUG("Inserting node %d from STM in WM...", *_stMem.begin()); Signature * s = this->_getSignature(*_stMem.begin()); if(!_localSpaceLinksKeptInWM) { // remove local space links outside STM UASSERT(s!=0); std::map links = s->getLinks(); // get a copy because we will remove some links in "s" for(std::map::iterator iter=links.begin(); iter!=links.end(); ++iter) { if(iter->second.type() == Link::kLocalSpaceClosure) { Signature * sTo = this->_getSignature(iter->first); if(sTo) { sTo->removeLink(s->id()); } else { UERROR("Link %d of %d not in WM/STM?!?", iter->first, s->id()); } s->removeLink(iter->first); } } } if(!s->isBadSignature()) { --validSignaturesCount; } _workingMem.insert(_workingMem.end(), std::make_pair(*_stMem.begin(), UTimer::now())); _stMem.erase(*_stMem.begin()); ++_signaturesAdded; } if(!_memoryChanged && _incrementalMemory) { _memoryChanged = true; } UDEBUG("totalTimer = %fs", totalTimer.ticks()); return true; } void Memory::setRoi(const std::string & roi) { std::list strValues = uSplit(roi, ' '); if(strValues.size() != 4) { ULOGGER_ERROR("The number of values must be 4 (roi=\"%s\")", roi.c_str()); } else { std::vector tmpValues(4); unsigned int i=0; for(std::list::iterator iter = strValues.begin(); iter!=strValues.end(); ++iter) { tmpValues[i] = uStr2Float(*iter); ++i; } if(tmpValues[0] >= 0 && tmpValues[0] < 1 && tmpValues[0] < 1.0f-tmpValues[1] && tmpValues[1] >= 0 && tmpValues[1] < 1 && tmpValues[1] < 1.0f-tmpValues[0] && tmpValues[2] >= 0 && tmpValues[2] < 1 && tmpValues[2] < 1.0f-tmpValues[3] && tmpValues[3] >= 0 && tmpValues[3] < 1 && tmpValues[3] < 1.0f-tmpValues[2]) { _roiRatios = tmpValues; } else { ULOGGER_ERROR("The roi ratios are not valid (roi=\"%s\")", roi.c_str()); } } } void Memory::addSignatureToStm(Signature * signature, const cv::Mat & covariance) { UTimer timer; // add signature on top of the short-term memory if(signature) { UDEBUG("adding %d", signature->id()); // Update neighbors if(_stMem.size()) { if(_signatures.at(*_stMem.rbegin())->mapId() == signature->mapId()) { Transform motionEstimate; if(!signature->getPose().isNull() && !_signatures.at(*_stMem.rbegin())->getPose().isNull()) { cv::Mat infMatrix = covariance.inv(); motionEstimate = _signatures.at(*_stMem.rbegin())->getPose().inverse() * signature->getPose(); _signatures.at(*_stMem.rbegin())->addLink(Link(*_stMem.rbegin(), signature->id(), Link::kNeighbor, motionEstimate, infMatrix)); signature->addLink(Link(signature->id(), *_stMem.rbegin(), Link::kNeighbor, motionEstimate.inverse(), infMatrix)); } else { _signatures.at(*_stMem.rbegin())->addLink(Link(*_stMem.rbegin(), signature->id(), Link::kNeighbor, Transform())); signature->addLink(Link(signature->id(), *_stMem.rbegin(), Link::kNeighbor, Transform())); } UDEBUG("Min STM id = %d", *_stMem.begin()); } else { UDEBUG("Ignoring neighbor link between %d and %d because they are not in the same map! (%d vs %d)", *_stMem.rbegin(), signature->id(), _signatures.at(*_stMem.rbegin())->mapId(), signature->mapId()); //Tag the first node of the map std::string tag = uFormat("map%d", signature->mapId()); if(getSignatureIdByLabel(tag, false) == 0) { UINFO("Tagging node %d with label \"%s\"", signature->id(), tag.c_str()); signature->setLabel(tag); } } } else { //Tag the first node of the map std::string tag = uFormat("map%d", signature->mapId()); if(getSignatureIdByLabel(tag, false) == 0) { UINFO("Tagging node %d with label \"%s\"", signature->id(), tag.c_str()); signature->setLabel(tag); } } _signatures.insert(_signatures.end(), std::pair(signature->id(), signature)); _stMem.insert(_stMem.end(), signature->id()); if(_vwd) { UDEBUG("%d words ref for the signature %d", signature->getWords().size(), signature->id()); } if(signature->getWords().size()) { signature->setEnabled(true); } } UDEBUG("time = %fs", timer.ticks()); } void Memory::addSignatureToWm(Signature * signature) { if(signature) { UDEBUG("Inserting node %d in WM...", signature->id()); _workingMem.insert(std::make_pair(signature->id(), UTimer::now())); _signatures.insert(std::pair(signature->id(), signature)); ++_signaturesAdded; } else { UERROR("Signature is null ?!?"); } } const Signature * Memory::getSignature(int id) const { return _getSignature(id); } Signature * Memory::_getSignature(int id) const { return uValue(_signatures, id, (Signature*)0); } const VWDictionary * Memory::getVWDictionary() const { return _vwd; } std::map Memory::getNeighborLinks( int signatureId, bool lookInDatabase) const { std::map links; Signature * s = uValue(_signatures, signatureId, (Signature*)0); if(s) { const std::map & allLinks = s->getLinks(); for(std::map::const_iterator iter = allLinks.begin(); iter!=allLinks.end(); ++iter) { if(iter->second.type() == Link::kNeighbor) { links.insert(*iter); } } } else if(lookInDatabase && _dbDriver) { std::map neighbors; _dbDriver->loadLinks(signatureId, neighbors, Link::kNeighbor); links.insert(neighbors.begin(), neighbors.end()); } else { UWARN("Cannot find signature %d in memory", signatureId); } return links; } std::map Memory::getLoopClosureLinks( int signatureId, bool lookInDatabase) const { const Signature * s = this->getSignature(signatureId); std::map loopClosures; if(s) { const std::map & allLinks = s->getLinks(); for(std::map::const_iterator iter = allLinks.begin(); iter!=allLinks.end(); ++iter) { if(iter->second.type() > Link::kNeighbor && iter->second.type() != Link::kUndef) { loopClosures.insert(*iter); } } } else if(lookInDatabase && _dbDriver) { _dbDriver->loadLinks(signatureId, loopClosures); for(std::map::iterator iter=loopClosures.begin(); iter!=loopClosures.end();) { if(iter->second.type() == Link::kNeighbor) { loopClosures.erase(iter++); } else { ++iter; } } } return loopClosures; } std::map Memory::getLinks( int signatureId, bool lookInDatabase) const { std::map links; Signature * s = uValue(_signatures, signatureId, (Signature*)0); if(s) { links = s->getLinks(); } else if(lookInDatabase && _dbDriver) { _dbDriver->loadLinks(signatureId, links, Link::kUndef); } else { UWARN("Cannot find signature %d in memory", signatureId); } return links; } std::multimap Memory::getAllLinks(bool lookInDatabase, bool ignoreNullLinks) const { std::multimap links; if(lookInDatabase && _dbDriver) { _dbDriver->getAllLinks(links, ignoreNullLinks); } for(std::map::const_iterator iter=_signatures.begin(); iter!=_signatures.end(); ++iter) { links.erase(iter->first); for(std::map::const_iterator jter=iter->second->getLinks().begin(); jter!=iter->second->getLinks().end(); ++jter) { if(!ignoreNullLinks || jter->second.isValid()) { links.insert(std::make_pair(iter->first, jter->second)); } } } return links; } // return map, including signatureId // maxCheckedInDatabase = -1 means no limit to check in database (default) // maxCheckedInDatabase = 0 means don't check in database std::map Memory::getNeighborsId(int signatureId, int maxGraphDepth, // 0 means infinite margin int maxCheckedInDatabase, // default -1 (no limit) bool incrementMarginOnLoop, // default false bool ignoreLoopIds, // default false bool ignoreIntermediateNodes, // default false double * dbAccessTime ) const { UASSERT(maxGraphDepth >= 0); //UDEBUG("signatureId=%d, neighborsMargin=%d", signatureId, margin); if(dbAccessTime) { *dbAccessTime = 0; } std::map ids; if(signatureId<=0) { return ids; } int nbLoadedFromDb = 0; std::list curentMarginList; std::set currentMargin; std::set nextMargin; nextMargin.insert(signatureId); int m = 0; std::set ignoredIds; while((maxGraphDepth == 0 || m < maxGraphDepth) && nextMargin.size()) { // insert more recent first (priority to be loaded first from the database below if set) curentMarginList = std::list(nextMargin.rbegin(), nextMargin.rend()); nextMargin.clear(); for(std::list::iterator jter = curentMarginList.begin(); jter!=curentMarginList.end(); ++jter) { if(ids.find(*jter) == ids.end()) { //UDEBUG("Added %d with margin %d", *jter, m); // Look up in STM/WM if all ids are here, if not... load them from the database const Signature * s = this->getSignature(*jter); std::map tmpLinks; const std::map * links = &tmpLinks; if(s) { if(!ignoreIntermediateNodes || s->getWeight() != -1) { ids.insert(std::pair(*jter, m)); } else { ignoredIds.insert(*jter); } links = &s->getLinks(); } else if(maxCheckedInDatabase == -1 || (maxCheckedInDatabase > 0 && _dbDriver && nbLoadedFromDb < maxCheckedInDatabase)) { ++nbLoadedFromDb; ids.insert(std::pair(*jter, m)); UTimer timer; _dbDriver->loadLinks(*jter, tmpLinks); if(dbAccessTime) { *dbAccessTime += timer.getElapsedTime(); } } // links for(std::map::const_iterator iter=links->begin(); iter!=links->end(); ++iter) { if( !uContains(ids, iter->first) && ignoredIds.find(iter->first) == ignoredIds.end()) { UASSERT(iter->second.type() != Link::kUndef); if(iter->second.type() == Link::kNeighbor) { if(ignoreIntermediateNodes && s->getWeight()==-1) { // stay on the same margin if(currentMargin.insert(iter->first).second) { curentMarginList.push_back(iter->first); } } else { nextMargin.insert(iter->first); } } else if(!ignoreLoopIds) { if(incrementMarginOnLoop) { nextMargin.insert(iter->first); } else { if(currentMargin.insert(iter->first).second) { curentMarginList.push_back(iter->first); } } } } } } } ++m; } return ids; } // return map, including signatureId std::map Memory::getNeighborsIdRadius( int signatureId, float radius, // 0 means ignore radius const std::map & optimizedPoses, int maxGraphDepth // 0 means infinite margin ) const { UASSERT(maxGraphDepth >= 0); UASSERT(uContains(optimizedPoses, signatureId)); UASSERT(signatureId > 0); std::map ids; std::list curentMarginList; std::set currentMargin; std::set nextMargin; nextMargin.insert(signatureId); int m = 0; Transform referential = optimizedPoses.at(signatureId); UASSERT(!referential.isNull()); float radiusSqrd = radius*radius; std::map savedRadius; savedRadius.insert(std::make_pair(signatureId, 0)); while((maxGraphDepth == 0 || m < maxGraphDepth) && nextMargin.size()) { curentMarginList = std::list(nextMargin.begin(), nextMargin.end()); nextMargin.clear(); for(std::list::iterator jter = curentMarginList.begin(); jter!=curentMarginList.end(); ++jter) { if(ids.find(*jter) == ids.end()) { //UDEBUG("Added %d with margin %d", *jter, m); // Look up in STM/WM if all ids are here, if not... load them from the database const Signature * s = this->getSignature(*jter); std::map tmpLinks; const std::map * links = &tmpLinks; if(s) { ids.insert(std::pair(*jter, savedRadius.at(*jter))); links = &s->getLinks(); } // links for(std::map::const_iterator iter=links->begin(); iter!=links->end(); ++iter) { if(!uContains(ids, iter->first) && uContains(optimizedPoses, iter->first)) { const Transform & t = optimizedPoses.at(iter->first); UASSERT(!t.isNull()); float distanceSqrd = referential.getDistanceSquared(t); if(radiusSqrd == 0 || distanceSqrdfirst, distanceSqrd)); nextMargin.insert(iter->first); } } } } } ++m; } return ids; } int Memory::getNextId() { return ++_idCount; } int Memory::incrementMapId() { //don't increment if there is no location in the current map const Signature * s = getLastWorkingSignature(); if(s && s->mapId() == _idMapCount) { // New session! move all signatures from the STM to WM while(_stMem.size()) { UDEBUG("Inserting node %d from STM in WM...", *_stMem.begin()); if(!_localSpaceLinksKeptInWM) { // remove local space links outside STM Signature * s = this->_getSignature(*_stMem.begin()); UASSERT(s!=0); std::map links = s->getLinks(); // get a copy because we will remove some links in "s" for(std::map::iterator iter=links.begin(); iter!=links.end(); ++iter) { if(iter->second.type() == Link::kLocalSpaceClosure) { Signature * sTo = this->_getSignature(iter->first); if(sTo) { sTo->removeLink(s->id()); } else { UERROR("Link %d of %d not in WM/STM?!?", iter->first, s->id()); } s->removeLink(iter->first); } } } _workingMem.insert(_workingMem.end(), std::make_pair(*_stMem.begin(), UTimer::now())); _stMem.erase(*_stMem.begin()); } return ++_idMapCount; } return _idMapCount; } void Memory::updateAge(int signatureId) { std::map::iterator iter=_workingMem.find(signatureId); if(iter!=_workingMem.end()) { iter->second = UTimer::now(); } } int Memory::getDatabaseMemoryUsed() const { int memoryUsed = 0; if(_dbDriver) { memoryUsed = _dbDriver->getMemoryUsed()/(1024*1024); //Byte to MB } return memoryUsed; } std::string Memory::getDatabaseVersion() const { std::string version = "0.0.0"; if(_dbDriver) { version = _dbDriver->getDatabaseVersion(); } return version; } double Memory::getDbSavingTime() const { return _dbDriver?_dbDriver->getEmptyTrashesTime():0; } std::set Memory::getAllSignatureIds() const { std::set ids; if(_dbDriver) { _dbDriver->getAllNodeIds(ids); } for(std::map::const_iterator iter = _signatures.begin(); iter!=_signatures.end(); ++iter) { ids.insert(iter->first); } return ids; } void Memory::clear() { UDEBUG(""); this->cleanUnusedWords(); if(_dbDriver) { _dbDriver->emptyTrashes(); _dbDriver->join(); } // Save some stats to the db, save only when the mem is not empty if(_dbDriver && (_stMem.size() || _workingMem.size())) { unsigned int memSize = (unsigned int)(_workingMem.size() + _stMem.size()); if(_workingMem.size() && _workingMem.begin()->first < 0) { --memSize; } // this is only a safe check...not supposed to occur. UASSERT_MSG(memSize == _signatures.size(), uFormat("The number of signatures don't match! _workingMem=%d, _stMem=%d, _signatures=%d", _workingMem.size(), _stMem.size(), _signatures.size()).c_str()); UDEBUG("Adding statistics after run..."); if(_memoryChanged) { UDEBUG(""); _dbDriver->addStatisticsAfterRun(memSize, _lastSignature?_lastSignature->id():0, UProcessInfo::getMemoryUsage(), _dbDriver->getMemoryUsed(), (int)_vwd->getVisualWords().size()); } } UDEBUG(""); //Get the tree root (parents) std::map mem = _signatures; for(std::map::iterator i=mem.begin(); i!=mem.end(); ++i) { if(i->second) { UDEBUG("deleting from the working and the short-term memory: %d", i->first); this->moveToTrash(i->second); } } if(_workingMem.size() != 0 && !(_workingMem.size() == 1 && _workingMem.begin()->first == kIdVirtual)) { ULOGGER_ERROR("_workingMem must be empty here, size=%d", _workingMem.size()); } _workingMem.clear(); if(_stMem.size() != 0) { ULOGGER_ERROR("_stMem must be empty here, size=%d", _stMem.size()); } _stMem.clear(); if(_signatures.size()!=0) { ULOGGER_ERROR("_signatures must be empty here, size=%d", _signatures.size()); } _signatures.clear(); UDEBUG(""); // Wait until the db trash has finished cleaning the memory if(_dbDriver) { _dbDriver->emptyTrashes(); } UDEBUG(""); _lastSignature = 0; _lastGlobalLoopClosureId = 0; _idCount = kIdStart; _idMapCount = kIdStart; _memoryChanged = false; _linksChanged = false; if(_dbDriver) { _dbDriver->join(true); cleanUnusedWords(); _dbDriver->emptyTrashes(); } else { cleanUnusedWords(); } if(_vwd) { _vwd->clear(); } UDEBUG(""); } /** * Compute the likelihood of the signature with some others in the memory. * Important: Assuming that all other ids are under 'signature' id. * If an error occurs, the result is empty. */ std::map Memory::computeLikelihood(const Signature * signature, const std::list & ids) { if(!_tfIdfLikelihoodUsed) { UTimer timer; timer.start(); std::map likelihood; if(!signature) { ULOGGER_ERROR("The signature is null"); return likelihood; } else if(ids.empty()) { UWARN("ids list is empty"); return likelihood; } for(std::list::const_iterator iter = ids.begin(); iter!=ids.end(); ++iter) { float sim = 0.0f; if(*iter > 0) { const Signature * sB = this->getSignature(*iter); if(!sB) { UFATAL("Signature %d not found in WM ?!?", *iter); } sim = signature->compareTo(*sB); } likelihood.insert(likelihood.end(), std::pair(*iter, sim)); } UDEBUG("compute likelihood (similarity)... %f s", timer.ticks()); return likelihood; } else { UTimer timer; timer.start(); std::map likelihood; std::map calculatedWordsRatio; if(!signature) { ULOGGER_ERROR("The signature is null"); return likelihood; } else if(ids.empty()) { UWARN("ids list is empty"); return likelihood; } for(std::list::const_iterator iter = ids.begin(); iter!=ids.end(); ++iter) { likelihood.insert(likelihood.end(), std::pair(*iter, 0.0f)); } const std::list & wordIds = uUniqueKeys(signature->getWords()); float nwi; // nwi is the number of a specific word referenced by a place float ni; // ni is the total of words referenced by a place float nw; // nw is the number of places referenced by a specific word float N; // N is the total number of places float logNnw; const VisualWord * vw; N = this->getSignatures().size(); if(N) { UDEBUG("processing... "); // Pour chaque mot dans la signature SURF for(std::list::const_iterator i=wordIds.begin(); i!=wordIds.end(); ++i) { // "Inverted index" - Pour chaque endroit contenu dans chaque mot vw = _vwd->getWord(*i); if(vw) { const std::map & refs = vw->getReferences(); nw = refs.size(); if(nw) { logNnw = log10(N/nw); if(logNnw) { for(std::map::const_iterator j=refs.begin(); j!=refs.end(); ++j) { std::map::iterator iter = likelihood.find(j->first); if(iter != likelihood.end()) { nwi = j->second; ni = this->getNi(j->first); if(ni != 0) { //UDEBUG("%d, %f %f %f %f", vw->id(), logNnw, nwi, ni, ( nwi * logNnw ) / ni); iter->second += ( nwi * logNnw ) / ni; } } } } } } } } UDEBUG("compute likelihood (tf-idf) %f s", timer.ticks()); return likelihood; } } // Weights of the signatures in the working memory std::map Memory::getWeights() const { std::map weights; for(std::map::const_iterator iter=_workingMem.begin(); iter!=_workingMem.end(); ++iter) { if(iter->first > 0) { const Signature * s = this->getSignature(iter->first); if(!s) { UFATAL("Location %d must exist in memory", iter->first); } weights.insert(weights.end(), std::make_pair(iter->first, s->getWeight())); } else { weights.insert(weights.end(), std::make_pair(iter->first, -1)); } } return weights; } std::list Memory::forget(const std::set & ignoredIds) { UDEBUG(""); std::list signaturesRemoved; if(_vwd->isIncremental() && _vwd->getVisualWords().size()) { int newWords = 0; int wordsRemoved = 0; // Get how many new words added for the last run... newWords = _vwd->getNotIndexedWordsCount(); // So we need to remove at least "newWords" words from the // dictionary to respect the limit. while(wordsRemoved < newWords) { std::list signatures = this->getRemovableSignatures(1, ignoredIds); if(signatures.size()) { Signature * s = dynamic_cast(signatures.front()); if(s) { signaturesRemoved.push_back(s->id()); this->moveToTrash(s); wordsRemoved = _vwd->getUnusedWordsSize(); } else { break; } } else { break; } } UDEBUG("newWords=%d, wordsRemoved=%d", newWords, wordsRemoved); } else { UDEBUG(""); // Remove one more than total added during the iteration std::list signatures = getRemovableSignatures(_signaturesAdded+1, ignoredIds); for(std::list::iterator iter=signatures.begin(); iter!=signatures.end(); ++iter) { signaturesRemoved.push_back((*iter)->id()); // When a signature is deleted, it notifies the memory // and it is removed from the memory list this->moveToTrash(*iter); } UDEBUG("signaturesRemoved=%d, _signaturesAdded=%d", (int)signatures.size(), _signaturesAdded); } return signaturesRemoved; } int Memory::cleanup() { UDEBUG(""); int signatureRemoved = 0; // bad signature if(_lastSignature && ((_lastSignature->isBadSignature() && _badSignaturesIgnored) || !_incrementalMemory)) { if(_lastSignature->isBadSignature()) { UDEBUG("Bad signature! %d", _lastSignature->id()); } signatureRemoved = _lastSignature->id(); moveToTrash(_lastSignature, _incrementalMemory); } return signatureRemoved; } void Memory::emptyTrash() { if(_dbDriver) { _dbDriver->emptyTrashes(true); } } void Memory::joinTrashThread() { if(_dbDriver) { UDEBUG(""); _dbDriver->join(); UDEBUG(""); } } class WeightAgeIdKey { public: WeightAgeIdKey(int w, double a, int i) : weight(w), age(a), id(i){} bool operator<(const WeightAgeIdKey & k) const { if(weight < k.weight) { return true; } else if(weight == k.weight) { if(age < k.age) { return true; } else if(age == k.age) { if(id < k.id) { return true; } } } return false; } int weight, age, id; }; std::list Memory::getRemovableSignatures(int count, const std::set & ignoredIds) { //UDEBUG(""); std::list removableSignatures; std::map weightAgeIdMap; // Find the last index to check... UDEBUG("mem.size()=%d, ignoredIds.size()=%d", (int)_workingMem.size(), (int)ignoredIds.size()); if(_workingMem.size()) { int recentWmMaxSize = _recentWmRatio * float(_workingMem.size()); bool recentWmImmunized = false; // look for the position of the lastLoopClosureId in WM int currentRecentWmSize = 0; if(_lastGlobalLoopClosureId > 0 && _stMem.find(_lastGlobalLoopClosureId) == _stMem.end()) { // If set, it must be in WM std::map::const_iterator iter = _workingMem.find(_lastGlobalLoopClosureId); while(iter != _workingMem.end()) { ++currentRecentWmSize; ++iter; } if(currentRecentWmSize>1 && currentRecentWmSize < recentWmMaxSize) { recentWmImmunized = true; } else if(currentRecentWmSize == 0 && _workingMem.size() > 1) { UERROR("Last loop closure id not found in WM (%d)", _lastGlobalLoopClosureId); } UDEBUG("currentRecentWmSize=%d, recentWmMaxSize=%d, _recentWmRatio=%f, end recent wM = %d", currentRecentWmSize, recentWmMaxSize, _recentWmRatio, _lastGlobalLoopClosureId); } // Ignore neighbor of the last location in STM (for neighbor links redirection issue during Rehearsal). Signature * lastInSTM = 0; if(_stMem.size()) { lastInSTM = _signatures.at(*_stMem.begin()); } for(std::map::const_iterator memIter = _workingMem.begin(); memIter != _workingMem.end(); ++memIter) { if( (recentWmImmunized && memIter->first > _lastGlobalLoopClosureId) || memIter->first == _lastGlobalLoopClosureId) { // ignore recent memory } else if(memIter->first > 0 && ignoredIds.find(memIter->first) == ignoredIds.end() && (!lastInSTM || !lastInSTM->hasLink(memIter->first))) { Signature * s = this->_getSignature(memIter->first); if(s) { // Links must not be in STM to be removable, rehearsal issue bool foundInSTM = false; for(std::map::const_iterator iter = s->getLinks().begin(); iter!=s->getLinks().end(); ++iter) { if(_stMem.find(iter->first) != _stMem.end()) { UDEBUG("Ignored %d because it has a link (%d) to STM", s->id(), iter->first); foundInSTM = true; break; } } if(!foundInSTM) { // less weighted signature priority to be transferred weightAgeIdMap.insert(std::make_pair(WeightAgeIdKey(s->getWeight(), _transferSortingByWeightId?0.0:memIter->second, s->id()), s)); } } else { ULOGGER_ERROR("Not supposed to occur!!!"); } } else { //UDEBUG("Ignoring id %d", memIter->first); } } int recentWmCount = 0; // make the list of removable signatures // Criteria : Weight -> ID UDEBUG("signatureMap.size()=%d", (int)weightAgeIdMap.size()); for(std::map::iterator iter=weightAgeIdMap.begin(); iter!=weightAgeIdMap.end(); ++iter) { bool removable = true; if(removable) { if(!recentWmImmunized) { UDEBUG("weight=%d, id=%d", iter->second->getWeight(), iter->second->id()); removableSignatures.push_back(iter->second); if(iter->second->id() > _lastGlobalLoopClosureId) { ++recentWmCount; if(currentRecentWmSize - recentWmCount < recentWmMaxSize) { UDEBUG("switched recentWmImmunized"); recentWmImmunized = true; } } } else if(iter->second->id() < _lastGlobalLoopClosureId) { UDEBUG("weight=%d, id=%d", iter->second->getWeight(), iter->second->id()); removableSignatures.push_back(iter->second); } if(removableSignatures.size() >= (unsigned int)count) { break; } } } } else { ULOGGER_WARN("not enough signatures to get an old one..."); } return removableSignatures; } /** * If saveToDatabase=false, deleted words are filled in deletedWords. */ void Memory::moveToTrash(Signature * s, bool keepLinkedToGraph, std::list * deletedWords) { UDEBUG("id=%d", s?s->id():0); if(s) { // If not saved to database or it is a bad signature (not saved), remove links! if(!keepLinkedToGraph || (!s->isSaved() && s->isBadSignature() && _badSignaturesIgnored)) { UASSERT_MSG(this->isInSTM(s->id()), uFormat("Deleting location (%d) outside the STM is not implemented!", s->id()).c_str()); const std::map & links = s->getLinks(); for(std::map::const_iterator iter=links.begin(); iter!=links.end(); ++iter) { Signature * sTo = this->_getSignature(iter->first); // neighbor to s if(sTo) { if(iter->first > s->id() && (sTo->getLinks().size() == 1 || !sTo->hasLink(s->id()))) { UWARN("Link %d of %d is newer, removing neighbor link may split the map!", iter->first, s->id()); } // child if(iter->second.type() == Link::kGlobalClosure && s->id() > sTo->id()) { sTo->setWeight(sTo->getWeight() + s->getWeight()); // copy weight } sTo->removeLink(s->id()); } else { UERROR("Link %d of %d not in WM/STM?!?", iter->first, s->id()); } } s->removeLinks(); // remove all links s->setWeight(0); s->setLabel(""); // reset label } else { // Make sure that virtual links are removed. // It should be called before the signature is // removed from _signatures below. removeVirtualLinks(s->id()); } this->disableWordsRef(s->id()); if(!keepLinkedToGraph) { std::list keys = uUniqueKeys(s->getWords()); for(std::list::const_iterator i=keys.begin(); i!=keys.end(); ++i) { // assume just removed word doesn't have any other references VisualWord * w = _vwd->getUnusedWord(*i); if(w) { std::vector wordToDelete; wordToDelete.push_back(w); _vwd->removeWords(wordToDelete); if(deletedWords) { deletedWords->push_back(w->id()); } delete w; } } } _workingMem.erase(s->id()); _stMem.erase(s->id()); _signatures.erase(s->id()); if(_lastSignature == s) { _lastSignature = 0; if(_stMem.size()) { _lastSignature = this->_getSignature(*_stMem.rbegin()); } else if(_workingMem.size()) { _lastSignature = this->_getSignature(_workingMem.rbegin()->first); } } if( (_notLinkedNodesKeptInDb || keepLinkedToGraph) && _dbDriver && s->id()>0 && (_incrementalMemory || s->isSaved())) { _dbDriver->asyncSave(s); } else { delete s; } } } int Memory::getLastSignatureId() const { return _idCount; } const Signature * Memory::getLastWorkingSignature() const { UDEBUG(""); return _lastSignature; } int Memory::getSignatureIdByLabel(const std::string & label, bool lookInDatabase) const { UDEBUG("label=%s", label.c_str()); int id = 0; if(label.size()) { for(std::map::const_iterator iter=_signatures.begin(); iter!=_signatures.end(); ++iter) { UASSERT(iter->second != 0); if(iter->second->getLabel().compare(label) == 0) { id = iter->second->id(); break; } } if(id == 0 && _dbDriver && lookInDatabase) { _dbDriver->getNodeIdByLabel(label, id); } } return id; } bool Memory::labelSignature(int id, const std::string & label) { // verify that this label is not used int idFound=getSignatureIdByLabel(label); if(idFound == 0 || idFound == id) { Signature * s = this->_getSignature(id); if(s) { s->setLabel(label); return true; } else if(_dbDriver) { std::list ids; ids.push_back(id); std::list signatures; _dbDriver->loadSignatures(ids,signatures); if(signatures.size()) { signatures.front()->setLabel(label); _dbDriver->asyncSave(signatures.front()); // move it again to trash return true; } } else { UERROR("Node %d not found, failed to set label \"%s\"!", id, label.c_str()); } } else if(idFound) { UWARN("Node %d has already label \"%s\"", idFound, label.c_str()); } return false; } std::map Memory::getAllLabels() const { std::map labels; for(std::map::const_iterator iter = _signatures.begin(); iter!=_signatures.end(); ++iter) { if(!iter->second->getLabel().empty()) { labels.insert(std::make_pair(iter->first, iter->second->getLabel())); } } if(_dbDriver) { _dbDriver->getAllLabels(labels); } return labels; } bool Memory::setUserData(int id, const cv::Mat & data) { Signature * s = this->_getSignature(id); if(s) { s->sensorData().setUserData(data); return true; } else { UERROR("Node %d not found in RAM, failed to set user data (size=%d)!", id, data.total()); } return false; } void Memory::deleteLocation(int locationId, std::list * deletedWords) { UDEBUG("Deleting location %d", locationId); Signature * location = _getSignature(locationId); if(location) { this->moveToTrash(location, false, deletedWords); } } void Memory::removeLink(int oldId, int newId) { //this method assumes receiving oldId < newId, if not switch them Signature * oldS = this->_getSignature(oldId_getSignature(oldIdid(), newS->id()); if(oldS->hasLink(newS->id()) && newS->hasLink(oldS->id())) { Link::Type type = oldS->getLinks().at(newS->id()).type(); if(type == Link::kGlobalClosure && newS->getWeight() > 0) { // adjust the weight oldS->setWeight(oldS->getWeight()+1); newS->setWeight(newS->getWeight()>0?newS->getWeight()-1:0); } oldS->removeLink(newS->id()); newS->removeLink(oldS->id()); if(type!=Link::kVirtualClosure) { _linksChanged = true; } bool noChildrenAnymore = true; for(std::map::const_iterator iter=newS->getLinks().begin(); iter!=newS->getLinks().end(); ++iter) { if(iter->second.type() > Link::kNeighbor && iter->first < newS->id()) { noChildrenAnymore = false; break; } } if(noChildrenAnymore && newS->id() == _lastGlobalLoopClosureId) { _lastGlobalLoopClosureId = 0; } } else { UERROR("Signatures %d and %d don't have bidirectional link!", oldS->id(), newS->id()); } } else { if(!newS) { UERROR("Signature %d is not in working memory... cannot remove link.", newS->id()); } if(!oldS) { UERROR("Signature %d is not in working memory... cannot remove link.", oldS->id()); } } } // compute transform newId -> oldId Transform Memory::computeVisualTransform( int oldId, int newId, std::string * rejectedMsg, int * inliers, double * variance) const { const Signature * oldS = this->getSignature(oldId); const Signature * newS = this->getSignature(newId); Transform transform; if(oldS && newId) { return computeVisualTransform(*oldS, *newS, rejectedMsg, inliers, variance); } else { std::string msg = uFormat("Did not find nodes %d and/or %d", oldId, newId); if(rejectedMsg) { *rejectedMsg = msg; } UWARN(msg.c_str()); } return Transform(); } // compute transform newId -> oldId Transform Memory::computeVisualTransform( const Signature & oldS, const Signature & newS, std::string * rejectedMsg, int * inliersOut, double * varianceOut) const { Transform transform; std::string msg; // Guess transform from visual words int inliersCount= 0; double variance = 1.0; if(_bowEstimationType == 2) // Epipolar Geometry { if(!newS.sensorData().stereoCameraModel().isValid() && (newS.sensorData().cameraModels().size() != 1 || !newS.sensorData().cameraModels()[0].isValid())) { UERROR("Calibrated camera required (multi-cameras not supported)."); } else if((int)oldS.getWords().size() >= _bowMinInliers && (int)newS.getWords().size() >= _bowMinInliers) { UASSERT(oldS.sensorData().stereoCameraModel().isValid() || (oldS.sensorData().cameraModels().size() == 1 && oldS.sensorData().cameraModels()[0].isValid())); const CameraModel & cameraModel = oldS.sensorData().stereoCameraModel().isValid()?oldS.sensorData().stereoCameraModel().left():oldS.sensorData().cameraModels()[0]; // we only need the camera transform, send guess words3 for scale estimation Transform cameraTransform; std::multimap inliers3D = util3d::generateWords3DMono( oldS.getWords(), newS.getWords(), cameraModel, cameraTransform, _bowIterations, _bowPnPReprojError, _bowPnPFlags, // cv::SOLVEPNP_ITERATIVE 1.0f, 0.99f, oldS.getWords3(), // for scale estimation &variance); inliersCount = (int)inliers3D.size(); if(!cameraTransform.isNull()) { if((int)inliers3D.size() >= _bowMinInliers) { if(variance <= _bowEpipolarGeometryVar) { transform = cameraTransform.inverse(); } else { msg = uFormat("Variance is too high! (max inlier distance=%f, variance=%f)", _bowEpipolarGeometryVar, variance); UINFO(msg.c_str()); } } else { msg = uFormat("Not enough inliers %d < %d", (int)inliers3D.size(), _bowMinInliers); UINFO(msg.c_str()); } } else { msg = uFormat("No camera transform found"); UINFO(msg.c_str()); } } else if(oldS.getWords3().size() == 0) { msg = uFormat("No 3D guess words found"); UWARN(msg.c_str()); } else { msg = uFormat("No camera model"); UWARN(msg.c_str()); } } else if(_bowEstimationType == 1) // PnP { if(!newS.sensorData().stereoCameraModel().isValid() && (newS.sensorData().cameraModels().size() != 1 || !newS.sensorData().cameraModels()[0].isValid())) { UERROR("Calibrated camera required (multi-cameras not supported)."); } else { // 3D to 2D if((int)oldS.getWords3().size() >= _bowMinInliers && (int)newS.getWords().size() >= _bowMinInliers) { UASSERT(newS.sensorData().stereoCameraModel().isValid() || (newS.sensorData().cameraModels().size() == 1 && newS.sensorData().cameraModels()[0].isValid())); const CameraModel & cameraModel = newS.sensorData().stereoCameraModel().isValid()?newS.sensorData().stereoCameraModel().left():newS.sensorData().cameraModels()[0]; std::vector inliersV; transform = util3d::estimateMotion3DTo2D( uMultimapToMap(oldS.getWords3()), uMultimapToMap(newS.getWords()), cameraModel, _bowMinInliers, _bowIterations, _bowPnPReprojError, _bowPnPFlags, Transform::getIdentity(), uMultimapToMap(newS.getWords3()), &variance, 0, &inliersV); inliersCount = (int)inliersV.size(); if(transform.isNull()) { msg = uFormat("Not enough inliers %d/%d between %d and %d", inliersCount, _bowMinInliers, oldS.id(), newS.id()); UINFO(msg.c_str()); } else { transform = transform.inverse(); } } else { msg = uFormat("Not enough features in images (old=%d, new=%d, min=%d)", (int)oldS.getWords3().size(), (int)newS.getWords().size(), _bowMinInliers); UINFO(msg.c_str()); } } } else { // 3D -> 3D if((int)oldS.getWords3().size() >= _bowMinInliers && (int)newS.getWords3().size() >= _bowMinInliers) { std::vector inliersV; transform = util3d::estimateMotion3DTo3D( uMultimapToMap(oldS.getWords3()), uMultimapToMap(newS.getWords3()), _bowMinInliers, _bowInlierDistance, _bowIterations, _bowRefineIterations, &variance, 0, &inliersV); inliersCount = (int)inliersV.size(); if(transform.isNull()) { msg = uFormat("Not enough inliers %d/%d between %d and %d", inliersCount, _bowMinInliers, oldS.id(), newS.id()); UINFO(msg.c_str()); } else { transform = transform.inverse(); } } else { msg = uFormat("Not enough 3D features in images (old=%d, new=%d, min=%d)", (int)oldS.getWords3().size(), (int)newS.getWords3().size(), _bowMinInliers); UINFO(msg.c_str()); } } if(!transform.isNull()) { // verify if it is a 180 degree transform, well verify > 90 float x,y,z, roll,pitch,yaw; transform.getTranslationAndEulerAngles(x,y,z, roll,pitch,yaw); if(fabs(roll) > CV_PI/2 || fabs(pitch) > CV_PI/2 || fabs(yaw) > CV_PI/2) { transform.setNull(); msg = uFormat("Too large rotation detected! (roll=%f, pitch=%f, yaw=%f)", roll, pitch, yaw); UWARN(msg.c_str()); } else if(_bowForce2D) { UDEBUG("Forcing 2D..."); transform = Transform(x,y,0, 0, 0, yaw); } } if(rejectedMsg) { *rejectedMsg = msg; } if(inliersOut) { *inliersOut = inliersCount; } if(varianceOut) { *varianceOut = variance; } UDEBUG("transform=%s", transform.prettyPrint().c_str()); return transform; } // compute transform newId -> oldId Transform Memory::computeIcpTransform( int oldId, int newId, Transform guess, bool icp3D, std::string * rejectedMsg, int * inliers, double * variance, float * inliersRatio) { Signature * oldS = this->_getSignature(oldId); Signature * newS = this->_getSignature(newId); if(oldS && newS && _dbDriver) { std::list depthToLoad; std::set added; if(icp3D) { //Depth required, if not in RAM, load it from LTM if(oldS->sensorData().depthOrRightCompressed().empty()) { depthToLoad.push_back(oldS); added.insert(oldS->id()); } if(newS->sensorData().depthOrRightCompressed().empty()) { depthToLoad.push_back(newS); added.insert(newS->id()); } } else { //Depth required, if not in RAM, load it from LTM if(oldS->sensorData().laserScanCompressed().empty() && added.find(oldS->id()) == added.end()) { depthToLoad.push_back(oldS); } if(newS->sensorData().laserScanCompressed().empty() && added.find(newS->id()) == added.end()) { depthToLoad.push_back(newS); } } if(depthToLoad.size()) { _dbDriver->loadNodeData(depthToLoad); } } Transform t; if(oldS && newS) { //make sure data are uncompressed if(icp3D) { cv::Mat tmp1, tmp2; oldS->sensorData().uncompressData(0, &tmp1, 0); newS->sensorData().uncompressData(0, &tmp2, 0); } else { cv::Mat tmp1, tmp2; oldS->sensorData().uncompressData(0, 0, &tmp1); newS->sensorData().uncompressData(0, 0, &tmp2); } t = computeIcpTransform(*oldS, *newS, guess, icp3D, rejectedMsg, inliers, variance, inliersRatio); } else { std::string msg = uFormat("Did not find nodes %d and/or %d", oldId, newId); if(rejectedMsg) { *rejectedMsg = msg; } UWARN(msg.c_str()); } return t; } // get transform from the new to old node Transform Memory::computeIcpTransform( const Signature & oldS, const Signature & newS, Transform guess, bool icp3D, std::string * rejectedMsg, int * correspondencesOut, double * varianceOut, float * correspondencesRatioOut) const { if(guess.isNull()) { //Make a guess using odometry guess = oldS.getPose().inverse() * newS.getPose(); UASSERT_MSG(oldS.mapId() == newS.mapId(), "Compute ICP from two different maps is not implemented!"); } else { guess = guess.inverse(); // from pose to cloud data } UDEBUG("Guess transform = %s", guess.prettyPrint().c_str()); std::string msg; Transform transform; // ICP with guess transform if(icp3D) { UDEBUG("3D ICP"); if(!oldS.sensorData().depthOrRightRaw().empty() && !newS.sensorData().depthOrRightRaw().empty() && (oldS.sensorData().cameraModels().size() || oldS.sensorData().stereoCameraModel().isValid()) && (newS.sensorData().cameraModels().size() || newS.sensorData().stereoCameraModel().isValid())) { pcl::PointCloud::Ptr oldCloudXYZ = util3d::cloudFromSensorData( oldS.sensorData(), _icpDecimation, _icpMaxDepth, _icpVoxelSize, _icpSamples); pcl::PointCloud::Ptr newCloudXYZ = util3d::cloudFromSensorData( newS.sensorData(), _icpDecimation, _icpMaxDepth, _icpVoxelSize, _icpSamples); // 3D if(newCloudXYZ->size() && oldCloudXYZ->size()) { newCloudXYZ = util3d::transformPointCloud(newCloudXYZ, guess); bool hasConverged = false; Transform icpT; int correspondences = 0; float correspondencesRatio = 0.0f; double variance = 1; if(_icpPointToPlane) { pcl::PointCloud::Ptr oldCloud = util3d::computeNormals(oldCloudXYZ, _icpPointToPlaneNormalNeighbors); pcl::PointCloud::Ptr newCloud = util3d::computeNormals(newCloudXYZ, _icpPointToPlaneNormalNeighbors); std::vector indices; newCloud = util3d::removeNaNNormalsFromPointCloud(newCloud); oldCloud = util3d::removeNaNNormalsFromPointCloud(oldCloud); if(newCloud->size() && oldCloud->size()) { pcl::PointCloud::Ptr newCloudRegistered(new pcl::PointCloud); icpT = util3d::icpPointToPlane( newCloud, oldCloud, _icpMaxCorrespondenceDistance, _icpMaxIterations, hasConverged, *newCloudRegistered); util3d::computeVarianceAndCorrespondences( newCloudRegistered, oldCloud, _icpMaxCorrespondenceDistance, variance, correspondences); } } else { pcl::PointCloud::Ptr newCloudRegistered(new pcl::PointCloud); icpT = util3d::icp( newCloudXYZ, oldCloudXYZ, _icpMaxCorrespondenceDistance, _icpMaxIterations, hasConverged, *newCloudRegistered); util3d::computeVarianceAndCorrespondences( newCloudRegistered, oldCloudXYZ, _icpMaxCorrespondenceDistance, variance, correspondences); } // verify if there are enough correspondences correspondencesRatio = float(correspondences)/float(newCloudXYZ->size()>oldCloudXYZ->size()?newCloudXYZ->size():oldCloudXYZ->size()); UDEBUG("%d->%d hasConverged=%s, variance=%f, correspondences=%d/%d (%f%%)", hasConverged?"true":"false", variance, correspondences, (int)(oldCloudXYZ->size()>newCloudXYZ->size()?oldCloudXYZ->size():newCloudXYZ->size()), correspondencesRatio*100.0f); if(varianceOut) { *varianceOut = variance; } if(correspondencesOut) { *correspondencesOut = correspondences; } if(correspondencesRatioOut) { *correspondencesRatioOut = correspondencesRatio; } if(!icpT.isNull() && hasConverged && correspondencesRatio >= _icpCorrespondenceRatio) { float x,y,z, roll,pitch,yaw; icpT.getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw); if((_icpMaxTranslation>0.0f && (fabs(x) > _icpMaxTranslation || fabs(y) > _icpMaxTranslation || fabs(z) > _icpMaxTranslation)) || (_icpMaxRotation>0.0f && (fabs(roll) > _icpMaxRotation || fabs(pitch) > _icpMaxRotation || fabs(yaw) > _icpMaxRotation))) { msg = uFormat("Cannot compute transform (ICP correction too large)"); UINFO(msg.c_str()); } else { transform = icpT * guess; transform = transform.inverse(); } } else { msg = uFormat("Cannot compute transform (converged=%s var=%f corr=%d corrRatio=%f/%f)", hasConverged?"true":"false", variance, correspondences, correspondencesRatio, _icpCorrespondenceRatio); UINFO(msg.c_str()); } } else { msg = "Clouds empty ?!?"; UWARN(msg.c_str()); } } else { msg = uFormat("Depths 3D empty?!? (new[%d]=%d old[%d]=%d)", newS.id(), newS.sensorData().depthOrRightRaw().total(), oldS.id(), oldS.sensorData().depthOrRightRaw().total()); UERROR(msg.c_str()); } } else // icp 2D { UDEBUG("2D ICP"); // We are 2D here, make sure the guess has only YAW rotation float x,y,z,r,p,yaw; guess.getTranslationAndEulerAngles(x,y,z, r,p,yaw); guess = Transform(x,y,0, 0, 0, yaw); if(r!=0 || p!=0) { UINFO("2D ICP: Dropping z (%f), roll (%f) and pitch (%f) rotation!", z, r, p); } if(!oldS.sensorData().laserScanRaw().empty() && !newS.sensorData().laserScanRaw().empty()) { // 2D pcl::PointCloud::Ptr oldCloud = util3d::cvMat2Cloud(oldS.sensorData().laserScanRaw()); pcl::PointCloud::Ptr newCloud = util3d::cvMat2Cloud(newS.sensorData().laserScanRaw(), guess); //voxelize pcl::PointCloud::Ptr oldCloudVoxelized = oldCloud; pcl::PointCloud::Ptr newCloudVoxelized = newCloud; if(_icp2VoxelSize > _laserScanVoxelSize) { oldCloudVoxelized = util3d::voxelize(oldCloud, _icp2VoxelSize); newCloudVoxelized = util3d::voxelize(newCloud, _icp2VoxelSize); } if(newCloud->size() && oldCloud->size()) { Transform icpT; bool hasConverged = false; float correspondencesRatio = 0.0f; int correspondences = 0; double variance = 1; pcl::PointCloud::Ptr newCloudRegistered(new pcl::PointCloud()); icpT = util3d::icp2D( newCloudVoxelized, oldCloudVoxelized, _icp2MaxCorrespondenceDistance, _icp2MaxIterations, hasConverged, *newCloudRegistered); //pcl::io::savePCDFile("oldCloud.pcd", *oldCloud); //pcl::io::savePCDFile("newCloud.pcd", *newCloud); //UWARN("saved oldCloud.pcd and newCloud.pcd"); //if(!icpT.isNull()) //{ // newCloud = util3d::transformPointCloud(newCloud, icpT); // pcl::io::savePCDFile("newCloudFinal.pcd", *newCloud); // UWARN("saved newCloudFinal.pcd"); //} if(!icpT.isNull() && hasConverged) { float ix,iy,iz, iroll,ipitch,iyaw; icpT.getTranslationAndEulerAngles(ix,iy,iz,iroll,ipitch,iyaw); if((_icpMaxTranslation>0.0f && (fabs(ix) > _icpMaxTranslation || fabs(iy) > _icpMaxTranslation || fabs(iz) > _icpMaxTranslation)) || (_icpMaxRotation>0.0f && (fabs(iroll) > _icpMaxRotation || fabs(ipitch) > _icpMaxRotation || fabs(iyaw) > _icpMaxRotation))) { msg = uFormat("Cannot compute transform (ICP correction too large)"); UINFO(msg.c_str()); } else { if(_icp2VoxelSize <= _laserScanVoxelSize) { newCloud = util3d::transformPointCloud(newCloud, icpT); } else { newCloud = newCloudRegistered; } pcl::PointCloud::Ptr newCloudRegistered(new pcl::PointCloud); util3d::computeVarianceAndCorrespondences( newCloud, oldCloud, _icpMaxCorrespondenceDistance, variance, correspondences); // verify if there are enough correspondences if(newS.sensorData().laserScanMaxPts()) { correspondencesRatio = float(correspondences)/float(newS.sensorData().laserScanMaxPts()); } else { UWARN("Maximum laser scans points not set for signature %d, correspondences ratio set relative instead of absolute!", newS.id()); correspondencesRatio = float(correspondences)/float(newCloud->size()>oldCloud->size()?newCloud->size():oldCloud->size()); } UDEBUG("%d->%d hasConverged=%s, variance=%f, correspondences=%d/%d (%f%%)", newS.id(), oldS.id(), hasConverged?"true":"false", variance, correspondences, (int)(newS.sensorData().laserScanMaxPts()), correspondencesRatio*100.0f); if(varianceOut) { *varianceOut = variance; } if(correspondencesOut) { *correspondencesOut = correspondences; } if(correspondencesRatioOut) { *correspondencesRatioOut = correspondencesRatio; } if(correspondencesRatio < _icp2CorrespondenceRatio) { msg = uFormat("Cannot compute transform (cor=%d corrRatio=%f/%f)", correspondences, correspondencesRatio, _icp2CorrespondenceRatio); UINFO(msg.c_str()); } else { transform = icpT * guess; transform = transform.inverse(); } } } else { msg = uFormat("Cannot compute transform (converged=%s var=%f)", hasConverged?"true":"false", variance); UINFO(msg.c_str()); } } else { msg = "Clouds 2D empty ?!?"; UWARN(msg.c_str()); } } else { msg = uFormat("Depths 2D empty?!? (new[%d]=%d old[%d]=%d)", newS.id(), newS.sensorData().laserScanRaw().total(), oldS.id(), oldS.sensorData().laserScanRaw().total()); UERROR(msg.c_str()); } } if(rejectedMsg) { *rejectedMsg = msg; } UDEBUG("New transform = %s", transform.prettyPrint().c_str()); return transform; } // poses of newId and oldId must be in "poses" Transform Memory::computeScanMatchingTransform( int newId, int oldId, const std::map & poses, std::string * rejectedMsg, int * inliers, double * variance) { UASSERT(uContains(poses, newId) && uContains(_signatures, newId)); UASSERT(uContains(poses, oldId) && uContains(_signatures, oldId)); // make sure that all depth2D are loaded std::list depthToLoad; for(std::map::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter) { Signature * s = _getSignature(iter->first); UASSERT(s != 0); if(s->sensorData().laserScanCompressed().empty()) { depthToLoad.push_back(s); } } if(depthToLoad.size() && _dbDriver) { _dbDriver->loadNodeData(depthToLoad); } std::string msg; pcl::PointCloud::Ptr assembledOldClouds(new pcl::PointCloud); for(std::map::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter) { if(iter->first != newId) { Signature * s = this->_getSignature(iter->first); if(!s->sensorData().laserScanCompressed().empty()) { cv::Mat scan; s->sensorData().uncompressData(0, 0, &scan); *assembledOldClouds += *util3d::cvMat2Cloud(scan, iter->second); } else { UWARN("Depth2D not found for signature %d", iter->first); } } } //voxelize if(assembledOldClouds->size() && _icp2VoxelSize > 0.0f) { assembledOldClouds = util3d::voxelize(assembledOldClouds, _icp2VoxelSize); } // get the new cloud Signature * newS = _getSignature(newId); pcl::PointCloud::Ptr newCloud; cv::Mat newScan; newS->sensorData().uncompressData(0, 0, &newScan); newCloud = util3d::cvMat2Cloud(newScan, poses.at(newId)); //voxelize pcl::PointCloud::Ptr newCloudVoxelized = newCloud; if(newCloud->size() && _icp2VoxelSize > _laserScanVoxelSize) { newCloudVoxelized = util3d::voxelize(newCloud, _icp2VoxelSize); } Transform transform; if(assembledOldClouds->size() && newCloudVoxelized->size()) { int correspondences = 0; bool hasConverged = false; pcl::PointCloud::Ptr newCloudRegistered(new pcl::PointCloud); Transform icpT = util3d::icp2D( newCloudVoxelized, assembledOldClouds, _icp2MaxCorrespondenceDistance, _icp2MaxIterations, hasConverged, *newCloudRegistered); UDEBUG("icpT=%s", icpT.prettyPrint().c_str()); //pcl::io::savePCDFile("old.pcd", *assembledOldClouds, true); //pcl::io::savePCDFile("new.pcd", *newCloud, true); //UWARN("local scan matching old.pcd, new.pcd saved!"); //if(!icpT.isNull()) //{ // newCloud = util3d::transformPointCloud(newCloud, icpT); // pcl::io::savePCDFile("newFinal.pcd", *newCloud, true); // UWARN("local scan matching newFinal.pcd saved!"); //} if(!icpT.isNull() && hasConverged) { if(_icp2VoxelSize <= _laserScanVoxelSize) { newCloud = util3d::transformPointCloud(newCloud, icpT); } else { newCloud = newCloudRegistered; } pcl::PointCloud::Ptr newCloudRegistered(new pcl::PointCloud); double v = 1; util3d::computeVarianceAndCorrespondences( newCloud, assembledOldClouds, _icpMaxCorrespondenceDistance, v, correspondences); if(variance) { *variance = v; } // verify if there enough correspondences float correspondencesRatio = 0.0f; if(newS->sensorData().laserScanMaxPts()) { correspondencesRatio = float(correspondences)/float(newS->sensorData().laserScanMaxPts()); } else { UWARN("Maximum laser scans points not set for signature %d, correspondences ratio set relative instead of absolute!", newS->id()); correspondencesRatio = float(correspondences)/float(newCloud->size()); } UDEBUG("variance=%f, correspondences=%d/%d (%f%%) %f", variance?*variance:-1, correspondences, (int)newCloud->size(), correspondencesRatio*100.0f); if(inliers) { *inliers = correspondences; } if(correspondencesRatio >= _icp2CorrespondenceRatio) { transform = poses.at(newId).inverse()*icpT.inverse() * poses.at(oldId); } else { msg = uFormat("Constraints failed... variance=%f, correspondences=%d/%d (%f%%)", variance?*variance:-1, correspondences, (int)newCloud->size(), correspondencesRatio); UINFO(msg.c_str()); } } else { msg = uFormat("Constraints failed... hasConverged=%s", hasConverged?"true":"false"); UINFO(msg.c_str()); } } else { msg = "Empty data ?!?"; UWARN(msg.c_str()); } if(rejectedMsg) { *rejectedMsg = msg; } return transform; } bool Memory::addLink(const Link & link) { UASSERT(link.type() > Link::kNeighbor && link.type() != Link::kUndef); ULOGGER_INFO("to=%d, from=%d transform: %s", link.to(), link.from(), link.transform().prettyPrint().c_str()); Signature * toS = _getSignature(link.to()); Signature * fromS = _getSignature(link.from()); if(toS && fromS) { if(toS->hasLink(link.from())) { // do nothing, already merged UINFO("already linked! to=%d, from=%d", link.to(), link.from()); return true; } UDEBUG("Add link between %d and %d", toS->id(), fromS->id()); toS->addLink(Link(link.to(), link.from(), link.type(), link.transform().inverse(), link.infMatrix())); fromS->addLink(link); if(_incrementalMemory) { if(link.type()!=Link::kVirtualClosure) { _linksChanged = true; } if(link.type() == Link::kGlobalClosure) { _lastGlobalLoopClosureId = fromS->id()>toS->id()?fromS->id():toS->id(); // update weights only if the memory is incremental UASSERT(fromS->getWeight() >= 0 && toS->getWeight() >=0); if(fromS->id() > toS->id()) { fromS->setWeight(fromS->getWeight() + toS->getWeight()); toS->setWeight(0); } else { toS->setWeight(toS->getWeight() + fromS->getWeight()); fromS->setWeight(0); } } } return true; } else { if(!fromS) { UERROR("from=%d, to=%d, Signature %d not found in working/st memories", link.from(), link.to(), link.from()); } if(!toS) { UERROR("from=%d, to=%d, Signature %d not found in working/st memories", link.from(), link.to(), link.to()); } } return false; } void Memory::updateLink(int fromId, int toId, const Transform & transform, float rotVariance, float transVariance) { Signature * fromS = this->_getSignature(fromId); Signature * toS = this->_getSignature(toId); if(fromS->hasLink(toId) && toS->hasLink(fromId)) { Link::Type type = fromS->getLinks().at(toId).type(); fromS->removeLink(toId); toS->removeLink(fromId); fromS->addLink(Link(fromId, toId, type, transform, rotVariance, transVariance)); toS->addLink(Link(toId, fromId, type, transform.inverse(), rotVariance, transVariance)); if(type!=Link::kVirtualClosure) { _linksChanged = true; } } else { UERROR("fromId=%d and toId=%d are not linked!", fromId, toId); } } void Memory::updateLink(int fromId, int toId, const Transform & transform, const cv::Mat & covariance) { Signature * fromS = this->_getSignature(fromId); Signature * toS = this->_getSignature(toId); if(fromS->hasLink(toId) && toS->hasLink(fromId)) { Link::Type type = fromS->getLinks().at(toId).type(); fromS->removeLink(toId); toS->removeLink(fromId); cv::Mat infMatrix = covariance.inv(); fromS->addLink(Link(fromId, toId, type, transform, infMatrix)); toS->addLink(Link(toId, fromId, type, transform.inverse(), infMatrix)); if(type!=Link::kVirtualClosure) { _linksChanged = true; } } else { UERROR("fromId=%d and toId=%d are not linked!", fromId, toId); } } void Memory::removeAllVirtualLinks() { UDEBUG(""); for(std::map::iterator iter=_signatures.begin(); iter!=_signatures.end(); ++iter) { iter->second->removeVirtualLinks(); } } void Memory::removeVirtualLinks(int signatureId) { UDEBUG(""); Signature * s = this->_getSignature(signatureId); if(s) { const std::map & links = s->getLinks(); for(std::map::const_iterator iter=links.begin(); iter!=links.end(); ++iter) { if(iter->second.type() == Link::kVirtualClosure) { Signature * sTo = this->_getSignature(iter->first); if(sTo) { sTo->removeLink(s->id()); } else { UERROR("Link %d of %d not in WM/STM?!?", iter->first, s->id()); } } } s->removeVirtualLinks(); } else { UERROR("Signature %d not in WM/STM?!?", signatureId); } } void Memory::dumpMemory(std::string directory) const { UINFO("Dumping memory to directory \"%s\"", directory.c_str()); this->dumpDictionary((directory+"DumpMemoryWordRef.txt").c_str(), (directory+"DumpMemoryWordDesc.txt").c_str()); this->dumpSignatures((directory + "DumpMemorySign.txt").c_str(), false); this->dumpSignatures((directory + "DumpMemorySign3.txt").c_str(), true); this->dumpMemoryTree((directory + "DumpMemoryTree.txt").c_str()); } void Memory::dumpDictionary(const char * fileNameRef, const char * fileNameDesc) const { if(_vwd) { _vwd->exportDictionary(fileNameRef, fileNameDesc); } } void Memory::dumpSignatures(const char * fileNameSign, bool words3D) const { FILE* foutSign = 0; #ifdef _MSC_VER fopen_s(&foutSign, fileNameSign, "w"); #else foutSign = fopen(fileNameSign, "w"); #endif if(foutSign) { fprintf(foutSign, "SignatureID WordsID...\n"); const std::map & signatures = this->getSignatures(); for(std::map::const_iterator iter=signatures.begin(); iter!=signatures.end(); ++iter) { fprintf(foutSign, "%d ", iter->first); const Signature * ss = dynamic_cast(iter->second); if(ss) { if(words3D) { const std::multimap & ref = ss->getWords3(); for(std::multimap::const_iterator jter=ref.begin(); jter!=ref.end(); ++jter) { //show only valid point according to current parameters if(pcl::isFinite(jter->second) && (jter->second.x != 0 || jter->second.y != 0 || jter->second.z != 0)) { fprintf(foutSign, "%d ", (*jter).first); } } } else { const std::multimap & ref = ss->getWords(); for(std::multimap::const_iterator jter=ref.begin(); jter!=ref.end(); ++jter) { fprintf(foutSign, "%d ", (*jter).first); } } } fprintf(foutSign, "\n"); } fclose(foutSign); } } void Memory::dumpMemoryTree(const char * fileNameTree) const { FILE* foutTree = 0; #ifdef _MSC_VER fopen_s(&foutTree, fileNameTree, "w"); #else foutTree = fopen(fileNameTree, "w"); #endif if(foutTree) { fprintf(foutTree, "SignatureID Weight NbLoopClosureIds LoopClosureIds... NbChildLoopClosureIds ChildLoopClosureIds...\n"); for(std::map::const_iterator i=_signatures.begin(); i!=_signatures.end(); ++i) { fprintf(foutTree, "%d %d", i->first, i->second->getWeight()); std::map loopIds, childIds; for(std::map::const_iterator iter = i->second->getLinks().begin(); iter!=i->second->getLinks().end(); ++iter) { if(iter->second.type() > Link::kNeighbor) { if(iter->first < i->first) { childIds.insert(*iter); } else { loopIds.insert(*iter); } } } fprintf(foutTree, " %d", (int)loopIds.size()); for(std::map::const_iterator j=loopIds.begin(); j!=loopIds.end(); ++j) { fprintf(foutTree, " %d", j->first); } fprintf(foutTree, " %d", (int)childIds.size()); for(std::map::const_iterator j=childIds.begin(); j!=childIds.end(); ++j) { fprintf(foutTree, " %d", j->first); } fprintf(foutTree, "\n"); } fclose(foutTree); } } void Memory::rehearsal(Signature * signature, Statistics * stats) { UTimer timer; if(signature->getLinks().size() != 1) { return; } //============================================================ // Compare with the last (not null) //============================================================ Signature * sB = 0; for(std::set::reverse_iterator iter=_stMem.rbegin(); iter!=_stMem.rend(); ++iter) { Signature * s = this->_getSignature(*iter); UASSERT(s!=0); if(s->getWeight() >= 0 && s->id() != signature->id()) { sB = s; break; } } if(sB) { int id = sB->id(); UDEBUG("Comparing with signature (%d)...", id); float sim = signature->compareTo(*sB); int merged = 0; if(sim >= _similarityThreshold) { if(_incrementalMemory) { if(signature->hasLink(id)) { if(signature->getLinks().begin()->second.transform().isNull()) { if(this->rehearsalMerge(id, signature->id())) { merged = id; } } else { float x,y,z, roll,pitch,yaw; signature->getLinks().begin()->second.transform().getTranslationAndEulerAngles(x,y,z, roll,pitch,yaw); if((_rehearsalMaxDistance>0.0f && ( fabs(x) > _rehearsalMaxDistance || fabs(y) > _rehearsalMaxDistance || fabs(z) > _rehearsalMaxDistance)) || (_rehearsalMaxAngle>0.0f && ( fabs(roll) > _rehearsalMaxAngle || fabs(pitch) > _rehearsalMaxAngle || fabs(yaw) > _rehearsalMaxAngle))) { if(_rehearsalWeightIgnoredWhileMoving) { UINFO("Rehearsal ignored because the robot has moved more than %f m or %f rad", _rehearsalMaxDistance, _rehearsalMaxAngle); } else { // if the robot has moved, increase only weight of the new one signature->setWeight(sB->getWeight() + signature->getWeight() + 1); sB->setWeight(0); UINFO("Only updated weight to %d of %d (old=%d) because the robot has moved. (d=%f a=%f)", signature->getWeight(), signature->id(), sB->id(), _rehearsalMaxDistance, _rehearsalMaxAngle); } } else if(this->rehearsalMerge(id, signature->id())) { merged = id; } } } else { // cannot merge not neighbor signatures, just update weight signature->setWeight(sB->getWeight() + signature->getWeight() + 1); sB->setWeight(0); UINFO("Only updated weight to %d of %d (old=%d) because the signatures are not neighbors.", signature->getWeight(), signature->id(), sB->id()); } } else { signature->setWeight(signature->getWeight() + 1 + sB->getWeight()); } } if(stats) stats->addStatistic(Statistics::kMemoryRehearsal_merged(), merged); if(stats) stats->addStatistic(Statistics::kMemoryRehearsal_sim(), sim); UDEBUG("merged=%d, sim=%f t=%fs", merged, sim, timer.ticks()); } else { if(stats) stats->addStatistic(Statistics::kMemoryRehearsal_merged(), 0); if(stats) stats->addStatistic(Statistics::kMemoryRehearsal_sim(), 0); } } bool Memory::rehearsalMerge(int oldId, int newId) { ULOGGER_INFO("old=%d, new=%d", oldId, newId); Signature * oldS = _getSignature(oldId); Signature * newS = _getSignature(newId); if(oldS && newS && _incrementalMemory) { std::map::const_iterator iter = oldS->getLinks().find(newS->id()); if(iter != oldS->getLinks().end() && iter->second.type() > Link::kNeighbor) { // do nothing, already merged UWARN("already merged, old=%d, new=%d", oldId, newId); return false; } UASSERT(!newS->isSaved()); UINFO("Rehearsal merging %d and %d", oldS->id(), newS->id()); //remove mutual links oldS->removeLink(newId); newS->removeLink(oldId); if(_idUpdatedToNewOneRehearsal) { // redirect neighbor links const std::map & links = oldS->getLinks(); for(std::map::const_iterator iter = links.begin(); iter!=links.end(); ++iter) { Link link = iter->second; link.setFrom(newS->id()); Signature * s = this->_getSignature(link.to()); if(s) { // modify neighbor "from" s->changeLinkIds(oldS->id(), newS->id()); newS->addLink(link); } else { UERROR("Didn't find neighbor %d of %d in RAM...", link.to(), oldS->id()); } } newS->setLabel(oldS->getLabel()); oldS->setLabel(""); oldS->removeLinks(); // remove all links oldS->addLink(Link(oldS->id(), newS->id(), Link::kGlobalClosure, Transform(), 1, 1)); // to keep track of the merged location // Set old image to new signature this->copyData(oldS, newS); // update weight newS->setWeight(newS->getWeight() + 1 + oldS->getWeight()); if(_lastGlobalLoopClosureId == oldS->id()) { _lastGlobalLoopClosureId = newS->id(); } } else { newS->addLink(Link(newS->id(), oldS->id(), Link::kGlobalClosure, Transform() , 1, 1)); // to keep track of the merged location // update weight oldS->setWeight(newS->getWeight() + 1 + oldS->getWeight()); if(_lastSignature == newS) { _lastSignature = oldS; } } // remove location moveToTrash(_idUpdatedToNewOneRehearsal?oldS:newS, _notLinkedNodesKeptInDb); return true; } else { if(!newS) { UERROR("newId=%d, oldId=%d, Signature %d not found in working/st memories", newId, oldId, newId); } if(!oldS) { UERROR("newId=%d, oldId=%d, Signature %d not found in working/st memories", newId, oldId, oldId); } } return false; } Transform Memory::getOdomPose(int signatureId, bool lookInDatabase) const { Transform pose; int mapId, weight; std::string label; double stamp; getNodeInfo(signatureId, pose, mapId, weight, label, stamp, lookInDatabase); return pose; } bool Memory::getNodeInfo(int signatureId, Transform & odomPose, int & mapId, int & weight, std::string & label, double & stamp, bool lookInDatabase) const { const Signature * s = this->getSignature(signatureId); if(s) { odomPose = s->getPose(); mapId = s->mapId(); weight = s->getWeight(); label = s->getLabel(); stamp = s->getStamp(); return true; } else if(lookInDatabase && _dbDriver) { return _dbDriver->getNodeInfo(signatureId, odomPose, mapId, weight, label, stamp); } return false; } cv::Mat Memory::getImageCompressed(int signatureId) const { cv::Mat image; const Signature * s = this->getSignature(signatureId); if(s) { image = s->sensorData().imageCompressed(); } if(image.empty() && this->isBinDataKept() && _dbDriver) { SensorData data; _dbDriver->getNodeData(signatureId, data); image = data.imageCompressed(); } return image; } SensorData Memory::getNodeData(int nodeId, bool uncompressedData) { UDEBUG("nodeId=%d", nodeId); SensorData r; Signature * s = this->_getSignature(nodeId); if(s && !s->sensorData().imageCompressed().empty()) { if(uncompressedData) { s->sensorData().uncompressData(); } r = s->sensorData(); } else if(_dbDriver) { // load from database if(s) { std::list signatures; signatures.push_back(s); _dbDriver->loadNodeData(signatures); if(uncompressedData) { s->sensorData().uncompressData(); } r = s->sensorData(); } else { _dbDriver->getNodeData(nodeId, r); if(uncompressedData) { r.uncompressData(); } } } return r; } void Memory::getNodeWords(int nodeId, std::multimap & words, std::multimap & words3) { UDEBUG("nodeId=%d", nodeId); Signature * s = this->_getSignature(nodeId); if(s) { words = s->getWords(); words3 = s->getWords3(); } else if(_dbDriver) { // load from database std::list signatures; std::list ids; ids.push_back(nodeId); std::set loadedFromTrash; _dbDriver->loadSignatures(ids, signatures, &loadedFromTrash); if(signatures.size()) { words = signatures.front()->getWords(); words3 = signatures.front()->getWords3(); if(loadedFromTrash.size()) { //put back _dbDriver->asyncSave(signatures.front()); } else { delete signatures.front(); } } } } SensorData Memory::getSignatureDataConst(int locationId) const { UDEBUG(""); SensorData r; const Signature * s = this->getSignature(locationId); if(s && !s->sensorData().imageCompressed().empty()) { r = s->sensorData(); } else if(_dbDriver) { // load from database if(s) { std::list signatures; Signature tmp = *s; signatures.push_back(&tmp); _dbDriver->loadNodeData(signatures); r = tmp.sensorData(); } else { std::list ids; ids.push_back(locationId); std::list signatures; std::set loadedFromTrash; _dbDriver->loadSignatures(ids, signatures, &loadedFromTrash); if(signatures.size()) { Signature * sTmp = signatures.front(); if(sTmp->sensorData().imageCompressed().empty()) { _dbDriver->loadNodeData(signatures); } r = sTmp->sensorData(); if(loadedFromTrash.size()) { //put it back to trash _dbDriver->asyncSave(sTmp); } else { delete sTmp; } } } } return r; } void Memory::generateGraph(const std::string & fileName, std::set ids) { if(!_dbDriver) { UERROR("A database must must loaded first..."); return; } if(!fileName.empty()) { FILE* fout = 0; #ifdef _MSC_VER fopen_s(&fout, fileName.c_str(), "w"); #else fout = fopen(fileName.c_str(), "w"); #endif if (!fout) { UERROR("Cannot open file %s!", fileName.c_str()); return; } if(ids.size() == 0) { _dbDriver->getAllNodeIds(ids); UDEBUG("ids.size()=%d", ids.size()); for(std::map::iterator iter=_signatures.begin(); iter!=_signatures.end(); ++iter) { ids.insert(iter->first); } } const char * colorG = "green"; const char * colorP = "pink"; ; UINFO("Generating map with %d locations", ids.size()); fprintf(fout, "digraph G {\n"); for(std::set::iterator i=ids.begin(); i!=ids.end(); ++i) { if(_signatures.find(*i) == _signatures.end()) { int id = *i; std::map links; _dbDriver->loadLinks(id, links); int weight = 0; _dbDriver->getWeight(id, weight); for(std::map::iterator iter = links.begin(); iter!=links.end(); ++iter) { int weightNeighbor = 0; if(_signatures.find(iter->first) == _signatures.end()) { _dbDriver->getWeight(iter->first, weightNeighbor); } else { weightNeighbor = _signatures.find(iter->first)->second->getWeight(); } //UDEBUG("Add neighbor link from %d to %d", id, iter->first); if(iter->second.type() == Link::kNeighbor) { fprintf(fout, " \"%d\\n%d\" -> \"%d\\n%d\"\n", id, weight, iter->first, weightNeighbor); } else if(iter->first > id) { //loop fprintf(fout, " \"%d\\n%d\" -> \"%d\\n%d\" [label=\"L\", fontcolor=%s, fontsize=8];\n", id, weight, iter->first, weightNeighbor, colorG); } else { //child fprintf(fout, " \"%d\\n%d\" -> \"%d\\n%d\" [label=\"C\", fontcolor=%s, fontsize=8];\n", id, weight, iter->first, weightNeighbor, colorP); } } } } for(std::map::iterator i=_signatures.begin(); i!=_signatures.end(); ++i) { if(ids.find(i->first) != ids.end()) { int id = i->second->id(); const std::map & links = i->second->getLinks(); int weight = i->second->getWeight(); for(std::map::const_iterator iter = links.begin(); iter!=links.end(); ++iter) { int weightNeighbor = 0; const Signature * s = this->getSignature(iter->first); if(s) { weightNeighbor = s->getWeight(); } else { _dbDriver->getWeight(iter->first, weightNeighbor); } //UDEBUG("Add neighbor link from %d to %d", id, iter->first); if(iter->second.type() == Link::kNeighbor) { fprintf(fout, " \"%d\\n%d\" -> \"%d\\n%d\"\n", id, weight, iter->first, weightNeighbor); } else if(iter->first > id) { //loop fprintf(fout, " \"%d\\n%d\" -> \"%d\\n%d\" [label=\"L\", fontcolor=%s, fontsize=8];\n", id, weight, iter->first, weightNeighbor, colorG); } else { //child fprintf(fout, " \"%d\\n%d\" -> \"%d\\n%d\" [label=\"C\", fontcolor=%s, fontsize=8];\n", id, weight, iter->first, weightNeighbor, colorP); } } } } fprintf(fout, "}\n"); fclose(fout); UINFO("Graph saved to \"%s\"", fileName.c_str()); } } // Only used to generate a .dot file class GraphNode { public: GraphNode(int id, GraphNode * parent = 0) : _parent(parent), _id(id) { if(_parent) { _parent->addChild(this); } } virtual ~GraphNode() { //We copy the set because when a child is destroyed, it is removed from its parent. std::set children = _children; _children.clear(); for(std::set::iterator iter=children.begin(); iter!=children.end(); ++iter) { delete *iter; } children.clear(); if(_parent) { _parent->removeChild(this); } } int id() const {return _id;} bool isAncestor(int id) const { if(_parent) { if(_parent->id() == id) { return true; } return _parent->isAncestor(id); } return false; } void expand(std::list > & paths, std::list currentPath = std::list()) const { currentPath.push_back(_id); if(_children.size() == 0) { paths.push_back(currentPath); return; } for(std::set::const_iterator iter=_children.begin(); iter!=_children.end(); ++iter) { (*iter)->expand(paths, currentPath); } } private: void addChild(GraphNode * child) { _children.insert(child); } void removeChild(GraphNode * child) { _children.erase(child); } private: std::set _children; GraphNode * _parent; int _id; }; //recursive void Memory::createGraph(GraphNode * parent, unsigned int maxDepth, const std::set & endIds) { if(maxDepth == 0 || !parent) { return; } std::map neighbors = this->getNeighborsId(parent->id(), 1, -1, false); for(std::map::iterator iter=neighbors.begin(); iter!=neighbors.end(); ++iter) { if(!parent->isAncestor(iter->first)) { GraphNode * n = new GraphNode(iter->first, parent); if(endIds.find(iter->first) == endIds.end()) { this->createGraph(n, maxDepth-1, endIds); } } } } int Memory::getNi(int signatureId) const { int ni = 0; const Signature * s = this->getSignature(signatureId); if(s) { ni = (int)((Signature *)s)->getWords().size(); } else { _dbDriver->getInvertedIndexNi(signatureId, ni); } return ni; } void Memory::copyData(const Signature * from, Signature * to) { UTimer timer; timer.start(); if(from && to) { // words 2d this->disableWordsRef(to->id()); to->setWords(from->getWords()); std::list id; id.push_back(to->id()); this->enableWordsRef(id); if(from->isSaved() && _dbDriver) { _dbDriver->getNodeData(from->id(), to->sensorData()); UDEBUG("Loaded image data from database"); } else { to->sensorData() = (SensorData)from->sensorData(); } to->sensorData().setId(to->id()); to->setPose(from->getPose()); to->setWords3(from->getWords3()); } else { ULOGGER_ERROR("Can't merge the signatures because there are not same type."); } UDEBUG("Merging time = %fs", timer.ticks()); } class PreUpdateThread : public UThreadNode { public: PreUpdateThread(VWDictionary * vwp) : _vwp(vwp) {} virtual ~PreUpdateThread() {} private: void mainLoop() { if(_vwp) { _vwp->update(); } this->kill(); } VWDictionary * _vwp; }; Signature * Memory::createSignature(const SensorData & data, const Transform & pose, Statistics * stats) { UDEBUG(""); UASSERT(data.imageRaw().empty() || data.imageRaw().type() == CV_8UC1 || data.imageRaw().type() == CV_8UC3); UASSERT_MSG(data.depthOrRightRaw().empty() || ( (data.depthOrRightRaw().type() == CV_16UC1 || data.depthOrRightRaw().type() == CV_32FC1 || data.depthOrRightRaw().type() == CV_8UC1) && data.depthOrRightRaw().rows == data.imageRaw().rows && data.depthOrRightRaw().cols == data.imageRaw().cols), uFormat("image=(%d/%d) depth=(%d/%d, type=%d [accepted=%d,%d,%d])", data.imageRaw().cols, data.imageRaw().rows, data.depthOrRightRaw().cols, data.depthOrRightRaw().rows, data.depthOrRightRaw().type(), CV_16UC1, CV_32FC1, CV_8UC1).c_str()); UASSERT(data.laserScanRaw().empty() || data.laserScanRaw().type() == CV_32FC2); if(!data.depthOrRightRaw().empty() && data.cameraModels().size() == 0 && !data.stereoCameraModel().isValid()) { UERROR("Rectified images required! Calibrate your camera."); return 0; } UASSERT(_feature2D != 0); PreUpdateThread preUpdateThread(_vwd); UTimer timer; timer.start(); float t; std::vector keypoints; cv::Mat descriptors; int id = data.id(); if(_generateIds) { id = this->getNextId(); } else { if(id <= 0) { UERROR("Received image ID is null. " "Please set parameter Mem/GenerateIds to \"true\" or " "make sure the input source provides image ids (seq)."); return 0; } else if(id > _idCount) { _idCount = id; } else { UERROR("Id of acquired image (%d) is smaller than the last in memory (%d). " "Please set parameter Mem/GenerateIds to \"true\" or " "make sure the input source provides image ids (seq) over the last in " "memory, which is %d.", id, _idCount, _idCount); return 0; } } int treeSize= int(_workingMem.size() + _stMem.size()); int meanWordsPerLocation = 0; if(treeSize > 0) { meanWordsPerLocation = _vwd->getTotalActiveReferences() / treeSize; } if(_parallelized) { preUpdateThread.start(); } pcl::PointCloud::Ptr keypoints3D(new pcl::PointCloud); if(data.keypoints().size() == 0) { if(_feature2D->getMaxFeatures() >= 0 && !data.imageRaw().empty()) { // Extract features cv::Mat imageMono; // convert to grayscale if(data.imageRaw().channels() > 1) { cv::cvtColor(data.imageRaw(), imageMono, cv::COLOR_BGR2GRAY); } else { imageMono = data.imageRaw(); } cv::Rect roi = Feature2D::computeRoi(imageMono, _roiRatios); if(!data.depthOrRightRaw().empty() && data.stereoCameraModel().isValid()) { //stereo cv::Mat disparity; bool subPixelOn = false; if(_subPixWinSize > 0 && _subPixIterations > 0) { subPixelOn = true; } keypoints = _feature2D->generateKeypoints(imageMono, roi); t = timer.ticks(); if(stats) stats->addStatistic(Statistics::kTimingMemKeypoints_detection(), t*1000.0f); UDEBUG("time keypoints (%d) = %fs", (int)keypoints.size(), t); if(keypoints.size()) { std::vector leftCorners; if(subPixelOn) { // descriptors should be extracted before subpixel descriptors = _feature2D->generateDescriptors(imageMono, keypoints); t = timer.ticks(); if(stats) stats->addStatistic(Statistics::kTimingMemDescriptors_extraction(), t*1000.0f); UDEBUG("time descriptors (%d) = %fs", descriptors.rows, t); cv::KeyPoint::convert(keypoints, leftCorners); cv::cornerSubPix( imageMono, leftCorners, cv::Size( _subPixWinSize, _subPixWinSize ), cv::Size( -1, -1 ), cv::TermCriteria( CV_TERMCRIT_ITER | CV_TERMCRIT_EPS, _subPixIterations, _subPixEps ) ); for(unsigned int i=0;iaddStatistic(Statistics::kTimingMemSubpixel(), t*1000.0f); UDEBUG("time subpix left kpts=%fs", t); } else { cv::KeyPoint::convert(keypoints, leftCorners); } //generate a disparity map disparity = util2d::disparityFromStereoImages( imageMono, data.depthOrRightRaw(), leftCorners, _stereoFlowWinSize, _stereoFlowMaxLevel, _stereoFlowIterations, _stereoFlowEpsilon, _stereoMaxSlope); t = timer.ticks(); if(stats) stats->addStatistic(Statistics::kTimingMemStereo_correspondences(), t*1000.0f); UDEBUG("generate disparity = %fs", t); if(_wordsMaxDepth > 0.0f) { // disparity = baseline * fx / depth; float minDisparity = data.stereoCameraModel().baseline() * data.stereoCameraModel().left().fx() / _wordsMaxDepth; Feature2D::filterKeypointsByDisparity(keypoints, descriptors, disparity, minDisparity); UDEBUG("filter keypoints by disparity (%d)", (int)keypoints.size()); } if(keypoints.size()) { if(!subPixelOn) { descriptors = _feature2D->generateDescriptors(imageMono, keypoints); t = timer.ticks(); if(stats) stats->addStatistic(Statistics::kTimingMemDescriptors_extraction(), t*1000.0f); UDEBUG("time descriptors (%d) = %fs", descriptors.rows, t); } keypoints3D = util3d::generateKeypoints3DDisparity( keypoints, disparity, data.stereoCameraModel()); t = timer.ticks(); if(stats) stats->addStatistic(Statistics::kTimingMemKeypoints_3D(), t*1000.0f); UDEBUG("time keypoints 3D (%d) = %fs", (int)keypoints3D->size(), t); } } } else if(!data.depthOrRightRaw().empty() && data.cameraModels().size()) { //depth bool subPixelOn = false; if(_subPixWinSize > 0 && _subPixIterations > 0) { subPixelOn = true; } keypoints = _feature2D->generateKeypoints(imageMono, roi); t = timer.ticks(); if(stats) stats->addStatistic(Statistics::kTimingMemKeypoints_detection(), t*1000.0f); UDEBUG("time keypoints (%d) = %fs", (int)keypoints.size(), t); if(keypoints.size()) { if(subPixelOn) { // descriptors should be extracted before subpixel descriptors = _feature2D->generateDescriptors(imageMono, keypoints); t = timer.ticks(); if(stats) stats->addStatistic(Statistics::kTimingMemDescriptors_extraction(), t*1000.0f); UDEBUG("time descriptors (%d) = %fs", descriptors.rows, t); std::vector leftCorners; cv::KeyPoint::convert(keypoints, leftCorners); cv::cornerSubPix( imageMono, leftCorners, cv::Size( _subPixWinSize, _subPixWinSize ), cv::Size( -1, -1 ), cv::TermCriteria( CV_TERMCRIT_ITER | CV_TERMCRIT_EPS, _subPixIterations, _subPixEps ) ); for(unsigned int i=0;iaddStatistic(Statistics::kTimingMemSubpixel(), t*1000.0f); UDEBUG("time subpix left kpts=%fs", t); } if(_wordsMaxDepth > 0.0f) { Feature2D::filterKeypointsByDepth(keypoints, descriptors, data.depthOrRightRaw(), _wordsMaxDepth); UDEBUG("filter keypoints by depth (%d)", (int)keypoints.size()); } if(keypoints.size()) { if(!subPixelOn) { descriptors = _feature2D->generateDescriptors(imageMono, keypoints); t = timer.ticks(); if(stats) stats->addStatistic(Statistics::kTimingMemDescriptors_extraction(), t*1000.0f); UDEBUG("time descriptors (%d) = %fs", descriptors.rows, t); } keypoints3D = util3d::generateKeypoints3DDepth( keypoints, data.depthOrRightRaw(), data.cameraModels()); t = timer.ticks(); if(stats) stats->addStatistic(Statistics::kTimingMemKeypoints_3D(), t*1000.0f); UDEBUG("time keypoints 3D (%d) = %fs", (int)keypoints3D->size(), t); } } } else { //RGB only keypoints = _feature2D->generateKeypoints(imageMono, roi); t = timer.ticks(); if(stats) stats->addStatistic(Statistics::kTimingMemKeypoints_detection(), t*1000.0f); UDEBUG("time keypoints (%d) = %fs", (int)keypoints.size(), t); if(keypoints.size()) { descriptors = _feature2D->generateDescriptors(imageMono, keypoints); t = timer.ticks(); if(stats) stats->addStatistic(Statistics::kTimingMemDescriptors_extraction(), t*1000.0f); UDEBUG("time descriptors (%d) = %fs", descriptors.rows, t); if(_subPixWinSize > 0 && _subPixIterations > 0) { std::vector corners; cv::KeyPoint::convert(keypoints, corners); cv::cornerSubPix( imageMono, corners, cv::Size( _subPixWinSize, _subPixWinSize ), cv::Size( -1, -1 ), cv::TermCriteria( CV_TERMCRIT_ITER | CV_TERMCRIT_EPS, _subPixIterations, _subPixEps ) ); for(unsigned int i=0;iaddStatistic(Statistics::kTimingMemSubpixel(), t*1000.0f); UDEBUG("time subpix kpts=%fs", t); } } } UDEBUG("ratio=%f, meanWordsPerLocation=%d", _badSignRatio, meanWordsPerLocation); if(descriptors.rows && descriptors.rows < _badSignRatio * float(meanWordsPerLocation)) { descriptors = cv::Mat(); } } else if(data.imageRaw().empty()) { UDEBUG("Empty image, cannot extract features..."); } else { UDEBUG("_feature2D->getMaxFeatures()(%d<0) so don't extract any features...", _feature2D->getMaxFeatures()); } } else { keypoints = data.keypoints(); descriptors = data.descriptors().clone(); // filter by depth if(!data.depthOrRightRaw().empty() && data.stereoCameraModel().isValid()) { //stereo cv::Mat imageMono; // convert to grayscale if(data.imageRaw().channels() > 1) { cv::cvtColor(data.imageRaw(), imageMono, cv::COLOR_BGR2GRAY); } else { imageMono = data.imageRaw(); } //generate a disparity map std::vector leftCorners; cv::KeyPoint::convert(keypoints, leftCorners); cv::Mat disparity = util2d::disparityFromStereoImages( imageMono, data.depthOrRightRaw(), leftCorners, _stereoFlowWinSize, _stereoFlowMaxLevel, _stereoFlowIterations, _stereoFlowEpsilon, _stereoMaxSlope); t = timer.ticks(); if(stats) stats->addStatistic(Statistics::kTimingMemStereo_correspondences(), t*1000.0f); UDEBUG("generate disparity = %fs", t); if(_wordsMaxDepth) { // disparity = baseline * fx / depth; float minDisparity = data.stereoCameraModel().baseline() * data.stereoCameraModel().left().fx() / _wordsMaxDepth; Feature2D::filterKeypointsByDisparity(keypoints, descriptors, disparity, minDisparity); } keypoints3D = util3d::generateKeypoints3DDisparity( keypoints, disparity, data.stereoCameraModel()); t = timer.ticks(); if(stats) stats->addStatistic(Statistics::kTimingMemKeypoints_3D(), t*1000.0f); UDEBUG("time keypoints 3D (%d) = %fs", (int)keypoints3D->size(), t); } else if(!data.depthOrRightRaw().empty() && data.cameraModels().size()) { //depth if(_wordsMaxDepth) { Feature2D::filterKeypointsByDepth(keypoints, descriptors, _wordsMaxDepth); UDEBUG("filter keypoints by depth (%d)", (int)keypoints.size()); } keypoints3D = util3d::generateKeypoints3DDepth( keypoints, data.depthOrRightRaw(), data.cameraModels()); t = timer.ticks(); if(stats) stats->addStatistic(Statistics::kTimingMemKeypoints_3D(), t*1000.0f); UDEBUG("time keypoints 3D (%d) = %fs", (int)keypoints3D->size(), t); } } if(_parallelized) { preUpdateThread.join(); // Wait the dictionary to be updated } std::list wordIds; if(descriptors.rows) { t = timer.ticks(); if(stats) stats->addStatistic(Statistics::kTimingMemJoining_dictionary_update(), t*1000.0f); if(_parallelized) { UDEBUG("time descriptor and memory update (%d of size=%d) = %fs", descriptors.rows, descriptors.cols, t); } else { UDEBUG("time descriptor (%d of size=%d) = %fs", descriptors.rows, descriptors.cols, t); } wordIds = _vwd->addNewWords(descriptors, id); t = timer.ticks(); if(stats) stats->addStatistic(Statistics::kTimingMemAdd_new_words(), t*1000.0f); UDEBUG("time addNewWords %fs", t); } else if(id>0) { UDEBUG("id %d is a bad signature", id); } std::multimap words; std::multimap words3D; if(wordIds.size() > 0) { UASSERT(wordIds.size() == keypoints.size()); UASSERT(keypoints3D->size() == 0 || keypoints3D->size() == wordIds.size()); unsigned int i=0; for(std::list::iterator iter=wordIds.begin(); iter!=wordIds.end() && i < keypoints.size(); ++iter, ++i) { if(_imageDecimation > 1) { cv::KeyPoint kpt = keypoints[i]; kpt.pt.x /= float(_imageDecimation); kpt.pt.y /= float(_imageDecimation); kpt.size /= float(_imageDecimation); words.insert(std::pair(*iter, kpt)); } else { words.insert(std::pair(*iter, keypoints[i])); } if(keypoints3D->size()) { words3D.insert(std::pair(*iter, keypoints3D->at(i))); } } } if(words.size() > 8 && words3D.size() == 0 && !pose.isNull() && data.cameraModels().size() == 1 && _signatures.size()) { UDEBUG("Generate 3D words using odometry"); Signature * previousS = _signatures.rbegin()->second; if(previousS->getWords().size() > 8 && words.size() > 8 && !previousS->getPose().isNull()) { Transform cameraTransform = pose.inverse() * previousS->getPose(); // compute 3D words by epipolar geometry with the previous signature std::multimap inliers = util3d::generateWords3DMono( words, previousS->getWords(), data.cameraModels()[0], cameraTransform); // words3D should have the same size than words float bad_point = std::numeric_limits::quiet_NaN (); for(std::multimap::const_iterator iter=words.begin(); iter!=words.end(); ++iter) { std::multimap::iterator jter=inliers.find(iter->first); if(jter != inliers.end()) { words3D.insert(std::make_pair(iter->first, jter->second)); } else { words3D.insert(std::make_pair(iter->first, pcl::PointXYZ(bad_point,bad_point,bad_point))); } } t = timer.ticks(); UASSERT(words3D.size() == words.size()); if(stats) stats->addStatistic(Statistics::kTimingMemKeypoints_3D(), t*1000.0f); UDEBUG("time keypoints 3D (%d) = %fs", (int)keypoints3D->size(), t); } } cv::Mat image = data.imageRaw(); cv::Mat depthOrRightImage = data.depthOrRightRaw(); std::vector cameraModels = data.cameraModels(); StereoCameraModel stereoCameraModel = data.stereoCameraModel(); // apply decimation? if((this->isBinDataKept() || this->isRawDataKept()) && _imageDecimation > 1) { image = util2d::decimate(image, _imageDecimation); depthOrRightImage = util2d::decimate(depthOrRightImage, _imageDecimation); for(unsigned int i=0; i 0.0f) { laserScan = util3d::laserScanFromPointCloud(*util3d::voxelize(util3d::laserScanToPointCloud(laserScan), _laserScanVoxelSize)); } Signature * s; if(this->isBinDataKept()) { std::vector imageBytes; std::vector depthBytes; if(!depthOrRightImage.empty() && depthOrRightImage.type() == CV_32FC1) { UWARN("Keeping raw data in database: depth type is 32FC1, use 16UC1 depth format to avoid a conversion."); depthOrRightImage = util3d::cvtDepthFromFloat(depthOrRightImage); } rtabmap::CompressionThread ctImage(image, std::string(".jpg")); rtabmap::CompressionThread ctDepth(depthOrRightImage, std::string(".png")); rtabmap::CompressionThread ctDepth2d(laserScan); rtabmap::CompressionThread ctUserData(data.userDataRaw()); ctImage.start(); ctDepth.start(); ctDepth2d.start(); ctUserData.start(); ctImage.join(); ctDepth.join(); ctDepth2d.join(); ctUserData.join(); s = new Signature(id, _idMapCount, (data.imageRaw().empty()&&data.laserScanRaw().empty()&&words.size()==0)?-1:0, // tag intermediate nodes as weight=-1 data.stamp(), "", pose, stereoCameraModel.isValid()? SensorData( ctDepth2d.getCompressedData(), data.laserScanMaxPts(), ctImage.getCompressedData(), ctDepth.getCompressedData(), stereoCameraModel, id, 0, ctUserData.getCompressedData()): SensorData( ctDepth2d.getCompressedData(), data.laserScanMaxPts(), ctImage.getCompressedData(), ctDepth.getCompressedData(), cameraModels, id, 0, ctUserData.getCompressedData())); } else { rtabmap::CompressionThread ctDepth2d(laserScan); rtabmap::CompressionThread ctUserData(data.userDataRaw()); ctDepth2d.start(); ctUserData.start(); ctDepth2d.join(); ctUserData.join(); s = new Signature(id, _idMapCount, (data.imageRaw().empty()&&data.laserScanRaw().empty()&&words.size()==0)?-1:0, // tag intermediate nodes as weight=-1 data.stamp(), "", pose, stereoCameraModel.isValid()? SensorData( ctDepth2d.getCompressedData(), data.laserScanMaxPts(), cv::Mat(), cv::Mat(), stereoCameraModel, id, 0, ctUserData.getCompressedData()): SensorData( ctDepth2d.getCompressedData(), data.laserScanMaxPts(), cv::Mat(), cv::Mat(), cameraModels, id, 0, ctUserData.getCompressedData())); } s->setWords(words); s->setWords3(words3D); if(this->isRawDataKept()) { s->sensorData().setImageRaw(image); s->sensorData().setDepthOrRightRaw(depthOrRightImage); s->sensorData().setLaserScanRaw(laserScan, data.laserScanMaxPts()); s->sensorData().setUserDataRaw(data.userDataRaw()); } t = timer.ticks(); if(stats) stats->addStatistic(Statistics::kTimingMemCompressing_data(), t*1000.0f); UDEBUG("time compressing data (id=%d) %fs", id, t); if(words.size()) { s->setEnabled(true); // All references are already activated in the dictionary at this point (see _vwd->addNewWords()) } return s; } void Memory::disableWordsRef(int signatureId) { UDEBUG("id=%d", signatureId); Signature * ss = this->_getSignature(signatureId); if(ss && ss->isEnabled()) { const std::multimap & words = ss->getWords(); const std::list & keys = uUniqueKeys(words); int count = _vwd->getTotalActiveReferences(); // First remove all references for(std::list::const_iterator i=keys.begin(); i!=keys.end(); ++i) { _vwd->removeAllWordRef(*i, signatureId); } count -= _vwd->getTotalActiveReferences(); ss->setEnabled(false); UDEBUG("%d words total ref removed from signature %d... (total active ref = %d)", count, ss->id(), _vwd->getTotalActiveReferences()); } } void Memory::cleanUnusedWords() { if(_vwd->isIncremental()) { std::vector removedWords = _vwd->getUnusedWords(); UDEBUG("Removing %d words (dictionary size=%d)...", removedWords.size(), _vwd->getVisualWords().size()); if(removedWords.size()) { // remove them from the dictionary _vwd->removeWords(removedWords); for(unsigned int i=0; iasyncSave(removedWords[i]); } else { delete removedWords[i]; } } } } } void Memory::enableWordsRef(const std::list & signatureIds) { UDEBUG("size=%d", signatureIds.size()); UTimer timer; timer.start(); std::map refsToChange; // std::set oldWordIds; std::list surfSigns; for(std::list::const_iterator i=signatureIds.begin(); i!=signatureIds.end(); ++i) { Signature * ss = dynamic_cast(this->_getSignature(*i)); if(ss && !ss->isEnabled()) { surfSigns.push_back(ss); std::list uniqueKeys = uUniqueKeys(ss->getWords()); //Find words in the signature which they are not in the current dictionary for(std::list::const_iterator k=uniqueKeys.begin(); k!=uniqueKeys.end(); ++k) { if(_vwd->getWord(*k) == 0 && _vwd->getUnusedWord(*k) == 0) { oldWordIds.insert(oldWordIds.end(), *k); } } } } UDEBUG("oldWordIds.size()=%d, getOldIds time=%fs", oldWordIds.size(), timer.ticks()); // the words were deleted, so try to math it with an active word std::list vws; if(oldWordIds.size() && _dbDriver) { // get the descriptors _dbDriver->loadWords(oldWordIds, vws); } UDEBUG("loading words(%d) time=%fs", oldWordIds.size(), timer.ticks()); if(vws.size()) { //Search in the dictionary std::vector vwActiveIds = _vwd->findNN(vws); UDEBUG("find active ids (number=%d) time=%fs", vws.size(), timer.ticks()); int i=0; for(std::list::iterator iterVws=vws.begin(); iterVws!=vws.end(); ++iterVws) { if(vwActiveIds[i] > 0) { //UDEBUG("Match found %d with %d", (*iterVws)->id(), vwActiveIds[i]); refsToChange.insert(refsToChange.end(), std::pair((*iterVws)->id(), vwActiveIds[i])); if((*iterVws)->isSaved()) { delete (*iterVws); } else if(_dbDriver) { _dbDriver->asyncSave(*iterVws); } } else { //add to dictionary _vwd->addWord(*iterVws); // take ownership } ++i; } UDEBUG("Added %d to dictionary, time=%fs", vws.size()-refsToChange.size(), timer.ticks()); //update the global references map and update the signatures reactivated for(std::map::const_iterator iter=refsToChange.begin(); iter != refsToChange.end(); ++iter) { //uInsert(_wordRefsToChange, (const std::pair)*iter); // This will be used to change references in the database for(std::list::iterator j=surfSigns.begin(); j!=surfSigns.end(); ++j) { (*j)->changeWordsRef(iter->first, iter->second); } } UDEBUG("changing ref, total=%d, time=%fs", refsToChange.size(), timer.ticks()); } int count = _vwd->getTotalActiveReferences(); // Reactivate references and signatures for(std::list::iterator j=surfSigns.begin(); j!=surfSigns.end(); ++j) { const std::vector & keys = uKeys((*j)->getWords()); // Add all references for(std::vector::const_iterator i=keys.begin(); i!=keys.end(); ++i) { _vwd->addWordRef(*i, (*j)->id()); } if(keys.size()) { (*j)->setEnabled(true); } } count = _vwd->getTotalActiveReferences() - count; UDEBUG("%d words total ref added from %d signatures, time=%fs...", count, surfSigns.size(), timer.ticks()); } std::set Memory::reactivateSignatures(const std::list & ids, unsigned int maxLoaded, double & timeDbAccess) { // get the signatures, if not in the working memory, they // will be loaded from the database in an more efficient way // than how it is done in the Memory UDEBUG(""); UTimer timer; std::list idsToLoad; std::map::iterator wmIter; for(std::list::const_iterator i=ids.begin(); i!=ids.end(); ++i) { if(!this->getSignature(*i) && !uContains(idsToLoad, *i)) { if(!maxLoaded || idsToLoad.size() < maxLoaded) { idsToLoad.push_back(*i); UINFO("Loading location %d from database...", *i); } } } UDEBUG("idsToLoad = %d", idsToLoad.size()); std::list reactivatedSigns; if(_dbDriver) { _dbDriver->loadSignatures(idsToLoad, reactivatedSigns); } timeDbAccess = timer.getElapsedTime(); std::list idsLoaded; for(std::list::iterator i=reactivatedSigns.begin(); i!=reactivatedSigns.end(); ++i) { idsLoaded.push_back((*i)->id()); //append to working memory this->addSignatureToWm(*i); } this->enableWordsRef(idsLoaded); UDEBUG("time = %fs", timer.ticks()); return std::set(idsToLoad.begin(), idsToLoad.end()); } // return all non-null poses // return unique links between nodes (for neighbors: old->new, for loops: parent->child) void Memory::getMetricConstraints( const std::set & ids, std::map & poses, std::multimap & links, bool lookInDatabase) { UDEBUG(""); for(std::set::const_iterator iter=ids.begin(); iter!=ids.end(); ++iter) { Transform pose = getOdomPose(*iter, lookInDatabase); if(!pose.isNull()) { poses.insert(std::make_pair(*iter, pose)); } } for(std::set::const_iterator iter=ids.begin(); iter!=ids.end(); ++iter) { if(uContains(poses, *iter)) { std::map neighbors = this->getNeighborLinks(*iter, lookInDatabase); // only direct neighbors for(std::map::iterator jter=neighbors.begin(); jter!=neighbors.end(); ++jter) { if( jter->second.isValid() && uContains(poses, jter->first) && graph::findLink(links, *iter, jter->first) == links.end()) { if(!lookInDatabase) { Link link = jter->second; const Signature * s = this->getSignature(jter->first); UASSERT(s!=0); while(s && s->getWeight() == -1) { // skip to next neighbor, well we assume that bad signatures // are only linked by max 2 neighbor links. std::map n = this->getNeighborLinks(s->id(), false); UASSERT(n.size() <= 2); std::map::iterator uter = n.upper_bound(s->id()); if(uter != n.end()) { const Signature * s2 = this->getSignature(uter->first); if(s2) { link = link.merge(uter->second); poses.erase(s->id()); s = s2; } } else { break; } } links.insert(std::make_pair(*iter, link)); } else { links.insert(std::make_pair(*iter, jter->second)); } } } std::map loops = this->getLoopClosureLinks(*iter, lookInDatabase); for(std::map::iterator jter=loops.begin(); jter!=loops.end(); ++jter) { if( jter->second.isValid() && // null transform means a rehearsed location jter->first < *iter && // Loop parent to child uContains(poses, jter->first)) { links.insert(std::make_pair(*iter, jter->second)); } } } } } } // namespace rtabmap