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/Graph.h"
00032 #include "rtabmap/core/Signature.h"
00033
00034 #include "rtabmap/core/EpipolarGeometry.h"
00035
00036 #include "rtabmap/core/Memory.h"
00037 #include "rtabmap/core/VWDictionary.h"
00038 #include "BayesFilter.h"
00039
00040 #include <rtabmap/utilite/ULogger.h>
00041 #include <rtabmap/utilite/UFile.h>
00042 #include <rtabmap/utilite/UTimer.h>
00043 #include <rtabmap/utilite/UConversion.h>
00044 #include <rtabmap/utilite/UMath.h>
00045
00046 #include "SimpleIni.h"
00047
00048 #include <pcl/search/kdtree.h>
00049 #include <pcl/filters/crop_box.h>
00050 #include <pcl/io/pcd_io.h>
00051
00052 #include "rtabmap/core/util3d.h"
00053
00054 #include <stdlib.h>
00055 #include <set>
00056
00057 #define LOG_F "LogF.txt"
00058 #define LOG_I "LogI.txt"
00059
00060 #define GRAPH_FILE_NAME "Graph.dot"
00061
00062
00063
00064
00065
00066
00067
00068
00069
00070
00071
00072
00073 namespace rtabmap
00074 {
00075
00076 Rtabmap::Rtabmap() :
00077 _publishStats(Parameters::defaultRtabmapPublishStats()),
00078 _publishLastSignature(Parameters::defaultRtabmapPublishLastSignature()),
00079 _publishPdf(Parameters::defaultRtabmapPublishPdf()),
00080 _publishLikelihood(Parameters::defaultRtabmapPublishLikelihood()),
00081 _maxTimeAllowed(Parameters::defaultRtabmapTimeThr()),
00082 _maxMemoryAllowed(Parameters::defaultRtabmapMemoryThr()),
00083 _loopThr(Parameters::defaultRtabmapLoopThr()),
00084 _loopRatio(Parameters::defaultRtabmapLoopRatio()),
00085 _maxRetrieved(Parameters::defaultRtabmapMaxRetrieved()),
00086 _maxLocalRetrieved(Parameters::defaultRGBDMaxLocalRetrieved()),
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 _globalLoopClosureIcpType(Parameters::defaultLccIcpType()),
00095 _poseScanMatching(Parameters::defaultRGBDPoseScanMatching()),
00096 _localLoopClosureDetectionTime(Parameters::defaultRGBDLocalLoopDetectionTime()),
00097 _localLoopClosureDetectionSpace(Parameters::defaultRGBDLocalLoopDetectionSpace()),
00098 _localRadius(Parameters::defaultRGBDLocalRadius()),
00099 _localImmunizationRatio(Parameters::defaultRGBDLocalImmunizationRatio()),
00100 _localDetectMaxGraphDepth(Parameters::defaultRGBDLocalLoopDetectionMaxGraphDepth()),
00101 _localPathFilteringRadius(Parameters::defaultRGBDLocalLoopDetectionPathFilteringRadius()),
00102 _localPathOdomPosesUsed(Parameters::defaultRGBDLocalLoopDetectionPathOdomPosesUsed()),
00103 _databasePath(""),
00104 _optimizeFromGraphEnd(Parameters::defaultRGBDOptimizeFromGraphEnd()),
00105 _reextractLoopClosureFeatures(Parameters::defaultLccReextractActivated()),
00106 _reextractNNType(Parameters::defaultLccReextractNNType()),
00107 _reextractNNDR(Parameters::defaultLccReextractNNDR()),
00108 _reextractFeatureType(Parameters::defaultLccReextractFeatureType()),
00109 _reextractMaxWords(Parameters::defaultLccReextractMaxWords()),
00110 _startNewMapOnLoopClosure(Parameters::defaultRtabmapStartNewMapOnLoopClosure()),
00111 _goalReachedRadius(Parameters::defaultRGBDGoalReachedRadius()),
00112 _planVirtualLinks(Parameters::defaultRGBDPlanVirtualLinks()),
00113 _goalsSavedInUserData(Parameters::defaultRGBDGoalsSavedInUserData()),
00114 _loopClosureHypothesis(0,0.0f),
00115 _highestHypothesis(0,0.0f),
00116 _lastProcessTime(0.0),
00117 _epipolarGeometry(0),
00118 _bayesFilter(0),
00119 _graphOptimizer(0),
00120 _memory(0),
00121 _foutFloat(0),
00122 _foutInt(0),
00123 _wDir("."),
00124 _mapCorrection(Transform::getIdentity()),
00125 _mapTransform(Transform::getIdentity()),
00126 _pathCurrentIndex(0),
00127 _pathGoalIndex(0),
00128 _pathTransformToGoal(Transform::getIdentity())
00129 {
00130 }
00131
00132 Rtabmap::~Rtabmap() {
00133 UDEBUG("");
00134 this->close();
00135 }
00136
00137 std::string Rtabmap::getVersion()
00138 {
00139 return RTABMAP_VERSION;
00140 return "";
00141 }
00142
00143 void Rtabmap::setupLogFiles(bool overwrite)
00144 {
00145 flushStatisticLogs();
00146
00147 if(_foutFloat)
00148 {
00149 fclose(_foutFloat);
00150 _foutFloat = 0;
00151 }
00152 if(_foutInt)
00153 {
00154 fclose(_foutInt);
00155 _foutInt = 0;
00156 }
00157
00158 if(_statisticLogged)
00159 {
00160 std::string attributes = "a+";
00161 if(overwrite)
00162 {
00163
00164
00165
00166 attributes = "w";
00167 }
00168
00169 bool addLogFHeader = overwrite || !UFile::exists(_wDir+"/"+LOG_F);
00170 bool addLogIHeader = overwrite || !UFile::exists(_wDir+"/"+LOG_I);
00171
00172 #ifdef _MSC_VER
00173 fopen_s(&_foutFloat, (_wDir+"/"+LOG_F).c_str(), attributes.c_str());
00174 fopen_s(&_foutInt, (_wDir+"/"+LOG_I).c_str(), attributes.c_str());
00175 #else
00176 _foutFloat = fopen((_wDir+"/"+LOG_F).c_str(), attributes.c_str());
00177 _foutInt = fopen((_wDir+"/"+LOG_I).c_str(), attributes.c_str());
00178 #endif
00179
00180 if(_statisticLoggedHeaders && addLogFHeader && _foutFloat)
00181 {
00182 fprintf(_foutFloat, "Column headers:\n");
00183 fprintf(_foutFloat, " 1-Total iteration time (s)\n");
00184 fprintf(_foutFloat, " 2-Memory update time (s)\n");
00185 fprintf(_foutFloat, " 3-Retrieval time (s)\n");
00186 fprintf(_foutFloat, " 4-Likelihood time (s)\n");
00187 fprintf(_foutFloat, " 5-Posterior time (s)\n");
00188 fprintf(_foutFloat, " 6-Hypothesis selection time (s)\n");
00189 fprintf(_foutFloat, " 7-Transfer time (s)\n");
00190 fprintf(_foutFloat, " 8-Statistics creation time (s)\n");
00191 fprintf(_foutFloat, " 9-Loop closure hypothesis value\n");
00192 fprintf(_foutFloat, " 10-NAN\n");
00193 fprintf(_foutFloat, " 11-Maximum likelihood\n");
00194 fprintf(_foutFloat, " 12-Sum likelihood\n");
00195 fprintf(_foutFloat, " 13-Mean likelihood\n");
00196 fprintf(_foutFloat, " 14-Std dev likelihood\n");
00197 fprintf(_foutFloat, " 15-Virtual place hypothesis\n");
00198 fprintf(_foutFloat, " 16-Join trash time (s)\n");
00199 fprintf(_foutFloat, " 17-Weight Update (rehearsal) similarity\n");
00200 fprintf(_foutFloat, " 18-Empty trash time (s)\n");
00201 fprintf(_foutFloat, " 19-Retrieval database access time (s)\n");
00202 fprintf(_foutFloat, " 20-Add loop closure link time (s)\n");
00203 }
00204 if(_statisticLoggedHeaders && addLogIHeader && _foutInt)
00205 {
00206 fprintf(_foutInt, "Column headers:\n");
00207 fprintf(_foutInt, " 1-Loop closure ID\n");
00208 fprintf(_foutInt, " 2-Highest loop closure hypothesis\n");
00209 fprintf(_foutInt, " 3-Locations transferred\n");
00210 fprintf(_foutInt, " 4-NAN\n");
00211 fprintf(_foutInt, " 5-Words extracted from the last image\n");
00212 fprintf(_foutInt, " 6-Vocabulary size\n");
00213 fprintf(_foutInt, " 7-Working memory size\n");
00214 fprintf(_foutInt, " 8-Is loop closure hypothesis rejected?\n");
00215 fprintf(_foutInt, " 9-NAN\n");
00216 fprintf(_foutInt, " 10-NAN\n");
00217 fprintf(_foutInt, " 11-Locations retrieved\n");
00218 fprintf(_foutInt, " 12-Retrieval location ID\n");
00219 fprintf(_foutInt, " 13-Unique words extraced from last image\n");
00220 fprintf(_foutInt, " 14-Retrieval ID\n");
00221 fprintf(_foutInt, " 15-Non-null likelihood values\n");
00222 fprintf(_foutInt, " 16-Weight Update ID\n");
00223 fprintf(_foutInt, " 17-Is last location merged through Weight Update?\n");
00224 }
00225
00226 ULOGGER_DEBUG("Log file (int)=%s", (_wDir+"/"+LOG_I).c_str());
00227 ULOGGER_DEBUG("Log file (float)=%s", (_wDir+"/"+LOG_F).c_str());
00228 }
00229 else
00230 {
00231 UDEBUG("Log disabled!");
00232 }
00233 }
00234
00235 void Rtabmap::flushStatisticLogs()
00236 {
00237 if(_foutFloat && _bufferedLogsF.size())
00238 {
00239 UDEBUG("_bufferedLogsF.size=%d", _bufferedLogsF.size());
00240 for(std::list<std::string>::iterator iter = _bufferedLogsF.begin(); iter!=_bufferedLogsF.end(); ++iter)
00241 {
00242 fprintf(_foutFloat, "%s", iter->c_str());
00243 }
00244 _bufferedLogsF.clear();
00245 }
00246 if(_foutInt && _bufferedLogsI.size())
00247 {
00248 UDEBUG("_bufferedLogsI.size=%d", _bufferedLogsI.size());
00249 for(std::list<std::string>::iterator iter = _bufferedLogsI.begin(); iter!=_bufferedLogsI.end(); ++iter)
00250 {
00251 fprintf(_foutInt, "%s", iter->c_str());
00252 }
00253 _bufferedLogsI.clear();
00254 }
00255 }
00256
00257 void Rtabmap::init(const ParametersMap & parameters, const std::string & databasePath)
00258 {
00259 ParametersMap::const_iterator iter;
00260 if((iter=parameters.find(Parameters::kRtabmapWorkingDirectory())) != parameters.end())
00261 {
00262 this->setWorkingDirectory(iter->second.c_str());
00263 }
00264
00265 _databasePath = databasePath;
00266 if(!_databasePath.empty())
00267 {
00268 UASSERT(UFile::getExtension(_databasePath).compare("db") == 0);
00269 UINFO("Using database \"%s\".", _databasePath.c_str());
00270 }
00271 else
00272 {
00273 UWARN("Using empty database. Mapping session will not be saved.");
00274 }
00275
00276 bool newDatabase = _databasePath.empty() || !UFile::exists(_databasePath);
00277
00278
00279 if(!_memory)
00280 {
00281 _memory = new Memory(parameters);
00282 _memory->init(_databasePath, false, parameters, true);
00283 }
00284
00285
00286 this->parseParameters(parameters);
00287
00288 if(_databasePath.empty())
00289 {
00290 _statisticLogged = false;
00291 }
00292 setupLogFiles(newDatabase);
00293 }
00294
00295 void Rtabmap::init(const std::string & configFile, const std::string & databasePath)
00296 {
00297
00298 ParametersMap param;
00299
00300 if(!configFile.empty())
00301 {
00302 ULOGGER_DEBUG("Read parameters from = %s", configFile.c_str());
00303 this->readParameters(configFile, param);
00304 }
00305
00306 this->init(param, databasePath);
00307 }
00308
00309 void Rtabmap::close()
00310 {
00311 UINFO("");
00312 _highestHypothesis = std::make_pair(0,0.0f);
00313 _loopClosureHypothesis = std::make_pair(0,0.0f);
00314 _lastProcessTime = 0.0;
00315 _optimizedPoses.clear();
00316 _constraints.clear();
00317 _mapCorrection.setIdentity();
00318 _mapTransform.setIdentity();
00319 _lastLocalizationPose.setNull();
00320 this->clearPath();
00321
00322 flushStatisticLogs();
00323 if(_foutFloat)
00324 {
00325 fclose(_foutFloat);
00326 _foutFloat = 0;
00327 }
00328 if(_foutInt)
00329 {
00330 fclose(_foutInt);
00331 _foutInt = 0;
00332 }
00333
00334 if(_epipolarGeometry)
00335 {
00336 delete _epipolarGeometry;
00337 _epipolarGeometry = 0;
00338 }
00339 if(_memory)
00340 {
00341 delete _memory;
00342 _memory = 0;
00343 }
00344 if(_bayesFilter)
00345 {
00346 delete _bayesFilter;
00347 _bayesFilter = 0;
00348 }
00349 if(_graphOptimizer)
00350 {
00351 delete _graphOptimizer;
00352 _graphOptimizer = 0;
00353 }
00354 _databasePath.clear();
00355 parseParameters(Parameters::getDefaultParameters());
00356 _modifiedParameters.clear();
00357 }
00358
00359 void Rtabmap::parseParameters(const ParametersMap & parameters)
00360 {
00361 ULOGGER_DEBUG("");
00362 ParametersMap::const_iterator iter;
00363 if((iter=parameters.find(Parameters::kRtabmapWorkingDirectory())) != parameters.end())
00364 {
00365 this->setWorkingDirectory(iter->second.c_str());
00366 }
00367
00368 Parameters::parse(parameters, Parameters::kRtabmapPublishStats(), _publishStats);
00369 Parameters::parse(parameters, Parameters::kRtabmapPublishLastSignature(), _publishLastSignature);
00370 Parameters::parse(parameters, Parameters::kRtabmapPublishPdf(), _publishPdf);
00371 Parameters::parse(parameters, Parameters::kRtabmapPublishLikelihood(), _publishLikelihood);
00372 Parameters::parse(parameters, Parameters::kRtabmapTimeThr(), _maxTimeAllowed);
00373 Parameters::parse(parameters, Parameters::kRtabmapMemoryThr(), _maxMemoryAllowed);
00374 Parameters::parse(parameters, Parameters::kRtabmapLoopThr(), _loopThr);
00375 Parameters::parse(parameters, Parameters::kRtabmapLoopRatio(), _loopRatio);
00376 Parameters::parse(parameters, Parameters::kRtabmapMaxRetrieved(), _maxRetrieved);
00377 Parameters::parse(parameters, Parameters::kRGBDMaxLocalRetrieved(), _maxLocalRetrieved);
00378 Parameters::parse(parameters, Parameters::kRtabmapStatisticLogsBufferedInRAM(), _statisticLogsBufferedInRAM);
00379 Parameters::parse(parameters, Parameters::kRtabmapStatisticLogged(), _statisticLogged);
00380 Parameters::parse(parameters, Parameters::kRtabmapStatisticLoggedHeaders(), _statisticLoggedHeaders);
00381 Parameters::parse(parameters, Parameters::kRGBDEnabled(), _rgbdSlamMode);
00382 Parameters::parse(parameters, Parameters::kRGBDLinearUpdate(), _rgbdLinearUpdate);
00383 Parameters::parse(parameters, Parameters::kRGBDAngularUpdate(), _rgbdAngularUpdate);
00384 Parameters::parse(parameters, Parameters::kRGBDNewMapOdomChangeDistance(), _newMapOdomChangeDistance);
00385 Parameters::parse(parameters, Parameters::kRGBDPoseScanMatching(), _poseScanMatching);
00386 Parameters::parse(parameters, Parameters::kRGBDLocalLoopDetectionTime(), _localLoopClosureDetectionTime);
00387 Parameters::parse(parameters, Parameters::kRGBDLocalLoopDetectionSpace(), _localLoopClosureDetectionSpace);
00388 Parameters::parse(parameters, Parameters::kRGBDLocalRadius(), _localRadius);
00389 Parameters::parse(parameters, Parameters::kRGBDLocalImmunizationRatio(), _localImmunizationRatio);
00390 Parameters::parse(parameters, Parameters::kRGBDLocalLoopDetectionMaxGraphDepth(), _localDetectMaxGraphDepth);
00391 Parameters::parse(parameters, Parameters::kRGBDLocalLoopDetectionPathFilteringRadius(), _localPathFilteringRadius);
00392 Parameters::parse(parameters, Parameters::kRGBDLocalLoopDetectionPathOdomPosesUsed(), _localPathOdomPosesUsed);
00393 Parameters::parse(parameters, Parameters::kRGBDOptimizeFromGraphEnd(), _optimizeFromGraphEnd);
00394 Parameters::parse(parameters, Parameters::kLccReextractActivated(), _reextractLoopClosureFeatures);
00395 Parameters::parse(parameters, Parameters::kLccReextractNNType(), _reextractNNType);
00396 Parameters::parse(parameters, Parameters::kLccReextractNNDR(), _reextractNNDR);
00397 Parameters::parse(parameters, Parameters::kLccReextractFeatureType(), _reextractFeatureType);
00398 Parameters::parse(parameters, Parameters::kLccReextractMaxWords(), _reextractMaxWords);
00399 Parameters::parse(parameters, Parameters::kRtabmapStartNewMapOnLoopClosure(), _startNewMapOnLoopClosure);
00400 Parameters::parse(parameters, Parameters::kRGBDGoalReachedRadius(), _goalReachedRadius);
00401 Parameters::parse(parameters, Parameters::kRGBDPlanVirtualLinks(), _planVirtualLinks);
00402 Parameters::parse(parameters, Parameters::kRGBDGoalsSavedInUserData(), _goalsSavedInUserData);
00403
00404
00405 if((iter=parameters.find(Parameters::kLccIcpType())) != parameters.end())
00406 {
00407 int icpType = std::atoi((*iter).second.c_str());
00408 if(icpType >= 0 && icpType <= 2)
00409 {
00410 _globalLoopClosureIcpType = icpType;
00411 }
00412 else
00413 {
00414 UERROR("Icp type must be 0, 1 or 2 (value=%d)", icpType);
00415 }
00416 }
00417
00418
00419
00420
00421
00422 graph::Optimizer::Type optimizerType = graph::Optimizer::kTypeUndef;
00423 if((iter=parameters.find(Parameters::kRGBDOptimizeStrategy())) != parameters.end())
00424 {
00425 optimizerType = (graph::Optimizer::Type)std::atoi((*iter).second.c_str());
00426 }
00427 if(optimizerType!=graph::Optimizer::kTypeUndef)
00428 {
00429 UDEBUG("new detector strategy %d", int(optimizerType));
00430 if(_graphOptimizer)
00431 {
00432 delete _graphOptimizer;
00433 _graphOptimizer = 0;
00434 }
00435
00436 _graphOptimizer = graph::Optimizer::create(optimizerType, parameters);
00437 }
00438 else if(_graphOptimizer)
00439 {
00440 _graphOptimizer->parseParameters(parameters);
00441 }
00442 else
00443 {
00444 optimizerType = (graph::Optimizer::Type)Parameters::defaultRGBDOptimizeStrategy();
00445 _graphOptimizer = graph::Optimizer::create(optimizerType, parameters);
00446 }
00447
00448 if(_memory)
00449 {
00450 _memory->parseParameters(parameters);
00451 }
00452
00453 VhStrategy vhStrategy = kVhUndef;
00454
00455 if((iter=parameters.find(Parameters::kRtabmapVhStrategy())) != parameters.end())
00456 {
00457 vhStrategy = (VhStrategy)std::atoi((*iter).second.c_str());
00458 }
00459 if(!_epipolarGeometry && vhStrategy == kVhEpipolar)
00460 {
00461 _epipolarGeometry = new EpipolarGeometry(parameters);
00462 }
00463 else if(_epipolarGeometry && vhStrategy == kVhNone)
00464 {
00465 delete _epipolarGeometry;
00466 _epipolarGeometry = 0;
00467 }
00468 else if(_epipolarGeometry)
00469 {
00470 _epipolarGeometry->parseParameters(parameters);
00471 }
00472
00473
00474 if(!_bayesFilter)
00475 {
00476 _bayesFilter = new BayesFilter(parameters);
00477 }
00478 else
00479 {
00480 _bayesFilter->parseParameters(parameters);
00481 }
00482
00483 for(ParametersMap::const_iterator iter = parameters.begin(); iter!=parameters.end(); ++iter)
00484 {
00485 uInsert(_modifiedParameters, ParametersPair(iter->first, iter->second));
00486 }
00487 }
00488
00489 int Rtabmap::getLastLocationId() const
00490 {
00491 int id = 0;
00492 if(_memory)
00493 {
00494 id = _memory->getLastSignatureId();
00495 }
00496 return id;
00497 }
00498
00499 std::list<int> Rtabmap::getWM() const
00500 {
00501 std::list<int> mem;
00502 if(_memory)
00503 {
00504 mem = uKeysList(_memory->getWorkingMem());
00505 mem.remove(-1);
00506 }
00507 return mem;
00508 }
00509
00510 int Rtabmap::getWMSize() const
00511 {
00512 if(_memory)
00513 {
00514 return (int)_memory->getWorkingMem().size()-1;
00515 }
00516 return 0;
00517 }
00518
00519 std::map<int, int> Rtabmap::getWeights() const
00520 {
00521 std::map<int, int> weights;
00522 if(_memory)
00523 {
00524 weights = _memory->getWeights();
00525 weights.erase(-1);
00526 }
00527 return weights;
00528 }
00529
00530 std::set<int> Rtabmap::getSTM() const
00531 {
00532 if(_memory)
00533 {
00534 return _memory->getStMem();
00535 }
00536 return std::set<int>();
00537 }
00538
00539 int Rtabmap::getSTMSize() const
00540 {
00541 if(_memory)
00542 {
00543 return (int)_memory->getStMem().size();
00544 }
00545 return 0;
00546 }
00547
00548 int Rtabmap::getTotalMemSize() const
00549 {
00550 if(_memory)
00551 {
00552 const Signature * s =_memory->getLastWorkingSignature();
00553 if(s)
00554 {
00555 return s->id();
00556 }
00557 }
00558 return 0;
00559 }
00560
00561 std::multimap<int, cv::KeyPoint> Rtabmap::getWords(int locationId) const
00562 {
00563 if(_memory)
00564 {
00565 const Signature * s = _memory->getSignature(locationId);
00566 if(s)
00567 {
00568 return s->getWords();
00569 }
00570 }
00571 return std::multimap<int, cv::KeyPoint>();
00572 }
00573
00574 bool Rtabmap::isInSTM(int locationId) const
00575 {
00576 if(_memory)
00577 {
00578 return _memory->isInSTM(locationId);
00579 }
00580 return false;
00581 }
00582
00583 bool Rtabmap::isIDsGenerated() const
00584 {
00585 if(_memory)
00586 {
00587 return _memory->isIDsGenerated();
00588 }
00589 return Parameters::defaultMemGenerateIds();
00590 }
00591
00592 const Statistics & Rtabmap::getStatistics() const
00593 {
00594 return statistics_;
00595 }
00596
00597
00598
00599
00600
00601
00602
00603
00604
00605
00606
00607
00608
00609
00610
00611
00612
00613
00614
00615 Transform Rtabmap::getPose(int locationId) const
00616 {
00617 if(_memory)
00618 {
00619 const Signature * s = _memory->getSignature(locationId);
00620 if(s && _optimizedPoses.find(s->id()) != _optimizedPoses.end())
00621 {
00622 return _optimizedPoses.at(s->id());
00623 }
00624 }
00625 return Transform();
00626 }
00627
00628 int Rtabmap::triggerNewMap()
00629 {
00630 int mapId = -1;
00631 if(_memory)
00632 {
00633 mapId = _memory->incrementMapId();
00634 UINFO("New map triggered, new map = %d", mapId);
00635 _optimizedPoses.clear();
00636 _constraints.clear();
00637 }
00638 return mapId;
00639 }
00640
00641 bool Rtabmap::labelLocation(int id, const std::string & label)
00642 {
00643 if(_memory)
00644 {
00645 if(id > 0)
00646 {
00647 return _memory->labelSignature(id, label);
00648 }
00649 else if(_memory->getLastWorkingSignature())
00650 {
00651 return _memory->labelSignature(_memory->getLastWorkingSignature()->id(), label);
00652 }
00653 else
00654 {
00655 UERROR("Last signature is null! Cannot set label \"%s\"", label.c_str());
00656 }
00657 }
00658 return false;
00659 }
00660
00661 bool Rtabmap::setUserData(int id, const std::vector<unsigned char> & data)
00662 {
00663 if(_memory)
00664 {
00665 if(id > 0)
00666 {
00667 return _memory->setUserData(id, data);
00668 }
00669 else if(_memory->getLastWorkingSignature())
00670 {
00671 return _memory->setUserData(_memory->getLastWorkingSignature()->id(), data);
00672 }
00673 else
00674 {
00675 UERROR("Last signature is null! Cannot set user data!");
00676 }
00677 }
00678 return false;
00679 }
00680
00681 void Rtabmap::generateDOTGraph(const std::string & path, int id, int margin)
00682 {
00683 if(_memory)
00684 {
00685 _memory->joinTrashThread();
00686
00687 if(id > 0)
00688 {
00689 std::map<int, int> ids = _memory->getNeighborsId(id, margin, -1, false);
00690
00691 if(ids.size() > 0)
00692 {
00693 ids.insert(std::pair<int,int>(id, 0));
00694 std::set<int> idsSet;
00695 for(std::map<int, int>::iterator iter = ids.begin(); iter!=ids.end(); ++iter)
00696 {
00697 idsSet.insert(idsSet.end(), iter->first);
00698 }
00699 _memory->generateGraph(path, idsSet);
00700 }
00701 else
00702 {
00703 UERROR("No neighbors found for signature %d.", id);
00704 }
00705 }
00706 else
00707 {
00708 _memory->generateGraph(path);
00709 }
00710 }
00711 }
00712
00713 void Rtabmap::generateTOROGraph(const std::string & path, bool optimized, bool global)
00714 {
00715 if(_memory && _memory->getLastWorkingSignature())
00716 {
00717 std::map<int, Transform> poses;
00718 std::multimap<int, Link> constraints;
00719
00720 if(optimized)
00721 {
00722 this->optimizeCurrentMap(_memory->getLastWorkingSignature()->id(), global, poses, &constraints);
00723 }
00724 else
00725 {
00726 std::map<int, int> ids = _memory->getNeighborsId(_memory->getLastWorkingSignature()->id(), 0, global?-1:0, true);
00727 _memory->getMetricConstraints(uKeysSet(ids), poses, constraints, global);
00728 }
00729
00730 graph::TOROOptimizer::saveGraph(path, poses, constraints);
00731 }
00732 }
00733
00734 void Rtabmap::resetMemory()
00735 {
00736 _highestHypothesis = std::make_pair(0,0.0f);
00737 _loopClosureHypothesis = std::make_pair(0,0.0f);
00738 _lastProcessTime = 0.0;
00739 _optimizedPoses.clear();
00740 _constraints.clear();
00741 _mapCorrection.setIdentity();
00742 _mapTransform.setIdentity();
00743 _lastLocalizationPose.setNull();
00744 this->clearPath();
00745
00746 if(_memory)
00747 {
00748 _memory->init(_databasePath, true, _modifiedParameters, true);
00749 if(_memory->getLastWorkingSignature())
00750 {
00751 optimizeCurrentMap(_memory->getLastWorkingSignature()->id(), false, _optimizedPoses, &_constraints);
00752 }
00753 if(_bayesFilter)
00754 {
00755 _bayesFilter->reset();
00756 }
00757 }
00758 else
00759 {
00760 UERROR("RTAB-Map is not initialized. No memory to reset...");
00761 }
00762 this->setupLogFiles(true);
00763 }
00764
00765
00766
00767
00768 bool Rtabmap::process(const SensorData & data)
00769 {
00770 UDEBUG("");
00771
00772
00773
00774
00775 UTimer timer;
00776 UTimer timerTotal;
00777 double timeMemoryUpdate = 0;
00778 double timeScanMatching = 0;
00779 double timeLocalTimeDetection = 0;
00780 double timeLocalSpaceDetection = 0;
00781 double timeCleaningNeighbors = 0;
00782 double timeReactivations = 0;
00783 double timeAddLoopClosureLink = 0;
00784 double timeMapOptimization = 0;
00785 double timeRetrievalDbAccess = 0;
00786 double timeLikelihoodCalculation = 0;
00787 double timePosteriorCalculation = 0;
00788 double timeHypothesesCreation = 0;
00789 double timeHypothesesValidation = 0;
00790 double timeRealTimeLimitReachedProcess = 0;
00791 double timeMemoryCleanup = 0;
00792 double timeEmptyingTrash = 0;
00793 double timeJoiningTrash = 0;
00794 double timeStatsCreation = 0;
00795
00796 float hypothesisRatio = 0.0f;
00797 bool rejectedHypothesis = false;
00798
00799 std::map<int, float> rawLikelihood;
00800 std::map<int, float> adjustedLikelihood;
00801 std::map<int, float> likelihood;
00802 std::map<int, int> weights;
00803 std::map<int, float> posterior;
00804 std::list<std::pair<int, float> > reactivateHypotheses;
00805
00806 std::map<int, int> childCount;
00807 std::set<int> signaturesRetrieved;
00808 int localLoopClosuresInTimeFound = 0;
00809 bool scanMatchingSuccess = false;
00810
00811 const Signature * signature = 0;
00812 const Signature * sLoop = 0;
00813
00814 _loopClosureHypothesis = std::make_pair(0,0.0f);
00815 std::pair<int, float> lastHighestHypothesis = _highestHypothesis;
00816 _highestHypothesis = std::make_pair(0,0.0f);
00817
00818 std::set<int> immunizedLocations;
00819
00820 statistics_ = Statistics();
00821
00822
00823
00824
00825 ULOGGER_INFO("getting data...");
00826 if(!data.isValid())
00827 {
00828 ULOGGER_INFO("image is not valid...");
00829 return false;
00830 }
00831
00832 timer.start();
00833 timerTotal.start();
00834
00835 UASSERT_MSG(_memory, "RTAB-Map is not initialized!");
00836 UASSERT_MSG(_bayesFilter, "RTAB-Map is not initialized!");
00837 UASSERT_MSG(_graphOptimizer, "RTAB-Map is not initialized!");
00838
00839
00840
00841
00842 if(_rgbdSlamMode)
00843 {
00844 if(data.pose().isNull())
00845 {
00846 UERROR("RGB-D SLAM mode is enabled and no odometry is provided. "
00847 "Image %d is ignored!", data.id());
00848 return false;
00849 }
00850 else
00851 {
00852
00853 if(_memory->getLastWorkingSignature())
00854 {
00855 const Transform & lastPose = _memory->getLastWorkingSignature()->getPose();
00856 Transform lastPoseToNewPose = lastPose.inverse() * data.pose();
00857 float x,y,z, roll,pitch,yaw;
00858 lastPoseToNewPose.getTranslationAndEulerAngles(x,y,z, roll,pitch,yaw);
00859 if(_newMapOdomChangeDistance > 0.0 && (x*x + y*y + z*z) > _newMapOdomChangeDistance*_newMapOdomChangeDistance)
00860 {
00861 int mapId = triggerNewMap();
00862 UWARN("Odometry is reset (large odometry change detected > %f). A new map (%d) is created! Last pose = %s, new pose = %s",
00863 _newMapOdomChangeDistance,
00864 mapId,
00865 lastPose.prettyPrint().c_str(),
00866 data.pose().prettyPrint().c_str());
00867 }
00868 }
00869 }
00870 }
00871
00872
00873
00874
00875 ULOGGER_INFO("Updating memory...");
00876 if(_rgbdSlamMode)
00877 {
00878 if(!_memory->update(data, &statistics_))
00879 {
00880 return false;
00881 }
00882 }
00883 else
00884 {
00885 SensorData dataWithoutOdom = data;
00886 dataWithoutOdom.setPose(Transform(), 1, 1);
00887 if(!_memory->update(dataWithoutOdom, &statistics_))
00888 {
00889 return false;
00890 }
00891 }
00892
00893
00894 signature = _memory->getLastWorkingSignature();
00895 if(!signature)
00896 {
00897 UFATAL("Not supposed to be here...last signature is null?!?");
00898 }
00899 ULOGGER_INFO("Processing signature %d", signature->id());
00900 timeMemoryUpdate = timer.ticks();
00901 ULOGGER_INFO("timeMemoryUpdate=%fs", timeMemoryUpdate);
00902
00903
00904
00905
00906 bool smallDisplacement = false;
00907 if(_rgbdSlamMode)
00908 {
00909
00910 int rehearsedId = (int)uValue(statistics_.data(), Statistics::kMemoryRehearsal_merged(), 0.0f);
00911 if(rehearsedId > 0)
00912 {
00913 _optimizedPoses.erase(rehearsedId);
00914 }
00915 else if(_rgbdLinearUpdate > 0.0f && _rgbdAngularUpdate > 0.0f)
00916 {
00917
00918
00919
00920 const std::map<int, Link> & links = signature->getLinks();
00921 if(links.size() == 1)
00922 {
00923 float x,y,z, roll,pitch,yaw;
00924 links.begin()->second.transform().getTranslationAndEulerAngles(x,y,z, roll,pitch,yaw);
00925 if((_rgbdLinearUpdate==0.0f || (
00926 fabs(x) < _rgbdLinearUpdate &&
00927 fabs(y) < _rgbdLinearUpdate &&
00928 fabs(z) < _rgbdLinearUpdate)) &&
00929 (_rgbdAngularUpdate==0.0f || (
00930 fabs(roll) < _rgbdAngularUpdate &&
00931 fabs(pitch) < _rgbdAngularUpdate &&
00932 fabs(yaw) < _rgbdAngularUpdate)))
00933 {
00934
00935
00936 smallDisplacement = true;
00937 }
00938 }
00939 }
00940
00941 Transform newPose = _mapCorrection * signature->getPose();
00942 _optimizedPoses.insert(std::make_pair(signature->id(), newPose));
00943 _lastLocalizationPose = newPose;
00944
00945
00946
00947
00948 if(_poseScanMatching &&
00949 signature->getLinks().size() == 1 &&
00950 !signature->getLaserScanCompressed().empty() &&
00951 rehearsedId == 0)
00952 {
00953 UINFO("Odometry correction by scan matching");
00954 int oldId = signature->getLinks().begin()->first;
00955 const Signature * oldS = _memory->getSignature(oldId);
00956 UASSERT(oldS != 0);
00957 std::string rejectedMsg;
00958 Transform guess = signature->getLinks().begin()->second.transform();
00959 double variance = 1.0;
00960 int inliers = 0;
00961 float inliersRatio = 0;
00962 Transform t = _memory->computeIcpTransform(oldId, signature->id(), guess, false, &rejectedMsg, &inliers, &variance, &inliersRatio);
00963 if(!t.isNull())
00964 {
00965 scanMatchingSuccess = true;
00966 UINFO("Scan matching: update neighbor link (%d->%d) from %s to %s",
00967 signature->id(),
00968 oldId,
00969 signature->getLinks().at(oldId).transform().prettyPrint().c_str(),
00970 t.prettyPrint().c_str());
00971 _memory->updateLink(signature->id(), oldId, t, variance, variance);
00972 }
00973 else
00974 {
00975 UINFO("Scan matching rejected: %s", rejectedMsg.c_str());
00976 }
00977 statistics_.addStatistic(Statistics::kOdomCorrectionAccepted(), scanMatchingSuccess?1.0f:0);
00978 statistics_.addStatistic(Statistics::kOdomCorrectionInliers(), inliers);
00979 statistics_.addStatistic(Statistics::kOdomCorrectionInliers_ratio(), inliersRatio);
00980 statistics_.addStatistic(Statistics::kOdomCorrectionVariance(), variance);
00981 }
00982 timeScanMatching = timer.ticks();
00983 ULOGGER_INFO("timeScanMatching=%fs", timeScanMatching);
00984
00985 if(signature->getLinks().size() == 1)
00986 {
00987
00988 if(signature->id() > signature->getLinks().begin()->second.to())
00989 {
00990 Link tmp = signature->getLinks().begin()->second;
00991 tmp.setFrom(tmp.to());
00992 tmp.setTo(signature->id());
00993 tmp.setTransform(tmp.transform().inverse());
00994 _constraints.insert(std::make_pair(tmp.from(), tmp));
00995 }
00996 else
00997 {
00998 _constraints.insert(std::make_pair(signature->id(), signature->getLinks().begin()->second));
00999 }
01000 }
01001
01002
01003
01004
01005 if(_localLoopClosureDetectionTime &&
01006 rehearsedId == 0 &&
01007 signature->getWords3().size())
01008 {
01009 const std::set<int> & stm = _memory->getStMem();
01010 for(std::set<int>::const_reverse_iterator iter = stm.rbegin(); iter!=stm.rend(); ++iter)
01011 {
01012 if(*iter != signature->id() &&
01013 signature->getLinks().find(*iter) == signature->getLinks().end() &&
01014 _memory->getSignature(*iter)->mapId() == signature->mapId())
01015 {
01016 std::string rejectedMsg;
01017 UDEBUG("Check local transform between %d and %d", signature->id(), *iter);
01018 double variance = 1.0;
01019 int inliers = -1;
01020 Transform transform = _memory->computeVisualTransform(*iter, signature->id(), &rejectedMsg, &inliers, &variance);
01021 if(!transform.isNull() && _globalLoopClosureIcpType > 0)
01022 {
01023 transform = _memory->computeIcpTransform(*iter, signature->id(), transform, _globalLoopClosureIcpType==1, &rejectedMsg, 0, &variance);
01024 variance = 1.0f;
01025 }
01026 if(!transform.isNull())
01027 {
01028 UDEBUG("Add local loop closure in TIME (%d->%d) %s",
01029 signature->id(),
01030 *iter,
01031 transform.prettyPrint().c_str());
01032
01033 if(_memory->addLink(*iter, signature->id(), transform, Link::kLocalTimeClosure, variance, variance))
01034 {
01035 ++localLoopClosuresInTimeFound;
01036 UINFO("Local loop closure found between %d and %d with t=%s",
01037 *iter, signature->id(), transform.prettyPrint().c_str());
01038 }
01039 else
01040 {
01041 UWARN("Cannot add local loop closure between %d and %d ?!?",
01042 *iter, signature->id());
01043 }
01044 }
01045 else
01046 {
01047 UINFO("Local loop closure (time) between %d and %d rejected: %s",
01048 *iter, signature->id(), rejectedMsg.c_str());
01049 }
01050 }
01051 }
01052 }
01053 }
01054
01055 timeLocalTimeDetection = timer.ticks();
01056 UINFO("timeLocalTimeDetection=%fs", timeLocalTimeDetection);
01057
01058
01059
01060
01061 int previousId = signature->getLinks().size() == 1?signature->getLinks().begin()->first:0;
01062
01063 if(!signature->isBadSignature() && (!smallDisplacement || _memory->getLoopClosureLinks(previousId, false).size() == 0))
01064 {
01065
01066
01067
01068 if(_memory->getWorkingMem().size())
01069 {
01070
01071
01072
01073
01074
01075 ULOGGER_INFO("computing likelihood...");
01076 std::list<int> signaturesToCompare = uKeysList(_memory->getWorkingMem());
01077 rawLikelihood = _memory->computeLikelihood(signature, signaturesToCompare);
01078
01079
01080 likelihood = rawLikelihood;
01081 this->adjustLikelihood(likelihood);
01082
01083 timeLikelihoodCalculation = timer.ticks();
01084 ULOGGER_INFO("timeLikelihoodCalculation=%fs",timeLikelihoodCalculation);
01085
01086
01087
01088
01089
01090 ULOGGER_INFO("getting posterior...");
01091
01092
01093 posterior = _bayesFilter->computePosterior(_memory, likelihood);
01094 timePosteriorCalculation = timer.ticks();
01095 ULOGGER_INFO("timePosteriorCalculation=%fs",timePosteriorCalculation);
01096
01097
01098 if(_publishStats && (_publishLikelihood || _publishPdf))
01099 {
01100 weights = _memory->getWeights();
01101 }
01102
01103 timer.start();
01104
01105
01106
01107 ULOGGER_INFO("creating hypotheses...");
01108 if(posterior.size())
01109 {
01110 for(std::map<int, float>::const_reverse_iterator iter = posterior.rbegin(); iter != posterior.rend(); ++iter)
01111 {
01112 if(iter->first > 0 && iter->second > _highestHypothesis.second)
01113 {
01114 _highestHypothesis = *iter;
01115 }
01116 }
01117
01118 _highestHypothesis.second = 1-posterior.begin()->second;
01119 }
01120 timeHypothesesCreation = timer.ticks();
01121 ULOGGER_INFO("Highest hypothesis=%d, value=%f, timeHypothesesCreation=%fs", _highestHypothesis.first, _highestHypothesis.second, timeHypothesesCreation);
01122
01123 if(_highestHypothesis.first > 0)
01124 {
01125
01126
01127
01128 if(_highestHypothesis.second >= _loopThr)
01129 {
01130 rejectedHypothesis = true;
01131 if(posterior.size() <= 2)
01132 {
01133
01134 UDEBUG("rejected hypothesis: single hypothesis");
01135 }
01136 else if(_epipolarGeometry && !_epipolarGeometry->check(signature, _memory->getSignature(_highestHypothesis.first)))
01137 {
01138 UWARN("rejected hypothesis: by epipolar geometry");
01139 }
01140 else if(_loopRatio > 0.0f && lastHighestHypothesis.second && _highestHypothesis.second < _loopRatio*lastHighestHypothesis.second)
01141 {
01142 UWARN("rejected hypothesis: not satisfying hypothesis ratio (%f < %f * %f)",
01143 _highestHypothesis.second, _loopRatio, lastHighestHypothesis.second);
01144 }
01145 else if(_loopRatio > 0.0f && lastHighestHypothesis.second == 0)
01146 {
01147 UWARN("rejected hypothesis: last closure hypothesis is null (loop ratio is on)");
01148 }
01149 else
01150 {
01151 _loopClosureHypothesis = _highestHypothesis;
01152 rejectedHypothesis = false;
01153 }
01154
01155 timeHypothesesValidation = timer.ticks();
01156 ULOGGER_INFO("timeHypothesesValidation=%fs",timeHypothesesValidation);
01157 }
01158 else if(_highestHypothesis.second < _loopRatio*lastHighestHypothesis.second)
01159 {
01160
01161
01162
01163 rejectedHypothesis = true;
01164 }
01165
01166
01167 hypothesisRatio = _loopClosureHypothesis.second>0?_highestHypothesis.second/_loopClosureHypothesis.second:0;
01168 }
01169 }
01170 }
01171 else if(!signature->isBadSignature() && smallDisplacement)
01172 {
01173 _highestHypothesis = lastHighestHypothesis;
01174 }
01175
01176
01177
01178
01179 _memory->joinTrashThread();
01180 timeEmptyingTrash = _memory->getDbSavingTime();
01181 timeJoiningTrash = timer.ticks();
01182 ULOGGER_INFO("Time emptying memory trash = %fs, joining (actual overhead) = %fs", timeEmptyingTrash, timeJoiningTrash);
01183
01184
01185
01186
01187 int retrievalId = _highestHypothesis.first;
01188 std::list<int> reactivatedIds;
01189 double timeGetNeighborsTimeDb = 0.0;
01190 double timeGetNeighborsSpaceDb = 0.0;
01191 int immunizedGlobally = 0;
01192 int immunizedLocally = 0;
01193 if(retrievalId > 0 )
01194 {
01195
01196 ULOGGER_INFO("Retrieving locations... around id=%d", retrievalId);
01197 int neighborhoodSize = (int)_bayesFilter->getPredictionLC().size()-1;
01198 UASSERT(neighborhoodSize >= 0);
01199 ULOGGER_DEBUG("margin=%d maxRetieved=%d", neighborhoodSize, _maxRetrieved);
01200
01201 UTimer timeGetN;
01202 unsigned int nbLoadedFromDb = 0;
01203 std::set<int> reactivatedIdsSet;
01204 std::map<int, int> neighbors;
01205 int nbDirectNeighborsInDb = 0;
01206
01207
01208
01209 ULOGGER_DEBUG("In TIME");
01210 neighbors = _memory->getNeighborsId(retrievalId,
01211 neighborhoodSize,
01212 _maxRetrieved,
01213 true,
01214 true,
01215 &timeGetNeighborsTimeDb);
01216 ULOGGER_DEBUG("neighbors of %d in time = %d", retrievalId, (int)neighbors.size());
01217
01218 bool firstPassDone = false;
01219 int m = 0;
01220 while(m < neighborhoodSize)
01221 {
01222 std::set<int> idsSorted;
01223 for(std::map<int, int>::iterator iter=neighbors.begin(); iter!=neighbors.end();)
01224 {
01225 if(!firstPassDone && _memory->isInSTM(iter->first))
01226 {
01227 neighbors.erase(iter++);
01228 }
01229 else if(iter->second == m)
01230 {
01231 if(reactivatedIdsSet.find(iter->first) == reactivatedIdsSet.end())
01232 {
01233 idsSorted.insert(iter->first);
01234 reactivatedIdsSet.insert(iter->first);
01235
01236 if(m == 1 && _memory->getSignature(iter->first) == 0)
01237 {
01238 ++nbDirectNeighborsInDb;
01239 }
01240
01241
01242 if(immunizedLocations.insert(iter->first).second)
01243 {
01244 ++immunizedGlobally;
01245 }
01246
01247 UDEBUG("nt=%d m=%d immunized=1", iter->first, iter->second);
01248 }
01249 neighbors.erase(iter++);
01250 }
01251 else
01252 {
01253 ++iter;
01254 }
01255 }
01256 firstPassDone = true;
01257 reactivatedIds.insert(reactivatedIds.end(), idsSorted.rbegin(), idsSorted.rend());
01258 ++m;
01259 }
01260
01261
01262 ULOGGER_DEBUG("In SPACE");
01263 neighbors = _memory->getNeighborsId(retrievalId,
01264 neighborhoodSize,
01265 _maxRetrieved,
01266 true,
01267 false,
01268 &timeGetNeighborsSpaceDb);
01269 ULOGGER_DEBUG("neighbors of %d in space = %d", retrievalId, (int)neighbors.size());
01270 firstPassDone = false;
01271 m = 0;
01272 while(m < neighborhoodSize)
01273 {
01274 std::set<int> idsSorted;
01275 for(std::map<int, int>::iterator iter=neighbors.begin(); iter!=neighbors.end();)
01276 {
01277 if(!firstPassDone && _memory->isInSTM(iter->first))
01278 {
01279 neighbors.erase(iter++);
01280 }
01281 else if(iter->second == m)
01282 {
01283 if(reactivatedIdsSet.find(iter->first) == reactivatedIdsSet.end())
01284 {
01285 idsSorted.insert(iter->first);
01286 reactivatedIdsSet.insert(iter->first);
01287
01288 if(m == 1 && _memory->getSignature(iter->first) == 0)
01289 {
01290 ++nbDirectNeighborsInDb;
01291 }
01292 UDEBUG("nt=%d m=%d", iter->first, iter->second);
01293 }
01294 neighbors.erase(iter++);
01295 }
01296 else
01297 {
01298 ++iter;
01299 }
01300 }
01301 firstPassDone = true;
01302 reactivatedIds.insert(reactivatedIds.end(), idsSorted.rbegin(), idsSorted.rend());
01303 ++m;
01304 }
01305 ULOGGER_INFO("neighborhoodSize=%d, "
01306 "reactivatedIds.size=%d, "
01307 "nbLoadedFromDb=%d, "
01308 "nbDirectNeighborsInDb=%d, "
01309 "time=%fs (%fs %fs)",
01310 neighborhoodSize,
01311 reactivatedIds.size(),
01312 (int)nbLoadedFromDb,
01313 nbDirectNeighborsInDb,
01314 timeGetN.ticks(),
01315 timeGetNeighborsTimeDb,
01316 timeGetNeighborsSpaceDb);
01317
01318 }
01319
01320
01321
01322
01323 std::list<int> retrievalLocalIds;
01324 int maxLocalLocationsImmunized = _localImmunizationRatio * float(_memory->getWorkingMem().size());
01325 if(_rgbdSlamMode)
01326 {
01327
01328 if(_path.size())
01329 {
01330 updateGoalIndex();
01331
01332 float distanceSoFar = 0.0f;
01333
01334
01335 for(unsigned int i=_pathCurrentIndex; i<_path.size(); ++i)
01336 {
01337 if(_localRadius > 0.0f && i != _pathCurrentIndex)
01338 {
01339 distanceSoFar += _path[i-1].second.getDistance(_path[i].second);
01340 }
01341
01342 if(distanceSoFar <= _localRadius)
01343 {
01344 if(_memory->getSignature(_path[i].first) != 0)
01345 {
01346 if(immunizedLocations.insert(_path[i].first).second)
01347 {
01348 ++immunizedLocally;
01349 }
01350 UDEBUG("Path immunization: node %d (dist=%fm)", _path[i].first, distanceSoFar);
01351 }
01352 else if(retrievalLocalIds.size() < _maxLocalRetrieved)
01353 {
01354 UINFO("retrieval of node %d on path (dist=%fm)", _path[i].first, distanceSoFar);
01355 retrievalLocalIds.push_back(_path[i].first);
01356
01357 }
01358 }
01359 else
01360 {
01361 UDEBUG("Stop on node %d (dist=%fm > %fm)",
01362 _path[i].first, distanceSoFar, _localRadius);
01363 break;
01364 }
01365 }
01366 }
01367
01368
01369 if(immunizedLocally < maxLocalLocationsImmunized &&
01370 _memory->isIncremental())
01371 {
01372 std::map<int ,Transform> poses;
01373
01374 for(std::map<int, Transform>::iterator iter=_optimizedPoses.begin(); iter!=_optimizedPoses.end(); ++iter)
01375 {
01376 if(!_memory->isInSTM(iter->first))
01377 {
01378 poses.insert(*iter);
01379 }
01380 }
01381 int nearestId = graph::findNearestNode(poses, _optimizedPoses.at(signature->id()));
01382
01383 if(nearestId > 0 &&
01384 (_localRadius==0 ||
01385 _optimizedPoses.at(signature->id()).getDistance(_optimizedPoses.at(nearestId)) < _localRadius))
01386 {
01387 std::multimap<int, int> links;
01388 for(std::multimap<int, Link>::iterator iter=_constraints.begin(); iter!=_constraints.end(); ++iter)
01389 {
01390 if(uContains(_optimizedPoses, iter->second.from()) && uContains(_optimizedPoses, iter->second.to()))
01391 {
01392 links.insert(std::make_pair(iter->second.from(), iter->second.to()));
01393 links.insert(std::make_pair(iter->second.to(), iter->second.from()));
01394 }
01395 }
01396
01397 std::list<std::pair<int, Transform> > path = graph::computePath(_optimizedPoses, links, nearestId, signature->id());
01398 if(path.size() == 0)
01399 {
01400 UWARN("Could not compute a path between %d and %d", nearestId, signature->id());
01401 }
01402 else
01403 {
01404 for(std::list<std::pair<int, Transform> >::iterator iter=path.begin();
01405 iter!=path.end();
01406 ++iter)
01407 {
01408 if(immunizedLocally >= maxLocalLocationsImmunized)
01409 {
01410 UWARN("Could not immunize the whole local path (%d) between "
01411 "%d and %d (max location immunized=%d). You may want "
01412 "to increase RGBD/LocalImmunizationRatio (current=%f (%d of WM=%d)) "
01413 "to be able to immunize longer paths.",
01414 (int)path.size(),
01415 nearestId,
01416 signature->id(),
01417 maxLocalLocationsImmunized,
01418 _localImmunizationRatio,
01419 maxLocalLocationsImmunized,
01420 (int)_memory->getWorkingMem().size());
01421 break;
01422 }
01423 else if(!_memory->isInSTM(iter->first))
01424 {
01425 if(immunizedLocations.insert(iter->first).second)
01426 {
01427 ++immunizedLocally;
01428 }
01429 UDEBUG("local node %d on path immunized=1", iter->first);
01430 }
01431 }
01432 }
01433 }
01434 }
01435
01436
01437
01438 std::map<int, float> nearNodes = graph::getNodesInRadius(signature->id(), _optimizedPoses, _localRadius);
01439
01440 std::multimap<float, int> nearNodesByDist;
01441 for(std::map<int, float>::iterator iter=nearNodes.begin(); iter!=nearNodes.end(); ++iter)
01442 {
01443 nearNodesByDist.insert(std::make_pair(iter->second, iter->first));
01444 }
01445 UINFO("near nodes=%d, max local immunized=%d, ratio=%f WM=%d",
01446 (int)nearNodesByDist.size(),
01447 maxLocalLocationsImmunized,
01448 _localImmunizationRatio,
01449 (int)_memory->getWorkingMem().size());
01450 for(std::multimap<float, int>::iterator iter=nearNodesByDist.begin();
01451 iter!=nearNodesByDist.end() && (retrievalLocalIds.size() < _maxLocalRetrieved || immunizedLocally < maxLocalLocationsImmunized);
01452 ++iter)
01453 {
01454 const Signature * s = _memory->getSignature(iter->second);
01455 UASSERT(s!=0);
01456
01457
01458 const std::map<int, Link> & links = s->getLinks();
01459 for(std::map<int, Link>::const_reverse_iterator jter=links.rbegin();
01460 jter!=links.rend() && retrievalLocalIds.size() < _maxLocalRetrieved;
01461 ++jter)
01462 {
01463 if(_memory->getSignature(jter->first) == 0)
01464 {
01465 UINFO("retrieval of node %d on local map", jter->first);
01466 retrievalLocalIds.push_back(jter->first);
01467 }
01468 }
01469 if(!_memory->isInSTM(s->id()) && immunizedLocally < maxLocalLocationsImmunized)
01470 {
01471 if(immunizedLocations.insert(s->id()).second)
01472 {
01473 ++immunizedLocally;
01474 }
01475 UDEBUG("local node %d (%f m) immunized=1", iter->second, iter->first);
01476 }
01477 }
01478
01479 if(retrievalLocalIds.size() < _maxLocalRetrieved)
01480 {
01481 std::set<int> retrievalLocalIdsSet(retrievalLocalIds.begin(), retrievalLocalIds.end());
01482 for(std::list<int>::iterator iter=retrievalLocalIds.begin();
01483 iter!=retrievalLocalIds.end() && retrievalLocalIds.size() < _maxLocalRetrieved;
01484 ++iter)
01485 {
01486 std::map<int, int> ids = _memory->getNeighborsId(*iter, 2, _maxLocalRetrieved - retrievalLocalIds.size() + 1, true, false);
01487 for(std::map<int, int>::reverse_iterator jter=ids.rbegin();
01488 jter!=ids.rend() && retrievalLocalIds.size() < _maxLocalRetrieved;
01489 ++jter)
01490 {
01491 if(_memory->getSignature(jter->first) == 0 &&
01492 retrievalLocalIdsSet.find(jter->first) == retrievalLocalIdsSet.end())
01493 {
01494 UINFO("retrieval of node %d on local map", jter->first);
01495 retrievalLocalIds.push_back(jter->first);
01496 retrievalLocalIdsSet.insert(jter->first);
01497 }
01498 }
01499 }
01500 }
01501
01502
01503 for(std::multimap<float, int>::reverse_iterator iter=nearNodesByDist.rbegin(); iter!=nearNodesByDist.rend(); ++iter)
01504 {
01505 _memory->updateAge(iter->second);
01506 }
01507
01508
01509 reactivatedIds.insert(reactivatedIds.begin(), retrievalLocalIds.begin(), retrievalLocalIds.end());
01510 }
01511
01512
01513
01514
01515 if(reactivatedIds.size())
01516 {
01517
01518
01519 signaturesRetrieved = _memory->reactivateSignatures(
01520 reactivatedIds,
01521 _maxRetrieved+(unsigned int)retrievalLocalIds.size(),
01522 timeRetrievalDbAccess);
01523
01524 ULOGGER_INFO("retrieval of %d (db time = %fs)", (int)signaturesRetrieved.size(), timeRetrievalDbAccess);
01525
01526 timeRetrievalDbAccess += timeGetNeighborsTimeDb + timeGetNeighborsSpaceDb;
01527 UINFO("total timeRetrievalDbAccess=%fs", timeRetrievalDbAccess);
01528
01529
01530 immunizedLocations.insert(signaturesRetrieved.begin(), signaturesRetrieved.end());
01531 }
01532 timeReactivations = timer.ticks();
01533 ULOGGER_INFO("timeReactivations=%fs", timeReactivations);
01534
01535
01536
01537
01538
01539 int loopClosureVisualInliers = 0;
01540 if(_loopClosureHypothesis.first>0)
01541 {
01542
01543 Transform transform;
01544 double variance = 1;
01545 if(_rgbdSlamMode)
01546 {
01547 std::string rejectedMsg;
01548 if(_reextractLoopClosureFeatures)
01549 {
01550 ParametersMap customParameters = _modifiedParameters;
01551
01552 uInsert(customParameters, ParametersPair(Parameters::kMemIncrementalMemory(), "true"));
01553 uInsert(customParameters, ParametersPair(Parameters::kMemRehearsalSimilarity(), "1.0"));
01554 uInsert(customParameters, ParametersPair(Parameters::kMemBinDataKept(), "false"));
01555 uInsert(customParameters, ParametersPair(Parameters::kMemSTMSize(), "0"));
01556 uInsert(customParameters, ParametersPair(Parameters::kKpIncrementalDictionary(), "true"));
01557 uInsert(customParameters, ParametersPair(Parameters::kKpNewWordsComparedTogether(), "false"));
01558 uInsert(customParameters, ParametersPair(Parameters::kKpNNStrategy(), uNumber2Str(_reextractNNType)));
01559 uInsert(customParameters, ParametersPair(Parameters::kKpNndrRatio(), uNumber2Str(_reextractNNDR)));
01560 uInsert(customParameters, ParametersPair(Parameters::kKpDetectorStrategy(), uNumber2Str(_reextractFeatureType)));
01561 uInsert(customParameters, ParametersPair(Parameters::kKpWordsPerImage(), uNumber2Str(_reextractMaxWords)));
01562 uInsert(customParameters, ParametersPair(Parameters::kKpBadSignRatio(), "0"));
01563 uInsert(customParameters, ParametersPair(Parameters::kKpRoiRatios(), "0.0 0.0 0.0 0.0"));
01564 uInsert(customParameters, ParametersPair(Parameters::kMemGenerateIds(), "false"));
01565
01566
01567
01568
01569
01570
01571 Memory memory(customParameters);
01572
01573 UTimer timeT;
01574
01575
01576 SensorData dataFrom = data;
01577 dataFrom.setId(signature->id());
01578 Signature tmpTo = _memory->getSignatureData(_loopClosureHypothesis.first, true);
01579 SensorData dataTo = tmpTo.toSensorData();
01580 UDEBUG("timeTo = %fs", timeT.ticks());
01581
01582 if(dataFrom.isValid() &&
01583 dataFrom.isMetric() &&
01584 dataTo.isValid() &&
01585 dataTo.isMetric() &&
01586 dataFrom.id() != Memory::kIdInvalid &&
01587 tmpTo.id() != Memory::kIdInvalid)
01588 {
01589 memory.update(dataTo);
01590 UDEBUG("timeUpTo = %fs", timeT.ticks());
01591 memory.update(dataFrom);
01592 UDEBUG("timeUpFrom = %fs", timeT.ticks());
01593
01594 transform = memory.computeVisualTransform(dataTo.id(), dataFrom.id(), &rejectedMsg, &loopClosureVisualInliers, &variance);
01595 UDEBUG("timeTransform = %fs", timeT.ticks());
01596 }
01597 else
01598 {
01599
01600 UWARN("Loop closure: Some images not found in memory for re-extracting "
01601 "features, is Mem/RawDataKept=false? Falling back with already extracted 3D features.");
01602 transform = _memory->computeVisualTransform(_loopClosureHypothesis.first, signature->id(), &rejectedMsg, &loopClosureVisualInliers, &variance);
01603 }
01604 }
01605 else
01606 {
01607 transform = _memory->computeVisualTransform(_loopClosureHypothesis.first, signature->id(), &rejectedMsg, &loopClosureVisualInliers, &variance);
01608 }
01609 if(!transform.isNull() && _globalLoopClosureIcpType > 0)
01610 {
01611 transform = _memory->computeIcpTransform(_loopClosureHypothesis.first, signature->id(), transform, _globalLoopClosureIcpType == 1, &rejectedMsg, 0, &variance);
01612 }
01613 rejectedHypothesis = transform.isNull();
01614 if(rejectedHypothesis)
01615 {
01616 UINFO("Rejected loop closure %d -> %d: %s",
01617 _loopClosureHypothesis.first, signature->id(), rejectedMsg.c_str());
01618 }
01619 }
01620 if(!rejectedHypothesis)
01621 {
01622
01623 rejectedHypothesis = !_memory->addLink(_loopClosureHypothesis.first, signature->id(), transform, Link::kGlobalClosure, variance, variance);
01624 }
01625
01626 if(rejectedHypothesis)
01627 {
01628 _loopClosureHypothesis.first = 0;
01629 }
01630 else
01631 {
01632 const Signature * oldS = _memory->getSignature(_loopClosureHypothesis.first);
01633 UASSERT(oldS != 0);
01634
01635 _mapTransform = oldS->getPose() * transform.inverse() * signature->getPose().inverse();
01636 }
01637 }
01638
01639 timeAddLoopClosureLink = timer.ticks();
01640 ULOGGER_INFO("timeAddLoopClosureLink=%fs", timeAddLoopClosureLink);
01641
01642 int localSpaceClosuresAddedVisually = 0;
01643 int localSpaceClosuresAddedByICPOnly = 0;
01644 int lastLocalSpaceClosureId = 0;
01645 int localSpacePaths = 0;
01646 if(_localLoopClosureDetectionSpace &&
01647 _localRadius > 0)
01648 {
01649 if(_graphOptimizer->iterations() == 0)
01650 {
01651 UWARN("Cannot do local loop closure detection in space if graph optimization is disabled!");
01652 }
01653 else if(_memory->isIncremental() || _loopClosureHypothesis.first == 0)
01654 {
01655
01656
01657
01658
01659 if(!smallDisplacement || _memory->getLoopClosureLinks(previousId, false).size() == 0)
01660 {
01661
01662
01663
01664
01665
01666
01667
01668
01669 float r = _localRadius;
01670 if(_localPathFilteringRadius > 0 && _localPathFilteringRadius<_localRadius)
01671 {
01672 r = _localPathFilteringRadius;
01673 }
01674
01675 std::map<int, float> nearestIds;
01676 if(_memory->isIncremental())
01677 {
01678 nearestIds = _memory->getNeighborsIdRadius(signature->id(), r, _optimizedPoses, _localDetectMaxGraphDepth);
01679 }
01680 else
01681 {
01682 nearestIds = graph::getNodesInRadius(signature->id(), _optimizedPoses, r);
01683 }
01684 std::map<int, Transform> nearestPoses;
01685 for(std::map<int, float>::iterator iter=nearestIds.begin(); iter!=nearestIds.end(); ++iter)
01686 {
01687 nearestPoses.insert(std::make_pair(iter->first, _optimizedPoses.at(iter->first)));
01688 }
01689
01690 std::list<std::map<int, Transform> > nearestPaths = getPaths(nearestPoses);
01691 for(std::list<std::map<int, Transform> >::iterator iter=nearestPaths.begin();
01692 iter!=nearestPaths.end() && (_memory->isIncremental() || lastLocalSpaceClosureId == 0);
01693 ++iter)
01694 {
01695 std::map<int, Transform> & path = *iter;
01696 UASSERT(path.size());
01697
01698 int nearestId = rtabmap::graph::findNearestNode(path, _optimizedPoses.at(signature->id()));
01699 UASSERT(nearestId > 0);
01700
01701
01702 if(!signature->hasLink(nearestId) &&
01703 _memory->getStMem().find(nearestId) == _memory->getStMem().end())
01704 {
01705 double variance = 1.0;
01706 Transform transform;
01707 if(_reextractLoopClosureFeatures)
01708 {
01709 ParametersMap customParameters = _modifiedParameters;
01710
01711 uInsert(customParameters, ParametersPair(Parameters::kMemIncrementalMemory(), "true"));
01712 uInsert(customParameters, ParametersPair(Parameters::kMemRehearsalSimilarity(), "1.0"));
01713 uInsert(customParameters, ParametersPair(Parameters::kMemBinDataKept(), "false"));
01714 uInsert(customParameters, ParametersPair(Parameters::kMemSTMSize(), "0"));
01715 uInsert(customParameters, ParametersPair(Parameters::kKpIncrementalDictionary(), "true"));
01716 uInsert(customParameters, ParametersPair(Parameters::kKpNewWordsComparedTogether(), "false"));
01717 uInsert(customParameters, ParametersPair(Parameters::kKpNNStrategy(), uNumber2Str(_reextractNNType)));
01718 uInsert(customParameters, ParametersPair(Parameters::kKpNndrRatio(), uNumber2Str(_reextractNNDR)));
01719 uInsert(customParameters, ParametersPair(Parameters::kKpDetectorStrategy(), uNumber2Str(_reextractFeatureType)));
01720 uInsert(customParameters, ParametersPair(Parameters::kKpWordsPerImage(), uNumber2Str(_reextractMaxWords)));
01721 uInsert(customParameters, ParametersPair(Parameters::kKpBadSignRatio(), "0"));
01722 uInsert(customParameters, ParametersPair(Parameters::kKpRoiRatios(), "0.0 0.0 0.0 0.0"));
01723 uInsert(customParameters, ParametersPair(Parameters::kMemGenerateIds(), "false"));
01724
01725
01726
01727
01728
01729
01730 Memory memory(customParameters);
01731
01732 UTimer timeT;
01733
01734
01735 SensorData dataFrom = data;
01736 dataFrom.setId(signature->id());
01737 Signature tmpTo = _memory->getSignatureData(nearestId, true);
01738 SensorData dataTo = tmpTo.toSensorData();
01739 UDEBUG("timeTo = %fs", timeT.ticks());
01740
01741 if(dataFrom.isValid() &&
01742 dataFrom.isMetric() &&
01743 dataTo.isValid() &&
01744 dataTo.isMetric() &&
01745 dataFrom.id() != Memory::kIdInvalid &&
01746 tmpTo.id() != Memory::kIdInvalid)
01747 {
01748 memory.update(dataTo);
01749 UDEBUG("timeUpTo = %fs", timeT.ticks());
01750 memory.update(dataFrom);
01751 UDEBUG("timeUpFrom = %fs", timeT.ticks());
01752
01753 transform = memory.computeVisualTransform(dataTo.id(), dataFrom.id(), 0, 0, &variance);
01754 UDEBUG("timeTransform = %fs", timeT.ticks());
01755 }
01756 else
01757 {
01758
01759 UWARN("Loop closure: Some images not found in memory for re-extracting "
01760 "features, is Mem/RawDataKept=false? Falling back with already extracted 3D features.");
01761 transform = _memory->computeVisualTransform(nearestId, signature->id(), 0, 0, &variance);
01762 }
01763 }
01764 else
01765 {
01766 transform = _memory->computeVisualTransform(nearestId, signature->id(), 0, 0, &variance);
01767 }
01768 if(!transform.isNull() && _globalLoopClosureIcpType > 0)
01769 {
01770 transform = _memory->computeIcpTransform(nearestId, signature->id(), transform, _globalLoopClosureIcpType == 1, 0, 0, &variance);
01771 }
01772 if(!transform.isNull())
01773 {
01774 UINFO("[Visual] Add local loop closure in SPACE (%d->%d) %s",
01775 signature->id(),
01776 nearestId,
01777 transform.prettyPrint().c_str());
01778 _memory->addLink(nearestId, signature->id(), transform, Link::kLocalSpaceClosure, variance, variance);
01779
01780 if(_loopClosureHypothesis.first == 0)
01781 {
01782
01783 const Signature * oldS = _memory->getSignature(nearestId);
01784 UASSERT(oldS != 0);
01785 _mapTransform = oldS->getPose() * transform.inverse() * signature->getPose().inverse();
01786 ++localSpaceClosuresAddedVisually;
01787 lastLocalSpaceClosureId = nearestId;
01788 }
01789 }
01790 }
01791 }
01792
01793
01794
01795
01796 if( !signature->getLaserScanCompressed().empty() &&
01797 (_memory->isIncremental() || lastLocalSpaceClosureId == 0))
01798 {
01799
01800
01801
01802
01803 std::map<int, Transform> forwardPoses;
01804 forwardPoses = this->getForwardWMPoses(
01805 signature->id(),
01806 0,
01807 _localRadius,
01808 _localDetectMaxGraphDepth);
01809
01810 std::list<std::map<int, Transform> > forwardPaths = getPaths(forwardPoses);
01811 localSpacePaths = (int)forwardPaths.size();
01812
01813 for(std::list<std::map<int, Transform> >::iterator iter=forwardPaths.begin();
01814 iter!=forwardPaths.end() && (_memory->isIncremental() || lastLocalSpaceClosureId == 0);
01815 ++iter)
01816 {
01817 std::map<int, Transform> & path = *iter;
01818 UASSERT(path.size());
01819
01820
01821 int nearestId = rtabmap::graph::findNearestNode(path, _optimizedPoses.at(signature->id()));
01822 UASSERT(nearestId > 0);
01823
01824
01825 if(!signature->hasLink(nearestId) &&
01826 (_localPathFilteringRadius <= 0.0f ||
01827 _optimizedPoses.at(signature->id()).getDistanceSquared(_optimizedPoses.at(nearestId)) < _localPathFilteringRadius*_localPathFilteringRadius))
01828 {
01829
01830 if(_localPathOdomPosesUsed)
01831 {
01832
01833 path = optimizeGraph(nearestId, uKeysSet(path), false);
01834
01835 Transform t = _optimizedPoses.at(nearestId) * path.at(nearestId).inverse();
01836 for(std::map<int, Transform>::iterator jter=path.begin(); jter!=path.end(); ++jter)
01837 {
01838 jter->second = t * jter->second;
01839 }
01840 }
01841 if(_localPathFilteringRadius > 0.0f)
01842 {
01843
01844 std::map<int, Transform> filteredPath = graph::radiusPosesFiltering(path, _localPathFilteringRadius, CV_PI, true);
01845
01846 filteredPath.insert(*path.find(nearestId));
01847 filteredPath.insert(*path.begin());
01848 filteredPath.insert(*path.rbegin());
01849 path = filteredPath;
01850 }
01851
01852 if(path.size() > 2)
01853 {
01854
01855 path.insert(std::make_pair(signature->id(), _optimizedPoses.at(signature->id())));
01856
01857 if(signature->getLinks().find(nearestId) == signature->getLinks().end())
01858 {
01859 Transform transform = _memory->computeScanMatchingTransform(signature->id(), nearestId, path, 0, 0, 0);
01860 if(!transform.isNull())
01861 {
01862 UINFO("[Scan matching] Add local loop closure in SPACE (%d->%d) %s",
01863 signature->id(),
01864 nearestId,
01865 transform.prettyPrint().c_str());
01866
01867 _memory->addLink(nearestId, signature->id(), transform, Link::kLocalSpaceClosure, 1, 1);
01868
01869 ++localSpaceClosuresAddedByICPOnly;
01870
01871
01872 if(localSpaceClosuresAddedVisually == 0 && _loopClosureHypothesis.first == 0)
01873 {
01874
01875 const Signature * oldS = _memory->getSignature(nearestId);
01876 UASSERT(oldS != 0);
01877 _mapTransform = oldS->getPose() * transform.inverse() * signature->getPose().inverse();
01878 lastLocalSpaceClosureId = nearestId;
01879 }
01880 }
01881 }
01882 }
01883 }
01884 }
01885 }
01886 }
01887 }
01888 }
01889 timeLocalSpaceDetection = timer.ticks();
01890 ULOGGER_INFO("timeLocalSpaceDetection=%fs", timeLocalSpaceDetection);
01891
01892
01893
01894
01895 if(_rgbdSlamMode &&
01896 (_loopClosureHypothesis.first>0 ||
01897 localLoopClosuresInTimeFound>0 ||
01898 scanMatchingSuccess ||
01899 lastLocalSpaceClosureId>0 ||
01900 signaturesRetrieved.size()))
01901 {
01902 if(_memory->isIncremental())
01903 {
01904 UINFO("Update map correction: SLAM mode");
01905
01906 optimizeCurrentMap(signature->id(), false, _optimizedPoses, &_constraints);
01907
01908
01909 _mapCorrection = _optimizedPoses.at(signature->id()) * signature->getPose().inverse();
01910 _mapTransform.setIdentity();
01911 _lastLocalizationPose = _optimizedPoses.at(signature->id());
01912 if(_mapCorrection.getNormSquared() > 0.001f && _optimizeFromGraphEnd)
01913 {
01914 UERROR("Map correction should be identity when optimizing from the last node. T=%s", _mapCorrection.prettyPrint().c_str());
01915 }
01916 }
01917 else if(_loopClosureHypothesis.first > 0 || lastLocalSpaceClosureId > 0 || signaturesRetrieved.size())
01918 {
01919 UINFO("Update map correction: Localization mode");
01920 int oldId = _loopClosureHypothesis.first>0?_loopClosureHypothesis.first:lastLocalSpaceClosureId?lastLocalSpaceClosureId:_highestHypothesis.first;
01921 UASSERT(oldId != 0);
01922 if(signaturesRetrieved.size() || _optimizedPoses.find(oldId) == _optimizedPoses.end())
01923 {
01924
01925 optimizeCurrentMap(oldId, false, _optimizedPoses, &_constraints);
01926 }
01927 UASSERT(_optimizedPoses.find(oldId) != _optimizedPoses.end());
01928
01929
01930 const Signature * oldS = _memory->getSignature(oldId);
01931 UASSERT(oldS != 0);
01932 Transform correction = _optimizedPoses.at(oldId) * oldS->getPose().inverse();
01933 _mapCorrection = correction * _mapTransform;
01934 _lastLocalizationPose = _mapCorrection * signature->getPose();
01935 }
01936 else
01937 {
01938 UERROR("Not supposed to be here!");
01939 }
01940 }
01941
01942 timeMapOptimization = timer.ticks();
01943 ULOGGER_INFO("timeMapOptimization=%fs", timeMapOptimization);
01944
01945
01946
01947
01948 if(_path.size())
01949 {
01950
01951 if( signature->id() != _path[_pathCurrentIndex].first &&
01952 !signature->hasLink(_path[_pathCurrentIndex].first) &&
01953 uContains(_optimizedPoses, _path[_pathCurrentIndex].first))
01954 {
01955 Transform virtualLoop = _optimizedPoses.at(signature->id()).inverse() * _optimizedPoses.at(_path[_pathCurrentIndex].first);
01956 if(_localRadius > 0.0f && virtualLoop.getNorm() < _localRadius)
01957 {
01958 _memory->addLink(_path[_pathCurrentIndex].first, signature->id(), virtualLoop, Link::kVirtualClosure, 100, 100);
01959 }
01960 }
01961 }
01962
01963
01964
01965
01966
01967 int dictionarySize = 0;
01968 int refWordsCount = 0;
01969 int refUniqueWordsCount = 0;
01970 int lcHypothesisReactivated = 0;
01971 float rehearsalValue = uValue(statistics_.data(), Statistics::kMemoryRehearsal_sim(), 0.0f);
01972 int rehearsalMaxId = (int)uValue(statistics_.data(), Statistics::kMemoryRehearsal_merged(), 0.0f);
01973 sLoop = _memory->getSignature(_loopClosureHypothesis.first?_loopClosureHypothesis.first:lastLocalSpaceClosureId?lastLocalSpaceClosureId:_highestHypothesis.first);
01974 if(sLoop)
01975 {
01976 lcHypothesisReactivated = sLoop->isSaved()?1.0f:0.0f;
01977 }
01978 dictionarySize = (int)_memory->getVWDictionary()->getVisualWords().size();
01979 refWordsCount = (int)signature->getWords().size();
01980 refUniqueWordsCount = (int)uUniqueKeys(signature->getWords()).size();
01981
01982
01983 float vpHypothesis = posterior.size()?posterior.at(Memory::kIdVirtual):0.0f;
01984
01985
01986 if(_loopClosureHypothesis.first || _publishStats)
01987 {
01988 ULOGGER_INFO("sending stats...");
01989 statistics_.setRefImageId(signature->id());
01990 if(_loopClosureHypothesis.first != Memory::kIdInvalid)
01991 {
01992 statistics_.setLoopClosureId(_loopClosureHypothesis.first);
01993 ULOGGER_INFO("Loop closure detected! With id=%d", _loopClosureHypothesis.first);
01994 }
01995 if(_publishStats)
01996 {
01997 ULOGGER_INFO("send all stats...");
01998 statistics_.setExtended(1);
01999
02000 statistics_.addStatistic(Statistics::kLoopAccepted_hypothesis_id(), _loopClosureHypothesis.first);
02001 statistics_.addStatistic(Statistics::kLoopHighest_hypothesis_id(), _highestHypothesis.first);
02002 statistics_.addStatistic(Statistics::kLoopHighest_hypothesis_value(), _highestHypothesis.second);
02003 statistics_.addStatistic(Statistics::kLoopHypothesis_reactivated(), lcHypothesisReactivated);
02004 statistics_.addStatistic(Statistics::kLoopVp_hypothesis(), vpHypothesis);
02005 statistics_.addStatistic(Statistics::kLoopReactivateId(), retrievalId);
02006 statistics_.addStatistic(Statistics::kLoopHypothesis_ratio(), hypothesisRatio);
02007 statistics_.addStatistic(Statistics::kLoopVisualInliers(), loopClosureVisualInliers);
02008 statistics_.addStatistic(Statistics::kLoopLast_id(), _memory->getLastGlobalLoopClosureId());
02009
02010 statistics_.addStatistic(Statistics::kLocalLoopTime_closures(), localLoopClosuresInTimeFound);
02011 statistics_.addStatistic(Statistics::kLocalLoopSpace_closures_added_visually(), localSpaceClosuresAddedVisually);
02012 statistics_.addStatistic(Statistics::kLocalLoopSpace_closures_added_icp_only(), localSpaceClosuresAddedByICPOnly);
02013 statistics_.addStatistic(Statistics::kLocalLoopSpace_paths(), localSpacePaths);
02014 statistics_.addStatistic(Statistics::kLocalLoopSpace_last_closure_id(), lastLocalSpaceClosureId);
02015 statistics_.setLocalLoopClosureId(lastLocalSpaceClosureId);
02016 if(_loopClosureHypothesis.first || lastLocalSpaceClosureId)
02017 {
02018 UASSERT(uContains(sLoop->getLinks(), signature->id()));
02019 UINFO("Set loop closure transform = %s", sLoop->getLinks().at(signature->id()).transform().prettyPrint().c_str());
02020 statistics_.setLoopClosureTransform(sLoop->getLinks().at(signature->id()).transform());
02021 }
02022 statistics_.setMapCorrection(_mapCorrection);
02023 UINFO("Set map correction = %s", _mapCorrection.prettyPrint().c_str());
02024
02025
02026 if(!_rgbdSlamMode)
02027 {
02028
02029 std::map<int, int> ids = _memory->getNeighborsId(signature->id(), 0, 0, true);
02030 std::map<int, Transform> poses;
02031 std::map<int, int> mapIds;
02032 std::map<int, std::string> labels;
02033 std::map<int, double> stamps;
02034 std::map<int, std::vector<unsigned char> > userDatas;
02035 std::multimap<int, Link> constraints;
02036 _memory->getMetricConstraints(uKeysSet(ids), poses, constraints, false);
02037 for(std::map<int, Transform>::iterator iter=poses.begin(); iter!=poses.end(); ++iter)
02038 {
02039 Transform odomPose;
02040 int weight = -1;
02041 int mapId = -1;
02042 std::string label;
02043 double stamp = 0;
02044 std::vector<unsigned char> userData;
02045 _memory->getNodeInfo(iter->first, odomPose, mapId, weight, label, stamp, userData, false);
02046 mapIds.insert(std::make_pair(iter->first, mapId));
02047 labels.insert(std::make_pair(iter->first, label));
02048 stamps.insert(std::make_pair(iter->first, stamp));
02049 userDatas.insert(std::make_pair(iter->first, userData));
02050 }
02051 statistics_.setPoses(poses);
02052 statistics_.setConstraints(constraints);
02053 statistics_.setMapIds(mapIds);
02054 statistics_.setLabels(labels);
02055 statistics_.setStamps(stamps);
02056 statistics_.setUserDatas(userDatas);
02057 }
02058 else
02059 {
02060
02061 }
02062
02063
02064 statistics_.addStatistic(Statistics::kTimingMemory_update(), timeMemoryUpdate*1000);
02065 statistics_.addStatistic(Statistics::kTimingScan_matching(), timeScanMatching*1000);
02066 statistics_.addStatistic(Statistics::kTimingLocal_detection_TIME(), timeLocalTimeDetection*1000);
02067 statistics_.addStatistic(Statistics::kTimingLocal_detection_SPACE(), timeLocalSpaceDetection*1000);
02068 statistics_.addStatistic(Statistics::kTimingReactivation(), timeReactivations*1000);
02069 statistics_.addStatistic(Statistics::kTimingAdd_loop_closure_link(), timeAddLoopClosureLink*1000);
02070 statistics_.addStatistic(Statistics::kTimingMap_optimization(), timeMapOptimization*1000);
02071 statistics_.addStatistic(Statistics::kTimingLikelihood_computation(), timeLikelihoodCalculation*1000);
02072 statistics_.addStatistic(Statistics::kTimingPosterior_computation(), timePosteriorCalculation*1000);
02073 statistics_.addStatistic(Statistics::kTimingHypotheses_creation(), timeHypothesesCreation*1000);
02074 statistics_.addStatistic(Statistics::kTimingHypotheses_validation(), timeHypothesesValidation*1000);
02075 statistics_.addStatistic(Statistics::kTimingCleaning_neighbors(), timeCleaningNeighbors*1000);
02076
02077
02078 statistics_.addStatistic(Statistics::kMemorySignatures_retrieved(), (float)signaturesRetrieved.size());
02079
02080
02081 statistics_.addStatistic(Statistics::kKeypointDictionary_size(), dictionarySize);
02082
02083
02084 statistics_.addStatistic(Statistics::kLoopRejectedHypothesis(), rejectedHypothesis?1.0f:0);
02085
02086 if(_publishLastSignature)
02087 {
02088 statistics_.setSignature(*signature);
02089 }
02090
02091 if(_publishLikelihood || _publishPdf)
02092 {
02093
02094 statistics_.setWeights(weights);
02095 if(_publishPdf)
02096 {
02097 statistics_.setPosterior(posterior);
02098 }
02099 if(_publishLikelihood)
02100 {
02101 statistics_.setLikelihood(likelihood);
02102 statistics_.setRawLikelihood(rawLikelihood);
02103 }
02104 }
02105
02106
02107 if(_path.size())
02108 {
02109 statistics_.setLocalPath(this->getPathNextNodes());
02110 statistics_.setCurrentGoalId(this->getPathCurrentGoalId());
02111 }
02112 }
02113
02114 timeStatsCreation = timer.ticks();
02115 ULOGGER_INFO("Time creating stats = %f...", timeStatsCreation);
02116 }
02117
02118
02119
02120 std::list<int> signaturesRemoved = _memory->cleanup();
02121 timeMemoryCleanup = timer.ticks();
02122 ULOGGER_INFO("timeMemoryCleanup = %fs... %d signatures removed", timeMemoryCleanup, (int)signaturesRemoved.size());
02123
02124
02125
02126
02127
02128 if(_startNewMapOnLoopClosure &&
02129 _memory->isIncremental() &&
02130 signature->getLinks().size() == 0 &&
02131 _memory->getWorkingMem().size()>1)
02132 {
02133 UWARN("Ignoring location %d because a global loop closure is required before starting a new map!",
02134 signature->id());
02135 signaturesRemoved.push_back(signature->id());
02136 _memory->deleteLocation(signature->id());
02137 }
02138 else if(smallDisplacement && _loopClosureHypothesis.first == 0 && lastLocalSpaceClosureId == 0)
02139 {
02140
02141 UINFO("Ignoring location %d because the displacement is too small! (d=%f a=%f)",
02142 signature->id(), _rgbdLinearUpdate, _rgbdAngularUpdate);
02143
02144 signaturesRemoved.push_back(signature->id());
02145 _memory->deleteLocation(signature->id());
02146 }
02147
02148
02149 signature = 0;
02150
02151
02152
02153
02154
02155
02156
02157
02158
02159 double totalTime = timerTotal.ticks();
02160 ULOGGER_INFO("Total time processing = %fs...", totalTime);
02161 timer.start();
02162 if((_maxTimeAllowed != 0 && totalTime*1000>_maxTimeAllowed) ||
02163 (_maxMemoryAllowed != 0 && _memory->getWorkingMem().size() > _maxMemoryAllowed))
02164 {
02165 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);
02166 std::list<int> transferred = _memory->forget(immunizedLocations);
02167 signaturesRemoved.insert(signaturesRemoved.end(), transferred.begin(), transferred.end());
02168 }
02169 _lastProcessTime = totalTime;
02170
02171
02172 if(signaturesRemoved.size() && (_optimizedPoses.size() || _constraints.size()))
02173 {
02174
02175 if(_memory->getLastWorkingSignature())
02176 {
02177 std::map<int, int> ids = _memory->getNeighborsId(_memory->getLastWorkingSignature()->id(), 0, 0, true);
02178 for(std::map<int, Transform>::iterator iter=_optimizedPoses.begin(); iter!=_optimizedPoses.end();)
02179 {
02180 if(!uContains(ids, iter->first))
02181 {
02182 _optimizedPoses.erase(iter++);
02183 }
02184 else
02185 {
02186 ++iter;
02187 }
02188 }
02189 for(std::multimap<int, Link>::iterator iter=_constraints.begin(); iter!=_constraints.end();)
02190 {
02191 if(!uContains(ids, iter->second.from()) || !uContains(ids, iter->second.to()))
02192 {
02193 _constraints.erase(iter++);
02194 }
02195 else
02196 {
02197 ++iter;
02198 }
02199 }
02200 }
02201 else
02202 {
02203 _optimizedPoses.clear();
02204 _constraints.clear();
02205 }
02206 }
02207
02208
02209 timeRealTimeLimitReachedProcess = timer.ticks();
02210 ULOGGER_INFO("Time limit reached processing = %f...", timeRealTimeLimitReachedProcess);
02211
02212
02213
02214
02215 if(_publishStats)
02216 {
02217 statistics_.addStatistic(Statistics::kTimingStatistics_creation(), timeStatsCreation*1000);
02218 statistics_.addStatistic(Statistics::kTimingTotal(), totalTime*1000);
02219 statistics_.addStatistic(Statistics::kTimingForgetting(), timeRealTimeLimitReachedProcess*1000);
02220 statistics_.addStatistic(Statistics::kTimingJoining_trash(), timeJoiningTrash*1000);
02221 statistics_.addStatistic(Statistics::kTimingEmptying_trash(), timeEmptyingTrash*1000);
02222 statistics_.addStatistic(Statistics::kTimingMemory_cleanup(), timeMemoryCleanup*1000);
02223
02224
02225 statistics_.addStatistic(Statistics::kMemorySignatures_removed(), signaturesRemoved.size());
02226 statistics_.addStatistic(Statistics::kMemoryImmunized_globally(), immunizedGlobally);
02227 statistics_.addStatistic(Statistics::kMemoryImmunized_locally(), immunizedLocally);
02228 statistics_.addStatistic(Statistics::kMemoryImmunized_locally_max(), maxLocalLocationsImmunized);
02229
02230
02231 statistics_.addStatistic(Statistics::kMemoryWorking_memory_size(), _memory->getWorkingMem().size());
02232 statistics_.addStatistic(Statistics::kMemoryShort_time_memory_size(), _memory->getStMem().size());
02233 statistics_.addStatistic(Statistics::kMemoryLocal_graph_size(), _optimizedPoses.size());
02234
02235 if(_rgbdSlamMode)
02236 {
02237 std::map<int, int> mapIds;
02238 std::map<int, std::string> labels;
02239 std::map<int, double> stamps;
02240 std::map<int, std::vector<unsigned char> > userDatas;
02241 for(std::map<int, Transform>::iterator iter=_optimizedPoses.begin(); iter!=_optimizedPoses.end(); ++iter)
02242 {
02243 Transform odomPose;
02244 int weight = -1;
02245 int mapId = -1;
02246 std::string label;
02247 double stamp = 0;
02248 std::vector<unsigned char> userData;
02249 _memory->getNodeInfo(iter->first, odomPose, mapId, weight, label, stamp, userData, true);
02250 mapIds.insert(std::make_pair(iter->first, mapId));
02251 labels.insert(std::make_pair(iter->first, label));
02252 stamps.insert(std::make_pair(iter->first, stamp));
02253 userDatas.insert(std::make_pair(iter->first, userData));
02254 }
02255 statistics_.setPoses(_optimizedPoses);
02256 statistics_.setConstraints(_constraints);
02257 statistics_.setMapIds(mapIds);
02258 statistics_.setLabels(labels);
02259 statistics_.setStamps(stamps);
02260 statistics_.setUserDatas(userDatas);
02261 }
02262
02263 }
02264
02265
02266 _memory->emptyTrash();
02267
02268
02269
02270 if(_foutFloat && _foutInt)
02271 {
02272 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\n",
02273 totalTime,
02274 timeMemoryUpdate,
02275 timeReactivations,
02276 timeLikelihoodCalculation,
02277 timePosteriorCalculation,
02278 timeHypothesesCreation,
02279 timeHypothesesValidation,
02280 timeRealTimeLimitReachedProcess,
02281 timeStatsCreation,
02282 _highestHypothesis.second,
02283 0.0f,
02284 0.0f,
02285 0.0f,
02286 0.0f,
02287 0.0f,
02288 vpHypothesis,
02289 timeJoiningTrash,
02290 rehearsalValue,
02291 timeEmptyingTrash,
02292 timeRetrievalDbAccess,
02293 timeAddLoopClosureLink);
02294 std::string logI = uFormat("%d %d %d %d %d %d %d %d %d %d %d %d %d %d %d %d %d\n",
02295 _loopClosureHypothesis.first,
02296 _highestHypothesis.first,
02297 (int)signaturesRemoved.size(),
02298 0,
02299 refWordsCount,
02300 dictionarySize,
02301 int(_memory->getWorkingMem().size()),
02302 rejectedHypothesis?1:0,
02303 0,
02304 0,
02305 int(signaturesRetrieved.size()),
02306 lcHypothesisReactivated,
02307 refUniqueWordsCount,
02308 retrievalId,
02309 0.0f,
02310 rehearsalMaxId,
02311 rehearsalMaxId>0?1:0);
02312 if(_statisticLogsBufferedInRAM)
02313 {
02314 _bufferedLogsF.push_back(logF);
02315 _bufferedLogsI.push_back(logI);
02316 }
02317 else
02318 {
02319 if(_foutFloat)
02320 {
02321 fprintf(_foutFloat, "%s", logF.c_str());
02322 }
02323 if(_foutInt)
02324 {
02325 fprintf(_foutInt, "%s", logI.c_str());
02326 }
02327 }
02328 UINFO("Time logging = %f...", timer.ticks());
02329
02330 }
02331 UDEBUG("End process");
02332
02333 return true;
02334 }
02335
02336 bool Rtabmap::process(const cv::Mat & image, int id)
02337 {
02338 return this->process(SensorData(image, id));
02339 }
02340
02341
02342 void Rtabmap::setTimeThreshold(float maxTimeAllowed)
02343 {
02344
02345 _maxTimeAllowed = maxTimeAllowed;
02346 if(_maxTimeAllowed < 0)
02347 {
02348 ULOGGER_WARN("maxTimeAllowed < 0, then setting it to 0 (inf).");
02349 _maxTimeAllowed = 0;
02350 }
02351 else if(_maxTimeAllowed > 0.0f && _maxTimeAllowed < 1.0f)
02352 {
02353 ULOGGER_WARN("Time threshold set to %fms, it is not in seconds!", _maxTimeAllowed);
02354 }
02355 }
02356
02357 void Rtabmap::setWorkingDirectory(std::string path)
02358 {
02359 if(!path.empty() && UDirectory::exists(path))
02360 {
02361 ULOGGER_DEBUG("Comparing new working directory path \"%s\" with \"%s\"", path.c_str(), _wDir.c_str());
02362 if(path.compare(_wDir) != 0)
02363 {
02364 _wDir = path;
02365 if(_memory)
02366 {
02367 this->resetMemory();
02368 }
02369 else
02370 {
02371 setupLogFiles();
02372 }
02373 }
02374 }
02375 else
02376 {
02377 ULOGGER_ERROR("Directory \"%s\" doesn't exist!", path.c_str());
02378 }
02379 }
02380
02381 void Rtabmap::rejectLoopClosure(int oldId, int newId)
02382 {
02383 UDEBUG("_loopClosureHypothesis.first=%d", _loopClosureHypothesis.first);
02384 if(_loopClosureHypothesis.first)
02385 {
02386 _loopClosureHypothesis.first = 0;
02387 if(_memory)
02388 {
02389 _memory->removeLink(oldId, newId);
02390 }
02391 if(uContains(statistics_.data(), rtabmap::Statistics::kLoopRejectedHypothesis()))
02392 {
02393 statistics_.addStatistic(rtabmap::Statistics::kLoopRejectedHypothesis(), 1.0f);
02394 }
02395 statistics_.setLoopClosureId(0);
02396 }
02397 }
02398
02399 void Rtabmap::dumpData() const
02400 {
02401 UDEBUG("");
02402 if(_memory)
02403 {
02404 _memory->dumpMemory(this->getWorkingDir());
02405 }
02406 }
02407
02408
02409
02410 std::map<int, Transform> Rtabmap::getForwardWMPoses(
02411 int fromId,
02412 int maxNearestNeighbors,
02413 float radius,
02414 int maxGraphDepth
02415 ) const
02416 {
02417 std::map<int, Transform> poses;
02418 if(_memory && fromId > 0)
02419 {
02420 UDEBUG("");
02421 const Signature * fromS = _memory->getSignature(fromId);
02422 UASSERT(fromS != 0);
02423 UASSERT(_optimizedPoses.find(fromId) != _optimizedPoses.end());
02424
02425 pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
02426 cloud->resize(_optimizedPoses.size());
02427 std::vector<int> ids(_optimizedPoses.size());
02428 int oi = 0;
02429 const std::set<int> & stm = _memory->getStMem();
02430
02431 std::map<int, float> foundIds;
02432 if(_memory->isIncremental())
02433 {
02434 foundIds = _memory->getNeighborsIdRadius(fromId, radius, _optimizedPoses, maxGraphDepth);
02435 }
02436 else
02437 {
02438 foundIds = graph::getNodesInRadius(fromId, _optimizedPoses, radius);
02439 }
02440
02441 float radiusSqrd = radius * radius;
02442 for(std::map<int, Transform>::const_iterator iter = _optimizedPoses.begin(); iter!=_optimizedPoses.end(); ++iter)
02443 {
02444 if(iter->first != fromId)
02445 {
02446 if(stm.find(iter->first) == stm.end() &&
02447 uContains(foundIds, iter->first) &&
02448 (radiusSqrd==0 || foundIds.at(iter->first) <= radiusSqrd))
02449 {
02450 (*cloud)[oi] = pcl::PointXYZ(iter->second.x(), iter->second.y(), iter->second.z());
02451 ids[oi++] = iter->first;
02452 }
02453 }
02454 }
02455
02456 cloud->resize(oi);
02457 ids.resize(oi);
02458
02459 Transform fromT = _optimizedPoses.at(fromId);
02460
02461 if(cloud->size())
02462 {
02463
02464
02465
02466
02467
02468
02469
02470 float x,y,z, roll,pitch,yaw;
02471 fromT.getTranslationAndEulerAngles(x,y,z, roll,pitch,yaw);
02472
02473 pcl::CropBox<pcl::PointXYZ> cropbox;
02474 cropbox.setInputCloud(cloud);
02475 cropbox.setMin(Eigen::Vector4f(-1, -radius, -999999, 0));
02476 cropbox.setMax(Eigen::Vector4f(radius, radius, 999999, 0));
02477 cropbox.setRotation(Eigen::Vector3f(roll, pitch, yaw));
02478 cropbox.setTranslation(Eigen::Vector3f(x, y, z));
02479 cropbox.setRotation(Eigen::Vector3f(roll,pitch,yaw));
02480 pcl::IndicesPtr indices(new std::vector<int>());
02481 cropbox.filter(*indices);
02482
02483
02484
02485
02486
02487
02488
02489 if(indices->size())
02490 {
02491 pcl::search::KdTree<pcl::PointXYZ>::Ptr kdTree(new pcl::search::KdTree<pcl::PointXYZ>);
02492 kdTree->setInputCloud(cloud, indices);
02493 std::vector<int> ind;
02494 std::vector<float> dist;
02495 pcl::PointXYZ pt(fromT.x(), fromT.y(), fromT.z());
02496 kdTree->radiusSearch(pt, radius, ind, dist, maxNearestNeighbors);
02497
02498 for(unsigned int i=0; i<ind.size(); ++i)
02499 {
02500 if(ind[i] >=0)
02501 {
02502 Transform tmp = _optimizedPoses.find(ids[ind[i]])->second;
02503
02504 UDEBUG("Inlier %d: %s", ids[ind[i]], tmp.prettyPrint().c_str());
02505 poses.insert(std::make_pair(ids[ind[i]], tmp));
02506 }
02507 }
02508
02509
02510
02511
02512
02513
02514
02515
02516
02517
02518
02519
02520 }
02521 }
02522 }
02523 return poses;
02524 }
02525
02526
02527 std::list<std::map<int, Transform> > Rtabmap::getPaths(std::map<int, Transform> poses) const
02528 {
02529 std::list<std::map<int, Transform> > paths;
02530 if(_memory && poses.size())
02531 {
02532
02533 while(poses.size())
02534 {
02535 std::map<int, Transform> path;
02536 for(std::map<int, Transform>::iterator iter=poses.begin(); iter!=poses.end();)
02537 {
02538 if(path.size() == 0 || uContains(_memory->getNeighborLinks(path.rbegin()->first), iter->first))
02539 {
02540 path.insert(*iter);
02541 poses.erase(iter++);
02542 }
02543 else
02544 {
02545 break;
02546 }
02547 }
02548 UASSERT(path.size());
02549 paths.push_back(path);
02550 }
02551
02552 }
02553 return paths;
02554 }
02555
02556 void Rtabmap::optimizeCurrentMap(
02557 int id,
02558 bool lookInDatabase,
02559 std::map<int, Transform> & optimizedPoses,
02560 std::multimap<int, Link> * constraints) const
02561 {
02562
02563 optimizedPoses.clear();
02564 UDEBUG("Optimize map: around location %d", id);
02565 if(_memory && id > 0)
02566 {
02567 UTimer timer;
02568 std::map<int, int> ids = _memory->getNeighborsId(id, 0, lookInDatabase?-1:0, true);
02569 UDEBUG("get ids=%d", (int)ids.size());
02570 if(!_optimizeFromGraphEnd && ids.size() > 1)
02571 {
02572 id = ids.begin()->first;
02573 }
02574 UINFO("get ids time %f s", timer.ticks());
02575
02576 optimizedPoses = Rtabmap::optimizeGraph(id, uKeysSet(ids), lookInDatabase, constraints);
02577
02578 if(_memory->getSignature(id) && uContains(optimizedPoses, id))
02579 {
02580 Transform t = optimizedPoses.at(id) * _memory->getSignature(id)->getPose().inverse();
02581 UINFO("Correction (from node %d) %s", id, t.prettyPrint().c_str());
02582 }
02583 UINFO("optimize time %f s", timer.ticks());
02584 }
02585 }
02586
02587 std::map<int, Transform> Rtabmap::optimizeGraph(
02588 int fromId,
02589 const std::set<int> & ids,
02590 bool lookInDatabase,
02591 std::multimap<int, Link> * constraints) const
02592 {
02593 UTimer timer;
02594 std::map<int, Transform> optimizedPoses;
02595 std::map<int, Transform> poses;
02596 std::multimap<int, Link> edgeConstraints;
02597 UDEBUG("ids=%d", (int)ids.size());
02598 _memory->getMetricConstraints(ids, poses, edgeConstraints, lookInDatabase);
02599 UDEBUG("get constraints (%d poses, %d edges) time %f s", (int)poses.size(), (int)edgeConstraints.size(), timer.ticks());
02600
02601 if(constraints)
02602 {
02603 *constraints = edgeConstraints;
02604 }
02605
02606 UASSERT(_graphOptimizer!=0);
02607 if(_graphOptimizer->iterations() == 0)
02608 {
02609
02610 optimizedPoses = poses;
02611 }
02612 else
02613 {
02614 optimizedPoses = _graphOptimizer->optimize(fromId, poses, edgeConstraints);
02615 }
02616
02617 return optimizedPoses;
02618 }
02619
02620 void Rtabmap::adjustLikelihood(std::map<int, float> & likelihood) const
02621 {
02622 ULOGGER_DEBUG("likelihood.size()=%d", likelihood.size());
02623 UTimer timer;
02624 timer.start();
02625 if(likelihood.size()==0)
02626 {
02627 return;
02628 }
02629
02630
02631 std::list<float> values;
02632 bool likelihoodNullValuesIgnored = true;
02633 for(std::map<int, float>::iterator iter = ++likelihood.begin(); iter!=likelihood.end(); ++iter)
02634 {
02635 if((iter->second >= 0 && !likelihoodNullValuesIgnored) ||
02636 (iter->second > 0 && likelihoodNullValuesIgnored))
02637 {
02638 values.push_back(iter->second);
02639 }
02640 }
02641 UDEBUG("values.size=%d", values.size());
02642
02643 float mean = uMean(values);
02644 float stdDev = std::sqrt(uVariance(values, mean));
02645
02646
02647
02648 float epsilon = 0.0001;
02649 float max = 0.0f;
02650 int maxId = 0;
02651 for(std::map<int, float>::iterator iter=++likelihood.begin(); iter!= likelihood.end(); ++iter)
02652 {
02653 float value = iter->second;
02654 if(value > mean+stdDev && mean)
02655 {
02656 iter->second = (value-(stdDev-epsilon))/mean;
02657 if(value > max)
02658 {
02659 max = value;
02660 maxId = iter->first;
02661 }
02662 }
02663 else if(value == 1.0f && stdDev == 0)
02664 {
02665 iter->second = 1.0f;
02666 if(value > max)
02667 {
02668 max = value;
02669 maxId = iter->first;
02670 }
02671 }
02672 else
02673 {
02674 iter->second = 1.0f;
02675 }
02676 }
02677
02678 if(stdDev > epsilon && max)
02679 {
02680 likelihood.begin()->second = mean/stdDev + 1.0f;
02681 }
02682 else
02683 {
02684 likelihood.begin()->second = 2.0f;
02685 }
02686
02687 double time = timer.ticks();
02688 UDEBUG("mean=%f, stdDev=%f, max=%f, maxId=%d, time=%fs", mean, stdDev, max, maxId, time);
02689 }
02690
02691 void Rtabmap::dumpPrediction() const
02692 {
02693 if(_memory && _bayesFilter)
02694 {
02695 cv::Mat prediction = _bayesFilter->generatePrediction(_memory, uKeys(_memory->getWorkingMem()));
02696
02697 FILE* fout = 0;
02698 std::string fileName = this->getWorkingDir() + "/DumpPrediction.txt";
02699 #ifdef _MSC_VER
02700 fopen_s(&fout, fileName.c_str(), "w");
02701 #else
02702 fout = fopen(fileName.c_str(), "w");
02703 #endif
02704
02705 if(fout)
02706 {
02707 for(int i=0; i<prediction.rows; ++i)
02708 {
02709 for(int j=0; j<prediction.cols; ++j)
02710 {
02711 fprintf(fout, "%f ",((float*)prediction.data)[j + i*prediction.cols]);
02712 }
02713 fprintf(fout, "\n");
02714 }
02715 fclose(fout);
02716 }
02717 }
02718 else
02719 {
02720 UWARN("Memory and/or the Bayes filter are not created");
02721 }
02722 }
02723
02724 void Rtabmap::get3DMap(std::map<int, Signature> & signatures,
02725 std::map<int, Transform> & poses,
02726 std::multimap<int, Link> & constraints,
02727 std::map<int, int> & mapIds,
02728 std::map<int, double> & stamps,
02729 std::map<int, std::string> & labels,
02730 std::map<int, std::vector<unsigned char> > & userDatas,
02731 bool optimized,
02732 bool global) const
02733 {
02734 UDEBUG("");
02735 if(_memory && _memory->getLastWorkingSignature())
02736 {
02737 if(_rgbdSlamMode)
02738 {
02739 if(optimized)
02740 {
02741 this->optimizeCurrentMap(_memory->getLastWorkingSignature()->id(), global, poses, &constraints);
02742 }
02743 else
02744 {
02745 std::map<int, int> ids = _memory->getNeighborsId(_memory->getLastWorkingSignature()->id(), 0, global?-1:0, true);
02746 _memory->getMetricConstraints(uKeysSet(ids), poses, constraints, global);
02747 }
02748 }
02749 else
02750 {
02751
02752 std::map<int, int> ids = _memory->getNeighborsId(_memory->getLastWorkingSignature()->id(), 0, global?-1:0, true);
02753 _memory->getMetricConstraints(uKeysSet(ids), poses, constraints, global);
02754 }
02755
02756 for(std::map<int, Transform>::iterator iter=poses.begin(); iter!=poses.end(); ++iter)
02757 {
02758 Transform odomPose;
02759 int weight = -1;
02760 int mapId = -1;
02761 std::string label;
02762 double stamp = 0;
02763 std::vector<unsigned char> userData;
02764 _memory->getNodeInfo(iter->first, odomPose, mapId, weight, label, stamp, userData, true);
02765 mapIds.insert(std::make_pair(iter->first, mapId));
02766 stamps.insert(std::make_pair(iter->first, stamp));
02767 labels.insert(std::make_pair(iter->first, label));
02768 userDatas.insert(std::make_pair(iter->first, userData));
02769 }
02770
02771
02772
02773 std::set<int> ids = uKeysSet(_memory->getWorkingMem());
02774
02775
02776 ids.erase(Memory::kIdVirtual);
02777
02778 ids.insert(_memory->getStMem().begin(), _memory->getStMem().end());
02779 if(global)
02780 {
02781 ids = _memory->getAllSignatureIds();
02782 }
02783
02784 for(std::set<int>::iterator iter = ids.begin(); iter!=ids.end(); ++iter)
02785 {
02786 Signature data = _memory->getSignatureData(*iter);
02787 if(data.id() != Memory::kIdInvalid)
02788 {
02789 signatures.insert(std::make_pair(*iter, Signature())).first->second = data;
02790 }
02791 }
02792 }
02793 else if(_memory && (_memory->getStMem().size() || _memory->getWorkingMem().size() > 1))
02794 {
02795 UERROR("Last working signature is null!?");
02796 }
02797 else if(_memory == 0)
02798 {
02799 UWARN("Memory not initialized...");
02800 }
02801 }
02802
02803 void Rtabmap::getGraph(
02804 std::map<int, Transform> & poses,
02805 std::multimap<int, Link> & constraints,
02806 std::map<int, int> & mapIds,
02807 std::map<int, double> & stamps,
02808 std::map<int, std::string> & labels,
02809 std::map<int, std::vector<unsigned char> > & userDatas,
02810 bool optimized,
02811 bool global)
02812 {
02813 if(_memory && _memory->getLastWorkingSignature())
02814 {
02815 if(_rgbdSlamMode)
02816 {
02817 if(optimized)
02818 {
02819 this->optimizeCurrentMap(_memory->getLastWorkingSignature()->id(), global, poses, &constraints);
02820 }
02821 else
02822 {
02823 std::map<int, int> ids = _memory->getNeighborsId(_memory->getLastWorkingSignature()->id(), 0, global?-1:0, true);
02824 _memory->getMetricConstraints(uKeysSet(ids), poses, constraints, global);
02825 }
02826 }
02827 else
02828 {
02829
02830 std::map<int, int> ids = _memory->getNeighborsId(_memory->getLastWorkingSignature()->id(), 0, global?-1:0, true);
02831 _memory->getMetricConstraints(uKeysSet(ids), poses, constraints, global);
02832 }
02833
02834 for(std::map<int, Transform>::iterator iter=poses.begin(); iter!=poses.end(); ++iter)
02835 {
02836 Transform odomPose;
02837 int weight = -1;
02838 int mapId = -1;
02839 std::string label;
02840 double stamp = 0;
02841 std::vector<unsigned char> userData;
02842 _memory->getNodeInfo(iter->first, odomPose, mapId, weight, label, stamp, userData, true);
02843 mapIds.insert(std::make_pair(iter->first, mapId));
02844 stamps.insert(std::make_pair(iter->first, stamp));
02845 labels.insert(std::make_pair(iter->first, label));
02846 userDatas.insert(std::make_pair(iter->first, userData));
02847 }
02848 }
02849 else if(_memory && (_memory->getStMem().size() || _memory->getWorkingMem().size()))
02850 {
02851 UERROR("Last working signature is null!?");
02852 }
02853 else if(_memory == 0)
02854 {
02855 UWARN("Memory not initialized...");
02856 }
02857 }
02858
02859 void Rtabmap::clearPath()
02860 {
02861 _path.clear();
02862 _pathCurrentIndex=0;
02863 _pathGoalIndex = 0;
02864 _pathTransformToGoal.setIdentity();
02865 if(_memory)
02866 {
02867 _memory->removeAllVirtualLinks();
02868 }
02869 }
02870
02871 bool Rtabmap::computePath(
02872 int targetNode,
02873 std::map<int, Transform> nodes,
02874 const std::multimap<int, rtabmap::Link> & constraints)
02875 {
02876 if(_memory)
02877 {
02878 int currentNode;
02879 if(_memory->isIncremental())
02880 {
02881 if(!_memory->getLastWorkingSignature())
02882 {
02883 UWARN("Working memory is empty... cannot compute a path");
02884 return false;
02885 }
02886 currentNode = _memory->getLastWorkingSignature()->id();
02887 }
02888 else
02889 {
02890 if(_lastLocalizationPose.isNull() || _optimizedPoses.size() == 0)
02891 {
02892 UWARN("Last localization pose is null... cannot compute a path");
02893 return false;
02894 }
02895 currentNode = graph::findNearestNode(_optimizedPoses, _lastLocalizationPose);
02896 }
02897
02898 if(!uContains(nodes, currentNode))
02899 {
02900 UWARN("Last signature %d not found in the graph! Cannot compute a path", currentNode);
02901 return false;
02902 }
02903
02904 if(!uContains(nodes, targetNode))
02905 {
02906 UWARN("Goal %d not found in the graph! Cannot compute a path", targetNode);
02907 return false;
02908 }
02909
02910
02911 if(_optimizedPoses.size())
02912 {
02913 if(uContains(nodes, currentNode) && uContains(_optimizedPoses, currentNode))
02914 {
02915 Transform t = _optimizedPoses.at(currentNode) * nodes.at(currentNode).inverse();
02916 for(std::map<int, Transform>::iterator iter=nodes.begin(); iter!=nodes.end(); ++iter)
02917 {
02918 iter->second = t * iter->second;
02919 }
02920 }
02921 }
02922
02923 std::multimap<int, int> links;
02924 for(std::multimap<int, rtabmap::Link>::const_iterator iter=constraints.begin(); iter!=constraints.end(); ++iter)
02925 {
02926 links.insert(std::make_pair(iter->first, iter->second.to()));
02927 links.insert(std::make_pair(iter->second.to(), iter->first));
02928 }
02929
02930 if(_planVirtualLinks)
02931 {
02932 std::multimap<int, int> clusters = rtabmap::graph::radiusPosesClustering(nodes, _goalReachedRadius, CV_PI);
02933 for(std::multimap<int, int>::iterator iter=clusters.begin(); iter!=clusters.end(); ++iter)
02934 {
02935 if(graph::findLink(links, iter->first, iter->second) == links.end())
02936 {
02937 links.insert(*iter);
02938 links.insert(std::make_pair(iter->second, iter->first));
02939 }
02940 }
02941 }
02942
02943 UINFO("Computing path from location %d to %d", currentNode, targetNode);
02944 UTimer timer;
02945 _path = uListToVector(rtabmap::graph::computePath(nodes, links, currentNode, targetNode));
02946 UINFO("A* time = %fs", timer.ticks());
02947
02948 if(_path.size() == 0)
02949 {
02950 _path.clear();
02951 UWARN("Cannot compute a path!");
02952 }
02953 else
02954 {
02955 UINFO("Path generated! Size=%d", (int)_path.size());
02956 if(ULogger::level() == ULogger::kInfo)
02957 {
02958 std::stringstream stream;
02959 for(unsigned int i=0; i<_path.size(); ++i)
02960 {
02961 stream << _path[i].first;
02962 if(i+1 < _path.size())
02963 {
02964 stream << " ";
02965 }
02966 }
02967 UINFO("Path = [%s]", stream.str().c_str());
02968 }
02969 if(_goalsSavedInUserData)
02970 {
02971
02972 std::string goalStr = uFormat("GOAL:%d", targetNode);
02973 setUserData(0, uStr2Bytes(goalStr));
02974 }
02975 }
02976
02977 return _path.size()>0;
02978 }
02979 return false;
02980 }
02981
02982
02983 bool Rtabmap::computePath(int targetNode, bool global)
02984 {
02985 this->clearPath();
02986
02987 if(!_rgbdSlamMode)
02988 {
02989 UWARN("A path can only be computed in RGBD-SLAM mode");
02990 return false;
02991 }
02992
02993 UTimer timer;
02994 std::map<int, Transform> nodes;
02995 std::multimap<int, Link> constraints;
02996 std::map<int, int> mapIds;
02997 std::map<int, double> stamps;
02998 std::map<int, std::string> labels;
02999 std::map<int, std::vector<unsigned char> > userDatas;
03000 this->getGraph(nodes, constraints, mapIds, stamps, labels, userDatas, true, global);
03001 UINFO("Time creating graph (global=%s) = %fs", global?"true":"false", timer.ticks());
03002
03003 if(computePath(targetNode, nodes, constraints))
03004 {
03005 updateGoalIndex();
03006 }
03007 UINFO("Time computing path = %fs", timer.ticks());
03008
03009 return _path.size()>0;
03010 }
03011
03012 bool Rtabmap::computePath(const Transform & targetPose, bool global)
03013 {
03014 this->clearPath();
03015 std::list<std::pair<int, Transform> > pathPoses;
03016
03017 if(!_rgbdSlamMode)
03018 {
03019 UWARN("This method can only be used in RGBD-SLAM mode");
03020 return false;
03021 }
03022
03023
03024 UTimer timer;
03025 std::map<int, Transform> nodes;
03026 std::multimap<int, Link> constraints;
03027 std::map<int, int> mapIds;
03028 std::map<int, double> stamps;
03029 std::map<int, std::string> labels;
03030 std::map<int, std::vector<unsigned char> > userDatas;
03031 this->getGraph(nodes, constraints, mapIds, stamps, labels, userDatas, true, global);
03032 UINFO("Time creating graph (global=%s) = %fs", global?"true":"false", timer.ticks());
03033
03034 int nearestId = rtabmap::graph::findNearestNode(nodes, targetPose);
03035 UINFO("Nearest node found=%d ,%fs", nearestId, timer.ticks());
03036 if(nearestId > 0)
03037 {
03038 if(_localRadius != 0.0f && targetPose.getDistance(nodes.at(nearestId)) > _localRadius)
03039 {
03040 UWARN("Cannot plan farther than %f m from the graph! (distance=%f m from node %d)",
03041 _localRadius, targetPose.getDistance(nodes.at(nearestId)), nearestId);
03042 }
03043 else
03044 {
03045 if(computePath(nearestId, nodes, constraints))
03046 {
03047 UASSERT(_path.size() > 0);
03048 UASSERT(uContains(nodes, _path.back().first));
03049 _pathTransformToGoal = nodes.at(_path.back().first).inverse() * targetPose;
03050
03051 updateGoalIndex();
03052 }
03053 UINFO("Time computing path = %fs", timer.ticks());
03054 }
03055 }
03056 else
03057 {
03058 UWARN("Nearest node not found in graph (size=%d) for pose %s", (int)nodes.size(), targetPose.prettyPrint().c_str());
03059 }
03060
03061 return _path.size()>0;
03062 }
03063
03064 std::vector<std::pair<int, Transform> > Rtabmap::getPathNextPoses() const
03065 {
03066 std::vector<std::pair<int, Transform> > poses;
03067 if(_path.size())
03068 {
03069 UASSERT(_pathCurrentIndex < _path.size() && _pathGoalIndex < _path.size());
03070 poses.resize(_pathGoalIndex-_pathCurrentIndex+1);
03071 int oi=0;
03072 for(unsigned int i=_pathCurrentIndex; i<=_pathGoalIndex; ++i)
03073 {
03074 std::map<int, Transform>::const_iterator iter = _optimizedPoses.find(_path[i].first);
03075 if(iter != _optimizedPoses.end())
03076 {
03077 poses[oi++] = *iter;
03078 }
03079 else
03080 {
03081 break;
03082 }
03083 }
03084 poses.resize(oi);
03085 }
03086 return poses;
03087 }
03088
03089 std::vector<int> Rtabmap::getPathNextNodes() const
03090 {
03091 std::vector<int> ids;
03092 if(_path.size())
03093 {
03094 UASSERT(_pathCurrentIndex < _path.size() && _pathGoalIndex < _path.size());
03095 ids.resize(_pathGoalIndex-_pathCurrentIndex+1);
03096 int oi = 0;
03097 for(unsigned int i=_pathCurrentIndex; i<=_pathGoalIndex; ++i)
03098 {
03099 std::map<int, Transform>::const_iterator iter = _optimizedPoses.find(_path[i].first);
03100 if(iter != _optimizedPoses.end())
03101 {
03102 ids[oi++] = iter->first;
03103 }
03104 else
03105 {
03106 break;
03107 }
03108 }
03109 ids.resize(oi);
03110 }
03111 return ids;
03112 }
03113
03114 int Rtabmap::getPathCurrentGoalId() const
03115 {
03116 if(_path.size())
03117 {
03118 UASSERT(_pathGoalIndex <= _path.size());
03119 return _path[_pathGoalIndex].first;
03120 }
03121 return 0;
03122 }
03123
03124 void Rtabmap::updateGoalIndex()
03125 {
03126 if(!_rgbdSlamMode)
03127 {
03128 UWARN("This method can on be used in RGBD-SLAM mode!");
03129 return;
03130 }
03131
03132 if( _memory && _path.size())
03133 {
03134
03135 float distanceSoFar = 0.0f;
03136 for(unsigned int i=_pathCurrentIndex;
03137 i<_path.size();
03138 ++i)
03139 {
03140 if(i>0)
03141 {
03142 if(_localRadius > 0.0f)
03143 {
03144 distanceSoFar += _path[i-1].second.getDistance(_path[i].second);
03145 }
03146 if(distanceSoFar <= _localRadius)
03147 {
03148 const Signature * s = _memory->getSignature(_path[i].first);
03149 if(s)
03150 {
03151 if(!s->hasLink(_path[i-1].first) && _memory->getSignature(_path[i-1].first) != 0)
03152 {
03153 Transform virtualLoop = _path[i].second.inverse() * _path[i-1].second;
03154 _memory->addLink(_path[i-1].first, _path[i].first, virtualLoop, Link::kVirtualClosure, 1, 1);
03155 UINFO("Added Virtual link between %d and %d", _path[i-1].first, _path[i].first);
03156 }
03157 }
03158 }
03159 else
03160 {
03161 break;
03162 }
03163 }
03164 }
03165
03166 UDEBUG("current node = %d current goal = %d", _path[_pathCurrentIndex].first, _path[_pathGoalIndex].first);
03167 Transform currentPose;
03168 if(_memory->isIncremental())
03169 {
03170 if(_memory->getLastWorkingSignature() == 0 ||
03171 !uContains(_optimizedPoses, _memory->getLastWorkingSignature()->id()))
03172 {
03173 UERROR("Last node is null in memory or not in optimized poses. Aborting the plan...");
03174 this->clearPath();
03175 return;
03176 }
03177 currentPose = _optimizedPoses.at(_memory->getLastWorkingSignature()->id());
03178 }
03179 else
03180 {
03181 if(_lastLocalizationPose.isNull())
03182 {
03183 UERROR("Last localization pose is null. Aborting the plan...");
03184 this->clearPath();
03185 return;
03186 }
03187 currentPose = _lastLocalizationPose;
03188 }
03189
03190 int goalId = _path.back().first;
03191 if(uContains(_optimizedPoses, goalId))
03192 {
03193
03194 float d = currentPose.getDistance(_optimizedPoses.at(goalId)*_pathTransformToGoal);
03195 if(d < _goalReachedRadius)
03196 {
03197 UINFO("Goal %d reached!", goalId);
03198 this->clearPath();
03199 }
03200 }
03201
03202 if(_path.size())
03203 {
03204
03205 int goalIndex = _pathCurrentIndex;
03206 float distanceFromCurrentNode = 0.0f;
03207 for(unsigned int i=_pathCurrentIndex; i<_path.size(); ++i)
03208 {
03209 if(uContains(_optimizedPoses, _path[i].first))
03210 {
03211 if(_localRadius > 0.0f)
03212 {
03213 distanceFromCurrentNode = currentPose.getDistance(_optimizedPoses.at(_path[i].first));
03214 }
03215
03216 if(distanceFromCurrentNode <= _localRadius)
03217 {
03218 goalIndex = i;
03219 }
03220 else
03221 {
03222 break;
03223 }
03224 }
03225 else
03226 {
03227 break;
03228 }
03229 }
03230 UASSERT(_pathGoalIndex < _path.size() && goalIndex >= 0 && goalIndex < (int)_path.size());
03231 if((int)_pathGoalIndex != goalIndex)
03232 {
03233 UINFO("Updated current goal from %d to %d (%d/%d)",
03234 (int)_path[_pathGoalIndex].first, _path[goalIndex].first, goalIndex+1, (int)_path.size());
03235 _pathGoalIndex = goalIndex;
03236 }
03237
03238
03239 unsigned int nearestNodeIndex = 0;
03240 float distance = -1.0f;
03241 UASSERT(_pathGoalIndex < _path.size() && _pathGoalIndex >= 0);
03242 for(unsigned int i=_pathCurrentIndex; i<=_pathGoalIndex; ++i)
03243 {
03244 std::map<int, Transform>::iterator iter = _optimizedPoses.find(_path[i].first);
03245 if(iter != _optimizedPoses.end())
03246 {
03247 float d = currentPose.getDistanceSquared(iter->second);
03248 if(distance == -1.0f || distance > d)
03249 {
03250 distance = d;
03251 nearestNodeIndex = i;
03252 }
03253 }
03254 }
03255 if(distance < 0)
03256 {
03257 UERROR("The nearest pose on the path not found!");
03258 }
03259 else
03260 {
03261 UDEBUG("Nearest node = %d", _path[nearestNodeIndex].first);
03262 }
03263 if(distance >= 0 && nearestNodeIndex != _pathCurrentIndex)
03264 {
03265 _pathCurrentIndex = nearestNodeIndex;
03266 }
03267 }
03268 }
03269 }
03270
03271 void Rtabmap::readParameters(const std::string & configFile, ParametersMap & parameters)
03272 {
03273 CSimpleIniA ini;
03274 ini.LoadFile(configFile.c_str());
03275 const CSimpleIniA::TKeyVal * keyValMap = ini.GetSection("Core");
03276 if(keyValMap)
03277 {
03278 for(CSimpleIniA::TKeyVal::const_iterator iter=keyValMap->begin(); iter!=keyValMap->end(); ++iter)
03279 {
03280 std::string key = (*iter).first.pItem;
03281 if(key.compare("Version") == 0)
03282 {
03283
03284 std::vector<std::string> version = uListToVector(uSplit((*iter).second, '.'));
03285 if(version.size() == 3)
03286 {
03287 if(!RTABMAP_VERSION_COMPARE(std::atoi(version[0].c_str()), std::atoi(version[1].c_str()), std::atoi(version[2].c_str())))
03288 {
03289 if(configFile.find(".rtabmap") != std::string::npos)
03290 {
03291 UWARN("Version in the config file \"%s\" is more recent (\"%s\") than "
03292 "current RTAB-Map version used (\"%s\"). The config file will be upgraded "
03293 "to new version.",
03294 configFile.c_str(),
03295 (*iter).second,
03296 RTABMAP_VERSION);
03297 }
03298 else
03299 {
03300 UERROR("Version in the config file \"%s\" is more recent (\"%s\") than "
03301 "current RTAB-Map version used (\"%s\"). New parameters (if there are some) will "
03302 "be ignored.",
03303 configFile.c_str(),
03304 (*iter).second,
03305 RTABMAP_VERSION);
03306 }
03307 }
03308 }
03309 }
03310 else
03311 {
03312 key = uReplaceChar(key, '\\', '/');
03313 ParametersMap::iterator jter = parameters.find(key);
03314 if(jter != parameters.end())
03315 {
03316 parameters.erase(jter);
03317 }
03318 parameters.insert(ParametersPair(key, (*iter).second));
03319 }
03320 }
03321 }
03322 else
03323 {
03324 ULOGGER_WARN("Section \"Core\" in %s doesn't exist... "
03325 "Ignore this warning if the ini file does not exist yet. "
03326 "The ini file will be automatically created when this node will close.", configFile.c_str());
03327 }
03328 }
03329
03330 void Rtabmap::writeParameters(const std::string & configFile, const ParametersMap & parameters)
03331 {
03332 CSimpleIniA ini;
03333 ini.LoadFile(configFile.c_str());
03334
03335
03336 ini.SetValue("Core", "Version", RTABMAP_VERSION, NULL, true);
03337
03338 for(ParametersMap::const_iterator i=parameters.begin(); i!=parameters.end(); ++i)
03339 {
03340 std::string key = (*i).first;
03341 key = uReplaceChar(key, '/', '\\');
03342 ini.SetValue("Core", key.c_str(), (*i).second.c_str(), NULL, true);
03343 }
03344
03345 ini.SaveFile(configFile.c_str());
03346 }
03347
03348 }