/* Copyright (c) 2010-2016, 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 "rtabmap/core/Rtabmap.h" #include "rtabmap/core/Version.h" #include "rtabmap/core/Features2d.h" #include "rtabmap/core/Optimizer.h" #include "rtabmap/core/Graph.h" #include "rtabmap/core/Signature.h" #include "rtabmap/core/EpipolarGeometry.h" #include "rtabmap/core/util3d.h" #include "rtabmap/core/util3d_transforms.h" #include "rtabmap/core/util3d_filtering.h" #include "rtabmap/core/util3d_surface.h" #include "rtabmap/core/DBDriver.h" #include "rtabmap/core/Memory.h" #include "rtabmap/core/VWDictionary.h" #include "rtabmap/core/BayesFilter.h" #include "rtabmap/core/Compression.h" #include "rtabmap/core/Registration.h" #include "rtabmap/core/RegistrationInfo.h" #include #include #include #include #include #include #ifdef RTABMAP_PYTHON #include "rtabmap/core/PythonInterface.h" #endif #ifdef RTABMAP_MRPT // Used for odometry error propagation #include #endif #include #include #include #include #include #include #include #define LOG_F "LogF.txt" #define LOG_I "LogI.txt" #define GRAPH_FILE_NAME "Graph.dot" // // // // ======================================================= // MAIN LOOP, see method "void Rtabmap::process();" below. // ======================================================= // // // namespace rtabmap { Rtabmap::Rtabmap() : _publishStats(Parameters::defaultRtabmapPublishStats()), _publishLastSignatureData(Parameters::defaultRtabmapPublishLastSignature()), _publishPdf(Parameters::defaultRtabmapPublishPdf()), _publishLikelihood(Parameters::defaultRtabmapPublishLikelihood()), _publishRAMUsage(Parameters::defaultRtabmapPublishRAMUsage()), _computeRMSE(Parameters::defaultRtabmapComputeRMSE()), _saveWMState(Parameters::defaultRtabmapSaveWMState()), _maxTimeAllowed(Parameters::defaultRtabmapTimeThr()), // 700 ms _maxMemoryAllowed(Parameters::defaultRtabmapMemoryThr()), // 0=inf _loopThr(Parameters::defaultRtabmapLoopThr()), _loopRatio(Parameters::defaultRtabmapLoopRatio()), _aggressiveLoopThr(Parameters::defaultRGBDAggressiveLoopThr()), _virtualPlaceLikelihoodRatio(Parameters::defaultRtabmapVirtualPlaceLikelihoodRatio()), _maxLoopClosureDistance(Parameters::defaultRGBDMaxLoopClosureDistance()), _verifyLoopClosureHypothesis(Parameters::defaultVhEpEnabled()), _maxRetrieved(Parameters::defaultRtabmapMaxRetrieved()), _maxLocalRetrieved(Parameters::defaultRGBDMaxLocalRetrieved()), _maxRepublished(Parameters::defaultRtabmapMaxRepublished()), _rawDataKept(Parameters::defaultMemImageKept()), _statisticLogsBufferedInRAM(Parameters::defaultRtabmapStatisticLogsBufferedInRAM()), _statisticLogged(Parameters::defaultRtabmapStatisticLogged()), _statisticLoggedHeaders(Parameters::defaultRtabmapStatisticLoggedHeaders()), _rgbdSlamMode(Parameters::defaultRGBDEnabled()), _rgbdLinearUpdate(Parameters::defaultRGBDLinearUpdate()), _rgbdAngularUpdate(Parameters::defaultRGBDAngularUpdate()), _rgbdLinearSpeedUpdate(Parameters::defaultRGBDLinearSpeedUpdate()), _rgbdAngularSpeedUpdate(Parameters::defaultRGBDAngularSpeedUpdate()), _newMapOdomChangeDistance(Parameters::defaultRGBDNewMapOdomChangeDistance()), _neighborLinkRefining(Parameters::defaultRGBDNeighborLinkRefining()), _proximityByTime(Parameters::defaultRGBDProximityByTime()), _proximityBySpace(Parameters::defaultRGBDProximityBySpace()), _scanMatchingIdsSavedInLinks(Parameters::defaultRGBDScanMatchingIdsSavedInLinks()), _loopClosureIdentityGuess(Parameters::defaultRGBDLoopClosureIdentityGuess()), _localRadius(Parameters::defaultRGBDLocalRadius()), _localImmunizationRatio(Parameters::defaultRGBDLocalImmunizationRatio()), _proximityMaxGraphDepth(Parameters::defaultRGBDProximityMaxGraphDepth()), _proximityMaxPaths(Parameters::defaultRGBDProximityMaxPaths()), _proximityMaxNeighbors(Parameters::defaultRGBDProximityPathMaxNeighbors()), _proximityFilteringRadius(Parameters::defaultRGBDProximityPathFilteringRadius()), _proximityRawPosesUsed(Parameters::defaultRGBDProximityPathRawPosesUsed()), _proximityAngle(Parameters::defaultRGBDProximityAngle()*M_PI/180.0f), _proximityOdomGuess(Parameters::defaultRGBDProximityOdomGuess()), _proximityMergedScanCovFactor(Parameters::defaultRGBDProximityMergedScanCovFactor()), _databasePath(""), _optimizeFromGraphEnd(Parameters::defaultRGBDOptimizeFromGraphEnd()), _optimizationMaxError(Parameters::defaultRGBDOptimizeMaxError()), _optimizationMaxErrorRepairRadius(Parameters::defaultRGBDOptimizeMaxErrorRepairRadius()), _startNewMapOnLoopClosure(Parameters::defaultRtabmapStartNewMapOnLoopClosure()), _startNewMapOnGoodSignature(Parameters::defaultRtabmapStartNewMapOnGoodSignature()), _goalReachedRadius(Parameters::defaultRGBDGoalReachedRadius()), _goalsSavedInUserData(Parameters::defaultRGBDGoalsSavedInUserData()), _pathStuckIterations(Parameters::defaultRGBDPlanStuckIterations()), _pathLinearVelocity(Parameters::defaultRGBDPlanLinearVelocity()), _pathAngularVelocity(Parameters::defaultRGBDPlanAngularVelocity()), _forceOdom3doF(Parameters::defaultRGBDForceOdom3DoF()), _restartAtOrigin(Parameters::defaultRGBDStartAtOrigin()), _loopCovLimited(Parameters::defaultRGBDLoopCovLimited()), _loopGPS(Parameters::defaultRtabmapLoopGPS()), _maxOdomCacheSize(Parameters::defaultRGBDMaxOdomCacheSize()), _localizationSmoothing(Parameters::defaultRGBDLocalizationSmoothing()), _localizationPriorInf(1.0/(Parameters::defaultRGBDLocalizationPriorError()*Parameters::defaultRGBDLocalizationPriorError())), _localizationSecondTryWithoutProximityLinks(Parameters::defaultRGBDLocalizationSecondTryWithoutProximityLinks()), _createGlobalScanMap(Parameters::defaultRGBDProximityGlobalScanMap()), _markerPriorsLinearVariance(Parameters::defaultMarkerPriorsVarianceLinear()), _markerPriorsAngularVariance(Parameters::defaultMarkerPriorsVarianceAngular()), _loopClosureHypothesis(0,0.0f), _highestHypothesis(0,0.0f), _lastProcessTime(0.0), _someNodesHaveBeenTransferred(false), _distanceTravelled(0.0f), _distanceTravelledSinceLastLocalization(0.0f), _optimizeFromGraphEndChanged(false), _epipolarGeometry(0), _bayesFilter(0), _graphOptimizer(0), _memory(0), _foutFloat(0), _foutInt(0), _wDir(""), _mapCorrection(Transform::getIdentity()), _lastLocalizationNodeId(0), _currentSessionHasGPS(false), _lastRejectedLoopClosureIds(0,0), _pathStatus(0), _pathCurrentIndex(0), _pathGoalIndex(0), _pathTransformToGoal(Transform::getIdentity()), _pathStuckCount(0), _pathStuckDistance(0.0f), _dummyDictionary(false) { #ifdef RTABMAP_PYTHON // Ensure the embedded Python interpreter is up. The first call here will // assert that it runs on the main thread; callers building Rtabmap on a // worker thread should construct the singleton in main() beforehand. PythonInterface::instance("Rtabmap"); #endif } Rtabmap::~Rtabmap() { UDEBUG(""); this->close(); } void Rtabmap::setupLogFiles(bool overwrite) { flushStatisticLogs(); // Log files if(_foutFloat) { fclose(_foutFloat); _foutFloat = 0; } if(_foutInt) { fclose(_foutInt); _foutInt = 0; } if(_statisticLogged && !_wDir.empty()) { std::string attributes = "a+"; // append to log files if(overwrite) { // If a file with the same name already exists // its content is erased and the file is treated // as a new empty file. attributes = "w"; } bool addLogFHeader = overwrite || !UFile::exists(_wDir+"/"+LOG_F); bool addLogIHeader = overwrite || !UFile::exists(_wDir+"/"+LOG_I); #ifdef _MSC_VER fopen_s(&_foutFloat, (_wDir+"/"+LOG_F).c_str(), attributes.c_str()); fopen_s(&_foutInt, (_wDir+"/"+LOG_I).c_str(), attributes.c_str()); #else _foutFloat = fopen((_wDir+"/"+LOG_F).c_str(), attributes.c_str()); _foutInt = fopen((_wDir+"/"+LOG_I).c_str(), attributes.c_str()); #endif // add header (column identification) if(_statisticLoggedHeaders && addLogFHeader && _foutFloat) { fprintf(_foutFloat, "Column headers:\n"); fprintf(_foutFloat, " 1-Total iteration time (s)\n"); fprintf(_foutFloat, " 2-Memory update time (s)\n"); fprintf(_foutFloat, " 3-Retrieval time (s)\n"); fprintf(_foutFloat, " 4-Likelihood time (s)\n"); fprintf(_foutFloat, " 5-Posterior time (s)\n"); fprintf(_foutFloat, " 6-Hypothesis selection time (s)\n"); fprintf(_foutFloat, " 7-Hypothesis validation time (s)\n"); fprintf(_foutFloat, " 8-Transfer time (s)\n"); fprintf(_foutFloat, " 9-Statistics creation time (s)\n"); fprintf(_foutFloat, " 10-Loop closure hypothesis value\n"); fprintf(_foutFloat, " 11-NAN\n"); fprintf(_foutFloat, " 12-NAN\n"); fprintf(_foutFloat, " 13-NAN\n"); fprintf(_foutFloat, " 14-NAN\n"); fprintf(_foutFloat, " 15-NAN\n"); fprintf(_foutFloat, " 16-Virtual place hypothesis\n"); fprintf(_foutFloat, " 17-Join trash time (s)\n"); fprintf(_foutFloat, " 18-Weight Update (rehearsal) similarity\n"); fprintf(_foutFloat, " 19-Empty trash time (s)\n"); fprintf(_foutFloat, " 20-Retrieval database access time (s)\n"); fprintf(_foutFloat, " 21-Add loop closure link time (s)\n"); fprintf(_foutFloat, " 22-Memory cleanup time (s)\n"); fprintf(_foutFloat, " 23-Scan matching (odometry correction) time (s)\n"); fprintf(_foutFloat, " 24-Local time loop closure detection time (s)\n"); fprintf(_foutFloat, " 25-Local space loop closure detection time (s)\n"); fprintf(_foutFloat, " 26-Map optimization (s)\n"); } if(_statisticLoggedHeaders && addLogIHeader && _foutInt) { fprintf(_foutInt, "Column headers:\n"); fprintf(_foutInt, " 1-Loop closure ID\n"); fprintf(_foutInt, " 2-Highest loop closure hypothesis\n"); fprintf(_foutInt, " 3-Locations transferred\n"); fprintf(_foutInt, " 4-NAN\n"); fprintf(_foutInt, " 5-Words extracted from the last image\n"); fprintf(_foutInt, " 6-Vocabulary size\n"); fprintf(_foutInt, " 7-Working memory size\n"); fprintf(_foutInt, " 8-Is loop closure hypothesis rejected?\n"); fprintf(_foutInt, " 9-NAN\n"); fprintf(_foutInt, " 10-NAN\n"); fprintf(_foutInt, " 11-Locations retrieved\n"); fprintf(_foutInt, " 12-Retrieval location ID\n"); fprintf(_foutInt, " 13-Unique words extraced from last image\n"); fprintf(_foutInt, " 14-Retrieval ID\n"); fprintf(_foutInt, " 15-Non-null likelihood values\n"); fprintf(_foutInt, " 16-Weight Update ID\n"); fprintf(_foutInt, " 17-Is last location merged through Weight Update?\n"); fprintf(_foutInt, " 18-Local graph size\n"); fprintf(_foutInt, " 19-Sensor data id\n"); fprintf(_foutInt, " 20-Indexed words\n"); fprintf(_foutInt, " 21-Index memory usage (KB)\n"); } ULOGGER_DEBUG("Log file (int)=%s", (_wDir+"/"+LOG_I).c_str()); ULOGGER_DEBUG("Log file (float)=%s", (_wDir+"/"+LOG_F).c_str()); } else { if(_statisticLogged) { UWARN("Working directory is not set, log disabled!"); } UDEBUG("Log disabled!"); } } void Rtabmap::flushStatisticLogs() { if(_foutFloat && _bufferedLogsF.size()) { UDEBUG("_bufferedLogsF.size=%d", (int)_bufferedLogsF.size()); for(std::list::iterator iter = _bufferedLogsF.begin(); iter!=_bufferedLogsF.end(); ++iter) { fprintf(_foutFloat, "%s", iter->c_str()); } _bufferedLogsF.clear(); } if(_foutInt && _bufferedLogsI.size()) { UDEBUG("_bufferedLogsI.size=%d", (int)_bufferedLogsI.size()); for(std::list::iterator iter = _bufferedLogsI.begin(); iter!=_bufferedLogsI.end(); ++iter) { fprintf(_foutInt, "%s", iter->c_str()); } _bufferedLogsI.clear(); } } void Rtabmap::init(const ParametersMap & parameters, const std::string & databasePath, bool loadDatabaseParameters) { UDEBUG("path=%s", databasePath.c_str()); _databasePath = databasePath; if(!_databasePath.empty()) { UASSERT(UFile::getExtension(_databasePath).compare("db") == 0); UINFO("Using database \"%s\".", _databasePath.c_str()); } else { UWARN("Using empty database. Mapping session will not be saved unless it is closed with an output database path."); } bool newDatabase = _databasePath.empty() || !UFile::exists(_databasePath); ParametersMap allParameters; if(!newDatabase && loadDatabaseParameters) { DBDriver * driver = DBDriver::create(); if(driver->openConnection(_databasePath, false)) { allParameters = driver->getLastParameters(); // ignore working directory (we may be on a different computer) allParameters.erase(Parameters::kRtabmapWorkingDirectory()); } delete driver; } uInsert(allParameters, parameters); ParametersMap::const_iterator iter; if((iter=allParameters.find(Parameters::kRtabmapWorkingDirectory())) != allParameters.end()) { this->setWorkingDirectory(iter->second.c_str()); } // If doesn't exist, create a memory if(!_memory) { _memory = new Memory(allParameters); if(_dummyDictionary) { _memory->setDummyDictionary(true); } _memory->init(_databasePath, false, allParameters, true); } _optimizedPoses.clear(); _constraints.clear(); _globalScanMap.clear(); _globalScanMapPoses.clear(); _odomCachePoses.clear(); _odomCacheConstraints.clear(); _nodesToRepublish.clear(); // Parse all parameters this->parseParameters(allParameters); Transform lastPose; _optimizedPoses = _memory->loadOptimizedPoses(&lastPose); if(!_memory->isIncremental()) { if(_optimizedPoses.empty() && _memory->getWorkingMemSize(true) > 0) { cv::Mat cov; this->optimizeCurrentMap( _memory->getLastWorkingSignature(true)->id(), false, _optimizedPoses, cov, &_constraints); } if(_optimizedPoses.lower_bound(1) != _optimizedPoses.end()) { if(_restartAtOrigin) { UWARN("last localization pose is ignored (%s=true), assuming we start at the first node of the map.", Parameters::kRGBDStartAtOrigin().c_str()); lastPose = _optimizedPoses.lower_bound(1)->second; } _lastLocalizationPose = lastPose; UINFO("Loaded optimizedPoses=%d firstPose %d=%s lastLocalizationPose=%s", (int)_optimizedPoses.size(), _optimizedPoses.lower_bound(1)->first, _optimizedPoses.lower_bound(1)->second.prettyPrint().c_str(), _lastLocalizationPose.prettyPrint().c_str()); if(_constraints.empty()) { std::map tmp; // Get just the links _memory->getMetricConstraints(uKeysSet(_optimizedPoses), tmp, _constraints, false, true); } // Initialize Bayes' prediction matrix UTimer time; std::map likelihood; likelihood.insert(std::make_pair(Memory::kIdVirtual, 1)); for(std::map::iterator iter=_optimizedPoses.lower_bound(1); iter!=_optimizedPoses.end(); ++iter) { if(_memory->getSignature(iter->first)) { likelihood.insert(std::make_pair(iter->first, 0)); } } _bayesFilter->computePosterior(_memory, likelihood); UINFO("Time initializing Bayes' prediction with %ld nodes: %fs", _optimizedPoses.size(), time.ticks()); if(_createGlobalScanMap) createGlobalScanMap(); } else { UINFO("Loaded optimizedPoses=0, last localization pose is ignored!"); } } else { _lastLocalizationPose = lastPose; if(!_optimizedPoses.empty()) { std::map tmp; // Get just the links _memory->getMetricConstraints(uKeysSet(_optimizedPoses), tmp, _constraints, false, true); } } if(_databasePath.empty()) { _statisticLogged = false; } setupLogFiles(newDatabase); } void Rtabmap::init(const std::string & configFile, const std::string & databasePath, bool loadDatabaseParameters) { // fill ctrl struct with values from the configuration file ParametersMap param;// = Parameters::defaultParameters; if(!configFile.empty()) { ULOGGER_DEBUG("Read parameters from = %s", configFile.c_str()); Parameters::readINI(configFile, param); } this->init(param, databasePath, loadDatabaseParameters); } void Rtabmap::close(bool databaseSaved, const std::string & ouputDatabasePath) { UINFO("databaseSaved=%d", databaseSaved?1:0); _highestHypothesis = std::make_pair(0,0.0f); _loopClosureHypothesis = std::make_pair(0,0.0f); _lastProcessTime = 0.0; _someNodesHaveBeenTransferred = false; _constraints.clear(); _mapCorrection.setIdentity(); _mapCorrectionBackup.setNull(); _localizationCovariance = cv::Mat(); _lastLocalizationNodeId = 0; _odomCachePoses.clear(); _odomCacheConstraints.clear(); _distanceTravelled = 0.0f; _distanceTravelledSinceLastLocalization = 0.0f; _optimizeFromGraphEndChanged = false; this->clearPath(0); _gpsGeocentricCache.clear(); _currentSessionHasGPS = false; _globalScanMap.clear(); _globalScanMapPoses.clear(); _nodesToRepublish.clear(); flushStatisticLogs(); if(_foutFloat) { fclose(_foutFloat); _foutFloat = 0; } if(_foutInt) { fclose(_foutInt); _foutInt = 0; } if(_epipolarGeometry) { delete _epipolarGeometry; _epipolarGeometry = 0; } if(_memory) { if(_memory->isReadOnly() && databaseSaved) { UWARN("Database is read-only, latest optimized poses, latest localization pose and latest state of the memory are not saved."); databaseSaved = false; } if(databaseSaved) { if(_memory->isGraphReduced() && _memory->isIncremental()) { // Force reducing graph, then remove filtered nodes from the optimized poses std::map reducedIds; _memory->incrementMapId(&reducedIds); for(std::map::iterator iter=reducedIds.begin(); iter!=reducedIds.end(); ++iter) { _optimizedPoses.erase(iter->first); } } _memory->saveOptimizedPoses(_optimizedPoses, _lastLocalizationPose); } _memory->close(databaseSaved, true, ouputDatabasePath); delete _memory; _memory = 0; } _optimizedPoses.clear(); _lastLocalizationPose.setNull(); if(_bayesFilter) { delete _bayesFilter; _bayesFilter = 0; } if(_graphOptimizer) { delete _graphOptimizer; _graphOptimizer = 0; } _databasePath.clear(); parseParameters(Parameters::getDefaultParameters()); // reset to default parameters _parameters.clear(); } void Rtabmap::parseParameters(const ParametersMap & parameters) { uInsert(_parameters, parameters); // place this before changing working directory Parameters::parse(parameters, Parameters::kRtabmapStatisticLogsBufferedInRAM(), _statisticLogsBufferedInRAM); Parameters::parse(parameters, Parameters::kRtabmapStatisticLogged(), _statisticLogged); Parameters::parse(parameters, Parameters::kRtabmapStatisticLoggedHeaders(), _statisticLoggedHeaders); ULOGGER_DEBUG(""); ParametersMap::const_iterator iter; if((iter=parameters.find(Parameters::kRtabmapWorkingDirectory())) != parameters.end()) { this->setWorkingDirectory(iter->second.c_str()); } Parameters::parse(parameters, Parameters::kRtabmapPublishStats(), _publishStats); Parameters::parse(parameters, Parameters::kRtabmapPublishLastSignature(), _publishLastSignatureData); Parameters::parse(parameters, Parameters::kRtabmapPublishPdf(), _publishPdf); Parameters::parse(parameters, Parameters::kRtabmapPublishLikelihood(), _publishLikelihood); Parameters::parse(parameters, Parameters::kRtabmapPublishRAMUsage(), _publishRAMUsage); Parameters::parse(parameters, Parameters::kRtabmapComputeRMSE(), _computeRMSE); Parameters::parse(parameters, Parameters::kRtabmapSaveWMState(), _saveWMState); Parameters::parse(parameters, Parameters::kRtabmapTimeThr(), _maxTimeAllowed); Parameters::parse(parameters, Parameters::kRtabmapMemoryThr(), _maxMemoryAllowed); Parameters::parse(parameters, Parameters::kRtabmapLoopThr(), _loopThr); Parameters::parse(parameters, Parameters::kRtabmapLoopRatio(), _loopRatio); Parameters::parse(parameters, Parameters::kRGBDAggressiveLoopThr(), _aggressiveLoopThr); Parameters::parse(parameters, Parameters::kRtabmapVirtualPlaceLikelihoodRatio(), _virtualPlaceLikelihoodRatio); Parameters::parse(parameters, Parameters::kRGBDMaxLoopClosureDistance(), _maxLoopClosureDistance); Parameters::parse(parameters, Parameters::kVhEpEnabled(), _verifyLoopClosureHypothesis); Parameters::parse(parameters, Parameters::kRtabmapMaxRetrieved(), _maxRetrieved); Parameters::parse(parameters, Parameters::kRGBDMaxLocalRetrieved(), _maxLocalRetrieved); Parameters::parse(parameters, Parameters::kRtabmapMaxRepublished(), _maxRepublished); if(_maxRepublished == 0 || !_publishLastSignatureData) { _nodesToRepublish.clear(); } Parameters::parse(parameters, Parameters::kMemImageKept(), _rawDataKept); Parameters::parse(parameters, Parameters::kRGBDEnabled(), _rgbdSlamMode); Parameters::parse(parameters, Parameters::kRGBDLinearUpdate(), _rgbdLinearUpdate); Parameters::parse(parameters, Parameters::kRGBDAngularUpdate(), _rgbdAngularUpdate); Parameters::parse(parameters, Parameters::kRGBDLinearSpeedUpdate(), _rgbdLinearSpeedUpdate); Parameters::parse(parameters, Parameters::kRGBDAngularSpeedUpdate(), _rgbdAngularSpeedUpdate); Parameters::parse(parameters, Parameters::kRGBDNewMapOdomChangeDistance(), _newMapOdomChangeDistance); Parameters::parse(parameters, Parameters::kRGBDNeighborLinkRefining(), _neighborLinkRefining); Parameters::parse(parameters, Parameters::kRGBDProximityByTime(), _proximityByTime); Parameters::parse(parameters, Parameters::kRGBDProximityBySpace(), _proximityBySpace); Parameters::parse(parameters, Parameters::kRGBDScanMatchingIdsSavedInLinks(), _scanMatchingIdsSavedInLinks); Parameters::parse(parameters, Parameters::kRGBDLoopClosureIdentityGuess(), _loopClosureIdentityGuess); Parameters::parse(parameters, Parameters::kRGBDLocalRadius(), _localRadius); Parameters::parse(parameters, Parameters::kRGBDLocalImmunizationRatio(), _localImmunizationRatio); Parameters::parse(parameters, Parameters::kRGBDProximityMaxGraphDepth(), _proximityMaxGraphDepth); Parameters::parse(parameters, Parameters::kRGBDProximityMaxPaths(), _proximityMaxPaths); Parameters::parse(parameters, Parameters::kRGBDProximityPathMaxNeighbors(), _proximityMaxNeighbors); Parameters::parse(parameters, Parameters::kRGBDProximityPathFilteringRadius(), _proximityFilteringRadius); Parameters::parse(parameters, Parameters::kRGBDProximityPathRawPosesUsed(), _proximityRawPosesUsed); if(Parameters::parse(parameters, Parameters::kRGBDProximityAngle(), _proximityAngle)) { _proximityAngle *= M_PI/180.0f; } Parameters::parse(parameters, Parameters::kRGBDProximityOdomGuess(), _proximityOdomGuess); Parameters::parse(parameters, Parameters::kRGBDProximityMergedScanCovFactor(), _proximityMergedScanCovFactor); UASSERT(_proximityMergedScanCovFactor>0.0); bool optimizeFromGraphEndPrevious = _optimizeFromGraphEnd; Parameters::parse(parameters, Parameters::kRGBDOptimizeFromGraphEnd(), _optimizeFromGraphEnd); if(optimizeFromGraphEndPrevious != _optimizeFromGraphEnd && !_optimizedPoses.empty()) { _optimizeFromGraphEndChanged = true; } Parameters::parse(parameters, Parameters::kRGBDOptimizeMaxError(), _optimizationMaxError); Parameters::parse(parameters, Parameters::kRGBDOptimizeMaxErrorRepairRadius(), _optimizationMaxErrorRepairRadius); Parameters::parse(parameters, Parameters::kRtabmapStartNewMapOnLoopClosure(), _startNewMapOnLoopClosure); Parameters::parse(parameters, Parameters::kRtabmapStartNewMapOnGoodSignature(), _startNewMapOnGoodSignature); Parameters::parse(parameters, Parameters::kRGBDGoalReachedRadius(), _goalReachedRadius); Parameters::parse(parameters, Parameters::kRGBDGoalsSavedInUserData(), _goalsSavedInUserData); Parameters::parse(parameters, Parameters::kRGBDPlanStuckIterations(), _pathStuckIterations); Parameters::parse(parameters, Parameters::kRGBDPlanLinearVelocity(), _pathLinearVelocity); Parameters::parse(parameters, Parameters::kRGBDPlanAngularVelocity(), _pathAngularVelocity); Parameters::parse(parameters, Parameters::kRGBDForceOdom3DoF(), _forceOdom3doF); Parameters::parse(parameters, Parameters::kRGBDStartAtOrigin(), _restartAtOrigin); Parameters::parse(parameters, Parameters::kRGBDLoopCovLimited(), _loopCovLimited); Parameters::parse(parameters, Parameters::kRtabmapLoopGPS(), _loopGPS); Parameters::parse(parameters, Parameters::kRGBDMaxOdomCacheSize(), _maxOdomCacheSize); Parameters::parse(parameters, Parameters::kRGBDLocalizationSmoothing(), _localizationSmoothing); double localizationPriorError = Parameters::defaultRGBDLocalizationPriorError(); Parameters::parse(parameters, Parameters::kRGBDLocalizationPriorError(), localizationPriorError); UASSERT(localizationPriorError>0.0); _localizationPriorInf = 1.0/(localizationPriorError*localizationPriorError); Parameters::parse(parameters, Parameters::kRGBDLocalizationSecondTryWithoutProximityLinks(), _localizationSecondTryWithoutProximityLinks); Parameters::parse(parameters, Parameters::kRGBDProximityGlobalScanMap(), _createGlobalScanMap); Parameters::parse(parameters, Parameters::kMarkerPriorsVarianceLinear(), _markerPriorsLinearVariance); UASSERT(_markerPriorsLinearVariance>0.0f); Parameters::parse(parameters, Parameters::kMarkerPriorsVarianceAngular(), _markerPriorsAngularVariance); UASSERT(_markerPriorsAngularVariance>0.0f); std::string markerPriorsStr; if(Parameters::parse(parameters, Parameters::kMarkerPriors(), markerPriorsStr)) { _markerPriors.clear(); std::list strList = uSplit(markerPriorsStr, '|'); for(std::list::iterator iter=strList.begin(); iter!=strList.end(); ++iter) { std::string markerStr = *iter; while(!markerStr.empty() && !uIsDigit(markerStr[0])) { markerStr.erase(markerStr.begin()); } if(!markerStr.empty()) { std::string idStr = uSplitNumChar(markerStr).front(); int id = uStr2Int(idStr); Transform prior = Transform::fromString(markerStr.substr(idStr.size())); if(!prior.isNull() && id>0) { _markerPriors.insert(std::make_pair(-id, prior)); UDEBUG("Added landmark prior %d: %s", id, prior.prettyPrint().c_str()); } else { UERROR("Failed to parse element \"%s\" in parameter %s", markerStr.c_str(), Parameters::kMarkerPriors().c_str()); } } else if(!iter->empty()) { UERROR("Failed to parse parameter %s, value=\"%s\"", Parameters::kMarkerPriors().c_str(), iter->c_str()); } } } UASSERT(_rgbdLinearUpdate >= 0.0f); UASSERT(_rgbdAngularUpdate >= 0.0f); UASSERT(_rgbdLinearSpeedUpdate >= 0.0f); UASSERT(_rgbdAngularSpeedUpdate >= 0.0f); UASSERT(_maxOdomCacheSize >= 0); // By default, we create our strategies if they are not already created. // If they already exists, we check the parameters if a change is requested // Graph optimizer Optimizer::Type optimizerType = Optimizer::kTypeUndef; if((iter=parameters.find(Parameters::kOptimizerStrategy())) != parameters.end()) { optimizerType = (Optimizer::Type)std::atoi((*iter).second.c_str()); } if(optimizerType!=Optimizer::kTypeUndef) { UDEBUG("new detector strategy %d", int(optimizerType)); if(_graphOptimizer) { delete _graphOptimizer; _graphOptimizer = 0; } _graphOptimizer = Optimizer::create(optimizerType, _parameters); } else if(_graphOptimizer) { _graphOptimizer->parseParameters(parameters); } else { optimizerType = (Optimizer::Type)Parameters::defaultOptimizerStrategy(); _graphOptimizer = Optimizer::create(optimizerType, parameters); } if(!_createGlobalScanMap) { _globalScanMap.clear(); _globalScanMapPoses.clear(); } if(_memory) { bool isMemIncremental = _memory->isIncremental(); if(Parameters::parse(parameters, Parameters::kMemIncrementalMemory(), isMemIncremental) && isMemIncremental != _memory->isIncremental()) { // Mode has changed from Mapping to Localization, cleanup the local graph if(_memory->isIncremental()) { if(_memory->isGraphReduced()) { // Force reducing graph, then remove filtered nodes from the optimized poses std::map reducedIds; _memory->incrementMapId(&reducedIds); for(std::map::iterator iter=reducedIds.begin(); iter!=reducedIds.end(); ++iter) { _optimizedPoses.erase(iter->first); } } _odomCachePoses.clear(); _odomCacheConstraints.clear(); } // In both cases, we save the latest optimized graph and latest localization pose _memory->saveOptimizedPoses(_optimizedPoses, _lastLocalizationPose); } _memory->parseParameters(parameters); if(_memory->isIncremental() && !_globalScanMap.empty()) { UWARN("Map is now incremental, clearing global scan map..."); _globalScanMap.clear(); _globalScanMapPoses.clear(); } if(_createGlobalScanMap && !_memory->isIncremental() && _globalScanMap.empty() && !_optimizedPoses.empty()) { this->createGlobalScanMap(); } } if(!_epipolarGeometry) { _epipolarGeometry = new EpipolarGeometry(_parameters); } else { _epipolarGeometry->parseParameters(parameters); } // Bayes filter, create one if not exists if(!_bayesFilter) { _bayesFilter = new BayesFilter(_parameters); } else { _bayesFilter->parseParameters(parameters); } } int Rtabmap::getLastLocationId() const { int id = 0; if(_memory) { id = _memory->getLastSignatureId(); } return id; } std::list Rtabmap::getWM() const { std::list mem; if(_memory) { mem = uKeysList(_memory->getWorkingMem()); mem.remove(-1);// Ignore the virtual signature (if here) } return mem; } int Rtabmap::getWMSize() const { if(_memory) { return (int)_memory->getWorkingMemSize(false); } return 0; } std::map Rtabmap::getWeights() const { std::map weights; if(_memory) { weights = _memory->getWeights(); weights.erase(-1);// Ignore the virtual signature (if here) } return weights; } std::set Rtabmap::getSTM() const { if(_memory) { return _memory->getStMem(); } return std::set(); } int Rtabmap::getSTMSize() const { if(_memory) { return (int)_memory->getStMem().size(); } return 0; } int Rtabmap::getTotalMemSize() const { if(_memory) { const Signature * s =_memory->getLastWorkingSignature(false); if(s) { return s->id(); } } return 0; } bool Rtabmap::isInSTM(int locationId) const { if(_memory) { return _memory->isInSTM(locationId); } return false; } bool Rtabmap::isIDsGenerated() const { if(_memory) { return _memory->isIDsGenerated(); } return Parameters::defaultMemGenerateIds(); } const Statistics & Rtabmap::getStatistics() const { return statistics_; } Transform Rtabmap::getPose(int locationId) const { return uValue(_optimizedPoses, locationId, Transform()); } void Rtabmap::setInitialPose(const Transform & initialPose) { if(_memory) { if(!_memory->isIncremental()) { _lastLocalizationPose = initialPose; _localizationCovariance = cv::Mat(); _lastLocalizationNodeId = 0; _odomCachePoses.clear(); _odomCacheConstraints.clear(); _mapCorrection.setIdentity(); _mapCorrectionBackup.setNull(); if(_memory->getLastWorkingSignature(true)->id() && _optimizedPoses.empty()) { cv::Mat covariance; this->optimizeCurrentMap(_memory->getLastWorkingSignature(true)->id(), false, _optimizedPoses, covariance, &_constraints); } } else { UWARN("Initial pose can only be set in localization mode (%s=false), ignoring it...", Parameters::kMemIncrementalMemory().c_str()); } } } int Rtabmap::triggerNewMap() { int mapId = -1; if(_memory) { _localizationCovariance = cv::Mat(); _lastLocalizationNodeId = 0; _odomCachePoses.clear(); _odomCacheConstraints.clear(); _distanceTravelled = 0.0f; _distanceTravelledSinceLastLocalization = 0.0f; if(!_memory->isIncremental()) { _mapCorrection.setIdentity(); if(_restartAtOrigin) { _lastLocalizationPose.setIdentity(); } return mapId; } std::map reducedIds; mapId = _memory->incrementMapId(&reducedIds); UINFO("New map triggered, new map = %d", mapId); _optimizedPoses.clear(); _constraints.clear(); _lastRejectedLoopClosureIds = std::make_pair(0,0); if(_bayesFilter) { _bayesFilter->reset(); } //Verify if there are nodes that were merged through graph reduction if(reducedIds.size() && _path.size()) { for(unsigned int i=0; i<_path.size(); ++i) { std::map::const_iterator iter = reducedIds.find(_path[i].first); if(iter!= reducedIds.end()) { // change path ID to loop closure ID _path[i].first = iter->second; } } } } return mapId; } bool Rtabmap::labelLocation(int id, const std::string & label) { if(_memory) { if(id > 0) { return _memory->labelSignature(id, label); } else if(_memory->isIncremental() && _memory->getLastWorkingSignature(true)) { return _memory->labelSignature(_memory->getLastWorkingSignature(true)->id(), label); } else if(!_memory->isIncremental() && !_lastLocalizationPose.isNull() && !_lastLocalizationPose.isIdentity()) { std::map nearestNodes = getNodesInRadius(_lastLocalizationPose, _localRadius, 1); if(!nearestNodes.empty()) { return _memory->labelSignature(nearestNodes.begin()->first, label); } else { UERROR("No nodes found inside %s=%fm of the current pose (%s). Cannot set label \"%s\"", Parameters::kRGBDLocalRadius().c_str(), _localRadius, _lastLocalizationPose.prettyPrint().c_str(), label.c_str()); } } else { UERROR("Last signature is null! Cannot set label \"%s\"", label.c_str()); } } return false; } bool Rtabmap::setUserData(int id, const cv::Mat & data) { if(_memory) { if(id > 0) { return _memory->setUserData(id, data); } else if(_memory->getLastWorkingSignature(true)) { return _memory->setUserData(_memory->getLastWorkingSignature(true)->id(), data); } else { UERROR("Last signature is null! Cannot set user data!"); } } return false; } void Rtabmap::generateDOTGraph(const std::string & path, int id, int margin) { if(_memory) { _memory->joinTrashThread(); // make sure the trash is flushed if(id > 0) { std::map ids = _memory->getNeighborsId(id, margin, -1, false); if(ids.size() > 0) { ids.insert(std::pair(id, 0)); std::set idsSet; for(std::map::iterator iter = ids.begin(); iter!=ids.end(); ++iter) { idsSet.insert(idsSet.end(), iter->first); } _memory->generateGraph(path, idsSet); } else { UERROR("No neighbors found for signature %d.", id); } } else { _memory->generateGraph(path); } } } void Rtabmap::exportPoses(const std::string & path, bool optimized, bool global, int format) { if(_memory && _memory->getLastWorkingSignature(!global)) { std::map poses; std::multimap constraints; if(optimized) { cv::Mat covariance; this->optimizeCurrentMap(_memory->getLastWorkingSignature(!global)->id(), global, poses, covariance, &constraints); } else { std::map ids = _memory->getNeighborsId(_memory->getLastWorkingSignature(!global)->id(), 0, global?-1:0, true); _memory->getMetricConstraints(uKeysSet(ids), poses, constraints, global); } std::map stamps; if(format == 1 || format == 10 || format == 11) { for(std::map::iterator iter=poses.begin(); iter!=poses.end(); ++iter) { Transform o,g; int m, w; std::string l; double stamp = 0.0; std::vector v; GPS gps; EnvSensors sensors; _memory->getNodeInfo(iter->first, o, m, w, l, stamp, g, v, gps, sensors, true); stamps.insert(std::make_pair(iter->first, stamp)); } } graph::exportPoses(path, format, poses, constraints, stamps, _parameters); } } void Rtabmap::resetMemory() { UDEBUG(""); _highestHypothesis = std::make_pair(0,0.0f); _loopClosureHypothesis = std::make_pair(0,0.0f); _lastProcessTime = 0.0; _someNodesHaveBeenTransferred = false; _optimizedPoses.clear(); _constraints.clear(); _mapCorrection.setIdentity(); _mapCorrectionBackup.setNull(); _lastLocalizationPose.setNull(); _localizationCovariance = cv::Mat(); _lastLocalizationNodeId = 0; _odomCachePoses.clear(); _odomCacheConstraints.clear(); _distanceTravelled = 0.0f; _distanceTravelledSinceLastLocalization = 0.0f; _optimizeFromGraphEndChanged = false; _globalScanMap.clear(); _globalScanMapPoses.clear(); _nodesToRepublish.clear(); _lastRejectedLoopClosureIds = std::make_pair(0,0); this->clearPath(0); if(_memory) { if(_memory->isReadOnly()) { UWARN("Memory is reset but the database won't be cleared because read-only mode is enabled."); } _memory->init(_databasePath, true, _parameters, true); if(_memory->getLastWorkingSignature(true)) { cv::Mat covariance; optimizeCurrentMap(_memory->getLastWorkingSignature(true)->id(), false, _optimizedPoses, covariance, &_constraints); } if(_bayesFilter) { _bayesFilter->reset(); } } else { UERROR("RTAB-Map is not initialized. No memory to reset..."); } if(_graphOptimizer) { delete _graphOptimizer; _graphOptimizer = Optimizer::create(_parameters); } this->setupLogFiles(true); } class NearestPathKey { public: NearestPathKey(float l, int i, float d) : likelihood(l), id(i), distance(d){} bool operator<(const NearestPathKey & k) const { if(likelihood < k.likelihood) { return true; } else if(likelihood == k.likelihood) { if(distance > k.distance) { return true; } else if(distance == k.distance && id < k.id) { return true; } } return false; } float likelihood; int id; float distance; }; //============================================================ // MAIN LOOP //============================================================ bool Rtabmap::process( const cv::Mat & image, int id, const std::map & externalStats) { return this->process(SensorData(image, id), Transform()); } bool Rtabmap::process( const SensorData & data, Transform odomPose, float odomLinearVariance, float odomAngularVariance, const std::vector & odomVelocity, const std::map & externalStats) { if(!odomPose.isNull()) { UASSERT(odomLinearVariance>0.0f); UASSERT(odomAngularVariance>0.0f); } cv::Mat covariance = cv::Mat::eye(6,6,CV_64FC1); covariance.at(0,0) = odomLinearVariance; covariance.at(1,1) = odomLinearVariance; covariance.at(2,2) = odomLinearVariance; covariance.at(3,3) = odomAngularVariance; covariance.at(4,4) = odomAngularVariance; covariance.at(5,5) = odomAngularVariance; return process(data, odomPose, covariance, odomVelocity, externalStats); } bool Rtabmap::process( const SensorData & data, Transform odomPose, const cv::Mat & odomCovariance, const std::vector & odomVelocity, const std::map & externalStats) { UDEBUG(""); //============================================================ // Initialization //============================================================ UTimer timer; UTimer timerTotal; double timeMemoryUpdate = 0; double timeNeighborLinkRefining = 0; double timeProximityByTimeDetection = 0; double timeProximityBySpaceSearch = 0; double timeProximityBySpaceVisualDetection = 0; double timeProximityBySpaceDetection = 0; double timeCleaningNeighbors = 0; double timeReactivations = 0; double timeAddLoopClosureLink = 0; double timeMapOptimization = 0; double timeRetrievalDbAccess = 0; double timeLikelihoodCalculation = 0; double timePosteriorCalculation = 0; double timeHypothesesCreation = 0; double timeHypothesesValidation = 0; double timeRealTimeLimitReachedProcess = 0; double timeMemoryCleanup = 0; double timeEmptyingTrash = 0; double timeFinalizingStatistics = 0; double timeJoiningTrash = 0; double timeStatsCreation = 0; float hypothesisRatio = 0.0f; // Only used for statistics bool rejectedLoopClosure = false; std::map rawLikelihood; std::map adjustedLikelihood; std::map likelihood; std::map weights; std::list > reactivateHypotheses; std::map childCount; std::set signaturesRetrieved; int proximityDetectionsInTimeFound = 0; const Signature * signature = 0; const Signature * sLoop = 0; _loopClosureHypothesis = std::make_pair(0,0.0f); std::pair lastHighestHypothesis = _highestHypothesis; _highestHypothesis = std::make_pair(0,0.0f); std::set immunizedLocations; statistics_ = Statistics(); // reset for(std::map::const_iterator iter=externalStats.begin(); iter!=externalStats.end(); ++iter) { statistics_.addStatistic(iter->first, iter->second); } //============================================================ // Wait for an image... //============================================================ ULOGGER_INFO("getting data..."); timer.start(); timerTotal.start(); UASSERT_MSG(_memory, "RTAB-Map is not initialized!"); UASSERT_MSG(_bayesFilter, "RTAB-Map is not initialized!"); UASSERT_MSG(_graphOptimizer, "RTAB-Map is not initialized!"); //============================================================ // If RGBD SLAM is enabled, a pose must be set. //============================================================ bool fakeOdom = false; if(_rgbdSlamMode) { if(!odomPose.isNull()) { // If we are doing 2D mapping, make sure the pose is 3DoF so that landmark logic works. if(_forceOdom3doF && _graphOptimizer->isSlam2d() && !odomPose.is3DoF()) { odomPose = odomPose.to3DoF(); } // this will make sure that all inverse operations will work! if(!odomPose.isInvertible()) { UWARN("Input odometry is not invertible! pose = %s\n" "[%f %f %f %f;\n" " %f %f %f %f;\n" " %f %f %f %f;\n" " 0 0 0 1]\n" "Trying to normalize rotation to see if it makes it invertible...", odomPose.prettyPrint().c_str(), odomPose.r11(), odomPose.r12(), odomPose.r13(), odomPose.o14(), odomPose.r21(), odomPose.r22(), odomPose.r23(), odomPose.o24(), odomPose.r31(), odomPose.r32(), odomPose.r33(), odomPose.o34()); odomPose.normalizeRotation(); UASSERT_MSG(odomPose.isInvertible(), uFormat("Odometry pose is not invertible! %s\n" "[%f %f %f %f;\n" " %f %f %f %f;\n" " %f %f %f %f;\n" " 0 0 0 1]", odomPose.prettyPrint().c_str(), odomPose.r11(), odomPose.r12(), odomPose.r13(), odomPose.o14(), odomPose.r21(), odomPose.r22(), odomPose.r23(), odomPose.o24(), odomPose.r31(), odomPose.r32(), odomPose.r33(), odomPose.o34()).c_str()); UWARN("Normalizing rotation succeeded! fixed pose = %s\n" "[%f %f %f %f;\n" " %f %f %f %f;\n" " %f %f %f %f;\n" " 0 0 0 1]\n" "If the resulting rotation is very different from original one, try to fix the odometry or TF.", odomPose.prettyPrint().c_str(), odomPose.r11(), odomPose.r12(), odomPose.r13(), odomPose.o14(), odomPose.r21(), odomPose.r22(), odomPose.r23(), odomPose.o24(), odomPose.r31(), odomPose.r32(), odomPose.r33(), odomPose.o34()); } } UDEBUG("incremental=%d odomPose=%s optimizedPoses=%d mapCorrection=%s lastLocalizationPose=%s lastLocalizationNodeId=%d", _memory->isIncremental()?1:0, odomPose.prettyPrint().c_str(), (int)_optimizedPoses.size(), _mapCorrection.prettyPrint().c_str(), _lastLocalizationPose.prettyPrint().c_str(), _lastLocalizationNodeId); if(!_memory->isIncremental() && !odomPose.isNull() && _optimizedPoses.size() && _mapCorrection.isIdentity() && !_lastLocalizationPose.isNull() && _lastLocalizationNodeId == 0) { // Localization mode if(!_optimizeFromGraphEnd) { //set map->odom so that odom is moved back to last saved localization if(_graphOptimizer->isSlam2d()) { _mapCorrection = _lastLocalizationPose.to3DoF() * odomPose.to3DoF().inverse(); } else if((!data.imu().empty() || _memory->isOdomGravityUsed()) && _graphOptimizer->gravitySigma()>0.0f) { _mapCorrection = _lastLocalizationPose.to4DoF() * odomPose.to4DoF().inverse(); } else { _mapCorrection = _lastLocalizationPose * odomPose.inverse(); } std::map nodesOnly(_optimizedPoses.lower_bound(1), _optimizedPoses.end()); _lastLocalizationNodeId = graph::findNearestNode(nodesOnly, _lastLocalizationPose); UWARN("Update map correction based on last localization saved in database! correction = %s, nearest id = %d of last pose = %s, odom = %s", _mapCorrection.prettyPrint().c_str(), _lastLocalizationNodeId, _lastLocalizationPose.prettyPrint().c_str(), odomPose.prettyPrint().c_str()); } else { //move optimized poses accordingly to last saved localization Transform mapCorrectionInv; if(_graphOptimizer->isSlam2d()) { mapCorrectionInv = odomPose.to3DoF() * _lastLocalizationPose.to3DoF().inverse(); } else if((!data.imu().empty() || _memory->isOdomGravityUsed()) && _graphOptimizer->gravitySigma()>0.0f) { mapCorrectionInv = odomPose.to4DoF() * _lastLocalizationPose.to4DoF().inverse(); } else { mapCorrectionInv = odomPose * _lastLocalizationPose.inverse(); } for(std::map::iterator iter=_optimizedPoses.begin(); iter!=_optimizedPoses.end(); ++iter) { iter->second = mapCorrectionInv * iter->second; } std::map nodesOnly(_optimizedPoses.lower_bound(1), _optimizedPoses.end()); _lastLocalizationNodeId = graph::findNearestNode(nodesOnly, _lastLocalizationPose); UWARN("Transformed map accordingly to last localization pose saved in database (%s=true)! nearest id = %d of last pose = %s", Parameters::kRGBDOptimizeFromGraphEnd().c_str(), _lastLocalizationNodeId, _lastLocalizationPose.prettyPrint().c_str()); } } if(odomPose.isNull()) { if(_memory->isIncremental()) { UERROR("RGB-D SLAM mode is enabled, memory is incremental but no odometry is provided. " "Image %d is ignored!", data.id()); return false; } else // fake localization { if(!_mapCorrectionBackup.isNull()) { _mapCorrection = _mapCorrectionBackup; _mapCorrectionBackup.setNull(); } if(_lastLocalizationPose.isNull()) { _lastLocalizationPose = Transform::getIdentity(); } fakeOdom = true; odomPose = _mapCorrection.inverse() * _lastLocalizationPose; UDEBUG("Map correction = %s", _mapCorrection.prettyPrint().c_str()); UDEBUG("Last localization pose: %s", _lastLocalizationPose.prettyPrint().c_str()); UDEBUG("Fake odom: %s", odomPose.prettyPrint().c_str()); } } else if(_memory->isIncremental()) // only in mapping mode { // Detect if the odometry is reset. If yes, trigger a new map. if(_memory->getLastWorkingSignature(false)) { const Transform & lastPose = _memory->getLastWorkingSignature(false)->getPose(); // use raw odometry // look for identity if(!lastPose.isIdentity() && odomPose.isIdentity()) { int mapId = triggerNewMap(); UWARN("Odometry is reset (identity pose detected). Increment map id to %d!", mapId); } else if(_newMapOdomChangeDistance > 0.0) { // look for large change Transform lastPoseToNewPose = lastPose.inverse() * odomPose; float x,y,z, roll,pitch,yaw; lastPoseToNewPose.getTranslationAndEulerAngles(x,y,z, roll,pitch,yaw); if((x*x + y*y + z*z) > _newMapOdomChangeDistance*_newMapOdomChangeDistance) { int mapId = triggerNewMap(); UWARN("Odometry is reset (large odometry change detected > %f). A new map (%d) is created! Last pose = %s, new pose = %s", _newMapOdomChangeDistance, mapId, lastPose.prettyPrint().c_str(), odomPose.prettyPrint().c_str()); } } } } } //============================================================ // Memory Update : Location creation + Add to STM + Weight Update (Rehearsal) //============================================================ ULOGGER_INFO("Updating memory..."); if(_rgbdSlamMode) { if(!_memory->update(data, odomPose, odomCovariance, odomVelocity, &statistics_)) { return false; } } else { if(!_memory->update(data, Transform(), cv::Mat(), std::vector(), &statistics_)) { return false; } } signature = _memory->getLastWorkingSignature(false); _currentSessionHasGPS = _currentSessionHasGPS || signature->sensorData().gps().stamp() > 0.0; if(!signature) { UFATAL("Not supposed to be here...last signature is null?!?"); } ULOGGER_INFO("Processing signature %d (%f) w=%d map=%d", signature->id(), signature->getStamp(), signature->getWeight(), signature->mapId()); timeMemoryUpdate = timer.ticks(); ULOGGER_INFO("timeMemoryUpdate=%fs", timeMemoryUpdate); //============================================================ // Metric //============================================================ bool smallDisplacement = false; bool tooFastMovement = false; std::list signaturesRemoved; bool neighborLinkRefined = false; bool addedNewLandmark = false; float distanceToClosestNodeInTheGraph = 0; float angleToClosestNodeInTheGraph = 0; if(_rgbdSlamMode) { double linVar = odomCovariance.empty()?0.0f:uMax3(odomCovariance.at(0,0), odomCovariance.at(1,1)>=9999?0:odomCovariance.at(1,1), odomCovariance.at(2,2)>=9999?0:odomCovariance.at(2,2)); double angVar = odomCovariance.empty()?0.0f:uMax3(odomCovariance.at(3,3)>=9999?0:odomCovariance.at(3,3), odomCovariance.at(4,4)>=9999?0:odomCovariance.at(4,4), odomCovariance.at(5,5)); statistics_.addStatistic(Statistics::kMemoryOdometry_variance_lin(), (float)linVar); statistics_.addStatistic(Statistics::kMemoryOdometry_variance_ang(), (float)angVar); //Verify if there was a rehearsal int rehearsedId = (int)uValue(statistics_.data(), Statistics::kMemoryRehearsal_merged(), 0.0f); if(rehearsedId > 0) { _optimizedPoses.erase(rehearsedId); } else { bool linkedToIntermediateNode = false; Transform t; if(_memory->isIncremental()) { // Check small motion if current node is not an intermediate node already if(signature->getWeight() >= 0) { // It should contain only the query and its first (non-intermediate) neighbor (smaller id) std::map neighbors = _memory->getNeighborsId(signature->id(), 2, 0, true, true, true, true); if(neighbors.size() == 2) { int nid = neighbors.begin()->first; const std::multimap & links = signature->getLinks(); if(links.find(nid) != links.end()) { // direct neighbor t = links.find(nid)->second.transform(); } else { // Use optimized poses to check how far it is from the latest non-intermediate node std::map::iterator niter = _optimizedPoses.find(nid); if(niter != _optimizedPoses.end()) { t = niter->second.inverse() * _mapCorrection * signature->getPose(); } // not direct link, it means there are intermediate nodes linkedToIntermediateNode = true; } } } } else if(!_odomCachePoses.empty()) { t = _odomCachePoses.rbegin()->second.inverse() * signature->getPose(); } if(_rgbdLinearUpdate > 0.0f || _rgbdAngularUpdate > 0.0f) { //============================================================ // Minimum displacement required to add to Memory //============================================================ if(!t.isNull()) { float x,y,z, roll,pitch,yaw; t.getTranslationAndEulerAngles(x,y,z, roll,pitch,yaw); bool isMoving = (_rgbdLinearUpdate > 0.0f && ( fabs(x) > _rgbdLinearUpdate || fabs(y) > _rgbdLinearUpdate || fabs(z) > _rgbdLinearUpdate)) || (_rgbdAngularUpdate>0.0f && ( fabs(roll) > _rgbdAngularUpdate || fabs(pitch) > _rgbdAngularUpdate || fabs(yaw) > _rgbdAngularUpdate)); if(!isMoving) { // This will disable global loop closure detection, only retrieval will be done. // The location will also be deleted at the end. smallDisplacement = true; UDEBUG("smallDisplacement: %f %f %f %f %f %f", x,y,z, roll,pitch,yaw); } } } if(odomVelocity.size() == 6 && signature->getWeight() != -1) { // This will disable global loop closure detection, only retrieval will be done. // The location will also be deleted at the end. tooFastMovement = (_rgbdLinearSpeedUpdate>0.0f && uMax3(fabs(odomVelocity[0]), fabs(odomVelocity[1]), fabs(odomVelocity[2])) > _rgbdLinearSpeedUpdate) || (_rgbdAngularSpeedUpdate>0.0f && uMax3(fabs(odomVelocity[3]), fabs(odomVelocity[4]), fabs(odomVelocity[5])) > _rgbdAngularSpeedUpdate); } if(linkedToIntermediateNode && (smallDisplacement || tooFastMovement)) { _memory->convertToIntermediate(signature->id()); } } // Update optimizedPoses with the newly added node Transform newPose; bool intermediateNodeRefining = false; if(_neighborLinkRefining && signature->getLinks().size() && signature->getLinks().begin()->second.type() == Link::kNeighbor && _memory->isIncremental() && // ignore pose matching in localization mode rehearsedId == 0 && // don't do it if rehearsal happened !tooFastMovement) // ignore if too fast movement has been detected { int oldId = signature->getLinks().begin()->first; const Signature * oldS = _memory->getSignature(oldId); UASSERT(oldS != 0); if(signature->getWeight() >= 0 && oldS->getWeight()>=0) // ignore intermediate nodes { Transform guess = signature->getLinks().begin()->second.transform().inverse(); if(smallDisplacement) { if(signature->getLinks().begin()->second.transVariance() == 1) { // set small variance UDEBUG("Set small variance. The robot is not moving."); _memory->updateLink(Link(oldId, signature->id(), signature->getLinks().begin()->second.type(), guess, cv::Mat::eye(6,6,CV_64FC1)*1000)); } } else { //============================================================ // Refine neighbor links //============================================================ UINFO("Odometry refining: guess = %s", guess.prettyPrint().c_str()); RegistrationInfo info; Transform t = _memory->computeTransform(oldId, signature->id(), guess, &info); if(!t.isNull()) { UASSERT(!info.covariance.empty() && info.covariance.at(0,0) > 0.0 && info.covariance.at(5,5) > 0.0); UINFO("Odometry refining: update neighbor link (%d->%d, variance:lin=%f, ang=%f) from %s to %s", oldId, signature->id(), info.covariance.at(0,0), info.covariance.at(5,5), guess.prettyPrint().c_str(), t.prettyPrint().c_str()); _memory->updateLink(Link(oldId, signature->id(), signature->getLinks().begin()->second.type(), t, info.covariance.inv())); if(_optimizeFromGraphEnd) { // update all previous nodes // Normally _mapCorrection should be identity, but if _optimizeFromGraphEnd // parameters just changed state, we should put back all poses without map correction. Transform u = guess * t.inverse(); std::map::iterator jter = _optimizedPoses.find(oldId); UASSERT(jter!=_optimizedPoses.end()); Transform up = jter->second * u * jter->second.inverse(); Transform mapCorrectionInv = _mapCorrection.inverse(); for(std::map::iterator iter=_optimizedPoses.begin(); iter!=_optimizedPoses.end(); ++iter) { iter->second = mapCorrectionInv * up * iter->second; } } } else { UINFO("Odometry refining rejected: %s", info.rejectedMsg.c_str()); if(!info.covariance.empty() && info.covariance.at(0,0) > 0.0 && info.covariance.at(0,0) != 1.0 && info.covariance.at(5,5) > 0.0 && info.covariance.at(5,5) != 1.0) { if(ULogger::level() <= ULogger::kInfo) { std::cout << info.covariance << std::endl; } _memory->updateLink(Link(oldId, signature->id(), signature->getLinks().begin()->second.type(), guess, (info.covariance*100.0).inv())); } } neighborLinkRefined = !t.isNull(); statistics_.addStatistic(Statistics::kNeighborLinkRefiningAccepted(),neighborLinkRefined?1.0f:0); statistics_.addStatistic(Statistics::kNeighborLinkRefiningInliers(), info.inliers); statistics_.addStatistic(Statistics::kNeighborLinkRefiningICP_inliers_ratio(), info.icpInliersRatio); statistics_.addStatistic(Statistics::kNeighborLinkRefiningICP_rotation(), info.icpRotation); statistics_.addStatistic(Statistics::kNeighborLinkRefiningICP_translation(), info.icpTranslation); statistics_.addStatistic(Statistics::kNeighborLinkRefiningICP_complexity(), info.icpStructuralComplexity); statistics_.addStatistic(Statistics::kNeighborLinkRefiningPts(), signature->sensorData().laserScanRaw().size()); } timeNeighborLinkRefining = timer.ticks(); ULOGGER_INFO("timeOdometryRefining=%fs", timeNeighborLinkRefining); UASSERT(oldS->hasLink(signature->id())); UASSERT(uContains(_optimizedPoses, oldId)); statistics_.addStatistic(Statistics::kNeighborLinkRefiningVariance(), oldS->getLinks().find(signature->id())->second.transVariance()); newPose = _optimizedPoses.at(oldId) * oldS->getLinks().find(signature->id())->second.transform(); _mapCorrection = newPose * signature->getPose().inverse(); if(_mapCorrection.getNormSquared() > 0.001f && _optimizeFromGraphEnd) { UERROR("Map correction should be identity when optimizing from the last node. T=%s NewPose=%s OldPose=%s", _mapCorrection.prettyPrint().c_str(), newPose.prettyPrint().c_str(), signature->getPose().prettyPrint().c_str()); } } else { newPose = _mapCorrection * signature->getPose(); intermediateNodeRefining = true; } } else { newPose = _mapCorrection * signature->getPose(); } // Get statistics about the closest node in the graph if(!_memory->isIncremental()) { int closestNode = 0; float sqrdDistance = 0.0f; if(_optimizedPoses.begin()->first < 0) { std::map poses(_optimizedPoses.lower_bound(1), _optimizedPoses.end()); closestNode = graph::findNearestNode(poses, newPose, &sqrdDistance); } else { closestNode = graph::findNearestNode(_optimizedPoses, newPose, &sqrdDistance); } if(closestNode>0 && sqrdDistance>0.0f) { distanceToClosestNodeInTheGraph = sqrt(sqrdDistance); UDEBUG("Last localization pose = %s, closest node=%d (%f m)", newPose.prettyPrint().c_str(), closestNode, distanceToClosestNodeInTheGraph); angleToClosestNodeInTheGraph = newPose.getAngle(_optimizedPoses.at(closestNode)); } } UDEBUG("Added pose %s (odom=%s)", newPose.prettyPrint().c_str(), signature->getPose().prettyPrint().c_str()); // Update Poses and Constraints _optimizedPoses.insert(std::make_pair(signature->id(), newPose)); if(_memory->isIncremental() && signature->getWeight() >= 0) { for(std::map::const_iterator iter = signature->getLandmarks().begin(); iter!=signature->getLandmarks().end(); ++iter) { if(_optimizedPoses.find(iter->first) == _optimizedPoses.end()) { _optimizedPoses.insert(std::make_pair(iter->first, newPose*iter->second.transform())); UDEBUG("Added landmark %d : %s", iter->first, (newPose*iter->second.transform()).prettyPrint().c_str()); addedNewLandmark = true; } _constraints.insert(std::make_pair(iter->first, iter->second.inverse())); } } float distanceTravelledOld = _distanceTravelled; // only in mapping mode we add a neighbor link if(signature->getLinks().size() && signature->getLinks().begin()->second.type() == Link::kNeighbor) { // link should be old to new UASSERT_MSG(signature->id() > signature->getLinks().begin()->second.to(), "Only forward links should be added."); Link tmp = signature->getLinks().begin()->second.inverse(); if(!smallDisplacement) { _distanceTravelled += tmp.transform().getNorm(); } // if the previous node is an intermediate node, remove it from the local graph if(_constraints.size() && _constraints.rbegin()->second.to() == signature->getLinks().begin()->second.to()) { const Signature * s = _memory->getSignature(signature->getLinks().begin()->second.to()); UASSERT(s!=0); if(s->getWeight() == -1) { tmp = _constraints.rbegin()->second.merge(tmp, tmp.type()); _optimizedPoses.erase(s->id()); _constraints.erase(--_constraints.end()); } } _constraints.insert(std::make_pair(tmp.from(), tmp)); } // Localization mode stuff if( signature->getWeight() >= 0 && !smallDisplacement && odomCovariance.cols == 6 && odomCovariance.rows == 6 && odomCovariance.type() == CV_64FC1 && odomCovariance.at(0,0) < 1) { if( _memory->isIncremental() && _localizationCovariance.empty()) { _localizationCovariance = cv::Mat::zeros(6,6,CV_64FC1); } if(_localizationCovariance.total() == 36) { #ifdef RTABMAP_MRPT // Transform odometry covariance (which in base frame) into global frame // "odometry error propagation law" Eigen::Quaterniond rotation = _lastLocalizationPose.getQuaterniond(); mrpt::poses::CPose3D pose = mrpt::poses::CPose3D::FromQuaternion(mrpt::math::CQuaternionDouble(rotation.w(), rotation.x(), rotation.y(), rotation.z())); mrpt::math::CMatrixDouble66 gaussian; gaussian.loadFromRawPointer((const double*)odomCovariance.data); mrpt::poses::CPose3DPDFGaussian gaussianTransformed(mrpt::poses::CPose3D(), gaussian); gaussianTransformed.changeCoordinatesReference(pose); _localizationCovariance += cv::Mat(6,6,CV_64FC1, gaussianTransformed.cov.data()); #else // Assuming diagonal uniform covariance matrix! // If variance is different for each axis, // build rtabmap with MRPT to use approach above. _localizationCovariance += odomCovariance; #endif } } _lastLocalizationPose = newPose; // keep in cache the latest corrected pose if(signature->getWeight() >= 0) { UDEBUG("Update odometry localization cache (size=%d/%d)", (int)_odomCachePoses.size(), _maxOdomCacheSize); if(!_odomCachePoses.empty()) { float odomDistance = (_odomCachePoses.rbegin()->second.inverse() * signature->getPose()).getNorm(); if(!smallDisplacement) { _distanceTravelled += odomDistance; } while(!_odomCachePoses.empty() && (int)_odomCachePoses.size() > _maxOdomCacheSize) { _odomCacheConstraints.erase(_odomCachePoses.begin()->first); _odomCachePoses.erase(_odomCachePoses.begin()); } if(!_odomCachePoses.empty()) { Link odomLink(_odomCachePoses.rbegin()->first, signature->id(), Link::kNeighbor, _odomCachePoses.rbegin()->second.inverse() * signature->getPose(), odomCovariance.inv()); _odomCacheConstraints.insert(std::make_pair(_odomCachePoses.rbegin()->first, odomLink)); UDEBUG("Added odom cov = %f %f", odomLink.transVariance(), odomLink.rotVariance()); } } _odomCachePoses.insert(std::make_pair(signature->id(), signature->getPose())); } _distanceTravelledSinceLastLocalization += _distanceTravelled - distanceTravelledOld; //============================================================ // Reduced graph //============================================================ //Verify if there are nodes that were merged through graph reduction if(statistics_.reducedIds().size()) { for(unsigned int i=0; i<_path.size(); ++i) { std::map::const_iterator iter = statistics_.reducedIds().find(_path[i].first); if(iter!= statistics_.reducedIds().end()) { // change path ID to loop closure ID _path[i].first = iter->second; } } for(std::map::const_iterator iter=statistics_.reducedIds().begin(); iter!=statistics_.reducedIds().end(); ++iter) { int erased = (int)_optimizedPoses.erase(iter->first); if(erased) { for(std::multimap::iterator jter = _constraints.begin(); jter!=_constraints.end();) { if(jter->second.from() == iter->first || jter->second.to() == iter->first) { _constraints.erase(jter++); } else { ++jter; } } } } } //============================================================ // Local loop closure in TIME //============================================================ if((_proximityByTime || intermediateNodeRefining) && rehearsedId == 0 && // don't do it if rehearsal happened _memory->isIncremental() && // don't do it in localization mode signature->getWeight()>=0) { const std::set & stm = _memory->getStMem(); for(std::set::const_reverse_iterator iter = stm.rbegin(); iter!=stm.rend(); ++iter) { if(*iter != signature->id() && signature->getLinks().find(*iter) == signature->getLinks().end() && _memory->getSignature(*iter)->mapId() == signature->mapId() && _memory->getSignature(*iter)->getWeight()>=0) { std::string rejectedMsg; UDEBUG("Check local transform between %d and %d", signature->id(), *iter); RegistrationInfo info; Transform guess; if(_optimizedPoses.find(*iter) != _optimizedPoses.end()) { guess = _optimizedPoses.at(*iter).inverse() * newPose; } // For proximity by time, correspondences should be already enough precise, so don't recompute them Transform transform = _memory->computeTransform(*iter, signature->id(), guess, &info, true); if(!transform.isNull()) { transform = transform.inverse(); UDEBUG("Add local loop closure in TIME (%d->%d) %s", signature->id(), *iter, transform.prettyPrint().c_str()); // Add a loop constraint UASSERT(!info.covariance.empty() && info.covariance.at(0,0) > 0.0 && info.covariance.at(5,5) > 0.0); if(_memory->addLink(Link(signature->id(), *iter, Link::kLocalTimeClosure, transform, getInformation(info.covariance)))) { ++proximityDetectionsInTimeFound; UINFO("Local loop closure found between %d and %d with t=%s", *iter, signature->id(), transform.prettyPrint().c_str()); } else { UWARN("Cannot add local loop closure between %d and %d ?!?", *iter, signature->id()); } } else { UINFO("Local loop closure (time) between %d and %d rejected: %s", *iter, signature->id(), rejectedMsg.c_str()); } if(!_proximityByTime && intermediateNodeRefining) { // Do it only with the latest non-intermediate node break; } } } } } timeProximityByTimeDetection = timer.ticks(); UINFO("timeProximityByTimeDetection=%fs", timeProximityByTimeDetection); //============================================================ // Bayes filter update //============================================================ bool localizationOnPreviousUpdate = false; if(_memory->isIncremental()) { localizationOnPreviousUpdate = signature->getLinks().size() && signature->getLinks().begin()->first!=signature->id() && _memory->getLoopClosureLinks(signature->getLinks().begin()->first, false).size() != 0; } else { // localization mode // Count how many localization links are in the constraints int localizationLinks = 0; int previousIdWithLocalizationLink = 0; for(std::multimap::iterator iter=_odomCacheConstraints.begin(); iter!=_odomCacheConstraints.end(); ++iter) { if(previousIdWithLocalizationLink == iter->first) { // ignore links with node already counted continue; } if(iter->second.type() == Link::kGlobalClosure || iter->second.type() == Link::kLocalSpaceClosure || iter->second.type() == Link::kLocalTimeClosure || iter->second.type() == Link::kUserClosure || iter->second.type() == Link::kNeighborMerged || iter->second.type() == Link::kLandmark) { ++localizationLinks; previousIdWithLocalizationLink = iter->first; } } localizationOnPreviousUpdate = localizationLinks > 1; // need two links in case we have delayed localization } // Not a bad signature, not an intermediate node, not a small displacement unless the previous signature didn't have a loop closure, not too fast movement if(!signature->isBadSignature() && signature->getWeight()>=0 && (!smallDisplacement || !localizationOnPreviousUpdate) && !tooFastMovement) { // If the working memory is empty, don't do the detection. It happens when it // is the first time the detector is started (there needs some images to // fill the short-time memory before a signature is added to the working memory). if(_memory->getWorkingMemSize(true)) { //============================================================ // Likelihood computation // Get the likelihood of the new signature // with all images contained in the working memory + reactivated. //============================================================ ULOGGER_INFO("computing likelihood..."); std::list signaturesToCompare; GPS originGPS; Transform originOffsetENU = Transform::getIdentity(); if(_loopGPS) { originGPS = signature->sensorData().gps(); if(originGPS.stamp() == 0.0 && _currentSessionHasGPS) { UTimer tmpT; if(_optimizedPoses.size() && _memory->isIncremental()) { //Search for latest node having GPS linked to current signature not too far. std::map nearestIds = graph::findNearestNodes(signature->id(), _optimizedPoses, _localRadius); for(std::map::reverse_iterator iter=nearestIds.rbegin(); iter!=nearestIds.rend() && iter->first>0; ++iter) { const Signature * s = _memory->getSignature(iter->first); UASSERT(s!=0); if(s->sensorData().gps().stamp() > 0.0) { originGPS = s->sensorData().gps(); const Transform & sPose = _optimizedPoses.at(s->id()); Transform localToENU(0,0,(float)((-(originGPS.bearing()-90))*M_PI/180.0) - sPose.theta()); originOffsetENU = localToENU * (sPose.rotation()*(sPose.inverse()*_optimizedPoses.at(signature->id()))); break; } } } //else if(!_memory->isIncremental()) // TODO, how can we estimate current GPS position in localization? //{ //} } if(originGPS.stamp() > 0.0) { // no need to save it if it is in localization mode _gpsGeocentricCache.insert(std::make_pair(signature->id(), std::make_pair(originGPS.toGeodeticCoords().toGeocentric_WGS84(), originOffsetENU))); } } for(std::map::const_iterator iter=_memory->getWorkingMem().begin(); iter!=_memory->getWorkingMem().end(); ++iter) { if(iter->first > 0) { const Signature * s = _memory->getSignature(iter->first); UASSERT(s!=0); if(s->getWeight() != -1) // ignore intermediate nodes { bool accept = true; if(originGPS.stamp()>0.0) { std::map >::iterator cacheIter = _gpsGeocentricCache.find(s->id()); if(cacheIter == _gpsGeocentricCache.end()) { GPS gps = s->sensorData().gps(); Transform offsetENU = Transform::getIdentity(); if(gps.stamp()==0.0) { _memory->getGPS(s->id(), gps, offsetENU, false); } if(gps.stamp() > 0.0) { cacheIter = _gpsGeocentricCache.insert( std::make_pair(s->id(), std::make_pair(gps.toGeodeticCoords().toGeocentric_WGS84(), offsetENU))).first; } } if(cacheIter != _gpsGeocentricCache.end()) { std::map >::iterator originIter = _gpsGeocentricCache.find(signature->id()); UASSERT(originIter != _gpsGeocentricCache.end()); cv::Point3d relativePose = GeodeticCoords::Geocentric_WGS84ToENU_WGS84(cacheIter->second.first, originIter->second.first, originGPS.toGeodeticCoords()); const double & error = originGPS.error(); const Transform & offsetENU = cacheIter->second.second; relativePose.x += offsetENU.x() - originOffsetENU.x(); relativePose.y += offsetENU.y() - originOffsetENU.y(); relativePose.z += offsetENU.z() - originOffsetENU.z(); // ignore altitude if difference is under GPS error if(relativePose.z>error) { relativePose.z -= error; } else if(relativePose.z < -error) { relativePose.z += error; } else { relativePose.z = 0; } accept = uNormSquared(relativePose.x, relativePose.y, relativePose.z) < _localRadius*_localRadius; } } if(accept) { signaturesToCompare.push_back(iter->first); } } } else { // virtual signature should be added signaturesToCompare.push_back(iter->first); } } rawLikelihood = _memory->computeLikelihood(signature, signaturesToCompare); // Adjust the likelihood (with mean and std dev) likelihood = rawLikelihood; this->adjustLikelihood(likelihood); timeLikelihoodCalculation = timer.ticks(); ULOGGER_INFO("timeLikelihoodCalculation=%fs",timeLikelihoodCalculation); //============================================================ // Apply the Bayes filter // Posterior = Likelihood x Prior //============================================================ ULOGGER_INFO("getting posterior..."); // Compute the posterior _bayesFilter->computePosterior(_memory, likelihood); timePosteriorCalculation = timer.ticks(); ULOGGER_INFO("timePosteriorCalculation=%fs",timePosteriorCalculation); // For statistics, copy weights if(_publishStats && (_publishLikelihood || _publishPdf)) { weights = _memory->getWeights(); } //============================================================ // Select the highest hypothesis //============================================================ ULOGGER_INFO("creating hypotheses..."); const std::vector & posteriorIds = _bayesFilter->getPosteriorIds(); const std::vector & posteriorValues = _bayesFilter->getPosteriorValues(); if(posteriorIds.size()) { // Highest id first, so the highest id wins on equal probabilities. for(size_t i=posteriorIds.size(); i-- > 0;) { if(posteriorIds[i] > 0 && posteriorValues[i] > _highestHypothesis.second) { _highestHypothesis = std::make_pair(posteriorIds[i], posteriorValues[i]); } } // With the virtual place, use sum of LC probabilities (1 - virtual place hypothesis). _highestHypothesis.second = 1-posteriorValues[0]; } timeHypothesesCreation = timer.ticks(); ULOGGER_INFO("Highest hypothesis=%d, value=%f, timeHypothesesCreation=%fs", _highestHypothesis.first, _highestHypothesis.second, timeHypothesesCreation); if(_highestHypothesis.first > 0) { float loopThr = _loopThr; bool hasLoopClosureConstraints = false; for(std::multimap::iterator iter=_odomCacheConstraints.begin(); iter!=_odomCacheConstraints.end() && !hasLoopClosureConstraints; ++iter) { hasLoopClosureConstraints = iter->second.type() == Link::kGlobalClosure || iter->second.type() == Link::kLocalSpaceClosure || iter->second.type() == Link::kLandmark; } if( (( _memory->isIncremental() && !uContains(_optimizedPoses, _highestHypothesis.first)) || // not linked to previous map of that hypothesis (!_memory->isIncremental() && !hasLoopClosureConstraints)) && // not yet localized to any previous sessions _memory->getWorkingMemSize(true)>0 && // should have an old map _rgbdSlamMode && loopThr > _aggressiveLoopThr) { // use the best hypothesis directly. UDEBUG("Using %s=%f", Parameters::kRGBDAggressiveLoopThr().c_str(), _aggressiveLoopThr); loopThr = _aggressiveLoopThr; } // Loop closure Threshold if(_highestHypothesis.second >= loopThr) { rejectedLoopClosure = true; if(_bayesFilter->getPosteriorIds().size() <= 2 && loopThr>0.0f) { // Ignore loop closure if there is only one loop closure hypothesis UDEBUG("rejected hypothesis: single hypothesis"); } else if(_verifyLoopClosureHypothesis && !_epipolarGeometry->check(signature, _memory->getSignature(_highestHypothesis.first))) { UWARN("rejected hypothesis: by epipolar geometry"); } else if(_loopRatio > 0.0f && lastHighestHypothesis.second && _highestHypothesis.second < _loopRatio*lastHighestHypothesis.second) { UWARN("rejected hypothesis: not satisfying hypothesis ratio (%f < %f * %f)", _highestHypothesis.second, _loopRatio, lastHighestHypothesis.second); } else if(_loopRatio > 0.0f && lastHighestHypothesis.second == 0) { UWARN("rejected hypothesis: last closure hypothesis is null (loop ratio is on)"); } else { _loopClosureHypothesis = _highestHypothesis; rejectedLoopClosure = false; } timeHypothesesValidation = timer.ticks(); ULOGGER_INFO("timeHypothesesValidation=%fs",timeHypothesesValidation); } else if(_highestHypothesis.second < _loopRatio*lastHighestHypothesis.second) { // Used for Precision-Recall computation. // When analyzing logs, it's convenient to know // if the hypothesis would be rejected if T_loop would be lower. rejectedLoopClosure = true; UDEBUG("rejected hypothesis: under loop ratio %f < %f", _highestHypothesis.second, _loopRatio*lastHighestHypothesis.second); } //for statistic... hypothesisRatio = _loopClosureHypothesis.second>0?_highestHypothesis.second/_loopClosureHypothesis.second:0; } } // if(_memory->getWorkingMemSize()) } // !isBadSignature else if(!signature->isBadSignature() && signature->getWeight()>=0 && (smallDisplacement || tooFastMovement)) { _highestHypothesis = lastHighestHypothesis; UDEBUG("smallDisplacement=%d tooFastMovement=%d", smallDisplacement?1:0, tooFastMovement?1:0); } else { UDEBUG("Ignoring likelihood and loop closure hypotheses as current signature doesn't have enough visual features."); } //============================================================ // Before retrieval, make sure the trash has finished //============================================================ _memory->joinTrashThread(); timeEmptyingTrash = _memory->getDbSavingTime(); timeJoiningTrash = timer.ticks(); ULOGGER_INFO("Time emptying memory trash = %fs, joining (actual overhead) = %fs", timeEmptyingTrash, timeJoiningTrash); //============================================================ // RETRIEVAL 1/3 : Loop closure neighbors reactivation //============================================================ int retrievalId = _highestHypothesis.first; std::list reactivatedIds; double timeGetNeighborsTimeDb = 0.0; double timeGetNeighborsSpaceDb = 0.0; int immunizedGlobally = 0; int immunizedLocally = 0; int maxLocalLocationsImmunized = 0; if(_maxTimeAllowed != 0 || _maxMemoryAllowed != 0) { // with memory management, we have to immunize some nodes maxLocalLocationsImmunized = _localImmunizationRatio * float(_memory->getWorkingMemSize(true)); } // no need to do retrieval or immunization of locations if memory management // is disabled and all nodes are in WM. // Also skip memory mangement on intermediate nodes if(!((_memory->allNodesInWM() || _maxRetrieved==0) && maxLocalLocationsImmunized==0) && signature->getWeight()>=0) { if(retrievalId > 0) { //Load neighbors ULOGGER_INFO("Retrieving locations... around id=%d", retrievalId); int neighborhoodSize = (int)_bayesFilter->getPredictionLC().size()-1; UASSERT(neighborhoodSize >= 0); ULOGGER_DEBUG("margin=%d maxRetieved=%d", neighborhoodSize, _maxRetrieved); UTimer timeGetN; unsigned int nbLoadedFromDb = 0; std::set reactivatedIdsSet; std::map neighbors; int nbDirectNeighborsInDb = 0; // priority in time // Direct neighbors TIME ULOGGER_DEBUG("In TIME"); neighbors = _memory->getNeighborsId(retrievalId, neighborhoodSize, _maxRetrieved, true, true, false, true, std::set(), &timeGetNeighborsTimeDb); ULOGGER_DEBUG("neighbors of %d in time = %d", retrievalId, (int)neighbors.size()); //Priority to locations near in time (direct neighbor) then by space (loop closure) bool firstPassDone = false; // just to avoid checking to STM after the first pass int m = 0; while(m < neighborhoodSize) { std::set idsSorted; for(std::map::iterator iter=neighbors.begin(); iter!=neighbors.end();) { if(!firstPassDone && _memory->isInSTM(iter->first)) { neighbors.erase(iter++); } else if(iter->second == m) { if(reactivatedIdsSet.find(iter->first) == reactivatedIdsSet.end()) { idsSorted.insert(iter->first); reactivatedIdsSet.insert(iter->first); if(m == 1 && _memory->getSignature(iter->first) == 0) { ++nbDirectNeighborsInDb; } //immunized locations in the neighborhood from being transferred if(immunizedLocations.insert(iter->first).second) { // Count only non-intermediate nodes (intermediate nodes are still // immunized but don't consume the immunization budget/statistic). const Signature * sImmunized = _memory->getSignature(iter->first); if(sImmunized == 0 || sImmunized->getWeight() >= 0) { ++immunizedGlobally; } } //UDEBUG("nt=%d m=%d immunized=1", iter->first, iter->second); } neighbors.erase(iter++); } else { ++iter; } } firstPassDone = true; reactivatedIds.insert(reactivatedIds.end(), idsSorted.rbegin(), idsSorted.rend()); ++m; } // neighbors SPACE, already added direct neighbors will be ignored ULOGGER_DEBUG("In SPACE"); neighbors = _memory->getNeighborsId(retrievalId, neighborhoodSize, _maxRetrieved, true, false, false, false, std::set(), &timeGetNeighborsSpaceDb); ULOGGER_DEBUG("neighbors of %d in space = %d", retrievalId, (int)neighbors.size()); firstPassDone = false; m = 0; while(m < neighborhoodSize) { std::set idsSorted; for(std::map::iterator iter=neighbors.begin(); iter!=neighbors.end();) { if(!firstPassDone && _memory->isInSTM(iter->first)) { neighbors.erase(iter++); } else if(iter->second == m) { if(reactivatedIdsSet.find(iter->first) == reactivatedIdsSet.end()) { idsSorted.insert(iter->first); reactivatedIdsSet.insert(iter->first); if(m == 1 && _memory->getSignature(iter->first) == 0) { ++nbDirectNeighborsInDb; } //UDEBUG("nt=%d m=%d", iter->first, iter->second); } neighbors.erase(iter++); } else { ++iter; } } firstPassDone = true; reactivatedIds.insert(reactivatedIds.end(), idsSorted.rbegin(), idsSorted.rend()); ++m; } ULOGGER_INFO("neighborhoodSize=%d, " "reactivatedIds.size=%d, " "nbLoadedFromDb=%d, " "nbDirectNeighborsInDb=%d, " "time=%fs (%fs %fs)", neighborhoodSize, (int)reactivatedIds.size(), (int)nbLoadedFromDb, nbDirectNeighborsInDb, timeGetN.ticks(), timeGetNeighborsTimeDb, timeGetNeighborsSpaceDb); } } //============================================================ // RETRIEVAL 2/3 : Update planned path and get next nodes to retrieve //============================================================ std::list retrievalLocalIds; if(_rgbdSlamMode && signature->getWeight()>=0) { // Priority on locations on the planned path if(_path.size()) { // Note: retrieval on path with intermediate nodes is not supported. Note that the planned path would // eventually fail anyway because intermediate nodes are not in _optimizedPoses. updateGoalIndex(); float distanceSoFar = 0.0f; // immunize all nodes after current node and // retrieve nodes after current node in the maximum radius from the current node for(unsigned int i=_pathCurrentIndex; i<_path.size(); ++i) { if(_localRadius > 0.0f && i != _pathCurrentIndex) { distanceSoFar += _path[i-1].second.getDistance(_path[i].second); } const Signature * sPath = _memory->getSignature(_path[i].first); if(sPath != 0) { // Count only non-intermediate nodes (intermediate nodes are still // immunized but don't consume the immunization budget/statistic). if(immunizedLocations.insert(_path[i].first).second && sPath->getWeight() >= 0) { ++immunizedLocally; } UDEBUG("Path immunization: node %d (dist=%fm)", _path[i].first, distanceSoFar); } else if(retrievalLocalIds.size() < _maxLocalRetrieved) { UINFO("retrieval of node %d on path (dist=%fm)", _path[i].first, distanceSoFar); retrievalLocalIds.push_back(_path[i].first); // retrieved locations are automatically immunized } if(distanceSoFar > _localRadius) { UDEBUG("Stop on node %d (dist=%fm > %fm)", _path[i].first, distanceSoFar, _localRadius); break; } } } if(!(_memory->allNodesInWM() && maxLocalLocationsImmunized==0)) { // immunize the path from the nearest local location to the current location if(immunizedLocally < maxLocalLocationsImmunized && _memory->isIncremental()) // Can only work in mapping mode { std::map poses; // remove poses from STM for(std::map::iterator iter=_optimizedPoses.begin(); iter!=_optimizedPoses.end(); ++iter) { if(iter->first > 0 && !_memory->isInSTM(iter->first)) { poses.insert(*iter); } } int nearestId = graph::findNearestNode(poses, _optimizedPoses.at(signature->id())); if(nearestId > 0 && (_localRadius==0 || _optimizedPoses.at(signature->id()).getDistance(_optimizedPoses.at(nearestId)) < _localRadius)) { std::multimap links; for(std::multimap::iterator iter=_constraints.begin(); iter!=_constraints.end(); ++iter) { if(uContains(_optimizedPoses, iter->second.from()) && uContains(_optimizedPoses, iter->second.to())) { links.insert(std::make_pair(iter->second.from(), iter->second.to())); links.insert(std::make_pair(iter->second.to(), iter->second.from())); // <-> } } std::list > path = graph::computePath(_optimizedPoses, links, nearestId, signature->id()); if(path.size() == 0) { UWARN("Could not compute a path between %d and %d", nearestId, signature->id()); } else { for(std::list >::iterator iter=path.begin(); iter!=path.end(); ++iter) { if(iter->first>0) { if(immunizedLocally >= maxLocalLocationsImmunized) { // set 20 to avoid this warning when starting mapping if(maxLocalLocationsImmunized > 20 && _someNodesHaveBeenTransferred) { UWARN("Could not immunize the whole local path (%d) between " "%d and %d (max location immunized=%d). You may want " "to increase %s (current=%f (%d of WM=%d)) " "to be able to immunize longer paths.", (int)path.size(), nearestId, signature->id(), maxLocalLocationsImmunized, Parameters::kRGBDLocalImmunizationRatio().c_str(), _localImmunizationRatio, maxLocalLocationsImmunized, (int)_memory->getWorkingMemSize(true)); } break; } else if(!_memory->isInSTM(iter->first)) { if(immunizedLocations.insert(iter->first).second) { // Count only non-intermediate nodes (intermediate nodes are still // immunized but don't consume the immunization budget/statistic). const Signature * sLocal = _memory->getSignature(iter->first); if(sLocal == 0 || sLocal->getWeight() >= 0) { ++immunizedLocally; } } //UDEBUG("local node %d on path immunized=1", iter->first); } } } } } } // retrieval based on the nodes close the the nearest pose in WM // immunize closest nodes std::map nearNodes = graph::findNearestNodes(signature->id(), _optimizedPoses, _localRadius); // sort by distance std::multimap nearNodesByDist; for(std::map::iterator iter=nearNodes.lower_bound(1); iter!=nearNodes.end(); ++iter) { nearNodesByDist.insert(std::make_pair(iter->second, iter->first)); } UINFO("near nodes=%d, max local immunized=%d (immunized by path so far=%d), ratio=%f WM=%d", (int)nearNodesByDist.size(), maxLocalLocationsImmunized, immunizedLocally, _localImmunizationRatio, (int)_memory->getWorkingMemSize(true)); std::list retrievalLocalIdsIntermediate; for(std::multimap::iterator iter=nearNodesByDist.begin(); iter!=nearNodesByDist.end() && (retrievalLocalIds.size() < _maxLocalRetrieved || immunizedLocally < maxLocalLocationsImmunized); ++iter) { const Signature * s = _memory->getSignature(iter->second); if(s!=0) { if(s->getWeight() != -1 && retrievalLocalIds.size() < _maxLocalRetrieved) { // If there is a change of direction, better to be retrieving // all nearest signatures than only newest neighbors. // Use getNeighborsId instead of direct links to support intermediate nodes. std::map ids = _memory->getNeighborsId(s->id(), 2, _maxLocalRetrieved-retrievalLocalIds.size(), true, false, false); for(std::map::const_reverse_iterator jter=ids.rbegin(); jter!=ids.rend() && (retrievalLocalIds.size() < _maxLocalRetrieved || jter->second == 0); ++jter) { if(_memory->getSignature(jter->first) == 0) { if(jter->second == 0) { UINFO("retrieval of intermediate node %d (margin=%d) on local map (from=%d)", jter->first, jter->second, s->id()); retrievalLocalIdsIntermediate.push_back(jter->first); } else { UINFO("retrieval of node %d (margin=%d) on local map (from=%d)", jter->first, jter->second, s->id()); retrievalLocalIds.push_back(jter->first); } } } } if(!_memory->isInSTM(s->id()) && immunizedLocally < maxLocalLocationsImmunized) { // Count only non-intermediate nodes (intermediate nodes are still // immunized but don't consume the immunization budget/statistic). if(immunizedLocations.insert(s->id()).second && s->getWeight() >= 0) { ++immunizedLocally; } //UDEBUG("local node %d (%f m) immunized=1", iter->second, iter->first); } } } // well, if the maximum retrieved is not reached, look for neighbors in database if(retrievalLocalIds.size() < _maxLocalRetrieved) { std::set retrievalLocalIdsSet(retrievalLocalIds.begin(), retrievalLocalIds.end()); retrievalLocalIdsSet.insert(retrievalLocalIdsIntermediate.begin(), retrievalLocalIdsIntermediate.end()); for(std::list::iterator iter=retrievalLocalIds.begin(); iter!=retrievalLocalIds.end() && retrievalLocalIds.size() < _maxLocalRetrieved; ++iter) { std::map ids = _memory->getNeighborsId(*iter, 2, _maxLocalRetrieved - (unsigned int)retrievalLocalIds.size() + 1, true, false, false); for(std::map::reverse_iterator jter=ids.rbegin(); jter!=ids.rend() && (retrievalLocalIds.size() < _maxLocalRetrieved || jter->second == 0); ++jter) { if(_memory->getSignature(jter->first) == 0 && retrievalLocalIdsSet.find(jter->first) == retrievalLocalIdsSet.end()) { if(jter->second == 0) { UINFO("retrieval of intermediate node %d (margin=%d) on local map (from=%d)", jter->first, jter->second, *iter); retrievalLocalIdsIntermediate.push_back(jter->first); } else { UINFO("retrieval of node %d (margin=%d) on local map (from=%d)", jter->first, jter->second, *iter); retrievalLocalIds.push_back(jter->first); } retrievalLocalIdsSet.insert(jter->first); } } } } // update Age of the close signatures (oldest the farthest) for(std::multimap::reverse_iterator iter=nearNodesByDist.rbegin(); iter!=nearNodesByDist.rend(); ++iter) { _memory->updateAge(iter->second); } // insert them first to make sure they are loaded. reactivatedIds.insert(reactivatedIds.begin(), retrievalLocalIdsIntermediate.begin(), retrievalLocalIdsIntermediate.end()); reactivatedIds.insert(reactivatedIds.begin(), retrievalLocalIds.begin(), retrievalLocalIds.end()); } } //============================================================ // RETRIEVAL 3/3 : Load signatures from the database //============================================================ if(reactivatedIds.size()) { // Not important if the loop closure hypothesis don't have all its neighbors loaded, // only a loop closure link is added... signaturesRetrieved = _memory->reactivateSignatures( reactivatedIds, _maxRetrieved+(unsigned int)retrievalLocalIds.size(), // add path retrieved timeRetrievalDbAccess); ULOGGER_INFO("retrieval of %d (db time = %fs)", (int)signaturesRetrieved.size(), timeRetrievalDbAccess); timeRetrievalDbAccess += timeGetNeighborsTimeDb + timeGetNeighborsSpaceDb; UINFO("total timeRetrievalDbAccess=%fs", timeRetrievalDbAccess); // Immunize just retrieved signatures immunizedLocations.insert(signaturesRetrieved.begin(), signaturesRetrieved.end()); if(!signaturesRetrieved.empty() && !_globalScanMap.empty()) { UWARN("Some signatures have been retrieved from memory management, clearing global scan map..."); _globalScanMap.clear(); _globalScanMapPoses.clear(); } } timeReactivations = timer.ticks(); ULOGGER_INFO("timeReactivations=%fs", timeReactivations); //============================================================ // Proximity detections //============================================================ std::list > loopClosureLinksAdded; int loopClosureVisualInliers = 0; // for statistics float loopClosureVisualInliersRatio = 0.0f; int loopClosureVisualMatches = 0; float loopClosureVisualVariance = 0.0f; float loopClosureLinearVariance = 0.0f; float loopClosureAngularVariance = 0.0f; float loopClosureVisualInliersMeanDist = 0; float loopClosureVisualInliersDistribution = 0; int proximityDetectionsAddedVisually = 0; int proximityDetectionsAddedByICPMulti = 0; int proximityDetectionsAddedByICPGlobal = 0; int lastProximitySpaceClosureId = 0; int proximitySpacePaths = 0; int localVisualPathsChecked = 0; int localScanPathsChecked = 0; int loopIdSuppressedByProximity = 0; if(_proximityBySpace && _localRadius > 0 && _rgbdSlamMode && signature->getWeight() >= 0) // not an intermediate node { if(_startNewMapOnLoopClosure && _memory->getWorkingMemSize(true)>0 && // must have an old map _localizationCovariance.empty() && // if we didn't localize yet graph::filterLinks(signature->getLinks(), Link::kSelfRefLink).size() == 0) // alone in new session { UINFO("Proximity detection by space disabled as if we force to have a global loop " "closure with previous map before doing proximity detections (%s=true).", Parameters::kRtabmapStartNewMapOnLoopClosure().c_str()); } else if(_graphOptimizer->iterations() == 0) { UWARN("Cannot do local loop closure detection in space if graph optimization is disabled!"); } else { // In localization mode, no need to check local loop // closures if we are already localized by a landmark. // don't do it if it is a small displacement unless the previous signature didn't have a loop closure // don't do it if there is a too fast movement if((!smallDisplacement || !localizationOnPreviousUpdate) && !tooFastMovement) { //============================================================ // LOCAL LOOP CLOSURE SPACE //============================================================ // // 1) compare visually with nearest locations // UDEBUG("Proximity detection (local loop closure in SPACE using matching images, local radius=%fm)", _localRadius); std::map nearestIds = graph::findNearestNodes(signature->id(), _optimizedPoses, _localRadius); UDEBUG("nearestIds=%d/%d", (int)nearestIds.size(), (int)_optimizedPoses.size()); std::map nearestPoses; std::map optimizedPosesWithOdomCache; std::multimap links; if(_memory->isIncremental() && _proximityMaxGraphDepth>0) { // get bidirectional links for(std::multimap::iterator iter=_constraints.begin(); iter!=_constraints.end(); ++iter) { if(uContains(_optimizedPoses, iter->second.from()) && uContains(_optimizedPoses, iter->second.to())) { links.insert(std::make_pair(iter->second.from(), iter->second.to())); links.insert(std::make_pair(iter->second.to(), iter->second.from())); // <-> } } if(_odomCachePoses.size() > 1) { // Add odometry cache if it contains a loop closure // That could happen when we just switched from localization mode to // mapping mode while being localized on the previous session. optimizedPosesWithOdomCache = _optimizedPoses; optimizedPosesWithOdomCache.insert(_odomCachePoses.begin(), _odomCachePoses.end()); for(std::multimap::iterator iter=_odomCacheConstraints.begin(); iter!=_odomCacheConstraints.end(); ++iter) { if(uContains(optimizedPosesWithOdomCache, iter->second.from()) && uContains(optimizedPosesWithOdomCache, iter->second.to()) && iter->second.from() != iter->second.to()) { links.insert(std::make_pair(iter->second.from(), iter->second.to())); links.insert(std::make_pair(iter->second.to(), iter->second.from())); // <-> } } } } std::map proximityPathDepths; if(_memory->isIncremental() && _proximityMaxGraphDepth > 0) { proximityPathDepths = graph::computePathDepths(links, signature->id(), _proximityMaxGraphDepth); } for(std::map::iterator iter=nearestIds.lower_bound(1); iter!=nearestIds.end(); ++iter) { if(_memory->getStMem().find(iter->first) == _memory->getStMem().end()) { if(_memory->isIncremental() && _proximityMaxGraphDepth > 0) { std::map::const_iterator depthIter = proximityPathDepths.find(iter->first); if(depthIter == proximityPathDepths.end()) { continue; } nearestPoses.insert(std::make_pair(iter->first, _optimizedPoses.at(iter->first))); } else { nearestPoses.insert(std::make_pair(iter->first, _optimizedPoses.at(iter->first))); } } } UDEBUG("nearestPoses=%d", (int)nearestPoses.size()); // segment poses by paths, only one detection per path, landmarks are ignored std::map > nearestPathsNotSorted = getPaths(nearestPoses, _optimizedPoses.at(signature->id()), _proximityMaxGraphDepth); UDEBUG("got %d paths", (int)nearestPathsNotSorted.size()); // sort nearest paths by highest likelihood (if two have same likelihood, sort by id) std::map > nearestPaths; Transform currentPoseInv = _optimizedPoses.at(signature->id()).inverse(); for(std::map >::const_iterator iter=nearestPathsNotSorted.begin();iter!=nearestPathsNotSorted.end(); ++iter) { const std::map & path = iter->second; float highestLikelihood = 0.0f; int highestLikelihoodId = iter->first; float smallestDistanceSqr = -1; for(std::map::const_iterator jter=path.begin(); jter!=path.end(); ++jter) { float v = uValue(likelihood, jter->first, 0.0f); float distance = (currentPoseInv * jter->second).getNormSquared(); if(v > highestLikelihood || (v == highestLikelihood && (smallestDistanceSqr < 0 || distance < smallestDistanceSqr))) { highestLikelihood = v; highestLikelihoodId = jter->first; smallestDistanceSqr = distance; } } nearestPaths.insert(std::make_pair(NearestPathKey(highestLikelihood, highestLikelihoodId, smallestDistanceSqr), path)); } UDEBUG("nearestPaths=%d proximityMaxPaths=%d", (int)nearestPaths.size(), _proximityMaxPaths); timeProximityBySpaceSearch = timer.ticks(); ULOGGER_INFO("timeProximityBySpaceSearch=%fs", timeProximityBySpaceSearch); float proximityFilteringRadius = _proximityFilteringRadius; if(_maxLoopClosureDistance>0.0f && (proximityFilteringRadius <= 0.0f || _maxLoopClosureDistance >::const_reverse_iterator iter=nearestPaths.rbegin(); iter!=nearestPaths.rend() && (_proximityMaxPaths <= 0 || localVisualPathsChecked < _proximityMaxPaths); ++iter) { std::map path = iter->second; UASSERT(path.size()); //find the nearest pose on the path looking in the same direction path.insert(std::make_pair(signature->id(), _optimizedPoses.at(signature->id()))); path = graph::findNearestPoses(signature->id(), path, _localRadius, _proximityAngle); //take the one with highest likelihood if not null int nearestId = 0; if(iter->first.likelihood > 0.0f && path.find(iter->first.id)!=path.end()) { nearestId = iter->first.id; } else { nearestId = rtabmap::graph::findNearestNode(path, _optimizedPoses.at(signature->id())); } if(nearestId > 0) { // nearest pose must not be linked to current location and enough close if(!signature->hasLink(nearestId) && (proximityFilteringRadius <= 0.0f || _optimizedPoses.at(signature->id()).getDistanceSquared(_optimizedPoses.at(nearestId)) < proximityFilteringRadius*proximityFilteringRadius)) { ++localVisualPathsChecked; RegistrationInfo info; Transform guess; if(_proximityOdomGuess) { // Use odometry as guess so that correspondences can be computed by projection guess = _optimizedPoses.at(nearestId).inverse()*_optimizedPoses.at(signature->id()); } //else: guess is null to make sure visual correspondences are globally computed Transform transform = _memory->computeTransform(nearestId, signature->id(), guess, &info); if(!transform.isNull()) { transform = transform.inverse(); if(proximityFilteringRadius <= 0 || transform.getNormSquared() <= proximityFilteringRadius*proximityFilteringRadius) { UINFO("[Visual] Add local loop closure in SPACE (%d->%d) %s", signature->id(), nearestId, transform.prettyPrint().c_str()); UASSERT(!info.covariance.empty() && info.covariance.at(0,0) > 0.0 && info.covariance.at(5,5) > 0.0); //for statistics loopClosureVisualInliersMeanDist = info.inliersMeanDistance; loopClosureVisualInliersDistribution = info.inliersDistribution; ++proximityDetectionsAddedVisually; lastProximitySpaceClosureId = nearestId; loopClosureVisualInliers = info.inliers; loopClosureVisualInliersRatio = info.inliersRatio; loopClosureVisualMatches = info.matches; loopClosureVisualVariance = info.variance; cv::Mat information = getInformation(info.covariance); loopClosureLinearVariance = 1.0/information.at(0,0); loopClosureAngularVariance = 1.0/information.at(5,5); Link::Type type = Link::kLocalSpaceClosure; if(_loopClosureHypothesis.first>0 && nearestIds.find(_loopClosureHypothesis.first)!=nearestIds.end()) { // Avoid transform computation on the global loop closure if a visual proximity // one has been detected close (inside proximity radius) to that hypothesis. UDEBUG("Proximity detection on %d is close to loop closure %d, ignoring loop closure transform estimation...", nearestId, _loopClosureHypothesis.first); if(nearestId == _loopClosureHypothesis.first) { type = Link::kGlobalClosure; loopIdSuppressedByProximity = nearestId; } else if(loopIdSuppressedByProximity == 0) { loopIdSuppressedByProximity = nearestId; } } _memory->addLink(Link(signature->id(), nearestId, type, transform, information)); loopClosureLinksAdded.push_back(std::make_pair(signature->id(), nearestId)); } else { UWARN("Ignoring local loop closure with %d because resulting " "transform is too large!? (%fm > %fm)", nearestId, transform.getNorm(), proximityFilteringRadius); } } } else if(!signature->hasLink(nearestId) && proximityFilteringRadius>0.0f) { UDEBUG("Skipping path %d as most likely ID %d is too far %f > %f (%s)", iter->first.id, nearestId, _optimizedPoses.at(signature->id()).getDistance(_optimizedPoses.at(nearestId)), proximityFilteringRadius, Parameters::kRGBDProximityPathFilteringRadius().c_str()); } } } timeProximityBySpaceVisualDetection = timer.ticks(); ULOGGER_INFO("timeProximityBySpaceVisualDetection=%fs", timeProximityBySpaceVisualDetection); // // 2) compare locally with nearest locations by scan matching // UDEBUG("Proximity detection (local loop closure in SPACE with scan matching)"); if( _proximityMaxNeighbors <= 0) { UDEBUG("Proximity by scan matching is disabled (%s=%d).", Parameters::kRGBDProximityPathMaxNeighbors().c_str(), _proximityMaxNeighbors); } else if(!signature->sensorData().laserScanCompressed().isEmpty()) { proximitySpacePaths = (int)nearestPaths.size(); for(std::map >::const_reverse_iterator iter=nearestPaths.rbegin(); iter!=nearestPaths.rend() && (_proximityMaxPaths <= 0 || localScanPathsChecked < _proximityMaxPaths); ++iter) { std::map path = iter->second; // should contain only nodes (no landmarks) UASSERT(path.size()); UASSERT(path.begin()->first > 0); //find the nearest pose on the path int nearestId = rtabmap::graph::findNearestNode(path, _optimizedPoses.at(signature->id())); UASSERT(nearestId > 0); //UDEBUG("Path %d (size=%d) distance=%fm", nearestId, (int)path.size(), _optimizedPoses.at(signature->id()).getDistance(_optimizedPoses.at(nearestId))); // nearest pose must be close and not linked to current location if(!signature->hasLink(nearestId)) { if(_proximityMaxNeighbors < _proximityMaxGraphDepth || _proximityMaxGraphDepth == 0) { std::map filteredPath; int i=0; std::map::iterator nearestIdIter = path.find(nearestId); // "_proximityMaxNeighbors-1" means that if _proximityMaxNeighbors=1, // only nearest node on the path is taken (no scan merging). Useful to find // proximity detection between only 2 nodes with 360x360 lidar scans. for(std::map::iterator iter=nearestIdIter; iter!=path.end() && i<=_proximityMaxNeighbors-1; ++iter, ++i) { filteredPath.insert(*iter); } i=1; for(std::map::reverse_iterator iter(nearestIdIter); iter!=path.rend() && i<=_proximityMaxNeighbors-1; ++iter, ++i) { filteredPath.insert(*iter); } path = filteredPath; } // Assemble scans in the path and do ICP only std::map optimizedLocalPath; if(_globalScanMap.empty() && _proximityRawPosesUsed) { //optimize the path's poses locally cv::Mat covariance; path = optimizeGraph(nearestId, uKeysSet(path), std::map(), false, covariance); // transform local poses in optimized graph referential if(!uContains(path, nearestId)) { UERROR("Proximity path not containing nearest ID ?! Skipping this path."); continue; } Transform t = _optimizedPoses.at(nearestId) * path.at(nearestId).inverse(); for(std::map::iterator jter=path.lower_bound(1); jter!=path.end(); ++jter) { optimizedLocalPath.insert(std::make_pair(jter->first, t * jter->second)); } } else { optimizedLocalPath = path; } std::map filteredPath; if(_globalScanMap.empty() && optimizedLocalPath.size() > 2 && proximityFilteringRadius > 0.0f) { // path filtering filteredPath = graph::radiusPosesFiltering(optimizedLocalPath, proximityFilteringRadius, 0, true); // make sure the current pose is still here filteredPath.insert(*optimizedLocalPath.find(nearestId)); } else { filteredPath = optimizedLocalPath; } if(filteredPath.size() > 0) { // add current node to poses filteredPath.insert(std::make_pair(signature->id(), _optimizedPoses.at(signature->id()))); //The nearest will be the reference for a loop closure transform if(signature->getLinks().find(nearestId) == signature->getLinks().end()) { ++localScanPathsChecked; RegistrationInfo info; Transform transform; bool icpMulti = true; if(_globalScanMap.empty()) { transform = _memory->computeIcpTransformMulti(signature->id(), nearestId, filteredPath, &info); } else { UASSERT_MSG(_globalScanMapPoses.find(nearestId) != _globalScanMapPoses.end(), uFormat("Pose of %d not found in global scan poses", nearestId).c_str()); icpMulti = false; // use pre-assembled scan map SensorData assembledData; assembledData.setId(nearestId); assembledData.setLaserScan( LaserScan(_globalScanMap, signature->sensorData().laserScanCompressed().maxPoints(), signature->sensorData().laserScanCompressed().rangeMax(), _globalScanMapPoses.at(nearestId).inverse() * (signature->sensorData().laserScanCompressed().is2d()?Transform(0,0,signature->sensorData().laserScanCompressed().localTransform().z(),0,0,0):Transform::getIdentity()))); Signature nearestNode(assembledData); Transform guess = filteredPath.at(nearestId).inverse() * filteredPath.at(signature->id()); transform = _memory->computeIcpTransform(nearestNode, *signature, guess, &info); if(!transform.isNull()) { transform = transform.inverse(); } } if(!transform.isNull()) { UINFO("[Scan matching] Add local loop closure in SPACE (%d->%d) %s", signature->id(), nearestId, transform.prettyPrint().c_str()); cv::Mat scanMatchingIds; if(_scanMatchingIdsSavedInLinks) { std::stringstream stream; stream << "SCANS:"; for(std::map::iterator iter=optimizedLocalPath.begin(); iter!=optimizedLocalPath.end(); ++iter) { if(iter != optimizedLocalPath.begin()) { stream << ";"; } stream << uNumber2Str(iter->first); } std::string scansStr = stream.str(); scanMatchingIds = cv::Mat(1, int(scansStr.size()+1), CV_8SC1, (void *)scansStr.c_str()); scanMatchingIds = compressData2(scanMatchingIds); // compressed } // set Identify covariance for laser scan matching only UASSERT(!info.covariance.empty() && info.covariance.at(0,0) > 0.0 && info.covariance.at(5,5) > 0.0); _memory->addLink(Link(signature->id(), nearestId, Link::kLocalSpaceClosure, transform, getInformation(info.covariance)/_proximityMergedScanCovFactor, scanMatchingIds)); loopClosureLinksAdded.push_back(std::make_pair(signature->id(), nearestId)); if(icpMulti) { ++proximityDetectionsAddedByICPMulti; } else { ++proximityDetectionsAddedByICPGlobal; } // no local loop closure added visually if(proximityDetectionsAddedVisually == 0) { lastProximitySpaceClosureId = nearestId; } } else { UINFO("Local scan matching rejected: %s", info.rejectedMsg.c_str()); } if(!_globalScanMap.empty()) { break; } } } } else { //UDEBUG("Path %d ignored", nearestId); } } } } } } timeProximityBySpaceDetection = timer.ticks(); ULOGGER_INFO("timeProximityBySpaceDetection=%fs", timeProximityBySpaceDetection); //============================================================= // Global loop closure detection // (updated: place this after retrieval to be sure that neighbors of the loop closure are in RAM) //============================================================= if(_loopClosureHypothesis.first>0) { if(loopIdSuppressedByProximity==0) { //Compute transform if metric data are present Transform transform; RegistrationInfo info; info.covariance = cv::Mat::eye(6,6,CV_64FC1); if(_rgbdSlamMode) { transform = _memory->computeTransform( _loopClosureHypothesis.first, signature->id(), _loopClosureIdentityGuess?Transform::getIdentity():Transform(), &info); loopClosureVisualInliersMeanDist = info.inliersMeanDistance; loopClosureVisualInliersDistribution = info.inliersDistribution; loopClosureVisualInliers = info.inliers; loopClosureVisualInliersRatio = info.inliersRatio; loopClosureVisualMatches = info.matches; loopClosureVisualVariance = info.variance; rejectedLoopClosure = transform.isNull(); if(rejectedLoopClosure) { UWARN("Rejected loop closure %d -> %d: %s", _loopClosureHypothesis.first, signature->id(), info.rejectedMsg.c_str()); } else if(_maxLoopClosureDistance>0.0f && transform.getNorm() > _maxLoopClosureDistance) { rejectedLoopClosure = true; UWARN("Rejected localization %d -> %d because distance to map (%fm) is over %s=%fm.", _loopClosureHypothesis.first, signature->id(), transform.getNorm(), Parameters::kRGBDMaxLoopClosureDistance().c_str(), _maxLoopClosureDistance); } else { transform = transform.inverse(); } } if(!rejectedLoopClosure) { // Make the new one the parent of the old one UASSERT(!info.covariance.empty() && info.covariance.at(0,0) > 0.0 && info.covariance.at(5,5) > 0.0); loopClosureLinearVariance = uMax3(info.covariance.at(0,0), info.covariance.at(1,1)>=9999?0:info.covariance.at(1,1), info.covariance.at(2,2)>=9999?0:info.covariance.at(2,2)); loopClosureAngularVariance = uMax3(info.covariance.at(3,3)>=9999?0:info.covariance.at(3,3), info.covariance.at(4,4)>=9999?0:info.covariance.at(4,4), info.covariance.at(5,5)); cv::Mat information = getInformation(info.covariance); rejectedLoopClosure = !_memory->addLink(Link(signature->id(), _loopClosureHypothesis.first, Link::kGlobalClosure, transform, information)); if(!rejectedLoopClosure) { loopClosureLinksAdded.push_back(std::make_pair(signature->id(), _loopClosureHypothesis.first)); } } if(rejectedLoopClosure) { _loopClosureHypothesis.first = 0; } } else if(loopIdSuppressedByProximity != _loopClosureHypothesis.first) { _loopClosureHypothesis.first = 0; } } timeAddLoopClosureLink = timer.ticks(); ULOGGER_INFO("timeAddLoopClosureLink=%fs", timeAddLoopClosureLink); //============================================================ // Landmark //============================================================ std::map > landmarksDetected; // if(!signature->getLandmarks().empty() && !_graphOptimizer->landmarksIgnored() && signature->getWeight()!=-1) { bool hasGlobalLoopClosuresInOdomCache = !graph::filterLinks(_odomCacheConstraints, Link::kGlobalClosure, true).empty() || _loopClosureHypothesis.first != 0; UDEBUG("hasGlobalLoopClosuresInOdomCache=%d", hasGlobalLoopClosuresInOdomCache?1:0); for(std::map::const_iterator iter=signature->getLandmarks().begin(); iter!=signature->getLandmarks().end(); ++iter) { if(uContains(_memory->getLandmarksIndex(), iter->first) && _memory->getLandmarksIndex().find(iter->first)->second.size()>1) { if(!_memory->isIncremental() && // In localization mode !hasGlobalLoopClosuresInOdomCache && // If there are global loop closures in odom cache, we can keep far landmarks _localRadius>0.0 && iter->second.transform().getNormSquared() > _localRadius*_localRadius) { // Ignore landmark detections over local radius UWARN("Ignoring landmark %d for localization as it is too far (%fm > %s=%f) " "and odom cache doesn't contain global loop closure(s).", iter->first, iter->second.transform().getNorm(), Parameters::kRGBDLocalRadius().c_str(), _localRadius); } else { UINFO("Landmark %d observed again! Seen the first time by node %d.", -iter->first, *_memory->getLandmarksIndex().find(iter->first)->second.begin()); landmarksDetected.insert(std::make_pair(iter->first, _memory->getLandmarksIndex().find(iter->first)->second)); rejectedLoopClosure = false; // If it was true, it will be set back to false if landmarks are rejected on graph optimization loopClosureLinksAdded.push_back(std::make_pair(signature->id(), iter->first)); } } } } //============================================================ // Add virtual links if a path is activated //============================================================ if(_path.size()) { // Add a virtual loop closure link to keep the path linked to local map if( signature->id() != _path[_pathCurrentIndex].first && !signature->hasLink(_path[_pathCurrentIndex].first)) { UASSERT(uContains(_optimizedPoses, signature->id())); UASSERT_MSG(uContains(_optimizedPoses, _path[_pathCurrentIndex].first), uFormat("id=%d", _path[_pathCurrentIndex].first).c_str()); Transform virtualLoop = _optimizedPoses.at(signature->id()).inverse() * _optimizedPoses.at(_path[_pathCurrentIndex].first); _memory->addLink(Link(signature->id(), _path[_pathCurrentIndex].first, Link::kVirtualClosure, virtualLoop, cv::Mat::eye(6,6,CV_64FC1)*0.01)); // set high variance } } //============================================================ // Optimize map graph //============================================================ graph::MaxGraphErrors maxGraphErrors; std::pair maxGraphErrorsLinearIds(0,0); std::pair maxGraphErrorsAngularIds(0,0); std::pair maxGraphErrorsRemovedIds(0,0); int maxGraphErrorsRemovedCount = 0; double optimizationError = 0.0; int optimizationIterations = 0; Transform previousMapCorrection; bool delayedLocalization = false; int odomCacheProximityLinksCleared = 0; UDEBUG("RGB-D SLAM mode: %d", _rgbdSlamMode?1:0); UDEBUG("Incremental: %d", _memory->isIncremental()); UDEBUG("Loop hyp: %d", _loopClosureHypothesis.first); UDEBUG("Last prox: %d", lastProximitySpaceClosureId); UDEBUG("Reduced ids: %d", (int)statistics_.reducedIds().size()); UDEBUG("Has prior: %d (prior ignored=%d)", signature->hasLink(signature->id(), Link::kPosePrior)?1:0, _graphOptimizer->priorsIgnored()?1:0); UDEBUG("Has gravity: %d (sigma=%f, odomGravity=%d, refined=%d)", signature->hasLink(signature->id(), Link::kGravity)?1:0, _graphOptimizer->gravitySigma(), _memory->isOdomGravityUsed()?1:0, neighborLinkRefined?1:0); UDEBUG("Has virtual link: %d", (int)graph::filterLinks(signature->getLinks(), Link::kVirtualClosure, true).size()); UDEBUG("Prox Time: %d", proximityDetectionsInTimeFound); UDEBUG("Landmarks: %d", (int)landmarksDetected.size()); UDEBUG("Retrieved: %d", (int)signaturesRetrieved.size()); UDEBUG("Not self ref links: %d", (int)graph::filterLinks(signature->getLinks(), Link::kSelfRefLink).size()); if(_rgbdSlamMode && signature->getWeight() != -1 // Ignore graph optimization on intermediate nodes && (_loopClosureHypothesis.first>0 || lastProximitySpaceClosureId>0 || // can be different map of the current one statistics_.reducedIds().size() || (signature->hasLink(signature->id(), Link::kPosePrior) && !_graphOptimizer->priorsIgnored()) || // prior edge (signature->hasLink(signature->id(), Link::kGravity) && _graphOptimizer->gravitySigma()>0.0f && (!_memory->isOdomGravityUsed() || neighborLinkRefined)) || // gravity edge proximityDetectionsInTimeFound>0 || !landmarksDetected.empty() || signaturesRetrieved.size()) // can be different map of the current one && (_memory->isIncremental() || // In localization mode, the new node should be linked to another node or a landmark already in the working memory graph::filterLinks(graph::filterLinks(signature->getLinks(), Link::kVirtualClosure), Link::kSelfRefLink).size() || !landmarksDetected.empty())) { UASSERT(uContains(_optimizedPoses, signature->id())); //used in localization mode: filter virtual links std::multimap localizationLinks = graph::filterLinks(signature->getLinks(), Link::kVirtualClosure); localizationLinks = graph::filterLinks(localizationLinks, Link::kSelfRefLink); if(!landmarksDetected.empty() && !_memory->isIncremental()) { for(std::map >::iterator iter=landmarksDetected.begin(); iter!=landmarksDetected.end(); ++iter) { if(_optimizedPoses.find(iter->first)!=_optimizedPoses.end()) { UASSERT(uContains(signature->getLandmarks(), iter->first)); localizationLinks.insert(std::make_pair(iter->first, signature->getLandmarks().at(iter->first))); } } } bool allLocalizationLinksInGraph = !localizationLinks.empty(); for(std::multimap::iterator iter=localizationLinks.begin(); iter!=localizationLinks.end(); ++iter) { if(!uContains(_optimizedPoses, iter->first)) { allLocalizationLinksInGraph = false; break; } } // Note that in localization mode, we don't re-optimize the graph // if: // 1- there are no signatures retrieved, // 2- we are relocalizing on a node already in the optimized graph if(!_memory->isIncremental() && signaturesRetrieved.empty() && !localizationLinks.empty() && allLocalizationLinksInGraph) { bool rejectLocalization = _odomCachePoses.empty(); if(!_odomCachePoses.empty()) { // Verify if the new localization is valid by checking if there is // not too much deformation using current odometry poses // This will also refine localization links std::map poses = _odomCachePoses; std::multimap constraints = _odomCacheConstraints; // add self referring links (e.g., gravity) std::multimap selfLinks = graph::filterLinks(signature->getLinks(), Link::kSelfRefLink, true); if(_graphOptimizer->priorsIgnored()) { selfLinks = graph::filterLinks(selfLinks, Link::kPosePrior); } constraints.insert(selfLinks.begin(), selfLinks.end()); for(std::multimap::iterator iter=localizationLinks.begin(); iter!=localizationLinks.end(); ++iter) { constraints.insert(std::make_pair(iter->second.from(), iter->second)); } cv::Mat priorInfMat = cv::Mat::eye(6,6, CV_64FC1)*_localizationPriorInf; for(std::multimap::iterator iter=constraints.begin(); iter!=constraints.end(); ++iter) { std::map::iterator iterPose = _optimizedPoses.find(iter->second.to()); if(iterPose != _optimizedPoses.end() && poses.find(iterPose->first) == poses.end()) { poses.insert(*iterPose); // make the poses in the map fixed constraints.insert(std::make_pair(iterPose->first, Link(iterPose->first, iterPose->first, Link::kPosePrior, iterPose->second, priorInfMat))); UDEBUG("Constraint %d->%d: %s (type=%s, var=%f)", iterPose->first, iterPose->first, iterPose->second.prettyPrint().c_str(), Link::typeName(Link::kPosePrior).c_str(), 1./_localizationPriorInf); } UDEBUG("Constraint %d->%d: %s (type=%s, var = %f %f)", iter->second.from(), iter->second.to(), iter->second.transform().prettyPrint().c_str(), iter->second.typeName().c_str(), iter->second.transVariance(), iter->second.rotVariance()); } std::map posesOut; std::multimap edgeConstraintsOut; bool priorsIgnored = _graphOptimizer->priorsIgnored(); UDEBUG("priorsIgnored was %s", priorsIgnored?"true":"false"); _graphOptimizer->setPriorsIgnored(false); //temporary set false to use priors above to fix nodes of the map // If slam2d: get connected graph while keeping original roll,pitch,z values. _graphOptimizer->getConnectedGraph(signature->id(), poses, constraints, posesOut, edgeConstraintsOut); if(ULogger::level() == ULogger::kDebug) { for(std::map::iterator iter=posesOut.begin(); iter!=posesOut.end(); ++iter) { UDEBUG("Pose %d %s", iter->first, iter->second.prettyPrint().c_str()); } } cv::Mat locOptCovariance; std::map optPoses; if(!posesOut.empty() && posesOut.begin()->first < _odomCachePoses.begin()->first) { optPoses = _graphOptimizer->optimize(posesOut.begin()->first, posesOut, edgeConstraintsOut, locOptCovariance, 0, &optimizationError, &optimizationIterations); } else { UERROR("Invalid localization constraints"); } _graphOptimizer->setPriorsIgnored(priorsIgnored); // set back for(std::map::iterator iter=optPoses.begin(); iter!=optPoses.end(); ++iter) { UDEBUG("Opt %d %s", iter->first, iter->second.prettyPrint().c_str()); } std::list > removedLinks; if(optPoses.empty()) { UWARN("Optimization failed, rejecting localization!"); rejectLocalization = true; } else { UINFO("Compute max graph errors..."); maxGraphErrors = graph::computeMaxGraphErrors( optPoses, edgeConstraintsOut, _graphOptimizer->isSlam2d()); if(!maxGraphErrors.linearLink.isValid() && !maxGraphErrors.angularLink.isValid()) { UWARN("Could not compute graph errors! Rejecting localization!"); rejectLocalization = true; } if(maxGraphErrors.linearLink.isValid()) { maxGraphErrorsLinearIds = std::make_pair(maxGraphErrors.linearLink.from(), maxGraphErrors.linearLink.to()); UINFO("Max optimization linear error = %f m (link %d->%d, var=%f, ratio error/std=%f, thr=%f)", maxGraphErrors.linear, maxGraphErrors.linearLink.from(), maxGraphErrors.linearLink.to(), maxGraphErrors.linearLink.transVariance(), maxGraphErrors.linear/sqrt(maxGraphErrors.linearLink.transVariance()), _optimizationMaxError); if(_optimizationMaxError > 0.0f && maxGraphErrors.linearRatio > _optimizationMaxError) { if( _optimizationMaxErrorRepairRadius > 0.0 && maxGraphErrorsLinearIds == _lastRejectedLoopClosureIds && graph::findLink(edgeConstraintsOut, maxGraphErrorsLinearIds.first, maxGraphErrorsLinearIds.second) != edgeConstraintsOut.end()) { UWARN("We detected 2 consecutive loop closure rejections because of the same loop closure link (%d->%d), trying optimization again without that link...", maxGraphErrorsLinearIds.first, maxGraphErrorsLinearIds.second); UDEBUG("priorsIgnored was %s", priorsIgnored?"true":"false"); _graphOptimizer->setPriorsIgnored(false); //temporary set false to use priors above to fix nodes of the map removedLinks = repairGraph( maxGraphErrors, optPoses, edgeConstraintsOut, optimizationError, optimizationIterations, locOptCovariance); _graphOptimizer->setPriorsIgnored(priorsIgnored); // set back if(removedLinks.empty()) { UWARN("Optimization failed when trying to repair the graph."); rejectLocalization = true; } } else { rejectLocalization = true; } if(rejectLocalization) { UWARN("Rejecting localization (%d <-> %d) in this " "iteration because a wrong loop closure has been " "detected after graph optimization, resulting in " "a maximum graph error ratio of %f (edge %d->%d, type=%d, abs error=%f m, stddev=%f). The " "maximum error ratio parameter \"%s\" is %f of std deviation.", localizationLinks.rbegin()->second.from(), localizationLinks.rbegin()->second.to(), maxGraphErrors.linearRatio, maxGraphErrors.linearLink.from(), maxGraphErrors.linearLink.to(), maxGraphErrors.linearLink.type(), maxGraphErrors.linear, sqrt(maxGraphErrors.linearLink.transVariance()), Parameters::kRGBDOptimizeMaxError().c_str(), _optimizationMaxError); if(maxGraphErrors.linearLink.type() != Link::kNeighbor) { _lastRejectedLoopClosureIds = maxGraphErrorsLinearIds; } } } else if(_optimizationMaxError == 0.0f && maxGraphErrors.linearRatio>100 && !_graphOptimizer->isRobust()) { UERROR("Huge optimization error detected!" "Linear error ratio of %f (edge %d->%d, type=%d, abs error=%f m, stddev=%f). You may consider " "enabling \"%s\" to reject those bad optimizations by setting it to a non null value!", maxGraphErrors.linearRatio, maxGraphErrors.linearLink.from(), maxGraphErrors.linearLink.to(), maxGraphErrors.linearLink.type(), maxGraphErrors.linear, sqrt(maxGraphErrors.linearLink.transVariance()), Parameters::kRGBDOptimizeMaxError().c_str()); } } if(maxGraphErrors.angularLink.isValid()) { maxGraphErrorsAngularIds = std::make_pair(maxGraphErrors.angularLink.from(), maxGraphErrors.angularLink.to()); UINFO("Max optimization angular error = %f deg (link %d->%d, var=%f, ratio error/std=%f, thr=%f)", maxGraphErrors.angular*180.0f/CV_PI, maxGraphErrors.angularLink.from(), maxGraphErrors.angularLink.to(), maxGraphErrors.angularLink.rotVariance(), maxGraphErrors.angular/sqrt(maxGraphErrors.angularLink.rotVariance()), _optimizationMaxError); if(_optimizationMaxError > 0.0f && maxGraphErrors.angularRatio > _optimizationMaxError) { UWARN("Rejecting localization (%d <-> %d) in this " "iteration because a wrong loop closure has been " "detected after graph optimization, resulting in " "a maximum graph error ratio of %f (edge %d->%d, type=%d, abs error=%f deg, stddev=%f). The " "maximum error ratio parameter \"%s\" is %f of std deviation.", localizationLinks.rbegin()->second.from(), localizationLinks.rbegin()->second.to(), maxGraphErrors.angularRatio, maxGraphErrors.angularLink.from(), maxGraphErrors.angularLink.to(), maxGraphErrors.angularLink.type(), maxGraphErrors.angular*180.0f/CV_PI, sqrt(maxGraphErrors.angularLink.rotVariance()), Parameters::kRGBDOptimizeMaxError().c_str(), _optimizationMaxError); rejectLocalization = true; } else if(_optimizationMaxError == 0.0f && maxGraphErrors.angularRatio>100 && !_graphOptimizer->isRobust()) { UERROR("Huge optimization error detected!" "Angular error ratio of %f (edge %d->%d, type=%d, abs error=%f m, stddev=%f). You may consider " "enabling \"%s\" to reject those bad optimizations by setting it to a non null value!", maxGraphErrors.angularRatio, maxGraphErrors.angularLink.from(), maxGraphErrors.angularLink.to(), maxGraphErrors.angularLink.type(), maxGraphErrors.angular*180.0f/CV_PI, sqrt(maxGraphErrors.angularLink.rotVariance()), Parameters::kRGBDOptimizeMaxError().c_str()); } } } bool hasGlobalLoopClosuresOrLandmarks = false; if(rejectLocalization && (_localizationSecondTryWithoutProximityLinks && !graph::filterLinks(constraints, Link::kLocalSpaceClosure, true).empty())) { // Let's try again without local loop closures localizationLinks = graph::filterLinks(localizationLinks, Link::kLocalSpaceClosure); constraints = graph::filterLinks(constraints, Link::kLocalSpaceClosure); for(std::multimap::iterator iter=constraints.begin(); iter!=constraints.end() && !hasGlobalLoopClosuresOrLandmarks; ++iter) { hasGlobalLoopClosuresOrLandmarks = iter->second.type() == Link::kGlobalClosure || iter->second.type() == Link::kLandmark; } if(hasGlobalLoopClosuresOrLandmarks && !localizationLinks.empty()) { rejectLocalization = false; UWARN("Global and loop closures seem not tallying together, try again to optimize without local loop closures..."); UDEBUG("priorsIgnored was %s", priorsIgnored?"true":"false"); _graphOptimizer->setPriorsIgnored(false); //temporary set false to use priors above to fix nodes of the map // If slam2d: get connected graph while keeping original roll,pitch,z values. _graphOptimizer->getConnectedGraph(signature->id(), poses, constraints, posesOut, edgeConstraintsOut); optPoses.clear(); if(!posesOut.empty() && posesOut.begin()->first < _odomCachePoses.begin()->first) { optPoses = _graphOptimizer->optimize(posesOut.begin()->first, posesOut, edgeConstraintsOut, locOptCovariance, 0, &optimizationError, &optimizationIterations); } else { UERROR("Invalid localization constraints"); } _graphOptimizer->setPriorsIgnored(priorsIgnored); // set back for(std::map::iterator iter=optPoses.begin(); iter!=optPoses.end(); ++iter) { UDEBUG("Opt2 %d %s", iter->first, iter->second.prettyPrint().c_str()); } if(optPoses.empty()) { UWARN("Optimization failed, rejecting localization!"); rejectLocalization = true; } else { UINFO("Compute max graph errors..."); maxGraphErrors = graph::computeMaxGraphErrors( optPoses, edgeConstraintsOut, _graphOptimizer->isSlam2d()); if(!maxGraphErrors.linearLink.isValid() && !maxGraphErrors.angularLink.isValid()) { UWARN("Could not compute graph errors! Rejecting localization!"); rejectLocalization = true; } if(maxGraphErrors.linearLink.isValid()) { maxGraphErrorsLinearIds = std::make_pair(maxGraphErrors.linearLink.from(), maxGraphErrors.linearLink.to()); UINFO("Max optimization linear error = %f m (link %d->%d, var=%f, ratio error/std=%f, thr=%f)", maxGraphErrors.linear, maxGraphErrors.linearLink.from(), maxGraphErrors.linearLink.to(), maxGraphErrors.linearLink.transVariance(), maxGraphErrors.linear/sqrt(maxGraphErrors.linearLink.transVariance()), _optimizationMaxError); if(_optimizationMaxError > 0.0f && maxGraphErrors.linearRatio > _optimizationMaxError) { UWARN("Rejecting localization (%d <-> %d) in this " "iteration because a wrong loop closure has been " "detected after graph optimization, resulting in " "a maximum graph error ratio of %f (edge %d->%d, type=%d, abs error=%f m, stddev=%f). The " "maximum error ratio parameter \"%s\" is %f of std deviation.", localizationLinks.rbegin()->second.from(), localizationLinks.rbegin()->second.to(), maxGraphErrors.linearRatio, maxGraphErrors.linearLink.from(), maxGraphErrors.linearLink.to(), maxGraphErrors.linearLink.type(), maxGraphErrors.linear, sqrt(maxGraphErrors.linearLink.transVariance()), Parameters::kRGBDOptimizeMaxError().c_str(), _optimizationMaxError); rejectLocalization = true; } else if(_optimizationMaxError == 0.0f && maxGraphErrors.linearRatio>100 && !_graphOptimizer->isRobust()) { UERROR("Huge optimization error detected!" "Linear error ratio of %f (edge %d->%d, type=%d, abs error=%f m, stddev=%f). You may consider " "enabling \"%s\" to reject those bad optimizations by setting it to a non null value!", maxGraphErrors.linearRatio, maxGraphErrors.linearLink.from(), maxGraphErrors.linearLink.to(), maxGraphErrors.linearLink.type(), maxGraphErrors.linear, sqrt(maxGraphErrors.linearLink.transVariance()), Parameters::kRGBDOptimizeMaxError().c_str()); } } if(maxGraphErrors.angularLink.isValid()) { maxGraphErrorsAngularIds = std::make_pair(maxGraphErrors.angularLink.from(), maxGraphErrors.angularLink.to()); UINFO("Max optimization angular error = %f deg (link %d->%d, var=%f, ratio error/std=%f, thr=%f)", maxGraphErrors.angular*180.0f/CV_PI, maxGraphErrors.angularLink.from(), maxGraphErrors.angularLink.to(), maxGraphErrors.angularLink.rotVariance(), maxGraphErrors.angular/sqrt(maxGraphErrors.angularLink.rotVariance()), _optimizationMaxError); if(_optimizationMaxError > 0.0f && maxGraphErrors.angularRatio > _optimizationMaxError) { UWARN("Rejecting localization (%d <-> %d) in this " "iteration because a wrong loop closure has been " "detected after graph optimization, resulting in " "a maximum graph error ratio of %f (edge %d->%d, type=%d, abs error=%f deg, stddev=%f). The " "maximum error ratio parameter \"%s\" is %f of std deviation.", localizationLinks.rbegin()->second.from(), localizationLinks.rbegin()->second.to(), maxGraphErrors.angularRatio, maxGraphErrors.angularLink.from(), maxGraphErrors.angularLink.to(), maxGraphErrors.angularLink.type(), maxGraphErrors.angular*180.0f/CV_PI, sqrt(maxGraphErrors.angularLink.rotVariance()), Parameters::kRGBDOptimizeMaxError().c_str(), _optimizationMaxError); rejectLocalization = true; } else if(_optimizationMaxError == 0.0f && maxGraphErrors.angularRatio>100 && !_graphOptimizer->isRobust()) { UERROR("Huge optimization error detected!" "Angular error ratio of %f (edge %d->%d, type=%d, abs error=%f m, stddev=%f). You may consider " "enabling \"%s\" to reject those bad optimizations by setting it to a non null value!", maxGraphErrors.angularRatio, maxGraphErrors.angularLink.from(), maxGraphErrors.angularLink.to(), maxGraphErrors.angularLink.type(), maxGraphErrors.angular*180.0f/CV_PI, sqrt(maxGraphErrors.angularLink.rotVariance()), Parameters::kRGBDOptimizeMaxError().c_str()); } } } } } if(!rejectLocalization) { if(hasGlobalLoopClosuresOrLandmarks) { // We successfully optimize the graph without local loop closures, // clear them as some of them may be wrong. size_t before = _odomCacheConstraints.size(); _odomCacheConstraints = graph::filterLinks(_odomCacheConstraints, Link::kLocalSpaceClosure); if(before != _odomCacheConstraints.size()) { UWARN("Successfully optimized without local loop closures! Clearing them from local odometry cache. %ld/%ld have been removed.", before - _odomCacheConstraints.size(), before); } else { UWARN("Successfully optimized without local loop closures!"); } odomCacheProximityLinksCleared = before - _odomCacheConstraints.size(); } else if(!removedLinks.empty()) { // If we removed links (repaired the graph) for(auto link: removedLinks) { UWARN("Removing link %d->%d from odometry cache", link.first, link.second); auto iter = graph::findLink(_odomCacheConstraints, link.first, link.second); if(iter!=_odomCacheConstraints.end()) { _odomCacheConstraints.erase(iter); } } UWARN("Successfully repaired the graph."); maxGraphErrorsRemovedIds = removedLinks.front(); maxGraphErrorsRemovedCount = removedLinks.size(); } // Count how many localization links are in the constraints bool hadAlreadyLocalizationLinks = false; for(std::multimap::iterator iter=_odomCacheConstraints.begin(); iter!=_odomCacheConstraints.end(); ++iter) { if(iter->second.type() == Link::kGlobalClosure || iter->second.type() == Link::kLocalSpaceClosure || iter->second.type() == Link::kLocalTimeClosure || iter->second.type() == Link::kUserClosure || iter->second.type() == Link::kNeighborMerged || iter->second.type() == Link::kLandmark) { hadAlreadyLocalizationLinks = true; break; } } // update localization links UASSERT(uContains(optPoses, signature->id())); Transform newOptPoseInv = optPoses.at(signature->id()).inverse(); for(std::multimap::iterator iter=localizationLinks.begin(); iter!=localizationLinks.end(); ++iter) { if(!_localizationSmoothing) { // Add original link without optimization UDEBUG("Adding new odom cache constraint %d->%d (%s)", iter->second.from(), iter->second.to(), iter->second.transform().prettyPrint().c_str()); } else { // Adjust with optimized poses, this will smooth the localization UASSERT(uContains(optPoses, iter->first)); Transform newT = newOptPoseInv * optPoses.at(iter->first); UDEBUG("Adjusted localization link %d->%d after optimization", iter->second.from(), iter->second.to()); UDEBUG("from %s", iter->second.transform().prettyPrint().c_str()); UDEBUG(" to %s", newT.prettyPrint().c_str()); iter->second.setTransform(newT); // Update link in the referred signatures if(iter->first > 0) _memory->updateLink(iter->second, false); } _odomCacheConstraints.insert(std::make_pair(signature->id(), iter->second)); } _odomCacheConstraints.insert(selfLinks.begin(), selfLinks.end()); // At least 2 localizations at 2 different time required if(hadAlreadyLocalizationLinks || _maxOdomCacheSize == 0) { UINFO("Update localization"); // update odomCachePoses with optimized poses (but make sure to put them back in odom frame) Transform mapToOdomCache = signature->getPose() * newOptPoseInv; for(std::map::iterator iter = _odomCachePoses.begin(); iter!=_odomCachePoses.end(); ++iter) { iter->second = mapToOdomCache * optPoses.at(iter->first); } if(_optimizeFromGraphEnd) { // update all previous nodes // Normally _mapCorrection should be identity, but if _optimizeFromGraphEnd // parameters just changed state, we should put back all poses without map correction. Transform oldPose = _optimizedPoses.at(localizationLinks.rbegin()->first); Transform mapCorrectionInv = _mapCorrection.inverse(); Transform u = signature->getPose() * localizationLinks.rbegin()->second.transform(); if(_graphOptimizer->gravitySigma() > 0) { // Adjust transform with gravity Transform transform = localizationLinks.rbegin()->second.transform(); int loopId = localizationLinks.rbegin()->first; int landmarkId = 0; if(loopId<0) { //For landmarks, use transform against other node looking the landmark // (because we don't assume that landmarks are aligned with gravity) landmarkId = loopId; UASSERT(landmarksDetected.find(landmarkId) != landmarksDetected.end() && !landmarksDetected.at(landmarkId).empty()); loopId = *landmarksDetected.at(landmarkId).begin(); } const Signature * loopS = _memory->getSignature(loopId); UASSERT(loopS !=0); std::multimap::const_iterator iterGravityLoop = graph::findLink(loopS->getLinks(), loopS->id(), loopS->id(), false, Link::kGravity); std::multimap::const_iterator iterGravitySign = graph::findLink(signature->getLinks(), signature->id(), signature->id(), false, Link::kGravity); if(iterGravityLoop!=loopS->getLinks().end() && iterGravitySign!=signature->getLinks().end()) { float roll,pitch,yaw; if(landmarkId < 0) { iterGravityLoop->second.transform().getEulerAngles(roll, pitch, yaw); Transform gravityCorr = Transform(_optimizedPoses.at(loopS->id()).x(), _optimizedPoses.at(loopS->id()).y(), _optimizedPoses.at(loopS->id()).z(), roll, pitch, _optimizedPoses.at(loopS->id()).theta()) * _optimizedPoses.at(loopS->id()).inverse(); (gravityCorr * _optimizedPoses.at(landmarkId)).getEulerAngles(roll,pitch,yaw); } else { iterGravityLoop->second.transform().getEulerAngles(roll, pitch, yaw); } Transform targetRotation = iterGravitySign->second.transform().rotation()*transform.rotation(); targetRotation = Transform(0,0,0,roll,pitch,targetRotation.theta()); Transform error = transform.rotation().inverse() * iterGravitySign->second.transform().rotation().inverse() * targetRotation; transform *= error; u = signature->getPose() * transform; } else if(iterGravityLoop!=loopS->getLinks().end() || iterGravitySign!=signature->getLinks().end()) { UWARN("Gravity link not found for %d or %d, localization won't be corrected with gravity.", loopId, signature->id()); } } Transform up = u * oldPose.inverse(); if(_graphOptimizer->isSlam2d()) { // in case of 3d landmarks, transform constraint to 2D up.to3DoF(); } for(std::map::iterator iter=_optimizedPoses.begin(); iter!=_optimizedPoses.end(); ++iter) { iter->second = mapCorrectionInv * up * iter->second; } _optimizedPoses.at(signature->id()) = signature->getPose(); } else { Transform newPose = _optimizedPoses.at(localizationLinks.rbegin()->first) * localizationLinks.rbegin()->second.transform().inverse(); UDEBUG("newPose=%s", newPose.prettyPrint().c_str()); if(_graphOptimizer->isSlam2d() && signature->getPose().is3DoF()) { // in case of 3d landmarks, transform constraint to 2D newPose = newPose.to3DoF(); UDEBUG("newPose 2D=%s", newPose.prettyPrint().c_str()); } else if(_graphOptimizer->gravitySigma() > 0) { // Adjust transform with gravity std::multimap::const_iterator iterGravitySign = graph::findLink(signature->getLinks(), signature->id(), signature->id(), false, Link::kGravity); if(iterGravitySign!=signature->getLinks().end()) { Transform transform = localizationLinks.rbegin()->second.transform(); float roll,pitch,yaw; float tmp1,tmp2; UDEBUG("Gravity link = %s", iterGravitySign->second.transform().prettyPrint().c_str()); _optimizedPoses.at(localizationLinks.rbegin()->first).getEulerAngles(roll, pitch, yaw); Transform targetRotation = iterGravitySign->second.transform().rotation()*transform.rotation(); targetRotation = Transform(0,0,0,roll,pitch,targetRotation.theta()); Transform error = transform.rotation().inverse() * iterGravitySign->second.transform().rotation().inverse() * targetRotation; transform *= error; newPose = _optimizedPoses.at(localizationLinks.rbegin()->first) * transform.inverse(); iterGravitySign->second.transform().getEulerAngles(roll, pitch, tmp1); newPose.getEulerAngles(tmp1, tmp2, yaw); newPose = Transform(newPose.x(), newPose.y(), newPose.z(), roll, pitch, yaw); UDEBUG("newPose gravity=%s", newPose.prettyPrint().c_str()); } else if(iterGravitySign!=signature->getLinks().end()) { UWARN("Gravity link not found for %d, localization won't be corrected with gravity.", signature->id()); } } _optimizedPoses.at(signature->id()) = newPose; } _localizationCovariance = locOptCovariance.empty()?localizationLinks.rbegin()->second.infMatrix().inv():locOptCovariance; } else //delayed localization (wait for more than 1 link) { UWARN("Localization was good, but waiting for another one to be more accurate (%s>0)", Parameters::kRGBDMaxOdomCacheSize().c_str()); delayedLocalization = true; rejectLocalization = true; } } } if(rejectLocalization) { _loopClosureHypothesis.first = 0; lastProximitySpaceClosureId = 0; rejectedLoopClosure = true; } } else { UINFO("Update map correction"); std::map poses = _optimizedPoses; // if _optimizeFromGraphEnd parameter just changed state, don't use optimized poses as guess if(_optimizeFromGraphEndChanged) { UWARN("Optimization: clearing guess poses as %s has changed state, now %s", Parameters::kRGBDOptimizeFromGraphEnd().c_str(), _optimizeFromGraphEnd?"true":"false"); poses.clear(); _optimizeFromGraphEndChanged = false; } std::multimap constraints; cv::Mat covariance; optimizeCurrentMap(signature->id(), false, poses, covariance, &constraints, &optimizationError, &optimizationIterations); // Check added loop closures have broken the graph // (in case of wrong loop closures). bool updateConstraints = true; if(poses.empty()) { UWARN("Graph optimization failed! Rejecting last loop closures added."); for(std::list >::iterator iter=loopClosureLinksAdded.begin(); iter!=loopClosureLinksAdded.end(); ++iter) { _memory->removeLink(iter->first, iter->second); UWARN("Loop closure %d->%d rejected!", iter->first, iter->second); } updateConstraints = false; _loopClosureHypothesis.first = 0; lastProximitySpaceClosureId = 0; rejectedLoopClosure = true; } else if(_memory->isIncremental() && loopClosureLinksAdded.size() && optimizationIterations > 0 && constraints.size()) { UINFO("Compute max graph errors..."); maxGraphErrors = graph::computeMaxGraphErrors( poses, constraints); if(!maxGraphErrors.linearLink.isValid() && !maxGraphErrors.angularLink.isValid()) { UWARN("Could not compute graph errors! Wrong loop closures could be accepted!"); } bool reject = false; if(maxGraphErrors.linearLink.isValid()) { maxGraphErrorsLinearIds = std::make_pair(maxGraphErrors.linearLink.from(), maxGraphErrors.linearLink.to()); UINFO("Max optimization linear error = %f m (link %d->%d, var=%f, ratio error/std=%f)", maxGraphErrors.linear, maxGraphErrors.linearLink.from(), maxGraphErrors.linearLink.to(), maxGraphErrors.linearLink.transVariance(), maxGraphErrors.linear/sqrt(maxGraphErrors.linearLink.transVariance())); if(_optimizationMaxError > 0.0f && maxGraphErrors.linearRatio > _optimizationMaxError) { if( _optimizationMaxErrorRepairRadius > 0.0 && maxGraphErrorsLinearIds == _lastRejectedLoopClosureIds && graph::findLink(constraints, maxGraphErrorsLinearIds.first, maxGraphErrorsLinearIds.second) != constraints.end()) { UWARN("We detected 2 consecutive loop closure rejections because of the same loop closure link (%d->%d), trying optimization again without that link...", maxGraphErrorsLinearIds.first, maxGraphErrorsLinearIds.second); std::list > removedLinks = repairGraph( maxGraphErrors, poses, constraints, optimizationError, optimizationIterations, covariance); if(removedLinks.empty()) { UWARN("Optimization failed when trying to repair the graph."); reject = true; } else { for(auto link: removedLinks) { UWARN("Removing link %d->%d from memory", link.first, link.second); _memory->removeLink(link.first, link.second); } UWARN("Successfully repaired the graph."); maxGraphErrorsRemovedIds = removedLinks.front(); maxGraphErrorsRemovedCount = removedLinks.size(); } } else { reject = true; } if(reject) { UWARN("Rejecting all added loop closures (%d, first is %d <-> %d) in this " "iteration because a wrong loop closure has been " "detected after graph optimization, resulting in " "a maximum graph error ratio of %f (edge %d->%d, type=%d, abs error=%f m, stddev=%f). The " "maximum error ratio parameter \"%s\" is %f of std deviation.", (int)loopClosureLinksAdded.size(), loopClosureLinksAdded.front().first, loopClosureLinksAdded.front().second, maxGraphErrors.linearRatio, maxGraphErrors.linearLink.from(), maxGraphErrors.linearLink.to(), maxGraphErrors.linearLink.type(), maxGraphErrors.linear, sqrt(maxGraphErrors.linearLink.transVariance()), Parameters::kRGBDOptimizeMaxError().c_str(), _optimizationMaxError); if(maxGraphErrors.linearLink.type() != Link::kNeighbor) { _lastRejectedLoopClosureIds = maxGraphErrorsLinearIds; } } } else if(_optimizationMaxError == 0.0f && maxGraphErrors.linearRatio>100 && !_graphOptimizer->isRobust()) { UERROR("Huge optimization error detected!" "Linear error ratio of %f (edge %d->%d, type=%d, abs error=%f m, stddev=%f). You may consider " "enabling \"%s\" to reject those bad optimizations by setting it to a non null value!", maxGraphErrors.linearRatio, maxGraphErrors.linearLink.from(), maxGraphErrors.linearLink.to(), maxGraphErrors.linearLink.type(), maxGraphErrors.linear, sqrt(maxGraphErrors.linearLink.transVariance()), Parameters::kRGBDOptimizeMaxError().c_str()); } } if(maxGraphErrors.angularLink.isValid()) { maxGraphErrorsAngularIds = std::make_pair(maxGraphErrors.angularLink.from(), maxGraphErrors.angularLink.to()); UINFO("Max optimization angular error = %f deg (link %d->%d, var=%f, ratio error/std=%f)", maxGraphErrors.angular*180.0f/CV_PI, maxGraphErrors.angularLink.from(), maxGraphErrors.angularLink.to(), maxGraphErrors.angularLink.rotVariance(), maxGraphErrors.angular/sqrt(maxGraphErrors.angularLink.rotVariance())); if(_optimizationMaxError > 0.0f && maxGraphErrors.angularRatio > _optimizationMaxError) { UWARN("Rejecting all added loop closures (%d, first is %d <-> %d) in this " "iteration because a wrong loop closure has been " "detected after graph optimization, resulting in " "a maximum graph error ratio of %f (edge %d->%d, type=%d, abs error=%f deg, stddev=%f). The " "maximum error ratio parameter \"%s\" is %f of std deviation.", (int)loopClosureLinksAdded.size(), loopClosureLinksAdded.front().first, loopClosureLinksAdded.front().second, maxGraphErrors.angularRatio, maxGraphErrors.angularLink.from(), maxGraphErrors.angularLink.to(), maxGraphErrors.angularLink.type(), maxGraphErrors.angular*180.0f/CV_PI, sqrt(maxGraphErrors.angularLink.rotVariance()), Parameters::kRGBDOptimizeMaxError().c_str(), _optimizationMaxError); reject = true; } else if(_optimizationMaxError == 0.0f && maxGraphErrors.angularRatio>100 && !_graphOptimizer->isRobust()) { UERROR("Huge optimization error detected!" "Angular error ratio of %f (edge %d->%d, type=%d, abs error=%f m, stddev=%f). You may consider " "enabling \"%s\" to reject those bad optimizations by setting it to a non null value!", maxGraphErrors.angularRatio, maxGraphErrors.angularLink.from(), maxGraphErrors.angularLink.to(), maxGraphErrors.angularLink.type(), maxGraphErrors.angular*180.0f/CV_PI, sqrt(maxGraphErrors.angularLink.rotVariance()), Parameters::kRGBDOptimizeMaxError().c_str()); } } if(reject) { for(std::list >::iterator iter=loopClosureLinksAdded.begin(); iter!=loopClosureLinksAdded.end(); ++iter) { _memory->removeLink(iter->first, iter->second); UWARN("Loop closure %d->%d rejected!", iter->first, iter->second); } updateConstraints = false; _loopClosureHypothesis.first = 0; lastProximitySpaceClosureId = 0; rejectedLoopClosure = true; } } if(updateConstraints) { UINFO("Updated local map (old size=%d, new size=%d)", (int)_optimizedPoses.size(), (int)poses.size()); _optimizedPoses = poses; _constraints = constraints; _localizationCovariance = covariance; } } // Update map correction, it should be identify when optimizing from the last node UASSERT(_optimizedPoses.find(signature->id()) != _optimizedPoses.end()); if(fakeOdom && _mapCorrectionBackup.isNull()) { _mapCorrectionBackup = _mapCorrection; } previousMapCorrection = _mapCorrection; _mapCorrection = _optimizedPoses.at(signature->id()) * signature->getPose().inverse(); // Update statistics about the closest node in the graph using the actual loop closure if(!_memory->isIncremental() && !_lastLocalizationPose.isNull()) { int closestNode = _loopClosureHypothesis.first>0?_loopClosureHypothesis.first:lastProximitySpaceClosureId; if(closestNode>0) { distanceToClosestNodeInTheGraph = _lastLocalizationPose.getDistance(_optimizedPoses.at(closestNode)); UDEBUG("Last localization pose = %s, updated closest node=%d (%f m)", _lastLocalizationPose.prettyPrint().c_str(), closestNode, distanceToClosestNodeInTheGraph); angleToClosestNodeInTheGraph = _lastLocalizationPose.getAngle(_optimizedPoses.at(closestNode)); } } _lastLocalizationPose = _optimizedPoses.at(signature->id()); // update if(_mapCorrection.getNormSquared() > 0.1f && _optimizeFromGraphEnd) { bool hasPrior = signature->hasLink(signature->id()); if(!_graphOptimizer->priorsIgnored()) { for(std::multimap::reverse_iterator iter=_constraints.rbegin(); !hasPrior && iter!=_constraints.rend(); ++iter) { if(iter->second.type() == Link::kPosePrior) { hasPrior = true; } } } if((!hasPrior || _graphOptimizer->priorsIgnored()) && _graphOptimizer->gravitySigma()==0.0f) { UERROR("Map correction should be identity when optimizing from the last node. T=%s", _mapCorrection.prettyPrint().c_str()); } } } int newLocId = _loopClosureHypothesis.first>0?_loopClosureHypothesis.first:lastProximitySpaceClosureId>0?lastProximitySpaceClosureId:0; _lastLocalizationNodeId = newLocId!=0?newLocId:_lastLocalizationNodeId; if(newLocId==0 && !landmarksDetected.empty()) { std::map >::const_iterator iter = _memory->getLandmarksIndex().find(landmarksDetected.begin()->first); if(iter!=_memory->getLandmarksIndex().end()) { if(iter->second.size() && *iter->second.begin()!=signature->id()) { _lastLocalizationNodeId = *iter->second.begin(); } } } timeMapOptimization = timer.ticks(); ULOGGER_INFO("timeMapOptimization=%fs", timeMapOptimization); //============================================================ // Prepare statistics //============================================================ // Data used for the statistics event and for the log files int dictionarySize = 0; int refWordsCount = 0; int refUniqueWordsCount = 0; int lcHypothesisReactivated = 0; float rehearsalValue = uValue(statistics_.data(), Statistics::kMemoryRehearsal_sim(), 0.0f); int rehearsalMaxId = (int)uValue(statistics_.data(), Statistics::kMemoryRehearsal_merged(), 0.0f); sLoop = _memory->getSignature(_loopClosureHypothesis.first?_loopClosureHypothesis.first:lastProximitySpaceClosureId?lastProximitySpaceClosureId:_highestHypothesis.first); if(sLoop) { lcHypothesisReactivated = sLoop->isSaved()?1.0f:0.0f; } dictionarySize = (int)_memory->getVWDictionary()->getVisualWords().size(); refWordsCount = (int)signature->getWords().size(); refUniqueWordsCount = (int)uUniqueKeys(signature->getWords()).size(); if(_graphOptimizer->isSlam2d() && _localizationCovariance.total() == 36) { // set very small _localizationCovariance.at(2,2) = Registration::COVARIANCE_LINEAR_EPSILON; _localizationCovariance.at(3,3) = Registration::COVARIANCE_ANGULAR_EPSILON; _localizationCovariance.at(4,4) = Registration::COVARIANCE_ANGULAR_EPSILON; } // Posterior is empty if a bad signature is detected // The virtual place is the first location of the posterior when it is one of them. const std::vector & vpIds = _bayesFilter->getPosteriorIds(); float vpHypothesis = (vpIds.size() && vpIds[0]==Memory::kIdVirtual)?_bayesFilter->getPosteriorValues()[0]:0.0f; int loopId = _loopClosureHypothesis.first>0?_loopClosureHypothesis.first:lastProximitySpaceClosureId; // prepare statistics if(_loopClosureHypothesis.first || _publishStats) { ULOGGER_INFO("sending stats..."); statistics_.setRefImageId(_memory->getLastSignatureId()); // Use last id from Memory (in case of rehearsal) statistics_.setRefImageMapId(signature->mapId()); statistics_.setStamp(data.stamp()); if(_loopClosureHypothesis.first != Memory::kIdInvalid) { statistics_.setLoopClosureId(_loopClosureHypothesis.first); statistics_.setLoopClosureMapId(_memory->getMapId(_loopClosureHypothesis.first)); ULOGGER_INFO("Loop closure detected! With id=%d", _loopClosureHypothesis.first); } if(_publishStats) { ULOGGER_INFO("send all stats..."); statistics_.setExtended(1); statistics_.addStatistic(Statistics::kLoopAccepted_hypothesis_id(), _loopClosureHypothesis.first); statistics_.addStatistic(Statistics::kLoopSuppressed_hypothesis_id(), loopIdSuppressedByProximity); statistics_.addStatistic(Statistics::kLoopHighest_hypothesis_id(), _highestHypothesis.first); statistics_.addStatistic(Statistics::kLoopHighest_hypothesis_value(), _highestHypothesis.second); statistics_.addStatistic(Statistics::kLoopHypothesis_reactivated(), lcHypothesisReactivated); statistics_.addStatistic(Statistics::kLoopVp_hypothesis(), vpHypothesis); statistics_.addStatistic(Statistics::kLoopReactivate_id(), retrievalId); statistics_.addStatistic(Statistics::kLoopHypothesis_ratio(), hypothesisRatio); statistics_.addStatistic(Statistics::kLoopVisual_inliers(), loopClosureVisualInliers); statistics_.addStatistic(Statistics::kLoopVisual_inliers_ratio(), loopClosureVisualInliersRatio); statistics_.addStatistic(Statistics::kLoopVisual_matches(), loopClosureVisualMatches); statistics_.addStatistic(Statistics::kLoopVisual_variance(), loopClosureVisualVariance); statistics_.addStatistic(Statistics::kLoopLinear_variance(), loopClosureLinearVariance); statistics_.addStatistic(Statistics::kLoopAngular_variance(), loopClosureAngularVariance); statistics_.addStatistic(Statistics::kLoopLast_id(), _memory->getLastGlobalLoopClosureId()); if(maxGraphErrors.linear>=0 || maxGraphErrors.angular>=0) { statistics_.addStatistic(Statistics::kLoopOptimization_max_error(), maxGraphErrors.linear); statistics_.addStatistic(Statistics::kLoopOptimization_max_error_ratio(), maxGraphErrors.linearRatio); statistics_.addStatistic(Statistics::kLoopOptimization_max_ang_error(), maxGraphErrors.angular*180.0f/M_PI); statistics_.addStatistic(Statistics::kLoopOptimization_max_ang_error_ratio(), maxGraphErrors.angularRatio); statistics_.addStatistic(Statistics::kLoopOptimization_max_error_from_id(), maxGraphErrorsLinearIds.first); statistics_.addStatistic(Statistics::kLoopOptimization_max_error_to_id(), maxGraphErrorsLinearIds.second); statistics_.addStatistic(Statistics::kLoopOptimization_max_ang_error_from_id(), maxGraphErrorsAngularIds.first); statistics_.addStatistic(Statistics::kLoopOptimization_max_ang_error_to_id(), maxGraphErrorsAngularIds.second); if(_optimizationMaxErrorRepairRadius > 0) { statistics_.addStatistic(Statistics::kLoopOptimization_max_error_removed_from_id(), maxGraphErrorsRemovedIds.first); statistics_.addStatistic(Statistics::kLoopOptimization_max_error_removed_to_id(), maxGraphErrorsRemovedIds.second); statistics_.addStatistic(Statistics::kLoopOptimization_max_error_removed_count(), maxGraphErrorsRemovedCount); } } statistics_.addStatistic(Statistics::kLoopOptimization_error(), optimizationError); statistics_.addStatistic(Statistics::kLoopOptimization_iterations(), optimizationIterations); statistics_.addStatistic(Statistics::kLoopLandmark_detected(), landmarksDetected.empty()?0:-landmarksDetected.begin()->first); statistics_.addStatistic(Statistics::kLoopLandmark_detected_node_ref(), landmarksDetected.empty() || landmarksDetected.begin()->second.empty()?0:*landmarksDetected.begin()->second.begin()); statistics_.addStatistic(Statistics::kLoopVisual_inliers_mean_dist(), loopClosureVisualInliersMeanDist); statistics_.addStatistic(Statistics::kLoopVisual_inliers_distribution(), loopClosureVisualInliersDistribution); statistics_.addStatistic(Statistics::kProximityTime_detections(), proximityDetectionsInTimeFound); statistics_.addStatistic(Statistics::kProximitySpace_detections_added_visually(), proximityDetectionsAddedVisually); statistics_.addStatistic(Statistics::kProximitySpace_detections_added_icp_multi(), proximityDetectionsAddedByICPMulti); statistics_.addStatistic(Statistics::kProximitySpace_detections_added_icp_global(), proximityDetectionsAddedByICPGlobal); statistics_.addStatistic(Statistics::kProximitySpace_paths(), proximitySpacePaths); statistics_.addStatistic(Statistics::kProximitySpace_visual_paths_checked(), localVisualPathsChecked); statistics_.addStatistic(Statistics::kProximitySpace_scan_paths_checked(), localScanPathsChecked); statistics_.addStatistic(Statistics::kProximitySpace_last_detection_id(), lastProximitySpaceClosureId); statistics_.setProximityDetectionId(lastProximitySpaceClosureId); statistics_.setProximityDetectionMapId(_memory->getMapId(lastProximitySpaceClosureId)); statistics_.addStatistic(Statistics::kLoopId(), loopId); statistics_.addStatistic(Statistics::kLoopMap_id(), (loopId>0 && sLoop)?sLoop->mapId():-1); statistics_.addStatistic(Statistics::kLoopDistance_since_last_loc(), _distanceTravelledSinceLastLocalization); float x,y,z,roll,pitch,yaw; if(_loopClosureHypothesis.first || lastProximitySpaceClosureId || (!rejectedLoopClosure && !landmarksDetected.empty())) { if(_loopClosureHypothesis.first || lastProximitySpaceClosureId) { // Loop closure transform UASSERT(sLoop); std::multimap::const_iterator loopIter = sLoop->getLinks().find(signature->id()); UASSERT(loopIter!=sLoop->getLinks().end()); UINFO("Set loop closure transform = %s", loopIter->second.transform().prettyPrint().c_str()); statistics_.setLoopClosureTransform(loopIter->second.transform()); statistics_.addStatistic(Statistics::kLoopVisual_words(), sLoop->getWords().size()); // if ground truth exists, compute localization error if(!sLoop->getGroundTruthPose().isNull() && !signature->getGroundTruthPose().isNull()) { Transform transformGT = sLoop->getGroundTruthPose().inverse() * signature->getGroundTruthPose(); statistics_.addStatistic(Statistics::kGtLocalization_linear_error(), loopIter->second.transform().getDistance(transformGT)); statistics_.addStatistic(Statistics::kGtLocalization_angular_error(), loopIter->second.transform().getAngle(transformGT)*180/M_PI); } } _distanceTravelledSinceLastLocalization = 0.0f; statistics_.addStatistic(Statistics::kLoopMapToOdom_norm(), _mapCorrection.getNorm()); statistics_.addStatistic(Statistics::kLoopMapToOdom_angle(), _mapCorrection.getAngle(Transform::getIdentity())*180.0f/M_PI); _mapCorrection.getTranslationAndEulerAngles(x, y, z, roll, pitch, yaw); statistics_.addStatistic(Statistics::kLoopMapToOdom_x(), x); statistics_.addStatistic(Statistics::kLoopMapToOdom_y(), y); statistics_.addStatistic(Statistics::kLoopMapToOdom_z(), z); statistics_.addStatistic(Statistics::kLoopMapToOdom_roll(), roll*180.0f/M_PI); statistics_.addStatistic(Statistics::kLoopMapToOdom_pitch(), pitch*180.0f/M_PI); statistics_.addStatistic(Statistics::kLoopMapToOdom_yaw(), yaw*180.0f/M_PI); // Odom correction (actual odometry pose change), ignore correction from first localization if(!odomPose.isNull() && !previousMapCorrection.isNull() && !previousMapCorrection.isIdentity()) { Transform odomCorrection = (previousMapCorrection*odomPose).inverse()*_mapCorrection*odomPose; statistics_.addStatistic(Statistics::kLoopOdom_correction_norm(), odomCorrection.getNorm()); statistics_.addStatistic(Statistics::kLoopOdom_correction_angle(), odomCorrection.getAngle(Transform::getIdentity())*180.0f/M_PI); odomCorrection.getTranslationAndEulerAngles(x, y, z, roll, pitch, yaw); statistics_.addStatistic(Statistics::kLoopOdom_correction_x(), x); statistics_.addStatistic(Statistics::kLoopOdom_correction_y(), y); statistics_.addStatistic(Statistics::kLoopOdom_correction_z(), z); statistics_.addStatistic(Statistics::kLoopOdom_correction_roll(), roll*180.0f/M_PI); statistics_.addStatistic(Statistics::kLoopOdom_correction_pitch(), pitch*180.0f/M_PI); statistics_.addStatistic(Statistics::kLoopOdom_correction_yaw(), yaw*180.0f/M_PI); } } if(!_lastLocalizationPose.isNull() && !_lastLocalizationPose.isIdentity()) { _lastLocalizationPose.getTranslationAndEulerAngles(x, y, z, roll, pitch, yaw); statistics_.addStatistic(Statistics::kLoopMapToBase_x(), x); statistics_.addStatistic(Statistics::kLoopMapToBase_y(), y); statistics_.addStatistic(Statistics::kLoopMapToBase_z(), z); statistics_.addStatistic(Statistics::kLoopMapToBase_roll(), roll*180.0f/M_PI); statistics_.addStatistic(Statistics::kLoopMapToBase_pitch(), pitch*180.0f/M_PI); statistics_.addStatistic(Statistics::kLoopMapToBase_yaw(), yaw*180.0f/M_PI); UINFO("Localization pose = %s", _lastLocalizationPose.prettyPrint().c_str()); if(_localizationSecondTryWithoutProximityLinks) { statistics_.addStatistic(Statistics::kLoopProximity_links_cleared(), (float)odomCacheProximityLinksCleared); } if(_localizationCovariance.total()==36) { double varLin = _graphOptimizer->isSlam2d()? std::max(_localizationCovariance.at(0,0), _localizationCovariance.at(1,1)): uMax3(_localizationCovariance.at(0,0), _localizationCovariance.at(1,1), _localizationCovariance.at(2,2)); statistics_.addStatistic(Statistics::kLoopMapToBase_lin_std(), sqrt(varLin)); statistics_.addStatistic(Statistics::kLoopMapToBase_lin_var(), varLin); } } statistics_.setMapCorrection(_mapCorrection); UINFO("Set map correction = %s", _mapCorrection.prettyPrint().c_str()); statistics_.setLocalizationCovariance(_localizationCovariance); // timings... statistics_.addStatistic(Statistics::kTimingMemory_update(), timeMemoryUpdate*1000); statistics_.addStatistic(Statistics::kTimingNeighbor_link_refining(), timeNeighborLinkRefining*1000); statistics_.addStatistic(Statistics::kTimingProximity_by_time(), timeProximityByTimeDetection*1000); statistics_.addStatistic(Statistics::kTimingProximity_by_space_search(), timeProximityBySpaceSearch*1000); statistics_.addStatistic(Statistics::kTimingProximity_by_space_visual(), timeProximityBySpaceVisualDetection*1000); statistics_.addStatistic(Statistics::kTimingProximity_by_space(), timeProximityBySpaceDetection*1000); statistics_.addStatistic(Statistics::kTimingReactivation(), timeReactivations*1000); statistics_.addStatistic(Statistics::kTimingAdd_loop_closure_link(), timeAddLoopClosureLink*1000); statistics_.addStatistic(Statistics::kTimingMap_optimization(), timeMapOptimization*1000); statistics_.addStatistic(Statistics::kTimingLikelihood_computation(), timeLikelihoodCalculation*1000); statistics_.addStatistic(Statistics::kTimingPosterior_computation(), timePosteriorCalculation*1000); statistics_.addStatistic(Statistics::kTimingHypotheses_creation(), timeHypothesesCreation*1000); statistics_.addStatistic(Statistics::kTimingHypotheses_validation(), timeHypothesesValidation*1000); statistics_.addStatistic(Statistics::kTimingCleaning_neighbors(), timeCleaningNeighbors*1000); // retrieval statistics_.addStatistic(Statistics::kMemorySignatures_retrieved(), (float)signaturesRetrieved.size()); // Feature specific parameters statistics_.addStatistic(Statistics::kKeypointDictionary_size(), dictionarySize); statistics_.addStatistic(Statistics::kKeypointCurrent_frame(), refWordsCount); statistics_.addStatistic(Statistics::kKeypointIndexed_words(), _memory->getVWDictionary()->getIndexedWordsCount()); statistics_.addStatistic(Statistics::kKeypointIndex_memory_usage(), _memory->getVWDictionary()->getIndexMemoryUsed()); //Epipolar geometry constraint statistics_.addStatistic(Statistics::kLoopRejectedHypothesis(), rejectedLoopClosure?1.0f:0); statistics_.addStatistic(Statistics::kMemorySmall_movement(), smallDisplacement?1.0f:0); statistics_.addStatistic(Statistics::kMemoryDistance_travelled(), _distanceTravelled); statistics_.addStatistic(Statistics::kMemoryFast_movement(), tooFastMovement?1.0f:0); statistics_.addStatistic(Statistics::kMemoryNew_landmark(), addedNewLandmark?1.0f:0); if(distanceToClosestNodeInTheGraph>0.0) { statistics_.addStatistic(Statistics::kMemoryClosest_node_distance(), distanceToClosestNodeInTheGraph); statistics_.addStatistic(Statistics::kMemoryClosest_node_angle(), angleToClosestNodeInTheGraph); } if(_publishRAMUsage) { UTimer ramTimer; statistics_.addStatistic(Statistics::kMemoryRAM_usage(), UProcessInfo::getMemoryUsage()/(1024*1024)); long estimatedMemoryUsage = sizeof(Rtabmap); estimatedMemoryUsage += _optimizedPoses.size() * (sizeof(int) + sizeof(Transform) + 12 * sizeof(float) + sizeof(std::map::iterator)) + sizeof(std::map); estimatedMemoryUsage += _constraints.size() * (sizeof(int) + sizeof(Transform) + 12 * sizeof(float) + sizeof(cv::Mat) + 36 * sizeof(double) + sizeof(std::map::iterator)) + sizeof(std::map); estimatedMemoryUsage += _memory->getMemoryUsed(); estimatedMemoryUsage += _bayesFilter->getMemoryUsed(); estimatedMemoryUsage += _parameters.size()*(sizeof(std::string)*2+sizeof(ParametersMap::iterator)) + sizeof(ParametersMap); statistics_.addStatistic(Statistics::kMemoryRAM_estimated(), (float)(estimatedMemoryUsage/(1024*1024)));//MB statistics_.addStatistic(Statistics::kTimingRAM_estimation(), ramTimer.ticks()*1000); } if(_publishLikelihood || _publishPdf) { // Child count by parent signature on the root of the memory ... for statistics statistics_.setWeights(weights); if(_publishPdf) { const std::vector & ids = _bayesFilter->getPosteriorIds(); const std::vector & values = _bayesFilter->getPosteriorValues(); std::map posterior; for(size_t i=0; igetAllLabels()); // Path if(_path.size()) { statistics_.setLocalPath(this->getPathNextNodes()); statistics_.setCurrentGoalId(this->getPathCurrentGoalId()); } } timeStatsCreation = timer.ticks(); ULOGGER_INFO("Time creating stats = %f...", timeStatsCreation); } Signature lastSignatureData = *signature; Transform lastSignatureLocalizedPose; if(_optimizedPoses.find(signature->id()) != _optimizedPoses.end()) { lastSignatureLocalizedPose = _optimizedPoses.at(signature->id()); } if(!_publishLastSignatureData) { // Keep the occupancy grid (compressed AND raw) on the published copy: // downstream consumers rely on it to generate global occupancy grid // in the same process (e.g. the raw occupancy grid used by the MainWindow // or ROS rtabmap_slam) or on an external process (the compressed occupancy // grid published over ROS rtabmap_msgs/MapData). lastSignatureData.sensorData().clearCompressedData(true, true, true, false); lastSignatureData.sensorData().clearRawData(true, true, true, false); } if(!_rawDataKept) { _memory->removeRawData(signature->id(), true, !_neighborLinkRefining && !_proximityBySpace, true, false); } // Localization mode and saving localization data: save odometry covariance in a prior link // so that DBReader can republish the covariance of localization data if(!_memory->isIncremental() && _memory->isLocalizationDataSaved() && !odomCovariance.empty()) { _memory->addLink(Link(signature->id(), signature->id(), Link::kPosePrior, odomPose, odomCovariance.inv())); } bool lastSignatureWasIntermediateNode = signature->getWeight() == -1; // remove last signature if the memory is not incremental or is a bad signature (if bad signatures are ignored) int signatureRemoved = _memory->cleanup(); if(signatureRemoved) { signaturesRemoved.push_back(signatureRemoved); } // If this option activated, add new nodes only if there are linked with a previous map. // Used when rtabmap is first started, it will wait a // global loop closure detection before starting the new map, // otherwise it deletes the current node. if(signatureRemoved != lastSignatureData.id()) { if(_startNewMapOnLoopClosure && _memory->isIncremental() && // only in mapping mode graph::filterLinks(signature->getLinks(), Link::kSelfRefLink).size() == 0 && // alone in the current map (landmarksDetected.empty() || rejectedLoopClosure) && // if we re not seeing a landmark from a previous map _memory->getWorkingMemSize(true)>0) // The working memory should not be empty { UWARN("Ignoring location %d because a global loop closure is required before starting a new map!", signature->id()); signaturesRemoved.push_back(signature->id()); _memory->deleteLocation(signature->id()); } else if(_startNewMapOnGoodSignature && (signature->getLandmarks().empty() && signature->isBadSignature()) && graph::filterLinks(signature->getLinks(), Link::kSelfRefLink).size() == 0) // alone in the current map { UWARN("Ignoring location %d because a good signature (with enough features or with a landmark detected) is required before starting a new map!", signature->id()); signaturesRemoved.push_back(signature->id()); _memory->deleteLocation(signature->id()); } else if((!_memory->isIncremental() || signature->getWeight()>=0) && (smallDisplacement || tooFastMovement) && _loopClosureHypothesis.first == 0 && lastProximitySpaceClosureId == 0 && (rejectedLoopClosure || landmarksDetected.empty()) && !addedNewLandmark) { // Don't delete the location if a loop closure is detected UINFO("Ignoring location %d because the displacement is too small! (d=%f a=%f)", signature->id(), _rgbdLinearUpdate, _rgbdAngularUpdate); // If there is a too small displacement, remove the node signaturesRemoved.push_back(signature->id()); _memory->deleteLocation(signature->id()); // Update odom cache (if we just switched from mapping mode to localization mode) _odomCachePoses.erase(signature->id()); for(std::multimap::iterator iter=_odomCacheConstraints.begin(); iter!=_odomCacheConstraints.end();) { if(iter->second.from() == signature->id() || iter->second.to() == signature->id()) { _odomCacheConstraints.erase(iter++); } else { ++iter; } } } else { _memory->saveLocationData(signature->id()); } } else if(!_memory->isIncremental() && (smallDisplacement || tooFastMovement) && _loopClosureHypothesis.first == 0 && lastProximitySpaceClosureId == 0 && !delayedLocalization) { _odomCachePoses.erase(signatureRemoved); for(std::multimap::iterator iter=_odomCacheConstraints.begin(); iter!=_odomCacheConstraints.end();) { if(iter->second.from() == signatureRemoved || iter->second.to() == signatureRemoved) { _odomCacheConstraints.erase(iter++); } else { ++iter; } } } // Pass this point signature should not be used, since it could have been transferred... signature = 0; timeMemoryCleanup = timer.ticks(); ULOGGER_INFO("timeMemoryCleanup = %fs... %d signatures removed", timeMemoryCleanup, (int)signaturesRemoved.size()); //============================================================ // TRANSFER //============================================================ // If time allowed for the detection exceeds the limit of // real-time, move the oldest signature with less frequency // entry (from X oldest) from the short term memory to the // long term memory. //============================================================ double totalTime = timerTotal.ticks(); ULOGGER_INFO("Total time processing = %fs...", totalTime); if(!lastSignatureWasIntermediateNode) // skip memory management on intermediate nodes { size_t workingMemSize = _memory->getWorkingMemSize(true); if((_maxTimeAllowed != 0 && totalTime*1000 > _maxTimeAllowed) || (_maxMemoryAllowed != 0 && workingMemSize > _maxMemoryAllowed)) { if(_maxTimeAllowed!=0 && totalTime*1000 > _maxTimeAllowed) { ULOGGER_INFO("Removing old signatures because time limit is reached %f ms > %f ms...", totalTime*1000, _maxTimeAllowed); } if(_maxMemoryAllowed != 0 && workingMemSize > _maxMemoryAllowed) { ULOGGER_INFO("Removing old signatures because memory limit is reached %d > %d...", (int)workingMemSize, _maxMemoryAllowed); } immunizedLocations.insert(_lastLocalizationNodeId); // keep the latest localization in working memory std::list transferred = _memory->forget(immunizedLocations); signaturesRemoved.insert(signaturesRemoved.end(), transferred.begin(), transferred.end()); if(!_someNodesHaveBeenTransferred && transferred.size()) { _someNodesHaveBeenTransferred = true; // only used to hide a warning on close nodes immunization } } } _lastProcessTime = totalTime; // cleanup cached gps values for(std::list::iterator iter=signaturesRemoved.begin(); iter!=signaturesRemoved.end() && _gpsGeocentricCache.size(); ++iter) { _gpsGeocentricCache.erase(*iter); } //Remove optimized poses from signatures transferred if(signaturesRemoved.size() && (_optimizedPoses.size() || _constraints.size())) { //refresh the local map because some transferred nodes may have broken the tree int id = 0; if(!_memory->isIncremental() && (_lastLocalizationNodeId > 0 || _path.size())) { if(_path.size()) { // priority on node on the path UASSERT(_pathCurrentIndex < _path.size()); UASSERT_MSG(uContains(_optimizedPoses, _path.at(_pathCurrentIndex).first), uFormat("id=%d", _path.at(_pathCurrentIndex).first).c_str()); id = _path.at(_pathCurrentIndex).first; UDEBUG("Refresh local map from %d", id); } else { if(uContains(_optimizedPoses, _lastLocalizationNodeId)) { id = _lastLocalizationNodeId; UDEBUG("Refresh local map from %d", id); } else { UDEBUG("Clearing _lastLocalizationNodeId(%d)", _lastLocalizationNodeId); _lastLocalizationNodeId = 0; } } } else if(_memory->isIncremental() && _optimizedPoses.size() && _memory->getLastWorkingSignature(true)) { id = _memory->getLastWorkingSignature(true)->id(); UDEBUG("Refresh local map from %d", id); } UDEBUG("id=%d _optimizedPoses=%d", id, (int)_optimizedPoses.size()); if(id > 0) { if(_lastLocalizationNodeId != 0) { _lastLocalizationNodeId = id; } UASSERT_MSG(_memory->getSignature(id) != 0, uFormat("id=%d", id).c_str()); if(signaturesRemoved.size() == 1 && signaturesRemoved.front() == lastSignatureData.id()) { int lastId = signaturesRemoved.front(); UDEBUG("Detected that only last signature has been removed (lastId=%d)", lastId); _optimizedPoses.erase(lastId); for(std::multimap::iterator iter=_constraints.lower_bound(lastId); iter!=_constraints.end() && iter->first==lastId;++iter) { if(iter->second.to() != iter->second.from()) { std::multimap::iterator jter = graph::findLink(_constraints, iter->second.to(), iter->second.from(), false); if(jter != _constraints.end()) { _constraints.erase(jter); } } } _constraints.erase(lastId); } else { std::map ids = _memory->getNeighborsId(id, 0, 0, true); for(std::map::iterator iter=_optimizedPoses.begin(); iter!=_optimizedPoses.end();) { if(iter->first > 0 && !uContains(ids, iter->first)) { UDEBUG("Removed %d from local map", iter->first); UASSERT(iter->first != _lastLocalizationNodeId); _optimizedPoses.erase(iter++); if(!_globalScanMap.empty()) { UWARN("optimized poses have been modified, clearing global scan map..."); _globalScanMap.clear(); _globalScanMapPoses.clear(); } } else { ++iter; } } for(std::multimap::iterator iter=_constraints.begin(); iter!=_constraints.end();) { if(iter->first > 0 && (!uContains(ids, iter->second.from()) || !uContains(ids, iter->second.to()))) { _constraints.erase(iter++); } else { ++iter; } } } } else { if(!_optimizedPoses.empty()) UDEBUG("Optimized poses cleared!"); _optimizedPoses.clear(); _constraints.clear(); } } // just some verifications to make sure that planning path is still in the local map! if(_path.size()) { UASSERT(_pathCurrentIndex < _path.size()); UASSERT(_pathGoalIndex < _path.size()); UASSERT_MSG(uContains(_optimizedPoses, _path.at(_pathCurrentIndex).first), uFormat("local map size=%d, id=%d", (int)_optimizedPoses.size(), _path.at(_pathCurrentIndex).first).c_str()); UASSERT_MSG(uContains(_optimizedPoses, _path.at(_pathGoalIndex).first), uFormat("local map size=%d, id=%d", (int)_optimizedPoses.size(), _path.at(_pathGoalIndex).first).c_str()); } timeRealTimeLimitReachedProcess = timer.ticks(); ULOGGER_INFO("Time limit reached processing = %f...", timeRealTimeLimitReachedProcess); //============================================================== // Finalize statistics and log files //============================================================== int localGraphSize = 0; if(_publishStats) { statistics_.addStatistic(Statistics::kTimingStatistics_creation(), timeStatsCreation*1000); statistics_.addStatistic(Statistics::kTimingTotal(), totalTime*1000); statistics_.addStatistic(Statistics::kTimingForgetting(), timeRealTimeLimitReachedProcess*1000); statistics_.addStatistic(Statistics::kTimingJoining_trash(), timeJoiningTrash*1000); statistics_.addStatistic(Statistics::kTimingEmptying_trash(), timeEmptyingTrash*1000); statistics_.addStatistic(Statistics::kTimingMemory_cleanup(), timeMemoryCleanup*1000); // Transfer statistics_.addStatistic(Statistics::kMemorySignatures_removed(), signaturesRemoved.size()); statistics_.addStatistic(Statistics::kMemoryImmunized_globally(), immunizedGlobally); statistics_.addStatistic(Statistics::kMemoryImmunized_locally(), immunizedLocally); statistics_.addStatistic(Statistics::kMemoryImmunized_locally_max(), maxLocalLocationsImmunized); // place after transfer because the memory/local graph may have changed statistics_.addStatistic(Statistics::kMemoryWorking_memory_size(), _memory->getWorkingMemSize(true)); statistics_.addStatistic(Statistics::kMemoryWorking_memory_inter_size(), _memory->getWorkingMemIntermediateNodesCount()); statistics_.addStatistic(Statistics::kMemoryShort_time_memory_size(), _memory->getStMem().size()-_memory->getStMemIntermediateNodesCount()); statistics_.addStatistic(Statistics::kMemoryShort_time_memory_inter_size(), _memory->getStMemIntermediateNodesCount()); statistics_.addStatistic(Statistics::kMemoryDatabase_memory_used(), _memory->getDatabaseMemoryUsed()); // Set local graph std::map poses; std::multimap constraints; if(!_rgbdSlamMode) { UDEBUG(""); // no optimization on appearance-only mode, create a local graph std::map ids = _memory->getNeighborsId(lastSignatureData.id(), 0, 0, true); _memory->getMetricConstraints(uKeysSet(ids), poses, constraints, false); } else // RGBD-SLAM mode { poses = _optimizedPoses; constraints = _constraints; } UINFO("Adding data %d [%d] (rgb/left=%d depth/right=%d)", lastSignatureData.id(), lastSignatureData.mapId(), lastSignatureData.sensorData().imageRaw().empty()?0:1, lastSignatureData.sensorData().depthOrRightRaw().empty()?0:1); statistics_.addSignatureData(lastSignatureData); if(_nodesToRepublish.size()) { std::multimap missingIds; // priority to loopId int tmpId = loopId>0?loopId:_highestHypothesis.first; if(tmpId>0 && _nodesToRepublish.find(tmpId) != _nodesToRepublish.end()) { missingIds.insert(std::make_pair(-1, tmpId)); } if(!_lastLocalizationPose.isNull()) { // Republish data from closest nodes of the current localization std::map nodesOnly(_optimizedPoses.lower_bound(1), _optimizedPoses.end()); int id = rtabmap::graph::findNearestNode(nodesOnly, _lastLocalizationPose); if(id>0) { std::map ids = _memory->getNeighborsId(id, 0, 0, true, false, true); for(std::map::iterator iter=ids.begin(); iter!=ids.end(); ++iter) { if(iter->first != loopId && _nodesToRepublish.find(iter->first) != _nodesToRepublish.end()) { missingIds.insert(std::make_pair(iter->second, iter->first)); } } if(_nodesToRepublish.size() != missingIds.size()) { // remove requested nodes not anymore in the graph for(std::set::iterator iter=_nodesToRepublish.begin(); iter!=_nodesToRepublish.end();) { if(ids.find(*iter) == ids.end()) { iter = _nodesToRepublish.erase(iter); } else { ++iter; } } } } } int loaded = 0; std::stringstream stream; for(std::multimap::iterator iter=missingIds.begin(); iter!=missingIds.end() && loaded<(int)_maxRepublished; ++iter) { statistics_.addSignatureData(getSignatureCopy(iter->second, true, true, true, true, true, true)); _nodesToRepublish.erase(iter->second); ++loaded; stream << iter->second << " "; } if(loaded) { UWARN("Republishing data of requested node(s) %s(%s=%d)", stream.str().c_str(), Parameters::kRtabmapMaxRepublished().c_str(), _maxRepublished); } } UDEBUG(""); localGraphSize = (int)poses.size(); if(!lastSignatureLocalizedPose.isNull()) { poses.insert(std::make_pair(lastSignatureData.id(), lastSignatureLocalizedPose)); // in case we are in localization } statistics_.setPoses(poses); statistics_.setConstraints(constraints); statistics_.addStatistic(Statistics::kMemoryLocal_graph_size(), poses.size()); statistics_.setOdomCachePoses(_odomCachePoses); statistics_.setOdomCacheConstraints(_odomCacheConstraints); statistics_.addStatistic(Statistics::kMemoryOdom_cache_poses(), _odomCachePoses.size()); statistics_.addStatistic(Statistics::kMemoryOdom_cache_links(), _odomCacheConstraints.size()); if(_computeRMSE && _memory->getGroundTruths().size()) { UDEBUG("Computing RMSE..."); float translational_rmse = 0.0f; float translational_mean = 0.0f; float translational_median = 0.0f; float translational_std = 0.0f; float translational_min = 0.0f; float translational_max = 0.0f; float rotational_rmse = 0.0f; float rotational_mean = 0.0f; float rotational_median = 0.0f; float rotational_std = 0.0f; float rotational_min = 0.0f; float rotational_max = 0.0f; graph::calcRMSE( _memory->getGroundTruths(), poses, translational_rmse, translational_mean, translational_median, translational_std, translational_min, translational_max, rotational_rmse, rotational_mean, rotational_median, rotational_std, rotational_min, rotational_max); statistics_.addStatistic(Statistics::kGtTranslational_rmse(), translational_rmse); statistics_.addStatistic(Statistics::kGtTranslational_mean(), translational_mean); statistics_.addStatistic(Statistics::kGtTranslational_median(), translational_median); statistics_.addStatistic(Statistics::kGtTranslational_std(), translational_std); statistics_.addStatistic(Statistics::kGtTranslational_min(), translational_min); statistics_.addStatistic(Statistics::kGtTranslational_max(), translational_max); statistics_.addStatistic(Statistics::kGtRotational_rmse(), rotational_rmse); statistics_.addStatistic(Statistics::kGtRotational_mean(), rotational_mean); statistics_.addStatistic(Statistics::kGtRotational_median(), rotational_median); statistics_.addStatistic(Statistics::kGtRotational_std(), rotational_std); statistics_.addStatistic(Statistics::kGtRotational_min(), rotational_min); statistics_.addStatistic(Statistics::kGtRotational_max(), rotational_max); UDEBUG("Computing RMSE...done!"); } std::vector ids; ids.reserve(_memory->getWorkingMemSize(false) + _memory->getStMem().size()); for(std::set::const_iterator iter=_memory->getStMem().begin(); iter!=_memory->getStMem().end(); ++iter) { ids.push_back(*iter); } for(std::map::const_iterator iter=_memory->getWorkingMem().lower_bound(0); iter!=_memory->getWorkingMem().end(); ++iter) { ids.push_back(iter->first); } statistics_.setWmState(ids); UDEBUG("wmState=%d", (int)ids.size()); } //Save statistics to database if(_memory->isIncremental() || _memory->isLocalizationDataSaved()) { _memory->saveStatistics(statistics_, _saveWMState); } //Start trashing UDEBUG("Empty trash..."); _memory->emptyTrash(); // Log info... // TODO : use a specific class which will handle the RtabmapEvent if(_foutFloat && _foutInt) { UDEBUG("Logging..."); std::string logF = uFormat("%f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f\n", totalTime, timeMemoryUpdate, timeReactivations, timeLikelihoodCalculation, timePosteriorCalculation, timeHypothesesCreation, timeHypothesesValidation, timeRealTimeLimitReachedProcess, timeStatsCreation, _highestHypothesis.second, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, vpHypothesis, timeJoiningTrash, rehearsalValue, timeEmptyingTrash, timeRetrievalDbAccess, timeAddLoopClosureLink, timeMemoryCleanup, timeNeighborLinkRefining, timeProximityByTimeDetection, timeProximityBySpaceDetection, timeMapOptimization); std::string logI = uFormat("%d %d %d %d %d %d %d %d %d %d %d %d %d %d %d %d %d %d %d %d %d\n", _loopClosureHypothesis.first, _highestHypothesis.first, (int)signaturesRemoved.size(), 0, refWordsCount, dictionarySize, int(_memory->getWorkingMemSize(false)), rejectedLoopClosure?1:0, 0, 0, int(signaturesRetrieved.size()), lcHypothesisReactivated, refUniqueWordsCount, retrievalId, 0, rehearsalMaxId, rehearsalMaxId>0?1:0, localGraphSize, data.id(), _memory->getVWDictionary()->getIndexedWordsCount(), _memory->getVWDictionary()->getIndexMemoryUsed()); if(_statisticLogsBufferedInRAM) { _bufferedLogsF.push_back(logF); _bufferedLogsI.push_back(logI); } else { if(_foutFloat) { fprintf(_foutFloat, "%s", logF.c_str()); } if(_foutInt) { fprintf(_foutInt, "%s", logI.c_str()); } } UINFO("Time logging = %f...", timer.ticks()); //ULogger::flush(); } timeFinalizingStatistics = timer.ticks(); UDEBUG("End process, timeFinalizingStatistics=%fs", timeFinalizingStatistics); if(_publishStats) { statistics_.addStatistic(Statistics::kTimingFinalizing_statistics(), timeFinalizingStatistics*1000); } return true; } // SETTERS void Rtabmap::setTimeThreshold(float maxTimeAllowed) { //must be positive, 0 mean inf time allowed (no time limit) _maxTimeAllowed = maxTimeAllowed; if(_maxTimeAllowed < 0) { ULOGGER_WARN("maxTimeAllowed < 0, then setting it to 0 (inf)."); _maxTimeAllowed = 0; } else if(_maxTimeAllowed > 0.0f && _maxTimeAllowed < 1.0f) { ULOGGER_WARN("Time threshold set to %fms, it is not in seconds!", _maxTimeAllowed); } } void Rtabmap::setMemoryThreshold(int maxMemoryAllowed) { //must be positive, 0 mean inf memory allowed (no memory limit) _maxMemoryAllowed = maxMemoryAllowed; if(maxMemoryAllowed < 0) { ULOGGER_WARN("maxMemoryAllowed < 0, then setting it to 0 (inf)."); _maxMemoryAllowed = 0; } } void Rtabmap::setWorkingDirectory(std::string path) { // Expand leading "~" to the user's home directory (shell convention). // We do NOT replace every "~" in the path -- Windows 8.3 short names // embed "~" in the middle (e.g. C:\Users\RUNNER~1\...), and a blanket // uReplaceChar would corrupt those. if(!path.empty() && path[0] == '~') { path = UDirectory::homeDir() + path.substr(1); } if(!path.empty() && UDirectory::exists(path)) { ULOGGER_DEBUG("Comparing new working directory path \"%s\" with \"%s\"", path.c_str(), _wDir.c_str()); if(path.compare(_wDir) != 0) { if (_foutFloat || _foutInt) { UWARN("Working directory has been changed from \"%s\" with \"%s\", new log files will be created.", path.c_str(), _wDir.c_str()); } _wDir = path; setupLogFiles(); } } else if(path.empty()) { _wDir.clear(); setupLogFiles(); } else { ULOGGER_ERROR("Directory \"%s\" doesn't exist!", path.c_str()); } } void Rtabmap::rejectLastLoopClosure() { if(!_memory) { return; } const Signature * lastS = _memory->getLastWorkingSignature(true); // last non-intermediate if(lastS && _memory->getStMem().find(lastS->id())!=_memory->getStMem().end()) { std::multimap links = _memory->getLinks(lastS->id(), false); bool linksRemoved = false; for(std::multimap::iterator iter = links.begin(); iter!=links.end(); ++iter) { if(iter->second.type() == Link::kGlobalClosure || iter->second.type() == Link::kLocalSpaceClosure || iter->second.type() == Link::kLocalTimeClosure || iter->second.type() == Link::kUserClosure) { _memory->removeLink(iter->second.from(), iter->second.to()); std::multimap::iterator jter = graph::findLink(_constraints, iter->second.from(), iter->second.to(), true); if(jter!=_constraints.end()) { _constraints.erase(jter); // second time if link is also inverted jter = graph::findLink(_constraints, iter->second.from(), iter->second.to(), true); if(jter!=_constraints.end()) { _constraints.erase(jter); } } linksRemoved = true; } } if(linksRemoved) { _loopClosureHypothesis.first = 0; // we have to re-optimize the graph without the rejected links if(_memory->isIncremental() && _optimizedPoses.size()) { UINFO("Update graph"); std::map poses = _optimizedPoses; std::multimap constraints; cv::Mat covariance; optimizeCurrentMap(lastS->id(), false, poses, covariance, &constraints); if(poses.empty()) { UWARN("Graph optimization failed after removing loop closure links from last location!"); } else { UINFO("Updated local map (old size=%d, new size=%d)", (int)_optimizedPoses.size(), (int)poses.size()); _optimizedPoses = poses; _constraints = constraints; _mapCorrection = _optimizedPoses.at(lastS->id()) * lastS->getPose().inverse(); } } } } } void Rtabmap::deleteLastLocation() { if(_memory && _memory->getStMem().size()) { int lastId = *_memory->getStMem().rbegin(); const Signature * s = _memory->getSignature(lastId); UASSERT(s); if(s->getWeight() == -1) { UERROR("Deleting last location with inermediate nodes is not supported. Aborting."); return; } _memory->deleteLocation(lastId); // we have to re-optimize the graph without the deleted location if(_memory->isIncremental() && _optimizedPoses.size()) { UINFO("Update graph"); _optimizedPoses.erase(lastId); std::map poses = _optimizedPoses; //remove all constraints with last localization id for(std::multimap::iterator iter=_constraints.begin(); iter!=_constraints.end();) { if(iter->second.from() == lastId || iter->second.to() == lastId) { _constraints.erase(iter++); } else { ++iter; } } if(poses.empty()) { _mapCorrection.setIdentity(); } else { std::multimap constraints; cv::Mat covariance; optimizeCurrentMap(_memory->getLastWorkingSignature(true)->id(), false, poses, covariance, &constraints); if(poses.empty()) { UWARN("Graph optimization failed after deleting the last location!"); } else { _optimizedPoses = poses; _constraints = constraints; _mapCorrection = _optimizedPoses.at(_memory->getLastWorkingSignature(true)->id()) * _memory->getLastWorkingSignature(true)->getPose().inverse(); } } } } } void Rtabmap::setOptimizedPoses(const std::map & poses, const std::multimap & constraints) { _optimizedPoses = poses; _constraints = constraints; } void Rtabmap::dumpData() const { UDEBUG(""); if(_memory) { if(this->getWorkingDir().empty()) { UERROR("Working directory not set."); } else { _memory->dumpMemory(this->getWorkingDir()); } } } // fromId must be in _memory and in _optimizedPoses // Get poses in front of the robot, return optimized poses std::map Rtabmap::getForwardWMPoses( int fromId, int maxNearestNeighbors, float radius, int maxGraphDepth // 0 means ignore ) const { std::map poses; if(_memory && fromId > 0) { UDEBUG(""); const Signature * fromS = _memory->getSignature(fromId); UASSERT(fromS != 0); UASSERT(_optimizedPoses.find(fromId) != _optimizedPoses.end()); pcl::PointCloud::Ptr cloud(new pcl::PointCloud); cloud->resize(_optimizedPoses.size()); std::vector ids(_optimizedPoses.size()); int oi = 0; const std::set & stm = _memory->getStMem(); //get distances std::map foundIds; if(_memory->isIncremental()) { foundIds = _memory->getNeighborsIdRadius(fromId, radius, _optimizedPoses, maxGraphDepth); } else { foundIds = graph::findNearestNodes(fromId, _optimizedPoses, radius); } float radiusSqrd = radius * radius; for(std::map::const_iterator iter = _optimizedPoses.begin(); iter!=_optimizedPoses.end(); ++iter) { if(iter->first != fromId) { if(stm.find(iter->first) == stm.end() && uContains(foundIds, iter->first) && (radiusSqrd==0 || foundIds.at(iter->first) <= radiusSqrd)) { (*cloud)[oi] = pcl::PointXYZ(iter->second.x(), iter->second.y(), iter->second.z()); ids[oi++] = iter->first; } } } cloud->resize(oi); ids.resize(oi); Transform fromT = _optimizedPoses.at(fromId); if(cloud->size()) { //if(cloud->size()) //{ // pcl::io::savePCDFile("radiusPoses.pcd", *cloud); // UWARN("Saved radiusPoses.pcd"); //} //filter poses in front of the fromId float x,y,z, roll,pitch,yaw; fromT.getTranslationAndEulerAngles(x,y,z, roll,pitch,yaw); pcl::CropBox cropbox; cropbox.setInputCloud(cloud); cropbox.setMin(Eigen::Vector4f(-1, -radius, -999999, 0)); cropbox.setMax(Eigen::Vector4f(radius, radius, 999999, 0)); cropbox.setRotation(Eigen::Vector3f(roll, pitch, yaw)); cropbox.setTranslation(Eigen::Vector3f(x, y, z)); cropbox.setRotation(Eigen::Vector3f(roll,pitch,yaw)); pcl::IndicesPtr indices(new std::vector()); cropbox.filter(*indices); //if(indices->size()) //{ // pcl::io::savePCDFile("radiusCrop.pcd", *cloud, *indices); // UWARN("Saved radiusCrop.pcd"); //} if(indices->size()) { pcl::search::KdTree::Ptr kdTree(new pcl::search::KdTree); kdTree->setInputCloud(cloud, indices); std::vector ind; std::vector dist; pcl::PointXYZ pt(fromT.x(), fromT.y(), fromT.z()); kdTree->radiusSearch(pt, radius, ind, dist, maxNearestNeighbors); //pcl::PointCloud inliers; for(unsigned int i=0; i=0) { Transform tmp = _optimizedPoses.find(ids[ind[i]])->second; //inliers.push_back(pcl::PointXYZ(tmp.x(), tmp.y(), tmp.z())); UDEBUG("Inlier %d: %s", ids[ind[i]], tmp.prettyPrint().c_str()); poses.insert(std::make_pair(ids[ind[i]], tmp)); } } //if(inliers.size()) //{ // pcl::io::savePCDFile("radiusInliers.pcd", inliers); //} //if(nearestId >0) //{ // pcl::PointCloud c; // Transform ct = _optimizedPoses.find(nearestId)->second; // c.push_back(pcl::PointXYZ(ct.x(), ct.y(), ct.z())); // pcl::io::savePCDFile("radiusNearestPt.pcd", c); //} } } } return poses; } std::map > Rtabmap::getPaths(const std::map & posesIn, const Transform & target, int maxGraphDepth) const { std::map > paths; std::set nodesSet; std::map poses; for(std::map::const_iterator iter=posesIn.lower_bound(1); iter!=posesIn.end(); ++iter) { nodesSet.insert(iter->first); poses.insert(*iter); } if(_memory && nodesSet.size() && !target.isNull()) { double e0=0,e1=0,e2=0,e3=0,e4=0; UTimer t; e0 = t.ticks(); // Segment poses connected only by neighbor links while(poses.size()) { std::map path; // select nearest pose and iterate neighbors from there int nearestId = rtabmap::graph::findNearestNode(poses, target); e1+=t.ticks(); if(nearestId == 0) { UWARN("Nearest id of %s in %d poses is 0 !? Returning empty path.", target.prettyPrint().c_str(), (int)poses.size()); break; } std::map ids = _memory->getNeighborsId(nearestId, maxGraphDepth, 0, true, true, true, true, nodesSet); e2+=t.ticks(); for(std::map::iterator iter=ids.begin(); iter!=ids.end(); ++iter) { std::map::iterator jter = poses.find(iter->first); if(jter != poses.end()) { bool valid = path.empty(); if(!valid) { // make sure it has a neighbor added to path std::multimap links = _memory->getNeighborLinks(iter->first); for(std::multimap::iterator kter=links.begin(); kter!=links.end() && !valid; ++kter) { valid = path.find(kter->first) != path.end(); } } if(valid) { //UDEBUG("%d <- %d", nearestId, jter->first); path.insert(*jter); poses.erase(jter); } } } e3+=t.ticks(); if (path.size()) { if (maxGraphDepth > 0 && !_memory->isGraphReduced() && (int)path.size() > maxGraphDepth * 2 + 1) { UWARN("%s=Off but path(%d) > maxGraphDepth(%d)*2+1, nearestId=%d ids=%d. Is reduce graph activated before?", Parameters::kMemReduceGraph().c_str(), (int)path.size(), maxGraphDepth, nearestId, (int)ids.size()); } paths.insert(std::make_pair(nearestId, path)); } else { UWARN("path.size()=0!? nearestId=%d ids=%d, aborting...", nearestId, (int)ids.size()); break; } e4+=t.ticks(); } UDEBUG("e0=%fs e1=%fs e2=%fs e3=%fs e4=%fs", e0, e1, e2, e3, e4); } return paths; } void Rtabmap::optimizeCurrentMap( int id, bool lookInDatabase, std::map & optimizedPoses, cv::Mat & covariance, std::multimap * constraints, double * error, int * iterationsDone) const { //Optimize the map UINFO("Optimize map: around location %d (lookInDatabase=%s)", id, lookInDatabase?"true":"false"); if(_memory && id > 0) { if(!lookInDatabase && (!_memory->getSignature(id) || _memory->getSignature(id)->getWeight() == -1)) { UERROR("When doing a local optimization, the root id (%d) must exist and not be an intermediate node! Aborting...", id); optimizedPoses.clear(); if(constraints) { constraints->clear(); } return; } UTimer timer; std::map ids = _memory->getNeighborsId(id, 0, lookInDatabase?-1:0, true, false); if(!_optimizeFromGraphEnd && ids.size() > 1) { if(lookInDatabase) { id = ids.begin()->first; } else { // Find first node that is not intermediate for(auto pair: ids) { // Make sure fromId is not an intermediate node const Signature * s = _memory->getSignature(pair.first); if(s && s->getWeight() != -1) { id = pair.first; break; } else if(!s) { UWARN("Not found node %d in memory?!", pair.first); } } } } UINFO("get %d ids time %f s", (int)ids.size(), timer.ticks()); std::map poses = Rtabmap::optimizeGraph(id, uKeysSet(ids), optimizedPoses, lookInDatabase, covariance, constraints, error, iterationsDone); UINFO("optimize time %f s", timer.ticks()); if(poses.size()) { optimizedPoses = poses; if(_memory->getSignature(id) && uContains(optimizedPoses, id)) { Transform t = optimizedPoses.at(id) * _memory->getSignature(id)->getPose().inverse(); UINFO("Correction (from node %d) %s", id, t.prettyPrint().c_str()); } } else { UWARN("Failed to optimize the graph! returning empty optimized poses..."); optimizedPoses.clear(); if(constraints) { constraints->clear(); } } } } std::map Rtabmap::optimizeGraph( int fromId, const std::set & ids, const std::map & guessPoses, bool lookInDatabase, cv::Mat & covariance, std::multimap * constraints, double * error, int * iterationsDone) const { UTimer timer; std::map optimizedPoses; std::map poses; std::multimap edgeConstraints; UDEBUG("ids=%d", (int)ids.size()); _memory->getMetricConstraints(ids, poses, edgeConstraints, lookInDatabase, !_graphOptimizer->landmarksIgnored()); UINFO("get constraints (ids=%d, %d poses, %d edges) time %f s", (int)ids.size(), (int)poses.size(), (int)edgeConstraints.size(), timer.ticks()); // add landmark priors if there are some for(std::map::iterator iter=poses.begin(); iter!=poses.end() && iter->first < 0; ++iter) { if(_markerPriors.find(iter->first) != _markerPriors.end()) { cv::Mat infMatrix = cv::Mat::eye(6, 6, CV_64FC1); infMatrix(cv::Range(0,3), cv::Range(0,3)) /= _markerPriorsLinearVariance; infMatrix(cv::Range(3,6), cv::Range(3,6)) /= _markerPriorsAngularVariance; edgeConstraints.insert(std::make_pair(iter->first, Link(iter->first, iter->first, Link::kPosePrior, _markerPriors.at(iter->first), infMatrix))); UDEBUG("Added prior %d : %s (variance: lin=%f ang=%f)", iter->first, _markerPriors.at(iter->first).prettyPrint().c_str(), _markerPriorsLinearVariance, _markerPriorsAngularVariance); } } if(_graphOptimizer->iterations() > 0) { for(std::map::iterator iter=poses.begin(); iter!=poses.end(); ++iter) { // Apply guess poses (if some), ignore for rootid to avoid origin drifting std::map::const_iterator foundGuess = guessPoses.find(iter->first); if(foundGuess!=guessPoses.end() && iter->first != fromId) { iter->second = foundGuess->second; } } } UASSERT(_graphOptimizer!=0); if(_graphOptimizer->iterations() == 0) { // Optimization disabled! Return not optimized poses. optimizedPoses = poses; if(constraints) { *constraints = edgeConstraints; } } else { bool hasLandmarks = !edgeConstraints.empty() && edgeConstraints.begin()->first < 0; if(poses.size() != guessPoses.size() || hasLandmarks) { UDEBUG("recompute poses using only links (robust to multi-session)"); std::map posesOut; std::multimap edgeConstraintsOut; _graphOptimizer->getConnectedGraph(fromId, poses, edgeConstraints, posesOut, edgeConstraintsOut); optimizedPoses = _graphOptimizer->optimize(fromId, posesOut, edgeConstraintsOut, covariance, 0, error, iterationsDone); if(constraints) { *constraints = edgeConstraintsOut; } } else { UDEBUG("use input guess poses"); optimizedPoses = _graphOptimizer->optimize(fromId, poses, edgeConstraints, covariance, 0, error, iterationsDone); if(constraints) { *constraints = edgeConstraints; } } if(!poses.empty() && optimizedPoses.empty()) { UWARN("Optimization has failed (poses=%d, guess=%d, links=%d)...", (int)poses.size(), (int)guessPoses.size(), (int)edgeConstraints.size()); } } UINFO("Optimization time %f s", timer.ticks()); return optimizedPoses; } // If repairing works, all input arguments are updated accordingly to new graph // Returns IDs of the links removed from constraints std::list > Rtabmap::repairGraph( graph::MaxGraphErrors & maxGraphErrors, std::map & poses, std::multimap & constraints, double & optimizationError, int & optimizationIterations, cv::Mat & optimizationCovariance) { UASSERT(maxGraphErrors.linearLink.isValid()); UASSERT(graph::findLink(constraints, maxGraphErrors.linearLink.from(), maxGraphErrors.linearLink.to()) != constraints.end()); int originalMaxErrorLinkFrom = maxGraphErrors.linearLink.from(); int originalMaxErrorLinkTo = maxGraphErrors.linearLink.to(); graph::MaxGraphErrors subMaxGraphErrors = maxGraphErrors; std::list > removedLinks; std::map subPoses = poses; std::multimap subConstraints = constraints; while(subMaxGraphErrors.linearLink.isValid() && subMaxGraphErrors.linearRatio > _optimizationMaxError) { removedLinks.push_back(std::make_pair(subMaxGraphErrors.linearLink.from(), subMaxGraphErrors.linearLink.to())); subConstraints.erase(graph::findLink(subConstraints, subMaxGraphErrors.linearLink.from(), subMaxGraphErrors.linearLink.to())); subMaxGraphErrors.linearLink = Link(); // Get connected graph just to check if a node got disconnected (localization mode) std::map posesOut; std::multimap subConstraintsOut; _graphOptimizer->getConnectedGraph(subPoses.rbegin()->first, subPoses, subConstraints, posesOut, subConstraintsOut); if(subPoses.size() - posesOut.size() > 1) { UWARN("More than one pose filtered in one iteration when trying to repair graph. Aborting repairing."); break; } else if(subPoses.size() != posesOut.size()) { for(std::map::iterator pter=subPoses.begin(); pter!=subPoses.end(); ++pter) { if(posesOut.find(pter->first) == posesOut.end()) { subPoses.erase(pter); break; // should be only one if different, break now } } } cv::Mat subOptimizationCovariance; double subOptimizationError = 0.0; int subOptimizationIterations = 0; int fromId = subPoses.rbegin()->first; if(!_optimizeFromGraphEnd) { // Find first node that is not intermediate for(std::map::iterator iter = subPoses.lower_bound(1); iter!=subPoses.end(); ++iter) { // Make sure fromId is not an intermediate node const Signature * s = _memory->getSignature(iter->first); if(s && s->getWeight() != -1) { fromId = iter->first; break; } else if(!s) { UWARN("Not found node %d in memory?!", iter->first); } } } subPoses = _graphOptimizer->optimize( fromId, subPoses, subConstraintsOut, subOptimizationCovariance, 0, &subOptimizationError, &subOptimizationIterations); if(subPoses.empty()) { UWARN("Optimization failed when trying to repair graph."); } else { subMaxGraphErrors = graph::computeMaxGraphErrors( subPoses, subConstraintsOut); if(!subMaxGraphErrors.linearLink.isValid()) { UWARN("Could not compute graph errors! Wrong loop closures could be accepted!"); } else if(subMaxGraphErrors.linearRatio > _optimizationMaxError) { float distance = poses.at(originalMaxErrorLinkFrom).getDistance(poses.at(subMaxGraphErrors.linearLink.from())); if(subMaxGraphErrors.linearLink.type() != Link::kNeighbor && distance < _optimizationMaxErrorRepairRadius) { UWARN("Optimization error is still high (%f, on link %d->%d type=%d) after removing the loop closure with the highest error. " "As it is close (%f m < %s=%f m) to original loop closure with high error (%d->%d), we will reject again this one to see if it helps.", subMaxGraphErrors.linearRatio, subMaxGraphErrors.linearLink.from(), subMaxGraphErrors.linearLink.to(), subMaxGraphErrors.linearLink.type(), distance, Parameters::kRGBDOptimizeMaxErrorRepairRadius().c_str(), _optimizationMaxErrorRepairRadius, originalMaxErrorLinkFrom, originalMaxErrorLinkTo); } else { UWARN("Optimization error is still high (%f, on link %d->%d) after removing loop closure with highest error.", subMaxGraphErrors.linearRatio, subMaxGraphErrors.linearLink.from(), subMaxGraphErrors.linearLink.to()); subMaxGraphErrors.linearLink = Link(); } } else { UWARN("Optimization error is lower (%f, on link %d->%d) after removing loop " "closure with highest error. We will remove the old link (%d->%d collaterals=%ld) and accept the new one.", subMaxGraphErrors.linearRatio, subMaxGraphErrors.linearLink.from(), subMaxGraphErrors.linearLink.to(), removedLinks.front().first, removedLinks.front().second, removedLinks.size()-1); maxGraphErrors = subMaxGraphErrors; // Make sure the link pointers in maxGraphErrors point on same constraints std::swap(poses, subPoses); std::swap(constraints, subConstraintsOut); optimizationCovariance = subOptimizationCovariance; optimizationError = subOptimizationError; optimizationIterations = subOptimizationIterations; return removedLinks; } } } return std::list >(); } void Rtabmap::adjustLikelihood(std::map & likelihood) const { ULOGGER_DEBUG("likelihood.size()=%d", (int)likelihood.size()); UTimer timer; timer.start(); if(likelihood.size()==0) { return; } // Use only non-null values (ignore virtual place) std::list values; bool likelihoodNullValuesIgnored = true; for(std::map::iterator iter = ++likelihood.begin(); iter!=likelihood.end(); ++iter) { if((iter->second >= 0 && !likelihoodNullValuesIgnored) || (iter->second > 0 && likelihoodNullValuesIgnored)) { values.push_back(iter->second); } } UDEBUG("values.size=%d", (int)values.size()); float mean = uMean(values); float stdDev = std::sqrt(uVariance(values, mean)); //Adjust likelihood with mean and standard deviation (see Angeli phd) float epsilon = 0.0001; float max = 0.0f; int maxId = 0; for(std::map::iterator iter=++likelihood.begin(); iter!= likelihood.end(); ++iter) { float value = iter->second; iter->second = 1.0f; if(value > mean+stdDev) { if(_virtualPlaceLikelihoodRatio==0 && mean) { iter->second = (value-(stdDev-epsilon))/mean; } else if(_virtualPlaceLikelihoodRatio!=0 && stdDev) { iter->second = (value-mean)/stdDev; } } if(value > max) { max = value; maxId = iter->first; } } if(_virtualPlaceLikelihoodRatio==0 && stdDev > epsilon && max) { likelihood.begin()->second = mean/stdDev + 1.0f; } else if(_virtualPlaceLikelihoodRatio!=0 && max > mean) { likelihood.begin()->second = stdDev/(max-mean) + 1.0f; } else { likelihood.begin()->second = 2.0f; //2 * std dev } double time = timer.ticks(); UDEBUG("mean=%f, stdDev=%f, max=%f, maxId=%d, time=%fs", mean, stdDev, max, maxId, time); } void Rtabmap::dumpPrediction() const { if(_memory && _bayesFilter) { if(this->getWorkingDir().empty()) { UERROR("Working directory not set."); return; } std::list signaturesToCompare; for(std::map::const_iterator iter=_memory->getWorkingMem().begin(); iter!=_memory->getWorkingMem().end(); ++iter) { if(iter->first > 0) { const Signature * s = _memory->getSignature(iter->first); UASSERT(s!=0); if(s->getWeight() != -1) // ignore intermediate nodes { signaturesToCompare.push_back(iter->first); } } else { // virtual signature should be added signaturesToCompare.push_back(iter->first); } } cv::Mat prediction = _bayesFilter->generatePrediction(_memory, uListToVector(signaturesToCompare)); FILE* fout = 0; std::string fileName = this->getWorkingDir() + "/DumpPrediction.txt"; #ifdef _MSC_VER fopen_s(&fout, fileName.c_str(), "w"); #else fout = fopen(fileName.c_str(), "w"); #endif if(fout) { for(int i=0; i velocity; GPS gps; EnvSensors sensors; _memory->getNodeInfo(id, odomPoseLocal, mapId, weight, label, stamp, groundTruth, velocity, gps, sensors, true); SensorData data; data.setId(id); if(images || scan || userData || occupancyGrid) { data = _memory->getNodeData(id, images, scan, userData, occupancyGrid); } if(!images && withWords) { std::vector models; std::vector stereoModels; _memory->getNodeCalibration(id, models, stereoModels); data.setCameraModels(models); data.setStereoCameraModels(stereoModels); } s=Signature(id, mapId, weight, stamp, label, odomPoseLocal, groundTruth, data); std::multimap links = _memory->getLinks(id, true, true); for(std::multimap::iterator iter=links.begin(); iter!=links.end(); ++iter) { if(iter->second.type() == Link::kLandmark) { s.addLandmark(iter->second); } else { s.addLink(iter->second); } } if(withWords || withGlobalDescriptors) { std::multimap words; std::vector wordsKpts; std::vector words3; cv::Mat wordsDescriptors; std::vector globalDescriptors; _memory->getNodeWordsAndGlobalDescriptors(id, words, wordsKpts, words3, wordsDescriptors, globalDescriptors); if(withWords) { s.setWords(words, wordsKpts, words3, wordsDescriptors); } if(withGlobalDescriptors) { s.sensorData().setGlobalDescriptors(globalDescriptors); } } if(!withGlobalDescriptors) { // Node data taken from memory comes with its global descriptors. s.sensorData().clearGlobalDescriptors(); } if(velocity.size()==6) { s.setVelocity(velocity[0], velocity[1], velocity[2], velocity[3], velocity[4], velocity[5]); } s.sensorData().setGPS(gps); s.sensorData().setEnvSensors(sensors); } return s; } void Rtabmap::get3DMap( std::map & signatures, std::map & poses, std::multimap & constraints, bool optimized, bool global) const { UDEBUG(""); return getGraph(poses, constraints, optimized, global, &signatures, true, true, true, true); } void Rtabmap::getGraph( std::map & poses, std::multimap & constraints, bool optimized, bool global, std::map * signatures, bool withImages, bool withScan, bool withUserData, bool withGrid, bool withWords, bool withGlobalDescriptors) const { if(_memory && _memory->getLastWorkingSignature(!global)) { if(_rgbdSlamMode) { if(optimized) { poses = _optimizedPoses; // guess cv::Mat covariance; this->optimizeCurrentMap(_memory->getLastWorkingSignature(!global)->id(), global, poses, covariance, &constraints); if(!global && !_optimizedPoses.empty()) { // We send directly the already optimized poses if they are set UDEBUG("_optimizedPoses=%ld poses=%ld", _optimizedPoses.size(), poses.size()); poses = _optimizedPoses; } } else { std::map ids = _memory->getNeighborsId(_memory->getLastWorkingSignature(!global)->id(), 0, global?-1:0, true); _memory->getMetricConstraints(uKeysSet(ids), poses, constraints, global); } } else { // no optimization on appearance-only mode std::map ids = _memory->getNeighborsId(_memory->getLastWorkingSignature(!global)->id(), 0, global?-1:0, true); _memory->getMetricConstraints(uKeysSet(ids), poses, constraints, global); } if(signatures) { // Get data std::set ids = uKeysSet(_memory->getWorkingMem()); // WM //remove virtual signature ids.erase(Memory::kIdVirtual); ids.insert(_memory->getStMem().begin(), _memory->getStMem().end()); // STM + WM if(global) { ids = _memory->getAllSignatureIds(); // STM + WM + LTM, ignoreChildren=true } for(std::set::iterator iter = ids.begin(); iter!=ids.end(); ++iter) { signatures->insert(std::make_pair(*iter, getSignatureCopy(*iter, withImages, withScan, withUserData, withGrid, withWords, withGlobalDescriptors))); } } } else if(_memory && (_memory->getStMem().size() || _memory->getWorkingMemSize(!global) > 0)) { UERROR("Last working signature is null!?"); } else if(_memory == 0) { UWARN("Memory not initialized..."); } } std::map Rtabmap::getNodesInRadius(const Transform & pose, float radius, int k, std::map * distsSqr) { std::map nearestNodesTmp; std::map * nearestNodesPtr = distsSqr == 0? &nearestNodesTmp : distsSqr; *nearestNodesPtr = graph::findNearestNodes(pose, _optimizedPoses, radius<=0?_localRadius:radius, 0, k); std::map nearestPoses; for(std::map::iterator iter=nearestNodesPtr->begin(); iter!=nearestNodesPtr->end(); ++iter) { nearestPoses.insert(*_optimizedPoses.find(iter->first)); } return nearestPoses; } std::map Rtabmap::getNodesInRadius(int nodeId, float radius, int k, std::map * distsSqr) { UDEBUG("nodeId=%d, radius=%f", nodeId, radius); std::map nearestNodesTmp; std::map * nearestNodesPtr = distsSqr == 0? &nearestNodesTmp : distsSqr; if(nodeId==0 && !_lastLocalizationPose.isNull() && !_lastLocalizationPose.isIdentity()) { *nearestNodesPtr = graph::findNearestNodes(_lastLocalizationPose, _optimizedPoses, radius<=0?_localRadius:radius, 0, k); } else { if(nodeId==0 && !_optimizedPoses.empty()) { nodeId = _optimizedPoses.rbegin()->first; } if(_optimizedPoses.find(nodeId) != _optimizedPoses.end()) { *nearestNodesPtr = graph::findNearestNodes(nodeId, _optimizedPoses, radius<=0?_localRadius:radius, 0, k); } } std::map nearestPoses; for(std::map::iterator iter=nearestNodesPtr->begin(); iter!=nearestNodesPtr->end(); ++iter) { nearestPoses.insert(*_optimizedPoses.find(iter->first)); } return nearestPoses; } int Rtabmap::detectMoreLoopClosures( float clusterRadiusMax, float clusterAngle, int iterations, bool intraSession, bool interSession, const ProgressState * processState, float clusterRadiusMin, int toFromMapId) { UDEBUG(""); UASSERT(iterations>0); if(_graphOptimizer->iterations() <= 0) { UERROR("Cannot detect more loop closures if graph optimization iterations = 0"); return -1; } if(!_rgbdSlamMode) { UERROR("Detecting more loop closures can be done only in RGBD-SLAM mode."); return -1; } if(!intraSession && !interSession) { UERROR("Intra and/or inter session argument should be true."); return -1; } std::list loopClosuresAdded; std::multimap checkedLoopClosures; std::map posesToCheckLoopClosures; std::map poses; std::multimap links; this->getGraph(poses, links, true, true); std::map mapIds; UDEBUG("remove all invalid or intermediate nodes, fill mapIds"); for(std::map::iterator iter=poses.upper_bound(0); iter!=poses.end();++iter) { Transform odom, gt; int mapId, weight; std::string l; double s; std::vector v; GPS gps; EnvSensors srs; if(_memory->getNodeInfo(iter->first, odom, mapId, weight, l, s, gt, v, gps, srs, true) && weight >= 0) { posesToCheckLoopClosures.insert(*iter); mapIds.insert(std::make_pair(iter->first, mapId)); } } for(int n=0; n clusters = graph::radiusPosesClustering( posesToCheckLoopClosures, clusterRadiusMax, clusterAngle); UINFO("Looking for more loop closures: clustering poses... found %ld clusters.", clusters.size()); if(toFromMapId >=0) { size_t clustersBefore = clusters.size(); for(std::multimap::iterator iter=clusters.begin(); iter!=clusters.end();) { int mapId = uValue(mapIds, iter->first, 0); if(mapId != toFromMapId) { iter = clusters.erase(iter); } else { ++iter; } } UINFO("Looking for more loop closures: filtered %ld/%ld clusters for map session %d.", clustersBefore-clusters.size(), clustersBefore, toFromMapId); if(clusters.empty()) { UERROR("No clusters belong to mapId %d, aborting.", toFromMapId); break; } } if(_memory->getMaxStMemSize() > 1) { size_t clustersBefore = clusters.size(); for(std::multimap::iterator iter=clusters.begin(); iter!=clusters.end();) { if(abs(iter->first - iter->second) < _memory->getMaxStMemSize()) { iter = clusters.erase(iter); } else { // compute path to know how far we are in terms of graph length std::map ids = _memory->getNeighborsId(iter->first, _memory->getMaxStMemSize(), -1, true, true, true); if(ids.find(iter->second) != ids.end()) { iter = clusters.erase(iter); } else { ++iter; } } } UINFO("Looking for more loop closures: filtered %ld/%ld clusters for too close nodes (below %s=%d).", clustersBefore-clusters.size(), clustersBefore, Parameters::kMemSTMSize().c_str(), _memory->getMaxStMemSize()); } int i=0; std::set addedLinks; for(std::multimap::iterator iter=clusters.begin(); iter!= clusters.end(); ++iter, ++i) { if(processState && processState->isCanceled()) { return -1; break; } int from = iter->first; int to = iter->second; if(from > to) { from = iter->second; to = iter->first; } int mapIdFrom = uValue(mapIds, from, 0); int mapIdTo = uValue(mapIds, to, 0); if((interSession && mapIdFrom != mapIdTo) || (intraSession && mapIdFrom == mapIdTo)) { bool alreadyChecked = false; for(std::multimap::iterator jter = checkedLoopClosures.lower_bound(from); !alreadyChecked && jter!=checkedLoopClosures.end() && jter->first == from; ++jter) { if(to == jter->second) { alreadyChecked = true; } } if(!alreadyChecked) { // only add new links and one per cluster per iteration if(addedLinks.find(from) == addedLinks.end() && addedLinks.find(to) == addedLinks.end() && rtabmap::graph::findLink(links, from, to) == links.end()) { // Reverify if in the bounds with the current optimized graph Transform delta = poses.at(from).inverse() * poses.at(to); if(delta.getNorm() < clusterRadiusMax && delta.getNorm() >= clusterRadiusMin) { checkedLoopClosures.insert(std::make_pair(from, to)); Signature fromS = getSignatureCopy(from, false, true, false, false, true, false); Signature toS = getSignatureCopy(to, false, true, false, false, true, false); UASSERT(fromS.getWeight()>=0); UASSERT(toS.getWeight()>=0); Transform guess; if(_proximityBySpace && uContains(poses, from) && uContains(poses, to)) { guess = poses.at(from).inverse() * poses.at(to); } RegistrationInfo info; // use signatures instead of IDs because some signatures may not be in WM Transform t = _memory->computeTransform(fromS, toS, guess, &info); if(!t.isNull()) { bool updateConstraints = true; //optimize the graph to see if the new constraint is globally valid int fromId = from; int mapId = fromS.mapId(); // use first node of the map containing from for(std::map::iterator ster=posesToCheckLoopClosures.begin(); ster!=posesToCheckLoopClosures.end(); ++ster) { if(uValue(mapIds, ster->first, 0) == mapId) { fromId = ster->first; break; } } std::multimap linksIn = links; linksIn.insert(std::make_pair(from, Link(from, to, Link::kUserClosure, t, getInformation(info.covariance)))); graph::MaxGraphErrors maxGraphErrors; std::map optimizedPoses; std::multimap linksOut; UASSERT(poses.find(fromId) != poses.end()); UASSERT_MSG(poses.find(from) != poses.end(), uFormat("id=%d poses=%d links=%d", from, (int)poses.size(), (int)links.size()).c_str()); UASSERT_MSG(poses.find(to) != poses.end(), uFormat("id=%d poses=%d links=%d", to, (int)poses.size(), (int)links.size()).c_str()); _graphOptimizer->getConnectedGraph(fromId, poses, linksIn, optimizedPoses, linksOut); UASSERT(optimizedPoses.find(fromId) != optimizedPoses.end()); UASSERT_MSG(optimizedPoses.find(from) != optimizedPoses.end(), uFormat("id=%d poses=%d links=%d", from, (int)optimizedPoses.size(), (int)linksOut.size()).c_str()); UASSERT_MSG(optimizedPoses.find(to) != optimizedPoses.end(), uFormat("id=%d poses=%d links=%d", to, (int)optimizedPoses.size(), (int)linksOut.size()).c_str()); UASSERT(graph::findLink(linksOut, from, to) != linksOut.end()); optimizedPoses = _graphOptimizer->optimize(fromId, optimizedPoses, linksOut); std::string msg; if(optimizedPoses.size()) { maxGraphErrors = graph::computeMaxGraphErrors( optimizedPoses, linksOut); if(maxGraphErrors.linearLink.isValid()) { UINFO("Max optimization linear error = %f m (link %d->%d)", maxGraphErrors.linear, maxGraphErrors.linearLink.from(), maxGraphErrors.linearLink.to()); if(_optimizationMaxError > 0.0f && maxGraphErrors.linearRatio > _optimizationMaxError) { msg = uFormat("Rejecting edge %d->%d because " "graph error is too large after optimization (%f m for edge %d->%d with ratio %f > std=%f m). " "\"%s\" is %f.", from, to, maxGraphErrors.linear, maxGraphErrors.linearLink.from(), maxGraphErrors.linearLink.to(), maxGraphErrors.linearRatio, sqrt(maxGraphErrors.linearLink.transVariance()), Parameters::kRGBDOptimizeMaxError().c_str(), _optimizationMaxError); } else if(_optimizationMaxError == 0.0f && maxGraphErrors.linearRatio>100 && !_graphOptimizer->isRobust()) { UERROR("Huge optimization error detected!" "Linear error ratio of %f (edge %d->%d, type=%d, abs error=%f m, stddev=%f). You may consider " "enabling \"%s\" to reject those bad optimizations by setting it to a non null value!", maxGraphErrors.linearRatio, maxGraphErrors.linearLink.from(), maxGraphErrors.linearLink.to(), maxGraphErrors.linearLink.type(), maxGraphErrors.linear, sqrt(maxGraphErrors.linearLink.transVariance()), Parameters::kRGBDOptimizeMaxError().c_str()); } } else if(maxGraphErrors.angularLink.isValid()) { UINFO("Max optimization angular error = %f deg (link %d->%d)", maxGraphErrors.angular*180.0f/M_PI, maxGraphErrors.angularLink.from(), maxGraphErrors.angularLink.to()); if(_optimizationMaxError > 0.0f && maxGraphErrors.angularRatio > _optimizationMaxError) { msg = uFormat("Rejecting edge %d->%d because " "graph error is too large after optimization (%f deg for edge %d->%d with ratio %f > std=%f deg). " "\"%s\" is %f m.", from, to, maxGraphErrors.angular*180.0f/M_PI, maxGraphErrors.angularLink.from(), maxGraphErrors.angularLink.to(), maxGraphErrors.angularRatio, sqrt(maxGraphErrors.angularLink.rotVariance()), Parameters::kRGBDOptimizeMaxError().c_str(), _optimizationMaxError); } else if(_optimizationMaxError == 0.0f && maxGraphErrors.angularRatio>100 && !_graphOptimizer->isRobust()) { UERROR("Huge optimization error detected!" "Angular error ratio of %f (edge %d->%d, type=%d, abs error=%f m, stddev=%f). You may consider " "enabling \"%s\" to reject those bad optimizations by setting it to a non null value!", maxGraphErrors.angularRatio, maxGraphErrors.angularLink.from(), maxGraphErrors.angularLink.to(), maxGraphErrors.angularLink.type(), maxGraphErrors.angular*180.0f/CV_PI, sqrt(maxGraphErrors.angularLink.rotVariance()), Parameters::kRGBDOptimizeMaxError().c_str()); } } } else { msg = uFormat("Rejecting edge %d->%d because graph optimization has failed!", from, to); } if(!msg.empty()) { UWARN("%s", msg.c_str()); updateConstraints = false; } else { poses = optimizedPoses; } if(updateConstraints) { addedLinks.insert(from); addedLinks.insert(to); cv::Mat inf = getInformation(info.covariance); links.insert(std::make_pair(from, Link(from, to, Link::kUserClosure, t, inf))); loopClosuresAdded.push_back(Link(from, to, Link::kUserClosure, t, inf)); std::string msg = uFormat("Iteration %d/%d: Added loop closure %d->%d! (%d/%d)", n+1, iterations, from, to, i+1, (int)clusters.size()); UINFO("%s", msg.c_str()); if(processState) { UINFO("%s", msg.c_str()); if(!processState->callback(msg)) { return -1; } } } } } } } } } if(processState) { std::string msg = uFormat("Iteration %d/%d: Detected %d total loop closures!", n+1, iterations, (int)addedLinks.size()/2); UINFO("%s", msg.c_str()); if(!processState->callback(msg)) { return -1; } } else { UINFO("Iteration %d/%d: Detected %d total loop closures!", n+1, iterations, (int)addedLinks.size()/2); } if(addedLinks.size() == 0) { break; } UINFO("Optimizing graph with new links (%d nodes, %d constraints)...", (int)poses.size(), (int)links.size()); int fromId = _optimizeFromGraphEnd?poses.rbegin()->first:poses.begin()->first; poses = _graphOptimizer->optimize(fromId, poses, links, 0); if(poses.size() == 0) { UERROR("Optimization failed! Rejecting all loop closures..."); loopClosuresAdded.clear(); return -1; } UINFO("Optimizing graph with new links... done!"); } UINFO("Total added %d loop closures.", (int)loopClosuresAdded.size()); if(loopClosuresAdded.size()) { for(std::list::iterator iter=loopClosuresAdded.begin(); iter!=loopClosuresAdded.end(); ++iter) { _memory->addLink(*iter, true); } // Update optimized poses for(std::map::iterator iter=_optimizedPoses.begin(); iter!=_optimizedPoses.end(); ++iter) { std::map::iterator jter = poses.find(iter->first); if(jter != poses.end()) { iter->second = jter->second; } } std::map tmp; // Update also the links if some have been added in WM _memory->getMetricConstraints(uKeysSet(_optimizedPoses), tmp, _constraints, false); // This will force rtabmap_ros to regenerate the global occupancy grid if there was one _memory->save2DMap(cv::Mat(), 0, 0, 0); } return (int)loopClosuresAdded.size(); } bool Rtabmap::globalBundleAdjustment( int optimizerType, bool rematchFeatures, int iterations, float pixelVariance) { if(!_optimizedPoses.empty() && !_constraints.empty()) { int iterations = Parameters::defaultOptimizerIterations(); float pixelVariance = Parameters::defaultOptimizerPixelVariance(); ParametersMap params = _parameters; Parameters::parse(params, Parameters::kOptimizerIterations(), iterations); Parameters::parse(params, Parameters::kOptimizerPixelVariance(), pixelVariance); if(iterations > 0) { uInsert(params, ParametersPair(Parameters::kOptimizerIterations(), uNumber2Str(iterations))); } if(pixelVariance > 0.0f) { uInsert(params, ParametersPair(Parameters::kOptimizerPixelVariance(), uNumber2Str(pixelVariance))); } std::map signatures; for(std::map::iterator iter=_optimizedPoses.lower_bound(1); iter!=_optimizedPoses.end(); ++iter) { if(_memory->getSignature(iter->first)) { signatures.insert(std::make_pair(iter->first, *_memory->getSignature(iter->first))); } } Optimizer * optimizer = Optimizer::create((Optimizer::Type)optimizerType, params); std::map poses = optimizer->optimizeBA( _optimizeFromGraphEnd?_optimizedPoses.lower_bound(1)->first:_optimizedPoses.rbegin()->first, _optimizedPoses, _constraints, signatures, rematchFeatures); delete optimizer; if(poses.empty()) { UERROR("Optimization failed!"); } else { _optimizedPoses = poses; // This will force rtabmap_ros to regenerate the global occupancy grid if there was one _memory->save2DMap(cv::Mat(), 0, 0, 0); return true; } } else { UERROR("Optimized poses (%ld) or constraints (%ld) are empty!", _optimizedPoses.size(), _constraints.size()); } return false; } int Rtabmap::cleanupLocalGrids( const std::map & poses, const cv::Mat & map, float xMin, float yMin, float cellSize, int cropRadius, bool filterScans) { if(_memory) { return _memory->cleanupLocalGrids( poses, map, xMin, yMin, cellSize, cropRadius, filterScans); } return -1; } int Rtabmap::refineLinks() { if(!_rgbdSlamMode) { UERROR("Refining links can be done only in RGBD-SLAM mode."); return -1; } std::list linksRefined; std::map poses; std::multimap links; std::map signatures; this->getGraph(poses, links, false, true, &signatures); int i=0; for(std::multimap::iterator iter=links.lower_bound(1); iter!= links.end(); ++iter) { int from = iter->second.from(); int to = iter->second.to(); UASSERT(signatures.find(from) != signatures.end()); UASSERT(signatures.find(to) != signatures.end()); RegistrationInfo info; // use signatures instead of IDs because some signatures may not be in WM Transform t = _memory->computeTransform(signatures.at(from), signatures.at(to), iter->second.transform(), &info); if(!t.isNull()) { linksRefined.push_back(Link(from, to, iter->second.type(), t, info.covariance.inv())); UINFO("Refined link %d->%d! (%d/%d)", from, to, ++i, (int)links.size()); } } UINFO("Total refined %d links.", (int)linksRefined.size()); if(linksRefined.size()) { for(std::list::iterator iter=linksRefined.begin(); iter!=linksRefined.end(); ++iter) { _memory->updateLink(*iter, true); } } return (int)linksRefined.size(); } bool Rtabmap::addLink(const Link & link) { const Transform & t = link.transform(); if(!_rgbdSlamMode) { UERROR("Adding new link can be done only in RGBD-SLAM mode."); return false; } if(!_memory) { UERROR("Memory is not initialized."); return false; } if(t.isNull()) { UERROR("Link's transform is null! (%d->%d type=%s)", link.from(), link.to(), link.typeName().c_str()); return false; } if(_memory->isIncremental()) { if(_memory->getSignature(link.from()) == 0) { UERROR("Link's \"from id\" %d is not in working memory", link.from()); return false; } if(_memory->getSignature(link.to()) == 0) { UERROR("Link's \"to id\" %d is not in working memory", link.to()); return false; } if(_optimizedPoses.find(link.from()) == _optimizedPoses.end() && _optimizedPoses.find(link.to()) == _optimizedPoses.end()) { UERROR("Neither nodes %d or %d are in the local graph (size=%d). One of the 2 nodes should be in the local graph.", (int)_optimizedPoses.size(), link.from(), link.to()); return false; } // add temporary the link if(!_memory->addLink(link)) { UERROR("Cannot add new link %d->%d to memory", link.from(), link.to()); return false; } // optimize with new link std::map poses = _optimizedPoses; std::multimap links; cv::Mat covariance; optimizeCurrentMap(_memory->getLastWorkingSignature(true)->id(), false, poses, covariance, &links); if(poses.find(link.from()) == poses.end()) { UERROR("Link's \"from id\" %d is not in the graph (size=%d)", link.from(), (int)poses.size()); _memory->removeLink(link.from(), link.to()); return false; } if(poses.find(link.to()) == poses.end()) { UERROR("Link's \"to id\" %d is not in the graph (size=%d)", link.to(), (int)poses.size()); _memory->removeLink(link.from(), link.to()); return false; } std::string msg; if(poses.empty()) { msg = uFormat("Rejecting edge %d->%d because graph optimization has failed!", link.from(), link.to()); } else { graph::MaxGraphErrors maxGraphErrors; maxGraphErrors = graph::computeMaxGraphErrors( poses, links); if(maxGraphErrors.linearLink.isValid()) { UINFO("Max optimization linear error = %f m (link %d->%d)", maxGraphErrors.linear, maxGraphErrors.linearLink.from(), maxGraphErrors.linearLink.to()); if(_optimizationMaxError > 0.0f && maxGraphErrors.linearRatio > _optimizationMaxError) { msg = uFormat("Rejecting edge %d->%d because " "graph error is too large after optimization (%f m for edge %d->%d with ratio %f > std=%f m). " "\"%s\" is %f.", link.from(), link.to(), maxGraphErrors.linear, maxGraphErrors.linearLink.from(), maxGraphErrors.linearLink.to(), maxGraphErrors.linearRatio, sqrt(maxGraphErrors.linearLink.transVariance()), Parameters::kRGBDOptimizeMaxError().c_str(), _optimizationMaxError); } else if(_optimizationMaxError == 0.0f && maxGraphErrors.linearRatio>100 && !_graphOptimizer->isRobust()) { UERROR("Huge optimization error detected!" "Linear error ratio of %f (edge %d->%d, type=%d, abs error=%f m, stddev=%f). You may consider " "enabling \"%s\" to reject those bad optimizations by setting it to a non null value!", maxGraphErrors.linearRatio, maxGraphErrors.linearLink.from(), maxGraphErrors.linearLink.to(), maxGraphErrors.linearLink.type(), maxGraphErrors.linear, sqrt(maxGraphErrors.linearLink.transVariance()), Parameters::kRGBDOptimizeMaxError().c_str()); } } else if(maxGraphErrors.angularLink.isValid()) { UINFO("Max optimization angular error = %f deg (link %d->%d)", maxGraphErrors.angular*180.0f/M_PI, maxGraphErrors.angularLink.from(), maxGraphErrors.angularLink.to()); if(_optimizationMaxError > 0.0f && maxGraphErrors.angularRatio > _optimizationMaxError) { msg = uFormat("Rejecting edge %d->%d because " "graph error is too large after optimization (%f deg for edge %d->%d with ratio %f > std=%f deg). " "\"%s\" is %f m.", link.from(), link.to(), maxGraphErrors.angular*180.0f/M_PI, maxGraphErrors.angularLink.from(), maxGraphErrors.angularLink.to(), maxGraphErrors.angularRatio, sqrt(maxGraphErrors.angularLink.rotVariance()), Parameters::kRGBDOptimizeMaxError().c_str(), _optimizationMaxError); } else if(_optimizationMaxError == 0.0f && maxGraphErrors.angularRatio>100 && !_graphOptimizer->isRobust()) { UERROR("Huge optimization error detected!" "Angular error ratio of %f (edge %d->%d, type=%d, abs error=%f m, stddev=%f). You may consider " "enabling \"%s\" to reject those bad optimizations by setting it to a non null value!", maxGraphErrors.angularRatio, maxGraphErrors.angularLink.from(), maxGraphErrors.angularLink.to(), maxGraphErrors.angularLink.type(), maxGraphErrors.angular*180.0f/CV_PI, sqrt(maxGraphErrors.angularLink.rotVariance()), Parameters::kRGBDOptimizeMaxError().c_str()); } } } if(!msg.empty()) { UERROR("%s", msg.c_str()); _memory->removeLink(link.from(), link.to()); return false; } // Update optimized poses for(std::map::iterator iter=_optimizedPoses.begin(); iter!=_optimizedPoses.end(); ++iter) { std::map::iterator jter = poses.find(iter->first); if(jter != poses.end()) { iter->second = jter->second; } } if(!_optimizeFromGraphEnd) { _mapCorrection = _optimizedPoses.rbegin()->second * _memory->getSignature(_optimizedPoses.rbegin()->first)->getPose().inverse(); } std::map tmp; // Update also the links if some have been added in WM _memory->getMetricConstraints(uKeysSet(_optimizedPoses), tmp, _constraints, false); // This will force rtabmap_ros to regenerate the global occupancy grid if there was one _memory->save2DMap(cv::Mat(), 0, 0, 0); return true; } else // localization mode { int oldestId = link.from()>link.to()?link.to():link.from(); int newestId = link.from()getSignature(oldestId) == 0) { UERROR("Link's id %d is not in working memory", oldestId); return false; } if(_optimizedPoses.find(oldestId) == _optimizedPoses.end()) { UERROR("Link's id %d is not in the optimized graph (_optimizedPoses=%d)", oldestId, (int)_optimizedPoses.size()); return false; } if(_optimizeFromGraphEnd) { UERROR("Adding link with %s=true in localization mode is not supported.", Parameters::kRGBDOptimizeFromGraphEnd().c_str()); return false; } if(_odomCachePoses.find(newestId) == _odomCachePoses.end()) { if(!_odomCachePoses.empty()) { UERROR("Link's id %d is not in the odometry cache (oldest=%d, newest=%d, %s=%d)", newestId, _odomCachePoses.begin()->first, _odomCachePoses.rbegin()->first, Parameters::kRGBDMaxOdomCacheSize().c_str(), _maxOdomCacheSize); } else { UERROR("Link's id %d is not in the odometry cache (%s=%d).", newestId, Parameters::kRGBDMaxOdomCacheSize().c_str(), _maxOdomCacheSize); } return false; } // Verify if the new localization is valid by checking if there is // not too much deformation using current odometry poses // This will also refine localization links std::map poses = _odomCachePoses; std::multimap constraints = _odomCacheConstraints; constraints.insert(std::make_pair(link.from(), link)); cv::Mat priorInfMat = cv::Mat::eye(6,6, CV_64FC1)*_localizationPriorInf; for(std::multimap::iterator iter=constraints.begin(); iter!=constraints.end(); ++iter) { std::map::iterator iterPose = _optimizedPoses.find(iter->second.to()); if(iterPose != _optimizedPoses.end() && poses.find(iterPose->first) == poses.end()) { poses.insert(*iterPose); // make the poses in the map fixed constraints.insert(std::make_pair(iterPose->first, Link(iterPose->first, iterPose->first, Link::kPosePrior, iterPose->second, priorInfMat))); } } std::map posesOut; std::multimap edgeConstraintsOut; bool priorsIgnored = _graphOptimizer->priorsIgnored(); _graphOptimizer->setPriorsIgnored(false); //temporary set false to use priors above to fix nodes of the map _graphOptimizer->getConnectedGraph(newestId, poses, constraints, posesOut, edgeConstraintsOut); std::map optPoses = _graphOptimizer->optimize(poses.begin()->first, posesOut, edgeConstraintsOut); _graphOptimizer->setPriorsIgnored(priorsIgnored); // set back bool rejectLocalization = false; if(optPoses.empty()) { UWARN("Optimization failed, rejecting localization!"); rejectLocalization = true; } else { UINFO("Compute max graph errors..."); graph::MaxGraphErrors maxGraphErrors = graph::computeMaxGraphErrors( optPoses, edgeConstraintsOut, _graphOptimizer->isSlam2d()); if(!maxGraphErrors.linearLink.isValid() && !maxGraphErrors.angularLink.isValid()) { UWARN("Could not compute graph errors! Wrong loop closures could be accepted!"); } if(maxGraphErrors.linearLink.isValid()) { UINFO("Max optimization linear error = %f m (link %d->%d, var=%f, ratio error/std=%f)", maxGraphErrors.linear, maxGraphErrors.linearLink.from(), maxGraphErrors.linearLink.to(), maxGraphErrors.linearLink.transVariance(), maxGraphErrors.linear/sqrt(maxGraphErrors.linearLink.transVariance())); if(_optimizationMaxError > 0.0f && maxGraphErrors.linearRatio > _optimizationMaxError) { UWARN("Rejecting localization (%d <-> %d) in this " "iteration because a wrong loop closure has been " "detected after graph optimization, resulting in " "a maximum graph error ratio of %f (edge %d->%d, type=%d, abs error=%f m, stddev=%f). The " "maximum error ratio parameter \"%s\" is %f of std deviation.", link.from(), link.to(), maxGraphErrors.linearRatio, maxGraphErrors.linearLink.from(), maxGraphErrors.linearLink.to(), maxGraphErrors.linearLink.type(), maxGraphErrors.linear, sqrt(maxGraphErrors.linearLink.transVariance()), Parameters::kRGBDOptimizeMaxError().c_str(), _optimizationMaxError); rejectLocalization = true; } else if(_optimizationMaxError == 0.0f && maxGraphErrors.linearRatio>100 && !_graphOptimizer->isRobust()) { UERROR("Huge optimization error detected!" "Linear error ratio of %f (edge %d->%d, type=%d, abs error=%f m, stddev=%f). You may consider " "enabling \"%s\" to reject those bad optimizations by setting it to a non null value!", maxGraphErrors.linearRatio, maxGraphErrors.linearLink.from(), maxGraphErrors.linearLink.to(), maxGraphErrors.linearLink.type(), maxGraphErrors.linear, sqrt(maxGraphErrors.linearLink.transVariance()), Parameters::kRGBDOptimizeMaxError().c_str()); } } if(maxGraphErrors.angularLink.isValid()) { UINFO("Max optimization angular error = %f deg (link %d->%d, var=%f, ratio error/std=%f)", maxGraphErrors.angular*180.0f/CV_PI, maxGraphErrors.angularLink.from(), maxGraphErrors.angularLink.to(), maxGraphErrors.angularLink.rotVariance(), maxGraphErrors.angular/sqrt(maxGraphErrors.angularLink.rotVariance())); if(_optimizationMaxError > 0.0f && maxGraphErrors.angularRatio > _optimizationMaxError) { UWARN("Rejecting localization (%d <-> %d) in this " "iteration because a wrong loop closure has been " "detected after graph optimization, resulting in " "a maximum graph error ratio of %f (edge %d->%d, type=%d, abs error=%f deg, stddev=%f). The " "maximum error ratio parameter \"%s\" is %f of std deviation.", link.from(), link.to(), maxGraphErrors.angularRatio, maxGraphErrors.angularLink.from(), maxGraphErrors.angularLink.to(), maxGraphErrors.angularLink.type(), maxGraphErrors.angular*180.0f/CV_PI, sqrt(maxGraphErrors.angularLink.rotVariance()), Parameters::kRGBDOptimizeMaxError().c_str(), _optimizationMaxError); rejectLocalization = true; } else if(_optimizationMaxError == 0.0f && maxGraphErrors.angularRatio>100 && !_graphOptimizer->isRobust()) { UERROR("Huge optimization error detected!" "Angular error ratio of %f (edge %d->%d, type=%d, abs error=%f m, stddev=%f). You may consider " "enabling \"%s\" to reject those bad optimizations by setting it to a non null value!", maxGraphErrors.angularRatio, maxGraphErrors.angularLink.from(), maxGraphErrors.angularLink.to(), maxGraphErrors.angularLink.type(), maxGraphErrors.angular*180.0f/CV_PI, sqrt(maxGraphErrors.angularLink.rotVariance()), Parameters::kRGBDOptimizeMaxError().c_str()); } } } if(!rejectLocalization) { Transform newOptPoseInv = optPoses.at(link.from()).inverse(); Transform newT = newOptPoseInv * optPoses.at(link.to()); Link linkTmp = link; linkTmp.setTransform(newT); if(oldestId == link.from()) { _lastLocalizationPose = _optimizedPoses.at(link.from()) * linkTmp.transform(); _odomCacheConstraints.insert(std::make_pair(linkTmp.to(), linkTmp.inverse())); } else { _lastLocalizationPose = _optimizedPoses.at(link.to()) * linkTmp.transform().inverse(); _odomCacheConstraints.insert(std::make_pair(linkTmp.from(), linkTmp)); } UINFO("Set _lastLocalizationPose=%s", _lastLocalizationPose.prettyPrint().c_str()); if(_graphOptimizer->isSlam2d()) { // transform constraint to 2D _lastLocalizationPose = _lastLocalizationPose.to3DoF(); } Transform odomPose = _odomCachePoses.find(newestId)->second; _mapCorrection = _lastLocalizationPose * odomPose.inverse(); _lastLocalizationNodeId = oldestId; return true; } } return false; } cv::Mat Rtabmap::getInformation(const cv::Mat & covariance) const { cv::Mat information = covariance.inv(); if(_loopCovLimited) { const std::vector & odomMaxInf = _memory->getOdomMaxInf(); if(odomMaxInf.size() == 6) { for(int i=0; i<6; ++i) { if(information.at(i,i) > odomMaxInf[i]) { information.at(i,i) = odomMaxInf[i]; } } } } return information; } void Rtabmap::addNodesToRepublish(const std::vector & ids) { if(ids.empty()) { _nodesToRepublish.clear(); } else if(_maxRepublished > 0 && _publishLastSignatureData) { _nodesToRepublish.insert(ids.begin(), ids.end()); } else if(_maxRepublished == 0) { UWARN("%s=0, so cannot republish the %d requested nodes.", Parameters::kRtabmapMaxRepublished().c_str(), (int)ids.size()); } else //_publishLastSignatureData=false { UWARN("%s=false, so cannot republish the %d requested nodes.", Parameters::kRtabmapPublishLastSignature().c_str(), (int)ids.size()); } } void Rtabmap::setDummyDictionary(bool enabled) { if(_memory && enabled) { UERROR("Memory is already initialized, cannot set dummy dictionary. This " "function can only be called after Rtabmap object is created, but " "before init() is called."); } else { _dummyDictionary = enabled; } } void Rtabmap::clearPath(int status) { UINFO("status=%d", status); _pathStatus = status; _path.clear(); _pathCurrentIndex=0; _pathGoalIndex = 0; _pathTransformToGoal.setIdentity(); _pathUnreachableNodes.clear(); _pathStuckCount = 0; _pathStuckDistance = 0.0f; if(_memory) { _memory->removeAllVirtualLinks(); } } // return true if path is updated bool Rtabmap::computePath(int targetNode, bool global) { this->clearPath(0); if(targetNode>0) { UINFO("Planning a path to node %d (global=%d)", targetNode, global?1:0); } else { UINFO("Planning a path to landmark %d (global=%d)", -targetNode, global?1:0); } if(!_rgbdSlamMode) { UWARN("A path can only be computed in RGBD-SLAM mode"); return false; } UTimer totalTimer; UTimer timer; Transform transformToLandmark = Transform::getIdentity(); // No need to optimize the graph if(_memory) { int currentNode = 0; if(_memory->isIncremental()) { if(!_memory->getLastWorkingSignature(true)) { UWARN("Working memory is empty... cannot compute a path"); return false; } currentNode = _memory->getLastWorkingSignature(true)->id(); } else { if(_lastLocalizationPose.isNull() || _optimizedPoses.empty()) { UWARN("Last localization pose is null or optimized graph is empty... cannot compute a path"); return false; } if(_optimizedPoses.begin()->first < 0) { std::map poses(_optimizedPoses.lower_bound(1), _optimizedPoses.end()); currentNode = graph::findNearestNode(poses, _lastLocalizationPose); } else { currentNode = graph::findNearestNode(_optimizedPoses, _lastLocalizationPose); } } if(currentNode && targetNode) { std::list > path = graph::computePath( currentNode, targetNode, _memory, global, false, _pathLinearVelocity, _pathAngularVelocity); //transform in current referential Transform t = uValue(_optimizedPoses, currentNode, Transform::getIdentity()); _path.resize(path.size()); int oi = 0; for(std::list >::iterator iter=path.begin(); iter!=path.end();++iter) { if(iter->first > 0) { // Skip intermediate nodes (weight==-1). They are not navigable // waypoints and updateGoalIndex would otherwise abort the // plan when it sees them. The poses of the remaining real // nodes already account for cumulative transform through any // intermediate chain (relative poses from graph::computePath). int weight = 0; const Signature * s = _memory->getSignature(iter->first); if(s) { weight = s->getWeight(); } else { // For nodes in LTM, fetch weight from the database. Transform p, gt; int mapId = 0; std::string label; double stamp = 0.0; std::vector vel; GPS gps; EnvSensors envs; _memory->getNodeInfo(iter->first, p, mapId, weight, label, stamp, gt, vel, gps, envs, true); } if(weight == -1) { continue; } // just keep nodes in the path _path[oi].first = iter->first; _path[oi++].second = t * iter->second; } } _path.resize(oi); if(!_path.empty() && !path.empty() && path.rbegin()->first < 0) { transformToLandmark = _path.back().second.inverse() * t * path.rbegin()->second; } } else if(currentNode == 0) { UWARN("We should be localized before planning."); } } UINFO("Total planning time = %fs (%d nodes, %f m long)", totalTimer.ticks(), (int)_path.size(), graph::computePathLength(_path)); if(_path.size() == 0) { _path.clear(); UWARN("Cannot compute a path!"); return false; } else { UINFO("Path generated! Size=%d", (int)_path.size()); if(ULogger::level() == ULogger::kInfo) { std::stringstream stream; for(unsigned int i=0; i<_path.size(); ++i) { stream << _path[i].first; if(i+1 < _path.size()) { stream << " "; } } UINFO("Path = [%s]", stream.str().c_str()); } if(_goalsSavedInUserData) { // set goal to latest signature std::string goalStr = uFormat("GOAL:%d", targetNode); // use label is exist if(_memory->getSignature(targetNode)) { if(!_memory->getSignature(targetNode)->getLabel().empty()) { goalStr = std::string("GOAL:")+_memory->getSignature(targetNode)->getLabel(); } } else if(global) { std::map labels = _memory->getAllLabels(); std::map::iterator iter = labels.find(targetNode); if(iter != labels.end() && !iter->second.empty()) { goalStr = std::string("GOAL:")+labels.at(targetNode); } } setUserData(0, cv::Mat(1, int(goalStr.size()+1), CV_8SC1, (void *)goalStr.c_str()).clone()); } _pathTransformToGoal = transformToLandmark; updateGoalIndex(); return _path.size() || _pathStatus > 0; } return false; } bool Rtabmap::computePath(const Transform & targetPose, float tolerance) { this->clearPath(0); UINFO("Planning a path to pose %s ", targetPose.prettyPrint().c_str()); if(tolerance < 0.0f) { tolerance = _localRadius; } std::list > pathPoses; if(!_rgbdSlamMode) { UWARN("This method can only be used in RGBD-SLAM mode"); return false; } //Find the nearest node UTimer timer; std::map nodes = _optimizedPoses; std::multimap links; for(std::map::iterator iter=nodes.upper_bound(0); iter!=nodes.end(); ++iter) { const Signature * s = _memory->getSignature(iter->first); UASSERT(s); for(std::map::const_iterator jter=s->getLinks().begin(); jter!=s->getLinks().end(); ++jter) { // only add links for which poses are in "nodes" if(jter->second.from() != jter->second.to() && uContains(nodes, jter->second.to())) { links.insert(std::make_pair(jter->second.from(), jter->second.to())); //links.insert(std::make_pair(jter->second.to(), jter->second.from())); // <-> (commented: already added when iterating in nodes) } } } UINFO("Time getting links = %fs", timer.ticks()); int currentNode = 0; if(_memory->isIncremental()) { if(!_memory->getLastWorkingSignature(true)) { UWARN("Working memory is empty... cannot compute a path"); return false; } currentNode = _memory->getLastWorkingSignature(true)->id(); } else { if(_lastLocalizationPose.isNull() || _optimizedPoses.empty()) { UWARN("Last localization pose is null... cannot compute a path"); return false; } if(_optimizedPoses.begin()->first < 0) { std::map poses(_optimizedPoses.lower_bound(1), _optimizedPoses.end()); currentNode = graph::findNearestNode(poses, _lastLocalizationPose); } else { currentNode = graph::findNearestNode(_optimizedPoses, _lastLocalizationPose); } } int nearestId; if(!_lastLocalizationPose.isNull() && _lastLocalizationPose.getDistance(targetPose) < tolerance) { // target can be reached from the current node nearestId = currentNode; } else { nearestId = rtabmap::graph::findNearestNode(nodes, targetPose); } UINFO("Nearest node found=%d ,%fs", nearestId, timer.ticks()); if(nearestId > 0) { if(tolerance != 0.0f && targetPose.getDistance(nodes.at(nearestId)) > tolerance) { UWARN("Cannot plan farther than %f m from the graph! (distance=%f m from node %d)", tolerance, targetPose.getDistance(nodes.at(nearestId)), nearestId); } else { UINFO("Computing path from location %d to %d", currentNode, nearestId); UTimer timer; _path = uListToVector(rtabmap::graph::computePath(nodes, links, currentNode, nearestId)); UINFO("A* time = %fs", timer.ticks()); if(_path.size() == 0) { UWARN("Cannot compute a path!"); } else { UINFO("Path generated! Size=%d", (int)_path.size()); if(ULogger::level() == ULogger::kInfo) { std::stringstream stream; for(unsigned int i=0; i<_path.size(); ++i) { stream << _path[i].first; if(i+1 < _path.size()) { stream << " "; } } UINFO("Path = [%s]", stream.str().c_str()); } UASSERT(uContains(nodes, _path.back().first)); _pathTransformToGoal = nodes.at(_path.back().first).inverse() * targetPose; updateGoalIndex(); return true; } } } else { UWARN("Nearest node not found in graph (size=%d) for pose %s", (int)nodes.size(), targetPose.prettyPrint().c_str()); } return false; } std::vector > Rtabmap::getPathNextPoses() const { std::vector > poses; if(_path.size()) { UASSERT(_pathCurrentIndex < _path.size() && _pathGoalIndex < _path.size()); poses.resize(_pathGoalIndex-_pathCurrentIndex+1); int oi=0; for(unsigned int i=_pathCurrentIndex; i<=_pathGoalIndex; ++i) { std::map::const_iterator iter = _optimizedPoses.find(_path[i].first); if(iter != _optimizedPoses.end()) { poses[oi++] = *iter; } else { break; } } poses.resize(oi); } return poses; } std::vector Rtabmap::getPathNextNodes() const { std::vector ids; if(_path.size()) { UASSERT(_pathCurrentIndex < _path.size() && _pathGoalIndex < _path.size()); ids.resize(_pathGoalIndex-_pathCurrentIndex+1); int oi = 0; for(unsigned int i=_pathCurrentIndex; i<=_pathGoalIndex; ++i) { std::map::const_iterator iter = _optimizedPoses.find(_path[i].first); if(iter != _optimizedPoses.end()) { ids[oi++] = iter->first; } else { break; } } ids.resize(oi); } return ids; } int Rtabmap::getPathCurrentGoalId() const { if(_path.size()) { UASSERT(_pathGoalIndex <= _path.size()); return _path[_pathGoalIndex].first; } return 0; } void Rtabmap::updateGoalIndex() { if(!_rgbdSlamMode) { UWARN("This method can on be used in RGBD-SLAM mode!"); return; } if( _memory && _path.size()) { // remove all previous virtual links for(unsigned int i=0; i<_pathCurrentIndex && i<_path.size(); ++i) { const Signature * s = _memory->getSignature(_path[i].first); if(s) { UASSERT_MSG(s->getWeight() != -1, uFormat("path[%u] id=%d is intermediate; computePath should have filtered it", i, _path[i].first).c_str()); _memory->removeVirtualLinks(s->id()); } } // for the current index, only keep the newest virtual link // This will make sure that the path is still connected even // if the new signature is removed (e.g., because of a small displacement) UASSERT(_pathCurrentIndex < _path.size()); const Signature * currentIndexS = _memory->getSignature(_path[_pathCurrentIndex].first); UASSERT_MSG(currentIndexS != 0, uFormat("_path[%d].first=%d", _pathCurrentIndex, _path[_pathCurrentIndex].first).c_str()); std::multimap links = currentIndexS->getLinks(); // make a copy bool latestVirtualLinkFound = false; for(std::multimap::reverse_iterator iter=links.rbegin(); iter!=links.rend(); ++iter) { if(iter->second.type() == Link::kVirtualClosure) { if(latestVirtualLinkFound) { _memory->removeLink(currentIndexS->id(), iter->first); } else { latestVirtualLinkFound = true; } } } // Make sure the next signatures on the path are linked together. // Intermediate nodes have been filtered out of _path by computePath, so // every entry is a real node here. float distanceSoFar = 0.0f; for(unsigned int i=_pathCurrentIndex+1; i<_path.size(); ++i) { if(_localRadius > 0.0f) { distanceSoFar += _path[i-1].second.getDistance(_path[i].second); } if(_path[i].first != _path[i-1].first) { const Signature * s = _memory->getSignature(_path[i].first); if(s) { UASSERT_MSG(s->getWeight() != -1, uFormat("path[%u] id=%d is intermediate; computePath should have filtered it", i, _path[i].first).c_str()); const Signature * sPrev = _memory->getSignature(_path[i-1].first); if(sPrev) { UASSERT_MSG(sPrev->getWeight() != -1, uFormat("path[%u] id=%d is intermediate; computePath should have filtered it", i-1, _path[i-1].first).c_str()); } if(!s->hasLink(_path[i-1].first) && sPrev != 0) { Transform virtualLoop = _path[i].second.inverse() * _path[i-1].second; _memory->addLink(Link(_path[i].first, _path[i-1].first, Link::kVirtualClosure, virtualLoop, cv::Mat::eye(6,6,CV_64FC1)*0.01)); // on the optimized path UINFO("Added Virtual link between %d and %d", _path[i-1].first, _path[i].first); } } } if(distanceSoFar > _localRadius) { UDEBUG("Farthest goal=%d : %f m", _path[i].first, distanceSoFar); break; } } UDEBUG("current node = %d current goal = %d", _path[_pathCurrentIndex].first, _path[_pathGoalIndex].first); Transform currentPose; if(_memory->isIncremental()) { if(_memory->getLastWorkingSignature(true) == 0 || !uContains(_optimizedPoses, _memory->getLastWorkingSignature(true)->id())) { UERROR("Last node is null in memory or not in optimized poses. Aborting the plan..."); this->clearPath(-1); return; } currentPose = _optimizedPoses.at(_memory->getLastWorkingSignature(true)->id()); } else { if(_lastLocalizationPose.isNull()) { UERROR("Last localization pose is null. Aborting the plan..."); this->clearPath(-1); return; } currentPose = _lastLocalizationPose; } int goalId = _path.back().first; if(uContains(_optimizedPoses, goalId)) { //use local position to know if the goal is reached float d = currentPose.getDistance(_optimizedPoses.at(goalId)*_pathTransformToGoal); if(d < _goalReachedRadius) { UINFO("Goal %d reached!", goalId); this->clearPath(1); } } if(_path.size()) { //Always check if the farthest node is accessible in local map (max to local space radius if set) unsigned int goalIndex = _pathCurrentIndex; float distanceFromCurrentNode = 0.0f; bool sameGoalIndex = false; for(unsigned int i=_pathCurrentIndex+1; i<_path.size(); ++i) { if(uContains(_optimizedPoses, _path[i].first)) { if(_localRadius > 0.0f) { distanceFromCurrentNode = _path[_pathCurrentIndex].second.getDistance(_path[i].second); } if((goalIndex == _pathCurrentIndex && i == _path.size()-1) || _pathUnreachableNodes.find(i) == _pathUnreachableNodes.end()) { goalIndex = i; if(distanceFromCurrentNode > _localRadius) { break; } } } else { break; } } UASSERT(_pathGoalIndex < _path.size() && goalIndex < _path.size()); if(_pathGoalIndex != goalIndex) { UINFO("Updated current goal from %d to %d (%d/%d)", (int)_path[_pathGoalIndex].first, _path[goalIndex].first, (int)goalIndex+1, (int)_path.size()); _pathGoalIndex = goalIndex; } else { sameGoalIndex = true; } // update nearest pose in the path unsigned int nearestNodeIndex = 0; float distance = -1.0f; bool sameCurrentIndex = false; UASSERT(_pathGoalIndex < _path.size()); for(unsigned int i=_pathCurrentIndex; i<=_pathGoalIndex; ++i) { std::map::iterator iter = _optimizedPoses.find(_path[i].first); if(iter != _optimizedPoses.end()) { float d = currentPose.getDistanceSquared(iter->second); if(distance == -1.0f || distance > d) { distance = d; nearestNodeIndex = i; } } } if(distance < 0) { UERROR("The nearest pose on the path not found! Aborting the plan..."); this->clearPath(-1); } else { UDEBUG("Nearest node = %d", _path[nearestNodeIndex].first); } if(distance >= 0 && nearestNodeIndex != _pathCurrentIndex) { _pathCurrentIndex = nearestNodeIndex; _pathUnreachableNodes.erase(nearestNodeIndex); // if we are on it, it is reachable } else { sameCurrentIndex = true; } bool isStuck = false; if(sameGoalIndex && sameCurrentIndex && _pathStuckIterations>0) { float distanceToCurrentGoal = 0.0f; std::map::iterator iter = _optimizedPoses.find(_path[_pathGoalIndex].first); if(iter != _optimizedPoses.end()) { if(_pathGoalIndex == _pathCurrentIndex && _pathGoalIndex == _path.size()-1) { distanceToCurrentGoal = currentPose.getDistanceSquared(iter->second*_pathTransformToGoal); } else { distanceToCurrentGoal = currentPose.getDistanceSquared(iter->second); } } if(distanceToCurrentGoal > 0.0f) { if(distanceToCurrentGoal >= _pathStuckDistance) { // we are not approaching the goal isStuck = true; if(_pathStuckDistance == 0.0f) { _pathStuckDistance = distanceToCurrentGoal; } } } else { // no nodes available, cannot plan isStuck = true; } } if(isStuck && ++_pathStuckCount > _pathStuckIterations) { UWARN("Current goal %d not reached since %d iterations (\"RGBD/PlanStuckIterations\"=%d), mark that node as unreachable.", _path[_pathGoalIndex].first, _pathStuckCount, _pathStuckIterations); _pathStuckCount = 0; _pathStuckDistance = 0.0; _pathUnreachableNodes.insert(_pathGoalIndex); // select previous reachable one while(_pathUnreachableNodes.find(_pathGoalIndex) != _pathUnreachableNodes.end()) { if(_pathGoalIndex == 0 || --_pathGoalIndex <= _pathCurrentIndex) { // plan failed! UERROR("No upcoming nodes on the path are reachable! Aborting the plan..."); this->clearPath(-1); return; } } } else if(!isStuck) { _pathStuckCount = 0; _pathStuckDistance = 0.0; } } } } void Rtabmap::createGlobalScanMap() { UDEBUG("Creating global scan map (if scans are available)"); _globalScanMap.clear(); _globalScanMapPoses.clear(); std::vector scanIndices; std::map scanViewpoints; for(std::map::iterator iter=_optimizedPoses.begin(); iter!=_optimizedPoses.end(); ++iter) { SensorData data = _memory->getNodeData(iter->first, false, true, false, false); if(!data.laserScanCompressed().empty()) { LaserScan scan; data.uncompressDataConst(0, 0, &scan, 0, 0, 0, 0); if(!scan.empty()) { UDEBUG("Adding scan %d (format=%s, points=%d)", iter->first, scan.formatName().c_str(), scan.size()); scan = util3d::transformLaserScan(scan, iter->second*scan.localTransform()); if(_globalScanMap.empty() || _globalScanMap.format() == scan.format()) { _globalScanMap += scan; _globalScanMapPoses.insert(*iter); scanViewpoints.insert(std::make_pair(iter->first, iter->second * scan.localTransform())); scanIndices.resize(_globalScanMap.size(), iter->first); } else { UWARN("Incompatible scan formats (%s vs %s), cannot create global scan map.", _globalScanMap.formatName().c_str(), scan.formatName().c_str()); _globalScanMap.clear(); _globalScanMapPoses.clear(); break; } } else { UDEBUG("Ignored %d (scan is empty), pose still added.", iter->first); _globalScanMapPoses.insert(*iter); } } else { UDEBUG("Ignored %d (no scan), pose still added.", iter->first); _globalScanMapPoses.insert(*iter); } } if(_globalScanMap.size() > 3) { float voxelSize = 0.0f; int normalK = 0; float normalRadius = 0.0f; Parameters::parse(_parameters, Parameters::kMemLaserScanVoxelSize(), voxelSize); Parameters::parse(_parameters, Parameters::kMemLaserScanNormalK(), normalK); Parameters::parse(_parameters, Parameters::kMemLaserScanNormalRadius(), normalRadius); if(voxelSize > 0.0f) { LaserScan voxelScan = util3d::commonFiltering(_globalScanMap, 1, 0, 0, voxelSize, normalK, normalRadius); if(voxelScan.hasNormals()) { // adjust with point of views util3d::adjustNormalsToViewPoints( scanViewpoints, _globalScanMap, scanIndices, voxelScan); } _globalScanMap = voxelScan; } UINFO("Global scan map has been assembled (size=%d points, %d poses) " "for proximity detection (only in localization mode %s=false and with %s=true)", (int)_globalScanMap.size(), (int)_globalScanMapPoses.size(), Parameters::kMemIncrementalMemory().c_str(), Parameters::kRGBDProximityGlobalScanMap().c_str()); //for debugging... if(!_globalScanMap.empty() && ULogger::level() == ULogger::kDebug) { if(!_wDir.empty()) { UWARN("Saving %s/rtabmap_global_scan_map.pcd (only saved when logger level is debug)", _wDir.c_str()); pcl::PCLPointCloud2::Ptr cloud2 = util3d::laserScanToPointCloud2(_globalScanMap); pcl::io::savePCDFile(_wDir+"/rtabmap_global_scan_map.pcd", *cloud2); } else { UWARN("%s is enabled and logger is debug, but %s is not set, cannot save global scan map for debugging.", Parameters::kRGBDProximityGlobalScanMap().c_str(), Parameters::kRtabmapWorkingDirectory().c_str()); } } } if(!_globalScanMap.empty() && _globalScanMap.size()<100) { UWARN("Ignoring global scan map because it is too small (%d points).", (int)_globalScanMap.size()); _globalScanMap.clear(); _globalScanMapPoses.clear(); } } } // namespace rtabmap