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