00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028 #include "rtabmap/core/Rtabmap.h"
00029 #include "rtabmap/core/Version.h"
00030 #include "rtabmap/core/Features2d.h"
00031 #include "rtabmap/core/Optimizer.h"
00032 #include "rtabmap/core/Graph.h"
00033 #include "rtabmap/core/Signature.h"
00034
00035 #include "rtabmap/core/EpipolarGeometry.h"
00036
00037 #include "rtabmap/core/Memory.h"
00038 #include "rtabmap/core/VWDictionary.h"
00039 #include "rtabmap/core/BayesFilter.h"
00040 #include "rtabmap/core/Compression.h"
00041 #include "rtabmap/core/RegistrationInfo.h"
00042
00043 #include <rtabmap/utilite/ULogger.h>
00044 #include <rtabmap/utilite/UFile.h>
00045 #include <rtabmap/utilite/UTimer.h>
00046 #include <rtabmap/utilite/UConversion.h>
00047 #include <rtabmap/utilite/UMath.h>
00048
00049 #include <pcl/search/kdtree.h>
00050 #include <pcl/filters/crop_box.h>
00051 #include <pcl/io/pcd_io.h>
00052
00053 #include <stdlib.h>
00054 #include <set>
00055
00056 #define LOG_F "LogF.txt"
00057 #define LOG_I "LogI.txt"
00058
00059 #define GRAPH_FILE_NAME "Graph.dot"
00060
00061
00062
00063
00064
00065
00066
00067
00068
00069
00070
00071
00072 namespace rtabmap
00073 {
00074
00075 Rtabmap::Rtabmap() :
00076 _publishStats(Parameters::defaultRtabmapPublishStats()),
00077 _publishLastSignatureData(Parameters::defaultRtabmapPublishLastSignature()),
00078 _publishPdf(Parameters::defaultRtabmapPublishPdf()),
00079 _publishLikelihood(Parameters::defaultRtabmapPublishLikelihood()),
00080 _maxTimeAllowed(Parameters::defaultRtabmapTimeThr()),
00081 _maxMemoryAllowed(Parameters::defaultRtabmapMemoryThr()),
00082 _loopThr(Parameters::defaultRtabmapLoopThr()),
00083 _loopRatio(Parameters::defaultRtabmapLoopRatio()),
00084 _maxRetrieved(Parameters::defaultRtabmapMaxRetrieved()),
00085 _maxLocalRetrieved(Parameters::defaultRGBDMaxLocalRetrieved()),
00086 _rawDataKept(Parameters::defaultMemImageKept()),
00087 _statisticLogsBufferedInRAM(Parameters::defaultRtabmapStatisticLogsBufferedInRAM()),
00088 _statisticLogged(Parameters::defaultRtabmapStatisticLogged()),
00089 _statisticLoggedHeaders(Parameters::defaultRtabmapStatisticLoggedHeaders()),
00090 _rgbdSlamMode(Parameters::defaultRGBDEnabled()),
00091 _rgbdLinearUpdate(Parameters::defaultRGBDLinearUpdate()),
00092 _rgbdAngularUpdate(Parameters::defaultRGBDAngularUpdate()),
00093 _newMapOdomChangeDistance(Parameters::defaultRGBDNewMapOdomChangeDistance()),
00094 _neighborLinkRefining(Parameters::defaultRGBDNeighborLinkRefining()),
00095 _proximityByTime(Parameters::defaultRGBDProximityByTime()),
00096 _proximityBySpace(Parameters::defaultRGBDProximityBySpace()),
00097 _scanMatchingIdsSavedInLinks(Parameters::defaultRGBDScanMatchingIdsSavedInLinks()),
00098 _localRadius(Parameters::defaultRGBDLocalRadius()),
00099 _localImmunizationRatio(Parameters::defaultRGBDLocalImmunizationRatio()),
00100 _proximityMaxGraphDepth(Parameters::defaultRGBDProximityMaxGraphDepth()),
00101 _proximityFilteringRadius(Parameters::defaultRGBDProximityPathFilteringRadius()),
00102 _proximityRawPosesUsed(Parameters::defaultRGBDProximityPathRawPosesUsed()),
00103 _proximityAngle(Parameters::defaultRGBDProximityAngle()*M_PI/180.0f),
00104 _databasePath(""),
00105 _optimizeFromGraphEnd(Parameters::defaultRGBDOptimizeFromGraphEnd()),
00106 _optimizationMaxLinearError(Parameters::defaultRGBDOptimizeMaxError()),
00107 _startNewMapOnLoopClosure(Parameters::defaultRtabmapStartNewMapOnLoopClosure()),
00108 _goalReachedRadius(Parameters::defaultRGBDGoalReachedRadius()),
00109 _goalsSavedInUserData(Parameters::defaultRGBDGoalsSavedInUserData()),
00110 _pathStuckIterations(Parameters::defaultRGBDPlanStuckIterations()),
00111 _pathLinearVelocity(Parameters::defaultRGBDPlanLinearVelocity()),
00112 _pathAngularVelocity(Parameters::defaultRGBDPlanAngularVelocity()),
00113 _loopClosureHypothesis(0,0.0f),
00114 _highestHypothesis(0,0.0f),
00115 _lastProcessTime(0.0),
00116 _someNodesHaveBeenTransferred(false),
00117 _distanceTravelled(0.0f),
00118 _epipolarGeometry(0),
00119 _bayesFilter(0),
00120 _graphOptimizer(0),
00121 _memory(0),
00122 _foutFloat(0),
00123 _foutInt(0),
00124 _wDir(""),
00125 _mapCorrection(Transform::getIdentity()),
00126 _lastLocalizationNodeId(0),
00127 _pathStatus(0),
00128 _pathCurrentIndex(0),
00129 _pathGoalIndex(0),
00130 _pathTransformToGoal(Transform::getIdentity()),
00131 _pathStuckCount(0),
00132 _pathStuckDistance(0.0f)
00133 {
00134 }
00135
00136 Rtabmap::~Rtabmap() {
00137 UDEBUG("");
00138 this->close();
00139 }
00140
00141 void Rtabmap::setupLogFiles(bool overwrite)
00142 {
00143 flushStatisticLogs();
00144
00145 if(_foutFloat)
00146 {
00147 fclose(_foutFloat);
00148 _foutFloat = 0;
00149 }
00150 if(_foutInt)
00151 {
00152 fclose(_foutInt);
00153 _foutInt = 0;
00154 }
00155
00156 if(_statisticLogged && !_wDir.empty())
00157 {
00158 std::string attributes = "a+";
00159 if(overwrite)
00160 {
00161
00162
00163
00164 attributes = "w";
00165 }
00166
00167 bool addLogFHeader = overwrite || !UFile::exists(_wDir+"/"+LOG_F);
00168 bool addLogIHeader = overwrite || !UFile::exists(_wDir+"/"+LOG_I);
00169
00170 #ifdef _MSC_VER
00171 fopen_s(&_foutFloat, (_wDir+"/"+LOG_F).c_str(), attributes.c_str());
00172 fopen_s(&_foutInt, (_wDir+"/"+LOG_I).c_str(), attributes.c_str());
00173 #else
00174 _foutFloat = fopen((_wDir+"/"+LOG_F).c_str(), attributes.c_str());
00175 _foutInt = fopen((_wDir+"/"+LOG_I).c_str(), attributes.c_str());
00176 #endif
00177
00178 if(_statisticLoggedHeaders && addLogFHeader && _foutFloat)
00179 {
00180 fprintf(_foutFloat, "Column headers:\n");
00181 fprintf(_foutFloat, " 1-Total iteration time (s)\n");
00182 fprintf(_foutFloat, " 2-Memory update time (s)\n");
00183 fprintf(_foutFloat, " 3-Retrieval time (s)\n");
00184 fprintf(_foutFloat, " 4-Likelihood time (s)\n");
00185 fprintf(_foutFloat, " 5-Posterior time (s)\n");
00186 fprintf(_foutFloat, " 6-Hypothesis selection time (s)\n");
00187 fprintf(_foutFloat, " 7-Hypothesis validation time (s)\n");
00188 fprintf(_foutFloat, " 8-Transfer time (s)\n");
00189 fprintf(_foutFloat, " 9-Statistics creation time (s)\n");
00190 fprintf(_foutFloat, " 10-Loop closure hypothesis value\n");
00191 fprintf(_foutFloat, " 11-NAN\n");
00192 fprintf(_foutFloat, " 12-NAN\n");
00193 fprintf(_foutFloat, " 13-NAN\n");
00194 fprintf(_foutFloat, " 14-NAN\n");
00195 fprintf(_foutFloat, " 15-NAN\n");
00196 fprintf(_foutFloat, " 16-Virtual place hypothesis\n");
00197 fprintf(_foutFloat, " 17-Join trash time (s)\n");
00198 fprintf(_foutFloat, " 18-Weight Update (rehearsal) similarity\n");
00199 fprintf(_foutFloat, " 19-Empty trash time (s)\n");
00200 fprintf(_foutFloat, " 20-Retrieval database access time (s)\n");
00201 fprintf(_foutFloat, " 21-Add loop closure link time (s)\n");
00202 fprintf(_foutFloat, " 22-Memory cleanup time (s)\n");
00203 fprintf(_foutFloat, " 23-Scan matching (odometry correction) time (s)\n");
00204 fprintf(_foutFloat, " 24-Local time loop closure detection time (s)\n");
00205 fprintf(_foutFloat, " 25-Local space loop closure detection time (s)\n");
00206 fprintf(_foutFloat, " 26-Map optimization (s)\n");
00207 }
00208 if(_statisticLoggedHeaders && addLogIHeader && _foutInt)
00209 {
00210 fprintf(_foutInt, "Column headers:\n");
00211 fprintf(_foutInt, " 1-Loop closure ID\n");
00212 fprintf(_foutInt, " 2-Highest loop closure hypothesis\n");
00213 fprintf(_foutInt, " 3-Locations transferred\n");
00214 fprintf(_foutInt, " 4-NAN\n");
00215 fprintf(_foutInt, " 5-Words extracted from the last image\n");
00216 fprintf(_foutInt, " 6-Vocabulary size\n");
00217 fprintf(_foutInt, " 7-Working memory size\n");
00218 fprintf(_foutInt, " 8-Is loop closure hypothesis rejected?\n");
00219 fprintf(_foutInt, " 9-NAN\n");
00220 fprintf(_foutInt, " 10-NAN\n");
00221 fprintf(_foutInt, " 11-Locations retrieved\n");
00222 fprintf(_foutInt, " 12-Retrieval location ID\n");
00223 fprintf(_foutInt, " 13-Unique words extraced from last image\n");
00224 fprintf(_foutInt, " 14-Retrieval ID\n");
00225 fprintf(_foutInt, " 15-Non-null likelihood values\n");
00226 fprintf(_foutInt, " 16-Weight Update ID\n");
00227 fprintf(_foutInt, " 17-Is last location merged through Weight Update?\n");
00228 fprintf(_foutInt, " 18-Local graph size\n");
00229 fprintf(_foutInt, " 19-Sensor data id\n");
00230 fprintf(_foutInt, " 20-Indexed words\n");
00231 fprintf(_foutInt, " 21-Index memory usage (KB)\n");
00232 }
00233
00234 ULOGGER_DEBUG("Log file (int)=%s", (_wDir+"/"+LOG_I).c_str());
00235 ULOGGER_DEBUG("Log file (float)=%s", (_wDir+"/"+LOG_F).c_str());
00236 }
00237 else
00238 {
00239 if(_statisticLogged)
00240 {
00241 UWARN("Working directory is not set, log disabled!");
00242 }
00243 UDEBUG("Log disabled!");
00244 }
00245 }
00246
00247 void Rtabmap::flushStatisticLogs()
00248 {
00249 if(_foutFloat && _bufferedLogsF.size())
00250 {
00251 UDEBUG("_bufferedLogsF.size=%d", _bufferedLogsF.size());
00252 for(std::list<std::string>::iterator iter = _bufferedLogsF.begin(); iter!=_bufferedLogsF.end(); ++iter)
00253 {
00254 fprintf(_foutFloat, "%s", iter->c_str());
00255 }
00256 _bufferedLogsF.clear();
00257 }
00258 if(_foutInt && _bufferedLogsI.size())
00259 {
00260 UDEBUG("_bufferedLogsI.size=%d", _bufferedLogsI.size());
00261 for(std::list<std::string>::iterator iter = _bufferedLogsI.begin(); iter!=_bufferedLogsI.end(); ++iter)
00262 {
00263 fprintf(_foutInt, "%s", iter->c_str());
00264 }
00265 _bufferedLogsI.clear();
00266 }
00267 }
00268
00269 void Rtabmap::init(const ParametersMap & parameters, const std::string & databasePath)
00270 {
00271 ParametersMap::const_iterator iter;
00272 if((iter=parameters.find(Parameters::kRtabmapWorkingDirectory())) != parameters.end())
00273 {
00274 this->setWorkingDirectory(iter->second.c_str());
00275 }
00276
00277 _databasePath = databasePath;
00278 if(!_databasePath.empty())
00279 {
00280 UASSERT(UFile::getExtension(_databasePath).compare("db") == 0);
00281 UINFO("Using database \"%s\".", _databasePath.c_str());
00282 }
00283 else
00284 {
00285 UWARN("Using empty database. Mapping session will not be saved.");
00286 }
00287
00288 bool newDatabase = _databasePath.empty() || !UFile::exists(_databasePath);
00289
00290
00291 if(!_memory)
00292 {
00293 _memory = new Memory(parameters);
00294 _memory->init(_databasePath, false, parameters, true);
00295 }
00296
00297
00298 this->parseParameters(parameters);
00299
00300 if(_databasePath.empty())
00301 {
00302 _statisticLogged = false;
00303 }
00304 setupLogFiles(newDatabase);
00305 }
00306
00307 void Rtabmap::init(const std::string & configFile, const std::string & databasePath)
00308 {
00309
00310 ParametersMap param;
00311
00312 if(!configFile.empty())
00313 {
00314 ULOGGER_DEBUG("Read parameters from = %s", configFile.c_str());
00315 Parameters::readINI(configFile, param);
00316 }
00317
00318 this->init(param, databasePath);
00319 }
00320
00321 void Rtabmap::close(bool databaseSaved)
00322 {
00323 UINFO("databaseSaved=%d", databaseSaved?1:0);
00324 _highestHypothesis = std::make_pair(0,0.0f);
00325 _loopClosureHypothesis = std::make_pair(0,0.0f);
00326 _lastProcessTime = 0.0;
00327 _someNodesHaveBeenTransferred = false;
00328 _optimizedPoses.clear();
00329 _constraints.clear();
00330 _mapCorrection.setIdentity();
00331 _lastLocalizationPose.setNull();
00332 _lastLocalizationNodeId = 0;
00333 _distanceTravelled = 0.0f;
00334 this->clearPath(0);
00335
00336 flushStatisticLogs();
00337 if(_foutFloat)
00338 {
00339 fclose(_foutFloat);
00340 _foutFloat = 0;
00341 }
00342 if(_foutInt)
00343 {
00344 fclose(_foutInt);
00345 _foutInt = 0;
00346 }
00347
00348 if(_epipolarGeometry)
00349 {
00350 delete _epipolarGeometry;
00351 _epipolarGeometry = 0;
00352 }
00353 if(_memory)
00354 {
00355 _memory->close(databaseSaved, true);
00356 delete _memory;
00357 _memory = 0;
00358 }
00359 if(_bayesFilter)
00360 {
00361 delete _bayesFilter;
00362 _bayesFilter = 0;
00363 }
00364 if(_graphOptimizer)
00365 {
00366 delete _graphOptimizer;
00367 _graphOptimizer = 0;
00368 }
00369 _databasePath.clear();
00370 parseParameters(Parameters::getDefaultParameters());
00371 _parameters.clear();
00372 }
00373
00374 void Rtabmap::parseParameters(const ParametersMap & parameters)
00375 {
00376 uInsert(_parameters, parameters);
00377
00378
00379 Parameters::parse(parameters, Parameters::kRtabmapStatisticLogsBufferedInRAM(), _statisticLogsBufferedInRAM);
00380 Parameters::parse(parameters, Parameters::kRtabmapStatisticLogged(), _statisticLogged);
00381 Parameters::parse(parameters, Parameters::kRtabmapStatisticLoggedHeaders(), _statisticLoggedHeaders);
00382
00383 ULOGGER_DEBUG("");
00384 ParametersMap::const_iterator iter;
00385 if((iter=parameters.find(Parameters::kRtabmapWorkingDirectory())) != parameters.end())
00386 {
00387 this->setWorkingDirectory(iter->second.c_str());
00388 }
00389
00390 Parameters::parse(parameters, Parameters::kRtabmapPublishStats(), _publishStats);
00391 Parameters::parse(parameters, Parameters::kRtabmapPublishLastSignature(), _publishLastSignatureData);
00392 Parameters::parse(parameters, Parameters::kRtabmapPublishPdf(), _publishPdf);
00393 Parameters::parse(parameters, Parameters::kRtabmapPublishLikelihood(), _publishLikelihood);
00394 Parameters::parse(parameters, Parameters::kRtabmapTimeThr(), _maxTimeAllowed);
00395 Parameters::parse(parameters, Parameters::kRtabmapMemoryThr(), _maxMemoryAllowed);
00396 Parameters::parse(parameters, Parameters::kRtabmapLoopThr(), _loopThr);
00397 Parameters::parse(parameters, Parameters::kRtabmapLoopRatio(), _loopRatio);
00398 Parameters::parse(parameters, Parameters::kRtabmapMaxRetrieved(), _maxRetrieved);
00399 Parameters::parse(parameters, Parameters::kRGBDMaxLocalRetrieved(), _maxLocalRetrieved);
00400 Parameters::parse(parameters, Parameters::kMemImageKept(), _rawDataKept);
00401 Parameters::parse(parameters, Parameters::kRGBDEnabled(), _rgbdSlamMode);
00402 Parameters::parse(parameters, Parameters::kRGBDLinearUpdate(), _rgbdLinearUpdate);
00403 Parameters::parse(parameters, Parameters::kRGBDAngularUpdate(), _rgbdAngularUpdate);
00404 Parameters::parse(parameters, Parameters::kRGBDNewMapOdomChangeDistance(), _newMapOdomChangeDistance);
00405 Parameters::parse(parameters, Parameters::kRGBDNeighborLinkRefining(), _neighborLinkRefining);
00406 Parameters::parse(parameters, Parameters::kRGBDProximityByTime(), _proximityByTime);
00407 Parameters::parse(parameters, Parameters::kRGBDProximityBySpace(), _proximityBySpace);
00408 Parameters::parse(parameters, Parameters::kRGBDScanMatchingIdsSavedInLinks(), _scanMatchingIdsSavedInLinks);
00409 Parameters::parse(parameters, Parameters::kRGBDLocalRadius(), _localRadius);
00410 Parameters::parse(parameters, Parameters::kRGBDLocalImmunizationRatio(), _localImmunizationRatio);
00411 Parameters::parse(parameters, Parameters::kRGBDProximityMaxGraphDepth(), _proximityMaxGraphDepth);
00412 Parameters::parse(parameters, Parameters::kRGBDProximityPathFilteringRadius(), _proximityFilteringRadius);
00413 Parameters::parse(parameters, Parameters::kRGBDProximityPathRawPosesUsed(), _proximityRawPosesUsed);
00414 Parameters::parse(parameters, Parameters::kRGBDProximityAngle(), _proximityAngle);
00415 _proximityAngle *= M_PI/180.0f;
00416 Parameters::parse(parameters, Parameters::kRGBDOptimizeFromGraphEnd(), _optimizeFromGraphEnd);
00417 Parameters::parse(parameters, Parameters::kRGBDOptimizeMaxError(), _optimizationMaxLinearError);
00418 Parameters::parse(parameters, Parameters::kRtabmapStartNewMapOnLoopClosure(), _startNewMapOnLoopClosure);
00419 Parameters::parse(parameters, Parameters::kRGBDGoalReachedRadius(), _goalReachedRadius);
00420 Parameters::parse(parameters, Parameters::kRGBDGoalsSavedInUserData(), _goalsSavedInUserData);
00421 Parameters::parse(parameters, Parameters::kRGBDPlanStuckIterations(), _pathStuckIterations);
00422 Parameters::parse(parameters, Parameters::kRGBDPlanLinearVelocity(), _pathLinearVelocity);
00423 Parameters::parse(parameters, Parameters::kRGBDPlanAngularVelocity(), _pathAngularVelocity);
00424
00425 UASSERT(_rgbdLinearUpdate >= 0.0f);
00426 UASSERT(_rgbdAngularUpdate >= 0.0f);
00427
00428
00429
00430
00431
00432 Optimizer::Type optimizerType = Optimizer::kTypeUndef;
00433 if((iter=parameters.find(Parameters::kOptimizerStrategy())) != parameters.end())
00434 {
00435 optimizerType = (Optimizer::Type)std::atoi((*iter).second.c_str());
00436 }
00437 if(optimizerType!=Optimizer::kTypeUndef)
00438 {
00439 UDEBUG("new detector strategy %d", int(optimizerType));
00440 if(_graphOptimizer)
00441 {
00442 delete _graphOptimizer;
00443 _graphOptimizer = 0;
00444 }
00445
00446 _graphOptimizer = Optimizer::create(optimizerType, _parameters);
00447 }
00448 else if(_graphOptimizer)
00449 {
00450 _graphOptimizer->parseParameters(parameters);
00451 }
00452 else
00453 {
00454 optimizerType = (Optimizer::Type)Parameters::defaultOptimizerStrategy();
00455 _graphOptimizer = Optimizer::create(optimizerType, parameters);
00456 }
00457
00458 if(_memory)
00459 {
00460 _memory->parseParameters(parameters);
00461 }
00462
00463 VhStrategy vhStrategy = kVhUndef;
00464
00465 if((iter=parameters.find(Parameters::kRtabmapVhStrategy())) != parameters.end())
00466 {
00467 vhStrategy = (VhStrategy)std::atoi((*iter).second.c_str());
00468 }
00469 if(!_epipolarGeometry && vhStrategy == kVhEpipolar)
00470 {
00471 _epipolarGeometry = new EpipolarGeometry(_parameters);
00472 }
00473 else if(_epipolarGeometry && vhStrategy == kVhNone)
00474 {
00475 delete _epipolarGeometry;
00476 _epipolarGeometry = 0;
00477 }
00478 else if(_epipolarGeometry)
00479 {
00480 _epipolarGeometry->parseParameters(parameters);
00481 }
00482
00483
00484 if(!_bayesFilter)
00485 {
00486 _bayesFilter = new BayesFilter(_parameters);
00487 }
00488 else
00489 {
00490 _bayesFilter->parseParameters(parameters);
00491 }
00492 }
00493
00494 int Rtabmap::getLastLocationId() const
00495 {
00496 int id = 0;
00497 if(_memory)
00498 {
00499 id = _memory->getLastSignatureId();
00500 }
00501 return id;
00502 }
00503
00504 std::list<int> Rtabmap::getWM() const
00505 {
00506 std::list<int> mem;
00507 if(_memory)
00508 {
00509 mem = uKeysList(_memory->getWorkingMem());
00510 mem.remove(-1);
00511 }
00512 return mem;
00513 }
00514
00515 int Rtabmap::getWMSize() const
00516 {
00517 if(_memory)
00518 {
00519 return (int)_memory->getWorkingMem().size()-1;
00520 }
00521 return 0;
00522 }
00523
00524 std::map<int, int> Rtabmap::getWeights() const
00525 {
00526 std::map<int, int> weights;
00527 if(_memory)
00528 {
00529 weights = _memory->getWeights();
00530 weights.erase(-1);
00531 }
00532 return weights;
00533 }
00534
00535 std::set<int> Rtabmap::getSTM() const
00536 {
00537 if(_memory)
00538 {
00539 return _memory->getStMem();
00540 }
00541 return std::set<int>();
00542 }
00543
00544 int Rtabmap::getSTMSize() const
00545 {
00546 if(_memory)
00547 {
00548 return (int)_memory->getStMem().size();
00549 }
00550 return 0;
00551 }
00552
00553 int Rtabmap::getTotalMemSize() const
00554 {
00555 if(_memory)
00556 {
00557 const Signature * s =_memory->getLastWorkingSignature();
00558 if(s)
00559 {
00560 return s->id();
00561 }
00562 }
00563 return 0;
00564 }
00565
00566 std::multimap<int, cv::KeyPoint> Rtabmap::getWords(int locationId) const
00567 {
00568 if(_memory)
00569 {
00570 const Signature * s = _memory->getSignature(locationId);
00571 if(s)
00572 {
00573 return s->getWords();
00574 }
00575 }
00576 return std::multimap<int, cv::KeyPoint>();
00577 }
00578
00579 bool Rtabmap::isInSTM(int locationId) const
00580 {
00581 if(_memory)
00582 {
00583 return _memory->isInSTM(locationId);
00584 }
00585 return false;
00586 }
00587
00588 bool Rtabmap::isIDsGenerated() const
00589 {
00590 if(_memory)
00591 {
00592 return _memory->isIDsGenerated();
00593 }
00594 return Parameters::defaultMemGenerateIds();
00595 }
00596
00597 const Statistics & Rtabmap::getStatistics() const
00598 {
00599 return statistics_;
00600 }
00601
00602
00603
00604
00605
00606
00607
00608
00609
00610
00611
00612
00613
00614
00615
00616
00617
00618
00619
00620 Transform Rtabmap::getPose(int locationId) const
00621 {
00622 if(_memory)
00623 {
00624 const Signature * s = _memory->getSignature(locationId);
00625 if(s && _optimizedPoses.find(s->id()) != _optimizedPoses.end())
00626 {
00627 return _optimizedPoses.at(s->id());
00628 }
00629 }
00630 return Transform();
00631 }
00632
00633 int Rtabmap::triggerNewMap()
00634 {
00635 int mapId = -1;
00636 if(_memory)
00637 {
00638 std::map<int, int> reducedIds;
00639 mapId = _memory->incrementMapId(&reducedIds);
00640 UINFO("New map triggered, new map = %d", mapId);
00641 _optimizedPoses.clear();
00642 _constraints.clear();
00643 _lastLocalizationNodeId = 0;
00644
00645
00646 if(reducedIds.size() && _path.size())
00647 {
00648 for(unsigned int i=0; i<_path.size(); ++i)
00649 {
00650 std::map<int, int>::const_iterator iter = reducedIds.find(_path[i].first);
00651 if(iter!= reducedIds.end())
00652 {
00653
00654 _path[i].first = iter->second;
00655 }
00656 }
00657 }
00658 }
00659 return mapId;
00660 }
00661
00662 bool Rtabmap::labelLocation(int id, const std::string & label)
00663 {
00664 if(_memory)
00665 {
00666 if(id > 0)
00667 {
00668 return _memory->labelSignature(id, label);
00669 }
00670 else if(_memory->getLastWorkingSignature())
00671 {
00672 return _memory->labelSignature(_memory->getLastWorkingSignature()->id(), label);
00673 }
00674 else
00675 {
00676 UERROR("Last signature is null! Cannot set label \"%s\"", label.c_str());
00677 }
00678 }
00679 return false;
00680 }
00681
00682 bool Rtabmap::setUserData(int id, const cv::Mat & data)
00683 {
00684 if(_memory)
00685 {
00686 if(id > 0)
00687 {
00688 return _memory->setUserData(id, data);
00689 }
00690 else if(_memory->getLastWorkingSignature())
00691 {
00692 return _memory->setUserData(_memory->getLastWorkingSignature()->id(), data);
00693 }
00694 else
00695 {
00696 UERROR("Last signature is null! Cannot set user data!");
00697 }
00698 }
00699 return false;
00700 }
00701
00702 void Rtabmap::generateDOTGraph(const std::string & path, int id, int margin)
00703 {
00704 if(_memory)
00705 {
00706 _memory->joinTrashThread();
00707
00708 if(id > 0)
00709 {
00710 std::map<int, int> ids = _memory->getNeighborsId(id, margin, -1, false);
00711
00712 if(ids.size() > 0)
00713 {
00714 ids.insert(std::pair<int,int>(id, 0));
00715 std::set<int> idsSet;
00716 for(std::map<int, int>::iterator iter = ids.begin(); iter!=ids.end(); ++iter)
00717 {
00718 idsSet.insert(idsSet.end(), iter->first);
00719 }
00720 _memory->generateGraph(path, idsSet);
00721 }
00722 else
00723 {
00724 UERROR("No neighbors found for signature %d.", id);
00725 }
00726 }
00727 else
00728 {
00729 _memory->generateGraph(path);
00730 }
00731 }
00732 }
00733
00734 void Rtabmap::exportPoses(const std::string & path, bool optimized, bool global, int format)
00735 {
00736 if(_memory && _memory->getLastWorkingSignature())
00737 {
00738 std::map<int, Transform> poses;
00739 std::multimap<int, Link> constraints;
00740
00741 if(optimized)
00742 {
00743 this->optimizeCurrentMap(_memory->getLastWorkingSignature()->id(), global, poses, &constraints);
00744 }
00745 else
00746 {
00747 std::map<int, int> ids = _memory->getNeighborsId(_memory->getLastWorkingSignature()->id(), 0, global?-1:0, true);
00748 _memory->getMetricConstraints(uKeysSet(ids), poses, constraints, global);
00749 }
00750
00751 std::map<int, double> stamps;
00752 if(format == 1)
00753 {
00754 for(std::map<int, Transform>::iterator iter=poses.begin(); iter!=poses.end(); ++iter)
00755 {
00756 Transform o,g;
00757 int m, w;
00758 std::string l;
00759 double stamp = 0.0;
00760 _memory->getNodeInfo(iter->first, o, m, w, l, stamp, g, true);
00761 stamps.insert(std::make_pair(iter->first, stamp));
00762 }
00763 }
00764
00765 graph::exportPoses(path, format, poses, constraints, stamps);
00766 }
00767 }
00768
00769 void Rtabmap::resetMemory()
00770 {
00771 _highestHypothesis = std::make_pair(0,0.0f);
00772 _loopClosureHypothesis = std::make_pair(0,0.0f);
00773 _lastProcessTime = 0.0;
00774 _someNodesHaveBeenTransferred = false;
00775 _optimizedPoses.clear();
00776 _constraints.clear();
00777 _mapCorrection.setIdentity();
00778 _lastLocalizationPose.setNull();
00779 _lastLocalizationNodeId = 0;
00780 _distanceTravelled = 0.0f;
00781 this->clearPath(0);
00782
00783 if(_memory)
00784 {
00785 _memory->init(_databasePath, true, _parameters, true);
00786 if(_memory->getLastWorkingSignature())
00787 {
00788 optimizeCurrentMap(_memory->getLastWorkingSignature()->id(), false, _optimizedPoses, &_constraints);
00789 }
00790 if(_bayesFilter)
00791 {
00792 _bayesFilter->reset();
00793 }
00794 }
00795 else
00796 {
00797 UERROR("RTAB-Map is not initialized. No memory to reset...");
00798 }
00799 this->setupLogFiles(true);
00800 }
00801
00802
00803
00804
00805 bool Rtabmap::process(
00806 const SensorData & data,
00807 const Transform & odomPose,
00808 const cv::Mat & covariance)
00809 {
00810 UDEBUG("");
00811
00812
00813
00814
00815 UTimer timer;
00816 UTimer timerTotal;
00817 double timeMemoryUpdate = 0;
00818 double timeNeighborLinkRefining = 0;
00819 double timeProximityByTimeDetection = 0;
00820 double timeProximityBySpaceDetection = 0;
00821 double timeCleaningNeighbors = 0;
00822 double timeReactivations = 0;
00823 double timeAddLoopClosureLink = 0;
00824 double timeMapOptimization = 0;
00825 double timeRetrievalDbAccess = 0;
00826 double timeLikelihoodCalculation = 0;
00827 double timePosteriorCalculation = 0;
00828 double timeHypothesesCreation = 0;
00829 double timeHypothesesValidation = 0;
00830 double timeRealTimeLimitReachedProcess = 0;
00831 double timeMemoryCleanup = 0;
00832 double timeEmptyingTrash = 0;
00833 double timeJoiningTrash = 0;
00834 double timeStatsCreation = 0;
00835
00836 float hypothesisRatio = 0.0f;
00837 bool rejectedHypothesis = false;
00838
00839 std::map<int, float> rawLikelihood;
00840 std::map<int, float> adjustedLikelihood;
00841 std::map<int, float> likelihood;
00842 std::map<int, int> weights;
00843 std::map<int, float> posterior;
00844 std::list<std::pair<int, float> > reactivateHypotheses;
00845
00846 std::map<int, int> childCount;
00847 std::set<int> signaturesRetrieved;
00848 int proximityDetectionsInTimeFound = 0;
00849
00850 const Signature * signature = 0;
00851 const Signature * sLoop = 0;
00852
00853 _loopClosureHypothesis = std::make_pair(0,0.0f);
00854 std::pair<int, float> lastHighestHypothesis = _highestHypothesis;
00855 _highestHypothesis = std::make_pair(0,0.0f);
00856
00857 std::set<int> immunizedLocations;
00858
00859 statistics_ = Statistics();
00860
00861
00862
00863
00864 ULOGGER_INFO("getting data...");
00865
00866 timer.start();
00867 timerTotal.start();
00868
00869 UASSERT_MSG(_memory, "RTAB-Map is not initialized!");
00870 UASSERT_MSG(_bayesFilter, "RTAB-Map is not initialized!");
00871 UASSERT_MSG(_graphOptimizer, "RTAB-Map is not initialized!");
00872
00873
00874
00875
00876 if(_rgbdSlamMode)
00877 {
00878 if(odomPose.isNull())
00879 {
00880 UERROR("RGB-D SLAM mode is enabled and no odometry is provided. "
00881 "Image %d is ignored!", data.id());
00882 return false;
00883 }
00884 else if(_memory->isIncremental())
00885 {
00886
00887 if(_memory->getLastWorkingSignature())
00888 {
00889 const Transform & lastPose = _memory->getLastWorkingSignature()->getPose();
00890
00891
00892 if(!lastPose.isIdentity() && odomPose.isIdentity())
00893 {
00894 int mapId = triggerNewMap();
00895 UWARN("Odometry is reset (identity pose detected). Increment map id to %d!", mapId);
00896 }
00897 else if(_newMapOdomChangeDistance > 0.0)
00898 {
00899
00900 Transform lastPoseToNewPose = lastPose.inverse() * odomPose;
00901 float x,y,z, roll,pitch,yaw;
00902 lastPoseToNewPose.getTranslationAndEulerAngles(x,y,z, roll,pitch,yaw);
00903 if((x*x + y*y + z*z) > _newMapOdomChangeDistance*_newMapOdomChangeDistance)
00904 {
00905 int mapId = triggerNewMap();
00906 UWARN("Odometry is reset (large odometry change detected > %f). A new map (%d) is created! Last pose = %s, new pose = %s",
00907 _newMapOdomChangeDistance,
00908 mapId,
00909 lastPose.prettyPrint().c_str(),
00910 odomPose.prettyPrint().c_str());
00911 }
00912 }
00913 }
00914 }
00915 }
00916
00917
00918
00919
00920 ULOGGER_INFO("Updating memory...");
00921 if(_rgbdSlamMode)
00922 {
00923 if(!_memory->update(data, odomPose, covariance, &statistics_))
00924 {
00925 return false;
00926 }
00927 }
00928 else
00929 {
00930 if(!_memory->update(data, Transform(), cv::Mat(), &statistics_))
00931 {
00932 return false;
00933 }
00934 }
00935
00936 signature = _memory->getLastWorkingSignature();
00937 if(!signature)
00938 {
00939 UFATAL("Not supposed to be here...last signature is null?!?");
00940 }
00941
00942 ULOGGER_INFO("Processing signature %d w=%d", signature->id(), signature->getWeight());
00943 timeMemoryUpdate = timer.ticks();
00944 ULOGGER_INFO("timeMemoryUpdate=%fs", timeMemoryUpdate);
00945
00946
00947
00948
00949 bool smallDisplacement = false;
00950 std::list<int> signaturesRemoved;
00951 if(_rgbdSlamMode)
00952 {
00953
00954 int rehearsedId = (int)uValue(statistics_.data(), Statistics::kMemoryRehearsal_merged(), 0.0f);
00955 if(rehearsedId > 0)
00956 {
00957 _optimizedPoses.erase(rehearsedId);
00958 }
00959 else if(signature->getWeight() >= 0 && _rgbdLinearUpdate > 0.0f && _rgbdAngularUpdate > 0.0f)
00960 {
00961
00962
00963
00964 const std::map<int, Link> & links = signature->getLinks();
00965 if(links.size() == 1)
00966 {
00967
00968 const Signature * s = _memory->getSignature(links.begin()->second.to());
00969 UASSERT(s!=0);
00970 if(s->getWeight() >= 0)
00971 {
00972 float x,y,z, roll,pitch,yaw;
00973 links.begin()->second.transform().getTranslationAndEulerAngles(x,y,z, roll,pitch,yaw);
00974 bool isMoving = fabs(x) > _rgbdLinearUpdate ||
00975 fabs(y) > _rgbdLinearUpdate ||
00976 fabs(z) > _rgbdLinearUpdate ||
00977 fabs(roll) > _rgbdAngularUpdate ||
00978 fabs(pitch) > _rgbdAngularUpdate ||
00979 fabs(yaw) > _rgbdAngularUpdate;
00980 if(!isMoving)
00981 {
00982
00983
00984 smallDisplacement = true;
00985 }
00986 }
00987 }
00988 }
00989
00990
00991 Transform newPose;
00992 if(_neighborLinkRefining &&
00993 signature->getLinks().size() == 1 &&
00994 _memory->isIncremental() &&
00995 rehearsedId == 0)
00996 {
00997 int oldId = signature->getLinks().begin()->first;
00998 const Signature * oldS = _memory->getSignature(oldId);
00999 UASSERT(oldS != 0);
01000
01001 Transform guess = signature->getLinks().begin()->second.transform().inverse();
01002
01003 if(smallDisplacement)
01004 {
01005 if(signature->getLinks().begin()->second.transVariance() == 1)
01006 {
01007
01008 UDEBUG("Set small variance. The robot is not moving.");
01009 _memory->updateLink(oldId, signature->id(), guess, 0.0001, 0.0001);
01010 }
01011 }
01012 else
01013 {
01014
01015
01016
01017 if(!signature->sensorData().laserScanCompressed().empty())
01018 {
01019 UINFO("Odometry refining: guess = %s", guess.prettyPrint().c_str());
01020 RegistrationInfo info;
01021 Transform t = _memory->computeTransform(oldId, signature->id(), guess, &info);
01022 if(!t.isNull())
01023 {
01024 UINFO("Odometry refining: update neighbor link (%d->%d, variance=%f) from %s to %s",
01025 oldId,
01026 signature->id(),
01027 info.variance,
01028 guess.prettyPrint().c_str(),
01029 t.prettyPrint().c_str());
01030 UASSERT(info.variance > 0.0);
01031 _memory->updateLink(oldId, signature->id(), t, info.variance, info.variance);
01032
01033 if(_optimizeFromGraphEnd)
01034 {
01035
01036
01037
01038 Transform u = guess * t.inverse();
01039 std::map<int, Transform>::iterator jter = _optimizedPoses.find(oldId);
01040 UASSERT(jter!=_optimizedPoses.end());
01041 Transform up = jter->second * u * jter->second.inverse();
01042 Transform mapCorrectionInv = _mapCorrection.inverse();
01043 for(std::map<int, Transform>::iterator iter=_optimizedPoses.begin(); iter!=_optimizedPoses.end(); ++iter)
01044 {
01045 iter->second = mapCorrectionInv * up * iter->second;
01046 }
01047 }
01048 }
01049 else
01050 {
01051 UINFO("Odometry refining rejected: %s", info.rejectedMsg.c_str());
01052 if(info.variance > 0)
01053 {
01054 double sqrtVar = sqrt(info.variance);
01055 _memory->updateLink(oldId, signature->id(), guess, sqrtVar, sqrtVar);
01056 }
01057 }
01058 statistics_.addStatistic(Statistics::kNeighborLinkRefiningAccepted(), !t.isNull()?1.0f:0);
01059 statistics_.addStatistic(Statistics::kNeighborLinkRefiningInliers(), info.inliers);
01060 statistics_.addStatistic(Statistics::kNeighborLinkRefiningInliers_ratio(), info.icpInliersRatio);
01061 statistics_.addStatistic(Statistics::kNeighborLinkRefiningVariance(), info.variance);
01062 statistics_.addStatistic(Statistics::kNeighborLinkRefiningPts(), signature->sensorData().laserScanRaw().cols);
01063 }
01064 }
01065 timeNeighborLinkRefining = timer.ticks();
01066 ULOGGER_INFO("timeOdometryRefining=%fs", timeNeighborLinkRefining);
01067
01068 UASSERT(oldS->hasLink(signature->id()));
01069 UASSERT(uContains(_optimizedPoses, oldId));
01070
01071 newPose = _optimizedPoses.at(oldId) * oldS->getLinks().at(signature->id()).transform();
01072 _mapCorrection = newPose * signature->getPose().inverse();
01073 if(_mapCorrection.getNormSquared() > 0.001f && _optimizeFromGraphEnd)
01074 {
01075 UERROR("Map correction should be identity when optimizing from the last node. T=%s NewPose=%s OldPose=%s",
01076 _mapCorrection.prettyPrint().c_str(),
01077 newPose.prettyPrint().c_str(),
01078 signature->getPose().prettyPrint().c_str());
01079 }
01080 }
01081 else
01082 {
01083 newPose = _mapCorrection * signature->getPose();
01084 }
01085
01086 UDEBUG("Added pose %s (odom=%s)", newPose.prettyPrint().c_str(), signature->getPose().prettyPrint().c_str());
01087
01088 _optimizedPoses.insert(std::make_pair(signature->id(), newPose));
01089 _lastLocalizationPose = newPose;
01090 if(signature->getLinks().size() == 1 &&
01091 signature->getLinks().begin()->second.type() == Link::kNeighbor)
01092 {
01093
01094 UASSERT_MSG(signature->id() > signature->getLinks().begin()->second.to(),
01095 "Only forward links should be added.");
01096
01097 Link tmp = signature->getLinks().begin()->second.inverse();
01098
01099 _distanceTravelled += tmp.transform().getNorm();
01100
01101
01102 if(_constraints.size() &&
01103 _constraints.rbegin()->second.to() == signature->getLinks().begin()->second.to())
01104 {
01105 const Signature * s = _memory->getSignature(signature->getLinks().begin()->second.to());
01106 UASSERT(s!=0);
01107 if(s->getWeight() == -1)
01108 {
01109 tmp = _constraints.rbegin()->second.merge(tmp, tmp.type());
01110 _optimizedPoses.erase(s->id());
01111 _constraints.erase(--_constraints.end());
01112 }
01113 }
01114 _constraints.insert(std::make_pair(tmp.from(), tmp));
01115 }
01116
01117
01118
01119
01120 if(statistics_.reducedIds().size())
01121 {
01122 for(unsigned int i=0; i<_path.size(); ++i)
01123 {
01124 std::map<int, int>::const_iterator iter = statistics_.reducedIds().find(_path[i].first);
01125 if(iter!= statistics_.reducedIds().end())
01126 {
01127
01128 _path[i].first = iter->second;
01129 }
01130 }
01131
01132 for(std::map<int, int>::const_iterator iter=statistics_.reducedIds().begin();
01133 iter!=statistics_.reducedIds().end();
01134 ++iter)
01135 {
01136 int erased = (int)_optimizedPoses.erase(iter->first);
01137 if(erased)
01138 {
01139 for(std::multimap<int, Link>::iterator jter = _constraints.begin(); jter!=_constraints.end();)
01140 {
01141 if(jter->second.from() == iter->first || jter->second.to() == iter->first)
01142 {
01143 _constraints.erase(jter++);
01144 }
01145 else
01146 {
01147 ++jter;
01148 }
01149 }
01150 }
01151 }
01152 }
01153
01154
01155
01156
01157 if(_proximityByTime &&
01158 rehearsedId == 0 &&
01159 signature->getWords3().size() &&
01160 _memory->isIncremental() &&
01161 !signature->isBadSignature() &&
01162 signature->getWeight()>=0)
01163 {
01164 const std::set<int> & stm = _memory->getStMem();
01165 for(std::set<int>::const_reverse_iterator iter = stm.rbegin(); iter!=stm.rend(); ++iter)
01166 {
01167 if(*iter != signature->id() &&
01168 signature->getLinks().find(*iter) == signature->getLinks().end() &&
01169 _memory->getSignature(*iter)->mapId() == signature->mapId() &&
01170 _memory->getSignature(*iter)->getWeight()>=0)
01171 {
01172 std::string rejectedMsg;
01173 UDEBUG("Check local transform between %d and %d", signature->id(), *iter);
01174 RegistrationInfo info;
01175 Transform guess;
01176 if(_optimizedPoses.find(*iter) != _optimizedPoses.end())
01177 {
01178 guess = newPose.inverse() * _optimizedPoses.at(*iter);
01179 }
01180
01181 Transform transform = _memory->computeTransform(signature->id(), *iter, guess, &info);
01182
01183 if(!transform.isNull())
01184 {
01185 UDEBUG("Add local loop closure in TIME (%d->%d) %s",
01186 signature->id(),
01187 *iter,
01188 transform.prettyPrint().c_str());
01189
01190 UASSERT(info.variance > 0.0);
01191 if(_memory->addLink(Link(signature->id(), *iter, Link::kLocalTimeClosure, transform, info.variance, info.variance)))
01192 {
01193 ++proximityDetectionsInTimeFound;
01194 UINFO("Local loop closure found between %d and %d with t=%s",
01195 *iter, signature->id(), transform.prettyPrint().c_str());
01196 }
01197 else
01198 {
01199 UWARN("Cannot add local loop closure between %d and %d ?!?",
01200 *iter, signature->id());
01201 }
01202 }
01203 else
01204 {
01205 UINFO("Local loop closure (time) between %d and %d rejected: %s",
01206 *iter, signature->id(), rejectedMsg.c_str());
01207 }
01208 }
01209 }
01210 }
01211 }
01212
01213 timeProximityByTimeDetection = timer.ticks();
01214 UINFO("timeLocalTimeDetection=%fs", timeProximityByTimeDetection);
01215
01216
01217
01218
01219 int previousId = signature->getLinks().size() == 1?signature->getLinks().begin()->first:0;
01220
01221 if(!signature->isBadSignature() && signature->getWeight()>=0 && (!smallDisplacement || _memory->getLoopClosureLinks(previousId, false).size() == 0))
01222 {
01223
01224
01225
01226 if(_memory->getWorkingMem().size())
01227 {
01228
01229
01230
01231
01232
01233 ULOGGER_INFO("computing likelihood...");
01234
01235 std::list<int> signaturesToCompare;
01236 for(std::map<int, double>::const_iterator iter=_memory->getWorkingMem().begin();
01237 iter!=_memory->getWorkingMem().end();
01238 ++iter)
01239 {
01240 if(iter->first > 0)
01241 {
01242 const Signature * s = _memory->getSignature(iter->first);
01243 UASSERT(s!=0);
01244 if(s->getWeight() != -1)
01245 {
01246 signaturesToCompare.push_back(iter->first);
01247 }
01248 }
01249 else
01250 {
01251
01252 signaturesToCompare.push_back(iter->first);
01253 }
01254 }
01255
01256 rawLikelihood = _memory->computeLikelihood(signature, signaturesToCompare);
01257
01258
01259 likelihood = rawLikelihood;
01260 this->adjustLikelihood(likelihood);
01261
01262 timeLikelihoodCalculation = timer.ticks();
01263 ULOGGER_INFO("timeLikelihoodCalculation=%fs",timeLikelihoodCalculation);
01264
01265
01266
01267
01268
01269 ULOGGER_INFO("getting posterior...");
01270
01271
01272 posterior = _bayesFilter->computePosterior(_memory, likelihood);
01273 timePosteriorCalculation = timer.ticks();
01274 ULOGGER_INFO("timePosteriorCalculation=%fs",timePosteriorCalculation);
01275
01276
01277 if(_publishStats && (_publishLikelihood || _publishPdf))
01278 {
01279 weights = _memory->getWeights();
01280 }
01281
01282
01283
01284
01285 ULOGGER_INFO("creating hypotheses...");
01286 if(posterior.size())
01287 {
01288 for(std::map<int, float>::const_reverse_iterator iter = posterior.rbegin(); iter != posterior.rend(); ++iter)
01289 {
01290 if(iter->first > 0 && iter->second > _highestHypothesis.second)
01291 {
01292 _highestHypothesis = *iter;
01293 }
01294 }
01295
01296 _highestHypothesis.second = 1-posterior.begin()->second;
01297 }
01298 timeHypothesesCreation = timer.ticks();
01299 ULOGGER_INFO("Highest hypothesis=%d, value=%f, timeHypothesesCreation=%fs", _highestHypothesis.first, _highestHypothesis.second, timeHypothesesCreation);
01300
01301 if(_highestHypothesis.first > 0)
01302 {
01303
01304
01305
01306 if(_highestHypothesis.second >= _loopThr)
01307 {
01308 rejectedHypothesis = true;
01309 if(posterior.size() <= 2)
01310 {
01311
01312 UDEBUG("rejected hypothesis: single hypothesis");
01313 }
01314 else if(_epipolarGeometry && !_epipolarGeometry->check(signature, _memory->getSignature(_highestHypothesis.first)))
01315 {
01316 UWARN("rejected hypothesis: by epipolar geometry");
01317 }
01318 else if(_loopRatio > 0.0f && lastHighestHypothesis.second && _highestHypothesis.second < _loopRatio*lastHighestHypothesis.second)
01319 {
01320 UWARN("rejected hypothesis: not satisfying hypothesis ratio (%f < %f * %f)",
01321 _highestHypothesis.second, _loopRatio, lastHighestHypothesis.second);
01322 }
01323 else if(_loopRatio > 0.0f && lastHighestHypothesis.second == 0)
01324 {
01325 UWARN("rejected hypothesis: last closure hypothesis is null (loop ratio is on)");
01326 }
01327 else
01328 {
01329 _loopClosureHypothesis = _highestHypothesis;
01330 rejectedHypothesis = false;
01331 }
01332
01333 timeHypothesesValidation = timer.ticks();
01334 ULOGGER_INFO("timeHypothesesValidation=%fs",timeHypothesesValidation);
01335 }
01336 else if(_highestHypothesis.second < _loopRatio*lastHighestHypothesis.second)
01337 {
01338
01339
01340
01341 rejectedHypothesis = true;
01342 UWARN("rejected hypothesis: under loop ratio %f < %f", _highestHypothesis.second, _loopRatio*lastHighestHypothesis.second);
01343 }
01344
01345
01346 hypothesisRatio = _loopClosureHypothesis.second>0?_highestHypothesis.second/_loopClosureHypothesis.second:0;
01347 }
01348 }
01349 }
01350 else if(!signature->isBadSignature() && smallDisplacement)
01351 {
01352 _highestHypothesis = lastHighestHypothesis;
01353 }
01354
01355
01356
01357
01358 _memory->joinTrashThread();
01359 timeEmptyingTrash = _memory->getDbSavingTime();
01360 timeJoiningTrash = timer.ticks();
01361 ULOGGER_INFO("Time emptying memory trash = %fs, joining (actual overhead) = %fs", timeEmptyingTrash, timeJoiningTrash);
01362
01363
01364
01365
01366 int retrievalId = _highestHypothesis.first;
01367 std::list<int> reactivatedIds;
01368 double timeGetNeighborsTimeDb = 0.0;
01369 double timeGetNeighborsSpaceDb = 0.0;
01370 int immunizedGlobally = 0;
01371 int immunizedLocally = 0;
01372 if(retrievalId > 0 )
01373 {
01374
01375 ULOGGER_INFO("Retrieving locations... around id=%d", retrievalId);
01376 int neighborhoodSize = (int)_bayesFilter->getPredictionLC().size()-1;
01377 UASSERT(neighborhoodSize >= 0);
01378 ULOGGER_DEBUG("margin=%d maxRetieved=%d", neighborhoodSize, _maxRetrieved);
01379
01380 UTimer timeGetN;
01381 unsigned int nbLoadedFromDb = 0;
01382 std::set<int> reactivatedIdsSet;
01383 std::map<int, int> neighbors;
01384 int nbDirectNeighborsInDb = 0;
01385
01386
01387
01388 ULOGGER_DEBUG("In TIME");
01389 neighbors = _memory->getNeighborsId(retrievalId,
01390 neighborhoodSize,
01391 _maxRetrieved,
01392 true,
01393 true,
01394 false,
01395 &timeGetNeighborsTimeDb);
01396 ULOGGER_DEBUG("neighbors of %d in time = %d", retrievalId, (int)neighbors.size());
01397
01398 bool firstPassDone = false;
01399 int m = 0;
01400 while(m < neighborhoodSize)
01401 {
01402 std::set<int> idsSorted;
01403 for(std::map<int, int>::iterator iter=neighbors.begin(); iter!=neighbors.end();)
01404 {
01405 if(!firstPassDone && _memory->isInSTM(iter->first))
01406 {
01407 neighbors.erase(iter++);
01408 }
01409 else if(iter->second == m)
01410 {
01411 if(reactivatedIdsSet.find(iter->first) == reactivatedIdsSet.end())
01412 {
01413 idsSorted.insert(iter->first);
01414 reactivatedIdsSet.insert(iter->first);
01415
01416 if(m == 1 && _memory->getSignature(iter->first) == 0)
01417 {
01418 ++nbDirectNeighborsInDb;
01419 }
01420
01421
01422 if(immunizedLocations.insert(iter->first).second)
01423 {
01424 ++immunizedGlobally;
01425 }
01426
01427 UDEBUG("nt=%d m=%d immunized=1", iter->first, iter->second);
01428 }
01429 neighbors.erase(iter++);
01430 }
01431 else
01432 {
01433 ++iter;
01434 }
01435 }
01436 firstPassDone = true;
01437 reactivatedIds.insert(reactivatedIds.end(), idsSorted.rbegin(), idsSorted.rend());
01438 ++m;
01439 }
01440
01441
01442 ULOGGER_DEBUG("In SPACE");
01443 neighbors = _memory->getNeighborsId(retrievalId,
01444 neighborhoodSize,
01445 _maxRetrieved,
01446 true,
01447 false,
01448 false,
01449 &timeGetNeighborsSpaceDb);
01450 ULOGGER_DEBUG("neighbors of %d in space = %d", retrievalId, (int)neighbors.size());
01451 firstPassDone = false;
01452 m = 0;
01453 while(m < neighborhoodSize)
01454 {
01455 std::set<int> idsSorted;
01456 for(std::map<int, int>::iterator iter=neighbors.begin(); iter!=neighbors.end();)
01457 {
01458 if(!firstPassDone && _memory->isInSTM(iter->first))
01459 {
01460 neighbors.erase(iter++);
01461 }
01462 else if(iter->second == m)
01463 {
01464 if(reactivatedIdsSet.find(iter->first) == reactivatedIdsSet.end())
01465 {
01466 idsSorted.insert(iter->first);
01467 reactivatedIdsSet.insert(iter->first);
01468
01469 if(m == 1 && _memory->getSignature(iter->first) == 0)
01470 {
01471 ++nbDirectNeighborsInDb;
01472 }
01473 UDEBUG("nt=%d m=%d", iter->first, iter->second);
01474 }
01475 neighbors.erase(iter++);
01476 }
01477 else
01478 {
01479 ++iter;
01480 }
01481 }
01482 firstPassDone = true;
01483 reactivatedIds.insert(reactivatedIds.end(), idsSorted.rbegin(), idsSorted.rend());
01484 ++m;
01485 }
01486 ULOGGER_INFO("neighborhoodSize=%d, "
01487 "reactivatedIds.size=%d, "
01488 "nbLoadedFromDb=%d, "
01489 "nbDirectNeighborsInDb=%d, "
01490 "time=%fs (%fs %fs)",
01491 neighborhoodSize,
01492 reactivatedIds.size(),
01493 (int)nbLoadedFromDb,
01494 nbDirectNeighborsInDb,
01495 timeGetN.ticks(),
01496 timeGetNeighborsTimeDb,
01497 timeGetNeighborsSpaceDb);
01498
01499 }
01500
01501
01502
01503
01504 std::list<int> retrievalLocalIds;
01505 int maxLocalLocationsImmunized = _localImmunizationRatio * float(_memory->getWorkingMem().size());
01506 if(_rgbdSlamMode)
01507 {
01508
01509 if(_path.size())
01510 {
01511 updateGoalIndex();
01512
01513 float distanceSoFar = 0.0f;
01514
01515
01516 for(unsigned int i=_pathCurrentIndex; i<_path.size(); ++i)
01517 {
01518 if(_localRadius > 0.0f && i != _pathCurrentIndex)
01519 {
01520 distanceSoFar += _path[i-1].second.getDistance(_path[i].second);
01521 }
01522
01523 if(distanceSoFar <= _localRadius)
01524 {
01525 if(_memory->getSignature(_path[i].first) != 0)
01526 {
01527 if(immunizedLocations.insert(_path[i].first).second)
01528 {
01529 ++immunizedLocally;
01530 }
01531 UDEBUG("Path immunization: node %d (dist=%fm)", _path[i].first, distanceSoFar);
01532 }
01533 else if(retrievalLocalIds.size() < _maxLocalRetrieved)
01534 {
01535 UINFO("retrieval of node %d on path (dist=%fm)", _path[i].first, distanceSoFar);
01536 retrievalLocalIds.push_back(_path[i].first);
01537
01538 }
01539 }
01540 else
01541 {
01542 UDEBUG("Stop on node %d (dist=%fm > %fm)",
01543 _path[i].first, distanceSoFar, _localRadius);
01544 break;
01545 }
01546 }
01547 }
01548
01549
01550 if(immunizedLocally < maxLocalLocationsImmunized &&
01551 _memory->isIncremental())
01552 {
01553 std::map<int ,Transform> poses;
01554
01555 for(std::map<int, Transform>::iterator iter=_optimizedPoses.begin(); iter!=_optimizedPoses.end(); ++iter)
01556 {
01557 if(!_memory->isInSTM(iter->first))
01558 {
01559 poses.insert(*iter);
01560 }
01561 }
01562 int nearestId = graph::findNearestNode(poses, _optimizedPoses.at(signature->id()));
01563
01564 if(nearestId > 0 &&
01565 (_localRadius==0 ||
01566 _optimizedPoses.at(signature->id()).getDistance(_optimizedPoses.at(nearestId)) < _localRadius))
01567 {
01568 std::multimap<int, int> links;
01569 for(std::multimap<int, Link>::iterator iter=_constraints.begin(); iter!=_constraints.end(); ++iter)
01570 {
01571 if(uContains(_optimizedPoses, iter->second.from()) && uContains(_optimizedPoses, iter->second.to()))
01572 {
01573 links.insert(std::make_pair(iter->second.from(), iter->second.to()));
01574 links.insert(std::make_pair(iter->second.to(), iter->second.from()));
01575 }
01576 }
01577
01578 std::list<std::pair<int, Transform> > path = graph::computePath(_optimizedPoses, links, nearestId, signature->id());
01579 if(path.size() == 0)
01580 {
01581 UWARN("Could not compute a path between %d and %d", nearestId, signature->id());
01582 }
01583 else
01584 {
01585 for(std::list<std::pair<int, Transform> >::iterator iter=path.begin();
01586 iter!=path.end();
01587 ++iter)
01588 {
01589 if(immunizedLocally >= maxLocalLocationsImmunized)
01590 {
01591
01592 if(maxLocalLocationsImmunized > 20 && _someNodesHaveBeenTransferred)
01593 {
01594 UWARN("Could not immunize the whole local path (%d) between "
01595 "%d and %d (max location immunized=%d). You may want "
01596 "to increase RGBD/LocalImmunizationRatio (current=%f (%d of WM=%d)) "
01597 "to be able to immunize longer paths.",
01598 (int)path.size(),
01599 nearestId,
01600 signature->id(),
01601 maxLocalLocationsImmunized,
01602 _localImmunizationRatio,
01603 maxLocalLocationsImmunized,
01604 (int)_memory->getWorkingMem().size());
01605 }
01606 break;
01607 }
01608 else if(!_memory->isInSTM(iter->first))
01609 {
01610 if(immunizedLocations.insert(iter->first).second)
01611 {
01612 ++immunizedLocally;
01613 }
01614 UDEBUG("local node %d on path immunized=1", iter->first);
01615 }
01616 }
01617 }
01618 }
01619 }
01620
01621
01622
01623 std::map<int, float> nearNodes = graph::getNodesInRadius(signature->id(), _optimizedPoses, _localRadius);
01624
01625 std::multimap<float, int> nearNodesByDist;
01626 for(std::map<int, float>::iterator iter=nearNodes.begin(); iter!=nearNodes.end(); ++iter)
01627 {
01628 nearNodesByDist.insert(std::make_pair(iter->second, iter->first));
01629 }
01630 UINFO("near nodes=%d, max local immunized=%d, ratio=%f WM=%d",
01631 (int)nearNodesByDist.size(),
01632 maxLocalLocationsImmunized,
01633 _localImmunizationRatio,
01634 (int)_memory->getWorkingMem().size());
01635 for(std::multimap<float, int>::iterator iter=nearNodesByDist.begin();
01636 iter!=nearNodesByDist.end() && (retrievalLocalIds.size() < _maxLocalRetrieved || immunizedLocally < maxLocalLocationsImmunized);
01637 ++iter)
01638 {
01639 const Signature * s = _memory->getSignature(iter->second);
01640 if(s!=0)
01641 {
01642
01643
01644 const std::map<int, Link> & links = s->getLinks();
01645 for(std::map<int, Link>::const_reverse_iterator jter=links.rbegin();
01646 jter!=links.rend() && retrievalLocalIds.size() < _maxLocalRetrieved;
01647 ++jter)
01648 {
01649 if(_memory->getSignature(jter->first) == 0)
01650 {
01651 UINFO("retrieval of node %d on local map", jter->first);
01652 retrievalLocalIds.push_back(jter->first);
01653 }
01654 }
01655 if(!_memory->isInSTM(s->id()) && immunizedLocally < maxLocalLocationsImmunized)
01656 {
01657 if(immunizedLocations.insert(s->id()).second)
01658 {
01659 ++immunizedLocally;
01660 }
01661 UDEBUG("local node %d (%f m) immunized=1", iter->second, iter->first);
01662 }
01663 }
01664 }
01665
01666 if(retrievalLocalIds.size() < _maxLocalRetrieved)
01667 {
01668 std::set<int> retrievalLocalIdsSet(retrievalLocalIds.begin(), retrievalLocalIds.end());
01669 for(std::list<int>::iterator iter=retrievalLocalIds.begin();
01670 iter!=retrievalLocalIds.end() && retrievalLocalIds.size() < _maxLocalRetrieved;
01671 ++iter)
01672 {
01673 std::map<int, int> ids = _memory->getNeighborsId(*iter, 2, _maxLocalRetrieved - (unsigned int)retrievalLocalIds.size() + 1, true, false);
01674 for(std::map<int, int>::reverse_iterator jter=ids.rbegin();
01675 jter!=ids.rend() && retrievalLocalIds.size() < _maxLocalRetrieved;
01676 ++jter)
01677 {
01678 if(_memory->getSignature(jter->first) == 0 &&
01679 retrievalLocalIdsSet.find(jter->first) == retrievalLocalIdsSet.end())
01680 {
01681 UINFO("retrieval of node %d on local map", jter->first);
01682 retrievalLocalIds.push_back(jter->first);
01683 retrievalLocalIdsSet.insert(jter->first);
01684 }
01685 }
01686 }
01687 }
01688
01689
01690 for(std::multimap<float, int>::reverse_iterator iter=nearNodesByDist.rbegin(); iter!=nearNodesByDist.rend(); ++iter)
01691 {
01692 _memory->updateAge(iter->second);
01693 }
01694
01695
01696 reactivatedIds.insert(reactivatedIds.begin(), retrievalLocalIds.begin(), retrievalLocalIds.end());
01697 }
01698
01699
01700
01701
01702 if(reactivatedIds.size())
01703 {
01704
01705
01706 signaturesRetrieved = _memory->reactivateSignatures(
01707 reactivatedIds,
01708 _maxRetrieved+(unsigned int)retrievalLocalIds.size(),
01709 timeRetrievalDbAccess);
01710
01711 ULOGGER_INFO("retrieval of %d (db time = %fs)", (int)signaturesRetrieved.size(), timeRetrievalDbAccess);
01712
01713 timeRetrievalDbAccess += timeGetNeighborsTimeDb + timeGetNeighborsSpaceDb;
01714 UINFO("total timeRetrievalDbAccess=%fs", timeRetrievalDbAccess);
01715
01716
01717 immunizedLocations.insert(signaturesRetrieved.begin(), signaturesRetrieved.end());
01718 }
01719 timeReactivations = timer.ticks();
01720 ULOGGER_INFO("timeReactivations=%fs", timeReactivations);
01721
01722
01723
01724
01725
01726 std::list<std::pair<int, int> > loopClosureLinksAdded;
01727 int loopClosureVisualInliers = 0;
01728 if(_loopClosureHypothesis.first>0)
01729 {
01730
01731 Transform transform;
01732 RegistrationInfo info;
01733 info.variance = 1.0f;
01734 if(_rgbdSlamMode)
01735 {
01736 transform = _memory->computeTransform(signature->id(), _loopClosureHypothesis.first, Transform(), &info);
01737 loopClosureVisualInliers = info.inliers;
01738 rejectedHypothesis = transform.isNull();
01739 if(rejectedHypothesis)
01740 {
01741 UWARN("Rejected loop closure %d -> %d: %s",
01742 _loopClosureHypothesis.first, signature->id(), info.rejectedMsg.c_str());
01743 }
01744 }
01745 if(!rejectedHypothesis)
01746 {
01747
01748 UASSERT(info.variance > 0.0);
01749 rejectedHypothesis = !_memory->addLink(Link(signature->id(), _loopClosureHypothesis.first, Link::kGlobalClosure, transform, info.variance, info.variance));
01750 if(!rejectedHypothesis)
01751 {
01752 loopClosureLinksAdded.push_back(std::make_pair(signature->id(), _loopClosureHypothesis.first));
01753 }
01754 }
01755
01756 if(rejectedHypothesis)
01757 {
01758 _loopClosureHypothesis.first = 0;
01759 }
01760 }
01761
01762 timeAddLoopClosureLink = timer.ticks();
01763 ULOGGER_INFO("timeAddLoopClosureLink=%fs", timeAddLoopClosureLink);
01764
01765 int proximityDetectionsAddedVisually = 0;
01766 int proximityDetectionsAddedByICPOnly = 0;
01767 int lastProximitySpaceClosureId = 0;
01768 int proximitySpacePaths = 0;
01769 if(_proximityBySpace &&
01770 _localRadius > 0 &&
01771 _rgbdSlamMode &&
01772 signature->getWeight() >= 0)
01773 {
01774 if(_graphOptimizer->iterations() == 0)
01775 {
01776 UWARN("Cannot do local loop closure detection in space if graph optimization is disabled!");
01777 }
01778 else if(_memory->isIncremental() || _loopClosureHypothesis.first == 0)
01779 {
01780
01781
01782
01783
01784 if(!smallDisplacement || _memory->getLoopClosureLinks(previousId, false).size() == 0)
01785 {
01786
01787
01788
01789
01790
01791
01792
01793
01794 UDEBUG("Proximity detection (local loop closure in SPACE using matching images)");
01795 std::map<int, float> nearestIds;
01796 if(_memory->isIncremental())
01797 {
01798 nearestIds = _memory->getNeighborsIdRadius(signature->id(), _localRadius, _optimizedPoses, _proximityMaxGraphDepth);
01799 }
01800 else
01801 {
01802 nearestIds = graph::getNodesInRadius(signature->id(), _optimizedPoses, _localRadius);
01803 }
01804 UDEBUG("nearestIds=%d/%d", (int)nearestIds.size(), (int)_optimizedPoses.size());
01805 std::map<int, Transform> nearestPoses;
01806 for(std::map<int, float>::iterator iter=nearestIds.begin(); iter!=nearestIds.end(); ++iter)
01807 {
01808 if(_memory->getStMem().find(iter->first) == _memory->getStMem().end())
01809 {
01810 nearestPoses.insert(std::make_pair(iter->first, _optimizedPoses.at(iter->first)));
01811 }
01812 }
01813 UDEBUG("nearestPoses=%d", (int)nearestPoses.size());
01814
01815
01816 std::list<std::map<int, Transform> > nearestPaths = getPaths(nearestPoses);
01817 UDEBUG("nearestPaths=%d", (int)nearestPaths.size());
01818
01819 for(std::list<std::map<int, Transform> >::const_iterator iter=nearestPaths.begin();
01820 iter!=nearestPaths.end() && (_memory->isIncremental() || lastProximitySpaceClosureId == 0);
01821 ++iter)
01822 {
01823 std::map<int, Transform> path = *iter;
01824 UASSERT(path.size());
01825
01826
01827 path.insert(std::make_pair(signature->id(), _optimizedPoses.at(signature->id())));
01828 path = graph::getPosesInRadius(signature->id(), path, _localRadius, _proximityAngle);
01829 int nearestId = rtabmap::graph::findNearestNode(path, _optimizedPoses.at(signature->id()));
01830 if(nearestId > 0)
01831 {
01832
01833 if(!signature->hasLink(nearestId) &&
01834 (_proximityFilteringRadius <= 0.0f ||
01835 _optimizedPoses.at(signature->id()).getDistanceSquared(_optimizedPoses.at(nearestId)) < _proximityFilteringRadius*_proximityFilteringRadius))
01836 {
01837 RegistrationInfo info;
01838 Transform guess = _optimizedPoses.at(signature->id()).inverse() * _optimizedPoses.at(nearestId);
01839 Transform transform = _memory->computeTransform(signature->id(), nearestId, guess, &info);
01840 if(!transform.isNull())
01841 {
01842 if(_proximityFilteringRadius <= 0 || transform.getNormSquared() <= _proximityFilteringRadius*_proximityFilteringRadius)
01843 {
01844 UINFO("[Visual] Add local loop closure in SPACE (%d->%d) %s",
01845 signature->id(),
01846 nearestId,
01847 transform.prettyPrint().c_str());
01848 UASSERT(info.variance > 0.0);
01849 _memory->addLink(Link(signature->id(), nearestId, Link::kLocalSpaceClosure, transform, info.variance, info.variance));
01850 loopClosureLinksAdded.push_back(std::make_pair(signature->id(), nearestId));
01851
01852 if(loopClosureVisualInliers == 0)
01853 {
01854 loopClosureVisualInliers = info.inliers;
01855 }
01856
01857 if(_loopClosureHypothesis.first == 0)
01858 {
01859 ++proximityDetectionsAddedVisually;
01860 lastProximitySpaceClosureId = nearestId;
01861 }
01862 }
01863 else
01864 {
01865 UWARN("Ignoring local loop closure with %d because resulting "
01866 "transform is to large!? (%fm > %fm)",
01867 nearestId, transform.getNorm(), _proximityFilteringRadius);
01868 }
01869 }
01870 }
01871 }
01872 }
01873
01874
01875
01876
01877 UDEBUG("Proximity detection (local loop closure in SPACE with scan matching)");
01878 if( !signature->sensorData().laserScanCompressed().empty() &&
01879 (_memory->isIncremental() || lastProximitySpaceClosureId == 0))
01880 {
01881
01882
01883
01884
01885 proximitySpacePaths = (int)nearestPaths.size();
01886
01887 for(std::list<std::map<int, Transform> >::iterator iter=nearestPaths.begin();
01888 iter!=nearestPaths.end() && (_memory->isIncremental() || lastProximitySpaceClosureId == 0);
01889 ++iter)
01890 {
01891 std::map<int, Transform> & path = *iter;
01892 UASSERT(path.size());
01893
01894
01895 int nearestId = rtabmap::graph::findNearestNode(path, _optimizedPoses.at(signature->id()));
01896 UASSERT(nearestId > 0);
01897 UDEBUG("Path %d distance=%fm", nearestId, _optimizedPoses.at(signature->id()).getDistance(_optimizedPoses.at(nearestId)));
01898
01899
01900 if(!signature->hasLink(nearestId) &&
01901 (_proximityFilteringRadius <= 0.0f ||
01902 _optimizedPoses.at(signature->id()).getDistanceSquared(_optimizedPoses.at(nearestId)) < _proximityFilteringRadius*_proximityFilteringRadius))
01903 {
01904
01905 if(_proximityRawPosesUsed)
01906 {
01907
01908 path = optimizeGraph(nearestId, uKeysSet(path), std::map<int, Transform>(), false);
01909
01910 UASSERT(uContains(path, nearestId));
01911 Transform t = _optimizedPoses.at(nearestId) * path.at(nearestId).inverse();
01912 for(std::map<int, Transform>::iterator jter=path.begin(); jter!=path.end(); ++jter)
01913 {
01914 jter->second = t * jter->second;
01915 }
01916 }
01917 std::map<int, Transform> filteredPath = path;
01918 if(path.size() > 2 && _proximityFilteringRadius > 0.0f)
01919 {
01920
01921 filteredPath = graph::radiusPosesFiltering(path, _proximityFilteringRadius, 0, true);
01922
01923 filteredPath.insert(*path.find(nearestId));
01924 }
01925
01926 if(filteredPath.size() > 0)
01927 {
01928
01929 filteredPath.insert(std::make_pair(signature->id(), _optimizedPoses.at(signature->id())));
01930
01931 if(signature->getLinks().find(nearestId) == signature->getLinks().end())
01932 {
01933 RegistrationInfo info;
01934 Transform transform = _memory->computeIcpTransformMulti(signature->id(), nearestId, filteredPath, &info);
01935 if(!transform.isNull())
01936 {
01937 if(_proximityFilteringRadius <= 0 || transform.getNormSquared() <= _proximityFilteringRadius*_proximityFilteringRadius)
01938 {
01939 UINFO("[Scan matching] Add local loop closure in SPACE (%d->%d) %s",
01940 signature->id(),
01941 nearestId,
01942 transform.prettyPrint().c_str());
01943
01944 cv::Mat scanMatchingIds;
01945 if(_scanMatchingIdsSavedInLinks)
01946 {
01947 std::stringstream stream;
01948 stream << "SCANS:";
01949 for(std::map<int, Transform>::iterator iter=path.begin(); iter!=path.end(); ++iter)
01950 {
01951 if(iter != path.begin())
01952 {
01953 stream << ";";
01954 }
01955 stream << uNumber2Str(iter->first);
01956 }
01957 std::string scansStr = stream.str();
01958 scanMatchingIds = cv::Mat(1, int(scansStr.size()+1), CV_8SC1, (void *)scansStr.c_str());
01959 scanMatchingIds = compressData2(scanMatchingIds);
01960 }
01961
01962
01963 UASSERT(info.variance>0.0);
01964 double sqrtVar = sqrt(info.variance);
01965 _memory->addLink(Link(signature->id(), nearestId, Link::kLocalSpaceClosure, transform, sqrtVar, sqrtVar, scanMatchingIds));
01966 loopClosureLinksAdded.push_back(std::make_pair(signature->id(), nearestId));
01967
01968 ++proximityDetectionsAddedByICPOnly;
01969
01970
01971 if(proximityDetectionsAddedVisually == 0 && _loopClosureHypothesis.first == 0)
01972 {
01973 lastProximitySpaceClosureId = nearestId;
01974 }
01975 }
01976 else
01977 {
01978 UWARN("Ignoring local loop closure with %d because resulting "
01979 "transform is to large!? (%fm > %fm)",
01980 nearestId, transform.getNorm(), _proximityFilteringRadius);
01981 }
01982 }
01983 else
01984 {
01985 UWARN("Local scan matching rejected: %s", info.rejectedMsg.c_str());
01986 }
01987 }
01988 }
01989 }
01990 else
01991 {
01992 UDEBUG("Path %d ignored", nearestId);
01993 }
01994 }
01995 }
01996 }
01997 }
01998 }
01999 timeProximityBySpaceDetection = timer.ticks();
02000 ULOGGER_INFO("timeProximityBySpaceDetection=%fs", timeProximityBySpaceDetection);
02001
02002
02003
02004
02005 if(_path.size())
02006 {
02007
02008 if( signature->id() != _path[_pathCurrentIndex].first &&
02009 !signature->hasLink(_path[_pathCurrentIndex].first))
02010 {
02011 UASSERT(uContains(_optimizedPoses, signature->id()));
02012 UASSERT_MSG(uContains(_optimizedPoses, _path[_pathCurrentIndex].first), uFormat("id=%d", _path[_pathCurrentIndex].first).c_str());
02013 Transform virtualLoop = _optimizedPoses.at(signature->id()).inverse() * _optimizedPoses.at(_path[_pathCurrentIndex].first);
02014
02015 if(_localRadius == 0.0f || virtualLoop.getNorm() < _localRadius)
02016 {
02017 _memory->addLink(Link(signature->id(), _path[_pathCurrentIndex].first, Link::kVirtualClosure, virtualLoop, 100, 100));
02018 }
02019 else
02020 {
02021 UERROR("Virtual link larger than local radius (%fm > %fm). Aborting the plan!",
02022 virtualLoop.getNorm(), _localRadius);
02023 this->clearPath(-1);
02024 }
02025 }
02026 }
02027
02028
02029
02030
02031 float maxLinearError = 0.0f;
02032 double optimizationError = 0.0;
02033 int optimizationIterations = 0;
02034 if(_rgbdSlamMode &&
02035 (_loopClosureHypothesis.first>0 ||
02036 lastProximitySpaceClosureId>0 ||
02037 statistics_.reducedIds().size() ||
02038 proximityDetectionsInTimeFound>0 ||
02039 ((_memory->isIncremental() || signature->getLinks().size()) &&
02040 signaturesRetrieved.size())))
02041 {
02042 UASSERT(uContains(_optimizedPoses, signature->id()));
02043
02044
02045 std::map<int, Link> localizationLinks = graph::filterLinks(signature->getLinks(), Link::kVirtualClosure);
02046
02047
02048
02049
02050
02051 if(!_memory->isIncremental() &&
02052 signaturesRetrieved.size() == 0 &&
02053 localizationLinks.size() &&
02054 uContains(_optimizedPoses, localizationLinks.begin()->first))
02055 {
02056
02057
02058
02059
02060 UINFO("Localization without map optimization");
02061 if(_optimizeFromGraphEnd)
02062 {
02063
02064
02065
02066 Transform oldPose = _optimizedPoses.at(localizationLinks.begin()->first);
02067 Transform u = signature->getPose() * localizationLinks.begin()->second.transform();
02068 Transform up = u * oldPose.inverse();
02069 Transform mapCorrectionInv = _mapCorrection.inverse();
02070 for(std::map<int, Transform>::iterator iter=_optimizedPoses.begin(); iter!=_optimizedPoses.end(); ++iter)
02071 {
02072 iter->second = mapCorrectionInv * up * iter->second;
02073 }
02074 _optimizedPoses.at(signature->id()) = signature->getPose();
02075 }
02076 else
02077 {
02078 _optimizedPoses.at(signature->id()) = _optimizedPoses.at(localizationLinks.begin()->first) * localizationLinks.begin()->second.transform().inverse();
02079 }
02080 }
02081 else
02082 {
02083
02084 UINFO("Update map correction");
02085 std::map<int, Transform> poses = _optimizedPoses;
02086
02087
02088 float normMapCorrection = _mapCorrection.getNormSquared();
02089 if((normMapCorrection > 0.001f && _optimizeFromGraphEnd) ||
02090 (normMapCorrection < 0.001f && !_optimizeFromGraphEnd))
02091 {
02092 poses.clear();
02093 }
02094
02095 std::multimap<int, Link> constraints;
02096 optimizeCurrentMap(signature->id(), false, poses, &constraints, &optimizationError, &optimizationIterations);
02097
02098
02099
02100 bool updateConstraints = true;
02101 if(poses.empty())
02102 {
02103 UWARN("Graph optimization failed! Rejecting last loop closures added.");
02104 for(std::list<std::pair<int, int> >::iterator iter=loopClosureLinksAdded.begin(); iter!=loopClosureLinksAdded.end(); ++iter)
02105 {
02106 _memory->removeLink(iter->first, iter->second);
02107 UWARN("Loop closure %d->%d rejected!", iter->first, iter->second);
02108 }
02109 updateConstraints = false;
02110 _loopClosureHypothesis.first = 0;
02111 lastProximitySpaceClosureId = 0;
02112 rejectedHypothesis = true;
02113 }
02114 else if(_memory->isIncremental() &&
02115 _optimizationMaxLinearError > 0.0f &&
02116 loopClosureLinksAdded.size() &&
02117 optimizationIterations > 0)
02118 {
02119 const Link * maxLinearLink = 0;
02120 for(std::multimap<int, Link>::iterator iter=constraints.begin(); iter!=constraints.end(); ++iter)
02121 {
02122
02123 if(iter->second.transVariance() < 1.0)
02124 {
02125 Transform t1 = uValue(poses, iter->second.from(), Transform());
02126 Transform t2 = uValue(poses, iter->second.to(), Transform());
02127 Transform t = t1.inverse()*t2;
02128 float linearError = uMax3(
02129 fabs(iter->second.transform().x() - t.x()),
02130 fabs(iter->second.transform().y() - t.y()),
02131 fabs(iter->second.transform().z() - t.z()));
02132 if(linearError > maxLinearError)
02133 {
02134 maxLinearError = linearError;
02135 maxLinearLink = &iter->second;
02136 }
02137 }
02138 }
02139
02140 if(maxLinearError > _optimizationMaxLinearError)
02141 {
02142 UWARN("Rejecting all added loop closures (%d) in this "
02143 "iteration because a wrong loop closure has been "
02144 "detected after graph optimization, resulting in "
02145 "a maximum graph error of %f m (edge %d->%d, type=%d). The "
02146 "maximum error parameter is %f m.",
02147 (int)loopClosureLinksAdded.size(),
02148 maxLinearError,
02149 maxLinearLink->from(),
02150 maxLinearLink->to(),
02151 maxLinearLink->type(),
02152 _optimizationMaxLinearError);
02153 for(std::list<std::pair<int, int> >::iterator iter=loopClosureLinksAdded.begin(); iter!=loopClosureLinksAdded.end(); ++iter)
02154 {
02155 _memory->removeLink(iter->first, iter->second);
02156 UWARN("Loop closure %d->%d rejected!", iter->first, iter->second);
02157 }
02158 updateConstraints = false;
02159 _loopClosureHypothesis.first = 0;
02160 lastProximitySpaceClosureId = 0;
02161 rejectedHypothesis = true;
02162 }
02163 }
02164
02165 if(updateConstraints)
02166 {
02167 UINFO("Updated local map (old size=%d, new size=%d)", (int)_optimizedPoses.size(), (int)poses.size());
02168 _optimizedPoses = poses;
02169 _constraints = constraints;
02170 }
02171 }
02172
02173
02174 _mapCorrection = _optimizedPoses.at(signature->id()) * signature->getPose().inverse();
02175 _lastLocalizationPose = _optimizedPoses.at(signature->id());
02176 if(_mapCorrection.getNormSquared() > 0.001f && _optimizeFromGraphEnd)
02177 {
02178 UERROR("Map correction should be identity when optimizing from the last node. T=%s", _mapCorrection.prettyPrint().c_str());
02179 }
02180 }
02181 _lastLocalizationNodeId = _loopClosureHypothesis.first>0?_loopClosureHypothesis.first:lastProximitySpaceClosureId>0?lastProximitySpaceClosureId:_lastLocalizationNodeId;
02182
02183 timeMapOptimization = timer.ticks();
02184 ULOGGER_INFO("timeMapOptimization=%fs", timeMapOptimization);
02185
02186
02187
02188
02189
02190 int dictionarySize = 0;
02191 int refWordsCount = 0;
02192 int refUniqueWordsCount = 0;
02193 int lcHypothesisReactivated = 0;
02194 float rehearsalValue = uValue(statistics_.data(), Statistics::kMemoryRehearsal_sim(), 0.0f);
02195 int rehearsalMaxId = (int)uValue(statistics_.data(), Statistics::kMemoryRehearsal_merged(), 0.0f);
02196 sLoop = _memory->getSignature(_loopClosureHypothesis.first?_loopClosureHypothesis.first:lastProximitySpaceClosureId?lastProximitySpaceClosureId:_highestHypothesis.first);
02197 if(sLoop)
02198 {
02199 lcHypothesisReactivated = sLoop->isSaved()?1.0f:0.0f;
02200 }
02201 dictionarySize = (int)_memory->getVWDictionary()->getVisualWords().size();
02202 refWordsCount = (int)signature->getWords().size();
02203 refUniqueWordsCount = (int)uUniqueKeys(signature->getWords()).size();
02204
02205
02206 float vpHypothesis = posterior.size()?posterior.at(Memory::kIdVirtual):0.0f;
02207
02208
02209 if(_loopClosureHypothesis.first || _publishStats)
02210 {
02211 ULOGGER_INFO("sending stats...");
02212 statistics_.setRefImageId(_memory->getLastSignatureId());
02213 statistics_.setStamp(data.stamp());
02214 if(_loopClosureHypothesis.first != Memory::kIdInvalid)
02215 {
02216 statistics_.setLoopClosureId(_loopClosureHypothesis.first);
02217 ULOGGER_INFO("Loop closure detected! With id=%d", _loopClosureHypothesis.first);
02218 }
02219 if(_publishStats)
02220 {
02221 ULOGGER_INFO("send all stats...");
02222 statistics_.setExtended(1);
02223
02224 statistics_.addStatistic(Statistics::kLoopAccepted_hypothesis_id(), _loopClosureHypothesis.first);
02225 statistics_.addStatistic(Statistics::kLoopHighest_hypothesis_id(), _highestHypothesis.first);
02226 statistics_.addStatistic(Statistics::kLoopHighest_hypothesis_value(), _highestHypothesis.second);
02227 statistics_.addStatistic(Statistics::kLoopHypothesis_reactivated(), lcHypothesisReactivated);
02228 statistics_.addStatistic(Statistics::kLoopVp_hypothesis(), vpHypothesis);
02229 statistics_.addStatistic(Statistics::kLoopReactivate_id(), retrievalId);
02230 statistics_.addStatistic(Statistics::kLoopHypothesis_ratio(), hypothesisRatio);
02231 statistics_.addStatistic(Statistics::kLoopVisual_inliers(), loopClosureVisualInliers);
02232 statistics_.addStatistic(Statistics::kLoopLast_id(), _memory->getLastGlobalLoopClosureId());
02233 statistics_.addStatistic(Statistics::kLoopOptimization_max_error(), maxLinearError);
02234 statistics_.addStatistic(Statistics::kLoopOptimization_error(), optimizationError);
02235 statistics_.addStatistic(Statistics::kLoopOptimization_iterations(), optimizationIterations);
02236
02237 statistics_.addStatistic(Statistics::kProximityTime_detections(), proximityDetectionsInTimeFound);
02238 statistics_.addStatistic(Statistics::kProximitySpace_detections_added_visually(), proximityDetectionsAddedVisually);
02239 statistics_.addStatistic(Statistics::kProximitySpace_detections_added_icp_only(), proximityDetectionsAddedByICPOnly);
02240 statistics_.addStatistic(Statistics::kProximitySpace_paths(), proximitySpacePaths);
02241 statistics_.addStatistic(Statistics::kProximitySpace_last_detection_id(), lastProximitySpaceClosureId);
02242 statistics_.setProximityDetectionId(lastProximitySpaceClosureId);
02243 if(_loopClosureHypothesis.first || lastProximitySpaceClosureId)
02244 {
02245 UASSERT(uContains(sLoop->getLinks(), signature->id()));
02246 UINFO("Set loop closure transform = %s", sLoop->getLinks().at(signature->id()).transform().prettyPrint().c_str());
02247 statistics_.setLoopClosureTransform(sLoop->getLinks().at(signature->id()).transform());
02248 }
02249 statistics_.setMapCorrection(_mapCorrection);
02250 UINFO("Set map correction = %s", _mapCorrection.prettyPrint().c_str());
02251
02252
02253 statistics_.addStatistic(Statistics::kTimingMemory_update(), timeMemoryUpdate*1000);
02254 statistics_.addStatistic(Statistics::kTimingNeighbor_link_refining(), timeNeighborLinkRefining*1000);
02255 statistics_.addStatistic(Statistics::kTimingProximity_by_time(), timeProximityByTimeDetection*1000);
02256 statistics_.addStatistic(Statistics::kTimingProximity_by_space(), timeProximityBySpaceDetection*1000);
02257 statistics_.addStatistic(Statistics::kTimingReactivation(), timeReactivations*1000);
02258 statistics_.addStatistic(Statistics::kTimingAdd_loop_closure_link(), timeAddLoopClosureLink*1000);
02259 statistics_.addStatistic(Statistics::kTimingMap_optimization(), timeMapOptimization*1000);
02260 statistics_.addStatistic(Statistics::kTimingLikelihood_computation(), timeLikelihoodCalculation*1000);
02261 statistics_.addStatistic(Statistics::kTimingPosterior_computation(), timePosteriorCalculation*1000);
02262 statistics_.addStatistic(Statistics::kTimingHypotheses_creation(), timeHypothesesCreation*1000);
02263 statistics_.addStatistic(Statistics::kTimingHypotheses_validation(), timeHypothesesValidation*1000);
02264 statistics_.addStatistic(Statistics::kTimingCleaning_neighbors(), timeCleaningNeighbors*1000);
02265
02266
02267 statistics_.addStatistic(Statistics::kMemorySignatures_retrieved(), (float)signaturesRetrieved.size());
02268
02269
02270 statistics_.addStatistic(Statistics::kKeypointDictionary_size(), dictionarySize);
02271 statistics_.addStatistic(Statistics::kKeypointIndexed_words(), _memory->getVWDictionary()->getIndexedWordsCount());
02272 statistics_.addStatistic(Statistics::kKeypointIndex_memory_usage(), _memory->getVWDictionary()->getIndexMemoryUsed());
02273
02274
02275 statistics_.addStatistic(Statistics::kLoopRejectedHypothesis(), rejectedHypothesis?1.0f:0);
02276
02277 statistics_.addStatistic(Statistics::kMemorySmall_movement(), smallDisplacement?1.0f:0);
02278 statistics_.addStatistic(Statistics::kMemoryDistance_travelled(), _distanceTravelled);
02279
02280 if(_publishLikelihood || _publishPdf)
02281 {
02282
02283 statistics_.setWeights(weights);
02284 if(_publishPdf)
02285 {
02286 statistics_.setPosterior(posterior);
02287 }
02288 if(_publishLikelihood)
02289 {
02290 statistics_.setLikelihood(likelihood);
02291 statistics_.setRawLikelihood(rawLikelihood);
02292 }
02293 }
02294
02295
02296 if(_path.size())
02297 {
02298 statistics_.setLocalPath(this->getPathNextNodes());
02299 statistics_.setCurrentGoalId(this->getPathCurrentGoalId());
02300 }
02301 }
02302
02303 timeStatsCreation = timer.ticks();
02304 ULOGGER_INFO("Time creating stats = %f...", timeStatsCreation);
02305 }
02306
02307 Signature lastSignatureData(signature->id());
02308 if(_publishLastSignatureData)
02309 {
02310 lastSignatureData = *signature;
02311 }
02312 if(!_rawDataKept)
02313 {
02314 _memory->removeRawData(signature->id(), true, !_neighborLinkRefining && !_proximityBySpace, true);
02315 }
02316
02317
02318 int signatureRemoved = _memory->cleanup();
02319 if(signatureRemoved)
02320 {
02321 signaturesRemoved.push_back(signatureRemoved);
02322 }
02323
02324
02325
02326
02327
02328 if(signatureRemoved != lastSignatureData.id())
02329 {
02330 if(_startNewMapOnLoopClosure &&
02331 _memory->isIncremental() &&
02332 signature->getLinks().size() == 0 &&
02333 _memory->getWorkingMem().size()>1)
02334 {
02335 UWARN("Ignoring location %d because a global loop closure is required before starting a new map!",
02336 signature->id());
02337 signaturesRemoved.push_back(signature->id());
02338 _memory->deleteLocation(signature->id());
02339 }
02340 else if(smallDisplacement && _loopClosureHypothesis.first == 0 && lastProximitySpaceClosureId == 0)
02341 {
02342
02343 UINFO("Ignoring location %d because the displacement is too small! (d=%f a=%f)",
02344 signature->id(), _rgbdLinearUpdate, _rgbdAngularUpdate);
02345
02346 signaturesRemoved.push_back(signature->id());
02347 _memory->deleteLocation(signature->id());
02348 }
02349 }
02350
02351
02352 signature = 0;
02353
02354 timeMemoryCleanup = timer.ticks();
02355 ULOGGER_INFO("timeMemoryCleanup = %fs... %d signatures removed", timeMemoryCleanup, (int)signaturesRemoved.size());
02356
02357
02358
02359
02360
02361
02362
02363
02364
02365
02366
02367 double totalTime = timerTotal.ticks();
02368 ULOGGER_INFO("Total time processing = %fs...", totalTime);
02369 if((_maxTimeAllowed != 0 && totalTime*1000>_maxTimeAllowed) ||
02370 (_maxMemoryAllowed != 0 && _memory->getWorkingMem().size() > _maxMemoryAllowed))
02371 {
02372 ULOGGER_INFO("Removing old signatures because time limit is reached %f>%f or memory is reached %d>%d...", totalTime*1000, _maxTimeAllowed, _memory->getWorkingMem().size(), _maxMemoryAllowed);
02373 immunizedLocations.insert(_lastLocalizationNodeId);
02374 std::list<int> transferred = _memory->forget(immunizedLocations);
02375 signaturesRemoved.insert(signaturesRemoved.end(), transferred.begin(), transferred.end());
02376 if(!_someNodesHaveBeenTransferred && transferred.size())
02377 {
02378 _someNodesHaveBeenTransferred = true;
02379 }
02380 }
02381 _lastProcessTime = totalTime;
02382
02383
02384 if(signaturesRemoved.size() && (_optimizedPoses.size() || _constraints.size()))
02385 {
02386
02387 int id = 0;
02388 if(!_memory->isIncremental() && (_lastLocalizationNodeId > 0 || _path.size()))
02389 {
02390 if(_path.size())
02391 {
02392
02393 UASSERT(_pathCurrentIndex < _path.size());
02394 UASSERT_MSG(uContains(_optimizedPoses, _path.at(_pathCurrentIndex).first), uFormat("id=%d", _path.at(_pathCurrentIndex).first).c_str());
02395 id = _path.at(_pathCurrentIndex).first;
02396 UDEBUG("Refresh local map from %d", id);
02397 }
02398 else
02399 {
02400 UASSERT_MSG(uContains(_optimizedPoses, _lastLocalizationNodeId), uFormat("id=%d isInWM?=%d", _lastLocalizationNodeId, _memory->isInWM(_lastLocalizationNodeId)?1:0).c_str());
02401 id = _lastLocalizationNodeId;
02402 UDEBUG("Refresh local map from %d", id);
02403 }
02404 }
02405 else if(_memory->isIncremental() &&
02406 _optimizedPoses.size() &&
02407 _memory->getLastWorkingSignature())
02408 {
02409 id = _memory->getLastWorkingSignature()->id();
02410 UDEBUG("Refresh local map from %d", id);
02411 }
02412 if(id > 0)
02413 {
02414 if(_lastLocalizationNodeId != 0)
02415 {
02416 _lastLocalizationNodeId = id;
02417 }
02418 UASSERT_MSG(_memory->getSignature(id) != 0, uFormat("id=%d", id).c_str());
02419 std::map<int, int> ids = _memory->getNeighborsId(id, 0, 0, true);
02420 for(std::map<int, Transform>::iterator iter=_optimizedPoses.begin(); iter!=_optimizedPoses.end();)
02421 {
02422 if(!uContains(ids, iter->first))
02423 {
02424 UDEBUG("Removed %d from local map", iter->first);
02425 UASSERT(iter->first != _lastLocalizationNodeId);
02426 _optimizedPoses.erase(iter++);
02427 }
02428 else
02429 {
02430 ++iter;
02431 }
02432 }
02433 for(std::multimap<int, Link>::iterator iter=_constraints.begin(); iter!=_constraints.end();)
02434 {
02435 if(!uContains(ids, iter->second.from()) || !uContains(ids, iter->second.to()))
02436 {
02437 _constraints.erase(iter++);
02438 }
02439 else
02440 {
02441 ++iter;
02442 }
02443 }
02444 }
02445 else
02446 {
02447 _optimizedPoses.clear();
02448 _constraints.clear();
02449 }
02450 }
02451
02452 if(_path.size())
02453 {
02454 UASSERT(_pathCurrentIndex < _path.size());
02455 UASSERT(_pathGoalIndex < _path.size());
02456 UASSERT_MSG(uContains(_optimizedPoses, _path.at(_pathCurrentIndex).first), uFormat("local map size=%d, id=%d", (int)_optimizedPoses.size(), _path.at(_pathCurrentIndex).first).c_str());
02457 UASSERT_MSG(uContains(_optimizedPoses, _path.at(_pathGoalIndex).first), uFormat("local map size=%d, id=%d", (int)_optimizedPoses.size(), _path.at(_pathGoalIndex).first).c_str());
02458 }
02459
02460
02461 timeRealTimeLimitReachedProcess = timer.ticks();
02462 ULOGGER_INFO("Time limit reached processing = %f...", timeRealTimeLimitReachedProcess);
02463
02464
02465
02466
02467 int localGraphSize = 0;
02468 if(_publishStats)
02469 {
02470 statistics_.addStatistic(Statistics::kTimingStatistics_creation(), timeStatsCreation*1000);
02471 statistics_.addStatistic(Statistics::kTimingTotal(), totalTime*1000);
02472 statistics_.addStatistic(Statistics::kTimingForgetting(), timeRealTimeLimitReachedProcess*1000);
02473 statistics_.addStatistic(Statistics::kTimingJoining_trash(), timeJoiningTrash*1000);
02474 statistics_.addStatistic(Statistics::kTimingEmptying_trash(), timeEmptyingTrash*1000);
02475 statistics_.addStatistic(Statistics::kTimingMemory_cleanup(), timeMemoryCleanup*1000);
02476
02477
02478 statistics_.addStatistic(Statistics::kMemorySignatures_removed(), signaturesRemoved.size());
02479 statistics_.addStatistic(Statistics::kMemoryImmunized_globally(), immunizedGlobally);
02480 statistics_.addStatistic(Statistics::kMemoryImmunized_locally(), immunizedLocally);
02481 statistics_.addStatistic(Statistics::kMemoryImmunized_locally_max(), maxLocalLocationsImmunized);
02482
02483
02484 statistics_.addStatistic(Statistics::kMemoryWorking_memory_size(), _memory->getWorkingMem().size());
02485 statistics_.addStatistic(Statistics::kMemoryShort_time_memory_size(), _memory->getStMem().size());
02486 statistics_.addStatistic(Statistics::kMemoryDatabase_memory_used(), _memory->getDatabaseMemoryUsed());
02487
02488 std::map<int, Signature> signatures;
02489 if(_publishLastSignatureData)
02490 {
02491 UINFO("Adding data %d (rgb/left=%d depth/right=%d)", lastSignatureData.id(), lastSignatureData.sensorData().imageRaw().empty()?0:1, lastSignatureData.sensorData().depthOrRightRaw().empty()?0:1);
02492 signatures.insert(std::make_pair(lastSignatureData.id(), lastSignatureData));
02493 }
02494
02495 std::map<int, Transform> poses;
02496 std::multimap<int, Link> constraints;
02497 if(!_rgbdSlamMode)
02498 {
02499
02500 std::map<int, int> ids = _memory->getNeighborsId(lastSignatureData.id(), 0, 0, true);
02501 _memory->getMetricConstraints(uKeysSet(ids), poses, constraints, false);
02502 }
02503 else
02504 {
02505 poses = _optimizedPoses;
02506 constraints = _constraints;
02507 }
02508 for(std::map<int, Transform>::iterator iter=poses.begin(); iter!=poses.end(); ++iter)
02509 {
02510 Transform odomPose;
02511 int weight = -1;
02512 int mapId = -1;
02513 std::string label;
02514 double stamp = 0;
02515 Transform groundTruth;
02516 std::vector<unsigned char> userData;
02517 _memory->getNodeInfo(iter->first, odomPose, mapId, weight, label, stamp, groundTruth, false);
02518 signatures.insert(std::make_pair(iter->first,
02519 Signature(iter->first,
02520 mapId,
02521 weight,
02522 stamp,
02523 label,
02524 odomPose,
02525 groundTruth)));
02526 }
02527 statistics_.setPoses(poses);
02528 statistics_.setConstraints(constraints);
02529 statistics_.setSignatures(signatures);
02530 statistics_.addStatistic(Statistics::kMemoryLocal_graph_size(), poses.size());
02531 localGraphSize = (int)poses.size();
02532 }
02533
02534
02535 _memory->emptyTrash();
02536
02537
02538
02539 if(_foutFloat && _foutInt)
02540 {
02541 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",
02542 totalTime,
02543 timeMemoryUpdate,
02544 timeReactivations,
02545 timeLikelihoodCalculation,
02546 timePosteriorCalculation,
02547 timeHypothesesCreation,
02548 timeHypothesesValidation,
02549 timeRealTimeLimitReachedProcess,
02550 timeStatsCreation,
02551 _highestHypothesis.second,
02552 0.0f,
02553 0.0f,
02554 0.0f,
02555 0.0f,
02556 0.0f,
02557 vpHypothesis,
02558 timeJoiningTrash,
02559 rehearsalValue,
02560 timeEmptyingTrash,
02561 timeRetrievalDbAccess,
02562 timeAddLoopClosureLink,
02563 timeMemoryCleanup,
02564 timeNeighborLinkRefining,
02565 timeProximityByTimeDetection,
02566 timeProximityBySpaceDetection,
02567 timeMapOptimization);
02568 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",
02569 _loopClosureHypothesis.first,
02570 _highestHypothesis.first,
02571 (int)signaturesRemoved.size(),
02572 0,
02573 refWordsCount,
02574 dictionarySize,
02575 int(_memory->getWorkingMem().size()),
02576 rejectedHypothesis?1:0,
02577 0,
02578 0,
02579 int(signaturesRetrieved.size()),
02580 lcHypothesisReactivated,
02581 refUniqueWordsCount,
02582 retrievalId,
02583 0,
02584 rehearsalMaxId,
02585 rehearsalMaxId>0?1:0,
02586 localGraphSize,
02587 data.id(),
02588 _memory->getVWDictionary()->getIndexedWordsCount(),
02589 _memory->getVWDictionary()->getIndexMemoryUsed());
02590 if(_statisticLogsBufferedInRAM)
02591 {
02592 _bufferedLogsF.push_back(logF);
02593 _bufferedLogsI.push_back(logI);
02594 }
02595 else
02596 {
02597 if(_foutFloat)
02598 {
02599 fprintf(_foutFloat, "%s", logF.c_str());
02600 }
02601 if(_foutInt)
02602 {
02603 fprintf(_foutInt, "%s", logI.c_str());
02604 }
02605 }
02606 UINFO("Time logging = %f...", timer.ticks());
02607
02608 }
02609 UDEBUG("End process");
02610
02611 return true;
02612 }
02613
02614 bool Rtabmap::process(const cv::Mat & image, int id)
02615 {
02616 return this->process(SensorData(image, id), Transform());
02617 }
02618
02619
02620 void Rtabmap::setTimeThreshold(float maxTimeAllowed)
02621 {
02622
02623 _maxTimeAllowed = maxTimeAllowed;
02624 if(_maxTimeAllowed < 0)
02625 {
02626 ULOGGER_WARN("maxTimeAllowed < 0, then setting it to 0 (inf).");
02627 _maxTimeAllowed = 0;
02628 }
02629 else if(_maxTimeAllowed > 0.0f && _maxTimeAllowed < 1.0f)
02630 {
02631 ULOGGER_WARN("Time threshold set to %fms, it is not in seconds!", _maxTimeAllowed);
02632 }
02633 }
02634
02635 void Rtabmap::setWorkingDirectory(std::string path)
02636 {
02637 if(!path.empty() && UDirectory::exists(path))
02638 {
02639 ULOGGER_DEBUG("Comparing new working directory path \"%s\" with \"%s\"", path.c_str(), _wDir.c_str());
02640 if(path.compare(_wDir) != 0)
02641 {
02642 _wDir = path;
02643 if(_memory)
02644 {
02645 this->resetMemory();
02646 }
02647 else
02648 {
02649 setupLogFiles();
02650 }
02651 }
02652 }
02653 else if(path.empty())
02654 {
02655 _wDir.clear();
02656 setupLogFiles();
02657 }
02658 else
02659 {
02660 ULOGGER_ERROR("Directory \"%s\" doesn't exist!", path.c_str());
02661 }
02662 }
02663
02664 void Rtabmap::rejectLoopClosure(int oldId, int newId)
02665 {
02666 UDEBUG("_loopClosureHypothesis.first=%d", _loopClosureHypothesis.first);
02667 if(_loopClosureHypothesis.first)
02668 {
02669 _loopClosureHypothesis.first = 0;
02670 if(_memory)
02671 {
02672 _memory->removeLink(oldId, newId);
02673 }
02674 if(uContains(statistics_.data(), rtabmap::Statistics::kLoopRejectedHypothesis()))
02675 {
02676 statistics_.addStatistic(rtabmap::Statistics::kLoopRejectedHypothesis(), 1.0f);
02677 }
02678 statistics_.setLoopClosureId(0);
02679 }
02680 }
02681
02682 void Rtabmap::setOptimizedPoses(const std::map<int, Transform> & poses)
02683 {
02684 _optimizedPoses = poses;
02685 }
02686
02687 void Rtabmap::dumpData() const
02688 {
02689 UDEBUG("");
02690 if(_memory)
02691 {
02692 if(this->getWorkingDir().empty())
02693 {
02694 UERROR("Working directory not set.");
02695 }
02696 else
02697 {
02698 _memory->dumpMemory(this->getWorkingDir());
02699 }
02700 }
02701 }
02702
02703
02704
02705 std::map<int, Transform> Rtabmap::getForwardWMPoses(
02706 int fromId,
02707 int maxNearestNeighbors,
02708 float radius,
02709 int maxGraphDepth
02710 ) const
02711 {
02712 std::map<int, Transform> poses;
02713 if(_memory && fromId > 0)
02714 {
02715 UDEBUG("");
02716 const Signature * fromS = _memory->getSignature(fromId);
02717 UASSERT(fromS != 0);
02718 UASSERT(_optimizedPoses.find(fromId) != _optimizedPoses.end());
02719
02720 pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
02721 cloud->resize(_optimizedPoses.size());
02722 std::vector<int> ids(_optimizedPoses.size());
02723 int oi = 0;
02724 const std::set<int> & stm = _memory->getStMem();
02725
02726 std::map<int, float> foundIds;
02727 if(_memory->isIncremental())
02728 {
02729 foundIds = _memory->getNeighborsIdRadius(fromId, radius, _optimizedPoses, maxGraphDepth);
02730 }
02731 else
02732 {
02733 foundIds = graph::getNodesInRadius(fromId, _optimizedPoses, radius);
02734 }
02735
02736 float radiusSqrd = radius * radius;
02737 for(std::map<int, Transform>::const_iterator iter = _optimizedPoses.begin(); iter!=_optimizedPoses.end(); ++iter)
02738 {
02739 if(iter->first != fromId)
02740 {
02741 if(stm.find(iter->first) == stm.end() &&
02742 uContains(foundIds, iter->first) &&
02743 (radiusSqrd==0 || foundIds.at(iter->first) <= radiusSqrd))
02744 {
02745 (*cloud)[oi] = pcl::PointXYZ(iter->second.x(), iter->second.y(), iter->second.z());
02746 ids[oi++] = iter->first;
02747 }
02748 }
02749 }
02750
02751 cloud->resize(oi);
02752 ids.resize(oi);
02753
02754 Transform fromT = _optimizedPoses.at(fromId);
02755
02756 if(cloud->size())
02757 {
02758
02759
02760
02761
02762
02763
02764
02765 float x,y,z, roll,pitch,yaw;
02766 fromT.getTranslationAndEulerAngles(x,y,z, roll,pitch,yaw);
02767
02768 pcl::CropBox<pcl::PointXYZ> cropbox;
02769 cropbox.setInputCloud(cloud);
02770 cropbox.setMin(Eigen::Vector4f(-1, -radius, -999999, 0));
02771 cropbox.setMax(Eigen::Vector4f(radius, radius, 999999, 0));
02772 cropbox.setRotation(Eigen::Vector3f(roll, pitch, yaw));
02773 cropbox.setTranslation(Eigen::Vector3f(x, y, z));
02774 cropbox.setRotation(Eigen::Vector3f(roll,pitch,yaw));
02775 pcl::IndicesPtr indices(new std::vector<int>());
02776 cropbox.filter(*indices);
02777
02778
02779
02780
02781
02782
02783
02784 if(indices->size())
02785 {
02786 pcl::search::KdTree<pcl::PointXYZ>::Ptr kdTree(new pcl::search::KdTree<pcl::PointXYZ>);
02787 kdTree->setInputCloud(cloud, indices);
02788 std::vector<int> ind;
02789 std::vector<float> dist;
02790 pcl::PointXYZ pt(fromT.x(), fromT.y(), fromT.z());
02791 kdTree->radiusSearch(pt, radius, ind, dist, maxNearestNeighbors);
02792
02793 for(unsigned int i=0; i<ind.size(); ++i)
02794 {
02795 if(ind[i] >=0)
02796 {
02797 Transform tmp = _optimizedPoses.find(ids[ind[i]])->second;
02798
02799 UDEBUG("Inlier %d: %s", ids[ind[i]], tmp.prettyPrint().c_str());
02800 poses.insert(std::make_pair(ids[ind[i]], tmp));
02801 }
02802 }
02803
02804
02805
02806
02807
02808
02809
02810
02811
02812
02813
02814
02815 }
02816 }
02817 }
02818 return poses;
02819 }
02820
02821 std::list<std::map<int, Transform> > Rtabmap::getPaths(std::map<int, Transform> poses) const
02822 {
02823 std::list<std::map<int, Transform> > paths;
02824 if(_memory && poses.size())
02825 {
02826
02827 while(poses.size())
02828 {
02829 std::map<int, Transform> path;
02830 for(std::map<int, Transform>::iterator iter=poses.begin(); iter!=poses.end();)
02831 {
02832 if(path.size() == 0 || uContains(_memory->getNeighborLinks(path.rbegin()->first), iter->first))
02833 {
02834 path.insert(*iter);
02835 poses.erase(iter++);
02836 }
02837 else
02838 {
02839 break;
02840 }
02841 }
02842 UASSERT(path.size());
02843 paths.push_back(path);
02844 }
02845
02846 }
02847 return paths;
02848 }
02849
02850 void Rtabmap::optimizeCurrentMap(
02851 int id,
02852 bool lookInDatabase,
02853 std::map<int, Transform> & optimizedPoses,
02854 std::multimap<int, Link> * constraints,
02855 double * error,
02856 int * iterationsDone) const
02857 {
02858
02859 UINFO("Optimize map: around location %d", id);
02860 if(_memory && id > 0)
02861 {
02862 UTimer timer;
02863 std::map<int, int> ids = _memory->getNeighborsId(id, 0, lookInDatabase?-1:0, true, false);
02864 if(!_optimizeFromGraphEnd && ids.size() > 1)
02865 {
02866 id = ids.begin()->first;
02867 }
02868 UINFO("get %d ids time %f s", (int)ids.size(), timer.ticks());
02869
02870 std::map<int, Transform> poses = Rtabmap::optimizeGraph(id, uKeysSet(ids), optimizedPoses, lookInDatabase, constraints, error, iterationsDone);
02871 UINFO("optimize time %f s", timer.ticks());
02872
02873 if(poses.size())
02874 {
02875 optimizedPoses = poses;
02876
02877 if(_memory->getSignature(id) && uContains(optimizedPoses, id))
02878 {
02879 Transform t = optimizedPoses.at(id) * _memory->getSignature(id)->getPose().inverse();
02880 UINFO("Correction (from node %d) %s", id, t.prettyPrint().c_str());
02881 }
02882 }
02883 else
02884 {
02885 UERROR("Failed to optimize the graph! returning empty optimized poses...");
02886 optimizedPoses.clear();
02887 if(constraints)
02888 {
02889 constraints->clear();
02890 }
02891 }
02892 }
02893 }
02894
02895 std::map<int, Transform> Rtabmap::optimizeGraph(
02896 int fromId,
02897 const std::set<int> & ids,
02898 const std::map<int, Transform> & guessPoses,
02899 bool lookInDatabase,
02900 std::multimap<int, Link> * constraints,
02901 double * error,
02902 int * iterationsDone) const
02903 {
02904 UTimer timer;
02905 std::map<int, Transform> optimizedPoses;
02906 std::map<int, Transform> poses, posesOut;
02907 std::multimap<int, Link> edgeConstraints, linksOut;
02908 UDEBUG("ids=%d", (int)ids.size());
02909 _memory->getMetricConstraints(ids, poses, edgeConstraints, lookInDatabase);
02910 UINFO("get constraints (ids=%d, %d poses, %d edges) time %f s", (int)ids.size(), (int)poses.size(), (int)edgeConstraints.size(), timer.ticks());
02911
02912
02913 if(_graphOptimizer->iterations() > 0)
02914 {
02915 for(std::map<int, Transform>::const_iterator iter=guessPoses.begin(); iter!=guessPoses.end(); ++iter)
02916 {
02917 std::map<int, Transform>::iterator foundPose = poses.find(iter->first);
02918 if(foundPose!=poses.end())
02919 {
02920 foundPose->second = iter->second;
02921 }
02922 }
02923 }
02924
02925
02926 if(ULogger::level() == ULogger::kDebug)
02927 {
02928 _graphOptimizer->getConnectedGraph(fromId, poses, edgeConstraints, posesOut, linksOut);
02929 if(poses.size() != posesOut.size())
02930 {
02931 for(std::map<int, Transform>::iterator iter=poses.begin(); iter!=poses.end(); ++iter)
02932 {
02933 if(posesOut.find(iter->first) == posesOut.end())
02934 {
02935 UERROR("Not found %d in posesOut", iter->first);
02936 for(std::multimap<int, Link>::iterator jter=edgeConstraints.begin(); jter!=edgeConstraints.end(); ++jter)
02937 {
02938 if(jter->second.from() == iter->first || jter->second.to()==iter->first)
02939 {
02940 UERROR("Found link %d->%d", jter->second.from(), jter->second.to());
02941 }
02942 }
02943 }
02944 }
02945 }
02946 if(edgeConstraints.size() != linksOut.size())
02947 {
02948 for(std::multimap<int, Link>::iterator iter=edgeConstraints.begin(); iter!=edgeConstraints.end(); ++iter)
02949 {
02950 if(graph::findLink(linksOut, iter->second.from(), iter->second.to()) == linksOut.end())
02951 {
02952 UERROR("Not found link %d->%d in linksOut", iter->second.from(), iter->second.to());
02953 }
02954 }
02955 }
02956 UASSERT_MSG(poses.size() == posesOut.size() && edgeConstraints.size() == linksOut.size(),
02957 uFormat("nodes %d->%d, links %d->%d", poses.size(), posesOut.size(), edgeConstraints.size(), linksOut.size()).c_str());
02958 }
02959
02960 if(constraints)
02961 {
02962 *constraints = edgeConstraints;
02963 }
02964
02965 UASSERT(_graphOptimizer!=0);
02966 if(_graphOptimizer->iterations() == 0)
02967 {
02968
02969 optimizedPoses = poses;
02970 }
02971 else
02972 {
02973 optimizedPoses = _graphOptimizer->optimize(fromId, poses, edgeConstraints, 0, error, iterationsDone);
02974 }
02975 UINFO("Optimization time %f s", timer.ticks());
02976
02977 return optimizedPoses;
02978 }
02979
02980 void Rtabmap::adjustLikelihood(std::map<int, float> & likelihood) const
02981 {
02982 ULOGGER_DEBUG("likelihood.size()=%d", likelihood.size());
02983 UTimer timer;
02984 timer.start();
02985 if(likelihood.size()==0)
02986 {
02987 return;
02988 }
02989
02990
02991 std::list<float> values;
02992 bool likelihoodNullValuesIgnored = true;
02993 for(std::map<int, float>::iterator iter = ++likelihood.begin(); iter!=likelihood.end(); ++iter)
02994 {
02995 if((iter->second >= 0 && !likelihoodNullValuesIgnored) ||
02996 (iter->second > 0 && likelihoodNullValuesIgnored))
02997 {
02998 values.push_back(iter->second);
02999 }
03000 }
03001 UDEBUG("values.size=%d", values.size());
03002
03003 float mean = uMean(values);
03004 float stdDev = std::sqrt(uVariance(values, mean));
03005
03006
03007
03008 float epsilon = 0.0001;
03009 float max = 0.0f;
03010 int maxId = 0;
03011 for(std::map<int, float>::iterator iter=++likelihood.begin(); iter!= likelihood.end(); ++iter)
03012 {
03013 float value = iter->second;
03014 if(value > mean+stdDev && mean)
03015 {
03016 iter->second = (value-(stdDev-epsilon))/mean;
03017 if(value > max)
03018 {
03019 max = value;
03020 maxId = iter->first;
03021 }
03022 }
03023 else if(value == 1.0f && stdDev == 0)
03024 {
03025 iter->second = 1.0f;
03026 if(value > max)
03027 {
03028 max = value;
03029 maxId = iter->first;
03030 }
03031 }
03032 else
03033 {
03034 iter->second = 1.0f;
03035 }
03036 }
03037
03038 if(stdDev > epsilon && max)
03039 {
03040 likelihood.begin()->second = mean/stdDev + 1.0f;
03041 }
03042 else
03043 {
03044 likelihood.begin()->second = 2.0f;
03045 }
03046
03047 double time = timer.ticks();
03048 UDEBUG("mean=%f, stdDev=%f, max=%f, maxId=%d, time=%fs", mean, stdDev, max, maxId, time);
03049 }
03050
03051 void Rtabmap::dumpPrediction() const
03052 {
03053 if(_memory && _bayesFilter)
03054 {
03055 if(this->getWorkingDir().empty())
03056 {
03057 UERROR("Working directory not set.");
03058 return;
03059 }
03060 std::list<int> signaturesToCompare;
03061 for(std::map<int, double>::const_iterator iter=_memory->getWorkingMem().begin();
03062 iter!=_memory->getWorkingMem().end();
03063 ++iter)
03064 {
03065 if(iter->first > 0)
03066 {
03067 const Signature * s = _memory->getSignature(iter->first);
03068 UASSERT(s!=0);
03069 if(s->getWeight() != -1)
03070 {
03071 signaturesToCompare.push_back(iter->first);
03072 }
03073 }
03074 else
03075 {
03076
03077 signaturesToCompare.push_back(iter->first);
03078 }
03079 }
03080 cv::Mat prediction = _bayesFilter->generatePrediction(_memory, uListToVector(signaturesToCompare));
03081
03082 FILE* fout = 0;
03083 std::string fileName = this->getWorkingDir() + "/DumpPrediction.txt";
03084 #ifdef _MSC_VER
03085 fopen_s(&fout, fileName.c_str(), "w");
03086 #else
03087 fout = fopen(fileName.c_str(), "w");
03088 #endif
03089
03090 if(fout)
03091 {
03092 for(int i=0; i<prediction.rows; ++i)
03093 {
03094 for(int j=0; j<prediction.cols; ++j)
03095 {
03096 fprintf(fout, "%f ",((float*)prediction.data)[j + i*prediction.cols]);
03097 }
03098 fprintf(fout, "\n");
03099 }
03100 fclose(fout);
03101 }
03102 }
03103 else
03104 {
03105 UWARN("Memory and/or the Bayes filter are not created");
03106 }
03107 }
03108
03109 void Rtabmap::get3DMap(
03110 std::map<int, Signature> & signatures,
03111 std::map<int, Transform> & poses,
03112 std::multimap<int, Link> & constraints,
03113 bool optimized,
03114 bool global) const
03115 {
03116 UDEBUG("");
03117 if(_memory && _memory->getLastWorkingSignature())
03118 {
03119 if(_rgbdSlamMode)
03120 {
03121 if(optimized)
03122 {
03123 poses = _optimizedPoses;
03124 this->optimizeCurrentMap(_memory->getLastWorkingSignature()->id(), global, poses, &constraints);
03125 }
03126 else
03127 {
03128 std::map<int, int> ids = _memory->getNeighborsId(_memory->getLastWorkingSignature()->id(), 0, global?-1:0, true);
03129 _memory->getMetricConstraints(uKeysSet(ids), poses, constraints, global);
03130 }
03131 }
03132 else
03133 {
03134
03135 std::map<int, int> ids = _memory->getNeighborsId(_memory->getLastWorkingSignature()->id(), 0, global?-1:0, true);
03136 _memory->getMetricConstraints(uKeysSet(ids), poses, constraints, global);
03137 }
03138
03139
03140 std::set<int> ids = uKeysSet(_memory->getWorkingMem());
03141
03142
03143 ids.erase(Memory::kIdVirtual);
03144
03145 ids.insert(_memory->getStMem().begin(), _memory->getStMem().end());
03146 if(global)
03147 {
03148 ids = _memory->getAllSignatureIds();
03149 }
03150
03151 for(std::set<int>::iterator iter = ids.begin(); iter!=ids.end(); ++iter)
03152 {
03153 Transform odomPose;
03154 int weight = -1;
03155 int mapId = -1;
03156 std::string label;
03157 double stamp = 0;
03158 Transform groundTruth;
03159 _memory->getNodeInfo(*iter, odomPose, mapId, weight, label, stamp, groundTruth, true);
03160 SensorData data = _memory->getNodeData(*iter);
03161 data.setId(*iter);
03162 std::multimap<int, cv::KeyPoint> words;
03163 std::multimap<int, cv::Point3f> words3;
03164 std::multimap<int, cv::Mat> wordsDescriptors;
03165 _memory->getNodeWords(*iter, words, words3, wordsDescriptors);
03166 signatures.insert(std::make_pair(*iter,
03167 Signature(*iter,
03168 mapId,
03169 weight,
03170 stamp,
03171 label,
03172 odomPose,
03173 groundTruth,
03174 data)));
03175 signatures.at(*iter).setWords(words);
03176 signatures.at(*iter).setWords3(words3);
03177 signatures.at(*iter).setWordsDescriptors(wordsDescriptors);
03178 }
03179 }
03180 else if(_memory && (_memory->getStMem().size() || _memory->getWorkingMem().size() > 1))
03181 {
03182 UERROR("Last working signature is null!?");
03183 }
03184 else if(_memory == 0)
03185 {
03186 UWARN("Memory not initialized...");
03187 }
03188 }
03189
03190 void Rtabmap::getGraph(
03191 std::map<int, Transform> & poses,
03192 std::multimap<int, Link> & constraints,
03193 bool optimized,
03194 bool global,
03195 std::map<int, Signature> * signatures)
03196 {
03197 if(_memory && _memory->getLastWorkingSignature())
03198 {
03199 if(_rgbdSlamMode)
03200 {
03201 if(optimized)
03202 {
03203 poses = _optimizedPoses;
03204 this->optimizeCurrentMap(_memory->getLastWorkingSignature()->id(), global, poses, &constraints);
03205 }
03206 else
03207 {
03208 std::map<int, int> ids = _memory->getNeighborsId(_memory->getLastWorkingSignature()->id(), 0, global?-1:0, true);
03209 _memory->getMetricConstraints(uKeysSet(ids), poses, constraints, global);
03210 }
03211 }
03212 else
03213 {
03214
03215 std::map<int, int> ids = _memory->getNeighborsId(_memory->getLastWorkingSignature()->id(), 0, global?-1:0, true);
03216 _memory->getMetricConstraints(uKeysSet(ids), poses, constraints, global);
03217 }
03218
03219 if(signatures)
03220 {
03221 for(std::map<int, Transform>::iterator iter=poses.begin(); iter!=poses.end(); ++iter)
03222 {
03223 Transform odomPose;
03224 int weight = -1;
03225 int mapId = -1;
03226 std::string label;
03227 double stamp = 0;
03228 Transform groundTruth;
03229 _memory->getNodeInfo(iter->first, odomPose, mapId, weight, label, stamp, groundTruth, global);
03230 signatures->insert(std::make_pair(iter->first,
03231 Signature(iter->first,
03232 mapId,
03233 weight,
03234 stamp,
03235 label,
03236 odomPose,
03237 groundTruth)));
03238
03239 std::multimap<int, cv::KeyPoint> words;
03240 std::multimap<int, cv::Point3f> words3;
03241 std::multimap<int, cv::Mat> wordsDescriptors;
03242 _memory->getNodeWords(iter->first, words, words3, wordsDescriptors);
03243 signatures->at(iter->first).setWords(words);
03244 signatures->at(iter->first).setWords3(words3);
03245 signatures->at(iter->first).setWordsDescriptors(wordsDescriptors);
03246
03247 std::vector<CameraModel> models;
03248 StereoCameraModel stereoModel;
03249 _memory->getNodeCalibration(iter->first, models, stereoModel);
03250 signatures->at(iter->first).sensorData().setCameraModels(models);
03251 signatures->at(iter->first).sensorData().setStereoCameraModel(stereoModel);
03252 }
03253 }
03254 }
03255 else if(_memory && (_memory->getStMem().size() || _memory->getWorkingMem().size()))
03256 {
03257 UERROR("Last working signature is null!?");
03258 }
03259 else if(_memory == 0)
03260 {
03261 UWARN("Memory not initialized...");
03262 }
03263 }
03264
03265 int Rtabmap::detectMoreLoopClosures(float clusterRadius, float clusterAngle, int iterations)
03266 {
03267 UASSERT(iterations>0);
03268
03269 if(_graphOptimizer->iterations() <= 0)
03270 {
03271 UERROR("Cannot detect more loop closures if graph optimization iterations = 0");
03272 return -1;
03273 }
03274 if(!_rgbdSlamMode)
03275 {
03276 UERROR("Detecting more loop closures can be done only in RGBD-SLAM mode.");
03277 return -1;
03278 }
03279
03280 std::list<Link> loopClosuresAdded;
03281 std::multimap<int, int> checkedLoopClosures;
03282
03283 std::map<int, Transform> poses;
03284 std::multimap<int, Link> links;
03285 std::map<int, Signature> signatures;
03286 this->getGraph(poses, links, true, true, &signatures);
03287
03288 for(int n=0; n<iterations; ++n)
03289 {
03290 UINFO("Looking for more loop closures, clustering poses... (iteration=%d/%d, radius=%f m angle=%f rad)",
03291 n+1, iterations, clusterRadius, clusterAngle);
03292
03293 std::multimap<int, int> clusters = graph::radiusPosesClustering(
03294 poses,
03295 clusterRadius,
03296 clusterAngle);
03297
03298 UINFO("Looking for more loop closures, clustering poses... found %d clusters.", (int)clusters.size());
03299
03300 int i=0;
03301 std::set<int> addedLinks;
03302 for(std::multimap<int, int>::iterator iter=clusters.begin(); iter!= clusters.end(); ++iter, ++i)
03303 {
03304 int from = iter->first;
03305 int to = iter->second;
03306 if(iter->first < iter->second)
03307 {
03308 from = iter->second;
03309 to = iter->first;
03310 }
03311
03312 if(rtabmap::graph::findLink(checkedLoopClosures, from, to) == checkedLoopClosures.end())
03313 {
03314
03315 if(addedLinks.find(from) == addedLinks.end() &&
03316 addedLinks.find(to) == addedLinks.end() &&
03317 rtabmap::graph::findLink(links, from, to) == links.end())
03318 {
03319 checkedLoopClosures.insert(std::make_pair(from, to));
03320
03321 UASSERT(signatures.find(from) != signatures.end());
03322 UASSERT(signatures.find(to) != signatures.end());
03323
03324 RegistrationInfo info;
03325
03326 Transform t = _memory->computeTransform(signatures.at(from), signatures.at(to), Transform(), &info);
03327
03328 if(!t.isNull())
03329 {
03330 UINFO("Added new loop closure between %d and %d.", from, to);
03331 addedLinks.insert(from);
03332 addedLinks.insert(to);
03333 links.insert(std::make_pair(from, Link(from, to, Link::kUserClosure, t, info.variance, info.variance)));
03334 loopClosuresAdded.push_back(Link(from, to, Link::kUserClosure, t, info.variance, info.variance));
03335 UINFO("Detected loop closure %d->%d! (%d/%d)", from, to, i+1, (int)clusters.size());
03336 }
03337 }
03338 }
03339 }
03340 UINFO("Iteration %d/%d: Detected %d loop closures!", n+1, iterations, (int)addedLinks.size()/2);
03341 if(addedLinks.size() == 0)
03342 {
03343 break;
03344 }
03345
03346 if(n+1 < iterations)
03347 {
03348 UINFO("Optimizing graph with new links (%d nodes, %d constraints)...",
03349 (int)poses.size(), (int)links.size());
03350 int fromId = _optimizeFromGraphEnd?poses.rbegin()->first:poses.begin()->first;
03351 poses = _graphOptimizer->optimize(fromId, poses, links, 0);
03352 if(poses.size() == 0)
03353 {
03354 UERROR("Optimization failed! Rejecting all loop closures...");
03355 loopClosuresAdded.clear();
03356 return -1;
03357 }
03358 UINFO("Optimizing graph with new links... done!");
03359 }
03360 }
03361 UINFO("Total added %d loop closures.", (int)loopClosuresAdded.size());
03362
03363 if(loopClosuresAdded.size())
03364 {
03365 for(std::list<Link>::iterator iter=loopClosuresAdded.begin(); iter!=loopClosuresAdded.end(); ++iter)
03366 {
03367 _memory->addLink(*iter, true);
03368 }
03369 }
03370 return (int)loopClosuresAdded.size();
03371 }
03372
03373 void Rtabmap::clearPath(int status)
03374 {
03375 UINFO("status=%d", status);
03376 _pathStatus = status;
03377 _path.clear();
03378 _pathCurrentIndex=0;
03379 _pathGoalIndex = 0;
03380 _pathTransformToGoal.setIdentity();
03381 _pathUnreachableNodes.clear();
03382 _pathStuckCount = 0;
03383 _pathStuckDistance = 0.0f;
03384 if(_memory)
03385 {
03386 _memory->removeAllVirtualLinks();
03387 }
03388 }
03389
03390
03391 bool Rtabmap::computePath(int targetNode, bool global)
03392 {
03393 UINFO("Planning a path to node %d (global=%d)", targetNode, global?1:0);
03394 this->clearPath(0);
03395
03396 if(!_rgbdSlamMode)
03397 {
03398 UWARN("A path can only be computed in RGBD-SLAM mode");
03399 return false;
03400 }
03401
03402 UTimer totalTimer;
03403 UTimer timer;
03404
03405
03406 if(_memory)
03407 {
03408 int currentNode = 0;
03409 if(_memory->isIncremental())
03410 {
03411 if(!_memory->getLastWorkingSignature())
03412 {
03413 UWARN("Working memory is empty... cannot compute a path");
03414 return false;
03415 }
03416 currentNode = _memory->getLastWorkingSignature()->id();
03417 }
03418 else
03419 {
03420 if(_lastLocalizationPose.isNull() || _optimizedPoses.size() == 0)
03421 {
03422 UWARN("Last localization pose is null... cannot compute a path");
03423 return false;
03424 }
03425 currentNode = graph::findNearestNode(_optimizedPoses, _lastLocalizationPose);
03426 }
03427 if(currentNode && targetNode)
03428 {
03429 std::list<std::pair<int, Transform> > path = graph::computePath(
03430 currentNode,
03431 targetNode,
03432 _memory,
03433 global,
03434 false,
03435 _pathLinearVelocity,
03436 _pathAngularVelocity);
03437
03438
03439 Transform t = uValue(_optimizedPoses, currentNode, Transform::getIdentity());
03440 _path.resize(path.size());
03441 int oi = 0;
03442 for(std::list<std::pair<int, Transform> >::iterator iter=path.begin(); iter!=path.end();++iter)
03443 {
03444 _path[oi].first = iter->first;
03445 _path[oi++].second = t * iter->second;
03446 }
03447 }
03448 }
03449 UINFO("Total planning time = %fs (%d nodes, %f m long)", totalTimer.ticks(), (int)_path.size(), graph::computePathLength(_path));
03450
03451 if(_path.size() == 0)
03452 {
03453 _path.clear();
03454 UWARN("Cannot compute a path!");
03455 return false;
03456 }
03457 else
03458 {
03459 UINFO("Path generated! Size=%d", (int)_path.size());
03460 if(ULogger::level() == ULogger::kInfo)
03461 {
03462 std::stringstream stream;
03463 for(unsigned int i=0; i<_path.size(); ++i)
03464 {
03465 stream << _path[i].first;
03466 if(i+1 < _path.size())
03467 {
03468 stream << " ";
03469 }
03470 }
03471 UINFO("Path = [%s]", stream.str().c_str());
03472 }
03473 if(_goalsSavedInUserData)
03474 {
03475
03476 std::string goalStr = uFormat("GOAL:%d", targetNode);
03477
03478
03479 if(_memory->getSignature(targetNode))
03480 {
03481 if(!_memory->getSignature(targetNode)->getLabel().empty())
03482 {
03483 goalStr = std::string("GOAL:")+_memory->getSignature(targetNode)->getLabel();
03484 }
03485 }
03486 else if(global)
03487 {
03488 std::map<int, std::string> labels = _memory->getAllLabels();
03489 std::map<int, std::string>::iterator iter = labels.find(targetNode);
03490 if(iter != labels.end() && !iter->second.empty())
03491 {
03492 goalStr = std::string("GOAL:")+labels.at(targetNode);
03493 }
03494 }
03495 setUserData(0, cv::Mat(1, int(goalStr.size()+1), CV_8SC1, (void *)goalStr.c_str()).clone());
03496 }
03497 updateGoalIndex();
03498 return _path.size() || _pathStatus > 0;
03499 }
03500
03501 return false;
03502 }
03503
03504 bool Rtabmap::computePath(const Transform & targetPose)
03505 {
03506 UINFO("Planning a path to pose %s ", targetPose.prettyPrint().c_str());
03507
03508 this->clearPath(0);
03509 std::list<std::pair<int, Transform> > pathPoses;
03510
03511 if(!_rgbdSlamMode)
03512 {
03513 UWARN("This method can only be used in RGBD-SLAM mode");
03514 return false;
03515 }
03516
03517
03518 UTimer timer;
03519 std::map<int, Transform> nodes = _optimizedPoses;
03520 std::multimap<int, int> links;
03521 for(std::map<int, Transform>::iterator iter=nodes.begin(); iter!=nodes.end(); ++iter)
03522 {
03523 const Signature * s = _memory->getSignature(iter->first);
03524 UASSERT(s);
03525 for(std::map<int, Link>::const_iterator jter=s->getLinks().begin(); jter!=s->getLinks().end(); ++jter)
03526 {
03527
03528 if(uContains(nodes, jter->second.to()))
03529 {
03530 links.insert(std::make_pair(jter->second.from(), jter->second.to()));
03531
03532 }
03533 }
03534 }
03535 UINFO("Time getting links = %fs", timer.ticks());
03536
03537 int currentNode = 0;
03538 if(_memory->isIncremental())
03539 {
03540 if(!_memory->getLastWorkingSignature())
03541 {
03542 UWARN("Working memory is empty... cannot compute a path");
03543 return false;
03544 }
03545 currentNode = _memory->getLastWorkingSignature()->id();
03546 }
03547 else
03548 {
03549 if(_lastLocalizationPose.isNull() || _optimizedPoses.size() == 0)
03550 {
03551 UWARN("Last localization pose is null... cannot compute a path");
03552 return false;
03553 }
03554 currentNode = graph::findNearestNode(_optimizedPoses, _lastLocalizationPose);
03555 }
03556
03557 int nearestId;
03558 if(!_lastLocalizationPose.isNull() && _lastLocalizationPose.getDistance(targetPose) < _localRadius)
03559 {
03560
03561 nearestId = currentNode;
03562 }
03563 else
03564 {
03565 nearestId = rtabmap::graph::findNearestNode(nodes, targetPose);
03566 }
03567 UINFO("Nearest node found=%d ,%fs", nearestId, timer.ticks());
03568 if(nearestId > 0)
03569 {
03570 if(_localRadius != 0.0f && targetPose.getDistance(nodes.at(nearestId)) > _localRadius)
03571 {
03572 UWARN("Cannot plan farther than %f m from the graph! (distance=%f m from node %d)",
03573 _localRadius, targetPose.getDistance(nodes.at(nearestId)), nearestId);
03574 }
03575 else
03576 {
03577 UINFO("Computing path from location %d to %d", currentNode, nearestId);
03578 UTimer timer;
03579 _path = uListToVector(rtabmap::graph::computePath(nodes, links, currentNode, nearestId));
03580 UINFO("A* time = %fs", timer.ticks());
03581
03582 if(_path.size() == 0)
03583 {
03584 UWARN("Cannot compute a path!");
03585 }
03586 else
03587 {
03588 UINFO("Path generated! Size=%d", (int)_path.size());
03589 if(ULogger::level() == ULogger::kInfo)
03590 {
03591 std::stringstream stream;
03592 for(unsigned int i=0; i<_path.size(); ++i)
03593 {
03594 stream << _path[i].first;
03595 if(i+1 < _path.size())
03596 {
03597 stream << " ";
03598 }
03599 }
03600 UINFO("Path = [%s]", stream.str().c_str());
03601 }
03602
03603 UASSERT(uContains(nodes, _path.back().first));
03604 _pathTransformToGoal = nodes.at(_path.back().first).inverse() * targetPose;
03605
03606 updateGoalIndex();
03607
03608 return true;
03609 }
03610 }
03611 }
03612 else
03613 {
03614 UWARN("Nearest node not found in graph (size=%d) for pose %s", (int)nodes.size(), targetPose.prettyPrint().c_str());
03615 }
03616
03617 return false;
03618 }
03619
03620 std::vector<std::pair<int, Transform> > Rtabmap::getPathNextPoses() const
03621 {
03622 std::vector<std::pair<int, Transform> > poses;
03623 if(_path.size())
03624 {
03625 UASSERT(_pathCurrentIndex < _path.size() && _pathGoalIndex < _path.size());
03626 poses.resize(_pathGoalIndex-_pathCurrentIndex+1);
03627 int oi=0;
03628 for(unsigned int i=_pathCurrentIndex; i<=_pathGoalIndex; ++i)
03629 {
03630 std::map<int, Transform>::const_iterator iter = _optimizedPoses.find(_path[i].first);
03631 if(iter != _optimizedPoses.end())
03632 {
03633 poses[oi++] = *iter;
03634 }
03635 else
03636 {
03637 break;
03638 }
03639 }
03640 poses.resize(oi);
03641 }
03642 return poses;
03643 }
03644
03645 std::vector<int> Rtabmap::getPathNextNodes() const
03646 {
03647 std::vector<int> ids;
03648 if(_path.size())
03649 {
03650 UASSERT(_pathCurrentIndex < _path.size() && _pathGoalIndex < _path.size());
03651 ids.resize(_pathGoalIndex-_pathCurrentIndex+1);
03652 int oi = 0;
03653 for(unsigned int i=_pathCurrentIndex; i<=_pathGoalIndex; ++i)
03654 {
03655 std::map<int, Transform>::const_iterator iter = _optimizedPoses.find(_path[i].first);
03656 if(iter != _optimizedPoses.end())
03657 {
03658 ids[oi++] = iter->first;
03659 }
03660 else
03661 {
03662 break;
03663 }
03664 }
03665 ids.resize(oi);
03666 }
03667 return ids;
03668 }
03669
03670 int Rtabmap::getPathCurrentGoalId() const
03671 {
03672 if(_path.size())
03673 {
03674 UASSERT(_pathGoalIndex <= _path.size());
03675 return _path[_pathGoalIndex].first;
03676 }
03677 return 0;
03678 }
03679
03680 void Rtabmap::updateGoalIndex()
03681 {
03682 if(!_rgbdSlamMode)
03683 {
03684 UWARN("This method can on be used in RGBD-SLAM mode!");
03685 return;
03686 }
03687
03688 if( _memory && _path.size())
03689 {
03690
03691 for(unsigned int i=0; i<_pathCurrentIndex && i<_path.size(); ++i)
03692 {
03693 const Signature * s = _memory->getSignature(_path[i].first);
03694 if(s)
03695 {
03696 _memory->removeVirtualLinks(s->id());
03697 }
03698 }
03699
03700
03701
03702
03703 UASSERT(_pathCurrentIndex < _path.size());
03704 const Signature * currentIndexS = _memory->getSignature(_path[_pathCurrentIndex].first);
03705 UASSERT_MSG(currentIndexS != 0, uFormat("_path[%d].first=%d", _pathCurrentIndex, _path[_pathCurrentIndex].first).c_str());
03706 std::map<int, Link> links = currentIndexS->getLinks();
03707 bool latestVirtualLinkFound = false;
03708 for(std::map<int, Link>::reverse_iterator iter=links.rbegin(); iter!=links.rend(); ++iter)
03709 {
03710 if(iter->second.type() == Link::kVirtualClosure)
03711 {
03712 if(latestVirtualLinkFound)
03713 {
03714 _memory->removeLink(currentIndexS->id(), iter->first);
03715 }
03716 else
03717 {
03718 latestVirtualLinkFound = true;
03719 }
03720 }
03721 }
03722
03723
03724 float distanceSoFar = 0.0f;
03725 for(unsigned int i=_pathCurrentIndex+1;
03726 i<_path.size();
03727 ++i)
03728 {
03729 if(i>0)
03730 {
03731 if(_localRadius > 0.0f)
03732 {
03733 distanceSoFar += _path[i-1].second.getDistance(_path[i].second);
03734 }
03735 if(distanceSoFar <= _localRadius)
03736 {
03737 if(_path[i].first != _path[i-1].first)
03738 {
03739 const Signature * s = _memory->getSignature(_path[i].first);
03740 if(s)
03741 {
03742 if(!s->hasLink(_path[i-1].first) && _memory->getSignature(_path[i-1].first) != 0)
03743 {
03744 Transform virtualLoop = _path[i].second.inverse() * _path[i-1].second;
03745 _memory->addLink(Link(_path[i].first, _path[i-1].first, Link::kVirtualClosure, virtualLoop, 100, 100));
03746 UINFO("Added Virtual link between %d and %d", _path[i-1].first, _path[i].first);
03747 }
03748 }
03749 }
03750 }
03751 else
03752 {
03753 break;
03754 }
03755 }
03756 }
03757
03758 UDEBUG("current node = %d current goal = %d", _path[_pathCurrentIndex].first, _path[_pathGoalIndex].first);
03759 Transform currentPose;
03760 if(_memory->isIncremental())
03761 {
03762 if(_memory->getLastWorkingSignature() == 0 ||
03763 !uContains(_optimizedPoses, _memory->getLastWorkingSignature()->id()))
03764 {
03765 UERROR("Last node is null in memory or not in optimized poses. Aborting the plan...");
03766 this->clearPath(-1);
03767 return;
03768 }
03769 currentPose = _optimizedPoses.at(_memory->getLastWorkingSignature()->id());
03770 }
03771 else
03772 {
03773 if(_lastLocalizationPose.isNull())
03774 {
03775 UERROR("Last localization pose is null. Aborting the plan...");
03776 this->clearPath(-1);
03777 return;
03778 }
03779 currentPose = _lastLocalizationPose;
03780 }
03781
03782 int goalId = _path.back().first;
03783 if(uContains(_optimizedPoses, goalId))
03784 {
03785
03786 float d = currentPose.getDistance(_optimizedPoses.at(goalId)*_pathTransformToGoal);
03787 if(d < _goalReachedRadius)
03788 {
03789 UINFO("Goal %d reached!", goalId);
03790 this->clearPath(1);
03791 }
03792 }
03793
03794 if(_path.size())
03795 {
03796
03797 unsigned int goalIndex = _pathCurrentIndex;
03798 float distanceFromCurrentNode = 0.0f;
03799 bool sameGoalIndex = false;
03800 for(unsigned int i=_pathCurrentIndex+1; i<_path.size(); ++i)
03801 {
03802 if(uContains(_optimizedPoses, _path[i].first))
03803 {
03804 if(_localRadius > 0.0f)
03805 {
03806 distanceFromCurrentNode = _path[_pathCurrentIndex].second.getDistance(_path[i].second);
03807 }
03808
03809 if((goalIndex == _pathCurrentIndex && i == _path.size()-1) ||
03810 _pathUnreachableNodes.find(i) == _pathUnreachableNodes.end())
03811 {
03812 if(distanceFromCurrentNode <= _localRadius)
03813 {
03814 goalIndex = i;
03815 }
03816 else
03817 {
03818 break;
03819 }
03820 }
03821 }
03822 else
03823 {
03824 break;
03825 }
03826 }
03827 UASSERT(_pathGoalIndex < _path.size() && goalIndex < _path.size());
03828 if(_pathGoalIndex != goalIndex)
03829 {
03830 UINFO("Updated current goal from %d to %d (%d/%d)",
03831 (int)_path[_pathGoalIndex].first, _path[goalIndex].first, (int)goalIndex+1, (int)_path.size());
03832 _pathGoalIndex = goalIndex;
03833 }
03834 else
03835 {
03836 sameGoalIndex = true;
03837 }
03838
03839
03840 unsigned int nearestNodeIndex = 0;
03841 float distance = -1.0f;
03842 bool sameCurrentIndex = false;
03843 UASSERT(_pathGoalIndex < _path.size() && _pathGoalIndex >= 0);
03844 for(unsigned int i=_pathCurrentIndex; i<=_pathGoalIndex; ++i)
03845 {
03846 std::map<int, Transform>::iterator iter = _optimizedPoses.find(_path[i].first);
03847 if(iter != _optimizedPoses.end())
03848 {
03849 float d = currentPose.getDistanceSquared(iter->second);
03850 if(distance == -1.0f || distance > d)
03851 {
03852 distance = d;
03853 nearestNodeIndex = i;
03854 }
03855 }
03856 }
03857 if(distance < 0)
03858 {
03859 UERROR("The nearest pose on the path not found! Aborting the plan...");
03860 this->clearPath(-1);
03861 }
03862 else
03863 {
03864 UDEBUG("Nearest node = %d", _path[nearestNodeIndex].first);
03865 }
03866 if(distance >= 0 && nearestNodeIndex != _pathCurrentIndex)
03867 {
03868 _pathCurrentIndex = nearestNodeIndex;
03869 _pathUnreachableNodes.erase(nearestNodeIndex);
03870 }
03871 else
03872 {
03873 sameCurrentIndex = true;
03874 }
03875
03876 bool isStuck = false;
03877 if(sameGoalIndex && sameCurrentIndex && _pathStuckIterations>0)
03878 {
03879 float distanceToCurrentGoal = 0.0f;
03880 std::map<int, Transform>::iterator iter = _optimizedPoses.find(_path[_pathGoalIndex].first);
03881 if(iter != _optimizedPoses.end())
03882 {
03883 if(_pathGoalIndex == _pathCurrentIndex &&
03884 _pathGoalIndex == _path.size()-1)
03885 {
03886 distanceToCurrentGoal = currentPose.getDistanceSquared(iter->second*_pathTransformToGoal);
03887 }
03888 else
03889 {
03890 distanceToCurrentGoal = currentPose.getDistanceSquared(iter->second);
03891 }
03892 }
03893
03894 if(distanceToCurrentGoal > 0.0f)
03895 {
03896 if(distanceToCurrentGoal >= _pathStuckDistance)
03897 {
03898
03899 isStuck = true;
03900 if(_pathStuckDistance == 0.0f)
03901 {
03902 _pathStuckDistance = distanceToCurrentGoal;
03903 }
03904 }
03905 }
03906 else
03907 {
03908
03909 isStuck = true;
03910 }
03911 }
03912
03913 if(isStuck && ++_pathStuckCount > _pathStuckIterations)
03914 {
03915 UWARN("Current goal %d not reached since %d iterations (\"RGBD/PlanStuckIterations\"=%d), mark that node as unreachable.",
03916 _path[_pathGoalIndex].first,
03917 _pathStuckCount,
03918 _pathStuckIterations);
03919 _pathStuckCount = 0;
03920 _pathStuckDistance = 0.0;
03921 _pathUnreachableNodes.insert(_pathGoalIndex);
03922
03923 while(_pathUnreachableNodes.find(_pathGoalIndex) != _pathUnreachableNodes.end())
03924 {
03925 if(_pathGoalIndex == 0 || --_pathGoalIndex <= _pathCurrentIndex)
03926 {
03927
03928 UERROR("No upcoming nodes on the path are reachable! Aborting the plan...");
03929 this->clearPath(-1);
03930 return;
03931 }
03932 }
03933 }
03934 else if(!isStuck)
03935 {
03936 _pathStuckCount = 0;
03937 _pathStuckDistance = 0.0;
03938 }
03939 }
03940 }
03941 }
03942
03943 }