Rtabmap.cpp
Go to the documentation of this file.
1 /*
2 Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
3 All rights reserved.
4 
5 Redistribution and use in source and binary forms, with or without
6 modification, are permitted provided that the following conditions are met:
7  * Redistributions of source code must retain the above copyright
8  notice, this list of conditions and the following disclaimer.
9  * Redistributions in binary form must reproduce the above copyright
10  notice, this list of conditions and the following disclaimer in the
11  documentation and/or other materials provided with the distribution.
12  * Neither the name of the Universite de Sherbrooke nor the
13  names of its contributors may be used to endorse or promote products
14  derived from this software without specific prior written permission.
15 
16 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
17 ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
18 WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
19 DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
20 DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
21 (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
22 LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
23 ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
24 (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
25 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
26 */
27 
28 #include "rtabmap/core/Rtabmap.h"
29 #include "rtabmap/core/Version.h"
30 #include "rtabmap/core/Features2d.h"
31 #include "rtabmap/core/Optimizer.h"
32 #include "rtabmap/core/Graph.h"
33 #include "rtabmap/core/Signature.h"
34 
35 #include "rtabmap/core/EpipolarGeometry.h"
36 
37 #include "rtabmap/core/util3d.h"
38 #include "rtabmap/core/util3d_transforms.h"
39 #include "rtabmap/core/util3d_filtering.h"
40 #include "rtabmap/core/util3d_surface.h"
41 
42 #include "rtabmap/core/DBDriver.h"
43 #include "rtabmap/core/Memory.h"
44 #include "rtabmap/core/VWDictionary.h"
45 #include "rtabmap/core/BayesFilter.h"
46 #include "rtabmap/core/Compression.h"
47 #include "rtabmap/core/RegistrationInfo.h"
48 
49 #include <rtabmap/utilite/ULogger.h>
50 #include <rtabmap/utilite/UFile.h>
51 #include <rtabmap/utilite/UTimer.h>
52 #include <rtabmap/utilite/UConversion.h>
53 #include <rtabmap/utilite/UMath.h>
54 #include <rtabmap/utilite/UProcessInfo.h>
55 
56 #ifdef RTABMAP_PYTHON
57 #include "rtabmap/core/PythonInterface.h"
58 #endif
59 
60 #include <pcl/search/kdtree.h>
61 #include <pcl/filters/crop_box.h>
62 #include <pcl/io/pcd_io.h>
63 #include <pcl/common/common.h>
64 #include <pcl/TextureMesh.h>
65 
66 #include <stdlib.h>
67 #include <set>
68 
69 #define LOG_F "LogF.txt"
70 #define LOG_I "LogI.txt"
71 
72 #define GRAPH_FILE_NAME "Graph.dot"
73 
74 
75 //
76 //
77 //
78 // =======================================================
79 // MAIN LOOP, see method "void Rtabmap::process();" below.
80 // =======================================================
81 //
82 //
83 //
84 
85 namespace rtabmap
86 {
87 
88 Rtabmap::Rtabmap() :
89  _publishStats(Parameters::defaultRtabmapPublishStats()),
90  _publishLastSignatureData(Parameters::defaultRtabmapPublishLastSignature()),
91  _publishPdf(Parameters::defaultRtabmapPublishPdf()),
92  _publishLikelihood(Parameters::defaultRtabmapPublishLikelihood()),
93  _publishRAMUsage(Parameters::defaultRtabmapPublishRAMUsage()),
94  _computeRMSE(Parameters::defaultRtabmapComputeRMSE()),
95  _saveWMState(Parameters::defaultRtabmapSaveWMState()),
96  _maxTimeAllowed(Parameters::defaultRtabmapTimeThr()), // 700 ms
97  _maxMemoryAllowed(Parameters::defaultRtabmapMemoryThr()), // 0=inf
98  _loopThr(Parameters::defaultRtabmapLoopThr()),
99  _loopRatio(Parameters::defaultRtabmapLoopRatio()),
100  _maxLoopClosureDistance(Parameters::defaultRGBDMaxLoopClosureDistance()),
101  _verifyLoopClosureHypothesis(Parameters::defaultVhEpEnabled()),
102  _maxRetrieved(Parameters::defaultRtabmapMaxRetrieved()),
103  _maxLocalRetrieved(Parameters::defaultRGBDMaxLocalRetrieved()),
104  _maxRepublished(Parameters::defaultRtabmapMaxRepublished()),
105  _rawDataKept(Parameters::defaultMemImageKept()),
106  _statisticLogsBufferedInRAM(Parameters::defaultRtabmapStatisticLogsBufferedInRAM()),
107  _statisticLogged(Parameters::defaultRtabmapStatisticLogged()),
108  _statisticLoggedHeaders(Parameters::defaultRtabmapStatisticLoggedHeaders()),
109  _rgbdSlamMode(Parameters::defaultRGBDEnabled()),
110  _rgbdLinearUpdate(Parameters::defaultRGBDLinearUpdate()),
111  _rgbdAngularUpdate(Parameters::defaultRGBDAngularUpdate()),
112  _rgbdLinearSpeedUpdate(Parameters::defaultRGBDLinearSpeedUpdate()),
113  _rgbdAngularSpeedUpdate(Parameters::defaultRGBDAngularSpeedUpdate()),
114  _newMapOdomChangeDistance(Parameters::defaultRGBDNewMapOdomChangeDistance()),
115  _neighborLinkRefining(Parameters::defaultRGBDNeighborLinkRefining()),
116  _proximityByTime(Parameters::defaultRGBDProximityByTime()),
117  _proximityBySpace(Parameters::defaultRGBDProximityBySpace()),
118  _scanMatchingIdsSavedInLinks(Parameters::defaultRGBDScanMatchingIdsSavedInLinks()),
119  _loopClosureIdentityGuess(Parameters::defaultRGBDLoopClosureIdentityGuess()),
120  _localRadius(Parameters::defaultRGBDLocalRadius()),
121  _localImmunizationRatio(Parameters::defaultRGBDLocalImmunizationRatio()),
122  _proximityMaxGraphDepth(Parameters::defaultRGBDProximityMaxGraphDepth()),
123  _proximityMaxPaths(Parameters::defaultRGBDProximityMaxPaths()),
124  _proximityMaxNeighbors(Parameters::defaultRGBDProximityPathMaxNeighbors()),
125  _proximityFilteringRadius(Parameters::defaultRGBDProximityPathFilteringRadius()),
126  _proximityRawPosesUsed(Parameters::defaultRGBDProximityPathRawPosesUsed()),
127  _proximityAngle(Parameters::defaultRGBDProximityAngle()*M_PI/180.0f),
128  _proximityOdomGuess(Parameters::defaultRGBDProximityOdomGuess()),
129  _proximityMergedScanCovFactor(Parameters::defaultRGBDProximityMergedScanCovFactor()),
130  _databasePath(""),
131  _optimizeFromGraphEnd(Parameters::defaultRGBDOptimizeFromGraphEnd()),
132  _optimizationMaxError(Parameters::defaultRGBDOptimizeMaxError()),
133  _startNewMapOnLoopClosure(Parameters::defaultRtabmapStartNewMapOnLoopClosure()),
134  _startNewMapOnGoodSignature(Parameters::defaultRtabmapStartNewMapOnGoodSignature()),
135  _goalReachedRadius(Parameters::defaultRGBDGoalReachedRadius()),
136  _goalsSavedInUserData(Parameters::defaultRGBDGoalsSavedInUserData()),
137  _pathStuckIterations(Parameters::defaultRGBDPlanStuckIterations()),
138  _pathLinearVelocity(Parameters::defaultRGBDPlanLinearVelocity()),
139  _pathAngularVelocity(Parameters::defaultRGBDPlanAngularVelocity()),
140  _restartAtOrigin(Parameters::defaultRGBDStartAtOrigin()),
141  _loopCovLimited(Parameters::defaultRGBDLoopCovLimited()),
142  _loopGPS(Parameters::defaultRtabmapLoopGPS()),
143  _maxOdomCacheSize(Parameters::defaultRGBDMaxOdomCacheSize()),
144  _createGlobalScanMap(Parameters::defaultRGBDProximityGlobalScanMap()),
145  _markerPriorsLinearVariance(Parameters::defaultMarkerPriorsVarianceLinear()),
146  _markerPriorsAngularVariance(Parameters::defaultMarkerPriorsVarianceAngular()),
147  _loopClosureHypothesis(0,0.0f),
148  _highestHypothesis(0,0.0f),
149  _lastProcessTime(0.0),
150  _someNodesHaveBeenTransferred(false),
151  _distanceTravelled(0.0f),
152  _distanceTravelledSinceLastLocalization(0.0f),
153  _optimizeFromGraphEndChanged(false),
154  _epipolarGeometry(0),
155  _bayesFilter(0),
156  _graphOptimizer(0),
157  _memory(0),
158  _foutFloat(0),
159  _foutInt(0),
160  _wDir(""),
161  _mapCorrection(Transform::getIdentity()),
162  _lastLocalizationNodeId(0),
163  _currentSessionHasGPS(false),
164  _odomCorrectionAcc(6,0),
165  _pathStatus(0),
166  _pathCurrentIndex(0),
167  _pathGoalIndex(0),
168  _pathTransformToGoal(Transform::getIdentity()),
169  _pathStuckCount(0),
170  _pathStuckDistance(0.0f)
171 #ifdef RTABMAP_PYTHON
172  ,_python(new PythonInterface())
173 #endif
174 {
175 }
176 
177 Rtabmap::~Rtabmap() {
178  UDEBUG("");
179  this->close();
180 }
181 
182 void Rtabmap::setupLogFiles(bool overwrite)
183 {
184  flushStatisticLogs();
185  // Log files
186  if(_foutFloat)
187  {
188  fclose(_foutFloat);
189  _foutFloat = 0;
190  }
191  if(_foutInt)
192  {
193  fclose(_foutInt);
194  _foutInt = 0;
195  }
196 
197  if(_statisticLogged && !_wDir.empty())
198  {
199  std::string attributes = "a+"; // append to log files
200  if(overwrite)
201  {
202  // If a file with the same name already exists
203  // its content is erased and the file is treated
204  // as a new empty file.
205  attributes = "w";
206  }
207 
208  bool addLogFHeader = overwrite || !UFile::exists(_wDir+"/"+LOG_F);
209  bool addLogIHeader = overwrite || !UFile::exists(_wDir+"/"+LOG_I);
210 
211  #ifdef _MSC_VER
212  fopen_s(&_foutFloat, (_wDir+"/"+LOG_F).c_str(), attributes.c_str());
213  fopen_s(&_foutInt, (_wDir+"/"+LOG_I).c_str(), attributes.c_str());
214  #else
215  _foutFloat = fopen((_wDir+"/"+LOG_F).c_str(), attributes.c_str());
216  _foutInt = fopen((_wDir+"/"+LOG_I).c_str(), attributes.c_str());
217  #endif
218  // add header (column identification)
219  if(_statisticLoggedHeaders && addLogFHeader && _foutFloat)
220  {
221  fprintf(_foutFloat, "Column headers:\n");
222  fprintf(_foutFloat, " 1-Total iteration time (s)\n");
223  fprintf(_foutFloat, " 2-Memory update time (s)\n");
224  fprintf(_foutFloat, " 3-Retrieval time (s)\n");
225  fprintf(_foutFloat, " 4-Likelihood time (s)\n");
226  fprintf(_foutFloat, " 5-Posterior time (s)\n");
227  fprintf(_foutFloat, " 6-Hypothesis selection time (s)\n");
228  fprintf(_foutFloat, " 7-Hypothesis validation time (s)\n");
229  fprintf(_foutFloat, " 8-Transfer time (s)\n");
230  fprintf(_foutFloat, " 9-Statistics creation time (s)\n");
231  fprintf(_foutFloat, " 10-Loop closure hypothesis value\n");
232  fprintf(_foutFloat, " 11-NAN\n");
233  fprintf(_foutFloat, " 12-NAN\n");
234  fprintf(_foutFloat, " 13-NAN\n");
235  fprintf(_foutFloat, " 14-NAN\n");
236  fprintf(_foutFloat, " 15-NAN\n");
237  fprintf(_foutFloat, " 16-Virtual place hypothesis\n");
238  fprintf(_foutFloat, " 17-Join trash time (s)\n");
239  fprintf(_foutFloat, " 18-Weight Update (rehearsal) similarity\n");
240  fprintf(_foutFloat, " 19-Empty trash time (s)\n");
241  fprintf(_foutFloat, " 20-Retrieval database access time (s)\n");
242  fprintf(_foutFloat, " 21-Add loop closure link time (s)\n");
243  fprintf(_foutFloat, " 22-Memory cleanup time (s)\n");
244  fprintf(_foutFloat, " 23-Scan matching (odometry correction) time (s)\n");
245  fprintf(_foutFloat, " 24-Local time loop closure detection time (s)\n");
246  fprintf(_foutFloat, " 25-Local space loop closure detection time (s)\n");
247  fprintf(_foutFloat, " 26-Map optimization (s)\n");
248  }
249  if(_statisticLoggedHeaders && addLogIHeader && _foutInt)
250  {
251  fprintf(_foutInt, "Column headers:\n");
252  fprintf(_foutInt, " 1-Loop closure ID\n");
253  fprintf(_foutInt, " 2-Highest loop closure hypothesis\n");
254  fprintf(_foutInt, " 3-Locations transferred\n");
255  fprintf(_foutInt, " 4-NAN\n");
256  fprintf(_foutInt, " 5-Words extracted from the last image\n");
257  fprintf(_foutInt, " 6-Vocabulary size\n");
258  fprintf(_foutInt, " 7-Working memory size\n");
259  fprintf(_foutInt, " 8-Is loop closure hypothesis rejected?\n");
260  fprintf(_foutInt, " 9-NAN\n");
261  fprintf(_foutInt, " 10-NAN\n");
262  fprintf(_foutInt, " 11-Locations retrieved\n");
263  fprintf(_foutInt, " 12-Retrieval location ID\n");
264  fprintf(_foutInt, " 13-Unique words extraced from last image\n");
265  fprintf(_foutInt, " 14-Retrieval ID\n");
266  fprintf(_foutInt, " 15-Non-null likelihood values\n");
267  fprintf(_foutInt, " 16-Weight Update ID\n");
268  fprintf(_foutInt, " 17-Is last location merged through Weight Update?\n");
269  fprintf(_foutInt, " 18-Local graph size\n");
270  fprintf(_foutInt, " 19-Sensor data id\n");
271  fprintf(_foutInt, " 20-Indexed words\n");
272  fprintf(_foutInt, " 21-Index memory usage (KB)\n");
273  }
274 
275  ULOGGER_DEBUG("Log file (int)=%s", (_wDir+"/"+LOG_I).c_str());
276  ULOGGER_DEBUG("Log file (float)=%s", (_wDir+"/"+LOG_F).c_str());
277  }
278  else
279  {
280  if(_statisticLogged)
281  {
282  UWARN("Working directory is not set, log disabled!");
283  }
284  UDEBUG("Log disabled!");
285  }
286 }
287 
288 void Rtabmap::flushStatisticLogs()
289 {
290  if(_foutFloat && _bufferedLogsF.size())
291  {
292  UDEBUG("_bufferedLogsF.size=%d", _bufferedLogsF.size());
293  for(std::list<std::string>::iterator iter = _bufferedLogsF.begin(); iter!=_bufferedLogsF.end(); ++iter)
294  {
295  fprintf(_foutFloat, "%s", iter->c_str());
296  }
297  _bufferedLogsF.clear();
298  }
299  if(_foutInt && _bufferedLogsI.size())
300  {
301  UDEBUG("_bufferedLogsI.size=%d", _bufferedLogsI.size());
302  for(std::list<std::string>::iterator iter = _bufferedLogsI.begin(); iter!=_bufferedLogsI.end(); ++iter)
303  {
304  fprintf(_foutInt, "%s", iter->c_str());
305  }
306  _bufferedLogsI.clear();
307  }
308 }
309 
310 void Rtabmap::init(const ParametersMap & parameters, const std::string & databasePath, bool loadDatabaseParameters)
311 {
312  UDEBUG("path=%s", databasePath.c_str());
313  _databasePath = databasePath;
314  if(!_databasePath.empty())
315  {
316  UASSERT(UFile::getExtension(_databasePath).compare("db") == 0);
317  UINFO("Using database \"%s\".", _databasePath.c_str());
318  }
319  else
320  {
321  UWARN("Using empty database. Mapping session will not be saved unless it is closed with an output database path.");
322  }
323 
324  bool newDatabase = _databasePath.empty() || !UFile::exists(_databasePath);
325 
326  ParametersMap allParameters;
327  if(!newDatabase && loadDatabaseParameters)
328  {
329  DBDriver * driver = DBDriver::create();
330  if(driver->openConnection(_databasePath, false))
331  {
332  allParameters = driver->getLastParameters();
333  // ignore working directory (we may be on a different computer)
334  allParameters.erase(Parameters::kRtabmapWorkingDirectory());
335  }
336  delete driver;
337  }
338 
339  uInsert(allParameters, parameters);
340  ParametersMap::const_iterator iter;
341  if((iter=allParameters.find(Parameters::kRtabmapWorkingDirectory())) != allParameters.end())
342  {
343  this->setWorkingDirectory(iter->second.c_str());
344  }
345 
346  // If doesn't exist, create a memory
347  if(!_memory)
348  {
349  _memory = new Memory(allParameters);
350  _memory->init(_databasePath, false, allParameters, true);
351  }
352 
353  _optimizedPoses.clear();
354  _constraints.clear();
355  _globalScanMap.clear();
356  _globalScanMapPoses.clear();
357  _odomCachePoses.clear();
358  _odomCacheConstraints.clear();
359  _nodesToRepublish.clear();
360 
361  // Parse all parameters
362  this->parseParameters(allParameters);
363 
364  Transform lastPose;
365  _optimizedPoses = _memory->loadOptimizedPoses(&lastPose);
366  if(!_memory->isIncremental())
367  {
368  if(_optimizedPoses.empty() &&
369  _memory->getWorkingMem().size()>1 &&
370  _memory->getWorkingMem().lower_bound(1)!=_memory->getWorkingMem().end())
371  {
372  cv::Mat cov;
373  this->optimizeCurrentMap(
374  !_optimizeFromGraphEnd?_memory->getWorkingMem().lower_bound(1)->first:_memory->getWorkingMem().rbegin()->first,
375  false, _optimizedPoses, cov, &_constraints);
376  }
377  if(!_optimizedPoses.empty())
378  {
379  if(_restartAtOrigin)
380  {
381  UWARN("last localization pose is ignored (%s=true), assuming we start at the origin of the map.", Parameters::kRGBDStartAtOrigin().c_str());
382  lastPose = _optimizedPoses.begin()->second;
383  }
384  _lastLocalizationPose = lastPose;
385 
386  UINFO("Loaded optimizedPoses=%d firstPose %d=%s lastLocalizationPose=%s",
387  _optimizedPoses.size(),
388  _optimizedPoses.begin()->first,
389  _optimizedPoses.begin()->second.prettyPrint().c_str(),
390  _lastLocalizationPose.prettyPrint().c_str());
391 
392  if(_constraints.empty())
393  {
394  std::map<int, Transform> tmp;
395  // Get just the links
396  _memory->getMetricConstraints(uKeysSet(_optimizedPoses), tmp, _constraints, false, true);
397  }
398 
399  // Initialize Bayes' prediction matrix
400  UTimer time;
401  std::map<int, float> likelihood;
402  likelihood.insert(std::make_pair(Memory::kIdVirtual, 1));
403  for(std::map<int, Transform>::iterator iter=_optimizedPoses.begin(); iter!=_optimizedPoses.end(); ++iter)
404  {
405  if(_memory->getSignature(iter->first))
406  {
407  likelihood.insert(std::make_pair(iter->first, 0));
408  }
409  }
410  _bayesFilter->computePosterior(_memory, likelihood);
411  UINFO("Time initializing Bayes' prediction with %ld nodes: %fs", _optimizedPoses.size(), time.ticks());
412 
413  if(_createGlobalScanMap)
414  createGlobalScanMap();
415  }
416  else
417  {
418  UINFO("Loaded optimizedPoses=0, last localization pose is ignored!");
419  }
420  }
421  else
422  {
423  _lastLocalizationPose = lastPose;
424  if(!_optimizedPoses.empty())
425  {
426  std::map<int, Transform> tmp;
427  // Get just the links
428  _memory->getMetricConstraints(uKeysSet(_optimizedPoses), tmp, _constraints, false, true);
429  }
430  }
431 
432  if(_databasePath.empty())
433  {
434  _statisticLogged = false;
435  }
436  setupLogFiles(newDatabase);
437 }
438 
439 void Rtabmap::init(const std::string & configFile, const std::string & databasePath, bool loadDatabaseParameters)
440 {
441  // fill ctrl struct with values from the configuration file
442  ParametersMap param;// = Parameters::defaultParameters;
443 
444  if(!configFile.empty())
445  {
446  ULOGGER_DEBUG("Read parameters from = %s", configFile.c_str());
447  Parameters::readINI(configFile, param);
448  }
449 
450  this->init(param, databasePath, loadDatabaseParameters);
451 }
452 
453 void Rtabmap::close(bool databaseSaved, const std::string & ouputDatabasePath)
454 {
455  UINFO("databaseSaved=%d", databaseSaved?1:0);
456  _highestHypothesis = std::make_pair(0,0.0f);
457  _loopClosureHypothesis = std::make_pair(0,0.0f);
458  _lastProcessTime = 0.0;
459  _someNodesHaveBeenTransferred = false;
460  _constraints.clear();
461  _mapCorrection.setIdentity();
462  _mapCorrectionBackup.setNull();
463 
464  _lastLocalizationNodeId = 0;
465  _odomCachePoses.clear();
466  _odomCacheConstraints.clear();
467  _odomCorrectionAcc = std::vector<float>(6,0);
468  _distanceTravelled = 0.0f;
469  _distanceTravelledSinceLastLocalization = 0.0f;
470  _optimizeFromGraphEndChanged = false;
471  this->clearPath(0);
472  _gpsGeocentricCache.clear();
473  _currentSessionHasGPS = false;
474 
475  _globalScanMap.clear();
476  _globalScanMapPoses.clear();
477 
478  _nodesToRepublish.clear();
479 
480  flushStatisticLogs();
481  if(_foutFloat)
482  {
483  fclose(_foutFloat);
484  _foutFloat = 0;
485  }
486  if(_foutInt)
487  {
488  fclose(_foutInt);
489  _foutInt = 0;
490  }
491 
492  if(_epipolarGeometry)
493  {
494  delete _epipolarGeometry;
495  _epipolarGeometry = 0;
496  }
497  if(_memory)
498  {
499  if(databaseSaved)
500  {
501  if(_memory->isGraphReduced() && _memory->isIncremental())
502  {
503  // Force reducing graph, then remove filtered nodes from the optimized poses
504  std::map<int, int> reducedIds;
505  _memory->incrementMapId(&reducedIds);
506  for(std::map<int, int>::iterator iter=reducedIds.begin(); iter!=reducedIds.end(); ++iter)
507  {
508  _optimizedPoses.erase(iter->first);
509  }
510  }
511  _memory->saveOptimizedPoses(_optimizedPoses, _lastLocalizationPose);
512  }
513  _memory->close(databaseSaved, true, ouputDatabasePath);
514  delete _memory;
515  _memory = 0;
516  }
517  _optimizedPoses.clear();
518  _lastLocalizationPose.setNull();
519 
520  if(_bayesFilter)
521  {
522  delete _bayesFilter;
523  _bayesFilter = 0;
524  }
525  if(_graphOptimizer)
526  {
527  delete _graphOptimizer;
528  _graphOptimizer = 0;
529  }
530  _databasePath.clear();
531  parseParameters(Parameters::getDefaultParameters()); // reset to default parameters
532  _parameters.clear();
533 }
534 
535 void Rtabmap::parseParameters(const ParametersMap & parameters)
536 {
537  uInsert(_parameters, parameters);
538 
539  // place this before changing working directory
540  Parameters::parse(parameters, Parameters::kRtabmapStatisticLogsBufferedInRAM(), _statisticLogsBufferedInRAM);
541  Parameters::parse(parameters, Parameters::kRtabmapStatisticLogged(), _statisticLogged);
542  Parameters::parse(parameters, Parameters::kRtabmapStatisticLoggedHeaders(), _statisticLoggedHeaders);
543 
544  ULOGGER_DEBUG("");
545  ParametersMap::const_iterator iter;
546  if((iter=parameters.find(Parameters::kRtabmapWorkingDirectory())) != parameters.end())
547  {
548  this->setWorkingDirectory(iter->second.c_str());
549  }
550 
551  Parameters::parse(parameters, Parameters::kRtabmapPublishStats(), _publishStats);
552  Parameters::parse(parameters, Parameters::kRtabmapPublishLastSignature(), _publishLastSignatureData);
553  Parameters::parse(parameters, Parameters::kRtabmapPublishPdf(), _publishPdf);
554  Parameters::parse(parameters, Parameters::kRtabmapPublishLikelihood(), _publishLikelihood);
555  Parameters::parse(parameters, Parameters::kRtabmapPublishRAMUsage(), _publishRAMUsage);
556  Parameters::parse(parameters, Parameters::kRtabmapComputeRMSE(), _computeRMSE);
557  Parameters::parse(parameters, Parameters::kRtabmapSaveWMState(), _saveWMState);
558  Parameters::parse(parameters, Parameters::kRtabmapTimeThr(), _maxTimeAllowed);
559  Parameters::parse(parameters, Parameters::kRtabmapMemoryThr(), _maxMemoryAllowed);
560  Parameters::parse(parameters, Parameters::kRtabmapLoopThr(), _loopThr);
561  Parameters::parse(parameters, Parameters::kRtabmapLoopRatio(), _loopRatio);
562  Parameters::parse(parameters, Parameters::kRGBDMaxLoopClosureDistance(), _maxLoopClosureDistance);
563  Parameters::parse(parameters, Parameters::kVhEpEnabled(), _verifyLoopClosureHypothesis);
564  Parameters::parse(parameters, Parameters::kRtabmapMaxRetrieved(), _maxRetrieved);
565  Parameters::parse(parameters, Parameters::kRGBDMaxLocalRetrieved(), _maxLocalRetrieved);
566  Parameters::parse(parameters, Parameters::kRtabmapMaxRepublished(), _maxRepublished);
567  if(_maxRepublished == 0 || !_publishLastSignatureData)
568  {
569  _nodesToRepublish.clear();
570  }
571  Parameters::parse(parameters, Parameters::kMemImageKept(), _rawDataKept);
572  Parameters::parse(parameters, Parameters::kRGBDEnabled(), _rgbdSlamMode);
573  Parameters::parse(parameters, Parameters::kRGBDLinearUpdate(), _rgbdLinearUpdate);
574  Parameters::parse(parameters, Parameters::kRGBDAngularUpdate(), _rgbdAngularUpdate);
575  Parameters::parse(parameters, Parameters::kRGBDLinearSpeedUpdate(), _rgbdLinearSpeedUpdate);
576  Parameters::parse(parameters, Parameters::kRGBDAngularSpeedUpdate(), _rgbdAngularSpeedUpdate);
577  Parameters::parse(parameters, Parameters::kRGBDNewMapOdomChangeDistance(), _newMapOdomChangeDistance);
578  Parameters::parse(parameters, Parameters::kRGBDNeighborLinkRefining(), _neighborLinkRefining);
579  Parameters::parse(parameters, Parameters::kRGBDProximityByTime(), _proximityByTime);
580  Parameters::parse(parameters, Parameters::kRGBDProximityBySpace(), _proximityBySpace);
581  Parameters::parse(parameters, Parameters::kRGBDScanMatchingIdsSavedInLinks(), _scanMatchingIdsSavedInLinks);
582  Parameters::parse(parameters, Parameters::kRGBDLoopClosureIdentityGuess(), _loopClosureIdentityGuess);
583  Parameters::parse(parameters, Parameters::kRGBDLocalRadius(), _localRadius);
584  Parameters::parse(parameters, Parameters::kRGBDLocalImmunizationRatio(), _localImmunizationRatio);
585  Parameters::parse(parameters, Parameters::kRGBDProximityMaxGraphDepth(), _proximityMaxGraphDepth);
586  Parameters::parse(parameters, Parameters::kRGBDProximityMaxPaths(), _proximityMaxPaths);
587  Parameters::parse(parameters, Parameters::kRGBDProximityPathMaxNeighbors(), _proximityMaxNeighbors);
588  Parameters::parse(parameters, Parameters::kRGBDProximityPathFilteringRadius(), _proximityFilteringRadius);
589  Parameters::parse(parameters, Parameters::kRGBDProximityPathRawPosesUsed(), _proximityRawPosesUsed);
590  if(Parameters::parse(parameters, Parameters::kRGBDProximityAngle(), _proximityAngle))
591  {
592  _proximityAngle *= M_PI/180.0f;
593  }
594  Parameters::parse(parameters, Parameters::kRGBDProximityOdomGuess(), _proximityOdomGuess);
595  Parameters::parse(parameters, Parameters::kRGBDProximityMergedScanCovFactor(), _proximityMergedScanCovFactor);
596  UASSERT(_proximityMergedScanCovFactor>0.0);
597 
598  bool optimizeFromGraphEndPrevious = _optimizeFromGraphEnd;
599  Parameters::parse(parameters, Parameters::kRGBDOptimizeFromGraphEnd(), _optimizeFromGraphEnd);
600  if(optimizeFromGraphEndPrevious != _optimizeFromGraphEnd && !_optimizedPoses.empty())
601  {
602  _optimizeFromGraphEndChanged = true;
603  }
604  Parameters::parse(parameters, Parameters::kRGBDOptimizeMaxError(), _optimizationMaxError);
605  Parameters::parse(parameters, Parameters::kRtabmapStartNewMapOnLoopClosure(), _startNewMapOnLoopClosure);
606  Parameters::parse(parameters, Parameters::kRtabmapStartNewMapOnGoodSignature(), _startNewMapOnGoodSignature);
607  Parameters::parse(parameters, Parameters::kRGBDGoalReachedRadius(), _goalReachedRadius);
608  Parameters::parse(parameters, Parameters::kRGBDGoalsSavedInUserData(), _goalsSavedInUserData);
609  Parameters::parse(parameters, Parameters::kRGBDPlanStuckIterations(), _pathStuckIterations);
610  Parameters::parse(parameters, Parameters::kRGBDPlanLinearVelocity(), _pathLinearVelocity);
611  Parameters::parse(parameters, Parameters::kRGBDPlanAngularVelocity(), _pathAngularVelocity);
612  Parameters::parse(parameters, Parameters::kRGBDStartAtOrigin(), _restartAtOrigin);
613  Parameters::parse(parameters, Parameters::kRGBDLoopCovLimited(), _loopCovLimited);
614  Parameters::parse(parameters, Parameters::kRtabmapLoopGPS(), _loopGPS);
615  Parameters::parse(parameters, Parameters::kRGBDMaxOdomCacheSize(), _maxOdomCacheSize);
616  Parameters::parse(parameters, Parameters::kRGBDProximityGlobalScanMap(), _createGlobalScanMap);
617 
618  Parameters::parse(parameters, Parameters::kMarkerPriorsVarianceLinear(), _markerPriorsLinearVariance);
619  UASSERT(_markerPriorsLinearVariance>0.0f);
620  Parameters::parse(parameters, Parameters::kMarkerPriorsVarianceAngular(), _markerPriorsAngularVariance);
621  UASSERT(_markerPriorsAngularVariance>0.0f);
622  std::string markerPriorsStr;
623  if(Parameters::parse(parameters, Parameters::kMarkerPriors(), markerPriorsStr))
624  {
625  _markerPriors.clear();
626  std::list<std::string> strList = uSplit(markerPriorsStr, '|');
627  for(std::list<std::string>::iterator iter=strList.begin(); iter!=strList.end(); ++iter)
628  {
629  std::string markerStr = *iter;
630  while(!markerStr.empty() && !uIsDigit(markerStr[0]))
631  {
632  markerStr.erase(markerStr.begin());
633  }
634  if(!markerStr.empty())
635  {
636  std::string idStr = uSplitNumChar(markerStr).front();
637  int id = uStr2Int(idStr);
638  Transform prior = Transform::fromString(markerStr.substr(idStr.size()));
639  if(!prior.isNull() && id>0)
640  {
641  _markerPriors.insert(std::make_pair(-id, prior));
642  UDEBUG("Added landmark prior %d: %s", id, prior.prettyPrint().c_str());
643  }
644  else
645  {
646  UERROR("Failed to parse element \"%s\" in parameter %s", markerStr.c_str(), Parameters::kMarkerPriors().c_str());
647  }
648  }
649  else if(!iter->empty())
650  {
651  UERROR("Failed to parse parameter %s, value=\"%s\"", Parameters::kMarkerPriors().c_str(), iter->c_str());
652  }
653  }
654  }
655 
656  UASSERT(_rgbdLinearUpdate >= 0.0f);
657  UASSERT(_rgbdAngularUpdate >= 0.0f);
658  UASSERT(_rgbdLinearSpeedUpdate >= 0.0f);
659  UASSERT(_rgbdAngularSpeedUpdate >= 0.0f);
660  UASSERT(_maxOdomCacheSize >= 0);
661 
662  // By default, we create our strategies if they are not already created.
663  // If they already exists, we check the parameters if a change is requested
664 
665  // Graph optimizer
666  Optimizer::Type optimizerType = Optimizer::kTypeUndef;
667  if((iter=parameters.find(Parameters::kOptimizerStrategy())) != parameters.end())
668  {
669  optimizerType = (Optimizer::Type)std::atoi((*iter).second.c_str());
670  }
671  if(optimizerType!=Optimizer::kTypeUndef)
672  {
673  UDEBUG("new detector strategy %d", int(optimizerType));
674  if(_graphOptimizer)
675  {
676  delete _graphOptimizer;
677  _graphOptimizer = 0;
678  }
679 
680  _graphOptimizer = Optimizer::create(optimizerType, _parameters);
681  }
682  else if(_graphOptimizer)
683  {
684  _graphOptimizer->parseParameters(parameters);
685  }
686  else
687  {
688  optimizerType = (Optimizer::Type)Parameters::defaultOptimizerStrategy();
689  _graphOptimizer = Optimizer::create(optimizerType, parameters);
690  }
691 
692  if(!_createGlobalScanMap)
693  {
694  _globalScanMap.clear();
695  _globalScanMapPoses.clear();
696  }
697 
698  if(_memory)
699  {
700  _memory->parseParameters(parameters);
701  if(_memory->isIncremental() && !_globalScanMap.empty())
702  {
703  UWARN("Map is now incremental, clearing global scan map...");
704  _globalScanMap.clear();
705  _globalScanMapPoses.clear();
706  }
707 
708  if(_createGlobalScanMap && !_memory->isIncremental() && _globalScanMap.empty() && !_optimizedPoses.empty())
709  {
710  this->createGlobalScanMap();
711  }
712 
713  if(_memory->isIncremental())
714  {
715  _odomCachePoses.clear();
716  _odomCacheConstraints.clear();
717  }
718  }
719 
720  if(!_epipolarGeometry)
721  {
722  _epipolarGeometry = new EpipolarGeometry(_parameters);
723  }
724  else
725  {
726  _epipolarGeometry->parseParameters(parameters);
727  }
728 
729  // Bayes filter, create one if not exists
730  if(!_bayesFilter)
731  {
732  _bayesFilter = new BayesFilter(_parameters);
733  }
734  else
735  {
736  _bayesFilter->parseParameters(parameters);
737  }
738 }
739 
740 int Rtabmap::getLastLocationId() const
741 {
742  int id = 0;
743  if(_memory)
744  {
745  id = _memory->getLastSignatureId();
746  }
747  return id;
748 }
749 
750 std::list<int> Rtabmap::getWM() const
751 {
752  std::list<int> mem;
753  if(_memory)
754  {
755  mem = uKeysList(_memory->getWorkingMem());
756  mem.remove(-1);// Ignore the virtual signature (if here)
757  }
758  return mem;
759 }
760 
761 int Rtabmap::getWMSize() const
762 {
763  if(_memory)
764  {
765  return (int)_memory->getWorkingMem().size()-1; // remove virtual place
766  }
767  return 0;
768 }
769 
770 std::map<int, int> Rtabmap::getWeights() const
771 {
772  std::map<int, int> weights;
773  if(_memory)
774  {
775  weights = _memory->getWeights();
776  weights.erase(-1);// Ignore the virtual signature (if here)
777  }
778  return weights;
779 }
780 
781 std::set<int> Rtabmap::getSTM() const
782 {
783  if(_memory)
784  {
785  return _memory->getStMem();
786  }
787  return std::set<int>();
788 }
789 
790 int Rtabmap::getSTMSize() const
791 {
792  if(_memory)
793  {
794  return (int)_memory->getStMem().size();
795  }
796  return 0;
797 }
798 
799 int Rtabmap::getTotalMemSize() const
800 {
801  if(_memory)
802  {
803  const Signature * s =_memory->getLastWorkingSignature();
804  if(s)
805  {
806  return s->id();
807  }
808  }
809  return 0;
810 }
811 
812 bool Rtabmap::isInSTM(int locationId) const
813 {
814  if(_memory)
815  {
816  return _memory->isInSTM(locationId);
817  }
818  return false;
819 }
820 
821 bool Rtabmap::isIDsGenerated() const
822 {
823  if(_memory)
824  {
825  return _memory->isIDsGenerated();
826  }
827  return Parameters::defaultMemGenerateIds();
828 }
829 
830 const Statistics & Rtabmap::getStatistics() const
831 {
832  return statistics_;
833 }
834 
835 Transform Rtabmap::getPose(int locationId) const
836 {
837  return uValue(_optimizedPoses, locationId, Transform());
838 }
839 
840 void Rtabmap::setInitialPose(const Transform & initialPose)
841 {
842  if(_memory)
843  {
844  if(!_memory->isIncremental())
845  {
846  _lastLocalizationPose = initialPose;
847  _lastLocalizationNodeId = 0;
848  _odomCachePoses.clear();
849  _odomCacheConstraints.clear();
850  _odomCorrectionAcc = std::vector<float>(6,0);
851  _mapCorrection.setIdentity();
852  _mapCorrectionBackup.setNull();
853 
854  if(_memory->getLastWorkingSignature()->id() &&
855  _optimizedPoses.empty())
856  {
857  cv::Mat covariance;
858  this->optimizeCurrentMap(_memory->getLastWorkingSignature()->id(), false, _optimizedPoses, covariance, &_constraints);
859  }
860  }
861  else
862  {
863  UWARN("Initial pose can only be set in localization mode (%s=false), ignoring it...", Parameters::kMemIncrementalMemory().c_str());
864  }
865  }
866 }
867 
868 int Rtabmap::triggerNewMap()
869 {
870  int mapId = -1;
871  if(_memory)
872  {
873  _lastLocalizationNodeId = 0;
874  _odomCachePoses.clear();
875  _odomCacheConstraints.clear();
876  _odomCorrectionAcc = std::vector<float>(6,0);
877  _distanceTravelled = 0.0f;
878  _distanceTravelledSinceLastLocalization = 0.0f;
879 
880  if(!_memory->isIncremental())
881  {
882  _mapCorrection.setIdentity();
883  if(_restartAtOrigin)
884  {
885  _lastLocalizationPose.setIdentity();
886  }
887  return mapId;
888  }
889  std::map<int, int> reducedIds;
890  mapId = _memory->incrementMapId(&reducedIds);
891  UINFO("New map triggered, new map = %d", mapId);
892  _optimizedPoses.clear();
893  _constraints.clear();
894 
895  if(_bayesFilter)
896  {
897  _bayesFilter->reset();
898  }
899 
900  //Verify if there are nodes that were merged through graph reduction
901  if(reducedIds.size() && _path.size())
902  {
903  for(unsigned int i=0; i<_path.size(); ++i)
904  {
905  std::map<int, int>::const_iterator iter = reducedIds.find(_path[i].first);
906  if(iter!= reducedIds.end())
907  {
908  // change path ID to loop closure ID
909  _path[i].first = iter->second;
910  }
911  }
912  }
913  }
914  return mapId;
915 }
916 
917 bool Rtabmap::labelLocation(int id, const std::string & label)
918 {
919  if(_memory)
920  {
921  if(id > 0)
922  {
923  return _memory->labelSignature(id, label);
924  }
925  else if(_memory->isIncremental() && _memory->getLastWorkingSignature())
926  {
927  return _memory->labelSignature(_memory->getLastWorkingSignature()->id(), label);
928  }
929  else if(!_memory->isIncremental() && !_lastLocalizationPose.isNull() && !_lastLocalizationPose.isIdentity())
930  {
931  std::map<int, Transform> nearestNodes = getNodesInRadius(_lastLocalizationPose, _localRadius, 1);
932  if(!nearestNodes.empty())
933  {
934  return _memory->labelSignature(nearestNodes.begin()->first, label);
935  }
936  else
937  {
938  UERROR("No nodes found inside %s=%fm of the current pose (%s). Cannot set label \"%s\"",
939  Parameters::kRGBDLocalRadius().c_str(),
940  _localRadius,
941  _lastLocalizationPose.prettyPrint().c_str(),
942  label.c_str());
943  }
944  }
945  else
946  {
947  UERROR("Last signature is null! Cannot set label \"%s\"", label.c_str());
948  }
949  }
950  return false;
951 }
952 
953 bool Rtabmap::setUserData(int id, const cv::Mat & data)
954 {
955  if(_memory)
956  {
957  if(id > 0)
958  {
959  return _memory->setUserData(id, data);
960  }
961  else if(_memory->getLastWorkingSignature())
962  {
963  return _memory->setUserData(_memory->getLastWorkingSignature()->id(), data);
964  }
965  else
966  {
967  UERROR("Last signature is null! Cannot set user data!");
968  }
969  }
970  return false;
971 }
972 
973 void Rtabmap::generateDOTGraph(const std::string & path, int id, int margin)
974 {
975  if(_memory)
976  {
977  _memory->joinTrashThread(); // make sure the trash is flushed
978 
979  if(id > 0)
980  {
981  std::map<int, int> ids = _memory->getNeighborsId(id, margin, -1, false);
982 
983  if(ids.size() > 0)
984  {
985  ids.insert(std::pair<int,int>(id, 0));
986  std::set<int> idsSet;
987  for(std::map<int, int>::iterator iter = ids.begin(); iter!=ids.end(); ++iter)
988  {
989  idsSet.insert(idsSet.end(), iter->first);
990  }
991  _memory->generateGraph(path, idsSet);
992  }
993  else
994  {
995  UERROR("No neighbors found for signature %d.", id);
996  }
997  }
998  else
999  {
1000  _memory->generateGraph(path);
1001  }
1002  }
1003 }
1004 
1005 void Rtabmap::exportPoses(const std::string & path, bool optimized, bool global, int format)
1006 {
1007  if(_memory && _memory->getLastWorkingSignature())
1008  {
1009  std::map<int, Transform> poses;
1010  std::multimap<int, Link> constraints;
1011 
1012  if(optimized)
1013  {
1014  cv::Mat covariance;
1015  this->optimizeCurrentMap(_memory->getLastWorkingSignature()->id(), global, poses, covariance, &constraints);
1016  }
1017  else
1018  {
1019  std::map<int, int> ids = _memory->getNeighborsId(_memory->getLastWorkingSignature()->id(), 0, global?-1:0, true);
1020  _memory->getMetricConstraints(uKeysSet(ids), poses, constraints, global);
1021  }
1022 
1023  std::map<int, double> stamps;
1024  if(format == 1 || format == 10 || format == 11)
1025  {
1026  for(std::map<int, Transform>::iterator iter=poses.begin(); iter!=poses.end(); ++iter)
1027  {
1028  Transform o,g;
1029  int m, w;
1030  std::string l;
1031  double stamp = 0.0;
1032  std::vector<float> v;
1033  GPS gps;
1034  EnvSensors sensors;
1035  _memory->getNodeInfo(iter->first, o, m, w, l, stamp, g, v, gps, sensors, true);
1036  stamps.insert(std::make_pair(iter->first, stamp));
1037  }
1038  }
1039 
1040  graph::exportPoses(path, format, poses, constraints, stamps, _parameters);
1041  }
1042 }
1043 
1044 void Rtabmap::resetMemory()
1045 {
1046  UDEBUG("");
1047  _highestHypothesis = std::make_pair(0,0.0f);
1048  _loopClosureHypothesis = std::make_pair(0,0.0f);
1049  _lastProcessTime = 0.0;
1050  _someNodesHaveBeenTransferred = false;
1051  _optimizedPoses.clear();
1052  _constraints.clear();
1053  _mapCorrection.setIdentity();
1054  _mapCorrectionBackup.setNull();
1055  _lastLocalizationPose.setNull();
1056  _lastLocalizationNodeId = 0;
1057  _odomCachePoses.clear();
1058  _odomCacheConstraints.clear();
1059  _odomCorrectionAcc = std::vector<float>(6,0);
1060  _distanceTravelled = 0.0f;
1061  _distanceTravelledSinceLastLocalization = 0.0f;
1062  _optimizeFromGraphEndChanged = false;
1063  _globalScanMap.clear();
1064  _globalScanMapPoses.clear();
1065  _nodesToRepublish.clear();
1066  this->clearPath(0);
1067 
1068  if(_memory)
1069  {
1070  _memory->init(_databasePath, true, _parameters, true);
1071  if(_memory->getLastWorkingSignature())
1072  {
1073  cv::Mat covariance;
1074  optimizeCurrentMap(_memory->getLastWorkingSignature()->id(), false, _optimizedPoses, covariance, &_constraints);
1075  }
1076  if(_bayesFilter)
1077  {
1078  _bayesFilter->reset();
1079  }
1080  }
1081  else
1082  {
1083  UERROR("RTAB-Map is not initialized. No memory to reset...");
1084  }
1085  this->setupLogFiles(true);
1086 }
1087 
1088 class NearestPathKey
1089 {
1090 public:
1091  NearestPathKey(float l, int i, float d) :
1092  likelihood(l),
1093  id(i),
1094  distance(d){}
1095  bool operator<(const NearestPathKey & k) const
1096  {
1097  if(likelihood < k.likelihood)
1098  {
1099  return true;
1100  }
1101  else if(likelihood == k.likelihood)
1102  {
1103  if(distance > k.distance)
1104  {
1105  return true;
1106  }
1107  else if(distance == k.distance && id < k.id)
1108  {
1109  return true;
1110  }
1111  }
1112  return false;
1113  }
1114  float likelihood;
1115  int id;
1116  float distance;
1117 };
1118 
1119 //============================================================
1120 // MAIN LOOP
1121 //============================================================
1122 bool Rtabmap::process(
1123  const cv::Mat & image,
1124  int id,
1125  const std::map<std::string, float> & externalStats)
1126 {
1127  return this->process(SensorData(image, id), Transform());
1128 }
1129 bool Rtabmap::process(
1130  const SensorData & data,
1131  Transform odomPose,
1132  float odomLinearVariance,
1133  float odomAngularVariance,
1134  const std::vector<float> & odomVelocity,
1135  const std::map<std::string, float> & externalStats)
1136 {
1137  if(!odomPose.isNull())
1138  {
1139  UASSERT(odomLinearVariance>0.0f);
1140  UASSERT(odomAngularVariance>0.0f);
1141  }
1142  cv::Mat covariance = cv::Mat::eye(6,6,CV_64FC1);
1143  covariance.at<double>(0,0) = odomLinearVariance;
1144  covariance.at<double>(1,1) = odomLinearVariance;
1145  covariance.at<double>(2,2) = odomLinearVariance;
1146  covariance.at<double>(3,3) = odomAngularVariance;
1147  covariance.at<double>(4,4) = odomAngularVariance;
1148  covariance.at<double>(5,5) = odomAngularVariance;
1149  return process(data, odomPose, covariance, odomVelocity, externalStats);
1150 }
1151 bool Rtabmap::process(
1152  const SensorData & data,
1153  Transform odomPose,
1154  const cv::Mat & odomCovariance,
1155  const std::vector<float> & odomVelocity,
1156  const std::map<std::string, float> & externalStats)
1157 {
1158  UDEBUG("");
1159 
1160  //============================================================
1161  // Initialization
1162  //============================================================
1163  UTimer timer;
1164  UTimer timerTotal;
1165  double timeMemoryUpdate = 0;
1166  double timeNeighborLinkRefining = 0;
1167  double timeProximityByTimeDetection = 0;
1168  double timeProximityBySpaceVisualDetection = 0;
1169  double timeProximityBySpaceDetection = 0;
1170  double timeCleaningNeighbors = 0;
1171  double timeReactivations = 0;
1172  double timeAddLoopClosureLink = 0;
1173  double timeMapOptimization = 0;
1174  double timeRetrievalDbAccess = 0;
1175  double timeLikelihoodCalculation = 0;
1176  double timePosteriorCalculation = 0;
1177  double timeHypothesesCreation = 0;
1178  double timeHypothesesValidation = 0;
1179  double timeRealTimeLimitReachedProcess = 0;
1180  double timeMemoryCleanup = 0;
1181  double timeEmptyingTrash = 0;
1182  double timeFinalizingStatistics = 0;
1183  double timeJoiningTrash = 0;
1184  double timeStatsCreation = 0;
1185 
1186  float hypothesisRatio = 0.0f; // Only used for statistics
1187  bool rejectedGlobalLoopClosure = false;
1188 
1189  std::map<int, float> rawLikelihood;
1190  std::map<int, float> adjustedLikelihood;
1191  std::map<int, float> likelihood;
1192  std::map<int, int> weights;
1193  std::map<int, float> posterior;
1194  std::list<std::pair<int, float> > reactivateHypotheses;
1195 
1196  std::map<int, int> childCount;
1197  std::set<int> signaturesRetrieved;
1198  int proximityDetectionsInTimeFound = 0;
1199 
1200  const Signature * signature = 0;
1201  const Signature * sLoop = 0;
1202 
1203  _loopClosureHypothesis = std::make_pair(0,0.0f);
1204  std::pair<int, float> lastHighestHypothesis = _highestHypothesis;
1205  _highestHypothesis = std::make_pair(0,0.0f);
1206 
1207  std::set<int> immunizedLocations;
1208 
1209  statistics_ = Statistics(); // reset
1210  for(std::map<std::string, float>::const_iterator iter=externalStats.begin(); iter!=externalStats.end(); ++iter)
1211  {
1212  statistics_.addStatistic(iter->first, iter->second);
1213  }
1214 
1215  //============================================================
1216  // Wait for an image...
1217  //============================================================
1218  ULOGGER_INFO("getting data...");
1219 
1220  timer.start();
1221  timerTotal.start();
1222 
1223  UASSERT_MSG(_memory, "RTAB-Map is not initialized!");
1224  UASSERT_MSG(_bayesFilter, "RTAB-Map is not initialized!");
1225  UASSERT_MSG(_graphOptimizer, "RTAB-Map is not initialized!");
1226 
1227  //============================================================
1228  // If RGBD SLAM is enabled, a pose must be set.
1229  //============================================================
1230  bool fakeOdom = false;
1231  if(_rgbdSlamMode)
1232  {
1233  if(!odomPose.isNull())
1234  {
1235  // this will make sure that all inverse operations will work!
1236  if(!odomPose.isInvertible())
1237  {
1238  UWARN("Input odometry is not invertible! pose = %s\n"
1239  "[%f %f %f %f;\n"
1240  " %f %f %f %f;\n"
1241  " %f %f %f %f;\n"
1242  " 0 0 0 1]\n"
1243  "Trying to normalize rotation to see if it makes it invertible...",
1244  odomPose.prettyPrint().c_str(),
1245  odomPose.r11(), odomPose.r12(), odomPose.r13(), odomPose.o14(),
1246  odomPose.r21(), odomPose.r22(), odomPose.r23(), odomPose.o24(),
1247  odomPose.r31(), odomPose.r32(), odomPose.r33(), odomPose.o34());
1248  odomPose.normalizeRotation();
1249  UASSERT_MSG(odomPose.isInvertible(), uFormat("Odometry pose is not invertible!\n"
1250  "[%f %f %f %f;\n"
1251  " %f %f %f %f;\n"
1252  " %f %f %f %f;\n"
1253  " 0 0 0 1]", odomPose.prettyPrint().c_str(),
1254  odomPose.r11(), odomPose.r12(), odomPose.r13(), odomPose.o14(),
1255  odomPose.r21(), odomPose.r22(), odomPose.r23(), odomPose.o24(),
1256  odomPose.r31(), odomPose.r32(), odomPose.r33(), odomPose.o34()).c_str());
1257  UWARN("Normalizing rotation succeeded! fixed pose = %s\n"
1258  "[%f %f %f %f;\n"
1259  " %f %f %f %f;\n"
1260  " %f %f %f %f;\n"
1261  " 0 0 0 1]\n"
1262  "If the resulting rotation is very different from original one, try to fix the odometry or TF.",
1263  odomPose.prettyPrint().c_str(),
1264  odomPose.r11(), odomPose.r12(), odomPose.r13(), odomPose.o14(),
1265  odomPose.r21(), odomPose.r22(), odomPose.r23(), odomPose.o24(),
1266  odomPose.r31(), odomPose.r32(), odomPose.r33(), odomPose.o34());
1267  }
1268  }
1269 
1270  UDEBUG("incremental=%d odomPose=%s optimizedPoses=%d mapCorrection=%s lastLocalizationPose=%s lastLocalizationNodeId=%d",
1271  _memory->isIncremental()?1:0,
1272  odomPose.prettyPrint().c_str(),
1273  (int)_optimizedPoses.size(),
1274  _mapCorrection.prettyPrint().c_str(),
1275  _lastLocalizationPose.prettyPrint().c_str(),
1276  _lastLocalizationNodeId);
1277 
1278  if(!_memory->isIncremental() &&
1279  !odomPose.isNull() &&
1280  _optimizedPoses.size() &&
1281  _mapCorrection.isIdentity() &&
1282  !_lastLocalizationPose.isNull() &&
1283  _lastLocalizationNodeId == 0)
1284  {
1285  // Localization mode
1286  if(!_optimizeFromGraphEnd)
1287  {
1288  //set map->odom so that odom is moved back to last saved localization
1289  if(_graphOptimizer->isSlam2d())
1290  {
1291  _mapCorrection = _lastLocalizationPose.to3DoF() * odomPose.to3DoF().inverse();
1292  }
1293  else if((!data.imu().empty() || _memory->isOdomGravityUsed()) && _graphOptimizer->gravitySigma()>0.0f)
1294  {
1295  _mapCorrection = _lastLocalizationPose.to4DoF() * odomPose.to4DoF().inverse();
1296  }
1297  else
1298  {
1299  _mapCorrection = _lastLocalizationPose * odomPose.inverse();
1300  }
1301  std::map<int, Transform> nodesOnly(_optimizedPoses.lower_bound(1), _optimizedPoses.end());
1302  _lastLocalizationNodeId = graph::findNearestNode(nodesOnly, _lastLocalizationPose);
1303  UWARN("Update map correction based on last localization saved in database! correction = %s, nearest id = %d of last pose = %s, odom = %s",
1304  _mapCorrection.prettyPrint().c_str(),
1305  _lastLocalizationNodeId,
1306  _lastLocalizationPose.prettyPrint().c_str(),
1307  odomPose.prettyPrint().c_str());
1308  }
1309  else
1310  {
1311  //move optimized poses accordingly to last saved localization
1312  Transform mapCorrectionInv;
1313  if(_graphOptimizer->isSlam2d())
1314  {
1315  mapCorrectionInv = odomPose.to3DoF() * _lastLocalizationPose.to3DoF().inverse();
1316  }
1317  else if((!data.imu().empty() || _memory->isOdomGravityUsed()) && _graphOptimizer->gravitySigma()>0.0f)
1318  {
1319  mapCorrectionInv = odomPose.to4DoF() * _lastLocalizationPose.to4DoF().inverse();
1320  }
1321  else
1322  {
1323  mapCorrectionInv = odomPose * _lastLocalizationPose.inverse();
1324  }
1325  for(std::map<int, Transform>::iterator iter=_optimizedPoses.begin(); iter!=_optimizedPoses.end(); ++iter)
1326  {
1327  iter->second = mapCorrectionInv * iter->second;
1328  }
1329  }
1330  }
1331 
1332  if(odomPose.isNull())
1333  {
1334  if(_memory->isIncremental())
1335  {
1336  UERROR("RGB-D SLAM mode is enabled, memory is incremental but no odometry is provided. "
1337  "Image %d is ignored!", data.id());
1338  return false;
1339  }
1340  else // fake localization
1341  {
1342  if(!_mapCorrectionBackup.isNull())
1343  {
1344  _mapCorrection = _mapCorrectionBackup;
1345  _mapCorrectionBackup.setNull();
1346  }
1347  if(_lastLocalizationPose.isNull())
1348  {
1349  _lastLocalizationPose = Transform::getIdentity();
1350  }
1351  fakeOdom = true;
1352  odomPose = _mapCorrection.inverse() * _lastLocalizationPose;
1353  UDEBUG("Map correction = %s", _mapCorrection.prettyPrint().c_str());
1354  UDEBUG("Last localization pose: %s", _lastLocalizationPose.prettyPrint().c_str());
1355  UDEBUG("Fake odom: %s", odomPose.prettyPrint().c_str());
1356  }
1357  }
1358  else if(_memory->isIncremental()) // only in mapping mode
1359  {
1360  // Detect if the odometry is reset. If yes, trigger a new map.
1361  if(_memory->getLastWorkingSignature())
1362  {
1363  const Transform & lastPose = _memory->getLastWorkingSignature()->getPose(); // use raw odometry
1364 
1365  // look for identity
1366  if(!lastPose.isIdentity() && odomPose.isIdentity())
1367  {
1368  int mapId = triggerNewMap();
1369  UWARN("Odometry is reset (identity pose detected). Increment map id to %d!", mapId);
1370  }
1371  else if(_newMapOdomChangeDistance > 0.0)
1372  {
1373  // look for large change
1374  Transform lastPoseToNewPose = lastPose.inverse() * odomPose;
1375  float x,y,z, roll,pitch,yaw;
1376  lastPoseToNewPose.getTranslationAndEulerAngles(x,y,z, roll,pitch,yaw);
1377  if((x*x + y*y + z*z) > _newMapOdomChangeDistance*_newMapOdomChangeDistance)
1378  {
1379  int mapId = triggerNewMap();
1380  UWARN("Odometry is reset (large odometry change detected > %f). A new map (%d) is created! Last pose = %s, new pose = %s",
1381  _newMapOdomChangeDistance,
1382  mapId,
1383  lastPose.prettyPrint().c_str(),
1384  odomPose.prettyPrint().c_str());
1385  }
1386  }
1387  }
1388  }
1389  }
1390 
1391  //============================================================
1392  // Memory Update : Location creation + Add to STM + Weight Update (Rehearsal)
1393  //============================================================
1394  ULOGGER_INFO("Updating memory...");
1395  if(_rgbdSlamMode)
1396  {
1397  if(!_memory->update(data, odomPose, odomCovariance, odomVelocity, &statistics_))
1398  {
1399  return false;
1400  }
1401  }
1402  else
1403  {
1404  if(!_memory->update(data, Transform(), cv::Mat(), std::vector<float>(), &statistics_))
1405  {
1406  return false;
1407  }
1408  }
1409 
1410  signature = _memory->getLastWorkingSignature();
1411  _currentSessionHasGPS = _currentSessionHasGPS || signature->sensorData().gps().stamp() > 0.0;
1412  if(!signature)
1413  {
1414  UFATAL("Not supposed to be here...last signature is null?!?");
1415  }
1416 
1417  ULOGGER_INFO("Processing signature %d w=%d map=%d", signature->id(), signature->getWeight(), signature->mapId());
1418  timeMemoryUpdate = timer.ticks();
1419  ULOGGER_INFO("timeMemoryUpdate=%fs", timeMemoryUpdate);
1420 
1421  //============================================================
1422  // Metric
1423  //============================================================
1424  bool smallDisplacement = false;
1425  bool tooFastMovement = false;
1426  std::list<int> signaturesRemoved;
1427  bool neighborLinkRefined = false;
1428  bool addedNewLandmark = false;
1429  if(_rgbdSlamMode)
1430  {
1431  statistics_.addStatistic(Statistics::kMemoryOdometry_variance_lin(), odomCovariance.empty()?1.0f:(float)odomCovariance.at<double>(0,0));
1432  statistics_.addStatistic(Statistics::kMemoryOdometry_variance_ang(), odomCovariance.empty()?1.0f:(float)odomCovariance.at<double>(5,5));
1433 
1434  //Verify if there was a rehearsal
1435  int rehearsedId = (int)uValue(statistics_.data(), Statistics::kMemoryRehearsal_merged(), 0.0f);
1436  if(rehearsedId > 0)
1437  {
1438  _optimizedPoses.erase(rehearsedId);
1439  }
1440  else
1441  {
1442  if(_rgbdLinearUpdate > 0.0f || _rgbdAngularUpdate > 0.0f)
1443  {
1444  //============================================================
1445  // Minimum displacement required to add to Memory
1446  //============================================================
1447  Transform t;
1448 
1449  if(_memory->isIncremental())
1450  {
1451  const std::multimap<int, Link> & links = signature->getLinks();
1452  if(links.size() && links.begin()->second.type() == Link::kNeighbor)
1453  {
1454  const Signature * s = _memory->getSignature(links.begin()->second.to());
1455  UASSERT(s!=0);
1456  // don't filter if the new node is not intermediate but previous one is
1457  if(signature->getWeight() < 0 || s->getWeight() >= 0)
1458  {
1459  t = links.begin()->second.transform();
1460  }
1461  }
1462  }
1463  else if(!_odomCachePoses.empty())
1464  {
1465  t = _odomCachePoses.rbegin()->second.inverse() * signature->getPose();
1466  }
1467  if(!t.isNull())
1468  {
1469  float x,y,z, roll,pitch,yaw;
1470  t.getTranslationAndEulerAngles(x,y,z, roll,pitch,yaw);
1471  bool isMoving = fabs(x) > _rgbdLinearUpdate ||
1472  fabs(y) > _rgbdLinearUpdate ||
1473  fabs(z) > _rgbdLinearUpdate ||
1474  (_rgbdAngularUpdate>0.0f && (
1475  fabs(roll) > _rgbdAngularUpdate ||
1476  fabs(pitch) > _rgbdAngularUpdate ||
1477  fabs(yaw) > _rgbdAngularUpdate));
1478  if(!isMoving)
1479  {
1480  // This will disable global loop closure detection, only retrieval will be done.
1481  // The location will also be deleted at the end.
1482  smallDisplacement = true;
1483  UDEBUG("smallDisplacement: %f %f %f %f %f %f", x,y,z, roll,pitch,yaw);
1484  }
1485  }
1486  }
1487  if(odomVelocity.size() == 6)
1488  {
1489  // This will disable global loop closure detection, only retrieval will be done.
1490  // The location will also be deleted at the end.
1491  tooFastMovement =
1492  (_rgbdLinearSpeedUpdate>0.0f && uMax3(fabs(odomVelocity[0]), fabs(odomVelocity[1]), fabs(odomVelocity[2])) > _rgbdLinearSpeedUpdate) ||
1493  (_rgbdAngularSpeedUpdate>0.0f && uMax3(fabs(odomVelocity[3]), fabs(odomVelocity[4]), fabs(odomVelocity[5])) > _rgbdAngularSpeedUpdate);
1494  }
1495  }
1496 
1497  // Update optimizedPoses with the newly added node
1498  Transform newPose;
1499  bool intermediateNodeRefining = false;
1500  if(_neighborLinkRefining &&
1501  signature->getLinks().size() &&
1502  signature->getLinks().begin()->second.type() == Link::kNeighbor &&
1503  _memory->isIncremental() && // ignore pose matching in localization mode
1504  rehearsedId == 0 && // don't do it if rehearsal happened
1505  !tooFastMovement) // ignore if too fast movement has been detected
1506  {
1507  int oldId = signature->getLinks().begin()->first;
1508  const Signature * oldS = _memory->getSignature(oldId);
1509  UASSERT(oldS != 0);
1510 
1511  if(signature->getWeight() >= 0 && oldS->getWeight()>=0) // ignore intermediate nodes
1512  {
1513  Transform guess = signature->getLinks().begin()->second.transform().inverse();
1514 
1515  if(smallDisplacement)
1516  {
1517  if(signature->getLinks().begin()->second.transVariance() == 1)
1518  {
1519  // set small variance
1520  UDEBUG("Set small variance. The robot is not moving.");
1521  _memory->updateLink(Link(oldId, signature->id(), signature->getLinks().begin()->second.type(), guess, cv::Mat::eye(6,6,CV_64FC1)*1000));
1522  }
1523  }
1524  else
1525  {
1526  //============================================================
1527  // Refine neighbor links
1528  //============================================================
1529  UINFO("Odometry refining: guess = %s", guess.prettyPrint().c_str());
1530  RegistrationInfo info;
1531  Transform t = _memory->computeTransform(oldId, signature->id(), guess, &info);
1532  if(!t.isNull())
1533  {
1534  UINFO("Odometry refining: update neighbor link (%d->%d, variance:lin=%f, ang=%f) from %s to %s",
1535  oldId,
1536  signature->id(),
1537  info.covariance.at<double>(0,0),
1538  info.covariance.at<double>(5,5),
1539  guess.prettyPrint().c_str(),
1540  t.prettyPrint().c_str());
1541  UASSERT(info.covariance.at<double>(0,0) > 0.0 && info.covariance.at<double>(5,5) > 0.0);
1542  _memory->updateLink(Link(oldId, signature->id(), signature->getLinks().begin()->second.type(), t, info.covariance.inv()));
1543 
1544  if(_optimizeFromGraphEnd)
1545  {
1546  // update all previous nodes
1547  // Normally _mapCorrection should be identity, but if _optimizeFromGraphEnd
1548  // parameters just changed state, we should put back all poses without map correction.
1549  Transform u = guess * t.inverse();
1550  std::map<int, Transform>::iterator jter = _optimizedPoses.find(oldId);
1551  UASSERT(jter!=_optimizedPoses.end());
1552  Transform up = jter->second * u * jter->second.inverse();
1553  Transform mapCorrectionInv = _mapCorrection.inverse();
1554  for(std::map<int, Transform>::iterator iter=_optimizedPoses.begin(); iter!=_optimizedPoses.end(); ++iter)
1555  {
1556  iter->second = mapCorrectionInv * up * iter->second;
1557  }
1558  }
1559  }
1560  else
1561  {
1562  UINFO("Odometry refining rejected: %s", info.rejectedMsg.c_str());
1563  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)
1564  {
1565  if(ULogger::level() <= ULogger::kInfo)
1566  {
1567  std::cout << info.covariance << std::endl;
1568  }
1569  _memory->updateLink(Link(oldId, signature->id(), signature->getLinks().begin()->second.type(), guess, (info.covariance*100.0).inv()));
1570  }
1571  }
1572  neighborLinkRefined = !t.isNull();
1573  statistics_.addStatistic(Statistics::kNeighborLinkRefiningAccepted(),neighborLinkRefined?1.0f:0);
1574  statistics_.addStatistic(Statistics::kNeighborLinkRefiningInliers(), info.inliers);
1575  statistics_.addStatistic(Statistics::kNeighborLinkRefiningICP_inliers_ratio(), info.icpInliersRatio);
1576  statistics_.addStatistic(Statistics::kNeighborLinkRefiningICP_rotation(), info.icpRotation);
1577  statistics_.addStatistic(Statistics::kNeighborLinkRefiningICP_translation(), info.icpTranslation);
1578  statistics_.addStatistic(Statistics::kNeighborLinkRefiningICP_complexity(), info.icpStructuralComplexity);
1579  statistics_.addStatistic(Statistics::kNeighborLinkRefiningPts(), signature->sensorData().laserScanRaw().size());
1580  }
1581  timeNeighborLinkRefining = timer.ticks();
1582  ULOGGER_INFO("timeOdometryRefining=%fs", timeNeighborLinkRefining);
1583 
1584  UASSERT(oldS->hasLink(signature->id()));
1585  UASSERT(uContains(_optimizedPoses, oldId));
1586 
1587  statistics_.addStatistic(Statistics::kNeighborLinkRefiningVariance(), oldS->getLinks().find(signature->id())->second.transVariance());
1588 
1589  newPose = _optimizedPoses.at(oldId) * oldS->getLinks().find(signature->id())->second.transform();
1590  _mapCorrection = newPose * signature->getPose().inverse();
1591  if(_mapCorrection.getNormSquared() > 0.001f && _optimizeFromGraphEnd)
1592  {
1593  UERROR("Map correction should be identity when optimizing from the last node. T=%s NewPose=%s OldPose=%s",
1594  _mapCorrection.prettyPrint().c_str(),
1595  newPose.prettyPrint().c_str(),
1596  signature->getPose().prettyPrint().c_str());
1597  }
1598  }
1599  else
1600  {
1601  newPose = _mapCorrection * signature->getPose();
1602  intermediateNodeRefining = true;
1603  }
1604  }
1605  else
1606  {
1607  newPose = _mapCorrection * signature->getPose();
1608  }
1609 
1610  UDEBUG("Added pose %s (odom=%s)", newPose.prettyPrint().c_str(), signature->getPose().prettyPrint().c_str());
1611  // Update Poses and Constraints
1612  _optimizedPoses.insert(std::make_pair(signature->id(), newPose));
1613  if(_memory->isIncremental() && signature->getWeight() >= 0)
1614  {
1615  for(std::map<int, Link>::const_iterator iter = signature->getLandmarks().begin(); iter!=signature->getLandmarks().end(); ++iter)
1616  {
1617  if(_optimizedPoses.find(iter->first) == _optimizedPoses.end())
1618  {
1619  _optimizedPoses.insert(std::make_pair(iter->first, newPose*iter->second.transform()));
1620  UDEBUG("Added landmark %d : %s", iter->first, (newPose*iter->second.transform()).prettyPrint().c_str());
1621  addedNewLandmark = true;
1622  }
1623  _constraints.insert(std::make_pair(iter->first, iter->second.inverse()));
1624  }
1625  }
1626 
1627  float distanceTravelledOld = _distanceTravelled;
1628 
1629  // only in mapping mode we add a neighbor link
1630  if(signature->getLinks().size() &&
1631  signature->getLinks().begin()->second.type() == Link::kNeighbor)
1632  {
1633  // link should be old to new
1634  UASSERT_MSG(signature->id() > signature->getLinks().begin()->second.to(),
1635  "Only forward links should be added.");
1636 
1637  Link tmp = signature->getLinks().begin()->second.inverse();
1638 
1639  _distanceTravelled += tmp.transform().getNorm();
1640 
1641  // if the previous node is an intermediate node, remove it from the local graph
1642  if(_constraints.size() &&
1643  _constraints.rbegin()->second.to() == signature->getLinks().begin()->second.to())
1644  {
1645  const Signature * s = _memory->getSignature(signature->getLinks().begin()->second.to());
1646  UASSERT(s!=0);
1647  if(s->getWeight() == -1)
1648  {
1649  tmp = _constraints.rbegin()->second.merge(tmp, tmp.type());
1650  _optimizedPoses.erase(s->id());
1651  _constraints.erase(--_constraints.end());
1652  }
1653  }
1654  _constraints.insert(std::make_pair(tmp.from(), tmp));
1655  }
1656  // Localization mode stuff
1657  _lastLocalizationPose = newPose; // keep in cache the latest corrected pose
1658  if(!_memory->isIncremental() && signature->getWeight() >= 0)
1659  {
1660  UDEBUG("Update odometry localization cache (size=%d/%d)", (int)_odomCachePoses.size(), _maxOdomCacheSize);
1661  if(!_odomCachePoses.empty())
1662  {
1663  float odomDistance = (_odomCachePoses.rbegin()->second.inverse() * signature->getPose()).getNorm();
1664  _distanceTravelled += odomDistance;
1665 
1666  while(!_odomCachePoses.empty() && (int)_odomCachePoses.size() > _maxOdomCacheSize)
1667  {
1668  _odomCacheConstraints.erase(_odomCachePoses.begin()->first);
1669  _odomCachePoses.erase(_odomCachePoses.begin());
1670  }
1671  if(!_odomCachePoses.empty())
1672  {
1673  Link odomLink(_odomCachePoses.rbegin()->first,
1674  signature->id(),
1675  Link::kNeighbor,
1676  _odomCachePoses.rbegin()->second.inverse() * signature->getPose(),
1677  odomCovariance.inv());
1678  _odomCacheConstraints.insert(std::make_pair(_odomCachePoses.rbegin()->first, odomLink));
1679  UDEBUG("Added odom cov = %f %f", odomLink.transVariance(), odomLink.rotVariance());
1680  }
1681  }
1682 
1683  _odomCachePoses.insert(std::make_pair(signature->id(), signature->getPose()));
1684  }
1685  _distanceTravelledSinceLastLocalization += _distanceTravelled - distanceTravelledOld;
1686 
1687  //============================================================
1688  // Reduced graph
1689  //============================================================
1690  //Verify if there are nodes that were merged through graph reduction
1691  if(statistics_.reducedIds().size())
1692  {
1693  for(unsigned int i=0; i<_path.size(); ++i)
1694  {
1695  std::map<int, int>::const_iterator iter = statistics_.reducedIds().find(_path[i].first);
1696  if(iter!= statistics_.reducedIds().end())
1697  {
1698  // change path ID to loop closure ID
1699  _path[i].first = iter->second;
1700  }
1701  }
1702 
1703  for(std::map<int, int>::const_iterator iter=statistics_.reducedIds().begin();
1704  iter!=statistics_.reducedIds().end();
1705  ++iter)
1706  {
1707  int erased = (int)_optimizedPoses.erase(iter->first);
1708  if(erased)
1709  {
1710  for(std::multimap<int, Link>::iterator jter = _constraints.begin(); jter!=_constraints.end();)
1711  {
1712  if(jter->second.from() == iter->first || jter->second.to() == iter->first)
1713  {
1714  _constraints.erase(jter++);
1715  }
1716  else
1717  {
1718  ++jter;
1719  }
1720  }
1721  }
1722  }
1723  }
1724 
1725  //============================================================
1726  // Local loop closure in TIME
1727  //============================================================
1728  if((_proximityByTime || intermediateNodeRefining) &&
1729  rehearsedId == 0 && // don't do it if rehearsal happened
1730  _memory->isIncremental() && // don't do it in localization mode
1731  signature->getWeight()>=0)
1732  {
1733  const std::set<int> & stm = _memory->getStMem();
1734  for(std::set<int>::const_reverse_iterator iter = stm.rbegin(); iter!=stm.rend(); ++iter)
1735  {
1736  if(*iter != signature->id() &&
1737  signature->getLinks().find(*iter) == signature->getLinks().end() &&
1738  _memory->getSignature(*iter)->mapId() == signature->mapId() &&
1739  _memory->getSignature(*iter)->getWeight()>=0)
1740  {
1741  std::string rejectedMsg;
1742  UDEBUG("Check local transform between %d and %d", signature->id(), *iter);
1743  RegistrationInfo info;
1744  Transform guess;
1745  if(_optimizedPoses.find(*iter) != _optimizedPoses.end())
1746  {
1747  guess = _optimizedPoses.at(*iter).inverse() * newPose;
1748  }
1749 
1750  // For proximity by time, correspondences should be already enough precise, so don't recompute them
1751  Transform transform = _memory->computeTransform(*iter, signature->id(), guess, &info, true);
1752 
1753  if(!transform.isNull())
1754  {
1755  transform = transform.inverse();
1756  UDEBUG("Add local loop closure in TIME (%d->%d) %s",
1757  signature->id(),
1758  *iter,
1759  transform.prettyPrint().c_str());
1760  // Add a loop constraint
1761  UASSERT(info.covariance.at<double>(0,0) > 0.0 && info.covariance.at<double>(5,5) > 0.0);
1762  if(_memory->addLink(Link(signature->id(), *iter, Link::kLocalTimeClosure, transform, getInformation(info.covariance))))
1763  {
1764  ++proximityDetectionsInTimeFound;
1765  UINFO("Local loop closure found between %d and %d with t=%s",
1766  *iter, signature->id(), transform.prettyPrint().c_str());
1767  }
1768  else
1769  {
1770  UWARN("Cannot add local loop closure between %d and %d ?!?",
1771  *iter, signature->id());
1772  }
1773  }
1774  else
1775  {
1776  UINFO("Local loop closure (time) between %d and %d rejected: %s",
1777  *iter, signature->id(), rejectedMsg.c_str());
1778  }
1779 
1780  if(!_proximityByTime && intermediateNodeRefining)
1781  {
1782  // Do it only with the latest non-intermediate node
1783  break;
1784  }
1785  }
1786  }
1787  }
1788  }
1789 
1790  timeProximityByTimeDetection = timer.ticks();
1791  UINFO("timeProximityByTimeDetection=%fs", timeProximityByTimeDetection);
1792 
1793  //============================================================
1794  // Bayes filter update
1795  //============================================================
1796  int previousId = signature->getLinks().size() && signature->getLinks().begin()->first!=signature->id()?signature->getLinks().begin()->first:0;
1797  // Not a bad signature, not an intermediate node, not a small displacement unless the previous signature didn't have a loop closure, not too fast movement
1798  if(!signature->isBadSignature() && signature->getWeight()>=0 && (!smallDisplacement || _memory->getLoopClosureLinks(previousId, false).size() == 0) && !tooFastMovement)
1799  {
1800  // If the working memory is empty, don't do the detection. It happens when it
1801  // is the first time the detector is started (there needs some images to
1802  // fill the short-time memory before a signature is added to the working memory).
1803  if(_memory->getWorkingMem().size())
1804  {
1805  //============================================================
1806  // Likelihood computation
1807  // Get the likelihood of the new signature
1808  // with all images contained in the working memory + reactivated.
1809  //============================================================
1810  ULOGGER_INFO("computing likelihood...");
1811 
1812  std::list<int> signaturesToCompare;
1813  GPS originGPS;
1814  Transform originOffsetENU = Transform::getIdentity();
1815  if(_loopGPS)
1816  {
1817  originGPS = signature->sensorData().gps();
1818  if(originGPS.stamp() == 0.0 && _currentSessionHasGPS)
1819  {
1820  UTimer tmpT;
1821  if(_optimizedPoses.size() && _memory->isIncremental())
1822  {
1823  //Search for latest node having GPS linked to current signature not too far.
1824  std::map<int, float> nearestIds = graph::findNearestNodes(signature->id(), _optimizedPoses, _localRadius);
1825  for(std::map<int, float>::reverse_iterator iter=nearestIds.rbegin(); iter!=nearestIds.rend() && iter->first>0; ++iter)
1826  {
1827  const Signature * s = _memory->getSignature(iter->first);
1828  UASSERT(s!=0);
1829  if(s->sensorData().gps().stamp() > 0.0)
1830  {
1831  originGPS = s->sensorData().gps();
1832  const Transform & sPose = _optimizedPoses.at(s->id());
1833  Transform localToENU(0,0,(float)((-(originGPS.bearing()-90))*M_PI/180.0) - sPose.theta());
1834  originOffsetENU = localToENU * (sPose.rotation()*(sPose.inverse()*_optimizedPoses.at(signature->id())));
1835  break;
1836  }
1837  }
1838  }
1839  //else if(!_memory->isIncremental()) // TODO, how can we estimate current GPS position in localization?
1840  //{
1841  //}
1842  }
1843  if(originGPS.stamp() > 0.0)
1844  {
1845  // no need to save it if it is in localization mode
1846  _gpsGeocentricCache.insert(std::make_pair(signature->id(), std::make_pair(originGPS.toGeodeticCoords().toGeocentric_WGS84(), originOffsetENU)));
1847  }
1848  }
1849 
1850  for(std::map<int, double>::const_iterator iter=_memory->getWorkingMem().begin();
1851  iter!=_memory->getWorkingMem().end();
1852  ++iter)
1853  {
1854  if(iter->first > 0)
1855  {
1856  const Signature * s = _memory->getSignature(iter->first);
1857  UASSERT(s!=0);
1858  if(s->getWeight() != -1) // ignore intermediate nodes
1859  {
1860  bool accept = true;
1861  if(originGPS.stamp()>0.0)
1862  {
1863  std::map<int, std::pair<cv::Point3d, Transform> >::iterator cacheIter = _gpsGeocentricCache.find(s->id());
1864  if(cacheIter == _gpsGeocentricCache.end())
1865  {
1866  GPS gps = s->sensorData().gps();
1867  Transform offsetENU = Transform::getIdentity();
1868  if(gps.stamp()==0.0)
1869  {
1870  _memory->getGPS(s->id(), gps, offsetENU, false);
1871  }
1872  if(gps.stamp() > 0.0)
1873  {
1874  cacheIter = _gpsGeocentricCache.insert(
1875  std::make_pair(s->id(),
1876  std::make_pair(gps.toGeodeticCoords().toGeocentric_WGS84(), offsetENU))).first;
1877  }
1878  }
1879 
1880 
1881  if(cacheIter != _gpsGeocentricCache.end())
1882  {
1883  std::map<int, std::pair<cv::Point3d, Transform> >::iterator originIter = _gpsGeocentricCache.find(signature->id());
1884  UASSERT(originIter != _gpsGeocentricCache.end());
1885  cv::Point3d relativePose = GeodeticCoords::Geocentric_WGS84ToENU_WGS84(cacheIter->second.first, originIter->second.first, originGPS.toGeodeticCoords());
1886  const double & error = originGPS.error();
1887  const Transform & offsetENU = cacheIter->second.second;
1888  relativePose.x += offsetENU.x() - originOffsetENU.x();
1889  relativePose.y += offsetENU.y() - originOffsetENU.y();
1890  relativePose.z += offsetENU.z() - originOffsetENU.z();
1891  // ignore altitude if difference is under GPS error
1892  if(relativePose.z>error)
1893  {
1894  relativePose.z -= error;
1895  }
1896  else if(relativePose.z < -error)
1897  {
1898  relativePose.z += error;
1899  }
1900  else
1901  {
1902  relativePose.z = 0;
1903  }
1904  accept = uNormSquared(relativePose.x, relativePose.y, relativePose.z) < _localRadius*_localRadius;
1905  }
1906  }
1907 
1908  if(accept)
1909  {
1910  signaturesToCompare.push_back(iter->first);
1911  }
1912  }
1913  }
1914  else
1915  {
1916  // virtual signature should be added
1917  signaturesToCompare.push_back(iter->first);
1918  }
1919  }
1920 
1921  rawLikelihood = _memory->computeLikelihood(signature, signaturesToCompare);
1922 
1923  // Adjust the likelihood (with mean and std dev)
1924  likelihood = rawLikelihood;
1925  this->adjustLikelihood(likelihood);
1926 
1927  timeLikelihoodCalculation = timer.ticks();
1928  ULOGGER_INFO("timeLikelihoodCalculation=%fs",timeLikelihoodCalculation);
1929 
1930  //============================================================
1931  // Apply the Bayes filter
1932  // Posterior = Likelihood x Prior
1933  //============================================================
1934  ULOGGER_INFO("getting posterior...");
1935 
1936  // Compute the posterior
1937  posterior = _bayesFilter->computePosterior(_memory, likelihood);
1938  timePosteriorCalculation = timer.ticks();
1939  ULOGGER_INFO("timePosteriorCalculation=%fs",timePosteriorCalculation);
1940 
1941  // For statistics, copy weights
1942  if(_publishStats && (_publishLikelihood || _publishPdf))
1943  {
1944  weights = _memory->getWeights();
1945  }
1946 
1947  //============================================================
1948  // Select the highest hypothesis
1949  //============================================================
1950  ULOGGER_INFO("creating hypotheses...");
1951  if(posterior.size())
1952  {
1953  for(std::map<int, float>::const_reverse_iterator iter = posterior.rbegin(); iter != posterior.rend(); ++iter)
1954  {
1955  if(iter->first > 0 && iter->second > _highestHypothesis.second)
1956  {
1957  _highestHypothesis = *iter;
1958  }
1959  }
1960  // With the virtual place, use sum of LC probabilities (1 - virtual place hypothesis).
1961  _highestHypothesis.second = 1-posterior.begin()->second;
1962  }
1963  timeHypothesesCreation = timer.ticks();
1964  ULOGGER_INFO("Highest hypothesis=%d, value=%f, timeHypothesesCreation=%fs", _highestHypothesis.first, _highestHypothesis.second, timeHypothesesCreation);
1965 
1966  if(_highestHypothesis.first > 0)
1967  {
1968  float loopThr = _loopThr;
1969  if((_startNewMapOnLoopClosure || !_memory->isIncremental()) &&
1970  graph::filterLinks(signature->getLinks(), Link::kSelfRefLink).size() == 0 && // alone in the current map
1971  _memory->getWorkingMem().size()>1 && // should have an old map (beside virtual signature)
1972  (int)_memory->getWorkingMem().size()<=_memory->getMaxStMemSize() &&
1973  _rgbdSlamMode)
1974  {
1975  // If the map is very small (under STM size) and we need to find
1976  // a loop closure before continuing the map or localizing,
1977  // use the best hypothesis directly.
1978  loopThr = 0.0f;
1979  }
1980 
1981  // Loop closure Threshold
1982  // When _loopThr=0, accept loop closure if the hypothesis is over
1983  // the virtual (new) place hypothesis.
1984  if(_highestHypothesis.second >= loopThr)
1985  {
1986  rejectedGlobalLoopClosure = true;
1987  if(posterior.size() <= 2 && loopThr>0.0f)
1988  {
1989  // Ignore loop closure if there is only one loop closure hypothesis
1990  UDEBUG("rejected hypothesis: single hypothesis");
1991  }
1992  else if(_verifyLoopClosureHypothesis && !_epipolarGeometry->check(signature, _memory->getSignature(_highestHypothesis.first)))
1993  {
1994  UWARN("rejected hypothesis: by epipolar geometry");
1995  }
1996  else if(_loopRatio > 0.0f && lastHighestHypothesis.second && _highestHypothesis.second < _loopRatio*lastHighestHypothesis.second)
1997  {
1998  UWARN("rejected hypothesis: not satisfying hypothesis ratio (%f < %f * %f)",
1999  _highestHypothesis.second, _loopRatio, lastHighestHypothesis.second);
2000  }
2001  else if(_loopRatio > 0.0f && lastHighestHypothesis.second == 0)
2002  {
2003  UWARN("rejected hypothesis: last closure hypothesis is null (loop ratio is on)");
2004  }
2005  else
2006  {
2007  _loopClosureHypothesis = _highestHypothesis;
2008  rejectedGlobalLoopClosure = false;
2009  }
2010 
2011  timeHypothesesValidation = timer.ticks();
2012  ULOGGER_INFO("timeHypothesesValidation=%fs",timeHypothesesValidation);
2013  }
2014  else if(_highestHypothesis.second < _loopRatio*lastHighestHypothesis.second)
2015  {
2016  // Used for Precision-Recall computation.
2017  // When analyzing logs, it's convenient to know
2018  // if the hypothesis would be rejected if T_loop would be lower.
2019  rejectedGlobalLoopClosure = true;
2020  UDEBUG("rejected hypothesis: under loop ratio %f < %f", _highestHypothesis.second, _loopRatio*lastHighestHypothesis.second);
2021  }
2022 
2023  //for statistic...
2024  hypothesisRatio = _loopClosureHypothesis.second>0?_highestHypothesis.second/_loopClosureHypothesis.second:0;
2025  }
2026  } // if(_memory->getWorkingMemSize())
2027  }// !isBadSignature
2028  else if(!signature->isBadSignature() && (smallDisplacement || tooFastMovement))
2029  {
2030  _highestHypothesis = lastHighestHypothesis;
2031  UDEBUG("smallDisplacement=%d tooFastMovement=%d", smallDisplacement?1:0, tooFastMovement?1:0);
2032  }
2033  else
2034  {
2035  UDEBUG("Ignoring likelihood and loop closure hypotheses as current signature doesn't have enough visual features.");
2036  }
2037 
2038  //============================================================
2039  // Before retrieval, make sure the trash has finished
2040  //============================================================
2041  _memory->joinTrashThread();
2042  timeEmptyingTrash = _memory->getDbSavingTime();
2043  timeJoiningTrash = timer.ticks();
2044  ULOGGER_INFO("Time emptying memory trash = %fs, joining (actual overhead) = %fs", timeEmptyingTrash, timeJoiningTrash);
2045 
2046  //============================================================
2047  // RETRIEVAL 1/3 : Loop closure neighbors reactivation
2048  //============================================================
2049  int retrievalId = _highestHypothesis.first;
2050  std::list<int> reactivatedIds;
2051  double timeGetNeighborsTimeDb = 0.0;
2052  double timeGetNeighborsSpaceDb = 0.0;
2053  int immunizedGlobally = 0;
2054  int immunizedLocally = 0;
2055  int maxLocalLocationsImmunized = 0;
2056  if(_maxTimeAllowed != 0 || _maxMemoryAllowed != 0)
2057  {
2058  // with memory management, we have to immunize some nodes
2059  maxLocalLocationsImmunized = _localImmunizationRatio * float(_memory->getWorkingMem().size());
2060  }
2061  // no need to do retrieval or immunization of locations if memory management
2062  // is disabled and all nodes are in WM
2063  if(!(_memory->allNodesInWM() && maxLocalLocationsImmunized == 0))
2064  {
2065  if(retrievalId > 0)
2066  {
2067  //Load neighbors
2068  ULOGGER_INFO("Retrieving locations... around id=%d", retrievalId);
2069  int neighborhoodSize = (int)_bayesFilter->getPredictionLC().size()-1;
2070  UASSERT(neighborhoodSize >= 0);
2071  ULOGGER_DEBUG("margin=%d maxRetieved=%d", neighborhoodSize, _maxRetrieved);
2072 
2073  UTimer timeGetN;
2074  unsigned int nbLoadedFromDb = 0;
2075  std::set<int> reactivatedIdsSet;
2076  std::map<int, int> neighbors;
2077  int nbDirectNeighborsInDb = 0;
2078 
2079  // priority in time
2080  // Direct neighbors TIME
2081  ULOGGER_DEBUG("In TIME");
2082  neighbors = _memory->getNeighborsId(retrievalId,
2083  neighborhoodSize,
2084  _maxRetrieved,
2085  true,
2086  true,
2087  false,
2088  true,
2089  std::set<int>(),
2090  &timeGetNeighborsTimeDb);
2091  ULOGGER_DEBUG("neighbors of %d in time = %d", retrievalId, (int)neighbors.size());
2092  //Priority to locations near in time (direct neighbor) then by space (loop closure)
2093  bool firstPassDone = false; // just to avoid checking to STM after the first pass
2094  int m = 0;
2095  while(m < neighborhoodSize)
2096  {
2097  std::set<int> idsSorted;
2098  for(std::map<int, int>::iterator iter=neighbors.begin(); iter!=neighbors.end();)
2099  {
2100  if(!firstPassDone && _memory->isInSTM(iter->first))
2101  {
2102  neighbors.erase(iter++);
2103  }
2104  else if(iter->second == m)
2105  {
2106  if(reactivatedIdsSet.find(iter->first) == reactivatedIdsSet.end())
2107  {
2108  idsSorted.insert(iter->first);
2109  reactivatedIdsSet.insert(iter->first);
2110 
2111  if(m == 1 && _memory->getSignature(iter->first) == 0)
2112  {
2113  ++nbDirectNeighborsInDb;
2114  }
2115 
2116  //immunized locations in the neighborhood from being transferred
2117  if(immunizedLocations.insert(iter->first).second)
2118  {
2119  ++immunizedGlobally;
2120  }
2121 
2122  //UDEBUG("nt=%d m=%d immunized=1", iter->first, iter->second);
2123  }
2124  neighbors.erase(iter++);
2125  }
2126  else
2127  {
2128  ++iter;
2129  }
2130  }
2131  firstPassDone = true;
2132  reactivatedIds.insert(reactivatedIds.end(), idsSorted.rbegin(), idsSorted.rend());
2133  ++m;
2134  }
2135 
2136  // neighbors SPACE, already added direct neighbors will be ignored
2137  ULOGGER_DEBUG("In SPACE");
2138  neighbors = _memory->getNeighborsId(retrievalId,
2139  neighborhoodSize,
2140  _maxRetrieved,
2141  true,
2142  false,
2143  false,
2144  false,
2145  std::set<int>(),
2146  &timeGetNeighborsSpaceDb);
2147  ULOGGER_DEBUG("neighbors of %d in space = %d", retrievalId, (int)neighbors.size());
2148  firstPassDone = false;
2149  m = 0;
2150  while(m < neighborhoodSize)
2151  {
2152  std::set<int> idsSorted;
2153  for(std::map<int, int>::iterator iter=neighbors.begin(); iter!=neighbors.end();)
2154  {
2155  if(!firstPassDone && _memory->isInSTM(iter->first))
2156  {
2157  neighbors.erase(iter++);
2158  }
2159  else if(iter->second == m)
2160  {
2161  if(reactivatedIdsSet.find(iter->first) == reactivatedIdsSet.end())
2162  {
2163  idsSorted.insert(iter->first);
2164  reactivatedIdsSet.insert(iter->first);
2165 
2166  if(m == 1 && _memory->getSignature(iter->first) == 0)
2167  {
2168  ++nbDirectNeighborsInDb;
2169  }
2170  //UDEBUG("nt=%d m=%d", iter->first, iter->second);
2171  }
2172  neighbors.erase(iter++);
2173  }
2174  else
2175  {
2176  ++iter;
2177  }
2178  }
2179  firstPassDone = true;
2180  reactivatedIds.insert(reactivatedIds.end(), idsSorted.rbegin(), idsSorted.rend());
2181  ++m;
2182  }
2183  ULOGGER_INFO("neighborhoodSize=%d, "
2184  "reactivatedIds.size=%d, "
2185  "nbLoadedFromDb=%d, "
2186  "nbDirectNeighborsInDb=%d, "
2187  "time=%fs (%fs %fs)",
2188  neighborhoodSize,
2189  reactivatedIds.size(),
2190  (int)nbLoadedFromDb,
2191  nbDirectNeighborsInDb,
2192  timeGetN.ticks(),
2193  timeGetNeighborsTimeDb,
2194  timeGetNeighborsSpaceDb);
2195 
2196  }
2197  }
2198 
2199  //============================================================
2200  // RETRIEVAL 2/3 : Update planned path and get next nodes to retrieve
2201  //============================================================
2202  std::list<int> retrievalLocalIds;
2203  if(_rgbdSlamMode)
2204  {
2205  // Priority on locations on the planned path
2206  if(_path.size())
2207  {
2208  updateGoalIndex();
2209 
2210  float distanceSoFar = 0.0f;
2211  // immunize all nodes after current node and
2212  // retrieve nodes after current node in the maximum radius from the current node
2213  for(unsigned int i=_pathCurrentIndex; i<_path.size(); ++i)
2214  {
2215  if(_localRadius > 0.0f && i != _pathCurrentIndex)
2216  {
2217  distanceSoFar += _path[i-1].second.getDistance(_path[i].second);
2218  }
2219 
2220  if(distanceSoFar <= _localRadius)
2221  {
2222  if(_memory->getSignature(_path[i].first) != 0)
2223  {
2224  if(immunizedLocations.insert(_path[i].first).second)
2225  {
2226  ++immunizedLocally;
2227  }
2228  UDEBUG("Path immunization: node %d (dist=%fm)", _path[i].first, distanceSoFar);
2229  }
2230  else if(retrievalLocalIds.size() < _maxLocalRetrieved)
2231  {
2232  UINFO("retrieval of node %d on path (dist=%fm)", _path[i].first, distanceSoFar);
2233  retrievalLocalIds.push_back(_path[i].first);
2234  // retrieved locations are automatically immunized
2235  }
2236  }
2237  else
2238  {
2239  UDEBUG("Stop on node %d (dist=%fm > %fm)",
2240  _path[i].first, distanceSoFar, _localRadius);
2241  break;
2242  }
2243  }
2244  }
2245 
2246  if(!(_memory->allNodesInWM() && maxLocalLocationsImmunized == 0))
2247  {
2248  // immunize the path from the nearest local location to the current location
2249  if(immunizedLocally < maxLocalLocationsImmunized &&
2250  _memory->isIncremental()) // Can only work in mapping mode
2251  {
2252  std::map<int ,Transform> poses;
2253  // remove poses from STM
2254  for(std::map<int, Transform>::iterator iter=_optimizedPoses.begin(); iter!=_optimizedPoses.end(); ++iter)
2255  {
2256  if(iter->first > 0 && !_memory->isInSTM(iter->first))
2257  {
2258  poses.insert(*iter);
2259  }
2260  }
2261  int nearestId = graph::findNearestNode(poses, _optimizedPoses.at(signature->id()));
2262 
2263  if(nearestId > 0 &&
2264  (_localRadius==0 ||
2265  _optimizedPoses.at(signature->id()).getDistance(_optimizedPoses.at(nearestId)) < _localRadius))
2266  {
2267  std::multimap<int, int> links;
2268  for(std::multimap<int, Link>::iterator iter=_constraints.begin(); iter!=_constraints.end(); ++iter)
2269  {
2270  if(uContains(_optimizedPoses, iter->second.from()) && uContains(_optimizedPoses, iter->second.to()))
2271  {
2272  links.insert(std::make_pair(iter->second.from(), iter->second.to()));
2273  links.insert(std::make_pair(iter->second.to(), iter->second.from())); // <->
2274  }
2275  }
2276 
2277  std::list<std::pair<int, Transform> > path = graph::computePath(_optimizedPoses, links, nearestId, signature->id());
2278  if(path.size() == 0)
2279  {
2280  UWARN("Could not compute a path between %d and %d", nearestId, signature->id());
2281  }
2282  else
2283  {
2284  for(std::list<std::pair<int, Transform> >::iterator iter=path.begin();
2285  iter!=path.end();
2286  ++iter)
2287  {
2288  if(iter->first>0)
2289  {
2290  if(immunizedLocally >= maxLocalLocationsImmunized)
2291  {
2292  // set 20 to avoid this warning when starting mapping
2293  if(maxLocalLocationsImmunized > 20 && _someNodesHaveBeenTransferred)
2294  {
2295  UWARN("Could not immunize the whole local path (%d) between "
2296  "%d and %d (max location immunized=%d). You may want "
2297  "to increase RGBD/LocalImmunizationRatio (current=%f (%d of WM=%d)) "
2298  "to be able to immunize longer paths.",
2299  (int)path.size(),
2300  nearestId,
2301  signature->id(),
2302  maxLocalLocationsImmunized,
2303  _localImmunizationRatio,
2304  maxLocalLocationsImmunized,
2305  (int)_memory->getWorkingMem().size());
2306  }
2307  break;
2308  }
2309  else if(!_memory->isInSTM(iter->first))
2310  {
2311  if(immunizedLocations.insert(iter->first).second)
2312  {
2313  ++immunizedLocally;
2314  }
2315  //UDEBUG("local node %d on path immunized=1", iter->first);
2316  }
2317  }
2318  }
2319  }
2320  }
2321  }
2322 
2323  // retrieval based on the nodes close the the nearest pose in WM
2324  // immunize closest nodes
2325  std::map<int, float> nearNodes = graph::findNearestNodes(signature->id(), _optimizedPoses, _localRadius);
2326  // sort by distance
2327  std::multimap<float, int> nearNodesByDist;
2328  for(std::map<int, float>::iterator iter=nearNodes.lower_bound(1); iter!=nearNodes.end(); ++iter)
2329  {
2330  nearNodesByDist.insert(std::make_pair(iter->second, iter->first));
2331  }
2332  UINFO("near nodes=%d, max local immunized=%d, ratio=%f WM=%d",
2333  (int)nearNodesByDist.size(),
2334  maxLocalLocationsImmunized,
2335  _localImmunizationRatio,
2336  (int)_memory->getWorkingMem().size());
2337  for(std::multimap<float, int>::iterator iter=nearNodesByDist.begin();
2338  iter!=nearNodesByDist.end() && (retrievalLocalIds.size() < _maxLocalRetrieved || immunizedLocally < maxLocalLocationsImmunized);
2339  ++iter)
2340  {
2341  const Signature * s = _memory->getSignature(iter->second);
2342  if(s!=0)
2343  {
2344  // If there is a change of direction, better to be retrieving
2345  // ALL nearest signatures than only newest neighbors
2346  const std::multimap<int, Link> & links = s->getLinks();
2347  for(std::multimap<int, Link>::const_reverse_iterator jter=links.rbegin();
2348  jter!=links.rend() && retrievalLocalIds.size() < _maxLocalRetrieved;
2349  ++jter)
2350  {
2351  if(_memory->getSignature(jter->first) == 0)
2352  {
2353  UINFO("retrieval of node %d on local map", jter->first);
2354  retrievalLocalIds.push_back(jter->first);
2355  }
2356  }
2357  if(!_memory->isInSTM(s->id()) && immunizedLocally < maxLocalLocationsImmunized)
2358  {
2359  if(immunizedLocations.insert(s->id()).second)
2360  {
2361  ++immunizedLocally;
2362  }
2363  //UDEBUG("local node %d (%f m) immunized=1", iter->second, iter->first);
2364  }
2365  }
2366  }
2367  // well, if the maximum retrieved is not reached, look for neighbors in database
2368  if(retrievalLocalIds.size() < _maxLocalRetrieved)
2369  {
2370  std::set<int> retrievalLocalIdsSet(retrievalLocalIds.begin(), retrievalLocalIds.end());
2371  for(std::list<int>::iterator iter=retrievalLocalIds.begin();
2372  iter!=retrievalLocalIds.end() && retrievalLocalIds.size() < _maxLocalRetrieved;
2373  ++iter)
2374  {
2375  std::map<int, int> ids = _memory->getNeighborsId(*iter, 2, _maxLocalRetrieved - (unsigned int)retrievalLocalIds.size() + 1, true, false);
2376  for(std::map<int, int>::reverse_iterator jter=ids.rbegin();
2377  jter!=ids.rend() && retrievalLocalIds.size() < _maxLocalRetrieved;
2378  ++jter)
2379  {
2380  if(_memory->getSignature(jter->first) == 0 &&
2381  retrievalLocalIdsSet.find(jter->first) == retrievalLocalIdsSet.end())
2382  {
2383  UINFO("retrieval of node %d on local map", jter->first);
2384  retrievalLocalIds.push_back(jter->first);
2385  retrievalLocalIdsSet.insert(jter->first);
2386  }
2387  }
2388  }
2389  }
2390 
2391  // update Age of the close signatures (oldest the farthest)
2392  for(std::multimap<float, int>::reverse_iterator iter=nearNodesByDist.rbegin(); iter!=nearNodesByDist.rend(); ++iter)
2393  {
2394  _memory->updateAge(iter->second);
2395  }
2396 
2397  // insert them first to make sure they are loaded.
2398  reactivatedIds.insert(reactivatedIds.begin(), retrievalLocalIds.begin(), retrievalLocalIds.end());
2399  }
2400 
2401  //============================================================
2402  // RETRIEVAL 3/3 : Load signatures from the database
2403  //============================================================
2404  if(reactivatedIds.size())
2405  {
2406  // Not important if the loop closure hypothesis don't have all its neighbors loaded,
2407  // only a loop closure link is added...
2408  signaturesRetrieved = _memory->reactivateSignatures(
2409  reactivatedIds,
2410  _maxRetrieved+(unsigned int)retrievalLocalIds.size(), // add path retrieved
2411  timeRetrievalDbAccess);
2412 
2413  ULOGGER_INFO("retrieval of %d (db time = %fs)", (int)signaturesRetrieved.size(), timeRetrievalDbAccess);
2414 
2415  timeRetrievalDbAccess += timeGetNeighborsTimeDb + timeGetNeighborsSpaceDb;
2416  UINFO("total timeRetrievalDbAccess=%fs", timeRetrievalDbAccess);
2417 
2418  // Immunize just retrieved signatures
2419  immunizedLocations.insert(signaturesRetrieved.begin(), signaturesRetrieved.end());
2420 
2421  if(!signaturesRetrieved.empty() && !_globalScanMap.empty())
2422  {
2423  UWARN("Some signatures have been retrieved from memory management, clearing global scan map...");
2424  _globalScanMap.clear();
2425  _globalScanMapPoses.clear();
2426  }
2427  }
2428  timeReactivations = timer.ticks();
2429  ULOGGER_INFO("timeReactivations=%fs", timeReactivations);
2430  }
2431 
2432  //============================================================
2433  // Proximity detections
2434  //============================================================
2435  std::list<std::pair<int, int> > loopClosureLinksAdded;
2436  int loopClosureVisualInliers = 0; // for statistics
2437  float loopClosureVisualInliersRatio = 0.0f;
2438  int loopClosureVisualMatches = 0;
2439  float loopClosureLinearVariance = 0.0f;
2440  float loopClosureAngularVariance = 0.0f;
2441  float loopClosureVisualInliersMeanDist = 0;
2442  float loopClosureVisualInliersDistribution = 0;
2443 
2444  int proximityDetectionsAddedVisually = 0;
2445  int proximityDetectionsAddedByICPMulti = 0;
2446  int proximityDetectionsAddedByICPGlobal = 0;
2447  int lastProximitySpaceClosureId = 0;
2448  int proximitySpacePaths = 0;
2449  int localVisualPathsChecked = 0;
2450  int localScanPathsChecked = 0;
2451  int loopIdSuppressedByProximity = 0;
2452 
2453  if(_proximityBySpace &&
2454  _localRadius > 0 &&
2455  _rgbdSlamMode &&
2456  signature->getWeight() >= 0) // not an intermediate node
2457  {
2458  if(_startNewMapOnLoopClosure &&
2459  _memory->getWorkingMem().size()>=2 && // must have an old map (+1 virtual place)
2460  graph::filterLinks(signature->getLinks(), Link::kSelfRefLink).size() == 0) // alone in new session)
2461  {
2462  UINFO("Proximity detection by space disabled as if we force to have a global loop "
2463  "closure with previous map before doing proximity detections (%s=true).",
2464  Parameters::kRtabmapStartNewMapOnLoopClosure().c_str());
2465  }
2466  else if(_graphOptimizer->iterations() == 0)
2467  {
2468  UWARN("Cannot do local loop closure detection in space if graph optimization is disabled!");
2469  }
2470  else
2471  {
2472  // In localization mode, no need to check local loop
2473  // closures if we are already localized by a landmark.
2474 
2475  // don't do it if it is a small displacement unless the previous signature didn't have a loop closure
2476  // don't do it if there is a too fast movement
2477  if((!smallDisplacement || _memory->getLoopClosureLinks(previousId, false).size() == 0) && !tooFastMovement)
2478  {
2479 
2480  //============================================================
2481  // LOCAL LOOP CLOSURE SPACE
2482  //============================================================
2483 
2484  //
2485  // 1) compare visually with nearest locations
2486  //
2487  UDEBUG("Proximity detection (local loop closure in SPACE using matching images, local radius=%fm)", _localRadius);
2488  std::map<int, float> nearestIds;
2489  if(_memory->isIncremental() && _proximityMaxGraphDepth > 0)
2490  {
2491  nearestIds = _memory->getNeighborsIdRadius(signature->id(), _localRadius, _optimizedPoses, _proximityMaxGraphDepth);
2492  }
2493  else
2494  {
2495  nearestIds = graph::findNearestNodes(signature->id(), _optimizedPoses, _localRadius);
2496  }
2497  UDEBUG("nearestIds=%d/%d", (int)nearestIds.size(), (int)_optimizedPoses.size());
2498  std::map<int, Transform> nearestPoses;
2499  for(std::map<int, float>::iterator iter=nearestIds.lower_bound(1); iter!=nearestIds.end(); ++iter)
2500  {
2501  if(_memory->getStMem().find(iter->first) == _memory->getStMem().end())
2502  {
2503  nearestPoses.insert(std::make_pair(iter->first, _optimizedPoses.at(iter->first)));
2504  }
2505  }
2506  UDEBUG("nearestPoses=%d", (int)nearestPoses.size());
2507 
2508  // segment poses by paths, only one detection per path, landmarks are ignored
2509  std::map<int, std::map<int, Transform> > nearestPathsNotSorted = getPaths(nearestPoses, _optimizedPoses.at(signature->id()), _proximityMaxGraphDepth);
2510  UDEBUG("got %d paths", (int)nearestPathsNotSorted.size());
2511  // sort nearest paths by highest likelihood (if two have same likelihood, sort by id)
2512  std::map<NearestPathKey, std::map<int, Transform> > nearestPaths;
2513  Transform currentPoseInv = _optimizedPoses.at(signature->id());
2514  for(std::map<int, std::map<int, Transform> >::const_iterator iter=nearestPathsNotSorted.begin();iter!=nearestPathsNotSorted.end(); ++iter)
2515  {
2516  const std::map<int, Transform> & path = iter->second;
2517  float highestLikelihood = 0.0f;
2518  int highestLikelihoodId = iter->first;
2519  float smallestDistanceSqr = -1;
2520  for(std::map<int, Transform>::const_iterator jter=path.begin(); jter!=path.end(); ++jter)
2521  {
2522  float v = uValue(likelihood, jter->first, 0.0f);
2523  float distance = (currentPoseInv * jter->second).getNormSquared();
2524  if(v > highestLikelihood || (v == highestLikelihood && (smallestDistanceSqr < 0 || distance < smallestDistanceSqr)))
2525  {
2526  highestLikelihood = v;
2527  highestLikelihoodId = jter->first;
2528  smallestDistanceSqr = distance;
2529  }
2530  }
2531  nearestPaths.insert(std::make_pair(NearestPathKey(highestLikelihood, highestLikelihoodId, smallestDistanceSqr), path));
2532  }
2533  UDEBUG("nearestPaths=%d proximityMaxPaths=%d", (int)nearestPaths.size(), _proximityMaxPaths);
2534 
2535  float proximityFilteringRadius = _proximityFilteringRadius;
2536  if(_maxLoopClosureDistance>0.0f && (proximityFilteringRadius <= 0.0f || _maxLoopClosureDistance<proximityFilteringRadius))
2537  {
2538  proximityFilteringRadius = _maxLoopClosureDistance;
2539  }
2540  for(std::map<NearestPathKey, std::map<int, Transform> >::const_reverse_iterator iter=nearestPaths.rbegin();
2541  iter!=nearestPaths.rend() &&
2542  (_proximityMaxPaths <= 0 || localVisualPathsChecked < _proximityMaxPaths);
2543  ++iter)
2544  {
2545  std::map<int, Transform> path = iter->second;
2546  UASSERT(path.size());
2547 
2548  //find the nearest pose on the path looking in the same direction
2549  path.insert(std::make_pair(signature->id(), _optimizedPoses.at(signature->id())));
2550  path = graph::findNearestPoses(signature->id(), path, _localRadius, _proximityAngle);
2551  //take the one with highest likelihood if not null
2552  int nearestId = 0;
2553  if(iter->first.likelihood > 0.0f &&
2554  path.find(iter->first.id)!=path.end())
2555  {
2556  nearestId = iter->first.id;
2557  }
2558  else
2559  {
2560  nearestId = rtabmap::graph::findNearestNode(path, _optimizedPoses.at(signature->id()));
2561  }
2562 
2563  if(nearestId > 0)
2564  {
2565  // nearest pose must not be linked to current location and enough close
2566  if(!signature->hasLink(nearestId) &&
2567  (proximityFilteringRadius <= 0.0f ||
2568  _optimizedPoses.at(signature->id()).getDistanceSquared(_optimizedPoses.at(nearestId)) < proximityFilteringRadius*proximityFilteringRadius))
2569  {
2570  ++localVisualPathsChecked;
2571  RegistrationInfo info;
2572  Transform guess;
2573  if(_proximityOdomGuess)
2574  {
2575  // Use odometry as guess so that correspondences can be computed by projection
2576  guess = _optimizedPoses.at(nearestId).inverse()*_optimizedPoses.at(signature->id());
2577  } //else: guess is null to make sure visual correspondences are globally computed
2578  Transform transform = _memory->computeTransform(nearestId, signature->id(), guess, &info);
2579  if(!transform.isNull())
2580  {
2581  transform = transform.inverse();
2582  if(proximityFilteringRadius <= 0 || transform.getNormSquared() <= proximityFilteringRadius*proximityFilteringRadius)
2583  {
2584  UINFO("[Visual] Add local loop closure in SPACE (%d->%d) %s",
2585  signature->id(),
2586  nearestId,
2587  transform.prettyPrint().c_str());
2588  UASSERT(info.covariance.at<double>(0,0) > 0.0 && info.covariance.at<double>(5,5) > 0.0);
2589 
2590  //for statistics
2591  loopClosureVisualInliersMeanDist = info.inliersMeanDistance;
2592  loopClosureVisualInliersDistribution = info.inliersDistribution;
2593 
2594  ++proximityDetectionsAddedVisually;
2595  lastProximitySpaceClosureId = nearestId;
2596 
2597  loopClosureVisualInliers = info.inliers;
2598  loopClosureVisualInliersRatio = info.inliersRatio;
2599  loopClosureVisualMatches = info.matches;
2600 
2601  cv::Mat information = getInformation(info.covariance);
2602  loopClosureLinearVariance = 1.0/information.at<double>(0,0);
2603  loopClosureAngularVariance = 1.0/information.at<double>(5,5);
2604 
2605  Link::Type type = Link::kLocalSpaceClosure;
2606  if(_loopClosureHypothesis.first>0 &&
2607  nearestIds.find(_loopClosureHypothesis.first)!=nearestIds.end())
2608  {
2609  // Avoid transform computation on the global loop closure if a visual proximity
2610  // one has been detected close (inside proximity radius) to that hypothesis.
2611  UDEBUG("Proximity detection on %d is close to loop closure %d, ignoring loop closure transform estimation...",
2612  nearestId, _loopClosureHypothesis.first);
2613 
2614  if(nearestId == _loopClosureHypothesis.first)
2615  {
2616  type = Link::kGlobalClosure;
2617  loopIdSuppressedByProximity = nearestId;
2618  }
2619  else if(loopIdSuppressedByProximity == 0)
2620  {
2621  loopIdSuppressedByProximity = nearestId;
2622  }
2623  }
2624 
2625  _memory->addLink(Link(signature->id(), nearestId, type, transform, information));
2626  loopClosureLinksAdded.push_back(std::make_pair(signature->id(), nearestId));
2627  }
2628  else
2629  {
2630  UWARN("Ignoring local loop closure with %d because resulting "
2631  "transform is too large!? (%fm > %fm)",
2632  nearestId, transform.getNorm(), proximityFilteringRadius);
2633  }
2634  }
2635  }
2636  }
2637  }
2638 
2639  timeProximityBySpaceVisualDetection = timer.ticks();
2640  ULOGGER_INFO("timeProximityBySpaceVisualDetection=%fs", timeProximityBySpaceVisualDetection);
2641 
2642  //
2643  // 2) compare locally with nearest locations by scan matching
2644  //
2645  UDEBUG("Proximity detection (local loop closure in SPACE with scan matching)");
2646  if( _proximityMaxNeighbors <= 0)
2647  {
2648  UDEBUG("Proximity by scan matching is disabled (%s=%d).", Parameters::kRGBDProximityPathMaxNeighbors().c_str(), _proximityMaxNeighbors);
2649  }
2650  else if(!signature->sensorData().laserScanCompressed().isEmpty())
2651  {
2652  proximitySpacePaths = (int)nearestPaths.size();
2653  for(std::map<NearestPathKey, std::map<int, Transform> >::const_reverse_iterator iter=nearestPaths.rbegin();
2654  iter!=nearestPaths.rend() &&
2655  (_proximityMaxPaths <= 0 || localScanPathsChecked < _proximityMaxPaths);
2656  ++iter)
2657  {
2658  std::map<int, Transform> path = iter->second; // should contain only nodes (no landmarks)
2659  UASSERT(path.size());
2660  UASSERT(path.begin()->first > 0);
2661 
2662  //find the nearest pose on the path
2663  int nearestId = rtabmap::graph::findNearestNode(path, _optimizedPoses.at(signature->id()));
2664  UASSERT(nearestId > 0);
2665  //UDEBUG("Path %d (size=%d) distance=%fm", nearestId, (int)path.size(), _optimizedPoses.at(signature->id()).getDistance(_optimizedPoses.at(nearestId)));
2666 
2667  // nearest pose must be close and not linked to current location
2668  if(!signature->hasLink(nearestId))
2669  {
2670  if(_proximityMaxNeighbors < _proximityMaxGraphDepth || _proximityMaxGraphDepth == 0)
2671  {
2672  std::map<int, Transform> filteredPath;
2673  int i=0;
2674  std::map<int, Transform>::iterator nearestIdIter = path.find(nearestId);
2675  // "_proximityMaxNeighbors-1" means that if _proximityMaxNeighbors=1,
2676  // only nearest node on the path is taken (no scan merging). Useful to find
2677  // proximity detection between only 2 nodes with 360x360 lidar scans.
2678  for(std::map<int, Transform>::iterator iter=nearestIdIter; iter!=path.end() && i<=_proximityMaxNeighbors-1; ++iter, ++i)
2679  {
2680  filteredPath.insert(*iter);
2681  }
2682  i=1;
2683  for(std::map<int, Transform>::reverse_iterator iter(nearestIdIter); iter!=path.rend() && i<=_proximityMaxNeighbors-1; ++iter, ++i)
2684  {
2685  filteredPath.insert(*iter);
2686  }
2687  path = filteredPath;
2688  }
2689 
2690  // Assemble scans in the path and do ICP only
2691  std::map<int, Transform> optimizedLocalPath;
2692  if(_globalScanMap.empty() && _proximityRawPosesUsed)
2693  {
2694  //optimize the path's poses locally
2695  cv::Mat covariance;
2696  path = optimizeGraph(nearestId, uKeysSet(path), std::map<int, Transform>(), false, covariance);
2697  // transform local poses in optimized graph referential
2698  if(!uContains(path, nearestId))
2699  {
2700  UERROR("Proximity path not containing nearest ID ?! Skipping this path.");
2701  continue;
2702  }
2703  Transform t = _optimizedPoses.at(nearestId) * path.at(nearestId).inverse();
2704 
2705  for(std::map<int, Transform>::iterator jter=path.lower_bound(1); jter!=path.end(); ++jter)
2706  {
2707  optimizedLocalPath.insert(std::make_pair(jter->first, t * jter->second));
2708  }
2709  }
2710  else
2711  {
2712  optimizedLocalPath = path;
2713  }
2714 
2715  std::map<int, Transform> filteredPath;
2716  if(_globalScanMap.empty() && optimizedLocalPath.size() > 2 && proximityFilteringRadius > 0.0f)
2717  {
2718  // path filtering
2719  filteredPath = graph::radiusPosesFiltering(optimizedLocalPath, proximityFilteringRadius, 0, true);
2720  // make sure the current pose is still here
2721  filteredPath.insert(*optimizedLocalPath.find(nearestId));
2722  }
2723  else
2724  {
2725  filteredPath = optimizedLocalPath;
2726  }
2727 
2728  if(filteredPath.size() > 0)
2729  {
2730  // add current node to poses
2731  filteredPath.insert(std::make_pair(signature->id(), _optimizedPoses.at(signature->id())));
2732  //The nearest will be the reference for a loop closure transform
2733  if(signature->getLinks().find(nearestId) == signature->getLinks().end())
2734  {
2735  ++localScanPathsChecked;
2736  RegistrationInfo info;
2737  Transform transform;
2738  bool icpMulti = true;
2739  if(_globalScanMap.empty())
2740  {
2741  transform = _memory->computeIcpTransformMulti(signature->id(), nearestId, filteredPath, &info);
2742  }
2743  else
2744  {
2745  UASSERT_MSG(_globalScanMapPoses.find(nearestId) != _globalScanMapPoses.end(), uFormat("Pose of %d not found in global scan poses", nearestId).c_str());
2746  icpMulti = false;
2747  // use pre-assembled scan map
2748  SensorData assembledData;
2749  assembledData.setId(nearestId);
2750  assembledData.setLaserScan(
2751  LaserScan(_globalScanMap,
2752  signature->sensorData().laserScanCompressed().maxPoints(),
2753  signature->sensorData().laserScanCompressed().rangeMax(),
2754  _globalScanMapPoses.at(nearestId).inverse() * (signature->sensorData().laserScanCompressed().is2d()?Transform(0,0,signature->sensorData().laserScanCompressed().localTransform().z(),0,0,0):Transform::getIdentity())));
2755  Signature nearestNode(assembledData);
2756  Transform guess = filteredPath.at(nearestId).inverse() * filteredPath.at(signature->id());
2757  transform = _memory->computeIcpTransform(nearestNode, *signature, guess, &info);
2758  if(!transform.isNull())
2759  {
2760  transform = transform.inverse();
2761  }
2762  }
2763 
2764  if(!transform.isNull())
2765  {
2766  UINFO("[Scan matching] Add local loop closure in SPACE (%d->%d) %s",
2767  signature->id(),
2768  nearestId,
2769  transform.prettyPrint().c_str());
2770 
2771  cv::Mat scanMatchingIds;
2772  if(_scanMatchingIdsSavedInLinks)
2773  {
2774  std::stringstream stream;
2775  stream << "SCANS:";
2776  for(std::map<int, Transform>::iterator iter=optimizedLocalPath.begin(); iter!=optimizedLocalPath.end(); ++iter)
2777  {
2778  if(iter != optimizedLocalPath.begin())
2779  {
2780  stream << ";";
2781  }
2782  stream << uNumber2Str(iter->first);
2783  }
2784  std::string scansStr = stream.str();
2785  scanMatchingIds = cv::Mat(1, int(scansStr.size()+1), CV_8SC1, (void *)scansStr.c_str());
2786  scanMatchingIds = compressData2(scanMatchingIds); // compressed
2787  }
2788 
2789  // set Identify covariance for laser scan matching only
2790  UASSERT(info.covariance.at<double>(0,0) > 0.0 && info.covariance.at<double>(5,5) > 0.0);
2791  _memory->addLink(Link(signature->id(), nearestId, Link::kLocalSpaceClosure, transform, getInformation(info.covariance)/_proximityMergedScanCovFactor, scanMatchingIds));
2792  loopClosureLinksAdded.push_back(std::make_pair(signature->id(), nearestId));
2793 
2794  if(icpMulti)
2795  {
2796  ++proximityDetectionsAddedByICPMulti;
2797  }
2798  else
2799  {
2800  ++proximityDetectionsAddedByICPGlobal;
2801  }
2802 
2803  // no local loop closure added visually
2804  if(proximityDetectionsAddedVisually == 0)
2805  {
2806  lastProximitySpaceClosureId = nearestId;
2807  }
2808  }
2809  else
2810  {
2811  UINFO("Local scan matching rejected: %s", info.rejectedMsg.c_str());
2812  }
2813  if(!_globalScanMap.empty())
2814  {
2815  break;
2816  }
2817  }
2818  }
2819  }
2820  else
2821  {
2822  //UDEBUG("Path %d ignored", nearestId);
2823  }
2824  }
2825  }
2826  }
2827  }
2828  }
2829  timeProximityBySpaceDetection = timer.ticks();
2830  ULOGGER_INFO("timeProximityBySpaceDetection=%fs", timeProximityBySpaceDetection);
2831 
2832  //=============================================================
2833  // Global loop closure detection
2834  // (updated: place this after retrieval to be sure that neighbors of the loop closure are in RAM)
2835  //=============================================================
2836  if(_loopClosureHypothesis.first>0)
2837  {
2838  if(loopIdSuppressedByProximity==0)
2839  {
2840  //Compute transform if metric data are present
2841  Transform transform;
2842  RegistrationInfo info;
2843  info.covariance = cv::Mat::eye(6,6,CV_64FC1);
2844  if(_rgbdSlamMode)
2845  {
2846  transform = _memory->computeTransform(
2847  _loopClosureHypothesis.first,
2848  signature->id(),
2849  _loopClosureIdentityGuess?Transform::getIdentity():Transform(),
2850  &info);
2851 
2852  loopClosureVisualInliersMeanDist = info.inliersMeanDistance;
2853  loopClosureVisualInliersDistribution = info.inliersDistribution;
2854 
2855  loopClosureVisualInliers = info.inliers;
2856  loopClosureVisualInliersRatio = info.inliersRatio;
2857  loopClosureVisualMatches = info.matches;
2858  rejectedGlobalLoopClosure = transform.isNull();
2859  if(rejectedGlobalLoopClosure)
2860  {
2861  UWARN("Rejected loop closure %d -> %d: %s",
2862  _loopClosureHypothesis.first, signature->id(), info.rejectedMsg.c_str());
2863  }
2864  else if(_maxLoopClosureDistance>0.0f && transform.getNorm() > _maxLoopClosureDistance)
2865  {
2866  rejectedGlobalLoopClosure = true;
2867  UWARN("Rejected localization %d -> %d because distance to map (%fm) is over %s=%fm.",
2868  _loopClosureHypothesis.first, signature->id(), transform.getNorm(), Parameters::kRGBDMaxLoopClosureDistance().c_str(), _maxLoopClosureDistance);
2869  }
2870  else
2871  {
2872  transform = transform.inverse();
2873  }
2874  }
2875  if(!rejectedGlobalLoopClosure)
2876  {
2877  // Make the new one the parent of the old one
2878  UASSERT(info.covariance.at<double>(0,0) > 0.0 && info.covariance.at<double>(5,5) > 0.0);
2879  cv::Mat information = getInformation(info.covariance);
2880  loopClosureLinearVariance = 1.0/information.at<double>(0,0);
2881  loopClosureAngularVariance = 1.0/information.at<double>(5,5);
2882  rejectedGlobalLoopClosure = !_memory->addLink(Link(signature->id(), _loopClosureHypothesis.first, Link::kGlobalClosure, transform, information));
2883  if(!rejectedGlobalLoopClosure)
2884  {
2885  loopClosureLinksAdded.push_back(std::make_pair(signature->id(), _loopClosureHypothesis.first));
2886  }
2887  }
2888 
2889  if(rejectedGlobalLoopClosure)
2890  {
2891  _loopClosureHypothesis.first = 0;
2892  }
2893  }
2894  else if(loopIdSuppressedByProximity != _loopClosureHypothesis.first)
2895  {
2896  _loopClosureHypothesis.first = 0;
2897  }
2898  }
2899 
2900  timeAddLoopClosureLink = timer.ticks();
2901  ULOGGER_INFO("timeAddLoopClosureLink=%fs", timeAddLoopClosureLink);
2902 
2903  //============================================================
2904  // Landmark
2905  //============================================================
2906  std::map<int, std::set<int> > landmarksDetected; // <Landmark ID, list of nodes that saw this landmark>
2907  if(!signature->getLandmarks().empty())
2908  {
2909  bool hasGlobalLoopClosuresInOdomCache = !graph::filterLinks(_odomCacheConstraints, Link::kGlobalClosure, true).empty() || _loopClosureHypothesis.first != 0;
2910  UDEBUG("hasGlobalLoopClosuresInOdomCache=%d", hasGlobalLoopClosuresInOdomCache?1:0);
2911  for(std::map<int, Link>::const_iterator iter=signature->getLandmarks().begin(); iter!=signature->getLandmarks().end(); ++iter)
2912  {
2913  if(uContains(_memory->getLandmarksIndex(), iter->first) &&
2914  _memory->getLandmarksIndex().find(iter->first)->second.size()>1)
2915  {
2916  if(!_memory->isIncremental() && // In localization mode
2917  !hasGlobalLoopClosuresInOdomCache && // If there are global loop closures in odom cache, we can keep far landmarks
2918  _localRadius>0.0 &&
2919  iter->second.transform().getNormSquared() > _localRadius*_localRadius)
2920  {
2921  // Ignore landmark detections over local radius
2922  UWARN("Ignoring landmark %d for localization as it is too far (%fm > %s=%f) "
2923  "and odom cache doesn't contain global loop closure(s).",
2924  iter->first,
2925  iter->second.transform().getNorm(),
2926  Parameters::kRGBDLocalRadius().c_str(),
2927  _localRadius);
2928  }
2929  else
2930  {
2931  UINFO("Landmark %d observed again! Seen the first time by node %d.", -iter->first, *_memory->getLandmarksIndex().find(iter->first)->second.begin());
2932  landmarksDetected.insert(std::make_pair(iter->first, _memory->getLandmarksIndex().find(iter->first)->second));
2933  rejectedGlobalLoopClosure = false; // If it was true, it will be set back to false if landmarks are rejected on graph optimization
2934  }
2935  }
2936  }
2937  }
2938 
2939  //============================================================
2940  // Add virtual links if a path is activated
2941  //============================================================
2942  if(_path.size())
2943  {
2944  // Add a virtual loop closure link to keep the path linked to local map
2945  if( signature->id() != _path[_pathCurrentIndex].first &&
2946  !signature->hasLink(_path[_pathCurrentIndex].first))
2947  {
2948  UASSERT(uContains(_optimizedPoses, signature->id()));
2949  UASSERT_MSG(uContains(_optimizedPoses, _path[_pathCurrentIndex].first), uFormat("id=%d", _path[_pathCurrentIndex].first).c_str());
2950  Transform virtualLoop = _optimizedPoses.at(signature->id()).inverse() * _optimizedPoses.at(_path[_pathCurrentIndex].first);
2951 
2952  if(_localRadius == 0.0f || virtualLoop.getNorm() < _localRadius)
2953  {
2954  _memory->addLink(Link(signature->id(), _path[_pathCurrentIndex].first, Link::kVirtualClosure, virtualLoop, cv::Mat::eye(6,6,CV_64FC1)*0.01)); // set high variance
2955  }
2956  else
2957  {
2958  UERROR("Virtual link larger than local radius (%fm > %fm). Aborting the plan!",
2959  virtualLoop.getNorm(), _localRadius);
2960  this->clearPath(-1);
2961  }
2962  }
2963  }
2964 
2965  //============================================================
2966  // Optimize map graph
2967  //============================================================
2968  float maxLinearError = 0.0f;
2969  float maxLinearErrorRatio = 0.0f;
2970  float maxAngularError = 0.0f;
2971  float maxAngularErrorRatio = 0.0f;
2972  double optimizationError = 0.0;
2973  int optimizationIterations = 0;
2974  cv::Mat localizationCovariance;
2975  Transform previousMapCorrection;
2976  bool rejectedLandmark = false;
2977  bool delayedLocalization = false;
2978  UDEBUG("RGB-D SLAM mode: %d", _rgbdSlamMode?1:0);
2979  UDEBUG("Incremental: %d", _memory->isIncremental());
2980  UDEBUG("Loop hyp: %d", _loopClosureHypothesis.first);
2981  UDEBUG("Last prox: %d", lastProximitySpaceClosureId);
2982  UDEBUG("Reduced ids: %d", (int)statistics_.reducedIds().size());
2983  UDEBUG("Has prior: %d (prior ignored=%d)", signature->hasLink(signature->id(), Link::kPosePrior)?1:0, _graphOptimizer->priorsIgnored()?1:0);
2984  UDEBUG("Has gravity: %d (sigma=%f, odomGravity=%d, refined=%d)", signature->hasLink(signature->id(), Link::kGravity)?1:0, _graphOptimizer->gravitySigma(), _memory->isOdomGravityUsed()?1:0, neighborLinkRefined?1:0);
2985  UDEBUG("Has virtual link: %d", (int)graph::filterLinks(signature->getLinks(), Link::kVirtualClosure, true).size());
2986  UDEBUG("Prox Time: %d", proximityDetectionsInTimeFound);
2987  UDEBUG("Landmarks: %d", (int)landmarksDetected.size());
2988  UDEBUG("Retrieved: %d", (int)signaturesRetrieved.size());
2989  UDEBUG("Not self ref links: %d", (int)graph::filterLinks(signature->getLinks(), Link::kSelfRefLink).size());
2990 
2991  if(_rgbdSlamMode
2992  &&
2993  (_loopClosureHypothesis.first>0 ||
2994  lastProximitySpaceClosureId>0 || // can be different map of the current one
2995  statistics_.reducedIds().size() ||
2996  (signature->hasLink(signature->id(), Link::kPosePrior) && !_graphOptimizer->priorsIgnored()) || // prior edge
2997  (signature->hasLink(signature->id(), Link::kGravity) && _graphOptimizer->gravitySigma()>0.0f && (!_memory->isOdomGravityUsed() || neighborLinkRefined)) || // gravity edge
2998  proximityDetectionsInTimeFound>0 ||
2999  !landmarksDetected.empty() ||
3000  signaturesRetrieved.size()) // can be different map of the current one
3001  &&
3002  (_memory->isIncremental() ||
3003  // In localization mode, the new node should be linked to another node or a landmark already in the working memory
3004  graph::filterLinks(graph::filterLinks(signature->getLinks(), Link::kVirtualClosure), Link::kSelfRefLink).size() ||
3005  !landmarksDetected.empty()))
3006  {
3007  UASSERT(uContains(_optimizedPoses, signature->id()));
3008 
3009  //used in localization mode: filter virtual links
3010  std::multimap<int, Link> localizationLinks = graph::filterLinks(signature->getLinks(), Link::kVirtualClosure);
3011  localizationLinks = graph::filterLinks(localizationLinks, Link::kSelfRefLink);
3012  if(!landmarksDetected.empty() && !_memory->isIncremental())
3013  {
3014  for(std::map<int, std::set<int> >::iterator iter=landmarksDetected.begin(); iter!=landmarksDetected.end(); ++iter)
3015  {
3016  if(_optimizedPoses.find(iter->first)!=_optimizedPoses.end())
3017  {
3018  UASSERT(uContains(signature->getLandmarks(), iter->first));
3019  localizationLinks.insert(std::make_pair(iter->first, signature->getLandmarks().at(iter->first)));
3020  }
3021  }
3022  }
3023 
3024  bool allLocalizationLinksInGraph = !localizationLinks.empty();
3025  for(std::multimap<int, Link>::iterator iter=localizationLinks.begin(); iter!=localizationLinks.end(); ++iter)
3026  {
3027  if(!uContains(_optimizedPoses, iter->first))
3028  {
3029  allLocalizationLinksInGraph = false;
3030  break;
3031  }
3032  }
3033 
3034  // Note that in localization mode, we don't re-optimize the graph
3035  // if:
3036  // 1- there are no signatures retrieved,
3037  // 2- we are relocalizing on a node already in the optimized graph
3038  if(!_memory->isIncremental() &&
3039  signaturesRetrieved.empty() &&
3040  !localizationLinks.empty() &&
3041  allLocalizationLinksInGraph)
3042  {
3043  bool rejectLocalization = _odomCachePoses.empty();
3044  if(!_odomCachePoses.empty())
3045  {
3046  // Verify if the new localization is valid by checking if there is
3047  // not too much deformation using current odometry poses
3048  // This will also refine localization links
3049 
3050  std::map<int, Transform> poses = _odomCachePoses;
3051  std::multimap<int, Link> constraints = _odomCacheConstraints;
3052  // add self referring links (e.g., gravity)
3053  std::multimap<int, Link> selfLinks = graph::filterLinks(signature->getLinks(), Link::kSelfRefLink, true);
3054  if(_graphOptimizer->priorsIgnored())
3055  {
3056  selfLinks = graph::filterLinks(selfLinks, Link::kPosePrior);
3057  }
3058  constraints.insert(selfLinks.begin(), selfLinks.end());
3059  for(std::multimap<int, Link>::iterator iter=localizationLinks.begin(); iter!=localizationLinks.end(); ++iter)
3060  {
3061  constraints.insert(std::make_pair(iter->second.from(), iter->second));
3062  }
3063  for(std::multimap<int, Link>::iterator iter=constraints.begin(); iter!=constraints.end(); ++iter)
3064  {
3065  std::map<int, Transform>::iterator iterPose = _optimizedPoses.find(iter->second.to());
3066  if(iterPose != _optimizedPoses.end() && poses.find(iterPose->first) == poses.end())
3067  {
3068  poses.insert(*iterPose);
3069  // make the poses in the map fixed
3070  constraints.insert(std::make_pair(iterPose->first, Link(iterPose->first, iterPose->first, Link::kPosePrior, iterPose->second, cv::Mat::eye(6,6, CV_64FC1)*1000000)));
3071  UDEBUG("Constraint %d->%d (type=%s)", iterPose->first, iterPose->first, Link::typeName(Link::kPosePrior).c_str());
3072  }
3073  UDEBUG("Constraint %d->%d (type=%s, var = %f %f)", iter->second.from(), iter->second.to(), iter->second.typeName().c_str(), iter->second.transVariance(), iter->second.rotVariance());
3074  }
3075  for(std::map<int, Transform>::iterator iter=poses.begin(); iter!=poses.end(); ++iter)
3076  {
3077  UDEBUG("Pose %d %s", iter->first, iter->second.prettyPrint().c_str());
3078  }
3079 
3080 
3081  std::map<int, Transform> posesOut;
3082  std::multimap<int, Link> edgeConstraintsOut;
3083  bool priorsIgnored = _graphOptimizer->priorsIgnored();
3084  UDEBUG("priorsIgnored was %s", priorsIgnored?"true":"false");
3085  _graphOptimizer->setPriorsIgnored(false); //temporary set false to use priors above to fix nodes of the map
3086  // If slam2d: get connected graph while keeping original roll,pitch,z values.
3087  _graphOptimizer->getConnectedGraph(signature->id(), poses, constraints, posesOut, edgeConstraintsOut, !_graphOptimizer->isSlam2d());
3088  std::map<int, Transform> optPoses = _graphOptimizer->optimize(poses.begin()->first, posesOut, edgeConstraintsOut);
3089  _graphOptimizer->setPriorsIgnored(priorsIgnored); // set back
3090  for(std::map<int, Transform>::iterator iter=optPoses.begin(); iter!=optPoses.end(); ++iter)
3091  {
3092  UDEBUG("Opt %d %s", iter->first, iter->second.prettyPrint().c_str());
3093  }
3094 
3095  if(optPoses.empty())
3096  {
3097  UWARN("Optimization failed, rejecting localization!");
3098  rejectLocalization = true;
3099  }
3100  else if(_optimizationMaxError > 0.0f)
3101  {
3102  UINFO("Compute max graph errors...");
3103  const Link * maxLinearLink = 0;
3104  const Link * maxAngularLink = 0;
3105  graph::computeMaxGraphErrors(
3106  optPoses,
3107  edgeConstraintsOut,
3108  maxLinearErrorRatio,
3109  maxAngularErrorRatio,
3110  maxLinearError,
3111  maxAngularError,
3112  &maxLinearLink,
3113  &maxAngularLink,
3114  _graphOptimizer->isSlam2d());
3115  if(maxLinearLink == 0 && maxAngularLink==0 && _maxOdomCacheSize>0)
3116  {
3117  UWARN("Could not compute graph errors! Wrong loop closures could be accepted!");
3118  optPoses = posesOut;
3119  }
3120 
3121  if(maxLinearLink)
3122  {
3123  UINFO("Max optimization linear error = %f m (link %d->%d, var=%f, ratio error/std=%f, thr=%f)",
3124  maxLinearError,
3125  maxLinearLink->from(),
3126  maxLinearLink->to(),
3127  maxLinearLink->transVariance(),
3128  maxLinearError/sqrt(maxLinearLink->transVariance()),
3129  _optimizationMaxError);
3130  if(maxLinearErrorRatio > _optimizationMaxError)
3131  {
3132  UWARN("Rejecting localization (%d <-> %d) in this "
3133  "iteration because a wrong loop closure has been "
3134  "detected after graph optimization, resulting in "
3135  "a maximum graph error ratio of %f (edge %d->%d, type=%d, abs error=%f m, stddev=%f). The "
3136  "maximum error ratio parameter \"%s\" is %f of std deviation.",
3137  localizationLinks.rbegin()->second.from(),
3138  localizationLinks.rbegin()->second.to(),
3139  maxLinearErrorRatio,
3140  maxLinearLink->from(),
3141  maxLinearLink->to(),
3142  maxLinearLink->type(),
3143  maxLinearError,
3144  sqrt(maxLinearLink->transVariance()),
3145  Parameters::kRGBDOptimizeMaxError().c_str(),
3146  _optimizationMaxError);
3147  rejectLocalization = true;
3148  }
3149  }
3150  if(maxAngularLink)
3151  {
3152  UINFO("Max optimization angular error = %f deg (link %d->%d, var=%f, ratio error/std=%f, thr=%f)",
3153  maxAngularError*180.0f/CV_PI,
3154  maxAngularLink->from(),
3155  maxAngularLink->to(),
3156  maxAngularLink->rotVariance(),
3157  maxAngularError/sqrt(maxAngularLink->rotVariance()),
3158  _optimizationMaxError);
3159  if(maxAngularErrorRatio > _optimizationMaxError)
3160  {
3161  UWARN("Rejecting localization (%d <-> %d) in this "
3162  "iteration because a wrong loop closure has been "
3163  "detected after graph optimization, resulting in "
3164  "a maximum graph error ratio of %f (edge %d->%d, type=%d, abs error=%f deg, stddev=%f). The "
3165  "maximum error ratio parameter \"%s\" is %f of std deviation.",
3166  localizationLinks.rbegin()->second.from(),
3167  localizationLinks.rbegin()->second.to(),
3168  maxAngularErrorRatio,
3169  maxAngularLink->from(),
3170  maxAngularLink->to(),
3171  maxAngularLink->type(),
3172  maxAngularError*180.0f/CV_PI,
3173  sqrt(maxAngularLink->rotVariance()),
3174  Parameters::kRGBDOptimizeMaxError().c_str(),
3175  _optimizationMaxError);
3176  rejectLocalization = true;
3177  }
3178  }
3179  }
3180 
3181  bool hasGlobalLoopClosuresOrLandmarks = false;
3182  if(rejectLocalization && !graph::filterLinks(constraints, Link::kLocalSpaceClosure, true).empty())
3183  {
3184  // Let's try again without local loop closures
3185  localizationLinks = graph::filterLinks(localizationLinks, Link::kLocalSpaceClosure);
3186  constraints = graph::filterLinks(constraints, Link::kLocalSpaceClosure);
3187  for(std::multimap<int, Link>::iterator iter=constraints.begin(); iter!=constraints.end() && !hasGlobalLoopClosuresOrLandmarks; ++iter)
3188  {
3189  hasGlobalLoopClosuresOrLandmarks =
3190  iter->second.type() == Link::kGlobalClosure ||
3191  iter->second.type() == Link::kLandmark;
3192  }
3193  if(hasGlobalLoopClosuresOrLandmarks && !localizationLinks.empty())
3194  {
3195  rejectLocalization = false;
3196  UWARN("Global and loop closures seem not tallying together, try again to optimize without local loop closures...");
3197  priorsIgnored = _graphOptimizer->priorsIgnored();
3198  UDEBUG("priorsIgnored was %s", priorsIgnored?"true":"false");
3199  _graphOptimizer->setPriorsIgnored(false); //temporary set false to use priors above to fix nodes of the map
3200  // If slam2d: get connected graph while keeping original roll,pitch,z values.
3201  _graphOptimizer->getConnectedGraph(signature->id(), poses, constraints, posesOut, edgeConstraintsOut, !_graphOptimizer->isSlam2d());
3202  optPoses = _graphOptimizer->optimize(poses.begin()->first, posesOut, edgeConstraintsOut);
3203  _graphOptimizer->setPriorsIgnored(priorsIgnored); // set back
3204  for(std::map<int, Transform>::iterator iter=optPoses.begin(); iter!=optPoses.end(); ++iter)
3205  {
3206  UDEBUG("Opt2 %d %s", iter->first, iter->second.prettyPrint().c_str());
3207  }
3208 
3209  if(optPoses.empty())
3210  {
3211  UWARN("Optimization failed, rejecting localization!");
3212  rejectLocalization = true;
3213  }
3214  else if(_optimizationMaxError > 0.0f)
3215  {
3216  UINFO("Compute max graph errors...");
3217  const Link * maxLinearLink = 0;
3218  const Link * maxAngularLink = 0;
3219  graph::computeMaxGraphErrors(
3220  optPoses,
3221  edgeConstraintsOut,
3222  maxLinearErrorRatio,
3223  maxAngularErrorRatio,
3224  maxLinearError,
3225  maxAngularError,
3226  &maxLinearLink,
3227  &maxAngularLink,
3228  _graphOptimizer->isSlam2d());
3229  if(maxLinearLink == 0 && maxAngularLink==0 && _maxOdomCacheSize>0)
3230  {
3231  UWARN("Could not compute graph errors! Wrong loop closures could be accepted!");
3232  optPoses = posesOut;
3233  }
3234 
3235  if(maxLinearLink)
3236  {
3237  UINFO("Max optimization linear error = %f m (link %d->%d, var=%f, ratio error/std=%f, thr=%f)",
3238  maxLinearError,
3239  maxLinearLink->from(),
3240  maxLinearLink->to(),
3241  maxLinearLink->transVariance(),
3242  maxLinearError/sqrt(maxLinearLink->transVariance()),
3243  _optimizationMaxError);
3244  if(maxLinearErrorRatio > _optimizationMaxError)
3245  {
3246  UWARN("Rejecting localization (%d <-> %d) in this "
3247  "iteration because a wrong loop closure has been "
3248  "detected after graph optimization, resulting in "
3249  "a maximum graph error ratio of %f (edge %d->%d, type=%d, abs error=%f m, stddev=%f). The "
3250  "maximum error ratio parameter \"%s\" is %f of std deviation.",
3251  localizationLinks.rbegin()->second.from(),
3252  localizationLinks.rbegin()->second.to(),
3253  maxLinearErrorRatio,
3254  maxLinearLink->from(),
3255  maxLinearLink->to(),
3256  maxLinearLink->type(),
3257  maxLinearError,
3258  sqrt(maxLinearLink->transVariance()),
3259  Parameters::kRGBDOptimizeMaxError().c_str(),
3260  _optimizationMaxError);
3261  rejectLocalization = true;
3262  }
3263  }
3264  if(maxAngularLink)
3265  {
3266  UINFO("Max optimization angular error = %f deg (link %d->%d, var=%f, ratio error/std=%f, thr=%f)",
3267  maxAngularError*180.0f/CV_PI,
3268  maxAngularLink->from(),
3269  maxAngularLink->to(),
3270  maxAngularLink->rotVariance(),
3271  maxAngularError/sqrt(maxAngularLink->rotVariance()),
3272  _optimizationMaxError);
3273  if(maxAngularErrorRatio > _optimizationMaxError)
3274  {
3275  UWARN("Rejecting localization (%d <-> %d) in this "
3276  "iteration because a wrong loop closure has been "
3277  "detected after graph optimization, resulting in "
3278  "a maximum graph error ratio of %f (edge %d->%d, type=%d, abs error=%f deg, stddev=%f). The "
3279  "maximum error ratio parameter \"%s\" is %f of std deviation.",
3280  localizationLinks.rbegin()->second.from(),
3281  localizationLinks.rbegin()->second.to(),
3282  maxAngularErrorRatio,
3283  maxAngularLink->from(),
3284  maxAngularLink->to(),
3285  maxAngularLink->type(),
3286  maxAngularError*180.0f/CV_PI,
3287  sqrt(maxAngularLink->rotVariance()),
3288  Parameters::kRGBDOptimizeMaxError().c_str(),
3289  _optimizationMaxError);
3290  rejectLocalization = true;
3291  }
3292  }
3293  }
3294  }
3295  }
3296 
3297  if(!rejectLocalization)
3298  {
3299  if(hasGlobalLoopClosuresOrLandmarks)
3300  {
3301  // We successfully optimize the graph without local loop closures,
3302  // clear them as some of them may be wrong.
3303  size_t before = _odomCacheConstraints.size();
3304  _odomCacheConstraints = graph::filterLinks(_odomCacheConstraints, Link::kLocalSpaceClosure);
3305  if(before != _odomCacheConstraints.size())
3306  {
3307  UWARN("Successfully optimized without local loop closures! Clear them from local odometry cache. %ld/%ld have been removed.",
3308  before - _odomCacheConstraints.size(), before);
3309  }
3310  else
3311  {
3312  UWARN("Successfully optimized without local loop closures!");
3313  }
3314  }
3315 
3316  // Count how many localization links are in the constraints
3317  bool hadAlreadyLocalizationLinks = false;
3318  for(std::multimap<int, Link>::iterator iter=_odomCacheConstraints.begin();
3319  iter!=_odomCacheConstraints.end(); ++iter)
3320  {
3321  if(iter->second.type() == Link::kGlobalClosure ||
3322  iter->second.type() == Link::kLocalSpaceClosure ||
3323  iter->second.type() == Link::kLocalTimeClosure ||
3324  iter->second.type() == Link::kUserClosure ||
3325  iter->second.type() == Link::kNeighborMerged ||
3326  iter->second.type() == Link::kLandmark)
3327  {
3328  hadAlreadyLocalizationLinks = true;
3329  break;
3330  }
3331  }
3332 
3333  // update localization links
3334  Transform newOptPoseInv = optPoses.at(signature->id()).inverse();
3335  for(std::multimap<int, Link>::iterator iter=localizationLinks.begin(); iter!=localizationLinks.end(); ++iter)
3336  {
3337  Transform newT = newOptPoseInv * optPoses.at(iter->first);
3338  UDEBUG("Adjusted localization link %d->%d after optimization", iter->second.from(), iter->second.to());
3339  UDEBUG("from %s", iter->second.transform().prettyPrint().c_str());
3340  UDEBUG(" to %s", newT.prettyPrint().c_str());
3341  iter->second.setTransform(newT);
3342 
3343  // Update link in the referred signatures
3344  if(iter->first > 0)
3345  _memory->updateLink(iter->second, false);
3346 
3347  _odomCacheConstraints.insert(std::make_pair(signature->id(), iter->second));
3348  }
3349 
3350  _odomCacheConstraints.insert(selfLinks.begin(), selfLinks.end());
3351 
3352  // At least 2 localizations at 2 different time required
3353  if(hadAlreadyLocalizationLinks || _maxOdomCacheSize == 0)
3354  {
3355  UINFO("Update localization");
3356  if(_optimizeFromGraphEnd)
3357  {
3358  // update all previous nodes
3359  // Normally _mapCorrection should be identity, but if _optimizeFromGraphEnd
3360  // parameters just changed state, we should put back all poses without map correction.
3361  Transform oldPose = _optimizedPoses.at(localizationLinks.rbegin()->first);
3362  Transform mapCorrectionInv = _mapCorrection.inverse();
3363  Transform u = signature->getPose() * localizationLinks.rbegin()->second.transform();
3364  if(_graphOptimizer->gravitySigma() > 0)
3365  {
3366  // Adjust transform with gravity
3367  Transform transform = localizationLinks.rbegin()->second.transform();
3368  int loopId = localizationLinks.rbegin()->first;
3369  int landmarkId = 0;
3370  if(loopId<0)
3371  {
3372  //For landmarks, use transform against other node looking the landmark
3373  // (because we don't assume that landmarks are aligned with gravity)
3374  landmarkId = loopId;
3375  UASSERT(landmarksDetected.find(landmarkId) != landmarksDetected.end() &&
3376  !landmarksDetected.at(landmarkId).empty());
3377  loopId = *landmarksDetected.at(landmarkId).begin();
3378  }
3379 
3380  const Signature * loopS = _memory->getSignature(loopId);
3381  UASSERT(loopS !=0);
3382  std::multimap<int, Link>::const_iterator iterGravityLoop = graph::findLink(loopS->getLinks(), loopS->id(), loopS->id(), false, Link::kGravity);
3383  std::multimap<int, Link>::const_iterator iterGravitySign = graph::findLink(signature->getLinks(), signature->id(), signature->id(), false, Link::kGravity);
3384  if(iterGravityLoop!=loopS->getLinks().end() &&
3385  iterGravitySign!=signature->getLinks().end())
3386  {
3387  float roll,pitch,yaw;
3388  if(landmarkId < 0)
3389  {
3390  iterGravityLoop->second.transform().getEulerAngles(roll, pitch, yaw);
3391  Transform gravityCorr = Transform(_optimizedPoses.at(loopS->id()).x(),
3392  _optimizedPoses.at(loopS->id()).y(),
3393  _optimizedPoses.at(loopS->id()).z(),
3394  roll, pitch, _optimizedPoses.at(loopS->id()).theta()) * _optimizedPoses.at(loopS->id()).inverse();
3395  (gravityCorr * _optimizedPoses.at(landmarkId)).getEulerAngles(roll,pitch,yaw);
3396  }
3397  else
3398  {
3399  iterGravityLoop->second.transform().getEulerAngles(roll, pitch, yaw);
3400  }
3401  Transform targetRotation = iterGravitySign->second.transform().rotation()*transform.rotation();
3402  targetRotation = Transform(0,0,0,roll,pitch,targetRotation.theta());
3403  Transform error = transform.rotation().inverse() * iterGravitySign->second.transform().rotation().inverse() * targetRotation;
3404  transform *= error;
3405  u = signature->getPose() * transform;
3406  }
3407  else if(iterGravityLoop!=loopS->getLinks().end() ||
3408  iterGravitySign!=signature->getLinks().end())
3409  {
3410  UWARN("Gravity link not found for %d or %d, localization won't be corrected with gravity.", loopId, signature->id());
3411  }
3412  }
3413  Transform up = u * oldPose.inverse();
3414  if(_graphOptimizer->isSlam2d())
3415  {
3416  // in case of 3d landmarks, transform constraint to 2D
3417  up.to3DoF();
3418  }
3419  for(std::map<int, Transform>::iterator iter=_optimizedPoses.begin(); iter!=_optimizedPoses.end(); ++iter)
3420  {
3421  iter->second = mapCorrectionInv * up * iter->second;
3422  }
3423  _optimizedPoses.at(signature->id()) = signature->getPose();
3424  }
3425  else
3426  {
3427  Transform newPose = _optimizedPoses.at(localizationLinks.rbegin()->first) * localizationLinks.rbegin()->second.transform().inverse();
3428  UDEBUG("newPose=%s", newPose.prettyPrint().c_str());
3429  if(_graphOptimizer->isSlam2d() && signature->getPose().is3DoF())
3430  {
3431  // in case of 3d landmarks, transform constraint to 2D
3432  newPose = newPose.to3DoF();
3433  UDEBUG("newPose 2D=%s", newPose.prettyPrint().c_str());
3434  }
3435  else if(_graphOptimizer->gravitySigma() > 0)
3436  {
3437  // Adjust transform with gravity
3438  std::multimap<int, Link>::const_iterator iterGravitySign = graph::findLink(signature->getLinks(), signature->id(), signature->id(), false, Link::kGravity);
3439  if(iterGravitySign!=signature->getLinks().end())
3440  {
3441  Transform transform = localizationLinks.rbegin()->second.transform();
3442  float roll,pitch,yaw;
3443  float tmp1,tmp2;
3444  UDEBUG("Gravity link = %s", iterGravitySign->second.transform().prettyPrint().c_str());
3445  _optimizedPoses.at(localizationLinks.rbegin()->first).getEulerAngles(roll, pitch, yaw);
3446  Transform targetRotation = iterGravitySign->second.transform().rotation()*transform.rotation();
3447  targetRotation = Transform(0,0,0,roll,pitch,targetRotation.theta());
3448  Transform error = transform.rotation().inverse() * iterGravitySign->second.transform().rotation().inverse() * targetRotation;
3449  transform *= error;
3450  newPose = _optimizedPoses.at(localizationLinks.rbegin()->first) * transform.inverse();
3451  iterGravitySign->second.transform().getEulerAngles(roll, pitch, tmp1);
3452  newPose.getEulerAngles(tmp1, tmp2, yaw);
3453  newPose = Transform(newPose.x(), newPose.y(), newPose.z(), roll, pitch, yaw);
3454  UDEBUG("newPose gravity=%s", newPose.prettyPrint().c_str());
3455  }
3456  else if(iterGravitySign!=signature->getLinks().end())
3457  {
3458  UWARN("Gravity link not found for %d, localization won't be corrected with gravity.", signature->id());
3459  }
3460  }
3461  _optimizedPoses.at(signature->id()) = newPose;
3462  }
3463  localizationCovariance = localizationLinks.rbegin()->second.infMatrix().inv();
3464  }
3465  else //delayed localization (wait for more than 1 link)
3466  {
3467  UWARN("Localization was good, but waiting for another one to be more accurate (%s>0)", Parameters::kRGBDMaxOdomCacheSize().c_str());
3468  delayedLocalization = true;
3469  rejectLocalization = true;
3470  }
3471  }
3472  }
3473 
3474  if(rejectLocalization)
3475  {
3476  _loopClosureHypothesis.first = 0;
3477  lastProximitySpaceClosureId = 0;
3478  rejectedGlobalLoopClosure = true;
3479  rejectedLandmark = true;
3480  }
3481  }
3482  else
3483  {
3484  UINFO("Update map correction");
3485  std::map<int, Transform> poses = _optimizedPoses;
3486 
3487  // if _optimizeFromGraphEnd parameter just changed state, don't use optimized poses as guess
3488  if(_optimizeFromGraphEndChanged)
3489  {
3490  UWARN("Optimization: clearing guess poses as %s has changed state, now %s",
3491  Parameters::kRGBDOptimizeFromGraphEnd().c_str(), _optimizeFromGraphEnd?"true":"false");
3492  poses.clear();
3493  _optimizeFromGraphEndChanged = false;
3494  }
3495 
3496  std::multimap<int, Link> constraints;
3497  cv::Mat covariance;
3498  optimizeCurrentMap(signature->id(), false, poses, covariance, &constraints, &optimizationError, &optimizationIterations);
3499 
3500  // Check added loop closures have broken the graph
3501  // (in case of wrong loop closures).
3502  bool updateConstraints = true;
3503  if(poses.empty())
3504  {
3505  UWARN("Graph optimization failed! Rejecting last loop closures added.");
3506  for(std::list<std::pair<int, int> >::iterator iter=loopClosureLinksAdded.begin(); iter!=loopClosureLinksAdded.end(); ++iter)
3507  {
3508  _memory->removeLink(iter->first, iter->second);
3509  UWARN("Loop closure %d->%d rejected!", iter->first, iter->second);
3510  }
3511  updateConstraints = false;
3512  _loopClosureHypothesis.first = 0;
3513  lastProximitySpaceClosureId = 0;
3514  rejectedGlobalLoopClosure = true;
3515  rejectedLandmark = true;
3516  }
3517  else if(_memory->isIncremental() &&
3518  _optimizationMaxError > 0.0f &&
3519  loopClosureLinksAdded.size() &&
3520  optimizationIterations > 0 &&
3521  constraints.size())
3522  {
3523  UINFO("Compute max graph errors...");
3524  const Link * maxLinearLink = 0;
3525  const Link * maxAngularLink = 0;
3526  graph::computeMaxGraphErrors(
3527  poses,
3528  constraints,
3529  maxLinearErrorRatio,
3530  maxAngularErrorRatio,
3531  maxLinearError,
3532  maxAngularError,
3533  &maxLinearLink,
3534  &maxAngularLink);
3535  if(maxLinearLink == 0 && maxAngularLink==0)
3536  {
3537  UWARN("Could not compute graph errors! Wrong loop closures could be accepted!");
3538  }
3539 
3540  bool reject = false;
3541  if(maxLinearLink)
3542  {
3543  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()));
3544  if(maxLinearErrorRatio > _optimizationMaxError)
3545  {
3546  UWARN("Rejecting all added loop closures (%d, first is %d <-> %d) in this "
3547  "iteration because a wrong loop closure has been "
3548  "detected after graph optimization, resulting in "
3549  "a maximum graph error ratio of %f (edge %d->%d, type=%d, abs error=%f m, stddev=%f). The "
3550  "maximum error ratio parameter \"%s\" is %f of std deviation.",
3551  (int)loopClosureLinksAdded.size(),
3552  loopClosureLinksAdded.front().first,
3553  loopClosureLinksAdded.front().second,
3554  maxLinearErrorRatio,
3555  maxLinearLink->from(),
3556  maxLinearLink->to(),
3557  maxLinearLink->type(),
3558  maxLinearError,
3559  sqrt(maxLinearLink->transVariance()),
3560  Parameters::kRGBDOptimizeMaxError().c_str(),
3561  _optimizationMaxError);
3562  reject = true;
3563  }
3564  }
3565  if(maxAngularLink)
3566  {
3567  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()));
3568  if(maxAngularErrorRatio > _optimizationMaxError)
3569  {
3570  UWARN("Rejecting all added loop closures (%d, first is %d <-> %d) in this "
3571  "iteration because a wrong loop closure has been "
3572  "detected after graph optimization, resulting in "
3573  "a maximum graph error ratio of %f (edge %d->%d, type=%d, abs error=%f deg, stddev=%f). The "
3574  "maximum error ratio parameter \"%s\" is %f of std deviation.",
3575  (int)loopClosureLinksAdded.size(),
3576  loopClosureLinksAdded.front().first,
3577  loopClosureLinksAdded.front().second,
3578  maxAngularErrorRatio,
3579  maxAngularLink->from(),
3580  maxAngularLink->to(),
3581  maxAngularLink->type(),
3582  maxAngularError*180.0f/CV_PI,
3583  sqrt(maxAngularLink->rotVariance()),
3584  Parameters::kRGBDOptimizeMaxError().c_str(),
3585  _optimizationMaxError);
3586  reject = true;
3587  }
3588  }
3589 
3590  if(reject)
3591  {
3592  for(std::list<std::pair<int, int> >::iterator iter=loopClosureLinksAdded.begin(); iter!=loopClosureLinksAdded.end(); ++iter)
3593  {
3594  _memory->removeLink(iter->first, iter->second);
3595  UWARN("Loop closure %d->%d rejected!", iter->first, iter->second);
3596  }
3597  updateConstraints = false;
3598  _loopClosureHypothesis.first = 0;
3599  lastProximitySpaceClosureId = 0;
3600  rejectedGlobalLoopClosure = true;
3601  rejectedLandmark = true;
3602  }
3603  }
3604 
3605  if(updateConstraints)
3606  {
3607  UINFO("Updated local map (old size=%d, new size=%d)", (int)_optimizedPoses.size(), (int)poses.size());
3608  _optimizedPoses = poses;
3609  _constraints = constraints;
3610  localizationCovariance = covariance;
3611  }
3612  }
3613 
3614  // Update map correction, it should be identify when optimizing from the last node
3615  UASSERT(_optimizedPoses.find(signature->id()) != _optimizedPoses.end());
3616  if(fakeOdom && _mapCorrectionBackup.isNull())
3617  {
3618  _mapCorrectionBackup = _mapCorrection;
3619  }
3620  previousMapCorrection = _mapCorrection;
3621  _mapCorrection = _optimizedPoses.at(signature->id()) * signature->getPose().inverse();
3622  _lastLocalizationPose = _optimizedPoses.at(signature->id()); // update
3623  if(_mapCorrection.getNormSquared() > 0.1f && _optimizeFromGraphEnd)
3624  {
3625  bool hasPrior = signature->hasLink(signature->id());
3626  if(!_graphOptimizer->priorsIgnored())
3627  {
3628  for(std::multimap<int, Link>::reverse_iterator iter=_constraints.rbegin(); !hasPrior && iter!=_constraints.rend(); ++iter)
3629  {
3630  if(iter->second.type() == Link::kPosePrior)
3631  {
3632  hasPrior = true;
3633  }
3634  }
3635  }
3636  if((!hasPrior || _graphOptimizer->priorsIgnored()) && _graphOptimizer->gravitySigma()==0.0f)
3637  {
3638  UERROR("Map correction should be identity when optimizing from the last node. T=%s", _mapCorrection.prettyPrint().c_str());
3639  }
3640  }
3641  }
3642  int newLocId = _loopClosureHypothesis.first>0?_loopClosureHypothesis.first:lastProximitySpaceClosureId>0?lastProximitySpaceClosureId:0;
3643  _lastLocalizationNodeId = newLocId!=0?newLocId:_lastLocalizationNodeId;
3644  if(newLocId==0 && !landmarksDetected.empty())
3645  {
3646  std::map<int, std::set<int> >::const_iterator iter = _memory->getLandmarksIndex().find(landmarksDetected.begin()->first);
3647  if(iter!=_memory->getLandmarksIndex().end())
3648  {
3649  if(iter->second.size() && *iter->second.begin()!=signature->id())
3650  {
3651  _lastLocalizationNodeId = *iter->second.begin();
3652  }
3653  }
3654  }
3655 
3656  timeMapOptimization = timer.ticks();
3657  ULOGGER_INFO("timeMapOptimization=%fs", timeMapOptimization);
3658 
3659  //============================================================
3660  // Prepare statistics
3661  //============================================================
3662  // Data used for the statistics event and for the log files
3663  int dictionarySize = 0;
3664  int refWordsCount = 0;
3665  int refUniqueWordsCount = 0;
3666  int lcHypothesisReactivated = 0;
3667  float rehearsalValue = uValue(statistics_.data(), Statistics::kMemoryRehearsal_sim(), 0.0f);
3668  int rehearsalMaxId = (int)uValue(statistics_.data(), Statistics::kMemoryRehearsal_merged(), 0.0f);
3669  sLoop = _memory->getSignature(_loopClosureHypothesis.first?_loopClosureHypothesis.first:lastProximitySpaceClosureId?lastProximitySpaceClosureId:_highestHypothesis.first);
3670  if(sLoop)
3671  {
3672  lcHypothesisReactivated = sLoop->isSaved()?1.0f:0.0f;
3673  }
3674  dictionarySize = (int)_memory->getVWDictionary()->getVisualWords().size();
3675  refWordsCount = (int)signature->getWords().size();
3676  refUniqueWordsCount = (int)uUniqueKeys(signature->getWords()).size();
3677 
3678  // Posterior is empty if a bad signature is detected
3679  float vpHypothesis = posterior.size()?posterior.at(Memory::kIdVirtual):0.0f;
3680  int loopId = _loopClosureHypothesis.first>0?_loopClosureHypothesis.first:lastProximitySpaceClosureId;
3681 
3682  // prepare statistics
3683  if(_loopClosureHypothesis.first || _publishStats)
3684  {
3685  ULOGGER_INFO("sending stats...");
3686  statistics_.setRefImageId(_memory->getLastSignatureId()); // Use last id from Memory (in case of rehearsal)
3687  statistics_.setRefImageMapId(signature->mapId());
3688  statistics_.setStamp(data.stamp());
3689  if(_loopClosureHypothesis.first != Memory::kIdInvalid)
3690  {
3691  statistics_.setLoopClosureId(_loopClosureHypothesis.first);
3692  statistics_.setLoopClosureMapId(_memory->getMapId(_loopClosureHypothesis.first));
3693  ULOGGER_INFO("Loop closure detected! With id=%d", _loopClosureHypothesis.first);
3694  }
3695 
3696  if(_publishStats)
3697  {
3698  ULOGGER_INFO("send all stats...");
3699  statistics_.setExtended(1);
3700 
3701  statistics_.addStatistic(Statistics::kLoopAccepted_hypothesis_id(), _loopClosureHypothesis.first);
3702  statistics_.addStatistic(Statistics::kLoopSuppressed_hypothesis_id(), loopIdSuppressedByProximity);
3703  statistics_.addStatistic(Statistics::kLoopHighest_hypothesis_id(), _highestHypothesis.first);
3704  statistics_.addStatistic(Statistics::kLoopHighest_hypothesis_value(), _highestHypothesis.second);
3705  statistics_.addStatistic(Statistics::kLoopHypothesis_reactivated(), lcHypothesisReactivated);
3706  statistics_.addStatistic(Statistics::kLoopVp_hypothesis(), vpHypothesis);
3707  statistics_.addStatistic(Statistics::kLoopReactivate_id(), retrievalId);
3708  statistics_.addStatistic(Statistics::kLoopHypothesis_ratio(), hypothesisRatio);
3709  statistics_.addStatistic(Statistics::kLoopVisual_inliers(), loopClosureVisualInliers);
3710  statistics_.addStatistic(Statistics::kLoopVisual_inliers_ratio(), loopClosureVisualInliersRatio);
3711  statistics_.addStatistic(Statistics::kLoopVisual_matches(), loopClosureVisualMatches);
3712  statistics_.addStatistic(Statistics::kLoopLinear_variance(), loopClosureLinearVariance);
3713  statistics_.addStatistic(Statistics::kLoopAngular_variance(), loopClosureAngularVariance);
3714  statistics_.addStatistic(Statistics::kLoopLast_id(), _memory->getLastGlobalLoopClosureId());
3715  statistics_.addStatistic(Statistics::kLoopOptimization_max_error(), maxLinearError);
3716  statistics_.addStatistic(Statistics::kLoopOptimization_max_error_ratio(), maxLinearErrorRatio);
3717  statistics_.addStatistic(Statistics::kLoopOptimization_max_ang_error(), maxAngularError*180.0f/M_PI);
3718  statistics_.addStatistic(Statistics::kLoopOptimization_max_ang_error_ratio(), maxAngularErrorRatio);
3719  statistics_.addStatistic(Statistics::kLoopOptimization_error(), optimizationError);
3720  statistics_.addStatistic(Statistics::kLoopOptimization_iterations(), optimizationIterations);
3721  statistics_.addStatistic(Statistics::kLoopLandmark_detected(), landmarksDetected.empty()?0:-landmarksDetected.begin()->first);
3722  statistics_.addStatistic(Statistics::kLoopLandmark_detected_node_ref(), landmarksDetected.empty() || landmarksDetected.begin()->second.empty()?0:*landmarksDetected.begin()->second.begin());
3723  statistics_.addStatistic(Statistics::kLoopVisual_inliers_mean_dist(), loopClosureVisualInliersMeanDist);
3724  statistics_.addStatistic(Statistics::kLoopVisual_inliers_distribution(), loopClosureVisualInliersDistribution);
3725 
3726  statistics_.addStatistic(Statistics::kProximityTime_detections(), proximityDetectionsInTimeFound);
3727  statistics_.addStatistic(Statistics::kProximitySpace_detections_added_visually(), proximityDetectionsAddedVisually);
3728  statistics_.addStatistic(Statistics::kProximitySpace_detections_added_icp_multi(), proximityDetectionsAddedByICPMulti);
3729  statistics_.addStatistic(Statistics::kProximitySpace_detections_added_icp_global(), proximityDetectionsAddedByICPGlobal);
3730  statistics_.addStatistic(Statistics::kProximitySpace_paths(), proximitySpacePaths);
3731  statistics_.addStatistic(Statistics::kProximitySpace_visual_paths_checked(), localVisualPathsChecked);
3732  statistics_.addStatistic(Statistics::kProximitySpace_scan_paths_checked(), localScanPathsChecked);
3733  statistics_.addStatistic(Statistics::kProximitySpace_last_detection_id(), lastProximitySpaceClosureId);
3734  statistics_.setProximityDetectionId(lastProximitySpaceClosureId);
3735  statistics_.setProximityDetectionMapId(_memory->getMapId(lastProximitySpaceClosureId));
3736 
3737  statistics_.addStatistic(Statistics::kLoopId(), loopId);
3738  statistics_.addStatistic(Statistics::kLoopMap_id(), (loopId>0 && sLoop)?sLoop->mapId():-1);
3739 
3740  float x,y,z,roll,pitch,yaw;
3741  if(_loopClosureHypothesis.first || lastProximitySpaceClosureId || (!rejectedLandmark && !landmarksDetected.empty()))
3742  {
3743  if(_loopClosureHypothesis.first || lastProximitySpaceClosureId)
3744  {
3745  // Loop closure transform
3746  UASSERT(sLoop);
3747  std::multimap<int, Link>::const_iterator loopIter = sLoop->getLinks().find(signature->id());
3748  UASSERT(loopIter!=sLoop->getLinks().end());
3749  UINFO("Set loop closure transform = %s", loopIter->second.transform().prettyPrint().c_str());
3750  statistics_.setLoopClosureTransform(loopIter->second.transform());
3751 
3752  statistics_.addStatistic(Statistics::kLoopVisual_words(), sLoop->getWords().size());
3753 
3754  // if ground truth exists, compute localization error
3755  if(!sLoop->getGroundTruthPose().isNull() && !signature->getGroundTruthPose().isNull())
3756  {
3757  Transform transformGT = sLoop->getGroundTruthPose().inverse() * signature->getGroundTruthPose();
3758  Transform error = loopIter->second.transform().inverse() * transformGT;
3759  statistics_.addStatistic(Statistics::kGtLocalization_linear_error(), error.getNorm());
3760  statistics_.addStatistic(Statistics::kGtLocalization_angular_error(), error.getAngle(1,0,0)*180/M_PI);
3761  }
3762  }
3763  statistics_.addStatistic(Statistics::kLoopDistance_since_last_loc(), _distanceTravelledSinceLastLocalization);
3764  _distanceTravelledSinceLastLocalization = 0.0f;
3765 
3766  statistics_.addStatistic(Statistics::kLoopMapToOdom_norm(), _mapCorrection.getNorm());
3767  statistics_.addStatistic(Statistics::kLoopMapToOdom_angle(), _mapCorrection.getAngle()*180.0f/M_PI);
3768  _mapCorrection.getTranslationAndEulerAngles(x, y, z, roll, pitch, yaw);
3769  statistics_.addStatistic(Statistics::kLoopMapToOdom_x(), x);
3770  statistics_.addStatistic(Statistics::kLoopMapToOdom_y(), y);
3771  statistics_.addStatistic(Statistics::kLoopMapToOdom_z(), z);
3772  statistics_.addStatistic(Statistics::kLoopMapToOdom_roll(), roll*180.0f/M_PI);
3773  statistics_.addStatistic(Statistics::kLoopMapToOdom_pitch(), pitch*180.0f/M_PI);
3774  statistics_.addStatistic(Statistics::kLoopMapToOdom_yaw(), yaw*180.0f/M_PI);
3775 
3776  // Odom correction (actual odometry pose change), ignore correction from first localization
3777  if(!odomPose.isNull() && !previousMapCorrection.isNull() && !previousMapCorrection.isIdentity())
3778  {
3779  Transform odomCorrection = (previousMapCorrection*odomPose).inverse()*_mapCorrection*odomPose;
3780  statistics_.addStatistic(Statistics::kLoopOdom_correction_norm(), odomCorrection.getNorm());
3781  statistics_.addStatistic(Statistics::kLoopOdom_correction_angle(), odomCorrection.getAngle()*180.0f/M_PI);
3782  odomCorrection.getTranslationAndEulerAngles(x, y, z, roll, pitch, yaw);
3783  statistics_.addStatistic(Statistics::kLoopOdom_correction_x(), x);
3784  statistics_.addStatistic(Statistics::kLoopOdom_correction_y(), y);
3785  statistics_.addStatistic(Statistics::kLoopOdom_correction_z(), z);
3786  statistics_.addStatistic(Statistics::kLoopOdom_correction_roll(), roll*180.0f/M_PI);
3787  statistics_.addStatistic(Statistics::kLoopOdom_correction_pitch(), pitch*180.0f/M_PI);
3788  statistics_.addStatistic(Statistics::kLoopOdom_correction_yaw(), yaw*180.0f/M_PI);
3789 
3790  _odomCorrectionAcc[0]+=x;
3791  _odomCorrectionAcc[1]+=y;
3792  _odomCorrectionAcc[2]+=z;
3793  _odomCorrectionAcc[3]+=roll;
3794  _odomCorrectionAcc[4]+=pitch;
3795  _odomCorrectionAcc[5]+=yaw;
3796  Transform odomCorrectionAcc(
3797  _odomCorrectionAcc[0],
3798  _odomCorrectionAcc[1],
3799  _odomCorrectionAcc[2],
3800  _odomCorrectionAcc[3],
3801  _odomCorrectionAcc[4],
3802  _odomCorrectionAcc[5]);
3803 
3804  statistics_.addStatistic(Statistics::kLoopOdom_correction_acc_norm(), odomCorrectionAcc.getNorm());
3805  statistics_.addStatistic(Statistics::kLoopOdom_correction_acc_angle(), odomCorrectionAcc.getAngle()*180.0f/M_PI);
3806  odomCorrectionAcc.getTranslationAndEulerAngles(x, y, z, roll, pitch, yaw);
3807  statistics_.addStatistic(Statistics::kLoopOdom_correction_acc_x(), x);
3808  statistics_.addStatistic(Statistics::kLoopOdom_correction_acc_y(), y);
3809  statistics_.addStatistic(Statistics::kLoopOdom_correction_acc_z(), z);
3810  statistics_.addStatistic(Statistics::kLoopOdom_correction_acc_roll(), roll*180.0f/M_PI);
3811  statistics_.addStatistic(Statistics::kLoopOdom_correction_acc_pitch(), pitch*180.0f/M_PI);
3812  statistics_.addStatistic(Statistics::kLoopOdom_correction_acc_yaw(), yaw*180.0f/M_PI);
3813  }
3814  }
3815  if(!_lastLocalizationPose.isNull() && !_lastLocalizationPose.isIdentity())
3816  {
3817  _lastLocalizationPose.getTranslationAndEulerAngles(x, y, z, roll, pitch, yaw);
3818  statistics_.addStatistic(Statistics::kLoopMapToBase_x(), x);
3819  statistics_.addStatistic(Statistics::kLoopMapToBase_y(), y);
3820  statistics_.addStatistic(Statistics::kLoopMapToBase_z(), z);
3821  statistics_.addStatistic(Statistics::kLoopMapToBase_roll(), roll*180.0f/M_PI);
3822  statistics_.addStatistic(Statistics::kLoopMapToBase_pitch(), pitch*180.0f/M_PI);
3823  statistics_.addStatistic(Statistics::kLoopMapToBase_yaw(), yaw*180.0f/M_PI);
3824  UINFO("Localization pose = %s", _lastLocalizationPose.prettyPrint().c_str());
3825  }
3826 
3827  statistics_.setMapCorrection(_mapCorrection);
3828  UINFO("Set map correction = %s", _mapCorrection.prettyPrint().c_str());
3829  statistics_.setLocalizationCovariance(localizationCovariance);
3830 
3831  // timings...
3832  statistics_.addStatistic(Statistics::kTimingMemory_update(), timeMemoryUpdate*1000);
3833  statistics_.addStatistic(Statistics::kTimingNeighbor_link_refining(), timeNeighborLinkRefining*1000);
3834  statistics_.addStatistic(Statistics::kTimingProximity_by_time(), timeProximityByTimeDetection*1000);
3835  statistics_.addStatistic(Statistics::kTimingProximity_by_space_visual(), timeProximityBySpaceVisualDetection*1000);
3836  statistics_.addStatistic(Statistics::kTimingProximity_by_space(), timeProximityBySpaceDetection*1000);
3837  statistics_.addStatistic(Statistics::kTimingReactivation(), timeReactivations*1000);
3838  statistics_.addStatistic(Statistics::kTimingAdd_loop_closure_link(), timeAddLoopClosureLink*1000);
3839  statistics_.addStatistic(Statistics::kTimingMap_optimization(), timeMapOptimization*1000);
3840  statistics_.addStatistic(Statistics::kTimingLikelihood_computation(), timeLikelihoodCalculation*1000);
3841  statistics_.addStatistic(Statistics::kTimingPosterior_computation(), timePosteriorCalculation*1000);
3842  statistics_.addStatistic(Statistics::kTimingHypotheses_creation(), timeHypothesesCreation*1000);
3843  statistics_.addStatistic(Statistics::kTimingHypotheses_validation(), timeHypothesesValidation*1000);
3844  statistics_.addStatistic(Statistics::kTimingCleaning_neighbors(), timeCleaningNeighbors*1000);
3845 
3846  // retrieval
3847  statistics_.addStatistic(Statistics::kMemorySignatures_retrieved(), (float)signaturesRetrieved.size());
3848 
3849  // Feature specific parameters
3850  statistics_.addStatistic(Statistics::kKeypointDictionary_size(), dictionarySize);
3851  statistics_.addStatistic(Statistics::kKeypointCurrent_frame(), refWordsCount);
3852  statistics_.addStatistic(Statistics::kKeypointIndexed_words(), _memory->getVWDictionary()->getIndexedWordsCount());
3853  statistics_.addStatistic(Statistics::kKeypointIndex_memory_usage(), _memory->getVWDictionary()->getIndexMemoryUsed());
3854 
3855  //Epipolar geometry constraint
3856  statistics_.addStatistic(Statistics::kLoopRejectedHypothesis(), rejectedGlobalLoopClosure?1.0f:0);
3857 
3858  statistics_.addStatistic(Statistics::kMemorySmall_movement(), smallDisplacement?1.0f:0);
3859  statistics_.addStatistic(Statistics::kMemoryDistance_travelled(), _distanceTravelled);
3860  statistics_.addStatistic(Statistics::kMemoryFast_movement(), tooFastMovement?1.0f:0);
3861  statistics_.addStatistic(Statistics::kMemoryNew_landmark(), addedNewLandmark?1.0f:0);
3862 
3863  if(_publishRAMUsage)
3864  {
3865  UTimer ramTimer;
3866  statistics_.addStatistic(Statistics::kMemoryRAM_usage(), UProcessInfo::getMemoryUsage()/(1024*1024));
3867  long estimatedMemoryUsage = sizeof(Rtabmap);
3868  estimatedMemoryUsage += _optimizedPoses.size() * (sizeof(int) + sizeof(Transform) + 12 * sizeof(float) + sizeof(std::map<int, Transform>::iterator)) + sizeof(std::map<int, Transform>);
3869  estimatedMemoryUsage += _constraints.size() * (sizeof(int) + sizeof(Transform) + 12 * sizeof(float) + sizeof(cv::Mat) + 36 * sizeof(double) + sizeof(std::map<int, Link>::iterator)) + sizeof(std::map<int, Link>);
3870  estimatedMemoryUsage += _memory->getMemoryUsed();
3871  estimatedMemoryUsage += _bayesFilter->getMemoryUsed();
3872  estimatedMemoryUsage += _parameters.size()*(sizeof(std::string)*2+sizeof(ParametersMap::iterator)) + sizeof(ParametersMap);
3873  statistics_.addStatistic(Statistics::kMemoryRAM_estimated(), (float)(estimatedMemoryUsage/(1024*1024)));//MB
3874  statistics_.addStatistic(Statistics::kTimingRAM_estimation(), ramTimer.ticks()*1000);
3875  }
3876 
3877  if(_publishLikelihood || _publishPdf)
3878  {
3879  // Child count by parent signature on the root of the memory ... for statistics
3880  statistics_.setWeights(weights);
3881  if(_publishPdf)
3882  {
3883  statistics_.setPosterior(posterior);
3884  }
3885  if(_publishLikelihood)
3886  {
3887  statistics_.setLikelihood(likelihood);
3888  statistics_.setRawLikelihood(rawLikelihood);
3889  }
3890  }
3891 
3892  statistics_.setLabels(_memory->getAllLabels());
3893 
3894  // Path
3895  if(_path.size())
3896  {
3897  statistics_.setLocalPath(this->getPathNextNodes());
3898  statistics_.setCurrentGoalId(this->getPathCurrentGoalId());
3899  }
3900  }
3901 
3902  timeStatsCreation = timer.ticks();
3903  ULOGGER_INFO("Time creating stats = %f...", timeStatsCreation);
3904  }
3905 
3906  Signature lastSignatureData(signature->id());
3907  Transform lastSignatureLocalizedPose;
3908  if(_optimizedPoses.find(signature->id()) != _optimizedPoses.end())
3909  {
3910  lastSignatureLocalizedPose = _optimizedPoses.at(signature->id());
3911  }
3912  if(_publishLastSignatureData)
3913  {
3914  lastSignatureData = *signature;
3915  }
3916  if(!_rawDataKept)
3917  {
3918  _memory->removeRawData(signature->id(), true, !_neighborLinkRefining && !_proximityBySpace, true);
3919  }
3920 
3921  // Localization mode and saving localization data: save odometry covariance in a prior link
3922  // so that DBReader can republish the covariance of localization data
3923  if(!_memory->isIncremental() && _memory->isLocalizationDataSaved() && !odomCovariance.empty())
3924  {
3925  _memory->addLink(Link(signature->id(), signature->id(), Link::kPosePrior, odomPose, odomCovariance.inv()));
3926  }
3927 
3928  // remove last signature if the memory is not incremental or is a bad signature (if bad signatures are ignored)
3929  int signatureRemoved = _memory->cleanup();
3930  if(signatureRemoved)
3931  {
3932  signaturesRemoved.push_back(signatureRemoved);
3933  }
3934 
3935  // If this option activated, add new nodes only if there are linked with a previous map.
3936  // Used when rtabmap is first started, it will wait a
3937  // global loop closure detection before starting the new map,
3938  // otherwise it deletes the current node.
3939  if(signatureRemoved != lastSignatureData.id())
3940  {
3941  if(_startNewMapOnLoopClosure &&
3942  _memory->isIncremental() && // only in mapping mode
3943  graph::filterLinks(signature->getLinks(), Link::kSelfRefLink).size() == 0 && // alone in the current map
3944  (landmarksDetected.empty() || rejectedLandmark) && // if we re not seeing a landmark from a previous map
3945  _memory->getWorkingMem().size()>=2) // The working memory should not be empty (beside virtual signature)
3946  {
3947  UWARN("Ignoring location %d because a global loop closure is required before starting a new map!",
3948  signature->id());
3949  signaturesRemoved.push_back(signature->id());
3950  _memory->deleteLocation(signature->id());
3951  }
3952  else if(_startNewMapOnGoodSignature &&
3953  (signature->getLandmarks().empty() && signature->isBadSignature()) &&
3954  graph::filterLinks(signature->getLinks(), Link::kSelfRefLink).size() == 0) // alone in the current map
3955  {
3956  UWARN("Ignoring location %d because a good signature (with enough features or with a landmark detected) is required before starting a new map!",
3957  signature->id());
3958  signaturesRemoved.push_back(signature->id());
3959  _memory->deleteLocation(signature->id());
3960  }
3961  else if((smallDisplacement || tooFastMovement) &&
3962  _loopClosureHypothesis.first == 0 &&
3963  lastProximitySpaceClosureId == 0 &&
3964  (rejectedLandmark || landmarksDetected.empty()) &&
3965  !addedNewLandmark)
3966  {
3967  // Don't delete the location if a loop closure is detected
3968  UINFO("Ignoring location %d because the displacement is too small! (d=%f a=%f)",
3969  signature->id(), _rgbdLinearUpdate, _rgbdAngularUpdate);
3970  // If there is a too small displacement, remove the node
3971  signaturesRemoved.push_back(signature->id());
3972  _memory->deleteLocation(signature->id());
3973  }
3974  else
3975  {
3976  _memory->saveLocationData(signature->id());
3977  }
3978  }
3979  else if(!_memory->isIncremental() &&
3980  (smallDisplacement || tooFastMovement) &&
3981  _loopClosureHypothesis.first == 0 &&
3982  lastProximitySpaceClosureId == 0 &&
3983  !delayedLocalization &&
3984  (rejectedLandmark || landmarksDetected.empty()))
3985  {
3986  _odomCachePoses.erase(signatureRemoved);
3987  for(std::multimap<int, Link>::iterator iter=_odomCacheConstraints.begin(); iter!=_odomCacheConstraints.end();)
3988  {
3989  if(iter->second.from() == signatureRemoved || iter->second.to() == signatureRemoved)
3990  {
3991  _odomCacheConstraints.erase(iter++);
3992  }
3993  else
3994  {
3995  ++iter;
3996  }
3997  }
3998  }
3999 
4000  // Pass this point signature should not be used, since it could have been transferred...
4001  signature = 0;
4002 
4003  timeMemoryCleanup = timer.ticks();
4004  ULOGGER_INFO("timeMemoryCleanup = %fs... %d signatures removed", timeMemoryCleanup, (int)signaturesRemoved.size());
4005 
4006 
4007 
4008  //============================================================
4009  // TRANSFER
4010  //============================================================
4011  // If time allowed for the detection exceeds the limit of
4012  // real-time, move the oldest signature with less frequency
4013  // entry (from X oldest) from the short term memory to the
4014  // long term memory.
4015  //============================================================
4016  double totalTime = timerTotal.ticks();
4017  ULOGGER_INFO("Total time processing = %fs...", totalTime);
4018  if((_maxTimeAllowed != 0 && totalTime*1000>_maxTimeAllowed) ||
4019  (_maxMemoryAllowed != 0 && _memory->getWorkingMem().size() > _maxMemoryAllowed))
4020  {
4021  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);
4022  immunizedLocations.insert(_lastLocalizationNodeId); // keep the latest localization in working memory
4023  std::list<int> transferred = _memory->forget(immunizedLocations);
4024  signaturesRemoved.insert(signaturesRemoved.end(), transferred.begin(), transferred.end());
4025  if(!_someNodesHaveBeenTransferred && transferred.size())
4026  {
4027  _someNodesHaveBeenTransferred = true; // only used to hide a warning on close nodes immunization
4028  }
4029  }
4030  _lastProcessTime = totalTime;
4031 
4032  // cleanup cached gps values
4033  for(std::list<int>::iterator iter=signaturesRemoved.begin(); iter!=signaturesRemoved.end() && _gpsGeocentricCache.size(); ++iter)
4034  {
4035  _gpsGeocentricCache.erase(*iter);
4036  }
4037 
4038  //Remove optimized poses from signatures transferred
4039  if(signaturesRemoved.size() && (_optimizedPoses.size() || _constraints.size()))
4040  {
4041  //refresh the local map because some transferred nodes may have broken the tree
4042  int id = 0;
4043  if(!_memory->isIncremental() && (_lastLocalizationNodeId > 0 || _path.size()))
4044  {
4045  if(_path.size())
4046  {
4047  // priority on node on the path
4048  UASSERT(_pathCurrentIndex < _path.size());
4049  UASSERT_MSG(uContains(_optimizedPoses, _path.at(_pathCurrentIndex).first), uFormat("id=%d", _path.at(_pathCurrentIndex).first).c_str());
4050  id = _path.at(_pathCurrentIndex).first;
4051  UDEBUG("Refresh local map from %d", id);
4052  }
4053  else
4054  {
4055  if(uContains(_optimizedPoses, _lastLocalizationNodeId))
4056  {
4057  id = _lastLocalizationNodeId;
4058  UDEBUG("Refresh local map from %d", id);
4059  }
4060  else
4061  {
4062  _lastLocalizationNodeId = 0;
4063  }
4064  }
4065  }
4066  else if(_memory->isIncremental() &&
4067  _optimizedPoses.size() &&
4068  _memory->getLastWorkingSignature())
4069  {
4070  id = _memory->getLastWorkingSignature()->id();
4071  UDEBUG("Refresh local map from %d", id);
4072  }
4073  UDEBUG("id=%d _optimizedPoses=%d", id, (int)_optimizedPoses.size());
4074  if(id > 0)
4075  {
4076  if(_lastLocalizationNodeId != 0)
4077  {
4078  _lastLocalizationNodeId = id;
4079  }
4080  UASSERT_MSG(_memory->getSignature(id) != 0, uFormat("id=%d", id).c_str());
4081 
4082  if(signaturesRemoved.size() == 1 && signaturesRemoved.front() == lastSignatureData.id())
4083  {
4084  int lastId = signaturesRemoved.front();
4085  UDEBUG("Detected that only last signature has been removed (lastId=%d)", lastId);
4086  _optimizedPoses.erase(lastId);
4087  for(std::multimap<int, Link>::iterator iter=_constraints.find(lastId); iter!=_constraints.end() && iter->first==lastId;++iter)
4088  {
4089  if(iter->second.to() != iter->second.from())
4090  {
4091  std::multimap<int, Link>::iterator jter = graph::findLink(_constraints, iter->second.to(), iter->second.from(), false);
4092  if(jter != _constraints.end())
4093  {
4094  _constraints.erase(jter);
4095  }
4096  }
4097  }
4098  _constraints.erase(lastId);
4099  }
4100  else
4101  {
4102 
4103  std::map<int, int> ids = _memory->getNeighborsId(id, 0, 0, true);
4104  for(std::map<int, Transform>::iterator iter=_optimizedPoses.begin(); iter!=_optimizedPoses.end();)
4105  {
4106  if(iter->first > 0 && !uContains(ids, iter->first))
4107  {
4108  UDEBUG("Removed %d from local map", iter->first);
4109  UASSERT(iter->first != _lastLocalizationNodeId);
4110  _optimizedPoses.erase(iter++);
4111 
4112  if(!_globalScanMap.empty())
4113  {
4114  UWARN("optimized poses have been modified, clearing global scan map...");
4115  _globalScanMap.clear();
4116  _globalScanMapPoses.clear();
4117  }
4118  }
4119  else
4120  {
4121  ++iter;
4122  }
4123  }
4124  for(std::multimap<int, Link>::iterator iter=_constraints.begin(); iter!=_constraints.end();)
4125  {
4126  if(iter->first > 0 && (!uContains(ids, iter->second.from()) || !uContains(ids, iter->second.to())))
4127  {
4128  _constraints.erase(iter++);
4129  }
4130  else
4131  {
4132  ++iter;
4133  }
4134  }
4135  }
4136  }
4137  else
4138  {
4139  if(!_optimizedPoses.empty())
4140  UDEBUG("Optimized poses cleared!");
4141  _optimizedPoses.clear();
4142  _constraints.clear();
4143  }
4144  }
4145  // just some verifications to make sure that planning path is still in the local map!
4146  if(_path.size())
4147  {
4148  UASSERT(_pathCurrentIndex < _path.size());
4149  UASSERT(_pathGoalIndex < _path.size());
4150  UASSERT_MSG(uContains(_optimizedPoses, _path.at(_pathCurrentIndex).first), uFormat("local map size=%d, id=%d", (int)_optimizedPoses.size(), _path.at(_pathCurrentIndex).first).c_str());
4151  UASSERT_MSG(uContains(_optimizedPoses, _path.at(_pathGoalIndex).first), uFormat("local map size=%d, id=%d", (int)_optimizedPoses.size(), _path.at(_pathGoalIndex).first).c_str());
4152  }
4153 
4154 
4155  timeRealTimeLimitReachedProcess = timer.ticks();
4156  ULOGGER_INFO("Time limit reached processing = %f...", timeRealTimeLimitReachedProcess);
4157 
4158  //==============================================================
4159  // Finalize statistics and log files
4160  //==============================================================
4161  int localGraphSize = 0;
4162  if(_publishStats)
4163  {
4164  statistics_.addStatistic(Statistics::kTimingStatistics_creation(), timeStatsCreation*1000);
4165  statistics_.addStatistic(Statistics::kTimingTotal(), totalTime*1000);
4166  statistics_.addStatistic(Statistics::kTimingForgetting(), timeRealTimeLimitReachedProcess*1000);
4167  statistics_.addStatistic(Statistics::kTimingJoining_trash(), timeJoiningTrash*1000);
4168  statistics_.addStatistic(Statistics::kTimingEmptying_trash(), timeEmptyingTrash*1000);
4169  statistics_.addStatistic(Statistics::kTimingMemory_cleanup(), timeMemoryCleanup*1000);
4170 
4171  // Transfer
4172  statistics_.addStatistic(Statistics::kMemorySignatures_removed(), signaturesRemoved.size());
4173  statistics_.addStatistic(Statistics::kMemoryImmunized_globally(), immunizedGlobally);
4174  statistics_.addStatistic(Statistics::kMemoryImmunized_locally(), immunizedLocally);
4175  statistics_.addStatistic(Statistics::kMemoryImmunized_locally_max(), maxLocalLocationsImmunized);
4176 
4177  // place after transfer because the memory/local graph may have changed
4178  statistics_.addStatistic(Statistics::kMemoryWorking_memory_size(), _memory->getWorkingMem().size());
4179  statistics_.addStatistic(Statistics::kMemoryShort_time_memory_size(), _memory->getStMem().size());
4180  statistics_.addStatistic(Statistics::kMemoryDatabase_memory_used(), _memory->getDatabaseMemoryUsed());
4181 
4182  // Set local graph
4183  std::map<int, Transform> poses;
4184  std::multimap<int, Link> constraints;
4185  if(!_rgbdSlamMode)
4186  {
4187  UDEBUG("");
4188  // no optimization on appearance-only mode, create a local graph
4189  std::map<int, int> ids = _memory->getNeighborsId(lastSignatureData.id(), 0, 0, true);
4190  _memory->getMetricConstraints(uKeysSet(ids), poses, constraints, false);
4191  }
4192  else // RGBD-SLAM mode
4193  {
4194  poses = _optimizedPoses;
4195  constraints = _constraints;
4196  }
4197  UDEBUG("");
4198  if(_publishLastSignatureData)
4199  {
4200  UINFO("Adding data %d [%d] (rgb/left=%d depth/right=%d)", lastSignatureData.id(), lastSignatureData.mapId(), lastSignatureData.sensorData().imageRaw().empty()?0:1, lastSignatureData.sensorData().depthOrRightRaw().empty()?0:1);
4201  statistics_.addSignatureData(lastSignatureData);
4202 
4203  if(_nodesToRepublish.size())
4204  {
4205  std::multimap<int, int> missingIds;
4206 
4207  // priority to loopId
4208  int tmpId = loopId>0?loopId:_highestHypothesis.first;
4209  if(tmpId>0 && _nodesToRepublish.find(tmpId) != _nodesToRepublish.end())
4210  {
4211  missingIds.insert(std::make_pair(-1, tmpId));
4212  }
4213 
4214  if(!_lastLocalizationPose.isNull())
4215  {
4216  // Republish data from closest nodes of the current localization
4217  std::map<int, Transform> nodesOnly(_optimizedPoses.lower_bound(1), _optimizedPoses.end());
4218  int id = rtabmap::graph::findNearestNode(nodesOnly, _lastLocalizationPose);
4219  if(id>0)
4220  {
4221  std::map<int, int> ids = _memory->getNeighborsId(id, 0, 0, true, false, true);
4222  for(std::map<int, int>::iterator iter=ids.begin(); iter!=ids.end(); ++iter)
4223  {
4224  if(iter->first != loopId &&
4225  _nodesToRepublish.find(iter->first) != _nodesToRepublish.end())
4226  {
4227  missingIds.insert(std::make_pair(iter->second, iter->first));
4228  }
4229  }
4230 
4231  if(_nodesToRepublish.size() != missingIds.size())
4232  {
4233  // remove requested nodes not anymore in the graph
4234  for(std::set<int>::iterator iter=_nodesToRepublish.begin(); iter!=_nodesToRepublish.end();)
4235  {
4236  if(ids.find(*iter) == ids.end())
4237  {
4238  iter = _nodesToRepublish.erase(iter);
4239  }
4240  else
4241  {
4242  ++iter;
4243  }
4244  }
4245  }
4246  }
4247  }
4248 
4249  int loaded = 0;
4250  std::stringstream stream;
4251  for(std::multimap<int, int>::iterator iter=missingIds.begin(); iter!=missingIds.end() && loaded<(int)_maxRepublished; ++iter)
4252  {
4253  statistics_.addSignatureData(getSignatureCopy(iter->second, true, true, true, true, true, true));
4254  _nodesToRepublish.erase(iter->second);
4255  ++loaded;
4256  stream << iter->second << " ";
4257  }
4258  if(loaded)
4259  {
4260  UWARN("Republishing data of requested node(s) %s(%s=%d)",
4261  stream.str().c_str(),
4262  Parameters::kRtabmapMaxRepublished().c_str(),
4263  _maxRepublished);
4264  }
4265  }
4266  }
4267  else
4268  {
4269  // only copy node info
4270  Signature nodeInfo(
4271  lastSignatureData.id(),
4272  lastSignatureData.mapId(),
4273  lastSignatureData.getWeight(),
4274  lastSignatureData.getStamp(),
4275  lastSignatureData.getLabel(),
4276  lastSignatureData.getPose(),
4277  lastSignatureData.getGroundTruthPose());
4278  const std::vector<float> & v = lastSignatureData.getVelocity();
4279  if(v.size() == 6)
4280  {
4281  nodeInfo.setVelocity(v[0], v[1], v[2], v[3], v[4], v[5]);
4282  }
4283  nodeInfo.sensorData().setGPS(lastSignatureData.sensorData().gps());
4284  nodeInfo.sensorData().setEnvSensors(lastSignatureData.sensorData().envSensors());
4285  statistics_.addSignatureData(nodeInfo);
4286  }
4287  UDEBUG("");
4288  localGraphSize = (int)poses.size();
4289  if(!lastSignatureLocalizedPose.isNull())
4290  {
4291  poses.insert(std::make_pair(lastSignatureData.id(), lastSignatureLocalizedPose)); // in case we are in localization
4292  }
4293  statistics_.setPoses(poses);
4294  statistics_.setConstraints(constraints);
4295 
4296  statistics_.addStatistic(Statistics::kMemoryLocal_graph_size(), poses.size());
4297 
4298  statistics_.setOdomCachePoses(_odomCachePoses);
4299  statistics_.setOdomCacheConstraints(_odomCacheConstraints);
4300  statistics_.addStatistic(Statistics::kMemoryOdom_cache_poses(), _odomCachePoses.size());
4301  statistics_.addStatistic(Statistics::kMemoryOdom_cache_links(), _odomCacheConstraints.size());
4302 
4303  if(_computeRMSE && _memory->getGroundTruths().size())
4304  {
4305  UDEBUG("Computing RMSE...");
4306  float translational_rmse = 0.0f;
4307  float translational_mean = 0.0f;
4308  float translational_median = 0.0f;
4309  float translational_std = 0.0f;
4310  float translational_min = 0.0f;
4311  float translational_max = 0.0f;
4312  float rotational_rmse = 0.0f;
4313  float rotational_mean = 0.0f;
4314  float rotational_median = 0.0f;
4315  float rotational_std = 0.0f;
4316  float rotational_min = 0.0f;
4317  float rotational_max = 0.0f;
4318 
4319  graph::calcRMSE(
4320  _memory->getGroundTruths(),
4321  poses,
4322  translational_rmse,
4323  translational_mean,
4324  translational_median,
4325  translational_std,
4326  translational_min,
4327  translational_max,
4328  rotational_rmse,
4329  rotational_mean,
4330  rotational_median,
4331  rotational_std,
4332  rotational_min,
4333  rotational_max);
4334 
4335  statistics_.addStatistic(Statistics::kGtTranslational_rmse(), translational_rmse);
4336  statistics_.addStatistic(Statistics::kGtTranslational_mean(), translational_mean);
4337  statistics_.addStatistic(Statistics::kGtTranslational_median(), translational_median);
4338  statistics_.addStatistic(Statistics::kGtTranslational_std(), translational_std);
4339  statistics_.addStatistic(Statistics::kGtTranslational_min(), translational_min);
4340  statistics_.addStatistic(Statistics::kGtTranslational_max(), translational_max);
4341  statistics_.addStatistic(Statistics::kGtRotational_rmse(), rotational_rmse);
4342  statistics_.addStatistic(Statistics::kGtRotational_mean(), rotational_mean);
4343  statistics_.addStatistic(Statistics::kGtRotational_median(), rotational_median);
4344  statistics_.addStatistic(Statistics::kGtRotational_std(), rotational_std);
4345  statistics_.addStatistic(Statistics::kGtRotational_min(), rotational_min);
4346  statistics_.addStatistic(Statistics::kGtRotational_max(), rotational_max);
4347  UDEBUG("Computing RMSE...done!");
4348  }
4349 
4350  std::vector<int> ids;
4351  ids.reserve(_memory->getWorkingMem().size() + _memory->getStMem().size());
4352  for(std::set<int>::const_iterator iter=_memory->getStMem().begin(); iter!=_memory->getStMem().end(); ++iter)
4353  {
4354  ids.push_back(*iter);
4355  }
4356  for(std::map<int, double>::const_iterator iter=_memory->getWorkingMem().lower_bound(0); iter!=_memory->getWorkingMem().end(); ++iter)
4357  {
4358  ids.push_back(iter->first);
4359  }
4360  statistics_.setWmState(ids);
4361  UDEBUG("wmState=%d", (int)ids.size());
4362  }
4363 
4364  //Save statistics to database
4365  if(_memory->isIncremental() || _memory->isLocalizationDataSaved())
4366  {
4367  _memory->saveStatistics(statistics_, _saveWMState);
4368  }
4369 
4370  //Start trashing
4371  UDEBUG("Empty trash...");
4372  _memory->emptyTrash();
4373 
4374  // Log info...
4375  // TODO : use a specific class which will handle the RtabmapEvent
4376  if(_foutFloat && _foutInt)
4377  {
4378  UDEBUG("Logging...");
4379  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",
4380  totalTime,
4381  timeMemoryUpdate,
4382  timeReactivations,
4383  timeLikelihoodCalculation,
4384  timePosteriorCalculation,
4385  timeHypothesesCreation,
4386  timeHypothesesValidation,
4387  timeRealTimeLimitReachedProcess,
4388  timeStatsCreation,
4389  _highestHypothesis.second,
4390  0.0f,
4391  0.0f,
4392  0.0f,
4393  0.0f,
4394  0.0f,
4395  vpHypothesis,
4396  timeJoiningTrash,
4397  rehearsalValue,
4398  timeEmptyingTrash,
4399  timeRetrievalDbAccess,
4400  timeAddLoopClosureLink,
4401  timeMemoryCleanup,
4402  timeNeighborLinkRefining,
4403  timeProximityByTimeDetection,
4404  timeProximityBySpaceDetection,
4405  timeMapOptimization);
4406  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",
4407  _loopClosureHypothesis.first,
4408  _highestHypothesis.first,
4409  (int)signaturesRemoved.size(),
4410  0,
4411  refWordsCount,
4412  dictionarySize,
4413  int(_memory->getWorkingMem().size()),
4414  rejectedGlobalLoopClosure?1:0,
4415  0,
4416  0,
4417  int(signaturesRetrieved.size()),
4418  lcHypothesisReactivated,
4419  refUniqueWordsCount,
4420  retrievalId,
4421  0,
4422  rehearsalMaxId,
4423  rehearsalMaxId>0?1:0,
4424  localGraphSize,
4425  data.id(),
4426  _memory->getVWDictionary()->getIndexedWordsCount(),
4427  _memory->getVWDictionary()->getIndexMemoryUsed());
4428  if(_statisticLogsBufferedInRAM)
4429  {
4430  _bufferedLogsF.push_back(logF);
4431  _bufferedLogsI.push_back(logI);
4432  }
4433  else
4434  {
4435  if(_foutFloat)
4436  {
4437  fprintf(_foutFloat, "%s", logF.c_str());
4438  }
4439  if(_foutInt)
4440  {
4441  fprintf(_foutInt, "%s", logI.c_str());
4442  }
4443  }
4444  UINFO("Time logging = %f...", timer.ticks());
4445  //ULogger::flush();
4446  }
4447  timeFinalizingStatistics = timer.ticks();
4448  UDEBUG("End process, timeFinalizingStatistics=%fs", timeFinalizingStatistics);
4449  if(_publishStats)
4450  {
4451  statistics_.addStatistic(Statistics::kTimingFinalizing_statistics(), timeFinalizingStatistics*1000);
4452  }
4453 
4454  return true;
4455 }
4456 
4457 // SETTERS
4458 void Rtabmap::setTimeThreshold(float maxTimeAllowed)
4459 {
4460  //must be positive, 0 mean inf time allowed (no time limit)
4461  _maxTimeAllowed = maxTimeAllowed;
4462  if(_maxTimeAllowed < 0)
4463  {
4464  ULOGGER_WARN("maxTimeAllowed < 0, then setting it to 0 (inf).");
4465  _maxTimeAllowed = 0;
4466  }
4467  else if(_maxTimeAllowed > 0.0f && _maxTimeAllowed < 1.0f)
4468  {
4469  ULOGGER_WARN("Time threshold set to %fms, it is not in seconds!", _maxTimeAllowed);
4470  }
4471 }
4472 
4473 void Rtabmap::setWorkingDirectory(std::string path)
4474 {
4475  path = uReplaceChar(path, '~', UDirectory::homeDir());
4476  if(!path.empty() && UDirectory::exists(path))
4477  {
4478  ULOGGER_DEBUG("Comparing new working directory path \"%s\" with \"%s\"", path.c_str(), _wDir.c_str());
4479  if(path.compare(_wDir) != 0)
4480  {
4481  if (_foutFloat || _foutInt)
4482  {
4483  UWARN("Working directory has been changed from \"%s\" with \"%s\", new log files will be created.",
4484  path.c_str(), _wDir.c_str());
4485  }
4486  _wDir = path;
4487  setupLogFiles();
4488  }
4489  }
4490  else if(path.empty())
4491  {
4492  _wDir.clear();
4493  setupLogFiles();
4494  }
4495  else
4496  {
4497  ULOGGER_ERROR("Directory \"%s\" doesn't exist!", path.c_str());
4498  }
4499 }
4500 
4501 void Rtabmap::rejectLastLoopClosure()
4502 {
4503  if(_memory && _memory->getStMem().find(getLastLocationId())!=_memory->getStMem().end())
4504  {
4505  std::multimap<int, Link> links = _memory->getLinks(getLastLocationId(), false);
4506  bool linksRemoved = false;
4507  for(std::multimap<int, Link>::iterator iter = links.begin(); iter!=links.end(); ++iter)
4508  {
4509  if(iter->second.type() == Link::kGlobalClosure ||
4510  iter->second.type() == Link::kLocalSpaceClosure ||
4511  iter->second.type() == Link::kLocalTimeClosure ||
4512  iter->second.type() == Link::kUserClosure)
4513  {
4514  _memory->removeLink(iter->second.from(), iter->second.to());
4515  std::multimap<int, Link>::iterator jter = graph::findLink(_constraints, iter->second.from(), iter->second.to(), true);
4516  if(jter!=_constraints.end())
4517  {
4518  _constraints.erase(jter);
4519  // second time if link is also inverted
4520  jter = graph::findLink(_constraints, iter->second.from(), iter->second.to(), true);
4521  if(jter!=_constraints.end())
4522  {
4523  _constraints.erase(jter);
4524  }
4525  }
4526  linksRemoved = true;
4527  }
4528  }
4529 
4530  if(linksRemoved)
4531  {
4532  _loopClosureHypothesis.first = 0;
4533 
4534  // we have to re-optimize the graph without the rejected links
4535  if(_memory->isIncremental() && _optimizedPoses.size())
4536  {
4537  UINFO("Update graph");
4538  std::map<int, Transform> poses = _optimizedPoses;
4539  std::multimap<int, Link> constraints;
4540  cv::Mat covariance;
4541  optimizeCurrentMap(getLastLocationId(), false, poses, covariance, &constraints);
4542 
4543  if(poses.empty())
4544  {
4545  UWARN("Graph optimization failed after removing loop closure links from last location!");
4546  }
4547  else
4548  {
4549  UINFO("Updated local map (old size=%d, new size=%d)", (int)_optimizedPoses.size(), (int)poses.size());
4550  _optimizedPoses = poses;
4551  _constraints = constraints;
4552  _mapCorrection = _optimizedPoses.at(_memory->getLastWorkingSignature()->id()) * _memory->getLastWorkingSignature()->getPose().inverse();
4553  }
4554  }
4555  }
4556  }
4557 }
4558 
4559 void Rtabmap::deleteLastLocation()
4560 {
4561  if(_memory && _memory->getStMem().size())
4562  {
4563  int lastId = *_memory->getStMem().rbegin();
4564  _memory->deleteLocation(lastId);
4565  // we have to re-optimize the graph without the deleted location
4566  if(_memory->isIncremental() && _optimizedPoses.size())
4567  {
4568  UINFO("Update graph");
4569  _optimizedPoses.erase(lastId);
4570  std::map<int, Transform> poses = _optimizedPoses;
4571  //remove all constraints with last localization id
4572  for(std::multimap<int, Link>::iterator iter=_constraints.begin(); iter!=_constraints.end();)
4573  {
4574  if(iter->second.from() == lastId || iter->second.to() == lastId)
4575  {
4576  _constraints.erase(iter++);
4577  }
4578  else
4579  {
4580  ++iter;
4581  }
4582  }
4583 
4584  if(poses.empty())
4585  {
4586  _mapCorrection.setIdentity();
4587  }
4588  else
4589  {
4590  std::multimap<int, Link> constraints;
4591  cv::Mat covariance;
4592  optimizeCurrentMap(_memory->getLastWorkingSignature()->id(), false, poses, covariance, &constraints);
4593 
4594  if(poses.empty())
4595  {
4596  UWARN("Graph optimization failed after deleting the last location!");
4597  }
4598  else
4599  {
4600  _optimizedPoses = poses;
4601  _constraints = constraints;
4602  _mapCorrection = _optimizedPoses.at(_memory->getLastWorkingSignature()->id()) * _memory->getLastWorkingSignature()->getPose().inverse();
4603  }
4604  }
4605  }
4606  }
4607 }
4608 
4609 void Rtabmap::setOptimizedPoses(const std::map<int, Transform> & poses, const std::multimap<int, Link> & constraints)
4610 {
4611  _optimizedPoses = poses;
4612  _constraints = constraints;
4613 }
4614 
4615 void Rtabmap::dumpData() const
4616 {
4617  UDEBUG("");
4618  if(_memory)
4619  {
4620  if(this->getWorkingDir().empty())
4621  {
4622  UERROR("Working directory not set.");
4623  }
4624  else
4625  {
4626  _memory->dumpMemory(this->getWorkingDir());
4627  }
4628  }
4629 }
4630 
4631 // fromId must be in _memory and in _optimizedPoses
4632 // Get poses in front of the robot, return optimized poses
4633 std::map<int, Transform> Rtabmap::getForwardWMPoses(
4634  int fromId,
4635  int maxNearestNeighbors,
4636  float radius,
4637  int maxGraphDepth // 0 means ignore
4638  ) const
4639 {
4640  std::map<int, Transform> poses;
4641  if(_memory && fromId > 0)
4642  {
4643  UDEBUG("");
4644  const Signature * fromS = _memory->getSignature(fromId);
4645  UASSERT(fromS != 0);
4646  UASSERT(_optimizedPoses.find(fromId) != _optimizedPoses.end());
4647 
4648  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
4649  cloud->resize(_optimizedPoses.size());
4650  std::vector<int> ids(_optimizedPoses.size());
4651  int oi = 0;
4652  const std::set<int> & stm = _memory->getStMem();
4653  //get distances
4654  std::map<int, float> foundIds;
4655  if(_memory->isIncremental())
4656  {
4657  foundIds = _memory->getNeighborsIdRadius(fromId, radius, _optimizedPoses, maxGraphDepth);
4658  }
4659  else
4660  {
4661  foundIds = graph::findNearestNodes(fromId, _optimizedPoses, radius);
4662  }
4663 
4664  float radiusSqrd = radius * radius;
4665  for(std::map<int, Transform>::const_iterator iter = _optimizedPoses.begin(); iter!=_optimizedPoses.end(); ++iter)
4666  {
4667  if(iter->first != fromId)
4668  {
4669  if(stm.find(iter->first) == stm.end() &&
4670  uContains(foundIds, iter->first) &&
4671  (radiusSqrd==0 || foundIds.at(iter->first) <= radiusSqrd))
4672  {
4673  (*cloud)[oi] = pcl::PointXYZ(iter->second.x(), iter->second.y(), iter->second.z());
4674  ids[oi++] = iter->first;
4675  }
4676  }
4677  }
4678 
4679  cloud->resize(oi);
4680  ids.resize(oi);
4681 
4682  Transform fromT = _optimizedPoses.at(fromId);
4683 
4684  if(cloud->size())
4685  {
4686  //if(cloud->size())
4687  //{
4688  // pcl::io::savePCDFile("radiusPoses.pcd", *cloud);
4689  // UWARN("Saved radiusPoses.pcd");
4690  //}
4691 
4692  //filter poses in front of the fromId
4693  float x,y,z, roll,pitch,yaw;
4694  fromT.getTranslationAndEulerAngles(x,y,z, roll,pitch,yaw);
4695 
4696  pcl::CropBox<pcl::PointXYZ> cropbox;
4697  cropbox.setInputCloud(cloud);
4698  cropbox.setMin(Eigen::Vector4f(-1, -radius, -999999, 0));
4699  cropbox.setMax(Eigen::Vector4f(radius, radius, 999999, 0));
4700  cropbox.setRotation(Eigen::Vector3f(roll, pitch, yaw));
4701  cropbox.setTranslation(Eigen::Vector3f(x, y, z));
4702  cropbox.setRotation(Eigen::Vector3f(roll,pitch,yaw));
4703  pcl::IndicesPtr indices(new std::vector<int>());
4704  cropbox.filter(*indices);
4705 
4706  //if(indices->size())
4707  //{
4708  // pcl::io::savePCDFile("radiusCrop.pcd", *cloud, *indices);
4709  // UWARN("Saved radiusCrop.pcd");
4710  //}
4711 
4712  if(indices->size())
4713  {
4714  pcl::search::KdTree<pcl::PointXYZ>::Ptr kdTree(new pcl::search::KdTree<pcl::PointXYZ>);
4715  kdTree->setInputCloud(cloud, indices);
4716  std::vector<int> ind;
4717  std::vector<float> dist;
4718  pcl::PointXYZ pt(fromT.x(), fromT.y(), fromT.z());
4719  kdTree->radiusSearch(pt, radius, ind, dist, maxNearestNeighbors);
4720  //pcl::PointCloud<pcl::PointXYZ> inliers;
4721  for(unsigned int i=0; i<ind.size(); ++i)
4722  {
4723  if(ind[i] >=0)
4724  {
4725  Transform tmp = _optimizedPoses.find(ids[ind[i]])->second;
4726  //inliers.push_back(pcl::PointXYZ(tmp.x(), tmp.y(), tmp.z()));
4727  UDEBUG("Inlier %d: %s", ids[ind[i]], tmp.prettyPrint().c_str());
4728  poses.insert(std::make_pair(ids[ind[i]], tmp));
4729  }
4730  }
4731 
4732  //if(inliers.size())
4733  //{
4734  // pcl::io::savePCDFile("radiusInliers.pcd", inliers);
4735  //}
4736  //if(nearestId >0)
4737  //{
4738  // pcl::PointCloud<pcl::PointXYZ> c;
4739  // Transform ct = _optimizedPoses.find(nearestId)->second;
4740  // c.push_back(pcl::PointXYZ(ct.x(), ct.y(), ct.z()));
4741  // pcl::io::savePCDFile("radiusNearestPt.pcd", c);
4742  //}
4743  }
4744  }
4745  }
4746  return poses;
4747 }
4748 
4749 std::map<int, std::map<int, Transform> > Rtabmap::getPaths(const std::map<int, Transform> & posesIn, const Transform & target, int maxGraphDepth) const
4750 {
4751  std::map<int, std::map<int, Transform> > paths;
4752  std::set<int> nodesSet;
4753  std::map<int, Transform> poses;
4754  for(std::map<int, Transform>::const_iterator iter=posesIn.lower_bound(1); iter!=posesIn.end(); ++iter)
4755  {
4756  nodesSet.insert(iter->first);
4757  poses.insert(*iter);
4758  }
4759  if(_memory && nodesSet.size() && !target.isNull())
4760  {
4761  double e0=0,e1=0,e2=0,e3=0,e4=0;
4762  UTimer t;
4763  e0 = t.ticks();
4764  // Segment poses connected only by neighbor links
4765  while(poses.size())
4766  {
4767  std::map<int, Transform> path;
4768  // select nearest pose and iterate neighbors from there
4769  int nearestId = rtabmap::graph::findNearestNode(poses, target);
4770 
4771  e1+=t.ticks();
4772 
4773  if(nearestId == 0)
4774  {
4775  UWARN("Nearest id of %s in %d poses is 0 !? Returning empty path.", target.prettyPrint().c_str(), (int)poses.size());
4776  break;
4777  }
4778  std::map<int, int> ids = _memory->getNeighborsId(nearestId, maxGraphDepth, 0, true, true, true, true, nodesSet);
4779 
4780  e2+=t.ticks();
4781 
4782  for(std::map<int, int>::iterator iter=ids.begin(); iter!=ids.end(); ++iter)
4783  {
4784  std::map<int, Transform>::iterator jter = poses.find(iter->first);
4785  if(jter != poses.end())
4786  {
4787  bool valid = path.empty();
4788  if(!valid)
4789  {
4790  // make sure it has a neighbor added to path
4791  std::multimap<int, Link> links = _memory->getNeighborLinks(iter->first);
4792  for(std::multimap<int, Link>::iterator kter=links.begin(); kter!=links.end() && !valid; ++kter)
4793  {
4794  valid = path.find(kter->first) != path.end();
4795  }
4796  }
4797 
4798  if(valid)
4799  {
4800  //UDEBUG("%d <- %d", nearestId, jter->first);
4801  path.insert(*jter);
4802  poses.erase(jter);
4803  }
4804  }
4805  }
4806 
4807  e3+=t.ticks();
4808 
4809  if (path.size())
4810  {
4811  if (maxGraphDepth > 0 && !_memory->isGraphReduced() && (int)path.size() > maxGraphDepth * 2 + 1)
4812  {
4813  UWARN("%s=Off but path(%d) > maxGraphDepth(%d)*2+1, nearestId=%d ids=%d. Is reduce graph activated before?",
4814  Parameters::kMemReduceGraph().c_str(), (int)path.size(), maxGraphDepth, nearestId, (int)ids.size());
4815  }
4816  paths.insert(std::make_pair(nearestId, path));
4817  }
4818  else
4819  {
4820  UWARN("path.size()=0!? nearestId=%d ids=%d, aborting...", nearestId, (int)ids.size());
4821  break;
4822  }
4823 
4824  e4+=t.ticks();
4825  }
4826  UDEBUG("e0=%fs e1=%fs e2=%fs e3=%fs e4=%fs", e0, e1, e2, e3, e4);
4827  }
4828  return paths;
4829 }
4830 
4831 void Rtabmap::optimizeCurrentMap(
4832  int id,
4833  bool lookInDatabase,
4834  std::map<int, Transform> & optimizedPoses,
4835  cv::Mat & covariance,
4836  std::multimap<int, Link> * constraints,
4837  double * error,
4838  int * iterationsDone) const
4839 {
4840  //Optimize the map
4841  UINFO("Optimize map: around location %d (lookInDatabase=%s)", id, lookInDatabase?"true":"false");
4842  if(_memory && id > 0)
4843  {
4844  UTimer timer;
4845  std::map<int, int> ids = _memory->getNeighborsId(id, 0, lookInDatabase?-1:0, true, false);
4846  if(!_optimizeFromGraphEnd && ids.size() > 1)
4847  {
4848  id = ids.begin()->first;
4849  }
4850  UINFO("get %d ids time %f s", (int)ids.size(), timer.ticks());
4851 
4852  std::map<int, Transform> poses = Rtabmap::optimizeGraph(id, uKeysSet(ids), optimizedPoses, lookInDatabase, covariance, constraints, error, iterationsDone);
4853  UINFO("optimize time %f s", timer.ticks());
4854 
4855  if(poses.size())
4856  {
4857  optimizedPoses = poses;
4858 
4859  if(_memory->getSignature(id) && uContains(optimizedPoses, id))
4860  {
4861  Transform t = optimizedPoses.at(id) * _memory->getSignature(id)->getPose().inverse();
4862  UINFO("Correction (from node %d) %s", id, t.prettyPrint().c_str());
4863  }
4864  }
4865  else
4866  {
4867  UWARN("Failed to optimize the graph! returning empty optimized poses...");
4868  optimizedPoses.clear();
4869  if(constraints)
4870  {
4871  constraints->clear();
4872  }
4873  }
4874  }
4875 }
4876 
4877 std::map<int, Transform> Rtabmap::optimizeGraph(
4878  int fromId,
4879  const std::set<int> & ids,
4880  const std::map<int, Transform> & guessPoses,
4881  bool lookInDatabase,
4882  cv::Mat & covariance,
4883  std::multimap<int, Link> * constraints,
4884  double * error,
4885  int * iterationsDone) const
4886 {
4887  UTimer timer;
4888  std::map<int, Transform> optimizedPoses;
4889  std::map<int, Transform> poses;
4890  std::multimap<int, Link> edgeConstraints;
4891  UDEBUG("ids=%d", (int)ids.size());
4892  _memory->getMetricConstraints(ids, poses, edgeConstraints, lookInDatabase, !_graphOptimizer->landmarksIgnored());
4893  UINFO("get constraints (ids=%d, %d poses, %d edges) time %f s", (int)ids.size(), (int)poses.size(), (int)edgeConstraints.size(), timer.ticks());
4894 
4895  // add landmark priors if there are some
4896  for(std::map<int, Transform>::iterator iter=poses.begin(); iter!=poses.end() && iter->first < 0; ++iter)
4897  {
4898  if(_markerPriors.find(iter->first) != _markerPriors.end())
4899  {
4900  cv::Mat infMatrix = cv::Mat::eye(6, 6, CV_64FC1);
4901  infMatrix(cv::Range(0,3), cv::Range(0,3)) /= _markerPriorsLinearVariance;
4902  infMatrix(cv::Range(3,6), cv::Range(3,6)) /= _markerPriorsAngularVariance;
4903  edgeConstraints.insert(std::make_pair(iter->first, Link(iter->first, iter->first, Link::kPosePrior, _markerPriors.at(iter->first), infMatrix)));
4904  UDEBUG("Added prior %d : %s (variance: lin=%f ang=%f)", iter->first, _markerPriors.at(iter->first).prettyPrint().c_str(),
4905  _markerPriorsLinearVariance, _markerPriorsAngularVariance);
4906  }
4907  }
4908 
4909  if(_graphOptimizer->iterations() > 0)
4910  {
4911  for(std::map<int, Transform>::iterator iter=poses.begin(); iter!=poses.end(); ++iter)
4912  {
4913  // Apply guess poses (if some), ignore for rootid to avoid origin drifting
4914  std::map<int, Transform>::const_iterator foundGuess = guessPoses.find(iter->first);
4915  if(foundGuess!=guessPoses.end() && iter->first != fromId)
4916  {
4917  iter->second = foundGuess->second;
4918  }
4919  }
4920  }
4921 
4922 
4923  UASSERT(_graphOptimizer!=0);
4924  if(_graphOptimizer->iterations() == 0)
4925  {
4926  // Optimization disabled! Return not optimized poses.
4927  optimizedPoses = poses;
4928  if(constraints)
4929  {
4930  *constraints = edgeConstraints;
4931  }
4932  }
4933  else
4934  {
4935  bool hasLandmarks = !edgeConstraints.empty() && edgeConstraints.begin()->first < 0;
4936  if(poses.size() != guessPoses.size() || hasLandmarks)
4937  {
4938  UDEBUG("recompute poses using only links (robust to multi-session)");
4939  std::map<int, Transform> posesOut;
4940  std::multimap<int, Link> edgeConstraintsOut;
4941  _graphOptimizer->getConnectedGraph(fromId, poses, edgeConstraints, posesOut, edgeConstraintsOut);
4942  optimizedPoses = _graphOptimizer->optimize(fromId, posesOut, edgeConstraintsOut, covariance, 0, error, iterationsDone);
4943  if(constraints)
4944  {
4945  *constraints = edgeConstraintsOut;
4946  }
4947  }
4948  else
4949  {
4950  UDEBUG("use input guess poses");
4951  optimizedPoses = _graphOptimizer->optimize(fromId, poses, edgeConstraints, covariance, 0, error, iterationsDone);
4952  if(constraints)
4953  {
4954  *constraints = edgeConstraints;
4955  }
4956  }
4957 
4958  if(!poses.empty() && optimizedPoses.empty())
4959  {
4960  UWARN("Optimization has failed (poses=%d, guess=%d, links=%d)...",
4961  (int)poses.size(), (int)guessPoses.size(), (int)edgeConstraints.size());
4962  }
4963  }
4964 
4965  UINFO("Optimization time %f s", timer.ticks());
4966 
4967  return optimizedPoses;
4968 }
4969 
4970 void Rtabmap::adjustLikelihood(std::map<int, float> & likelihood) const
4971 {
4972  ULOGGER_DEBUG("likelihood.size()=%d", likelihood.size());
4973  UTimer timer;
4974  timer.start();
4975  if(likelihood.size()==0)
4976  {
4977  return;
4978  }
4979 
4980  // Use only non-null values (ignore virtual place)
4981  std::list<float> values;
4982  bool likelihoodNullValuesIgnored = true;
4983  for(std::map<int, float>::iterator iter = ++likelihood.begin(); iter!=likelihood.end(); ++iter)
4984  {
4985  if((iter->second >= 0 && !likelihoodNullValuesIgnored) ||
4986  (iter->second > 0 && likelihoodNullValuesIgnored))
4987  {
4988  values.push_back(iter->second);
4989  }
4990  }
4991  UDEBUG("values.size=%d", values.size());
4992 
4993  float mean = uMean(values);
4994  float stdDev = std::sqrt(uVariance(values, mean));
4995 
4996 
4997  //Adjust likelihood with mean and standard deviation (see Angeli phd)
4998  float epsilon = 0.0001;
4999  float max = 0.0f;
5000  int maxId = 0;
5001  for(std::map<int, float>::iterator iter=++likelihood.begin(); iter!= likelihood.end(); ++iter)
5002  {
5003  float value = iter->second;
5004  if(value > mean+stdDev && mean)
5005  {
5006  iter->second = (value-(stdDev-epsilon))/mean;
5007  if(value > max)
5008  {
5009  max = value;
5010  maxId = iter->first;
5011  }
5012  }
5013  else if(value == 1.0f && stdDev == 0)
5014  {
5015  iter->second = 1.0f;
5016  if(value > max)
5017  {
5018  max = value;
5019  maxId = iter->first;
5020  }
5021  }
5022  else
5023  {
5024  iter->second = 1.0f;
5025  }
5026  }
5027 
5028  if(stdDev > epsilon && max)
5029  {
5030  likelihood.begin()->second = mean/stdDev + 1.0f;
5031  }
5032  else
5033  {
5034  likelihood.begin()->second = 2.0f; //2 * std dev
5035  }
5036 
5037  double time = timer.ticks();
5038  UDEBUG("mean=%f, stdDev=%f, max=%f, maxId=%d, time=%fs", mean, stdDev, max, maxId, time);
5039 }
5040 
5041 void Rtabmap::dumpPrediction() const
5042 {
5043  if(_memory && _bayesFilter)
5044  {
5045  if(this->getWorkingDir().empty())
5046  {
5047  UERROR("Working directory not set.");
5048  return;
5049  }
5050  std::list<int> signaturesToCompare;
5051  for(std::map<int, double>::const_iterator iter=_memory->getWorkingMem().begin();
5052  iter!=_memory->getWorkingMem().end();
5053  ++iter)
5054  {
5055  if(iter->first > 0)
5056  {
5057  const Signature * s = _memory->getSignature(iter->first);
5058  UASSERT(s!=0);
5059  if(s->getWeight() != -1) // ignore intermediate nodes
5060  {
5061  signaturesToCompare.push_back(iter->first);
5062  }
5063  }
5064  else
5065  {
5066  // virtual signature should be added
5067  signaturesToCompare.push_back(iter->first);
5068  }
5069  }
5070  cv::Mat prediction = _bayesFilter->generatePrediction(_memory, uListToVector(signaturesToCompare));
5071 
5072  FILE* fout = 0;
5073  std::string fileName = this->getWorkingDir() + "/DumpPrediction.txt";
5074  #ifdef _MSC_VER
5075  fopen_s(&fout, fileName.c_str(), "w");
5076  #else
5077  fout = fopen(fileName.c_str(), "w");
5078  #endif
5079 
5080  if(fout)
5081  {
5082  for(int i=0; i<prediction.rows; ++i)
5083  {
5084  for(int j=0; j<prediction.cols; ++j)
5085  {
5086  fprintf(fout, "%f ",((float*)prediction.data)[j + i*prediction.cols]);
5087  }
5088  fprintf(fout, "\n");
5089  }
5090  fclose(fout);
5091  }
5092  }
5093  else
5094  {
5095  UWARN("Memory and/or the Bayes filter are not created");
5096  }
5097 }
5098 
5099 Signature Rtabmap::getSignatureCopy(int id, bool images, bool scan, bool userData, bool occupancyGrid, bool withWords, bool withGlobalDescriptors) const
5100 {
5101  Signature s;
5102  if(_memory)
5103  {
5104  Transform odomPoseLocal;
5105  int weight = -1;
5106  int mapId = -1;
5107  std::string label;
5108  double stamp = 0;
5109  Transform groundTruth;
5110  std::vector<float> velocity;
5111  GPS gps;
5112  EnvSensors sensors;
5113  _memory->getNodeInfo(id, odomPoseLocal, mapId, weight, label, stamp, groundTruth, velocity, gps, sensors, true);
5114  SensorData data;
5115  data.setId(id);
5116  if(images || scan || userData || occupancyGrid)
5117  {
5118  data = _memory->getNodeData(id, images, scan, userData, occupancyGrid);
5119  }
5120  if(!images && withWords)
5121  {
5122  std::vector<CameraModel> models;
5123  std::vector<StereoCameraModel> stereoModels;
5124  _memory->getNodeCalibration(id, models, stereoModels);
5125  data.setCameraModels(models);
5126  data.setStereoCameraModels(stereoModels);
5127  }
5128 
5129  s=Signature(id,
5130  mapId,
5131  weight,
5132  stamp,
5133  label,
5134  odomPoseLocal,
5135  groundTruth,
5136  data);
5137 
5138  std::multimap<int, Link> links = _memory->getLinks(id, true, true);
5139  for(std::multimap<int, Link>::iterator iter=links.begin(); iter!=links.end(); ++iter)
5140  {
5141  if(iter->second.type() == Link::kLandmark)
5142  {
5143  s.addLandmark(iter->second);
5144  }
5145  else
5146  {
5147  s.addLink(iter->second);
5148  }
5149  }
5150 
5151  if(withWords || withGlobalDescriptors)
5152  {
5153  std::multimap<int, int> words;
5154  std::vector<cv::KeyPoint> wordsKpts;
5155  std::vector<cv::Point3f> words3;
5156  cv::Mat wordsDescriptors;
5157  std::vector<rtabmap::GlobalDescriptor> globalDescriptors;
5158  _memory->getNodeWordsAndGlobalDescriptors(id, words, wordsKpts, words3, wordsDescriptors, globalDescriptors);
5159  if(withWords)
5160  {
5161  s.setWords(words, wordsKpts, words3, wordsDescriptors);
5162  }
5163  if(withGlobalDescriptors)
5164  {
5165  s.sensorData().setGlobalDescriptors(globalDescriptors);
5166  }
5167  }
5168  if(velocity.size()==6)
5169  {
5170  s.setVelocity(velocity[0], velocity[1], velocity[2], velocity[3], velocity[4], velocity[5]);
5171  }
5172  s.sensorData().setGPS(gps);
5173  s.sensorData().setEnvSensors(sensors);
5174  }
5175  return s;
5176 }
5177 
5178 void Rtabmap::get3DMap(
5179  std::map<int, Signature> & signatures,
5180  std::map<int, Transform> & poses,
5181  std::multimap<int, Link> & constraints,
5182  bool optimized,
5183  bool global) const
5184 {
5185  UDEBUG("");
5186  return getGraph(poses, constraints, optimized, global, &signatures, true, true, true, true);
5187 }
5188 
5189 void Rtabmap::getGraph(
5190  std::map<int, Transform> & poses,
5191  std::multimap<int, Link> & constraints,
5192  bool optimized,
5193  bool global,
5194  std::map<int, Signature> * signatures,
5195  bool withImages,
5196  bool withScan,
5197  bool withUserData,
5198  bool withGrid,
5199  bool withWords,
5200  bool withGlobalDescriptors) const
5201 {
5202  if(_memory && _memory->getLastWorkingSignature())
5203  {
5204  if(_rgbdSlamMode)
5205  {
5206  if(optimized)
5207  {
5208  poses = _optimizedPoses; // guess
5209  cv::Mat covariance;
5210  this->optimizeCurrentMap(_memory->getLastWorkingSignature()->id(), global, poses, covariance, &constraints);
5211  if(!global && !_optimizedPoses.empty())
5212  {
5213  // We send directly the already optimized poses if they are set
5214  UDEBUG("_optimizedPoses=%ld poses=%ld", _optimizedPoses.size(), poses.size());
5215  poses = _optimizedPoses;
5216  }
5217  }
5218  else
5219  {
5220  std::map<int, int> ids = _memory->getNeighborsId(_memory->getLastWorkingSignature()->id(), 0, global?-1:0, true);
5221  _memory->getMetricConstraints(uKeysSet(ids), poses, constraints, global);
5222  }
5223  }
5224  else
5225  {
5226  // no optimization on appearance-only mode
5227  std::map<int, int> ids = _memory->getNeighborsId(_memory->getLastWorkingSignature()->id(), 0, global?-1:0, true);
5228  _memory->getMetricConstraints(uKeysSet(ids), poses, constraints, global);
5229  }
5230 
5231  if(signatures)
5232  {
5233  // Get data
5234  std::set<int> ids = uKeysSet(_memory->getWorkingMem()); // WM
5235 
5236  //remove virtual signature
5237  ids.erase(Memory::kIdVirtual);
5238 
5239  ids.insert(_memory->getStMem().begin(), _memory->getStMem().end()); // STM + WM
5240  if(global)
5241  {
5242  ids = _memory->getAllSignatureIds(); // STM + WM + LTM, ignoreChildren=true
5243  }
5244 
5245  for(std::set<int>::iterator iter = ids.begin(); iter!=ids.end(); ++iter)
5246  {
5247  signatures->insert(std::make_pair(*iter, getSignatureCopy(*iter, withImages, withScan, withUserData, withGrid, withWords, withGlobalDescriptors)));
5248  }
5249  }
5250  }
5251  else if(_memory && (_memory->getStMem().size() || _memory->getWorkingMem().size() > 1))
5252  {
5253  UERROR("Last working signature is null!?");
5254  }
5255  else if(_memory == 0)
5256  {
5257  UWARN("Memory not initialized...");
5258  }
5259 }
5260 
5261 std::map<int, Transform> Rtabmap::getNodesInRadius(const Transform & pose, float radius, int k, std::map<int, float> * distsSqr)
5262 {
5263  std::map<int, float> nearestNodesTmp;
5264  std::map<int, float> * nearestNodesPtr = distsSqr == 0? &nearestNodesTmp : distsSqr;
5265  *nearestNodesPtr = graph::findNearestNodes(pose, _optimizedPoses, radius<=0?_localRadius:radius, 0, k);
5266  std::map<int, Transform> nearestPoses;
5267  for(std::map<int, float>::iterator iter=nearestNodesPtr->begin(); iter!=nearestNodesPtr->end(); ++iter)
5268  {
5269  nearestPoses.insert(*_optimizedPoses.find(iter->first));
5270  }
5271  return nearestPoses;
5272 }
5273 
5274 std::map<int, Transform> Rtabmap::getNodesInRadius(int nodeId, float radius, int k, std::map<int, float> * distsSqr)
5275 {
5276  UDEBUG("nodeId=%d, radius=%f", nodeId, radius);
5277  std::map<int, float> nearestNodesTmp;
5278  std::map<int, float> * nearestNodesPtr = distsSqr == 0? &nearestNodesTmp : distsSqr;
5279  if(nodeId==0 && !_lastLocalizationPose.isNull() && !_lastLocalizationPose.isIdentity())
5280  {
5281  *nearestNodesPtr = graph::findNearestNodes(_lastLocalizationPose, _optimizedPoses, radius<=0?_localRadius:radius, 0, k);
5282  }
5283  else
5284  {
5285  if(nodeId==0 && !_optimizedPoses.empty())
5286  {
5287  nodeId = _optimizedPoses.rbegin()->first;
5288  }
5289 
5290  if(_optimizedPoses.find(nodeId) != _optimizedPoses.end())
5291  {
5292  *nearestNodesPtr = graph::findNearestNodes(nodeId, _optimizedPoses, radius<=0?_localRadius:radius, 0, k);
5293  }
5294  }
5295 
5296  std::map<int, Transform> nearestPoses;
5297  for(std::map<int, float>::iterator iter=nearestNodesPtr->begin(); iter!=nearestNodesPtr->end(); ++iter)
5298  {
5299  nearestPoses.insert(*_optimizedPoses.find(iter->first));
5300  }
5301 
5302  return nearestPoses;
5303 }
5304 
5305 int Rtabmap::detectMoreLoopClosures(
5306  float clusterRadiusMax,
5307  float clusterAngle,
5308  int iterations,
5309  bool intraSession,
5310  bool interSession,
5311  const ProgressState * processState,
5312  float clusterRadiusMin)
5313 {
5314  UASSERT(iterations>0);
5315 
5316  if(_graphOptimizer->iterations() <= 0)
5317  {
5318  UERROR("Cannot detect more loop closures if graph optimization iterations = 0");
5319  return -1;
5320  }
5321  if(!_rgbdSlamMode)
5322  {
5323  UERROR("Detecting more loop closures can be done only in RGBD-SLAM mode.");
5324  return -1;
5325  }
5326  if(!intraSession && !interSession)
5327  {
5328  UERROR("Intra and/or inter session argument should be true.");
5329  return -1;
5330  }
5331 
5332  std::list<Link> loopClosuresAdded;
5333  std::multimap<int, int> checkedLoopClosures;
5334 
5335  std::map<int, Transform> posesToCheckLoopClosures;
5336  std::map<int, Transform> poses;
5337  std::multimap<int, Link> links;
5338  std::map<int, Signature> signatures; // some signatures may be in LTM, get them all
5339  this->getGraph(poses, links, true, true, &signatures);
5340 
5341  std::map<int, int> mapIds;
5342  UDEBUG("remove all invalid or intermediate nodes, fill mapIds");
5343  for(std::map<int, Transform>::iterator iter=poses.upper_bound(0); iter!=poses.end();++iter)
5344  {
5345  if(signatures.at(iter->first).getWeight() >= 0)
5346  {
5347  posesToCheckLoopClosures.insert(*iter);
5348  mapIds.insert(std::make_pair(iter->first, signatures.at(iter->first).mapId()));
5349  }
5350  }
5351 
5352  for(int n=0; n<iterations; ++n)
5353  {
5354  UINFO("Looking for more loop closures, clustering poses... (iteration=%d/%d, radius=%f m angle=%f rad)",
5355  n+1, iterations, clusterRadiusMax, clusterAngle);
5356 
5357  std::multimap<int, int> clusters = graph::radiusPosesClustering(
5358  posesToCheckLoopClosures,
5359  clusterRadiusMax,
5360  clusterAngle);
5361 
5362  UINFO("Looking for more loop closures, clustering poses... found %d clusters.", (int)clusters.size());
5363 
5364  int i=0;
5365  std::set<int> addedLinks;
5366  for(std::multimap<int, int>::iterator iter=clusters.begin(); iter!= clusters.end(); ++iter, ++i)
5367  {
5368  if(processState && processState->isCanceled())
5369  {
5370  return -1;
5371  break;
5372  }
5373 
5374  int from = iter->first;
5375  int to = iter->second;
5376  if(from > to)
5377  {
5378  from = iter->second;
5379  to = iter->first;
5380  }
5381 
5382  int mapIdFrom = uValue(mapIds, from, 0);
5383  int mapIdTo = uValue(mapIds, to, 0);
5384 
5385  if((interSession && mapIdFrom != mapIdTo) ||
5386  (intraSession && mapIdFrom == mapIdTo))
5387  {
5388 
5389  bool alreadyChecked = false;
5390  for(std::multimap<int, int>::iterator jter = checkedLoopClosures.lower_bound(from);
5391  !alreadyChecked && jter!=checkedLoopClosures.end() && jter->first == from;
5392  ++jter)
5393  {
5394  if(to == jter->second)
5395  {
5396  alreadyChecked = true;
5397  }
5398  }
5399 
5400  if(!alreadyChecked)
5401  {
5402  // only add new links and one per cluster per iteration
5403  if(addedLinks.find(from) == addedLinks.end() &&
5404  addedLinks.find(to) == addedLinks.end() &&
5405  rtabmap::graph::findLink(links, from, to) == links.end())
5406  {
5407  // Reverify if in the bounds with the current optimized graph
5408  Transform delta = poses.at(from).inverse() * poses.at(to);
5409  if(delta.getNorm() < clusterRadiusMax &&
5410  delta.getNorm() >= clusterRadiusMin)
5411  {
5412  checkedLoopClosures.insert(std::make_pair(from, to));
5413 
5414  UASSERT(signatures.find(from) != signatures.end());
5415  UASSERT(signatures.find(to) != signatures.end());
5416 
5417  Transform guess;
5418  if(_proximityBySpace && uContains(poses, from) && uContains(poses, to))
5419  {
5420  guess = poses.at(from).inverse() * poses.at(to);
5421  }
5422 
5423  RegistrationInfo info;
5424  // use signatures instead of IDs because some signatures may not be in WM
5425  Transform t = _memory->computeTransform(signatures.at(from), signatures.at(to), guess, &info);
5426 
5427  if(!t.isNull())
5428  {
5429  bool updateConstraints = true;
5430  if(_optimizationMaxError > 0.0f)
5431  {
5432  //optimize the graph to see if the new constraint is globally valid
5433 
5434  int fromId = from;
5435  int mapId = signatures.at(from).mapId();
5436  // use first node of the map containing from
5437  for(std::map<int, Signature>::iterator ster=signatures.begin(); ster!=signatures.end(); ++ster)
5438  {
5439  if(ster->second.mapId() == mapId)
5440  {
5441  fromId = ster->first;
5442  break;
5443  }
5444  }
5445  std::multimap<int, Link> linksIn = links;
5446  linksIn.insert(std::make_pair(from, Link(from, to, Link::kUserClosure, t, getInformation(info.covariance))));
5447  const Link * maxLinearLink = 0;
5448  const Link * maxAngularLink = 0;
5449  float maxLinearError = 0.0f;
5450  float maxAngularError = 0.0f;
5451  float maxLinearErrorRatio = 0.0f;
5452  float maxAngularErrorRatio = 0.0f;
5453  std::map<int, Transform> optimizedPoses;
5454  std::multimap<int, Link> links;
5455  UASSERT(poses.find(fromId) != poses.end());
5456  UASSERT_MSG(poses.find(from) != poses.end(), uFormat("id=%d poses=%d links=%d", from, (int)poses.size(), (int)links.size()).c_str());
5457  UASSERT_MSG(poses.find(to) != poses.end(), uFormat("id=%d poses=%d links=%d", to, (int)poses.size(), (int)links.size()).c_str());
5458  _graphOptimizer->getConnectedGraph(fromId, poses, linksIn, optimizedPoses, links);
5459  UASSERT(optimizedPoses.find(fromId) != optimizedPoses.end());
5460  UASSERT_MSG(optimizedPoses.find(from) != optimizedPoses.end(), uFormat("id=%d poses=%d links=%d", from, (int)optimizedPoses.size(), (int)links.size()).c_str());
5461  UASSERT_MSG(optimizedPoses.find(to) != optimizedPoses.end(), uFormat("id=%d poses=%d links=%d", to, (int)optimizedPoses.size(), (int)links.size()).c_str());
5462  UASSERT(graph::findLink(links, from, to) != links.end());
5463  optimizedPoses = _graphOptimizer->optimize(fromId, optimizedPoses, links);
5464  std::string msg;
5465  if(optimizedPoses.size())
5466  {
5467  graph::computeMaxGraphErrors(
5468  optimizedPoses,
5469  links,
5470  maxLinearErrorRatio,
5471  maxAngularErrorRatio,
5472  maxLinearError,
5473  maxAngularError,
5474  &maxLinearLink,
5475  &maxAngularLink);
5476  if(maxLinearLink)
5477  {
5478  UINFO("Max optimization linear error = %f m (link %d->%d)", maxLinearError, maxLinearLink->from(), maxLinearLink->to());
5479  if(maxLinearErrorRatio > _optimizationMaxError)
5480  {
5481  msg = uFormat("Rejecting edge %d->%d because "
5482  "graph error is too large after optimization (%f m for edge %d->%d with ratio %f > std=%f m). "
5483  "\"%s\" is %f.",
5484  from,
5485  to,
5486  maxLinearError,
5487  maxLinearLink->from(),
5488  maxLinearLink->to(),
5489  maxLinearErrorRatio,
5490  sqrt(maxLinearLink->transVariance()),
5491  Parameters::kRGBDOptimizeMaxError().c_str(),
5492  _optimizationMaxError);
5493  }
5494  }
5495  else if(maxAngularLink)
5496  {
5497  UINFO("Max optimization angular error = %f deg (link %d->%d)", maxAngularError*180.0f/M_PI, maxAngularLink->from(), maxAngularLink->to());
5498  if(maxAngularErrorRatio > _optimizationMaxError)
5499  {
5500  msg = uFormat("Rejecting edge %d->%d because "
5501  "graph error is too large after optimization (%f deg for edge %d->%d with ratio %f > std=%f deg). "
5502  "\"%s\" is %f m.",
5503  from,
5504  to,
5505  maxAngularError*180.0f/M_PI,
5506  maxAngularLink->from(),
5507  maxAngularLink->to(),
5508  maxAngularErrorRatio,
5509  sqrt(maxAngularLink->rotVariance()),
5510  Parameters::kRGBDOptimizeMaxError().c_str(),
5511  _optimizationMaxError);
5512  }
5513  }
5514  }
5515  else
5516  {
5517  msg = uFormat("Rejecting edge %d->%d because graph optimization has failed!",
5518  from,
5519  to);
5520  }
5521  if(!msg.empty())
5522  {
5523  UWARN("%s", msg.c_str());
5524  updateConstraints = false;
5525  }
5526  else
5527  {
5528  poses = optimizedPoses;
5529  }
5530  }
5531 
5532  if(updateConstraints)
5533  {
5534  addedLinks.insert(from);
5535  addedLinks.insert(to);
5536  cv::Mat inf = getInformation(info.covariance);
5537  links.insert(std::make_pair(from, Link(from, to, Link::kUserClosure, t, inf)));
5538  loopClosuresAdded.push_back(Link(from, to, Link::kUserClosure, t, inf));
5539  std::string msg = uFormat("Iteration %d/%d: Added loop closure %d->%d! (%d/%d)", n+1, iterations, from, to, i+1, (int)clusters.size());
5540  UINFO(msg.c_str());
5541 
5542  if(processState)
5543  {
5544  UINFO(msg.c_str());
5545  if(!processState->callback(msg))
5546  {
5547  return -1;
5548  }
5549  }
5550  }
5551  }
5552  }
5553  }
5554  }
5555  }
5556  }
5557 
5558  if(processState)
5559  {
5560  std::string msg = uFormat("Iteration %d/%d: Detected %d total loop closures!", n+1, iterations, (int)addedLinks.size()/2);
5561  UINFO(msg.c_str());
5562  if(!processState->callback(msg))
5563  {
5564  return -1;
5565  }
5566  }
5567  else
5568  {
5569  UINFO("Iteration %d/%d: Detected %d total loop closures!", n+1, iterations, (int)addedLinks.size()/2);
5570  }
5571 
5572  if(addedLinks.size() == 0)
5573  {
5574  break;
5575  }
5576 
5577  UINFO("Optimizing graph with new links (%d nodes, %d constraints)...",
5578  (int)poses.size(), (int)links.size());
5579  int fromId = _optimizeFromGraphEnd?poses.rbegin()->first:poses.begin()->first;
5580  poses = _graphOptimizer->optimize(fromId, poses, links, 0);
5581  if(poses.size() == 0)
5582  {
5583  UERROR("Optimization failed! Rejecting all loop closures...");
5584  loopClosuresAdded.clear();
5585  return -1;
5586  }
5587  UINFO("Optimizing graph with new links... done!");
5588  }
5589  UINFO("Total added %d loop closures.", (int)loopClosuresAdded.size());
5590 
5591  if(loopClosuresAdded.size())
5592  {
5593  for(std::list<Link>::iterator iter=loopClosuresAdded.begin(); iter!=loopClosuresAdded.end(); ++iter)
5594  {
5595  _memory->addLink(*iter, true);
5596  }
5597  // Update optimized poses
5598  for(std::map<int, Transform>::iterator iter=_optimizedPoses.begin(); iter!=_optimizedPoses.end(); ++iter)
5599  {
5600  std::map<int, Transform>::iterator jter = poses.find(iter->first);
5601  if(jter != poses.end())
5602  {
5603  iter->second = jter->second;
5604  }
5605  }
5606  std::map<int, Transform> tmp;
5607  // Update also the links if some have been added in WM
5608  _memory->getMetricConstraints(uKeysSet(_optimizedPoses), tmp, _constraints, false);
5609  // This will force rtabmap_ros to regenerate the global occupancy grid if there was one
5610  _memory->save2DMap(cv::Mat(), 0, 0, 0);
5611  }
5612  return (int)loopClosuresAdded.size();
5613 }
5614 
5615 bool Rtabmap::globalBundleAdjustment(
5616  int optimizerType,
5617  bool rematchFeatures,
5618  int iterations,
5619  float pixelVariance)
5620 {
5621  if(!_optimizedPoses.empty() && !_constraints.empty())
5622  {
5623  int iterations = Parameters::defaultOptimizerIterations();
5624  float pixelVariance = Parameters::defaultg2oPixelVariance();
5625  ParametersMap params = _parameters;
5626  Parameters::parse(params, Parameters::kOptimizerIterations(), iterations);
5627  Parameters::parse(params, Parameters::kg2oPixelVariance(), pixelVariance);
5628  if(iterations > 0)
5629  {
5630  uInsert(params, ParametersPair(Parameters::kOptimizerIterations(), uNumber2Str(iterations)));
5631  }
5632  if(pixelVariance > 0.0f)
5633  {
5634  uInsert(params, ParametersPair(Parameters::kg2oPixelVariance(), uNumber2Str(pixelVariance)));
5635  }
5636 
5637  std::map<int, Signature> signatures;
5638  for(std::map<int, Transform>::iterator iter=_optimizedPoses.lower_bound(1); iter!=_optimizedPoses.end(); ++iter)
5639  {
5640  if(_memory->getSignature(iter->first))
5641  {
5642  signatures.insert(std::make_pair(iter->first, *_memory->getSignature(iter->first)));
5643  }
5644  }
5645 
5646  Optimizer * optimizer = Optimizer::create((Optimizer::Type)optimizerType, params);
5647  std::map<int, Transform> poses = optimizer->optimizeBA(
5648  _optimizeFromGraphEnd?_optimizedPoses.lower_bound(1)->first:_optimizedPoses.rbegin()->first,
5649  _optimizedPoses,
5650  _constraints,
5651  signatures,
5652  rematchFeatures);
5653  delete optimizer;
5654 
5655  if(poses.empty())
5656  {
5657  UERROR("Optimization failed!");
5658  }
5659  else
5660  {
5661  _optimizedPoses = poses;
5662  // This will force rtabmap_ros to regenerate the global occupancy grid if there was one
5663  _memory->save2DMap(cv::Mat(), 0, 0, 0);
5664  return true;
5665  }
5666  }
5667  else
5668  {
5669  UERROR("Optimized poses (%ld) or constraints (%ld) are empty!", _optimizedPoses.size(), _constraints.size());
5670  }
5671  return false;
5672 }
5673 
5674 int Rtabmap::cleanupLocalGrids(
5675  const std::map<int, Transform> & poses,
5676  const cv::Mat & map,
5677  float xMin,
5678  float yMin,
5679  float cellSize,
5680  int cropRadius,
5681  bool filterScans)
5682 {
5683  if(_memory)
5684  {
5685  return _memory->cleanupLocalGrids(
5686  poses,
5687  map,
5688  xMin,
5689  yMin,
5690  cellSize,
5691  cropRadius,
5692  filterScans);
5693  }
5694  return -1;
5695 }
5696 
5697 int Rtabmap::refineLinks()
5698 {
5699  if(!_rgbdSlamMode)
5700  {
5701  UERROR("Refining links can be done only in RGBD-SLAM mode.");
5702  return -1;
5703  }
5704 
5705  std::list<Link> linksRefined;
5706 
5707  std::map<int, Transform> poses;
5708  std::multimap<int, Link> links;
5709  std::map<int, Signature> signatures;
5710  this->getGraph(poses, links, false, true, &signatures);
5711 
5712  int i=0;
5713  for(std::multimap<int, Link>::iterator iter=links.lower_bound(1); iter!= links.end(); ++iter)
5714  {
5715  int from = iter->second.from();
5716  int to = iter->second.to();
5717 
5718  UASSERT(signatures.find(from) != signatures.end());
5719  UASSERT(signatures.find(to) != signatures.end());
5720 
5721  RegistrationInfo info;
5722  // use signatures instead of IDs because some signatures may not be in WM
5723  Transform t = _memory->computeTransform(signatures.at(from), signatures.at(to), iter->second.transform(), &info);
5724 
5725  if(!t.isNull())
5726  {
5727  linksRefined.push_back(Link(from, to, iter->second.type(), t, info.covariance.inv()));
5728  UINFO("Refined link %d->%d! (%d/%d)", from, to, ++i, (int)links.size());
5729  }
5730  }
5731  UINFO("Total refined %d links.", (int)linksRefined.size());
5732 
5733  if(linksRefined.size())
5734  {
5735  for(std::list<Link>::iterator iter=linksRefined.begin(); iter!=linksRefined.end(); ++iter)
5736  {
5737  _memory->updateLink(*iter, true);
5738  }
5739  }
5740  return (int)linksRefined.size();
5741 }
5742 
5743 bool Rtabmap::addLink(const Link & link)
5744 {
5745  const Transform & t = link.transform();
5746  if(!_rgbdSlamMode)
5747  {
5748  UERROR("Adding new link can be done only in RGBD-SLAM mode.");
5749  return false;
5750  }
5751  if(!_memory)
5752  {
5753  UERROR("Memory is not initialized.");
5754  return false;
5755  }
5756  if(t.isNull())
5757  {
5758  UERROR("Link's transform is null! (%d->%d type=%s)", link.from(), link.to(), link.typeName().c_str());
5759  return false;
5760  }
5761  if(_memory->isIncremental())
5762  {
5763  if(_memory->getSignature(link.from()) == 0)
5764  {
5765  UERROR("Link's \"from id\" %d is not in working memory", link.from());
5766  return false;
5767  }
5768  if(_memory->getSignature(link.to()) == 0)
5769  {
5770  UERROR("Link's \"to id\" %d is not in working memory", link.to());
5771  return false;
5772  }
5773 
5774  if(_optimizedPoses.find(link.from()) == _optimizedPoses.end() &&
5775  _optimizedPoses.find(link.to()) == _optimizedPoses.end())
5776  {
5777  UERROR("Neither nodes %d or %d are in the local graph (size=%d). One of the 2 nodes should be in the local graph.", (int)_optimizedPoses.size(), link.from(), link.to());
5778  return false;
5779  }
5780 
5781  // add temporary the link
5782  if(!_memory->addLink(link))
5783  {
5784  UERROR("Cannot add new link %d->%d to memory", link.from(), link.to());
5785  return false;
5786  }
5787 
5788  // optimize with new link
5789  std::map<int, Transform> poses = _optimizedPoses;
5790  std::multimap<int, Link> links;
5791  cv::Mat covariance;
5792  optimizeCurrentMap(this->getLastLocationId(), false, poses, covariance, &links);
5793 
5794  if(poses.find(link.from()) == poses.end())
5795  {
5796  UERROR("Link's \"from id\" %d is not in the graph (size=%d)", link.from(), (int)poses.size());
5797  _memory->removeLink(link.from(), link.to());
5798  return false;
5799  }
5800  if(poses.find(link.to()) == poses.end())
5801  {
5802  UERROR("Link's \"to id\" %d is not in the graph (size=%d)", link.to(), (int)poses.size());
5803  _memory->removeLink(link.from(), link.to());
5804  return false;
5805  }
5806 
5807  std::string msg;
5808  if(poses.empty())
5809  {
5810  msg = uFormat("Rejecting edge %d->%d because graph optimization has failed!", link.from(), link.to());
5811  }
5812  else if(_optimizationMaxError > 0.0f)
5813  {
5814  float maxLinearError = 0.0f;
5815  float maxLinearErrorRatio = 0.0f;
5816  float maxAngularError = 0.0f;
5817  float maxAngularErrorRatio = 0.0f;
5818  const Link * maxLinearLink = 0;
5819  const Link * maxAngularLink = 0;
5820 
5821  graph::computeMaxGraphErrors(
5822  poses,
5823  links,
5824  maxLinearErrorRatio,
5825  maxAngularErrorRatio,
5826  maxLinearError,
5827  maxAngularError,
5828  &maxLinearLink,
5829  &maxAngularLink);
5830  if(maxLinearLink)
5831  {
5832  UINFO("Max optimization linear error = %f m (link %d->%d)", maxLinearError, maxLinearLink->from(), maxLinearLink->to());
5833  if(maxLinearErrorRatio > _optimizationMaxError)
5834  {
5835  msg = uFormat("Rejecting edge %d->%d because "
5836  "graph error is too large after optimization (%f m for edge %d->%d with ratio %f > std=%f m). "
5837  "\"%s\" is %f.",
5838  link.from(),
5839  link.to(),
5840  maxLinearError,
5841  maxLinearLink->from(),
5842  maxLinearLink->to(),
5843  maxLinearErrorRatio,
5844  sqrt(maxLinearLink->transVariance()),
5845  Parameters::kRGBDOptimizeMaxError().c_str(),
5846  _optimizationMaxError);
5847  }
5848  }
5849  else if(maxAngularLink)
5850  {
5851  UINFO("Max optimization angular error = %f deg (link %d->%d)", maxAngularError*180.0f/M_PI, maxAngularLink->from(), maxAngularLink->to());
5852  if(maxAngularErrorRatio > _optimizationMaxError)
5853  {
5854  msg = uFormat("Rejecting edge %d->%d because "
5855  "graph error is too large after optimization (%f deg for edge %d->%d with ratio %f > std=%f deg). "
5856  "\"%s\" is %f m.",
5857  link.from(),
5858  link.to(),
5859  maxAngularError*180.0f/M_PI,
5860  maxAngularLink->from(),
5861  maxAngularLink->to(),
5862  maxAngularErrorRatio,
5863  sqrt(maxAngularLink->rotVariance()),
5864  Parameters::kRGBDOptimizeMaxError().c_str(),
5865  _optimizationMaxError);
5866  }
5867  }
5868  }
5869  if(!msg.empty())
5870  {
5871  UERROR("%s", msg.c_str());
5872  _memory->removeLink(link.from(), link.to());
5873  return false;
5874  }
5875 
5876  // Update optimized poses
5877  for(std::map<int, Transform>::iterator iter=_optimizedPoses.begin(); iter!=_optimizedPoses.end(); ++iter)
5878  {
5879  std::map<int, Transform>::iterator jter = poses.find(iter->first);
5880  if(jter != poses.end())
5881  {
5882  iter->second = jter->second;
5883  }
5884  }
5885  if(!_optimizeFromGraphEnd)
5886  {
5887  _mapCorrection = _optimizedPoses.rbegin()->second * _memory->getSignature(_optimizedPoses.rbegin()->first)->getPose().inverse();
5888  }
5889 
5890  std::map<int, Transform> tmp;
5891  // Update also the links if some have been added in WM
5892  _memory->getMetricConstraints(uKeysSet(_optimizedPoses), tmp, _constraints, false);
5893  // This will force rtabmap_ros to regenerate the global occupancy grid if there was one
5894  _memory->save2DMap(cv::Mat(), 0, 0, 0);
5895 
5896  return true;
5897  }
5898  else // localization mode
5899  {
5900  int oldestId = link.from()>link.to()?link.to():link.from();
5901  int newestId = link.from()<link.to()?link.to():link.from();
5902 
5903  if(_memory->getSignature(oldestId) == 0)
5904  {
5905  UERROR("Link's id %d is not in working memory", oldestId);
5906  return false;
5907  }
5908  if(_optimizedPoses.find(oldestId) == _optimizedPoses.end())
5909  {
5910  UERROR("Link's id %d is not in the optimized graph (_optimizedPoses=%d)", oldestId, (int)_optimizedPoses.size());
5911  return false;
5912  }
5913  if(_optimizeFromGraphEnd)
5914  {
5915  UERROR("Adding link with %s=true in localization mode is not supported.", Parameters::kRGBDOptimizeFromGraphEnd().c_str());
5916  return false;
5917  }
5918  if(_odomCachePoses.find(newestId) == _odomCachePoses.end())
5919  {
5920  if(!_odomCachePoses.empty())
5921  {
5922  UERROR("Link's id %d is not in the odometry cache (oldest=%d, newest=%d, %s=%d)",
5923  newestId,
5924  _odomCachePoses.begin()->first,
5925  _odomCachePoses.rbegin()->first,
5926  Parameters::kRGBDMaxOdomCacheSize().c_str(),
5927  _maxOdomCacheSize);
5928  }
5929  else
5930  {
5931  UERROR("Link's id %d is not in the odometry cache (%s=%d).",
5932  newestId,
5933  Parameters::kRGBDMaxOdomCacheSize().c_str(),
5934  _maxOdomCacheSize);
5935  }
5936  return false;
5937  }
5938 
5939  // Verify if the new localization is valid by checking if there is
5940  // not too much deformation using current odometry poses
5941  // This will also refine localization links
5942 
5943  std::map<int, Transform> poses = _odomCachePoses;
5944  std::multimap<int, Link> constraints = _odomCacheConstraints;
5945  constraints.insert(std::make_pair(link.from(), link));
5946  for(std::multimap<int, Link>::iterator iter=constraints.begin(); iter!=constraints.end(); ++iter)
5947  {
5948  std::map<int, Transform>::iterator iterPose = _optimizedPoses.find(iter->second.to());
5949  if(iterPose != _optimizedPoses.end() && poses.find(iterPose->first) == poses.end())
5950  {
5951  poses.insert(*iterPose);
5952  // make the poses in the map fixed
5953  constraints.insert(std::make_pair(iterPose->first, Link(iterPose->first, iterPose->first, Link::kPosePrior, iterPose->second, cv::Mat::eye(6,6, CV_64FC1)*999999)));
5954  }
5955  }
5956 
5957  std::map<int, Transform> posesOut;
5958  std::multimap<int, Link> edgeConstraintsOut;
5959  bool priorsIgnored = _graphOptimizer->priorsIgnored();
5960  _graphOptimizer->setPriorsIgnored(false); //temporary set false to use priors above to fix nodes of the map
5961  _graphOptimizer->getConnectedGraph(newestId, poses, constraints, posesOut, edgeConstraintsOut);
5962  std::map<int, Transform> optPoses = _graphOptimizer->optimize(poses.begin()->first, posesOut, edgeConstraintsOut);
5963  _graphOptimizer->setPriorsIgnored(priorsIgnored); // set back
5964 
5965  bool rejectLocalization = false;
5966  if(optPoses.empty())
5967  {
5968  UWARN("Optimization failed, rejecting localization!");
5969  rejectLocalization = true;
5970  }
5971  else if(_optimizationMaxError > 0.0f)
5972  {
5973  UINFO("Compute max graph errors...");
5974  float maxLinearError = 0.0f;
5975  float maxLinearErrorRatio = 0.0f;
5976  float maxAngularError = 0.0f;
5977  float maxAngularErrorRatio = 0.0f;
5978  const Link * maxLinearLink = 0;
5979  const Link * maxAngularLink = 0;
5980  graph::computeMaxGraphErrors(
5981  optPoses,
5982  edgeConstraintsOut,
5983  maxLinearErrorRatio,
5984  maxAngularErrorRatio,
5985  maxLinearError,
5986  maxAngularError,
5987  &maxLinearLink,
5988  &maxAngularLink,
5989  _graphOptimizer->isSlam2d());
5990  if(maxLinearLink == 0 && maxAngularLink==0)
5991  {
5992  UWARN("Could not compute graph errors! Wrong loop closures could be accepted!");
5993  }
5994 
5995  if(maxLinearLink)
5996  {
5997  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()));
5998  if(maxLinearErrorRatio > _optimizationMaxError)
5999  {
6000  UWARN("Rejecting localization (%d <-> %d) in this "
6001  "iteration because a wrong loop closure has been "
6002  "detected after graph optimization, resulting in "
6003  "a maximum graph error ratio of %f (edge %d->%d, type=%d, abs error=%f m, stddev=%f). The "
6004  "maximum error ratio parameter \"%s\" is %f of std deviation.",
6005  link.from(),
6006  link.to(),
6007  maxLinearErrorRatio,
6008  maxLinearLink->from(),
6009  maxLinearLink->to(),
6010  maxLinearLink->type(),
6011  maxLinearError,
6012  sqrt(maxLinearLink->transVariance()),
6013  Parameters::kRGBDOptimizeMaxError().c_str(),
6014  _optimizationMaxError);
6015  rejectLocalization = true;
6016  }
6017  }
6018  if(maxAngularLink)
6019  {
6020  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()));
6021  if(maxAngularErrorRatio > _optimizationMaxError)
6022  {
6023  UWARN("Rejecting localization (%d <-> %d) in this "
6024  "iteration because a wrong loop closure has been "
6025  "detected after graph optimization, resulting in "
6026  "a maximum graph error ratio of %f (edge %d->%d, type=%d, abs error=%f deg, stddev=%f). The "
6027  "maximum error ratio parameter \"%s\" is %f of std deviation.",
6028  link.from(),
6029  link.to(),
6030  maxAngularErrorRatio,
6031  maxAngularLink->from(),
6032  maxAngularLink->to(),
6033  maxAngularLink->type(),
6034  maxAngularError*180.0f/CV_PI,
6035  sqrt(maxAngularLink->rotVariance()),
6036  Parameters::kRGBDOptimizeMaxError().c_str(),
6037  _optimizationMaxError);
6038  rejectLocalization = true;
6039  }
6040  }
6041  }
6042 
6043  if(!rejectLocalization)
6044  {
6045  Transform newOptPoseInv = optPoses.at(link.from()).inverse();
6046  Transform newT = newOptPoseInv * optPoses.at(link.to());
6047  Link linkTmp = link;
6048  linkTmp.setTransform(newT);
6049  if(oldestId == link.from())
6050  {
6051  _lastLocalizationPose = _optimizedPoses.at(link.from()) * linkTmp.transform();
6052  _odomCacheConstraints.insert(std::make_pair(linkTmp.to(), linkTmp.inverse()));
6053  }
6054  else
6055  {
6056  _lastLocalizationPose = _optimizedPoses.at(link.to()) * linkTmp.transform().inverse();
6057  _odomCacheConstraints.insert(std::make_pair(linkTmp.from(), linkTmp));
6058  }
6059  UINFO("Set _lastLocalizationPose=%s", _lastLocalizationPose.prettyPrint().c_str());
6060  if(_graphOptimizer->isSlam2d())
6061  {
6062  // transform constraint to 2D
6063  _lastLocalizationPose = _lastLocalizationPose.to3DoF();
6064  }
6065  Transform odomPose = _odomCachePoses.find(newestId)->second;
6066  _mapCorrection = _lastLocalizationPose * odomPose.inverse();
6067  _lastLocalizationNodeId = oldestId;
6068  return true;
6069  }
6070  }
6071  return false;
6072 }
6073 
6074 cv::Mat Rtabmap::getInformation(const cv::Mat & covariance) const
6075 {
6076  cv::Mat information = covariance.inv();
6077  if(_loopCovLimited)
6078  {
6079  const std::vector<double> & odomMaxInf = _memory->getOdomMaxInf();
6080  if(odomMaxInf.size() == 6)
6081  {
6082  for(int i=0; i<6; ++i)
6083  {
6084  if(information.at<double>(i,i) > odomMaxInf[i])
6085  {
6086  information.at<double>(i,i) = odomMaxInf[i];
6087  }
6088  }
6089  }
6090  }
6091  return information;
6092 }
6093 
6094 void Rtabmap::addNodesToRepublish(const std::vector<int> & ids)
6095 {
6096  if(ids.empty())
6097  {
6098  _nodesToRepublish.clear();
6099  }
6100  else if(_maxRepublished > 0 && _publishLastSignatureData)
6101  {
6102  _nodesToRepublish.insert(ids.begin(), ids.end());
6103  }
6104  else if(_maxRepublished == 0)
6105  {
6106  UWARN("%s=0, so cannot republish the %d requested nodes.", Parameters::kRtabmapMaxRepublished().c_str(), (int)ids.size());
6107  }
6108  else //_publishLastSignatureData=false
6109  {
6110  UWARN("%s=false, so cannot republish the %d requested nodes.", Parameters::kRtabmapPublishLastSignature().c_str(), (int)ids.size());
6111  }
6112 }
6113 
6114 void Rtabmap::clearPath(int status)
6115 {
6116  UINFO("status=%d", status);
6117  _pathStatus = status;
6118  _path.clear();
6119  _pathCurrentIndex=0;
6120  _pathGoalIndex = 0;
6121  _pathTransformToGoal.setIdentity();
6122  _pathUnreachableNodes.clear();
6123  _pathStuckCount = 0;
6124  _pathStuckDistance = 0.0f;
6125  if(_memory)
6126  {
6127  _memory->removeAllVirtualLinks();
6128  }
6129 }
6130 
6131 // return true if path is updated
6132 bool Rtabmap::computePath(int targetNode, bool global)
6133 {
6134  this->clearPath(0);
6135 
6136  if(targetNode>0)
6137  {
6138  UINFO("Planning a path to node %d (global=%d)", targetNode, global?1:0);
6139  }
6140  else
6141  {
6142  UINFO("Planning a path to landmark %d (global=%d)", -targetNode, global?1:0);
6143  }
6144 
6145  if(!_rgbdSlamMode)
6146  {
6147  UWARN("A path can only be computed in RGBD-SLAM mode");
6148  return false;
6149  }
6150 
6151  UTimer totalTimer;
6152  UTimer timer;
6153  Transform transformToLandmark = Transform::getIdentity();
6154 
6155  // No need to optimize the graph
6156  if(_memory)
6157  {
6158  int currentNode = 0;
6159  if(_memory->isIncremental())
6160  {
6161  if(!_memory->getLastWorkingSignature())
6162  {
6163  UWARN("Working memory is empty... cannot compute a path");
6164  return false;
6165  }
6166  currentNode = _memory->getLastWorkingSignature()->id();
6167  }
6168  else
6169  {
6170  if(_lastLocalizationPose.isNull() || _optimizedPoses.empty())
6171  {
6172  UWARN("Last localization pose is null or optimized graph is empty... cannot compute a path");
6173  return false;
6174  }
6175  if(_optimizedPoses.begin()->first < 0)
6176  {
6177  std::map<int, Transform> poses(_optimizedPoses.lower_bound(1), _optimizedPoses.end());
6178  currentNode = graph::findNearestNode(poses, _lastLocalizationPose);
6179  }
6180  else
6181  {
6182  currentNode = graph::findNearestNode(_optimizedPoses, _lastLocalizationPose);
6183  }
6184  }
6185  if(currentNode && targetNode)
6186  {
6187  std::list<std::pair<int, Transform> > path = graph::computePath(
6188  currentNode,
6189  targetNode,
6190  _memory,
6191  global,
6192  false,
6193  _pathLinearVelocity,
6194  _pathAngularVelocity);
6195 
6196  //transform in current referential
6197  Transform t = uValue(_optimizedPoses, currentNode, Transform::getIdentity());
6198  _path.resize(path.size());
6199  int oi = 0;
6200  for(std::list<std::pair<int, Transform> >::iterator iter=path.begin(); iter!=path.end();++iter)
6201  {
6202  if(iter->first > 0)
6203  {
6204  // just keep nodes in the path
6205  _path[oi].first = iter->first;
6206  _path[oi++].second = t * iter->second;
6207  }
6208  }
6209  _path.resize(oi);
6210  if(!_path.empty() && !path.empty() && path.rbegin()->first < 0)
6211  {
6212  transformToLandmark = _path.back().second.inverse() * t * path.rbegin()->second;
6213  }
6214  }
6215  else if(currentNode == 0)
6216  {
6217  UWARN("We should be localized before planning.");
6218  }
6219  }
6220  UINFO("Total planning time = %fs (%d nodes, %f m long)", totalTimer.ticks(), (int)_path.size(), graph::computePathLength(_path));
6221 
6222  if(_path.size() == 0)
6223  {
6224  _path.clear();
6225  UWARN("Cannot compute a path!");
6226  return false;
6227  }
6228  else
6229  {
6230  UINFO("Path generated! Size=%d", (int)_path.size());
6231  if(ULogger::level() == ULogger::kInfo)
6232  {
6233  std::stringstream stream;
6234  for(unsigned int i=0; i<_path.size(); ++i)
6235  {
6236  stream << _path[i].first;
6237  if(i+1 < _path.size())
6238  {
6239  stream << " ";
6240  }
6241  }
6242  UINFO("Path = [%s]", stream.str().c_str());
6243  }
6244  if(_goalsSavedInUserData)
6245  {
6246  // set goal to latest signature
6247  std::string goalStr = uFormat("GOAL:%d", targetNode);
6248 
6249  // use label is exist
6250  if(_memory->getSignature(targetNode))
6251  {
6252  if(!_memory->getSignature(targetNode)->getLabel().empty())
6253  {
6254  goalStr = std::string("GOAL:")+_memory->getSignature(targetNode)->getLabel();
6255  }
6256  }
6257  else if(global)
6258  {
6259  std::map<int, std::string> labels = _memory->getAllLabels();
6260  std::map<int, std::string>::iterator iter = labels.find(targetNode);
6261  if(iter != labels.end() && !iter->second.empty())
6262  {
6263  goalStr = std::string("GOAL:")+labels.at(targetNode);
6264  }
6265  }
6266  setUserData(0, cv::Mat(1, int(goalStr.size()+1), CV_8SC1, (void *)goalStr.c_str()).clone());
6267  }
6268  _pathTransformToGoal = transformToLandmark;
6269 
6270  updateGoalIndex();
6271  return _path.size() || _pathStatus > 0;
6272  }
6273 
6274  return false;
6275 }
6276 
6277 bool Rtabmap::computePath(const Transform & targetPose, float tolerance)
6278 {
6279  this->clearPath(0);
6280 
6281  UINFO("Planning a path to pose %s ", targetPose.prettyPrint().c_str());
6282  if(tolerance < 0.0f)
6283  {
6284  tolerance = _localRadius;
6285  }
6286 
6287  std::list<std::pair<int, Transform> > pathPoses;
6288 
6289  if(!_rgbdSlamMode)
6290  {
6291  UWARN("This method can only be used in RGBD-SLAM mode");
6292  return false;
6293  }
6294 
6295  //Find the nearest node
6296  UTimer timer;
6297  std::map<int, Transform> nodes = _optimizedPoses;
6298  std::multimap<int, int> links;
6299  for(std::map<int, Transform>::iterator iter=nodes.upper_bound(0); iter!=nodes.end(); ++iter)
6300  {
6301  const Signature * s = _memory->getSignature(iter->first);
6302  UASSERT(s);
6303  for(std::map<int, Link>::const_iterator jter=s->getLinks().begin(); jter!=s->getLinks().end(); ++jter)
6304  {
6305  // only add links for which poses are in "nodes"
6306  if(jter->second.from() != jter->second.to() && uContains(nodes, jter->second.to()))
6307  {
6308  links.insert(std::make_pair(jter->second.from(), jter->second.to()));
6309  //links.insert(std::make_pair(jter->second.to(), jter->second.from())); // <-> (commented: already added when iterating in nodes)
6310  }
6311  }
6312  }
6313  UINFO("Time getting links = %fs", timer.ticks());
6314 
6315  int currentNode = 0;
6316  if(_memory->isIncremental())
6317  {
6318  if(!_memory->getLastWorkingSignature())
6319  {
6320  UWARN("Working memory is empty... cannot compute a path");
6321  return false;
6322  }
6323  currentNode = _memory->getLastWorkingSignature()->id();
6324  }
6325  else
6326  {
6327  if(_lastLocalizationPose.isNull() || _optimizedPoses.empty())
6328  {
6329  UWARN("Last localization pose is null... cannot compute a path");
6330  return false;
6331  }
6332  if(_optimizedPoses.begin()->first < 0)
6333  {
6334  std::map<int, Transform> poses(_optimizedPoses.lower_bound(1), _optimizedPoses.end());
6335  currentNode = graph::findNearestNode(poses, _lastLocalizationPose);
6336  }
6337  else
6338  {
6339  currentNode = graph::findNearestNode(_optimizedPoses, _lastLocalizationPose);
6340  }
6341  }
6342 
6343  int nearestId;
6344  if(!_lastLocalizationPose.isNull() && _lastLocalizationPose.getDistance(targetPose) < tolerance)
6345  {
6346  // target can be reached from the current node
6347  nearestId = currentNode;
6348  }
6349  else
6350  {
6351  nearestId = rtabmap::graph::findNearestNode(nodes, targetPose);
6352  }
6353  UINFO("Nearest node found=%d ,%fs", nearestId, timer.ticks());
6354  if(nearestId > 0)
6355  {
6356  if(tolerance != 0.0f && targetPose.getDistance(nodes.at(nearestId)) > tolerance)
6357  {
6358  UWARN("Cannot plan farther than %f m from the graph! (distance=%f m from node %d)",
6359  tolerance, targetPose.getDistance(nodes.at(nearestId)), nearestId);
6360  }
6361  else
6362  {
6363  UINFO("Computing path from location %d to %d", currentNode, nearestId);
6364  UTimer timer;
6365  _path = uListToVector(rtabmap::graph::computePath(nodes, links, currentNode, nearestId));
6366  UINFO("A* time = %fs", timer.ticks());
6367 
6368  if(_path.size() == 0)
6369  {
6370  UWARN("Cannot compute a path!");
6371  }
6372  else
6373  {
6374  UINFO("Path generated! Size=%d", (int)_path.size());
6375  if(ULogger::level() == ULogger::kInfo)
6376  {
6377  std::stringstream stream;
6378  for(unsigned int i=0; i<_path.size(); ++i)
6379  {
6380  stream << _path[i].first;
6381  if(i+1 < _path.size())
6382  {
6383  stream << " ";
6384  }
6385  }
6386  UINFO("Path = [%s]", stream.str().c_str());
6387  }
6388 
6389  UASSERT(uContains(nodes, _path.back().first));
6390  _pathTransformToGoal = nodes.at(_path.back().first).inverse() * targetPose;
6391 
6392  updateGoalIndex();
6393 
6394  return true;
6395  }
6396  }
6397  }
6398  else
6399  {
6400  UWARN("Nearest node not found in graph (size=%d) for pose %s", (int)nodes.size(), targetPose.prettyPrint().c_str());
6401  }
6402 
6403  return false;
6404 }
6405 
6406 std::vector<std::pair<int, Transform> > Rtabmap::getPathNextPoses() const
6407 {
6408  std::vector<std::pair<int, Transform> > poses;
6409  if(_path.size())
6410  {
6411  UASSERT(_pathCurrentIndex < _path.size() && _pathGoalIndex < _path.size());
6412  poses.resize(_pathGoalIndex-_pathCurrentIndex+1);
6413  int oi=0;
6414  for(unsigned int i=_pathCurrentIndex; i<=_pathGoalIndex; ++i)
6415  {
6416  std::map<int, Transform>::const_iterator iter = _optimizedPoses.find(_path[i].first);
6417  if(iter != _optimizedPoses.end())
6418  {
6419  poses[oi++] = *iter;
6420  }
6421  else
6422  {
6423  break;
6424  }
6425  }
6426  poses.resize(oi);
6427  }
6428  return poses;
6429 }
6430 
6431 std::vector<int> Rtabmap::getPathNextNodes() const
6432 {
6433  std::vector<int> ids;
6434  if(_path.size())
6435  {
6436  UASSERT(_pathCurrentIndex < _path.size() && _pathGoalIndex < _path.size());
6437  ids.resize(_pathGoalIndex-_pathCurrentIndex+1);
6438  int oi = 0;
6439  for(unsigned int i=_pathCurrentIndex; i<=_pathGoalIndex; ++i)
6440  {
6441  std::map<int, Transform>::const_iterator iter = _optimizedPoses.find(_path[i].first);
6442  if(iter != _optimizedPoses.end())
6443  {
6444  ids[oi++] = iter->first;
6445  }
6446  else
6447  {
6448  break;
6449  }
6450  }
6451  ids.resize(oi);
6452  }
6453  return ids;
6454 }
6455 
6456 int Rtabmap::getPathCurrentGoalId() const
6457 {
6458  if(_path.size())
6459  {
6460  UASSERT(_pathGoalIndex <= _path.size());
6461  return _path[_pathGoalIndex].first;
6462  }
6463  return 0;
6464 }
6465 
6466 void Rtabmap::updateGoalIndex()
6467 {
6468  if(!_rgbdSlamMode)
6469  {
6470  UWARN("This method can on be used in RGBD-SLAM mode!");
6471  return;
6472  }
6473 
6474  if( _memory && _path.size())
6475  {
6476  // remove all previous virtual links
6477  for(unsigned int i=0; i<_pathCurrentIndex && i<_path.size(); ++i)
6478  {
6479  const Signature * s = _memory->getSignature(_path[i].first);
6480  if(s)
6481  {
6482  _memory->removeVirtualLinks(s->id());
6483  }
6484  }
6485 
6486  // for the current index, only keep the newest virtual link
6487  // This will make sure that the path is still connected even
6488  // if the new signature is removed (e.g., because of a small displacement)
6489  UASSERT(_pathCurrentIndex < _path.size());
6490  const Signature * currentIndexS = _memory->getSignature(_path[_pathCurrentIndex].first);
6491  UASSERT_MSG(currentIndexS != 0, uFormat("_path[%d].first=%d", _pathCurrentIndex, _path[_pathCurrentIndex].first).c_str());
6492  std::multimap<int, Link> links = currentIndexS->getLinks(); // make a copy
6493  bool latestVirtualLinkFound = false;
6494  for(std::multimap<int, Link>::reverse_iterator iter=links.rbegin(); iter!=links.rend(); ++iter)
6495  {
6496  if(iter->second.type() == Link::kVirtualClosure)
6497  {
6498  if(latestVirtualLinkFound)
6499  {
6500  _memory->removeLink(currentIndexS->id(), iter->first);
6501  }
6502  else
6503  {
6504  latestVirtualLinkFound = true;
6505  }
6506  }
6507  }
6508 
6509  // Make sure the next signatures on the path are linked together
6510  float distanceSoFar = 0.0f;
6511  for(unsigned int i=_pathCurrentIndex+1;
6512  i<_path.size();
6513  ++i)
6514  {
6515  if(i>0)
6516  {
6517  if(_localRadius > 0.0f)
6518  {
6519  distanceSoFar += _path[i-1].second.getDistance(_path[i].second);
6520  }
6521  if(distanceSoFar <= _localRadius)
6522  {
6523  if(_path[i].first != _path[i-1].first)
6524  {
6525  const Signature * s = _memory->getSignature(_path[i].first);
6526  if(s)
6527  {
6528  if(!s->hasLink(_path[i-1].first) && _memory->getSignature(_path[i-1].first) != 0)
6529  {
6530  Transform virtualLoop = _path[i].second.inverse() * _path[i-1].second;
6531  _memory->addLink(Link(_path[i].first, _path[i-1].first, Link::kVirtualClosure, virtualLoop, cv::Mat::eye(6,6,CV_64FC1)*0.01)); // on the optimized path
6532  UINFO("Added Virtual link between %d and %d", _path[i-1].first, _path[i].first);
6533  }
6534  }
6535  }
6536  }
6537  else
6538  {
6539  break;
6540  }
6541  }
6542  }
6543 
6544  UDEBUG("current node = %d current goal = %d", _path[_pathCurrentIndex].first, _path[_pathGoalIndex].first);
6545  Transform currentPose;
6546  if(_memory->isIncremental())
6547  {
6548  if(_memory->getLastWorkingSignature() == 0 ||
6549  !uContains(_optimizedPoses, _memory->getLastWorkingSignature()->id()))
6550  {
6551  UERROR("Last node is null in memory or not in optimized poses. Aborting the plan...");
6552  this->clearPath(-1);
6553  return;
6554  }
6555  currentPose = _optimizedPoses.at(_memory->getLastWorkingSignature()->id());
6556  }
6557  else
6558  {
6559  if(_lastLocalizationPose.isNull())
6560  {
6561  UERROR("Last localization pose is null. Aborting the plan...");
6562  this->clearPath(-1);
6563  return;
6564  }
6565  currentPose = _lastLocalizationPose;
6566  }
6567 
6568  int goalId = _path.back().first;
6569  if(uContains(_optimizedPoses, goalId))
6570  {
6571  //use local position to know if the goal is reached
6572  float d = currentPose.getDistance(_optimizedPoses.at(goalId)*_pathTransformToGoal);
6573  if(d < _goalReachedRadius)
6574  {
6575  UINFO("Goal %d reached!", goalId);
6576  this->clearPath(1);
6577  }
6578  }
6579 
6580  if(_path.size())
6581  {
6582  //Always check if the farthest node is accessible in local map (max to local space radius if set)
6583  unsigned int goalIndex = _pathCurrentIndex;
6584  float distanceFromCurrentNode = 0.0f;
6585  bool sameGoalIndex = false;
6586  for(unsigned int i=_pathCurrentIndex+1; i<_path.size(); ++i)
6587  {
6588  if(uContains(_optimizedPoses, _path[i].first))
6589  {
6590  if(_localRadius > 0.0f)
6591  {
6592  distanceFromCurrentNode = _path[_pathCurrentIndex].second.getDistance(_path[i].second);
6593  }
6594 
6595  if((goalIndex == _pathCurrentIndex && i == _path.size()-1) ||
6596  _pathUnreachableNodes.find(i) == _pathUnreachableNodes.end())
6597  {
6598  if(distanceFromCurrentNode <= _localRadius)
6599  {
6600  goalIndex = i;
6601  }
6602  else
6603  {
6604  break;
6605  }
6606  }
6607  }
6608  else
6609  {
6610  break;
6611  }
6612  }
6613  UASSERT(_pathGoalIndex < _path.size() && goalIndex < _path.size());
6614  if(_pathGoalIndex != goalIndex)
6615  {
6616  UINFO("Updated current goal from %d to %d (%d/%d)",
6617  (int)_path[_pathGoalIndex].first, _path[goalIndex].first, (int)goalIndex+1, (int)_path.size());
6618  _pathGoalIndex = goalIndex;
6619  }
6620  else
6621  {
6622  sameGoalIndex = true;
6623  }
6624 
6625  // update nearest pose in the path
6626  unsigned int nearestNodeIndex = 0;
6627  float distance = -1.0f;
6628  bool sameCurrentIndex = false;
6629  UASSERT(_pathGoalIndex < _path.size());
6630  for(unsigned int i=_pathCurrentIndex; i<=_pathGoalIndex; ++i)
6631  {
6632  std::map<int, Transform>::iterator iter = _optimizedPoses.find(_path[i].first);
6633  if(iter != _optimizedPoses.end())
6634  {
6635  float d = currentPose.getDistanceSquared(iter->second);
6636  if(distance == -1.0f || distance > d)
6637  {
6638  distance = d;
6639  nearestNodeIndex = i;
6640  }
6641  }
6642  }
6643  if(distance < 0)
6644  {
6645  UERROR("The nearest pose on the path not found! Aborting the plan...");
6646  this->clearPath(-1);
6647  }
6648  else
6649  {
6650  UDEBUG("Nearest node = %d", _path[nearestNodeIndex].first);
6651  }
6652  if(distance >= 0 && nearestNodeIndex != _pathCurrentIndex)
6653  {
6654  _pathCurrentIndex = nearestNodeIndex;
6655  _pathUnreachableNodes.erase(nearestNodeIndex); // if we are on it, it is reachable
6656  }
6657  else
6658  {
6659  sameCurrentIndex = true;
6660  }
6661 
6662  bool isStuck = false;
6663  if(sameGoalIndex && sameCurrentIndex && _pathStuckIterations>0)
6664  {
6665  float distanceToCurrentGoal = 0.0f;
6666  std::map<int, Transform>::iterator iter = _optimizedPoses.find(_path[_pathGoalIndex].first);
6667  if(iter != _optimizedPoses.end())
6668  {
6669  if(_pathGoalIndex == _pathCurrentIndex &&
6670  _pathGoalIndex == _path.size()-1)
6671  {
6672  distanceToCurrentGoal = currentPose.getDistanceSquared(iter->second*_pathTransformToGoal);
6673  }
6674  else
6675  {
6676  distanceToCurrentGoal = currentPose.getDistanceSquared(iter->second);
6677  }
6678  }
6679 
6680  if(distanceToCurrentGoal > 0.0f)
6681  {
6682  if(distanceToCurrentGoal >= _pathStuckDistance)
6683  {
6684  // we are not approaching the goal
6685  isStuck = true;
6686  if(_pathStuckDistance == 0.0f)
6687  {
6688  _pathStuckDistance = distanceToCurrentGoal;
6689  }
6690  }
6691  }
6692  else
6693  {
6694  // no nodes available, cannot plan
6695  isStuck = true;
6696  }
6697  }
6698 
6699  if(isStuck && ++_pathStuckCount > _pathStuckIterations)
6700  {
6701  UWARN("Current goal %d not reached since %d iterations (\"RGBD/PlanStuckIterations\"=%d), mark that node as unreachable.",
6702  _path[_pathGoalIndex].first,
6703  _pathStuckCount,
6704  _pathStuckIterations);
6705  _pathStuckCount = 0;
6706  _pathStuckDistance = 0.0;
6707  _pathUnreachableNodes.insert(_pathGoalIndex);
6708  // select previous reachable one
6709  while(_pathUnreachableNodes.find(_pathGoalIndex) != _pathUnreachableNodes.end())
6710  {
6711  if(_pathGoalIndex == 0 || --_pathGoalIndex <= _pathCurrentIndex)
6712  {
6713  // plan failed!
6714  UERROR("No upcoming nodes on the path are reachable! Aborting the plan...");
6715  this->clearPath(-1);
6716  return;
6717  }
6718  }
6719  }
6720  else if(!isStuck)
6721  {
6722  _pathStuckCount = 0;
6723  _pathStuckDistance = 0.0;
6724  }
6725  }
6726  }
6727 }
6728 
6729 void Rtabmap::createGlobalScanMap()
6730 {
6731  UDEBUG("Creating global scan map (if scans are available)");
6732  _globalScanMap.clear();
6733  _globalScanMapPoses.clear();
6734  std::vector<int> scanIndices;
6735  std::map<int, Transform> scanViewpoints;
6736  for(std::map<int, Transform>::iterator iter=_optimizedPoses.begin(); iter!=_optimizedPoses.end(); ++iter)
6737  {
6738  SensorData data = _memory->getNodeData(iter->first, false, true, false, false);
6739  if(!data.laserScanCompressed().empty())
6740  {
6741  LaserScan scan;
6742  data.uncompressDataConst(0, 0, &scan, 0, 0, 0, 0);
6743  if(!scan.empty())
6744  {
6745  UDEBUG("Adding scan %d (format=%s, points=%d)", iter->first, scan.formatName().c_str(), scan.size());
6746  scan = util3d::transformLaserScan(scan, iter->second*scan.localTransform());
6747  if(_globalScanMap.empty() || _globalScanMap.format() == scan.format())
6748  {
6749  _globalScanMap += scan;
6750  _globalScanMapPoses.insert(*iter);
6751  scanViewpoints.insert(std::make_pair(iter->first, iter->second * scan.localTransform()));
6752  scanIndices.resize(_globalScanMap.size(), iter->first);
6753  }
6754  else
6755  {
6756  UWARN("Incompatible scan formats (%s vs %s), cannot create global scan map.",
6757  _globalScanMap.formatName().c_str(),
6758  scan.formatName().c_str());
6759  _globalScanMap.clear();
6760  _globalScanMapPoses.clear();
6761  break;
6762  }
6763  }
6764  else
6765  {
6766  UDEBUG("Ignored %d (scan is empty), pose still added.", iter->first);
6767  _globalScanMapPoses.insert(*iter);
6768  }
6769  }
6770  else
6771  {
6772  UDEBUG("Ignored %d (no scan), pose still added.", iter->first);
6773  _globalScanMapPoses.insert(*iter);
6774  }
6775  }
6776  if(_globalScanMap.size() > 3)
6777  {
6778  float voxelSize = 0.0f;
6779  int normalK = 0;
6780  float normalRadius = 0.0f;
6781  Parameters::parse(_parameters, Parameters::kMemLaserScanVoxelSize(), voxelSize);
6782  Parameters::parse(_parameters, Parameters::kMemLaserScanNormalK(), normalK);
6783  Parameters::parse(_parameters, Parameters::kMemLaserScanNormalRadius(), normalRadius);
6784 
6785  if(voxelSize > 0.0f)
6786  {
6787  LaserScan voxelScan = util3d::commonFiltering(_globalScanMap, 1, 0, 0, voxelSize, normalK, normalRadius);
6788  if(voxelScan.hasNormals())
6789  {
6790  // adjust with point of views
6791  util3d::adjustNormalsToViewPoints(
6792  scanViewpoints,
6793  _globalScanMap,
6794  scanIndices,
6795  voxelScan);
6796  }
6797  _globalScanMap = voxelScan;
6798  }
6799 
6800  UINFO("Global scan map has been assembled (size=%d points, %d poses) "
6801  "for proximity detection (only in localization mode %s=false and with %s=true)",
6802  (int)_globalScanMap.size(),
6803  (int)_globalScanMapPoses.size(),
6804  Parameters::kMemIncrementalMemory().c_str(),
6805  Parameters::kRGBDProximityGlobalScanMap().c_str());
6806 
6807  //for debugging...
6808  if(!_globalScanMap.empty() && ULogger::level() == ULogger::kDebug)
6809  {
6810  if(!_wDir.empty())
6811  {
6812  UWARN("Saving %s/rtabmap_global_scan_map.pcd (only saved when logger level is debug)", _wDir.c_str());
6813  pcl::PCLPointCloud2::Ptr cloud2 = util3d::laserScanToPointCloud2(_globalScanMap);
6814  pcl::io::savePCDFile(_wDir+"/rtabmap_global_scan_map.pcd", *cloud2);
6815  }
6816  else
6817  {
6818  UWARN("%s is enabled and logger is debug, but %s is not set, cannot save global scan map for debugging.",
6819  Parameters::kRGBDProximityGlobalScanMap().c_str(), Parameters::kRtabmapWorkingDirectory().c_str());
6820  }
6821  }
6822  }
6823  if(!_globalScanMap.empty() && _globalScanMap.size()<100)
6824  {
6825  UWARN("Ignoring global scan map because it is too small (%d points).", (int)_globalScanMap.size());
6826  _globalScanMap.clear();
6827  _globalScanMapPoses.clear();
6828  }
6829 }
6830 
6831 } // namespace rtabmap
rtabmap::Memory::getNodeInfo
bool getNodeInfo(int signatureId, Transform &odomPose, int &mapId, int &weight, std::string &label, double &stamp, Transform &groundTruth, std::vector< float > &velocity, GPS &gps, EnvSensors &sensors, bool lookInDatabase=false) const
Definition: Memory.cpp:4021
uStr2Int
int UTILITE_EXP uStr2Int(const std::string &str)
Definition: UConversion.cpp:125
d
d
rtabmap::Rtabmap::_mapCorrectionBackup
Transform _mapCorrectionBackup
Definition: Rtabmap.h:361
UDirectory::homeDir
static std::string homeDir()
Definition: UDirectory.cpp:355
rtabmap::Rtabmap::_epipolarGeometry
EpipolarGeometry * _epipolarGeometry
Definition: Rtabmap.h:342
uUniqueKeys
std::list< K > uUniqueKeys(const std::multimap< K, V > &mm)
Definition: UStl.h:46
glm::roll
GLM_FUNC_DECL T roll(detail::tquat< T, P > const &x)
uVariance
T uVariance(const T *v, unsigned int size, T meanV)
Definition: UMath.h:491
rtabmap::Rtabmap::_loopThr
float _loopThr
Definition: Rtabmap.h:282
rtabmap::Rtabmap::_rgbdSlamMode
bool _rgbdSlamMode
Definition: Rtabmap.h:293
rtabmap::Parameters::parse
static bool parse(const ParametersMap &parameters, const std::string &key, bool &value)
Definition: Parameters.cpp:497
rtabmap::Memory::removeLink
void removeLink(int idA, int idB)
Definition: Memory.cpp:2717
rtabmap::Rtabmap::_markerPriors
std::map< int, Transform > _markerPriors
Definition: Rtabmap.h:371
rtabmap::Rtabmap::_currentSessionHasGPS
bool _currentSessionHasGPS
Definition: Rtabmap.h:365
rtabmap::Rtabmap::flushStatisticLogs
void flushStatisticLogs()
Definition: Rtabmap.cpp:288
rtabmap::Memory::labelSignature
bool labelSignature(int id, const std::string &label)
Definition: Memory.cpp:2595
rtabmap::Rtabmap::_restartAtOrigin
bool _restartAtOrigin
Definition: Rtabmap.h:324
rtabmap::Rtabmap::_proximityAngle
float _proximityAngle
Definition: Rtabmap.h:311
rtabmap::Memory::cleanupLocalGrids
int cleanupLocalGrids(const std::map< int, Transform > &poses, const cv::Mat &map, float xMin, float yMin, float cellSize, int cropRadius=1, bool filterScans=false)
Definition: Memory.cpp:4183
rtabmap::Memory::getNeighborsId
std::map< int, int > getNeighborsId(int signatureId, int maxGraphDepth, int maxCheckedInDatabase=-1, bool incrementMarginOnLoop=false, bool ignoreLoopIds=false, bool ignoreIntermediateNodes=false, bool ignoreLocalSpaceLoopIds=false, const std::set< int > &nodesSet=std::set< int >(), double *dbAccessTime=0) const
Definition: Memory.cpp:1404
rtabmap::Rtabmap::_maxLocalRetrieved
unsigned int _maxLocalRetrieved
Definition: Rtabmap.h:287
rtabmap::GPS::error
const double & error() const
Definition: GPS.h:63
rtabmap::Statistics::reducedIds
const std::map< int, int > & reducedIds() const
Definition: Statistics.h:290
epsilon
double epsilon
rtabmap::Rtabmap::_path
std::vector< std::pair< int, Transform > > _path
Definition: Rtabmap.h:377
rtabmap::Rtabmap::_proximityByTime
bool _proximityByTime
Definition: Rtabmap.h:300
UTimer
Definition: UTimer.h:46
rtabmap::Transform::getEulerAngles
void getEulerAngles(float &roll, float &pitch, float &yaw) const
Definition: Transform.cpp:253
rtabmap::Rtabmap::setupLogFiles
void setupLogFiles(bool overwrite=false)
Definition: Rtabmap.cpp:182
rtabmap::Rtabmap::isIDsGenerated
bool isIDsGenerated() const
Definition: Rtabmap.cpp:821
rtabmap::Transform::getAngle
float getAngle(float x=1.0f, float y=0.0f, float z=0.0f) const
Definition: Transform.cpp:266
rtabmap::Transform::isInvertible
bool isInvertible() const
Definition: Transform.cpp:169
rtabmap::NearestPathKey::operator<
bool operator<(const NearestPathKey &k) const
Definition: Rtabmap.cpp:1095
rtabmap::Rtabmap::isInSTM
bool isInSTM(int locationId) const
Definition: Rtabmap.cpp:812
rtabmap::Transform::r11
float r11() const
Definition: Transform.h:62
uMean
T uMean(const T *v, unsigned int size)
Definition: UMath.h:401
rtabmap::Transform::r33
float r33() const
Definition: Transform.h:70
rtabmap::Memory::getLastSignatureId
int getLastSignatureId() const
Definition: Memory.cpp:2523
rtabmap::Rtabmap::_optimizeFromGraphEndChanged
bool _optimizeFromGraphEndChanged
Definition: Rtabmap.h:338
rtabmap::GPS::stamp
const double & stamp() const
Definition: GPS.h:59
rtabmap::graph::radiusPosesFiltering
std::map< int, Transform > RTABMAP_EXP radiusPosesFiltering(const std::map< int, Transform > &poses, float radius, float angle, bool keepLatest=true)
Definition: Graph.cpp:1253
rtabmap::graph::findLink
std::multimap< int, Link >::iterator RTABMAP_EXP findLink(std::multimap< int, Link > &links, int from, int to, bool checkBothWays=true, Link::Type type=Link::kUndef)
Definition: Graph.cpp:992
rtabmap::graph::computeMaxGraphErrors
void RTABMAP_EXP computeMaxGraphErrors(const std::map< int, Transform > &poses, const std::multimap< int, Link > &links, float &maxLinearErrorRatio, float &maxAngularErrorRatio, float &maxLinearError, float &maxAngularError, const Link **maxLinearErrorLink=0, const Link **maxAngularErrorLink=0, bool for3DoF=false)
Definition: Graph.cpp:896
rtabmap::Signature::getGroundTruthPose
const Transform & getGroundTruthPose() const
Definition: Signature.h:134
rtabmap::Rtabmap::clearPath
void clearPath(int status)
Definition: Rtabmap.cpp:6114
rtabmap::Transform::r23
float r23() const
Definition: Transform.h:67
rtabmap::Memory::allNodesInWM
bool allNodesInWM() const
Definition: Memory.h:169
rtabmap::LaserScan::maxPoints
int maxPoints() const
Definition: LaserScan.h:116
uIsDigit
bool uIsDigit(const char c)
Definition: UStl.h:622
rtabmap::Memory::getWeights
std::map< int, int > getWeights() const
Definition: Memory.cpp:1973
rtabmap::Signature::getVelocity
const std::vector< float > & getVelocity() const
Definition: Signature.h:135
rtabmap::Rtabmap::exportPoses
void exportPoses(const std::string &path, bool optimized, bool global, int format)
Definition: Rtabmap.cpp:1005
rtabmap::Rtabmap::_pathStuckDistance
float _pathStuckDistance
Definition: Rtabmap.h:383
rtabmap::SensorData::setLaserScan
void setLaserScan(const LaserScan &laserScan, bool clearPreviousData=true)
Definition: SensorData.cpp:381
rtabmap::Rtabmap::_localRadius
float _localRadius
Definition: Rtabmap.h:304
rtabmap::Rtabmap::getPathNextPoses
std::vector< std::pair< int, Transform > > getPathNextPoses() const
Definition: Rtabmap.cpp:6406
f
f
rtabmap::SensorData::laserScanCompressed
const LaserScan & laserScanCompressed() const
Definition: SensorData.h:181
x
x
rtabmap::Rtabmap::_distanceTravelledSinceLastLocalization
float _distanceTravelledSinceLastLocalization
Definition: Rtabmap.h:337
rtabmap::Memory::getStMem
const std::set< int > & getStMem() const
Definition: Memory.h:149
rtabmap::Memory::removeVirtualLinks
void removeVirtualLinks(int signatureId)
Definition: Memory.cpp:3519
ULOGGER_INFO
#define ULOGGER_INFO(...)
Definition: ULogger.h:54
rtabmap::Memory::removeRawData
void removeRawData(int id, bool image=true, bool scan=true, bool userData=true)
Definition: Memory.cpp:2780
rtabmap::Transform::to4DoF
Transform to4DoF() const
Definition: Transform.cpp:217
rtabmap::Rtabmap::computePath
bool computePath(int targetNode, bool global)
Definition: Rtabmap.cpp:6132
rtabmap::Rtabmap::getSTMSize
int getSTMSize() const
Definition: Rtabmap.cpp:790
rtabmap::Rtabmap::createGlobalScanMap
void createGlobalScanMap()
Definition: Rtabmap.cpp:6729
glm::sqrt
GLM_FUNC_DECL vecType< T, P > sqrt(vecType< T, P > const &x)
rtabmap::Rtabmap::_rgbdAngularUpdate
float _rgbdAngularUpdate
Definition: Rtabmap.h:295
rtabmap::Rtabmap::getSignatureCopy
Signature getSignatureCopy(int id, bool images, bool scan, bool userData, bool occupancyGrid, bool withWords, bool withGlobalDescriptors) const
Definition: Rtabmap.cpp:5099
rtabmap::BayesFilter::parseParameters
virtual void parseParameters(const ParametersMap &parameters)
Definition: BayesFilter.cpp:56
rtabmap::Memory::getLastWorkingSignature
const Signature * getLastWorkingSignature() const
Definition: Memory.cpp:2528
rtabmap::Rtabmap::getStatistics
const Statistics & getStatistics() const
Definition: Rtabmap.cpp:830
rtabmap::Rtabmap::_proximityMaxPaths
int _proximityMaxPaths
Definition: Rtabmap.h:307
rtabmap::Memory::loadOptimizedPoses
std::map< int, Transform > loadOptimizedPoses(Transform *lastlocalizationPose) const
Definition: Memory.cpp:2118
rtabmap::Statistics::setLoopClosureId
void setLoopClosureId(int id)
Definition: Statistics.h:238
rtabmap::SensorData::setStereoCameraModels
void setStereoCameraModels(const std::vector< StereoCameraModel > &stereoCameraModels)
Definition: SensorData.h:207
rtabmap::Statistics::setLabels
void setLabels(const std::map< int, std::string > &labels)
Definition: Statistics.h:253
rtabmap::Statistics::setPosterior
void setPosterior(const std::map< int, float > &posterior)
Definition: Statistics.h:255
rtabmap::Rtabmap::dumpPrediction
void dumpPrediction() const
Definition: Rtabmap.cpp:5041
rtabmap::ParametersPair
std::pair< std::string, std::string > ParametersPair
Definition: Parameters.h:44
rtabmap::Transform::r13
float r13() const
Definition: Transform.h:64
rtabmap::Memory::saveOptimizedPoses
void saveOptimizedPoses(const std::map< int, Transform > &optimizedPoses, const Transform &lastlocalizationPose) const
Definition: Memory.cpp:2110
rtabmap::Statistics::setRawLikelihood
void setRawLikelihood(const std::map< int, float > &rawLikelihood)
Definition: Statistics.h:257
rtabmap::Transform::getIdentity
static Transform getIdentity()
Definition: Transform.cpp:401
rtabmap::Rtabmap::_scanMatchingIdsSavedInLinks
bool _scanMatchingIdsSavedInLinks
Definition: Rtabmap.h:302
rtabmap::Statistics::setLoopClosureMapId
void setLoopClosureMapId(int id)
Definition: Statistics.h:239
rtabmap::Memory::isIncremental
bool isIncremental() const
Definition: Memory.h:212
rtabmap::Rtabmap::_publishRAMUsage
bool _publishRAMUsage
Definition: Rtabmap.h:277
rtabmap::Rtabmap::generateDOTGraph
void generateDOTGraph(const std::string &path, int id=0, int margin=5)
Definition: Rtabmap.cpp:973
rtabmap::Rtabmap::getPathCurrentGoalId
int getPathCurrentGoalId() const
Definition: Rtabmap.cpp:6456
rtabmap::Memory::generateGraph
void generateGraph(const std::string &fileName, const std::set< int > &ids=std::set< int >())
Definition: Memory.cpp:4172
rtabmap::Rtabmap::setInitialPose
void setInitialPose(const Transform &initialPose)
Definition: Rtabmap.cpp:840
rtabmap::Statistics::setRefImageId
void setRefImageId(int id)
Definition: Statistics.h:236
rtabmap::graph::findNearestNode
int RTABMAP_EXP findNearestNode(const std::map< int, rtabmap::Transform > &poses, const rtabmap::Transform &targetPose, float *distance=0)
Definition: Graph.cpp:2085
rtabmap::Rtabmap::labelLocation
bool labelLocation(int id, const std::string &label)
Definition: Rtabmap.cpp:917
data
data
rtabmap::Optimizer::isSlam2d
bool isSlam2d() const
Definition: Optimizer.h:92
rtabmap::Statistics::setProximityDetectionId
void setProximityDetectionId(int id)
Definition: Statistics.h:240
uKeysSet
std::set< K > uKeysSet(const std::map< K, V > &m)
Definition: UStl.h:186
rtabmap::Memory::getNodeCalibration
void getNodeCalibration(int nodeId, std::vector< CameraModel > &models, std::vector< StereoCameraModel > &stereoModels) const
Definition: Memory.cpp:4154
rtabmap::Rtabmap::_publishPdf
bool _publishPdf
Definition: Rtabmap.h:275
rtabmap::util3d::laserScanToPointCloud2
pcl::PCLPointCloud2::Ptr RTABMAP_EXP laserScanToPointCloud2(const LaserScan &laserScan, const Transform &transform=Transform())
Definition: util3d.cpp:2158
rtabmap::Memory::getMetricConstraints
void getMetricConstraints(const std::set< int > &ids, std::map< int, Transform > &poses, std::multimap< int, Link > &links, bool lookInDatabase=false, bool landmarksAdded=false)
Definition: Memory.cpp:6228
rtabmap::EpipolarGeometry::parseParameters
void parseParameters(const ParametersMap &parameters)
Definition: EpipolarGeometry.cpp:58
rtabmap::Signature::mapId
int mapId() const
Definition: Signature.h:71
rtabmap::Rtabmap::_rgbdAngularSpeedUpdate
float _rgbdAngularSpeedUpdate
Definition: Rtabmap.h:297
rtabmap::ParametersMap
std::map< std::string, std::string > ParametersMap
Definition: Parameters.h:43
rtabmap::LaserScan::format
Format format() const
Definition: LaserScan.h:113
glm::distance
GLM_FUNC_DECL genType::value_type distance(genType const &p0, genType const &p1)
rtabmap::Rtabmap::_rgbdLinearSpeedUpdate
float _rgbdLinearSpeedUpdate
Definition: Rtabmap.h:296
rtabmap::Rtabmap::_distanceTravelled
float _distanceTravelled
Definition: Rtabmap.h:336
rtabmap::graph::filterLinks
std::multimap< int, Link > RTABMAP_EXP filterLinks(const std::multimap< int, Link > &links, Link::Type filteredType, bool inverted=false)
Definition: Graph.cpp:1154
rtabmap::Rtabmap::_maxOdomCacheSize
int _maxOdomCacheSize
Definition: Rtabmap.h:327
rtabmap::Rtabmap::init
void init(const ParametersMap &parameters, const std::string &databasePath="", bool loadDatabaseParameters=false)
Definition: Rtabmap.cpp:310
rtabmap::Memory::close
void close(bool databaseSaved=true, bool postInitClosingEvents=false, const std::string &ouputDatabasePath="")
Definition: Memory.cpp:474
rtabmap::Transform::r32
float r32() const
Definition: Transform.h:69
rtabmap::SensorData::setCameraModels
void setCameraModels(const std::vector< CameraModel > &models)
Definition: SensorData.h:205
rtabmap::Memory::computeIcpTransform
Transform computeIcpTransform(const Signature &fromS, const Signature &toS, Transform guess, RegistrationInfo *info=0) const
Definition: Memory.cpp:3150
rtabmap::Rtabmap::getLastLocationId
int getLastLocationId() const
Definition: Rtabmap.cpp:740
UMath.h
Basic mathematics functions.
rtabmap::Rtabmap::_markerPriorsAngularVariance
float _markerPriorsAngularVariance
Definition: Rtabmap.h:330
rtabmap::Rtabmap::_maxMemoryAllowed
unsigned int _maxMemoryAllowed
Definition: Rtabmap.h:281
rtabmap::Statistics::setCurrentGoalId
void setCurrentGoalId(int goal)
Definition: Statistics.h:259
rtabmap::SensorData::setGPS
void setGPS(const GPS &gps)
Definition: SensorData.h:286
rtabmap::VWDictionary::getVisualWords
const std::map< int, VisualWord * > & getVisualWords() const
Definition: VWDictionary.h:97
rtabmap::SensorData::setId
void setId(int id)
Definition: SensorData.h:175
rtabmap::Rtabmap::_proximityMaxGraphDepth
int _proximityMaxGraphDepth
Definition: Rtabmap.h:306
rtabmap::Transform::getNorm
float getNorm() const
Definition: Transform.cpp:273
rtabmap::Statistics::setConstraints
void setConstraints(const std::multimap< int, Link > &constraints)
Definition: Statistics.h:249
UConversion.h
Some conversion functions.
rtabmap::Rtabmap::_proximityMergedScanCovFactor
double _proximityMergedScanCovFactor
Definition: Rtabmap.h:313
rtabmap::Rtabmap::globalBundleAdjustment
bool globalBundleAdjustment(int optimizerType=1, bool rematchFeatures=true, int iterations=0, float pixelVariance=0.0f)
Definition: Rtabmap.cpp:5615
rtabmap::Signature::getWeight
int getWeight() const
Definition: Signature.h:74
rtabmap::Statistics::setOdomCacheConstraints
void setOdomCacheConstraints(const std::multimap< int, Link > &constraints)
Definition: Statistics.h:263
rtabmap::Statistics::addSignatureData
void addSignatureData(const Signature &data)
Definition: Statistics.h:245
rtabmap::Rtabmap::_bufferedLogsF
std::list< std::string > _bufferedLogsF
Definition: Rtabmap.h:351
rtabmap::Rtabmap::addNodesToRepublish
void addNodesToRepublish(const std::vector< int > &ids)
Definition: Rtabmap.cpp:6094
rtabmap::Signature::getLabel
const std::string & getLabel() const
Definition: Signature.h:77
UFile::getExtension
std::string getExtension()
Definition: UFile.h:140
glm::pitch
GLM_FUNC_DECL T pitch(detail::tquat< T, P > const &x)
rtabmap::Memory::deleteLocation
void deleteLocation(int locationId, std::list< int > *deletedWords=0)
Definition: Memory.cpp:2688
rtabmap::Rtabmap::_maxLoopClosureDistance
float _maxLoopClosureDistance
Definition: Rtabmap.h:284
rtabmap::LaserScan::isEmpty
bool isEmpty() const
Definition: LaserScan.h:125
rtabmap::Optimizer::optimizeBA
virtual std::map< int, Transform > optimizeBA(int rootId, const std::map< int, Transform > &poses, const std::multimap< int, Link > &links, const std::map< int, std::vector< CameraModel > > &models, std::map< int, cv::Point3f > &points3DMap, const std::map< int, std::map< int, FeatureBA > > &wordReferences, std::set< int > *outliers=0)
Definition: Optimizer.cpp:437
rtabmap::Transform::o34
float o34() const
Definition: Transform.h:74
rtabmap::LaserScan::is2d
bool is2d() const
Definition: LaserScan.h:128
rtabmap::Transform::getTranslationAndEulerAngles
void getTranslationAndEulerAngles(float &x, float &y, float &z, float &roll, float &pitch, float &yaw) const
Definition: Transform.cpp:248
rtabmap::NearestPathKey::NearestPathKey
NearestPathKey(float l, int i, float d)
Definition: Rtabmap.cpp:1091
rtabmap::Transform::r22
float r22() const
Definition: Transform.h:66
rtabmap::Rtabmap::triggerNewMap
int triggerNewMap()
Definition: Rtabmap.cpp:868
rtabmap::Rtabmap::getPaths
std::map< int, std::map< int, Transform > > getPaths(const std::map< int, Transform > &poses, const Transform &target, int maxGraphDepth=0) const
Definition: Rtabmap.cpp:4749
UFATAL
#define UFATAL(...)
rtabmap::BayesFilter::getPredictionLC
const std::vector< double > & getPredictionLC() const
Definition: BayesFilter.cpp:118
rtabmap::Rtabmap::_odomCachePoses
std::map< int, Transform > _odomCachePoses
Definition: Rtabmap.h:368
rtabmap::Memory::getNeighborLinks
std::multimap< int, Link > getNeighborLinks(int signatureId, bool lookInDatabase=false) const
Definition: Memory.cpp:1222
rtabmap::LaserScan::rangeMax
float rangeMax() const
Definition: LaserScan.h:118
rtabmap::Rtabmap::_newMapOdomChangeDistance
float _newMapOdomChangeDistance
Definition: Rtabmap.h:298
prior
prior
rtabmap::Optimizer::gravitySigma
float gravitySigma() const
Definition: Optimizer.h:98
rtabmap::Memory::getDbSavingTime
double getDbSavingTime() const
Definition: Memory.cpp:1709
rtabmap::Memory::saveLocationData
void saveLocationData(int locationId)
Definition: Memory.cpp:2698
rtabmap::Memory::getAllLabels
const std::map< int, std::string > & getAllLabels() const
Definition: Memory.h:167
rtabmap::Rtabmap::_pathLinearVelocity
float _pathLinearVelocity
Definition: Rtabmap.h:322
rtabmap::Rtabmap::getSTM
std::set< int > getSTM() const
Definition: Rtabmap.cpp:781
rtabmap::Signature::isBadSignature
bool isBadSignature() const
Definition: Signature.cpp:290
rtabmap::Rtabmap::_graphOptimizer
Optimizer * _graphOptimizer
Definition: Rtabmap.h:344
rtabmap::Memory::getSignature
const Signature * getSignature(int id) const
Definition: Memory.cpp:1207
rtabmap::Rtabmap::_nodesToRepublish
std::set< int > _nodesToRepublish
Definition: Rtabmap.h:373
rtabmap::Statistics::data
const std::map< std::string, float > & data() const
Definition: Statistics.h:295
rtabmap::Memory::init
bool init(const std::string &dbUrl, bool dbOverwritten=false, const ParametersMap &parameters=ParametersMap(), bool postInitClosingEvents=false)
Definition: Memory.cpp:161
rtabmap::Memory::getGPS
void getGPS(int id, GPS &gps, Transform &offsetENU, bool lookInDatabase, int maxGraphDepth=0) const
Definition: Memory.cpp:3973
rtabmap::Signature::getLandmarks
const std::map< int, Link > & getLandmarks() const
Definition: Signature.h:94
rtabmap::Rtabmap::_proximityBySpace
bool _proximityBySpace
Definition: Rtabmap.h:301
rtabmap::BayesFilter::generatePrediction
cv::Mat generatePrediction(const Memory *memory, const std::vector< int > &ids)
Definition: BayesFilter.cpp:273
rtabmap::Rtabmap::adjustLikelihood
void adjustLikelihood(std::map< int, float > &likelihood) const
Definition: Rtabmap.cpp:4970
rtabmap::Rtabmap::_proximityRawPosesUsed
bool _proximityRawPosesUsed
Definition: Rtabmap.h:310
rtabmap::DBDriver::openConnection
bool openConnection(const std::string &url, bool overwritten=false)
Definition: DBDriver.cpp:86
rtabmap::Rtabmap::_globalScanMap
LaserScan _globalScanMap
Definition: Rtabmap.h:366
rtabmap::Rtabmap::_optimizationMaxError
float _optimizationMaxError
Definition: Rtabmap.h:316
UASSERT
#define UASSERT(condition)
rtabmap::Rtabmap::optimizeGraph
std::map< int, Transform > optimizeGraph(int fromId, const std::set< int > &ids, const std::map< int, Transform > &guessPoses, bool lookInDatabase, cv::Mat &covariance, std::multimap< int, Link > *constraints=0, double *error=0, int *iterationsDone=0) const
Definition: Rtabmap.cpp:4877
rtabmap::Rtabmap::_markerPriorsLinearVariance
float _markerPriorsLinearVariance
Definition: Rtabmap.h:329
rtabmap::Rtabmap::_bayesFilter
BayesFilter * _bayesFilter
Definition: Rtabmap.h:343
rtabmap::Statistics::setLocalPath
void setLocalPath(const std::vector< int > &localPath)
Definition: Statistics.h:258
rtabmap::Memory::saveStatistics
void saveStatistics(const Statistics &statistics, bool saveWMState)
Definition: Memory.cpp:2086
uSplit
std::list< std::string > uSplit(const std::string &str, char separator=' ')
Definition: UStl.h:566
rtabmap::GPS::bearing
const double & bearing() const
Definition: GPS.h:64
rtabmap::Memory::reactivateSignatures
std::set< int > reactivateSignatures(const std::list< int > &ids, unsigned int maxLoaded, double &timeDbAccess)
Definition: Memory.cpp:6147
rtabmap::Rtabmap::getNodesInRadius
std::map< int, Transform > getNodesInRadius(const Transform &pose, float radius, int k=0, std::map< int, float > *distsSqr=0)
Definition: Rtabmap.cpp:5261
rtabmap::SensorData::id
int id() const
Definition: SensorData.h:174
rtabmap::Statistics::setLocalizationCovariance
void setLocalizationCovariance(const cv::Mat &covariance)
Definition: Statistics.h:252
rtabmap::Rtabmap::_statisticLoggedHeaders
bool _statisticLoggedHeaders
Definition: Rtabmap.h:292
rtabmap::Rtabmap::dumpData
void dumpData() const
Definition: Rtabmap.cpp:4615
rtabmap::LaserScan::hasNormals
bool hasNormals() const
Definition: LaserScan.h:129
rtabmap::Rtabmap::_rgbdLinearUpdate
float _rgbdLinearUpdate
Definition: Rtabmap.h:294
rtabmap::Rtabmap::_goalReachedRadius
float _goalReachedRadius
Definition: Rtabmap.h:319
rtabmap::util3d::commonFiltering
LaserScan RTABMAP_EXP commonFiltering(const LaserScan &scan, int downsamplingStep, float rangeMin=0.0f, float rangeMax=0.0f, float voxelSize=0.0f, int normalK=0, float normalRadius=0.0f, float groundNormalsUp=0.0f)
Definition: util3d_filtering.cpp:74
uMax3
T uMax3(const T &a, const T &b, const T &c)
Definition: UMath.h:80
rtabmap::Optimizer::parseParameters
virtual void parseParameters(const ParametersMap &parameters)
Definition: Optimizer.cpp:329
rtabmap::Rtabmap::close
void close(bool databaseSaved=true, const std::string &ouputDatabasePath="")
Definition: Rtabmap.cpp:453
rtabmap::Memory::computeTransform
Transform computeTransform(Signature &fromS, Signature &toS, Transform guess, RegistrationInfo *info=0, bool useKnownCorrespondencesIfPossible=false) const
Definition: Memory.cpp:2823
rtabmap::Optimizer::setPriorsIgnored
void setPriorsIgnored(bool enabled)
Definition: Optimizer.h:106
rtabmap::Signature::getLinks
const std::multimap< int, Link > & getLinks() const
Definition: Signature.h:100
rtabmap::Optimizer::landmarksIgnored
bool landmarksIgnored() const
Definition: Optimizer.h:97
rtabmap::Memory::isOdomGravityUsed
bool isOdomGravityUsed() const
Definition: Memory.h:223
rtabmap::SensorData::gps
const GPS & gps() const
Definition: SensorData.h:287
rtabmap::ProgressState::callback
virtual bool callback(const std::string &msg) const
Definition: ProgressState.h:39
rtabmap::Rtabmap::getWMSize
int getWMSize() const
Definition: Rtabmap.cpp:761
ULOGGER_DEBUG
#define ULOGGER_DEBUG(...)
Definition: ULogger.h:53
rtabmap::Statistics::setPoses
void setPoses(const std::map< int, Transform > &poses)
Definition: Statistics.h:248
rtabmap::Rtabmap::setWorkingDirectory
void setWorkingDirectory(std::string path)
Definition: Rtabmap.cpp:4473
rtabmap::Rtabmap::statistics_
Statistics statistics_
Definition: Rtabmap.h:354
rtabmap::Signature::addLandmark
void addLandmark(const Link &landmark)
Definition: Signature.h:93
rtabmap::Rtabmap::_maxRepublished
unsigned int _maxRepublished
Definition: Rtabmap.h:288
rtabmap::Statistics::setExtended
void setExtended(bool extended)
Definition: Statistics.h:235
rtabmap::Memory::save2DMap
void save2DMap(const cv::Mat &map, float xMin, float yMin, float cellSize) const
Definition: Memory.cpp:2150
rtabmap::Rtabmap::_foutInt
FILE * _foutInt
Definition: Rtabmap.h:350
rtabmap::Rtabmap::detectMoreLoopClosures
int detectMoreLoopClosures(float clusterRadiusMax=0.5f, float clusterAngle=M_PI/6.0f, int iterations=1, bool intraSession=true, bool interSession=true, const ProgressState *state=0, float clusterRadiusMin=0.0f)
Definition: Rtabmap.cpp:5305
rtabmap::Rtabmap::_saveWMState
bool _saveWMState
Definition: Rtabmap.h:279
rtabmap::Transform::o24
float o24() const
Definition: Transform.h:73
rtabmap::Rtabmap::getPathNextNodes
std::vector< int > getPathNextNodes() const
Definition: Rtabmap.cpp:6431
UASSERT_MSG
#define UASSERT_MSG(condition, msg_str)
Definition: ULogger.h:67
rtabmap::util3d::transformLaserScan
LaserScan RTABMAP_EXP transformLaserScan(const LaserScan &laserScan, const Transform &transform)
Definition: util3d_transforms.cpp:39
rtabmap::Rtabmap::getWM
std::list< int > getWM() const
Definition: Rtabmap.cpp:750
rtabmap::Memory::update
bool update(const SensorData &data, Statistics *stats=0)
Definition: Memory.cpp:819
rtabmap::Rtabmap::_memory
Memory * _memory
Definition: Rtabmap.h:347
util3d_filtering.h
rtabmap::Rtabmap::_pathStuckCount
int _pathStuckCount
Definition: Rtabmap.h:382
rtabmap::Memory::joinTrashThread
void joinTrashThread()
Definition: Memory.cpp:2207
rtabmap::Optimizer::optimize
std::map< int, Transform > optimize(int rootId, const std::map< int, Transform > &poses, const std::multimap< int, Link > &constraints, std::list< std::map< int, Transform > > *intermediateGraphes=0, double *finalError=0, int *iterationsDone=0)
Definition: Optimizer.cpp:406
rtabmap::Rtabmap::getWorkingDir
const std::string & getWorkingDir() const
Definition: Rtabmap.h:126
rtabmap::graph::calcRMSE
Transform RTABMAP_EXP calcRMSE(const std::map< int, Transform > &groundTruth, const std::map< int, Transform > &poses, float &translational_rmse, float &translational_mean, float &translational_median, float &translational_std, float &translational_min, float &translational_max, float &rotational_rmse, float &rotational_mean, float &rotational_median, float &rotational_std, float &rotational_min, float &rotational_max)
Definition: Graph.cpp:759
rtabmap::Transform::prettyPrint
std::string prettyPrint() const
Definition: Transform.cpp:316
rtabmap::Statistics::setRefImageMapId
void setRefImageMapId(int id)
Definition: Statistics.h:237
rtabmap::Rtabmap::_loopCovLimited
bool _loopCovLimited
Definition: Rtabmap.h:325
rtabmap::Optimizer::iterations
int iterations() const
Definition: Optimizer.h:91
rtabmap::Rtabmap::_statisticLogsBufferedInRAM
bool _statisticLogsBufferedInRAM
Definition: Rtabmap.h:290
rtabmap::Signature::hasLink
bool hasLink(int idTo, Link::Type type=Link::kUndef) const
Definition: Signature.cpp:129
rtabmap::Rtabmap::deleteLastLocation
void deleteLastLocation()
Definition: Rtabmap.cpp:4559
rtabmap::Rtabmap::_foutFloat
FILE * _foutFloat
Definition: Rtabmap.h:349
rtabmap::Rtabmap::addLink
bool addLink(const Link &link)
Definition: Rtabmap.cpp:5743
rtabmap::Rtabmap::process
bool process(const SensorData &data, Transform odomPose, const cv::Mat &odomCovariance=cv::Mat::eye(6, 6, CV_64FC1), const std::vector< float > &odomVelocity=std::vector< float >(), const std::map< std::string, float > &externalStats=std::map< std::string, float >())
Main loop of rtabmap.
Definition: Rtabmap.cpp:1151
rtabmap::LaserScan::empty
bool empty() const
Definition: LaserScan.h:124
rtabmap::GeodeticCoords::Geocentric_WGS84ToENU_WGS84
static cv::Point3d Geocentric_WGS84ToENU_WGS84(const cv::Point3d &geocentric_WGS84, const cv::Point3d &origin_geocentric_WGS84, const GeodeticCoords &origin)
Definition: GeodeticCoords.cpp:119
rtabmap::Signature::setVelocity
void setVelocity(float vx, float vy, float vz, float vroll, float vpitch, float vyaw)
Definition: Signature.h:121
rtabmap::Transform::o14
float o14() const
Definition: Transform.h:72
uReplaceChar
std::string UTILITE_EXP uReplaceChar(const std::string &str, char before, char after)
Definition: UConversion.cpp:33
rtabmap::EpipolarGeometry::check
bool check(const Signature *ssA, const Signature *ssB)
Definition: EpipolarGeometry.cpp:65
rtabmap::graph::computePath
std::list< std::pair< int, Transform > > RTABMAP_EXP computePath(const std::map< int, rtabmap::Transform > &poses, const std::multimap< int, int > &links, int from, int to, bool updateNewCosts=false)
Definition: Graph.cpp:1658
LOG_F
#define LOG_F
Definition: Rtabmap.cpp:69
rtabmap::Memory::emptyTrash
void emptyTrash()
Definition: Memory.cpp:2199
rtabmap::Memory::isIDsGenerated
bool isIDsGenerated() const
Definition: Memory.h:218
rtabmap::Rtabmap::_proximityOdomGuess
bool _proximityOdomGuess
Definition: Rtabmap.h:312
rtabmap::Memory::getNeighborsIdRadius
std::map< int, float > getNeighborsIdRadius(int signatureId, float radius, const std::map< int, Transform > &optimizedPoses, int maxGraphDepth) const
Definition: Memory.cpp:1581
rtabmap::Rtabmap::_lastLocalizationPose
Transform _lastLocalizationPose
Definition: Rtabmap.h:362
rtabmap::Rtabmap::_odomCacheConstraints
std::multimap< int, Link > _odomCacheConstraints
Definition: Rtabmap.h:369
rtabmap::Memory::forget
std::list< int > forget(const std::set< int > &ignoredIds=std::set< int >())
Definition: Memory.cpp:1995
rtabmap::Memory::getLastGlobalLoopClosureId
int getLastGlobalLoopClosureId() const
Definition: Memory.h:219
uSplitNumChar
std::list< std::string > uSplitNumChar(const std::string &str)
Definition: UStl.h:672
UTimer::start
void start()
Definition: UTimer.cpp:87
rtabmap::compressData2
cv::Mat RTABMAP_EXP compressData2(const cv::Mat &data)
Definition: Compression.cpp:201
rtabmap::Memory::getOdomMaxInf
const std::vector< double > & getOdomMaxInf() const
Definition: Memory.h:222
rtabmap::Rtabmap::_pathCurrentIndex
unsigned int _pathCurrentIndex
Definition: Rtabmap.h:379
rtabmap::Memory::kIdInvalid
static const int kIdInvalid
Definition: Memory.h:68
ULogger::level
static ULogger::Level level()
Definition: ULogger.h:340
rtabmap::Rtabmap::_maxTimeAllowed
float _maxTimeAllowed
Definition: Rtabmap.h:280
util3d_transforms.h
rtabmap::Rtabmap::_pathAngularVelocity
float _pathAngularVelocity
Definition: Rtabmap.h:323
rtabmap::Rtabmap::_wDir
std::string _wDir
Definition: Rtabmap.h:356
rtabmap::Rtabmap::_loopClosureIdentityGuess
bool _loopClosureIdentityGuess
Definition: Rtabmap.h:303
rtabmap::Rtabmap::updateGoalIndex
void updateGoalIndex()
Definition: Rtabmap.cpp:6466
params
params
rtabmap::Transform::normalizeRotation
void normalizeRotation()
Definition: Transform.cpp:306
rtabmap::Memory::getLoopClosureLinks
std::multimap< int, Link > getLoopClosureLinks(int signatureId, bool lookInDatabase=false) const
Definition: Memory.cpp:1263
rtabmap::Rtabmap::_statisticLogged
bool _statisticLogged
Definition: Rtabmap.h:291
rtabmap::Rtabmap::getWeights
std::map< int, int > getWeights() const
Definition: Rtabmap.cpp:770
rtabmap::SensorData::setEnvSensors
void setEnvSensors(const EnvSensors &sensors)
Definition: SensorData.h:292
rtabmap::Optimizer::getConnectedGraph
void getConnectedGraph(int fromId, const std::map< int, Transform > &posesIn, const std::multimap< int, Link > &linksIn, std::map< int, Transform > &posesOut, std::multimap< int, Link > &linksOut, bool adjustPosesWithConstraints=true) const
Definition: Optimizer.cpp:188
rtabmap::GPS::toGeodeticCoords
GeodeticCoords toGeodeticCoords() const
Definition: GPS.h:66
uContains
bool uContains(const std::list< V > &list, const V &value)
Definition: UStl.h:409
rtabmap::Transform::getNormSquared
float getNormSquared() const
Definition: Transform.cpp:278
rtabmap::graph::radiusPosesClustering
std::multimap< int, int > RTABMAP_EXP radiusPosesClustering(const std::map< int, Transform > &poses, float radius, float angle)
Definition: Graph.cpp:1365
rtabmap::Memory::getNodeData
SensorData getNodeData(int locationId, bool images, bool scan, bool userData, bool occupancyGrid) const
Definition: Memory.cpp:4071
rtabmap::Memory::getDatabaseMemoryUsed
int getDatabaseMemoryUsed() const
Definition: Memory.cpp:1678
rtabmap::Rtabmap::_highestHypothesis
std::pair< int, float > _highestHypothesis
Definition: Rtabmap.h:333
rtabmap::graph::exportPoses
bool RTABMAP_EXP exportPoses(const std::string &filePath, int format, const std::map< int, Transform > &poses, const std::multimap< int, Link > &constraints=std::multimap< int, Link >(), const std::map< int, double > &stamps=std::map< int, double >(), const ParametersMap &parameters=ParametersMap())
Definition: Graph.cpp:54
rtabmap::DBDriver::create
static DBDriver * create(const ParametersMap &parameters=ParametersMap())
Definition: DBDriver.cpp:41
rtabmap::Rtabmap::parseParameters
void parseParameters(const ParametersMap &parameters)
Definition: Rtabmap.cpp:535
rtabmap::Memory::getGroundTruths
const std::map< int, Transform > & getGroundTruths() const
Definition: Memory.h:186
rtabmap::Statistics::setMapCorrection
void setMapCorrection(const Transform &mapCorrection)
Definition: Statistics.h:250
rtabmap::Statistics::setLikelihood
void setLikelihood(const std::map< int, float > &likelihood)
Definition: Statistics.h:256
rtabmap::Transform::rotation
Transform rotation() const
Definition: Transform.cpp:195
rtabmap::Transform::getDistance
float getDistance(const Transform &t) const
Definition: Transform.cpp:283
rtabmap::Transform::isNull
bool isNull() const
Definition: Transform.cpp:107
rtabmap::Rtabmap::getInformation
cv::Mat getInformation(const cv::Mat &covariance) const
Definition: Rtabmap.cpp:6074
rtabmap::Memory::incrementMapId
int incrementMapId(std::map< int, int > *reducedIds=0)
Definition: Memory.cpp:1646
rtabmap::Rtabmap::_startNewMapOnLoopClosure
bool _startNewMapOnLoopClosure
Definition: Rtabmap.h:317
rtabmap::Memory::dumpMemory
virtual void dumpMemory(std::string directory) const
Definition: Memory.cpp:3549
false
#define false
Definition: ConvertUTF.c:56
rtabmap::Memory::removeAllVirtualLinks
void removeAllVirtualLinks()
Definition: Memory.cpp:3510
uListToVector
std::vector< V > uListToVector(const std::list< V > &list)
Definition: UStl.h:475
rtabmap::Rtabmap::_someNodesHaveBeenTransferred
bool _someNodesHaveBeenTransferred
Definition: Rtabmap.h:335
rtabmap::Transform::r21
float r21() const
Definition: Transform.h:65
rtabmap::Parameters::getDefaultParameters
static const ParametersMap & getDefaultParameters()
Definition: Parameters.h:807
rtabmap::Transform::getDistanceSquared
float getDistanceSquared(const Transform &t) const
Definition: Transform.cpp:288
rtabmap::Statistics::setWmState
void setWmState(const std::vector< int > &state)
Definition: Statistics.h:261
LOG_I
#define LOG_I
Definition: Rtabmap.cpp:70
rtabmap::Memory::parseParameters
virtual void parseParameters(const ParametersMap &parameters)
Definition: Memory.cpp:547
rtabmap::Memory::getWorkingMem
const std::map< int, double > & getWorkingMem() const
Definition: Memory.h:148
rtabmap::Rtabmap::_publishLikelihood
bool _publishLikelihood
Definition: Rtabmap.h:276
rtabmap::Rtabmap::getForwardWMPoses
std::map< int, Transform > getForwardWMPoses(int fromId, int maxNearestNeighbors, float radius, int maxDiffID) const
Definition: Rtabmap.cpp:4633
rtabmap::Rtabmap::_mapCorrection
Transform _mapCorrection
Definition: Rtabmap.h:360
rtabmap::Rtabmap::_computeRMSE
bool _computeRMSE
Definition: Rtabmap.h:278
rtabmap::Rtabmap::_gpsGeocentricCache
std::map< int, std::pair< cv::Point3d, Transform > > _gpsGeocentricCache
Definition: Rtabmap.h:364
rtabmap::SensorData::laserScanRaw
const LaserScan & laserScanRaw() const
Definition: SensorData.h:185
rtabmap::Statistics::setStamp
void setStamp(double stamp)
Definition: Statistics.h:242
rtabmap::Transform::r12
float r12() const
Definition: Transform.h:63
rtabmap::SensorData::setGlobalDescriptors
void setGlobalDescriptors(const std::vector< GlobalDescriptor > &descriptors)
Definition: SensorData.h:275
UDEBUG
#define UDEBUG(...)
rtabmap::Rtabmap::cleanupLocalGrids
int cleanupLocalGrids(const std::map< int, Transform > &mapPoses, const cv::Mat &map, float xMin, float yMin, float cellSize, int cropRadius=1, bool filterScans=false)
Definition: Rtabmap.cpp:5674
glm::max
GLM_FUNC_DECL genType max(genType const &x, genType const &y)
rtabmap::Memory::kIdVirtual
static const int kIdVirtual
Definition: Memory.h:67
rtabmap::Signature::sensorData
SensorData & sensorData()
Definition: Signature.h:137
rtabmap::ProgressState::isCanceled
bool isCanceled() const
Definition: ProgressState.h:51
uNormSquared
T uNormSquared(const std::vector< T > &v)
Definition: UMath.h:560
rtabmap::IMU::empty
bool empty() const
Definition: IMU.h:67
rtabmap::Memory::updateLink
void updateLink(const Link &link, bool updateInDatabase=false)
Definition: Memory.cpp:3450
uKeysList
std::list< K > uKeysList(const std::multimap< K, V > &mm)
Definition: UStl.h:84
rtabmap::Rtabmap::_proximityFilteringRadius
float _proximityFilteringRadius
Definition: Rtabmap.h:309
rtabmap::Rtabmap::resetMemory
void resetMemory()
Definition: Rtabmap.cpp:1044
UFile::exists
bool exists()
Definition: UFile.h:104
rtabmap::graph::findNearestPoses
std::map< int, Transform > RTABMAP_EXP findNearestPoses(int nodeId, const std::map< int, Transform > &poses, float radius, float angle=0.0f, int k=0)
Definition: Graph.cpp:2193
rtabmap::Memory::getMapId
int getMapId(int id, bool lookInDatabase=false) const
Definition: Memory.cpp:3934
rtabmap::Signature::setWords
void setWords(const std::multimap< int, int > &words, const std::vector< cv::KeyPoint > &keypoints, const std::vector< cv::Point3f > &words3, const cv::Mat &descriptors)
Definition: Signature.cpp:262
rtabmap::Statistics::setWeights
void setWeights(const std::map< int, int > &weights)
Definition: Statistics.h:254
UProcessInfo::getMemoryUsage
static long int getMemoryUsage()
Definition: UProcessInfo.cpp:38
dist
dist
rtabmap::Rtabmap::_pathGoalIndex
unsigned int _pathGoalIndex
Definition: Rtabmap.h:380
UERROR
#define UERROR(...)
rtabmap::Memory::isGraphReduced
bool isGraphReduced() const
Definition: Memory.h:221
rtabmap::Rtabmap::_globalScanMapPoses
std::map< int, Transform > _globalScanMapPoses
Definition: Rtabmap.h:367
rtabmap::VWDictionary::getIndexMemoryUsed
unsigned int getIndexMemoryUsed() const
Definition: VWDictionary.cpp:364
rtabmap::Rtabmap::setUserData
bool setUserData(int id, const cv::Mat &data)
Definition: Rtabmap.cpp:953
ULOGGER_WARN
#define ULOGGER_WARN(...)
Definition: ULogger.h:55
rtabmap::Signature::getPose
const Transform & getPose() const
Definition: Signature.h:132
ULogger.h
ULogger class and convenient macros.
UWARN
#define UWARN(...)
rtabmap::Rtabmap::_odomCorrectionAcc
std::vector< float > _odomCorrectionAcc
Definition: Rtabmap.h:370
rtabmap::Rtabmap::_constraints
std::multimap< int, Link > _constraints
Definition: Rtabmap.h:359
rtabmap::Signature::id
int id() const
Definition: Signature.h:70
rtabmap::SensorData::stamp
double stamp() const
Definition: SensorData.h:176
rtabmap::Rtabmap::_optimizeFromGraphEnd
bool _optimizeFromGraphEnd
Definition: Rtabmap.h:315
rtabmap::Rtabmap::getGraph
void getGraph(std::map< int, Transform > &poses, std::multimap< int, Link > &constraints, bool optimized, bool global, std::map< int, Signature > *signatures=0, bool withImages=false, bool withScan=false, bool withUserData=false, bool withGrid=false, bool withWords=true, bool withGlobalDescriptors=true) const
Definition: Rtabmap.cpp:5189
rtabmap::DBDriver::getLastParameters
ParametersMap getLastParameters() const
Definition: DBDriver.cpp:239
rtabmap::Memory::getMemoryUsed
unsigned long getMemoryUsed() const
Definition: Memory.cpp:3676
UTimer::ticks
double ticks()
Definition: UTimer.cpp:117
glm::yaw
GLM_FUNC_DECL T yaw(detail::tquat< T, P > const &x)
rtabmap::Memory::isLocalizationDataSaved
bool isLocalizationDataSaved() const
Definition: Memory.h:213
rtabmap::Rtabmap::_publishLastSignatureData
bool _publishLastSignatureData
Definition: Rtabmap.h:274
rtabmap::Memory::computeLikelihood
std::map< int, float > computeLikelihood(const Signature *signature, const std::list< int > &ids)
Definition: Memory.cpp:1855
rtabmap::Rtabmap::_bufferedLogsI
std::list< std::string > _bufferedLogsI
Definition: Rtabmap.h:352
rtabmap::Rtabmap::_lastProcessTime
double _lastProcessTime
Definition: Rtabmap.h:334
UDirectory::exists
static bool exists(const std::string &dirPath)
Definition: UDirectory.cpp:249
rtabmap::Parameters::readINI
static void readINI(const std::string &configFile, ParametersMap &parameters, bool modifiedOnly=false)
Definition: Parameters.cpp:1161
rtabmap::Signature::isSaved
bool isSaved() const
Definition: Signature.h:101
rtabmap::Statistics::setLoopClosureTransform
void setLoopClosureTransform(const Transform &loopClosureTransform)
Definition: Statistics.h:251
rtabmap::Signature::addLink
void addLink(const Link &link)
Definition: Signature.cpp:119
rtabmap::Rtabmap::_startNewMapOnGoodSignature
bool _startNewMapOnGoodSignature
Definition: Rtabmap.h:318
rtabmap::Rtabmap::_pathStuckIterations
int _pathStuckIterations
Definition: Rtabmap.h:321
rtabmap::Transform::r31
float r31() const
Definition: Transform.h:68
rtabmap::Memory::getLinks
std::multimap< int, Link > getLinks(int signatureId, bool lookInDatabase=false, bool withLandmarks=false) const
Definition: Memory.cpp:1303
rtabmap::Rtabmap::setTimeThreshold
void setTimeThreshold(float maxTimeAllowed)
Definition: Rtabmap.cpp:4458
rtabmap::Rtabmap::getPose
Transform getPose(int locationId) const
Definition: Rtabmap.cpp:835
rtabmap::Rtabmap::_parameters
ParametersMap _parameters
Definition: Rtabmap.h:345
rtabmap::Transform::to3DoF
Transform to3DoF() const
Definition: Transform.cpp:210
rtabmap::Rtabmap::getTotalMemSize
int getTotalMemSize() const
Definition: Rtabmap.cpp:799
rtabmap::Rtabmap::_lastLocalizationNodeId
int _lastLocalizationNodeId
Definition: Rtabmap.h:363
rtabmap::Statistics::setProximityDetectionMapId
void setProximityDetectionMapId(int id)
Definition: Statistics.h:241
rtabmap::util3d::adjustNormalsToViewPoints
void RTABMAP_EXP adjustNormalsToViewPoints(const std::map< int, Transform > &poses, const pcl::PointCloud< pcl::PointXYZ >::Ptr &rawCloud, const std::vector< int > &rawCameraIndices, pcl::PointCloud< pcl::PointNormal >::Ptr &cloud, float groundNormalsUp=0.0f)
Definition: util3d_surface.cpp:3660
rtabmap::EnvSensors
std::map< EnvSensor::Type, EnvSensor > EnvSensors
Definition: EnvSensor.h:81
rtabmap::Rtabmap::~Rtabmap
virtual ~Rtabmap()
Definition: Rtabmap.cpp:177
rtabmap::LaserScan::formatName
static std::string formatName(const Format &format)
Definition: LaserScan.cpp:34
rtabmap::Transform::theta
float theta() const
Definition: Transform.cpp:162
rtabmap::Memory::updateAge
void updateAge(int signatureId)
Definition: Memory.cpp:1669
rtabmap::Rtabmap::_loopRatio
float _loopRatio
Definition: Rtabmap.h:283
rtabmap::Rtabmap::_goalsSavedInUserData
bool _goalsSavedInUserData
Definition: Rtabmap.h:320
rtabmap::Rtabmap::_loopClosureHypothesis
std::pair< int, float > _loopClosureHypothesis
Definition: Rtabmap.h:332
rtabmap::GeodeticCoords::toGeocentric_WGS84
cv::Point3d toGeocentric_WGS84() const
Definition: GeodeticCoords.cpp:75
rtabmap::Rtabmap::_localImmunizationRatio
float _localImmunizationRatio
Definition: Rtabmap.h:305
rtabmap::Rtabmap::_pathUnreachableNodes
std::set< unsigned int > _pathUnreachableNodes
Definition: Rtabmap.h:378
rtabmap::Rtabmap::_rawDataKept
bool _rawDataKept
Definition: Rtabmap.h:289
rtabmap::SensorData::imu
const IMU & imu() const
Definition: SensorData.h:290
rtabmap::BayesFilter::getMemoryUsed
unsigned long getMemoryUsed() const
Definition: BayesFilter.cpp:420
rtabmap::Rtabmap::_neighborLinkRefining
bool _neighborLinkRefining
Definition: Rtabmap.h:299
ULOGGER_ERROR
#define ULOGGER_ERROR(...)
Definition: ULogger.h:56
rtabmap::Transform::fromString
static Transform fromString(const std::string &string)
Definition: Transform.cpp:465
rtabmap::Memory::getVWDictionary
const VWDictionary * getVWDictionary() const
Definition: Memory.cpp:1217
rtabmap::Rtabmap::_verifyLoopClosureHypothesis
bool _verifyLoopClosureHypothesis
Definition: Rtabmap.h:285
newDatabase
static int newDatabase(BtShared *pBt)
Definition: sqlite3.c:52932
rtabmap::Memory::getAllSignatureIds
std::set< int > getAllSignatureIds(bool ignoreChildren=true) const
Definition: Memory.cpp:1714
rtabmap::Memory::computeIcpTransformMulti
Transform computeIcpTransformMulti(int newId, int oldId, const std::map< int, Transform > &poses, RegistrationInfo *info=0)
Definition: Memory.cpp:3164
rtabmap::Optimizer::priorsIgnored
bool priorsIgnored() const
Definition: Optimizer.h:96
rtabmap::Rtabmap::_optimizedPoses
std::map< int, Transform > _optimizedPoses
Definition: Rtabmap.h:358
rtabmap::graph::computePathLength
float RTABMAP_EXP computePathLength(const std::vector< std::pair< int, Transform > > &path, unsigned int fromIndex=0, unsigned int toIndex=0)
Definition: Graph.cpp:2248
uFormat
std::string UTILITE_EXP uFormat(const char *fmt,...)
Definition: UConversion.cpp:365
uNumber2Str
std::string UTILITE_EXP uNumber2Str(unsigned int number)
Definition: UConversion.cpp:91
rtabmap::Rtabmap::rejectLastLoopClosure
void rejectLastLoopClosure()
Definition: Rtabmap.cpp:4501
rtabmap::Memory::setUserData
bool setUserData(int id, const cv::Mat &data)
Definition: Memory.cpp:2673
rtabmap::Memory::getMaxStMemSize
int getMaxStMemSize() const
Definition: Memory.h:150
rtabmap::SensorData::uncompressDataConst
void uncompressDataConst(cv::Mat *imageRaw, cv::Mat *depthOrRightRaw, LaserScan *laserScanRaw=0, cv::Mat *userDataRaw=0, cv::Mat *groundCellsRaw=0, cv::Mat *obstacleCellsRaw=0, cv::Mat *emptyCellsRaw=0) const
Definition: SensorData.cpp:623
rtabmap::Memory::isInSTM
bool isInSTM(int signatureId) const
Definition: Memory.h:215
rtabmap::graph::findNearestNodes
std::map< int, float > RTABMAP_EXP findNearestNodes(int nodeId, const std::map< int, Transform > &poses, float radius, float angle=0.0f, int k=0)
Definition: Graph.cpp:2104
rtabmap::Rtabmap::_publishStats
bool _publishStats
Definition: Rtabmap.h:273
rtabmap::LaserScan::localTransform
Transform localTransform() const
Definition: LaserScan.h:122
rtabmap::Rtabmap::optimizeCurrentMap
void optimizeCurrentMap(int id, bool lookInDatabase, std::map< int, Transform > &optimizedPoses, cv::Mat &covariance, std::multimap< int, Link > *constraints=0, double *error=0, int *iterationsDone=0) const
Definition: Rtabmap.cpp:4831
rtabmap::Optimizer::create
static Optimizer * create(const ParametersMap &parameters)
Definition: Optimizer.cpp:72
uValue
V uValue(const std::map< K, V > &m, const K &key, const V &defaultValue=V())
Definition: UStl.h:240
rtabmap::Rtabmap::setOptimizedPoses
void setOptimizedPoses(const std::map< int, Transform > &poses, const std::multimap< int, Link > &constraints)
Definition: Rtabmap.cpp:4609
rtabmap::LaserScan::size
int size() const
Definition: LaserScan.h:126
rtabmap::Rtabmap::_proximityMaxNeighbors
int _proximityMaxNeighbors
Definition: Rtabmap.h:308
rtabmap::Memory::getLandmarksIndex
const std::map< int, std::set< int > > & getLandmarksIndex() const
Definition: Memory.h:168
rtabmap::BayesFilter::computePosterior
const std::map< int, float > & computePosterior(const Memory *memory, const std::map< int, float > &likelihood)
Definition: BayesFilter.cpp:145
rtabmap::Rtabmap::_databasePath
std::string _databasePath
Definition: Rtabmap.h:314
rtabmap::Memory::getNodeWordsAndGlobalDescriptors
void getNodeWordsAndGlobalDescriptors(int nodeId, std::multimap< int, int > &words, std::vector< cv::KeyPoint > &wordsKpts, std::vector< cv::Point3f > &words3, cv::Mat &wordsDescriptors, std::vector< GlobalDescriptor > &globalDescriptors) const
Definition: Memory.cpp:4109
rtabmap::Rtabmap::_createGlobalScanMap
bool _createGlobalScanMap
Definition: Rtabmap.h:328
rtabmap::Memory::addLink
bool addLink(const Link &link, bool addInDatabase=false)
Definition: Memory.cpp:3365
rtabmap::Transform::isIdentity
bool isIdentity() const
Definition: Transform.cpp:136
rtabmap::Transform::is3DoF
bool is3DoF() const
Definition: Transform.cpp:224
rtabmap::Rtabmap::_pathTransformToGoal
Transform _pathTransformToGoal
Definition: Rtabmap.h:381
rtabmap::Transform::inverse
Transform inverse() const
Definition: Transform.cpp:178
rtabmap::VWDictionary::getIndexedWordsCount
unsigned int getIndexedWordsCount() const
Definition: VWDictionary.cpp:359
rtabmap::Statistics::setOdomCachePoses
void setOdomCachePoses(const std::map< int, Transform > &poses)
Definition: Statistics.h:262
rtabmap::Statistics::addStatistic
void addStatistic(const std::string &name, float value)
Definition: Statistics.cpp:93
uInsert
void uInsert(std::map< K, V > &map, const std::pair< K, V > &pair)
Definition: UStl.h:443
rtabmap::Rtabmap::_maxRetrieved
unsigned int _maxRetrieved
Definition: Rtabmap.h:286
rtabmap::Signature::getWords
const std::multimap< int, int > & getWords() const
Definition: Signature.h:111
util3d_surface.h
glm::inverse
GLM_FUNC_DECL matType< T, P > inverse(matType< T, P > const &m)
UINFO
#define UINFO(...)

rtabmap
Author(s): Mathieu Labbe
autogenerated on Mon Jan 23 2023 03:37:30