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