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/DBDriver.h"
38 #include "rtabmap/core/Memory.h"
43 
45 #include <rtabmap/utilite/UFile.h>
46 #include <rtabmap/utilite/UTimer.h>
48 #include <rtabmap/utilite/UMath.h>
50 
51 #include <pcl/search/kdtree.h>
52 #include <pcl/filters/crop_box.h>
53 #include <pcl/io/pcd_io.h>
54 #include <pcl/common/common.h>
55 #include <pcl/TextureMesh.h>
56 
57 #include <stdlib.h>
58 #include <set>
59 
60 #define LOG_F "LogF.txt"
61 #define LOG_I "LogI.txt"
62 
63 #define GRAPH_FILE_NAME "Graph.dot"
64 
65 
66 //
67 //
68 //
69 // =======================================================
70 // MAIN LOOP, see method "void Rtabmap::process();" below.
71 // =======================================================
72 //
73 //
74 //
75 
76 namespace rtabmap
77 {
78 
80  _publishStats(Parameters::defaultRtabmapPublishStats()),
81  _publishLastSignatureData(Parameters::defaultRtabmapPublishLastSignature()),
82  _publishPdf(Parameters::defaultRtabmapPublishPdf()),
83  _publishLikelihood(Parameters::defaultRtabmapPublishLikelihood()),
84  _publishRAMUsage(Parameters::defaultRtabmapPublishRAMUsage()),
85  _computeRMSE(Parameters::defaultRtabmapComputeRMSE()),
86  _saveWMState(Parameters::defaultRtabmapSaveWMState()),
87  _maxTimeAllowed(Parameters::defaultRtabmapTimeThr()), // 700 ms
88  _maxMemoryAllowed(Parameters::defaultRtabmapMemoryThr()), // 0=inf
89  _loopThr(Parameters::defaultRtabmapLoopThr()),
90  _loopRatio(Parameters::defaultRtabmapLoopRatio()),
91  _maxLoopClosureDistance(Parameters::defaultRGBDMaxLoopClosureDistance()),
92  _verifyLoopClosureHypothesis(Parameters::defaultVhEpEnabled()),
93  _maxRetrieved(Parameters::defaultRtabmapMaxRetrieved()),
94  _maxLocalRetrieved(Parameters::defaultRGBDMaxLocalRetrieved()),
95  _rawDataKept(Parameters::defaultMemImageKept()),
96  _statisticLogsBufferedInRAM(Parameters::defaultRtabmapStatisticLogsBufferedInRAM()),
97  _statisticLogged(Parameters::defaultRtabmapStatisticLogged()),
98  _statisticLoggedHeaders(Parameters::defaultRtabmapStatisticLoggedHeaders()),
99  _rgbdSlamMode(Parameters::defaultRGBDEnabled()),
100  _rgbdLinearUpdate(Parameters::defaultRGBDLinearUpdate()),
101  _rgbdAngularUpdate(Parameters::defaultRGBDAngularUpdate()),
102  _rgbdLinearSpeedUpdate(Parameters::defaultRGBDLinearSpeedUpdate()),
103  _rgbdAngularSpeedUpdate(Parameters::defaultRGBDAngularSpeedUpdate()),
104  _newMapOdomChangeDistance(Parameters::defaultRGBDNewMapOdomChangeDistance()),
105  _neighborLinkRefining(Parameters::defaultRGBDNeighborLinkRefining()),
106  _proximityByTime(Parameters::defaultRGBDProximityByTime()),
107  _proximityBySpace(Parameters::defaultRGBDProximityBySpace()),
108  _scanMatchingIdsSavedInLinks(Parameters::defaultRGBDScanMatchingIdsSavedInLinks()),
109  _localRadius(Parameters::defaultRGBDLocalRadius()),
110  _localImmunizationRatio(Parameters::defaultRGBDLocalImmunizationRatio()),
111  _proximityMaxGraphDepth(Parameters::defaultRGBDProximityMaxGraphDepth()),
112  _proximityMaxPaths(Parameters::defaultRGBDProximityMaxPaths()),
113  _proximityMaxNeighbors(Parameters::defaultRGBDProximityPathMaxNeighbors()),
114  _proximityFilteringRadius(Parameters::defaultRGBDProximityPathFilteringRadius()),
115  _proximityRawPosesUsed(Parameters::defaultRGBDProximityPathRawPosesUsed()),
116  _proximityAngle(Parameters::defaultRGBDProximityAngle()*M_PI/180.0f),
117  _proximityOdomGuess(Parameters::defaultRGBDProximityOdomGuess()),
118  _databasePath(""),
119  _optimizeFromGraphEnd(Parameters::defaultRGBDOptimizeFromGraphEnd()),
120  _optimizationMaxError(Parameters::defaultRGBDOptimizeMaxError()),
121  _startNewMapOnLoopClosure(Parameters::defaultRtabmapStartNewMapOnLoopClosure()),
122  _startNewMapOnGoodSignature(Parameters::defaultRtabmapStartNewMapOnGoodSignature()),
123  _goalReachedRadius(Parameters::defaultRGBDGoalReachedRadius()),
124  _goalsSavedInUserData(Parameters::defaultRGBDGoalsSavedInUserData()),
125  _pathStuckIterations(Parameters::defaultRGBDPlanStuckIterations()),
126  _pathLinearVelocity(Parameters::defaultRGBDPlanLinearVelocity()),
127  _pathAngularVelocity(Parameters::defaultRGBDPlanAngularVelocity()),
128  _savedLocalizationIgnored(Parameters::defaultRGBDSavedLocalizationIgnored()),
129  _loopCovLimited(Parameters::defaultRGBDLoopCovLimited()),
130  _loopGPS(Parameters::defaultRtabmapLoopGPS()),
131  _maxOdomCacheSize(Parameters::defaultRGBDMaxOdomCacheSize()),
132  _loopClosureHypothesis(0,0.0f),
133  _highestHypothesis(0,0.0f),
134  _lastProcessTime(0.0),
135  _someNodesHaveBeenTransferred(false),
136  _distanceTravelled(0.0f),
137  _distanceTravelledSinceLastLocalization(0.0f),
138  _optimizeFromGraphEndChanged(false),
139  _epipolarGeometry(0),
140  _bayesFilter(0),
141  _graphOptimizer(0),
142  _memory(0),
143  _foutFloat(0),
144  _foutInt(0),
145  _wDir(""),
146  _mapCorrection(Transform::getIdentity()),
147  _lastLocalizationNodeId(0),
148  _currentSessionHasGPS(false),
149  _odomCorrectionAcc(6,0),
150  _pathStatus(0),
151  _pathCurrentIndex(0),
152  _pathGoalIndex(0),
153  _pathTransformToGoal(Transform::getIdentity()),
154  _pathStuckCount(0),
155  _pathStuckDistance(0.0f)
156 {
157 }
158 
160  UDEBUG("");
161  this->close();
162 }
163 
164 void Rtabmap::setupLogFiles(bool overwrite)
165 {
167  // Log files
168  if(_foutFloat)
169  {
170  fclose(_foutFloat);
171  _foutFloat = 0;
172  }
173  if(_foutInt)
174  {
175  fclose(_foutInt);
176  _foutInt = 0;
177  }
178 
179  if(_statisticLogged && !_wDir.empty())
180  {
181  std::string attributes = "a+"; // append to log files
182  if(overwrite)
183  {
184  // If a file with the same name already exists
185  // its content is erased and the file is treated
186  // as a new empty file.
187  attributes = "w";
188  }
189 
190  bool addLogFHeader = overwrite || !UFile::exists(_wDir+"/"+LOG_F);
191  bool addLogIHeader = overwrite || !UFile::exists(_wDir+"/"+LOG_I);
192 
193  #ifdef _MSC_VER
194  fopen_s(&_foutFloat, (_wDir+"/"+LOG_F).c_str(), attributes.c_str());
195  fopen_s(&_foutInt, (_wDir+"/"+LOG_I).c_str(), attributes.c_str());
196  #else
197  _foutFloat = fopen((_wDir+"/"+LOG_F).c_str(), attributes.c_str());
198  _foutInt = fopen((_wDir+"/"+LOG_I).c_str(), attributes.c_str());
199  #endif
200  // add header (column identification)
201  if(_statisticLoggedHeaders && addLogFHeader && _foutFloat)
202  {
203  fprintf(_foutFloat, "Column headers:\n");
204  fprintf(_foutFloat, " 1-Total iteration time (s)\n");
205  fprintf(_foutFloat, " 2-Memory update time (s)\n");
206  fprintf(_foutFloat, " 3-Retrieval time (s)\n");
207  fprintf(_foutFloat, " 4-Likelihood time (s)\n");
208  fprintf(_foutFloat, " 5-Posterior time (s)\n");
209  fprintf(_foutFloat, " 6-Hypothesis selection time (s)\n");
210  fprintf(_foutFloat, " 7-Hypothesis validation time (s)\n");
211  fprintf(_foutFloat, " 8-Transfer time (s)\n");
212  fprintf(_foutFloat, " 9-Statistics creation time (s)\n");
213  fprintf(_foutFloat, " 10-Loop closure hypothesis value\n");
214  fprintf(_foutFloat, " 11-NAN\n");
215  fprintf(_foutFloat, " 12-NAN\n");
216  fprintf(_foutFloat, " 13-NAN\n");
217  fprintf(_foutFloat, " 14-NAN\n");
218  fprintf(_foutFloat, " 15-NAN\n");
219  fprintf(_foutFloat, " 16-Virtual place hypothesis\n");
220  fprintf(_foutFloat, " 17-Join trash time (s)\n");
221  fprintf(_foutFloat, " 18-Weight Update (rehearsal) similarity\n");
222  fprintf(_foutFloat, " 19-Empty trash time (s)\n");
223  fprintf(_foutFloat, " 20-Retrieval database access time (s)\n");
224  fprintf(_foutFloat, " 21-Add loop closure link time (s)\n");
225  fprintf(_foutFloat, " 22-Memory cleanup time (s)\n");
226  fprintf(_foutFloat, " 23-Scan matching (odometry correction) time (s)\n");
227  fprintf(_foutFloat, " 24-Local time loop closure detection time (s)\n");
228  fprintf(_foutFloat, " 25-Local space loop closure detection time (s)\n");
229  fprintf(_foutFloat, " 26-Map optimization (s)\n");
230  }
231  if(_statisticLoggedHeaders && addLogIHeader && _foutInt)
232  {
233  fprintf(_foutInt, "Column headers:\n");
234  fprintf(_foutInt, " 1-Loop closure ID\n");
235  fprintf(_foutInt, " 2-Highest loop closure hypothesis\n");
236  fprintf(_foutInt, " 3-Locations transferred\n");
237  fprintf(_foutInt, " 4-NAN\n");
238  fprintf(_foutInt, " 5-Words extracted from the last image\n");
239  fprintf(_foutInt, " 6-Vocabulary size\n");
240  fprintf(_foutInt, " 7-Working memory size\n");
241  fprintf(_foutInt, " 8-Is loop closure hypothesis rejected?\n");
242  fprintf(_foutInt, " 9-NAN\n");
243  fprintf(_foutInt, " 10-NAN\n");
244  fprintf(_foutInt, " 11-Locations retrieved\n");
245  fprintf(_foutInt, " 12-Retrieval location ID\n");
246  fprintf(_foutInt, " 13-Unique words extraced from last image\n");
247  fprintf(_foutInt, " 14-Retrieval ID\n");
248  fprintf(_foutInt, " 15-Non-null likelihood values\n");
249  fprintf(_foutInt, " 16-Weight Update ID\n");
250  fprintf(_foutInt, " 17-Is last location merged through Weight Update?\n");
251  fprintf(_foutInt, " 18-Local graph size\n");
252  fprintf(_foutInt, " 19-Sensor data id\n");
253  fprintf(_foutInt, " 20-Indexed words\n");
254  fprintf(_foutInt, " 21-Index memory usage (KB)\n");
255  }
256 
257  ULOGGER_DEBUG("Log file (int)=%s", (_wDir+"/"+LOG_I).c_str());
258  ULOGGER_DEBUG("Log file (float)=%s", (_wDir+"/"+LOG_F).c_str());
259  }
260  else
261  {
262  if(_statisticLogged)
263  {
264  UWARN("Working directory is not set, log disabled!");
265  }
266  UDEBUG("Log disabled!");
267  }
268 }
269 
271 {
272  if(_foutFloat && _bufferedLogsF.size())
273  {
274  UDEBUG("_bufferedLogsF.size=%d", _bufferedLogsF.size());
275  for(std::list<std::string>::iterator iter = _bufferedLogsF.begin(); iter!=_bufferedLogsF.end(); ++iter)
276  {
277  fprintf(_foutFloat, "%s", iter->c_str());
278  }
279  _bufferedLogsF.clear();
280  }
281  if(_foutInt && _bufferedLogsI.size())
282  {
283  UDEBUG("_bufferedLogsI.size=%d", _bufferedLogsI.size());
284  for(std::list<std::string>::iterator iter = _bufferedLogsI.begin(); iter!=_bufferedLogsI.end(); ++iter)
285  {
286  fprintf(_foutInt, "%s", iter->c_str());
287  }
288  _bufferedLogsI.clear();
289  }
290 }
291 
292 void Rtabmap::init(const ParametersMap & parameters, const std::string & databasePath, bool loadDatabaseParameters)
293 {
294  UDEBUG("path=%s", databasePath.c_str());
295  _databasePath = databasePath;
296  if(!_databasePath.empty())
297  {
298  UASSERT(UFile::getExtension(_databasePath).compare("db") == 0);
299  UINFO("Using database \"%s\".", _databasePath.c_str());
300  }
301  else
302  {
303  UWARN("Using empty database. Mapping session will not be saved unless it is closed with an output database path.");
304  }
305 
306  bool newDatabase = _databasePath.empty() || !UFile::exists(_databasePath);
307 
308  ParametersMap allParameters;
309  if(!newDatabase && loadDatabaseParameters)
310  {
311  DBDriver * driver = DBDriver::create();
312  if(driver->openConnection(_databasePath, false))
313  {
314  allParameters = driver->getLastParameters();
315  // ignore working directory (we may be on a different computer)
316  allParameters.erase(Parameters::kRtabmapWorkingDirectory());
317  }
318  delete driver;
319  }
320 
321  uInsert(allParameters, parameters);
322  ParametersMap::const_iterator iter;
323  if((iter=allParameters.find(Parameters::kRtabmapWorkingDirectory())) != allParameters.end())
324  {
325  this->setWorkingDirectory(iter->second.c_str());
326  }
327 
328  // If doesn't exist, create a memory
329  if(!_memory)
330  {
331  _memory = new Memory(allParameters);
332  _memory->init(_databasePath, false, allParameters, true);
333  }
334 
335  // Parse all parameters
336  this->parseParameters(allParameters);
337 
338  Transform lastPose;
340  if(!_optimizedPoses.empty())
341  {
343  {
344  UDEBUG("lastPose is ignored (%s=true), assuming we start at the origin of the map.", Parameters::kRGBDSavedLocalizationIgnored().c_str());
345  lastPose.setIdentity();
346  }
347  _lastLocalizationPose = lastPose;
348 
349  UINFO("Loaded optimizedPoses=%d lastPose=%s", _optimizedPoses.size(), _lastLocalizationPose.prettyPrint().c_str());
350 
351  std::map<int, Transform> tmp;
352  // Get just the links
354 
355  // Initialize Bayes' prediction matrix
356  UTimer time;
357  std::map<int, float> likelihood;
358  likelihood.insert(std::make_pair(Memory::kIdVirtual, 1));
359  for(std::map<int, Transform>::iterator iter=_optimizedPoses.begin(); iter!=_optimizedPoses.end(); ++iter)
360  {
361  likelihood.insert(std::make_pair(iter->first, 0));
362  }
363  _bayesFilter->computePosterior(_memory, likelihood);
364  UINFO("Time initializing Bayes' prediction with %ld nodes: %fs", _optimizedPoses.size(), time.ticks());
365  }
366  else
367  {
368  UINFO("Loaded optimizedPoses=0, last localization pose is ignored!");
369  }
370 
371  if(_databasePath.empty())
372  {
373  _statisticLogged = false;
374  }
375  setupLogFiles(newDatabase);
376 }
377 
378 void Rtabmap::init(const std::string & configFile, const std::string & databasePath, bool loadDatabaseParameters)
379 {
380  // fill ctrl struct with values from the configuration file
381  ParametersMap param;// = Parameters::defaultParameters;
382 
383  if(!configFile.empty())
384  {
385  ULOGGER_DEBUG("Read parameters from = %s", configFile.c_str());
386  Parameters::readINI(configFile, param);
387  }
388 
389  this->init(param, databasePath, loadDatabaseParameters);
390 }
391 
392 void Rtabmap::close(bool databaseSaved, const std::string & ouputDatabasePath)
393 {
394  UINFO("databaseSaved=%d", databaseSaved?1:0);
395  _highestHypothesis = std::make_pair(0,0.0f);
396  _loopClosureHypothesis = std::make_pair(0,0.0f);
397  _lastProcessTime = 0.0;
399  _constraints.clear();
402 
404  _odomCachePoses.clear();
405  _odomCacheConstraints.clear();
406  _odomCorrectionAcc = std::vector<float>(6,0);
407  _distanceTravelled = 0.0f;
410  this->clearPath(0);
411  _gpsGeocentricCache.clear();
412  _currentSessionHasGPS = false;
413 
415  if(_foutFloat)
416  {
417  fclose(_foutFloat);
418  _foutFloat = 0;
419  }
420  if(_foutInt)
421  {
422  fclose(_foutInt);
423  _foutInt = 0;
424  }
425 
427  {
428  delete _epipolarGeometry;
429  _epipolarGeometry = 0;
430  }
431  if(_memory)
432  {
433  if(databaseSaved)
434  {
436  {
437  // Force reducing graph, then remove filtered nodes from the optimized poses
438  std::map<int, int> reducedIds;
439  _memory->incrementMapId(&reducedIds);
440  for(std::map<int, int>::iterator iter=reducedIds.begin(); iter!=reducedIds.end(); ++iter)
441  {
442  _optimizedPoses.erase(iter->first);
443  }
444  }
446  }
447  _memory->close(databaseSaved, true, ouputDatabasePath);
448  delete _memory;
449  _memory = 0;
450  }
451  _optimizedPoses.clear();
453 
454  if(_bayesFilter)
455  {
456  delete _bayesFilter;
457  _bayesFilter = 0;
458  }
459  if(_graphOptimizer)
460  {
461  delete _graphOptimizer;
462  _graphOptimizer = 0;
463  }
464  _databasePath.clear();
465  parseParameters(Parameters::getDefaultParameters()); // reset to default parameters
466  _parameters.clear();
467 }
468 
469 void Rtabmap::parseParameters(const ParametersMap & parameters)
470 {
471  uInsert(_parameters, parameters);
472 
473  // place this before changing working directory
474  Parameters::parse(parameters, Parameters::kRtabmapStatisticLogsBufferedInRAM(), _statisticLogsBufferedInRAM);
475  Parameters::parse(parameters, Parameters::kRtabmapStatisticLogged(), _statisticLogged);
476  Parameters::parse(parameters, Parameters::kRtabmapStatisticLoggedHeaders(), _statisticLoggedHeaders);
477 
478  ULOGGER_DEBUG("");
479  ParametersMap::const_iterator iter;
480  if((iter=parameters.find(Parameters::kRtabmapWorkingDirectory())) != parameters.end())
481  {
482  this->setWorkingDirectory(iter->second.c_str());
483  }
484 
485  Parameters::parse(parameters, Parameters::kRtabmapPublishStats(), _publishStats);
486  Parameters::parse(parameters, Parameters::kRtabmapPublishLastSignature(), _publishLastSignatureData);
487  Parameters::parse(parameters, Parameters::kRtabmapPublishPdf(), _publishPdf);
488  Parameters::parse(parameters, Parameters::kRtabmapPublishLikelihood(), _publishLikelihood);
489  Parameters::parse(parameters, Parameters::kRtabmapPublishRAMUsage(), _publishRAMUsage);
490  Parameters::parse(parameters, Parameters::kRtabmapComputeRMSE(), _computeRMSE);
491  Parameters::parse(parameters, Parameters::kRtabmapSaveWMState(), _saveWMState);
492  Parameters::parse(parameters, Parameters::kRtabmapTimeThr(), _maxTimeAllowed);
493  Parameters::parse(parameters, Parameters::kRtabmapMemoryThr(), _maxMemoryAllowed);
494  Parameters::parse(parameters, Parameters::kRtabmapLoopThr(), _loopThr);
495  Parameters::parse(parameters, Parameters::kRtabmapLoopRatio(), _loopRatio);
496  Parameters::parse(parameters, Parameters::kRGBDMaxLoopClosureDistance(), _maxLoopClosureDistance);
497  Parameters::parse(parameters, Parameters::kVhEpEnabled(), _verifyLoopClosureHypothesis);
498  Parameters::parse(parameters, Parameters::kRtabmapMaxRetrieved(), _maxRetrieved);
499  Parameters::parse(parameters, Parameters::kRGBDMaxLocalRetrieved(), _maxLocalRetrieved);
500  Parameters::parse(parameters, Parameters::kMemImageKept(), _rawDataKept);
501  Parameters::parse(parameters, Parameters::kRGBDEnabled(), _rgbdSlamMode);
502  Parameters::parse(parameters, Parameters::kRGBDLinearUpdate(), _rgbdLinearUpdate);
503  Parameters::parse(parameters, Parameters::kRGBDAngularUpdate(), _rgbdAngularUpdate);
504  Parameters::parse(parameters, Parameters::kRGBDLinearSpeedUpdate(), _rgbdLinearSpeedUpdate);
505  Parameters::parse(parameters, Parameters::kRGBDAngularSpeedUpdate(), _rgbdAngularSpeedUpdate);
506  Parameters::parse(parameters, Parameters::kRGBDNewMapOdomChangeDistance(), _newMapOdomChangeDistance);
507  Parameters::parse(parameters, Parameters::kRGBDNeighborLinkRefining(), _neighborLinkRefining);
508  Parameters::parse(parameters, Parameters::kRGBDProximityByTime(), _proximityByTime);
509  Parameters::parse(parameters, Parameters::kRGBDProximityBySpace(), _proximityBySpace);
510  Parameters::parse(parameters, Parameters::kRGBDScanMatchingIdsSavedInLinks(), _scanMatchingIdsSavedInLinks);
511  Parameters::parse(parameters, Parameters::kRGBDLocalRadius(), _localRadius);
512  Parameters::parse(parameters, Parameters::kRGBDLocalImmunizationRatio(), _localImmunizationRatio);
513  Parameters::parse(parameters, Parameters::kRGBDProximityMaxGraphDepth(), _proximityMaxGraphDepth);
514  Parameters::parse(parameters, Parameters::kRGBDProximityMaxPaths(), _proximityMaxPaths);
515  Parameters::parse(parameters, Parameters::kRGBDProximityPathMaxNeighbors(), _proximityMaxNeighbors);
516  Parameters::parse(parameters, Parameters::kRGBDProximityPathFilteringRadius(), _proximityFilteringRadius);
517  Parameters::parse(parameters, Parameters::kRGBDProximityPathRawPosesUsed(), _proximityRawPosesUsed);
518  if(Parameters::parse(parameters, Parameters::kRGBDProximityAngle(), _proximityAngle))
519  {
520  _proximityAngle *= M_PI/180.0f;
521  }
522  Parameters::parse(parameters, Parameters::kRGBDProximityOdomGuess(), _proximityOdomGuess);
523  bool optimizeFromGraphEndPrevious = _optimizeFromGraphEnd;
524  Parameters::parse(parameters, Parameters::kRGBDOptimizeFromGraphEnd(), _optimizeFromGraphEnd);
525  if(optimizeFromGraphEndPrevious != _optimizeFromGraphEnd)
526  {
528  }
529  Parameters::parse(parameters, Parameters::kRGBDOptimizeMaxError(), _optimizationMaxError);
531  {
532  UWARN("RGBD/OptimizeMaxError (value=%f) is smaller than 1.0, setting to default %f "
533  "instead (for backward compatibility issues when this parameter was previously "
534  "an absolute error value).", _optimizationMaxError, Parameters::defaultRGBDOptimizeMaxError());
535  _optimizationMaxError = Parameters::defaultRGBDOptimizeMaxError();
536  }
537  Parameters::parse(parameters, Parameters::kRtabmapStartNewMapOnLoopClosure(), _startNewMapOnLoopClosure);
538  Parameters::parse(parameters, Parameters::kRtabmapStartNewMapOnGoodSignature(), _startNewMapOnGoodSignature);
539  Parameters::parse(parameters, Parameters::kRGBDGoalReachedRadius(), _goalReachedRadius);
540  Parameters::parse(parameters, Parameters::kRGBDGoalsSavedInUserData(), _goalsSavedInUserData);
541  Parameters::parse(parameters, Parameters::kRGBDPlanStuckIterations(), _pathStuckIterations);
542  Parameters::parse(parameters, Parameters::kRGBDPlanLinearVelocity(), _pathLinearVelocity);
543  Parameters::parse(parameters, Parameters::kRGBDPlanAngularVelocity(), _pathAngularVelocity);
544  Parameters::parse(parameters, Parameters::kRGBDSavedLocalizationIgnored(), _savedLocalizationIgnored);
545  Parameters::parse(parameters, Parameters::kRGBDLoopCovLimited(), _loopCovLimited);
546  Parameters::parse(parameters, Parameters::kRtabmapLoopGPS(), _loopGPS);
547  Parameters::parse(parameters, Parameters::kRGBDMaxOdomCacheSize(), _maxOdomCacheSize);
548 
549  UASSERT(_rgbdLinearUpdate >= 0.0f);
553 
554  // By default, we create our strategies if they are not already created.
555  // If they already exists, we check the parameters if a change is requested
556 
557  // Graph optimizer
558  Optimizer::Type optimizerType = Optimizer::kTypeUndef;
559  if((iter=parameters.find(Parameters::kOptimizerStrategy())) != parameters.end())
560  {
561  optimizerType = (Optimizer::Type)std::atoi((*iter).second.c_str());
562  }
563  if(optimizerType!=Optimizer::kTypeUndef)
564  {
565  UDEBUG("new detector strategy %d", int(optimizerType));
566  if(_graphOptimizer)
567  {
568  delete _graphOptimizer;
569  _graphOptimizer = 0;
570  }
571 
573  }
574  else if(_graphOptimizer)
575  {
576  _graphOptimizer->parseParameters(parameters);
577  }
578  else
579  {
580  optimizerType = (Optimizer::Type)Parameters::defaultOptimizerStrategy();
581  _graphOptimizer = Optimizer::create(optimizerType, parameters);
582  }
583 
584  if(_memory)
585  {
586  _memory->parseParameters(parameters);
587  }
588 
589  if(!_epipolarGeometry)
590  {
592  }
593  else
594  {
595  _epipolarGeometry->parseParameters(parameters);
596  }
597 
598  // Bayes filter, create one if not exists
599  if(!_bayesFilter)
600  {
602  }
603  else
604  {
605  _bayesFilter->parseParameters(parameters);
606  }
607 }
608 
610 {
611  int id = 0;
612  if(_memory)
613  {
614  id = _memory->getLastSignatureId();
615  }
616  return id;
617 }
618 
619 std::list<int> Rtabmap::getWM() const
620 {
621  std::list<int> mem;
622  if(_memory)
623  {
624  mem = uKeysList(_memory->getWorkingMem());
625  mem.remove(-1);// Ignore the virtual signature (if here)
626  }
627  return mem;
628 }
629 
631 {
632  if(_memory)
633  {
634  return (int)_memory->getWorkingMem().size()-1; // remove virtual place
635  }
636  return 0;
637 }
638 
639 std::map<int, int> Rtabmap::getWeights() const
640 {
641  std::map<int, int> weights;
642  if(_memory)
643  {
644  weights = _memory->getWeights();
645  weights.erase(-1);// Ignore the virtual signature (if here)
646  }
647  return weights;
648 }
649 
650 std::set<int> Rtabmap::getSTM() const
651 {
652  if(_memory)
653  {
654  return _memory->getStMem();
655  }
656  return std::set<int>();
657 }
658 
660 {
661  if(_memory)
662  {
663  return (int)_memory->getStMem().size();
664  }
665  return 0;
666 }
667 
669 {
670  if(_memory)
671  {
673  if(s)
674  {
675  return s->id();
676  }
677  }
678  return 0;
679 }
680 
681 bool Rtabmap::isInSTM(int locationId) const
682 {
683  if(_memory)
684  {
685  return _memory->isInSTM(locationId);
686  }
687  return false;
688 }
689 
691 {
692  if(_memory)
693  {
694  return _memory->isIDsGenerated();
695  }
696  return Parameters::defaultMemGenerateIds();
697 }
698 
700 {
701  return statistics_;
702 }
703 
704 Transform Rtabmap::getPose(int locationId) const
705 {
706  return uValue(_optimizedPoses, locationId, Transform());
707 }
708 
709 void Rtabmap::setInitialPose(const Transform & initialPose)
710 {
711  if(_memory)
712  {
713  if(!_memory->isIncremental())
714  {
715  _lastLocalizationPose = initialPose;
717  _odomCachePoses.clear();
718  _odomCacheConstraints.clear();
719  _odomCorrectionAcc = std::vector<float>(6,0);
722 
724  _optimizedPoses.empty())
725  {
726  cv::Mat covariance;
728  }
729  }
730  else
731  {
732  UWARN("Initial pose can only be set in localization mode (%s=false), ignoring it...", Parameters::kMemIncrementalMemory().c_str());
733  }
734  }
735 }
736 
738 {
739  int mapId = -1;
740  if(_memory)
741  {
743  _odomCachePoses.clear();
744  _odomCacheConstraints.clear();
745  _odomCorrectionAcc = std::vector<float>(6,0);
746  _distanceTravelled = 0.0f;
748 
749  if(!_memory->isIncremental())
750  {
752  {
755  }
756  return mapId;
757  }
758  std::map<int, int> reducedIds;
759  mapId = _memory->incrementMapId(&reducedIds);
760  UINFO("New map triggered, new map = %d", mapId);
761  _optimizedPoses.clear();
762  _constraints.clear();
763 
764  if(_bayesFilter)
765  {
766  _bayesFilter->reset();
767  }
768 
769  //Verify if there are nodes that were merged through graph reduction
770  if(reducedIds.size() && _path.size())
771  {
772  for(unsigned int i=0; i<_path.size(); ++i)
773  {
774  std::map<int, int>::const_iterator iter = reducedIds.find(_path[i].first);
775  if(iter!= reducedIds.end())
776  {
777  // change path ID to loop closure ID
778  _path[i].first = iter->second;
779  }
780  }
781  }
782  }
783  return mapId;
784 }
785 
786 bool Rtabmap::labelLocation(int id, const std::string & label)
787 {
788  if(_memory)
789  {
790  if(id > 0)
791  {
792  return _memory->labelSignature(id, label);
793  }
794  else if(_memory->getLastWorkingSignature())
795  {
797  }
798  else
799  {
800  UERROR("Last signature is null! Cannot set label \"%s\"", label.c_str());
801  }
802  }
803  return false;
804 }
805 
806 bool Rtabmap::setUserData(int id, const cv::Mat & data)
807 {
808  if(_memory)
809  {
810  if(id > 0)
811  {
812  return _memory->setUserData(id, data);
813  }
814  else if(_memory->getLastWorkingSignature())
815  {
817  }
818  else
819  {
820  UERROR("Last signature is null! Cannot set user data!");
821  }
822  }
823  return false;
824 }
825 
826 void Rtabmap::generateDOTGraph(const std::string & path, int id, int margin)
827 {
828  if(_memory)
829  {
830  _memory->joinTrashThread(); // make sure the trash is flushed
831 
832  if(id > 0)
833  {
834  std::map<int, int> ids = _memory->getNeighborsId(id, margin, -1, false);
835 
836  if(ids.size() > 0)
837  {
838  ids.insert(std::pair<int,int>(id, 0));
839  std::set<int> idsSet;
840  for(std::map<int, int>::iterator iter = ids.begin(); iter!=ids.end(); ++iter)
841  {
842  idsSet.insert(idsSet.end(), iter->first);
843  }
844  _memory->generateGraph(path, idsSet);
845  }
846  else
847  {
848  UERROR("No neighbors found for signature %d.", id);
849  }
850  }
851  else
852  {
853  _memory->generateGraph(path);
854  }
855  }
856 }
857 
858 void Rtabmap::exportPoses(const std::string & path, bool optimized, bool global, int format)
859 {
861  {
862  std::map<int, Transform> poses;
863  std::multimap<int, Link> constraints;
864 
865  if(optimized)
866  {
867  cv::Mat covariance;
868  this->optimizeCurrentMap(_memory->getLastWorkingSignature()->id(), global, poses, covariance, &constraints);
869  }
870  else
871  {
872  std::map<int, int> ids = _memory->getNeighborsId(_memory->getLastWorkingSignature()->id(), 0, global?-1:0, true);
873  _memory->getMetricConstraints(uKeysSet(ids), poses, constraints, global);
874  }
875 
876  std::map<int, double> stamps;
877  if(format == 1)
878  {
879  for(std::map<int, Transform>::iterator iter=poses.begin(); iter!=poses.end(); ++iter)
880  {
881  Transform o,g;
882  int m, w;
883  std::string l;
884  double stamp = 0.0;
885  std::vector<float> v;
886  GPS gps;
887  EnvSensors sensors;
888  _memory->getNodeInfo(iter->first, o, m, w, l, stamp, g, v, gps, sensors, true);
889  stamps.insert(std::make_pair(iter->first, stamp));
890  }
891  }
892 
893  graph::exportPoses(path, format, poses, constraints, stamps, _parameters);
894  }
895 }
896 
898 {
899  UDEBUG("");
900  _highestHypothesis = std::make_pair(0,0.0f);
901  _loopClosureHypothesis = std::make_pair(0,0.0f);
902  _lastProcessTime = 0.0;
904  _optimizedPoses.clear();
905  _constraints.clear();
910  _odomCachePoses.clear();
911  _odomCacheConstraints.clear();
912  _odomCorrectionAcc = std::vector<float>(6,0);
913  _distanceTravelled = 0.0f;
916  this->clearPath(0);
917 
918  if(_memory)
919  {
920  _memory->init(_databasePath, true, _parameters, true);
922  {
923  cv::Mat covariance;
925  }
926  if(_bayesFilter)
927  {
928  _bayesFilter->reset();
929  }
930  }
931  else
932  {
933  UERROR("RTAB-Map is not initialized. No memory to reset...");
934  }
935  this->setupLogFiles(true);
936 }
937 
939 {
940 public:
941  NearestPathKey(float l, int i) :
942  likelihood(l),
943  id(i){}
944  bool operator<(const NearestPathKey & k) const
945  {
946  if(likelihood < k.likelihood)
947  {
948  return true;
949  }
950  else if(likelihood == k.likelihood && id < k.id)
951  {
952  return true;
953  }
954  return false;
955  }
956  float likelihood;
957  int id;
958 };
959 
960 //============================================================
961 // MAIN LOOP
962 //============================================================
964  const cv::Mat & image,
965  int id,
966  const std::map<std::string, float> & externalStats)
967 {
968  return this->process(SensorData(image, id), Transform());
969 }
971  const SensorData & data,
972  Transform odomPose,
973  float odomLinearVariance,
974  float odomAngularVariance,
975  const std::vector<float> & odomVelocity,
976  const std::map<std::string, float> & externalStats)
977 {
978  if(!odomPose.isNull())
979  {
980  UASSERT(odomLinearVariance>0.0f);
981  UASSERT(odomAngularVariance>0.0f);
982  }
983  cv::Mat covariance = cv::Mat::eye(6,6,CV_64FC1);
984  covariance.at<double>(0,0) = odomLinearVariance;
985  covariance.at<double>(1,1) = odomLinearVariance;
986  covariance.at<double>(2,2) = odomLinearVariance;
987  covariance.at<double>(3,3) = odomAngularVariance;
988  covariance.at<double>(4,4) = odomAngularVariance;
989  covariance.at<double>(5,5) = odomAngularVariance;
990  return process(data, odomPose, covariance, odomVelocity, externalStats);
991 }
993  const SensorData & data,
994  Transform odomPose,
995  const cv::Mat & odomCovariance,
996  const std::vector<float> & odomVelocity,
997  const std::map<std::string, float> & externalStats)
998 {
999  UDEBUG("");
1000 
1001  //============================================================
1002  // Initialization
1003  //============================================================
1004  UTimer timer;
1005  UTimer timerTotal;
1006  double timeMemoryUpdate = 0;
1007  double timeNeighborLinkRefining = 0;
1008  double timeProximityByTimeDetection = 0;
1009  double timeProximityBySpaceVisualDetection = 0;
1010  double timeProximityBySpaceDetection = 0;
1011  double timeCleaningNeighbors = 0;
1012  double timeReactivations = 0;
1013  double timeAddLoopClosureLink = 0;
1014  double timeMapOptimization = 0;
1015  double timeRetrievalDbAccess = 0;
1016  double timeLikelihoodCalculation = 0;
1017  double timePosteriorCalculation = 0;
1018  double timeHypothesesCreation = 0;
1019  double timeHypothesesValidation = 0;
1020  double timeRealTimeLimitReachedProcess = 0;
1021  double timeMemoryCleanup = 0;
1022  double timeEmptyingTrash = 0;
1023  double timeFinalizingStatistics = 0;
1024  double timeJoiningTrash = 0;
1025  double timeStatsCreation = 0;
1026 
1027  float hypothesisRatio = 0.0f; // Only used for statistics
1028  bool rejectedGlobalLoopClosure = false;
1029 
1030  std::map<int, float> rawLikelihood;
1031  std::map<int, float> adjustedLikelihood;
1032  std::map<int, float> likelihood;
1033  std::map<int, int> weights;
1034  std::map<int, float> posterior;
1035  std::list<std::pair<int, float> > reactivateHypotheses;
1036 
1037  std::map<int, int> childCount;
1038  std::set<int> signaturesRetrieved;
1039  int proximityDetectionsInTimeFound = 0;
1040 
1041  const Signature * signature = 0;
1042  const Signature * sLoop = 0;
1043 
1044  _loopClosureHypothesis = std::make_pair(0,0.0f);
1045  std::pair<int, float> lastHighestHypothesis = _highestHypothesis;
1046  _highestHypothesis = std::make_pair(0,0.0f);
1047 
1048  std::set<int> immunizedLocations;
1049 
1050  statistics_ = Statistics(); // reset
1051  for(std::map<std::string, float>::const_iterator iter=externalStats.begin(); iter!=externalStats.end(); ++iter)
1052  {
1053  statistics_.addStatistic(iter->first, iter->second);
1054  }
1055 
1056  //============================================================
1057  // Wait for an image...
1058  //============================================================
1059  ULOGGER_INFO("getting data...");
1060 
1061  timer.start();
1062  timerTotal.start();
1063 
1064  UASSERT_MSG(_memory, "RTAB-Map is not initialized!");
1065  UASSERT_MSG(_bayesFilter, "RTAB-Map is not initialized!");
1066  UASSERT_MSG(_graphOptimizer, "RTAB-Map is not initialized!");
1067 
1068  //============================================================
1069  // If RGBD SLAM is enabled, a pose must be set.
1070  //============================================================
1071  bool fakeOdom = false;
1072  if(_rgbdSlamMode)
1073  {
1074  if(!odomPose.isNull())
1075  {
1076  // this will make sure that all inverse operations will work!
1077  if(!odomPose.isInvertible())
1078  {
1079  UWARN("Input odometry is not invertible! pose = %s\n"
1080  "[%f %f %f %f;\n"
1081  " %f %f %f %f;\n"
1082  " %f %f %f %f;\n"
1083  " 0 0 0 1]\n"
1084  "Trying to normalize rotation to see if it makes it invertible...",
1085  odomPose.prettyPrint().c_str(),
1086  odomPose.r11(), odomPose.r12(), odomPose.r13(), odomPose.o14(),
1087  odomPose.r21(), odomPose.r22(), odomPose.r23(), odomPose.o24(),
1088  odomPose.r31(), odomPose.r32(), odomPose.r33(), odomPose.o34());
1089  odomPose.normalizeRotation();
1090  UASSERT_MSG(odomPose.isInvertible(), uFormat("Odometry pose is not invertible!\n"
1091  "[%f %f %f %f;\n"
1092  " %f %f %f %f;\n"
1093  " %f %f %f %f;\n"
1094  " 0 0 0 1]", odomPose.prettyPrint().c_str(),
1095  odomPose.r11(), odomPose.r12(), odomPose.r13(), odomPose.o14(),
1096  odomPose.r21(), odomPose.r22(), odomPose.r23(), odomPose.o24(),
1097  odomPose.r31(), odomPose.r32(), odomPose.r33(), odomPose.o34()).c_str());
1098  UWARN("Normalizing rotation succeeded! fixed pose = %s\n"
1099  "[%f %f %f %f;\n"
1100  " %f %f %f %f;\n"
1101  " %f %f %f %f;\n"
1102  " 0 0 0 1]\n"
1103  "If the resulting rotation is very different from original one, try to fix the odometry or TF.",
1104  odomPose.prettyPrint().c_str(),
1105  odomPose.r11(), odomPose.r12(), odomPose.r13(), odomPose.o14(),
1106  odomPose.r21(), odomPose.r22(), odomPose.r23(), odomPose.o24(),
1107  odomPose.r31(), odomPose.r32(), odomPose.r33(), odomPose.o34());
1108  }
1109  }
1110 
1111  if(!_memory->isIncremental() &&
1112  !odomPose.isNull() &&
1113  _optimizedPoses.size() &&
1118  {
1119  // Localization mode
1121  {
1122  //set map->odom so that odom is moved back to last saved localization
1124  std::map<int, Transform> nodesOnly(_optimizedPoses.lower_bound(1), _optimizedPoses.end());
1126  UWARN("Update map correction based on last localization saved in database! correction = %s, nearest id = %d of last pose = %s, odom = %s",
1127  _mapCorrection.prettyPrint().c_str(),
1130  odomPose.prettyPrint().c_str());
1131  }
1132  else
1133  {
1134  //move optimized poses accordingly to last saved localization
1135  Transform mapCorrectionInv = odomPose * _lastLocalizationPose.inverse();
1136  for(std::map<int, Transform>::iterator iter=_optimizedPoses.begin(); iter!=_optimizedPoses.end(); ++iter)
1137  {
1138  iter->second = mapCorrectionInv * iter->second;
1139  }
1140  }
1141  }
1142 
1143  if(odomPose.isNull())
1144  {
1145  if(_memory->isIncremental())
1146  {
1147  UERROR("RGB-D SLAM mode is enabled, memory is incremental but no odometry is provided. "
1148  "Image %d is ignored!", data.id());
1149  return false;
1150  }
1151  else // fake localization
1152  {
1154  {
1157  }
1159  {
1161  }
1162  fakeOdom = true;
1164  UDEBUG("Map correction = %s", _mapCorrection.prettyPrint().c_str());
1165  UDEBUG("Last localization pose: %s", _lastLocalizationPose.prettyPrint().c_str());
1166  UDEBUG("Fake odom: %s", odomPose.prettyPrint().c_str());
1167  }
1168  }
1169  else if(_memory->isIncremental()) // only in mapping mode
1170  {
1171  // Detect if the odometry is reset. If yes, trigger a new map.
1173  {
1174  const Transform & lastPose = _memory->getLastWorkingSignature()->getPose(); // use raw odometry
1175 
1176  // look for identity
1177  if(!lastPose.isIdentity() && odomPose.isIdentity())
1178  {
1179  int mapId = triggerNewMap();
1180  UWARN("Odometry is reset (identity pose detected). Increment map id to %d!", mapId);
1181  }
1182  else if(_newMapOdomChangeDistance > 0.0)
1183  {
1184  // look for large change
1185  Transform lastPoseToNewPose = lastPose.inverse() * odomPose;
1186  float x,y,z, roll,pitch,yaw;
1187  lastPoseToNewPose.getTranslationAndEulerAngles(x,y,z, roll,pitch,yaw);
1189  {
1190  int mapId = triggerNewMap();
1191  UWARN("Odometry is reset (large odometry change detected > %f). A new map (%d) is created! Last pose = %s, new pose = %s",
1192  _newMapOdomChangeDistance,
1193  mapId,
1194  lastPose.prettyPrint().c_str(),
1195  odomPose.prettyPrint().c_str());
1196  }
1197  }
1198  }
1199  }
1200  }
1201 
1202  //============================================================
1203  // Memory Update : Location creation + Add to STM + Weight Update (Rehearsal)
1204  //============================================================
1205  ULOGGER_INFO("Updating memory...");
1206  if(_rgbdSlamMode)
1207  {
1208  if(!_memory->update(data, odomPose, odomCovariance, odomVelocity, &statistics_))
1209  {
1210  return false;
1211  }
1212  }
1213  else
1214  {
1215  if(!_memory->update(data, Transform(), cv::Mat(), std::vector<float>(), &statistics_))
1216  {
1217  return false;
1218  }
1219  }
1220 
1221  signature = _memory->getLastWorkingSignature();
1222  _currentSessionHasGPS = _currentSessionHasGPS || signature->sensorData().gps().stamp() > 0.0;
1223  if(!signature)
1224  {
1225  UFATAL("Not supposed to be here...last signature is null?!?");
1226  }
1227 
1228  ULOGGER_INFO("Processing signature %d w=%d map=%d", signature->id(), signature->getWeight(), signature->mapId());
1229  timeMemoryUpdate = timer.ticks();
1230  ULOGGER_INFO("timeMemoryUpdate=%fs", timeMemoryUpdate);
1231 
1232  //============================================================
1233  // Metric
1234  //============================================================
1235  bool smallDisplacement = false;
1236  bool tooFastMovement = false;
1237  std::list<int> signaturesRemoved;
1238  if(_rgbdSlamMode)
1239  {
1240  statistics_.addStatistic(Statistics::kMemoryOdometry_variance_lin(), odomCovariance.empty()?1.0f:(float)odomCovariance.at<double>(0,0));
1241  statistics_.addStatistic(Statistics::kMemoryOdometry_variance_ang(), odomCovariance.empty()?1.0f:(float)odomCovariance.at<double>(5,5));
1242 
1243  //Verify if there was a rehearsal
1244  int rehearsedId = (int)uValue(statistics_.data(), Statistics::kMemoryRehearsal_merged(), 0.0f);
1245  if(rehearsedId > 0)
1246  {
1247  _optimizedPoses.erase(rehearsedId);
1248  }
1249  else
1250  {
1251  if(_rgbdLinearUpdate > 0.0f || _rgbdAngularUpdate > 0.0f)
1252  {
1253  //============================================================
1254  // Minimum displacement required to add to Memory
1255  //============================================================
1256  const std::multimap<int, Link> & links = signature->getLinks();
1257  if(links.size() && links.begin()->second.type() == Link::kNeighbor)
1258  {
1259  const Signature * s = _memory->getSignature(links.begin()->second.to());
1260  UASSERT(s!=0);
1261  // don't filter if the new node is not intermediate but previous one is
1262  if(signature->getWeight() < 0 || s->getWeight() >= 0)
1263  {
1264  float x,y,z, roll,pitch,yaw;
1265  links.begin()->second.transform().getTranslationAndEulerAngles(x,y,z, roll,pitch,yaw);
1266  bool isMoving = fabs(x) > _rgbdLinearUpdate ||
1267  fabs(y) > _rgbdLinearUpdate ||
1268  fabs(z) > _rgbdLinearUpdate ||
1269  (_rgbdAngularUpdate>0.0f && (
1270  fabs(roll) > _rgbdAngularUpdate ||
1271  fabs(pitch) > _rgbdAngularUpdate ||
1272  fabs(yaw) > _rgbdAngularUpdate));
1273  if(!isMoving)
1274  {
1275  // This will disable global loop closure detection, only retrieval will be done.
1276  // The location will also be deleted at the end.
1277  smallDisplacement = true;
1278  UDEBUG("smallDisplacement: %f %f %f %f %f %f", x,y,z, roll,pitch,yaw);
1279  }
1280  }
1281  }
1282  }
1283  if(odomVelocity.size() == 6)
1284  {
1285  // This will disable global loop closure detection, only retrieval will be done.
1286  // The location will also be deleted at the end.
1287  tooFastMovement =
1288  (_rgbdLinearSpeedUpdate>0.0f && uMax3(fabs(odomVelocity[0]), fabs(odomVelocity[1]), fabs(odomVelocity[2])) > _rgbdLinearSpeedUpdate) ||
1289  (_rgbdAngularSpeedUpdate>0.0f && uMax3(fabs(odomVelocity[3]), fabs(odomVelocity[4]), fabs(odomVelocity[5])) > _rgbdAngularSpeedUpdate);
1290  }
1291  }
1292 
1293  // Update optimizedPoses with the newly added node
1294  Transform newPose;
1295  if(_neighborLinkRefining &&
1296  signature->getLinks().size() &&
1297  signature->getLinks().begin()->second.type() == Link::kNeighbor &&
1298  _memory->isIncremental() && // ignore pose matching in localization mode
1299  rehearsedId == 0) // don't do it if rehearsal happened
1300  {
1301  int oldId = signature->getLinks().begin()->first;
1302  const Signature * oldS = _memory->getSignature(oldId);
1303  UASSERT(oldS != 0);
1304 
1305  if(signature->getWeight() >= 0 && oldS->getWeight()>=0) // ignore intermediate nodes
1306  {
1307  Transform guess = signature->getLinks().begin()->second.transform().inverse();
1308 
1309  if(smallDisplacement)
1310  {
1311  if(signature->getLinks().begin()->second.transVariance() == 1)
1312  {
1313  // set small variance
1314  UDEBUG("Set small variance. The robot is not moving.");
1315  _memory->updateLink(Link(oldId, signature->id(), signature->getLinks().begin()->second.type(), guess, cv::Mat::eye(6,6,CV_64FC1)*1000));
1316  }
1317  }
1318  else
1319  {
1320  //============================================================
1321  // Refine neighbor links
1322  //============================================================
1323  UINFO("Odometry refining: guess = %s", guess.prettyPrint().c_str());
1324  RegistrationInfo info;
1325  Transform t = _memory->computeTransform(oldId, signature->id(), guess, &info);
1326  if(!t.isNull())
1327  {
1328  UINFO("Odometry refining: update neighbor link (%d->%d, variance:lin=%f, ang=%f) from %s to %s",
1329  oldId,
1330  signature->id(),
1331  info.covariance.at<double>(0,0),
1332  info.covariance.at<double>(5,5),
1333  guess.prettyPrint().c_str(),
1334  t.prettyPrint().c_str());
1335  UASSERT(info.covariance.at<double>(0,0) > 0.0 && info.covariance.at<double>(5,5) > 0.0);
1336  _memory->updateLink(Link(oldId, signature->id(), signature->getLinks().begin()->second.type(), t, info.covariance.inv()));
1337 
1339  {
1340  // update all previous nodes
1341  // Normally _mapCorrection should be identity, but if _optimizeFromGraphEnd
1342  // parameters just changed state, we should put back all poses without map correction.
1343  Transform u = guess * t.inverse();
1344  std::map<int, Transform>::iterator jter = _optimizedPoses.find(oldId);
1345  UASSERT(jter!=_optimizedPoses.end());
1346  Transform up = jter->second * u * jter->second.inverse();
1347  Transform mapCorrectionInv = _mapCorrection.inverse();
1348  for(std::map<int, Transform>::iterator iter=_optimizedPoses.begin(); iter!=_optimizedPoses.end(); ++iter)
1349  {
1350  iter->second = mapCorrectionInv * up * iter->second;
1351  }
1352  }
1353  }
1354  else
1355  {
1356  UINFO("Odometry refining rejected: %s", info.rejectedMsg.c_str());
1357  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)
1358  {
1360  {
1361  std::cout << info.covariance << std::endl;
1362  }
1363  _memory->updateLink(Link(oldId, signature->id(), signature->getLinks().begin()->second.type(), guess, (info.covariance*100.0).inv()));
1364  }
1365  }
1366  statistics_.addStatistic(Statistics::kNeighborLinkRefiningAccepted(), !t.isNull()?1.0f:0);
1367  statistics_.addStatistic(Statistics::kNeighborLinkRefiningInliers(), info.inliers);
1368  statistics_.addStatistic(Statistics::kNeighborLinkRefiningICP_inliers_ratio(), info.icpInliersRatio);
1369  statistics_.addStatistic(Statistics::kNeighborLinkRefiningICP_rotation(), info.icpRotation);
1370  statistics_.addStatistic(Statistics::kNeighborLinkRefiningICP_translation(), info.icpTranslation);
1371  statistics_.addStatistic(Statistics::kNeighborLinkRefiningICP_complexity(), info.icpStructuralComplexity);
1372  statistics_.addStatistic(Statistics::kNeighborLinkRefiningPts(), signature->sensorData().laserScanRaw().size());
1373  }
1374  timeNeighborLinkRefining = timer.ticks();
1375  ULOGGER_INFO("timeOdometryRefining=%fs", timeNeighborLinkRefining);
1376 
1377  UASSERT(oldS->hasLink(signature->id()));
1379 
1380  statistics_.addStatistic(Statistics::kNeighborLinkRefiningVariance(), oldS->getLinks().find(signature->id())->second.transVariance());
1381 
1382  newPose = _optimizedPoses.at(oldId) * oldS->getLinks().find(signature->id())->second.transform();
1383  _mapCorrection = newPose * signature->getPose().inverse();
1385  {
1386  UERROR("Map correction should be identity when optimizing from the last node. T=%s NewPose=%s OldPose=%s",
1387  _mapCorrection.prettyPrint().c_str(),
1388  newPose.prettyPrint().c_str(),
1389  signature->getPose().prettyPrint().c_str());
1390  }
1391  }
1392  else
1393  {
1394  UWARN("Neighbor link refining is activated but there are intermediate nodes (%d=%d %d=%d), aborting refining...",
1395  signature->id(), signature->getWeight(), oldS->id(), oldS->getWeight());
1396  newPose = _mapCorrection * signature->getPose();
1397  }
1398  }
1399  else
1400  {
1401  newPose = _mapCorrection * signature->getPose();
1402  }
1403 
1404  UDEBUG("Added pose %s (odom=%s)", newPose.prettyPrint().c_str(), signature->getPose().prettyPrint().c_str());
1405  // Update Poses and Constraints
1406  _optimizedPoses.insert(std::make_pair(signature->id(), newPose));
1407  if(_memory->isIncremental() && signature->getWeight() >= 0)
1408  {
1409  for(std::map<int, Link>::const_iterator iter = signature->getLandmarks().begin(); iter!=signature->getLandmarks().end(); ++iter)
1410  {
1411  if(_optimizedPoses.find(iter->first) == _optimizedPoses.end())
1412  {
1413  _optimizedPoses.insert(std::make_pair(iter->first, newPose*iter->second.transform()));
1414  }
1415  _constraints.insert(std::make_pair(iter->first, iter->second.inverse()));
1416  }
1417  }
1418 
1419  float distanceTravelledOld = _distanceTravelled;
1420 
1421  // only in mapping mode we add a neighbor link
1422  if(signature->getLinks().size() &&
1423  signature->getLinks().begin()->second.type() == Link::kNeighbor)
1424  {
1425  // link should be old to new
1426  UASSERT_MSG(signature->id() > signature->getLinks().begin()->second.to(),
1427  "Only forward links should be added.");
1428 
1429  Link tmp = signature->getLinks().begin()->second.inverse();
1430 
1431  _distanceTravelled += tmp.transform().getNorm();
1432 
1433  // if the previous node is an intermediate node, remove it from the local graph
1434  if(_constraints.size() &&
1435  _constraints.rbegin()->second.to() == signature->getLinks().begin()->second.to())
1436  {
1437  const Signature * s = _memory->getSignature(signature->getLinks().begin()->second.to());
1438  UASSERT(s!=0);
1439  if(s->getWeight() == -1)
1440  {
1441  tmp = _constraints.rbegin()->second.merge(tmp, tmp.type());
1442  _optimizedPoses.erase(s->id());
1443  _constraints.erase(--_constraints.end());
1444  }
1445  }
1446  _constraints.insert(std::make_pair(tmp.from(), tmp));
1447  }
1448  // Localization mode stuff
1449  _lastLocalizationPose = newPose; // keep in cache the latest corrected pose
1450  if(!_memory->isIncremental())
1451  {
1452  if(!_odomCacheAddLink.empty())
1453  {
1454  float odomDistance = (_odomCacheAddLink.rbegin()->second.inverse() * signature->getPose()).getNorm();
1455  _distanceTravelled += odomDistance;
1456  }
1457  _odomCacheAddLink.insert(std::make_pair(signature->id(), signature->getPose()));
1458  while(!_odomCacheAddLink.empty() && (int)_odomCacheAddLink.size() > _maxOdomCacheSize+1)
1459  {
1460  _odomCacheAddLink.erase(_odomCacheAddLink.begin());
1461  }
1462 
1463  if(_optimizationMaxError <= 0.0f || _maxOdomCacheSize <= 0)
1464  {
1465  _odomCachePoses.clear();
1466  _odomCacheConstraints.clear();
1467  }
1468  else if(!_odomCachePoses.empty())
1469  {
1470  if((int)_odomCachePoses.size() > _maxOdomCacheSize)
1471  {
1472  UWARN("Odometry poses cached for localization verification reached the "
1473  "maximum numbers of %d, clearing the buffer. The next localization "
1474  "won't be verified! Set %s to 0 if you want to disable the localization verification.",
1476  Parameters::kRGBDMaxOdomCacheSize().c_str());
1477  _odomCachePoses.clear();
1478  _odomCacheConstraints.clear();
1479  }
1480  else
1481  {
1482  _odomCacheConstraints.insert(
1483  std::make_pair(signature->id(),
1484  Link(signature->id(),
1485  _odomCacheConstraints.rbegin()->first,
1487  signature->getPose().inverse() * _odomCachePoses.rbegin()->second,
1488  odomCovariance.inv())));
1489  _odomCachePoses.insert(std::make_pair(signature->id(), signature->getPose())); // keep odometry poses
1490  }
1491  }
1492  }
1494 
1495  //============================================================
1496  // Reduced graph
1497  //============================================================
1498  //Verify if there are nodes that were merged through graph reduction
1499  if(statistics_.reducedIds().size())
1500  {
1501  for(unsigned int i=0; i<_path.size(); ++i)
1502  {
1503  std::map<int, int>::const_iterator iter = statistics_.reducedIds().find(_path[i].first);
1504  if(iter!= statistics_.reducedIds().end())
1505  {
1506  // change path ID to loop closure ID
1507  _path[i].first = iter->second;
1508  }
1509  }
1510 
1511  for(std::map<int, int>::const_iterator iter=statistics_.reducedIds().begin();
1512  iter!=statistics_.reducedIds().end();
1513  ++iter)
1514  {
1515  int erased = (int)_optimizedPoses.erase(iter->first);
1516  if(erased)
1517  {
1518  for(std::multimap<int, Link>::iterator jter = _constraints.begin(); jter!=_constraints.end();)
1519  {
1520  if(jter->second.from() == iter->first || jter->second.to() == iter->first)
1521  {
1522  _constraints.erase(jter++);
1523  }
1524  else
1525  {
1526  ++jter;
1527  }
1528  }
1529  }
1530  }
1531  }
1532 
1533  //============================================================
1534  // Local loop closure in TIME
1535  //============================================================
1536  if(_proximityByTime &&
1537  rehearsedId == 0 && // don't do it if rehearsal happened
1538  _memory->isIncremental() && // don't do it in localization mode
1539  signature->getWeight()>=0)
1540  {
1541  const std::set<int> & stm = _memory->getStMem();
1542  for(std::set<int>::const_reverse_iterator iter = stm.rbegin(); iter!=stm.rend(); ++iter)
1543  {
1544  if(*iter != signature->id() &&
1545  signature->getLinks().find(*iter) == signature->getLinks().end() &&
1546  _memory->getSignature(*iter)->mapId() == signature->mapId() &&
1547  _memory->getSignature(*iter)->getWeight()>=0)
1548  {
1549  std::string rejectedMsg;
1550  UDEBUG("Check local transform between %d and %d", signature->id(), *iter);
1551  RegistrationInfo info;
1552  Transform guess;
1553  if(_optimizedPoses.find(*iter) != _optimizedPoses.end())
1554  {
1555  guess = _optimizedPoses.at(*iter).inverse() * newPose;
1556  }
1557 
1558  // For proximity by time, correspondences should be already enough precise, so don't recompute them
1559  Transform transform = _memory->computeTransform(*iter, signature->id(), guess, &info, true);
1560 
1561  if(!transform.isNull())
1562  {
1563  transform = transform.inverse();
1564  UDEBUG("Add local loop closure in TIME (%d->%d) %s",
1565  signature->id(),
1566  *iter,
1567  transform.prettyPrint().c_str());
1568  // Add a loop constraint
1569  UASSERT(info.covariance.at<double>(0,0) > 0.0 && info.covariance.at<double>(5,5) > 0.0);
1570  if(_memory->addLink(Link(signature->id(), *iter, Link::kLocalTimeClosure, transform, getInformation(info.covariance))))
1571  {
1572  ++proximityDetectionsInTimeFound;
1573  UINFO("Local loop closure found between %d and %d with t=%s",
1574  *iter, signature->id(), transform.prettyPrint().c_str());
1575  }
1576  else
1577  {
1578  UWARN("Cannot add local loop closure between %d and %d ?!?",
1579  *iter, signature->id());
1580  }
1581  }
1582  else
1583  {
1584  UINFO("Local loop closure (time) between %d and %d rejected: %s",
1585  *iter, signature->id(), rejectedMsg.c_str());
1586  }
1587  }
1588  }
1589  }
1590  }
1591 
1592  timeProximityByTimeDetection = timer.ticks();
1593  UINFO("timeProximityByTimeDetection=%fs", timeProximityByTimeDetection);
1594 
1595  //============================================================
1596  // Bayes filter update
1597  //============================================================
1598  int previousId = signature->getLinks().size() && signature->getLinks().begin()->first!=signature->id()?signature->getLinks().begin()->first:0;
1599  // 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
1600  if(!signature->isBadSignature() && signature->getWeight()>=0 && (!smallDisplacement || _memory->getLoopClosureLinks(previousId, false).size() == 0) && !tooFastMovement)
1601  {
1602  // If the working memory is empty, don't do the detection. It happens when it
1603  // is the first time the detector is started (there needs some images to
1604  // fill the short-time memory before a signature is added to the working memory).
1605  if(_memory->getWorkingMem().size())
1606  {
1607  //============================================================
1608  // Likelihood computation
1609  // Get the likelihood of the new signature
1610  // with all images contained in the working memory + reactivated.
1611  //============================================================
1612  ULOGGER_INFO("computing likelihood...");
1613 
1614  std::list<int> signaturesToCompare;
1615  GPS originGPS;
1616  Transform originOffsetENU = Transform::getIdentity();
1617  if(_loopGPS)
1618  {
1619  originGPS = signature->sensorData().gps();
1620  if(originGPS.stamp() == 0.0 && _currentSessionHasGPS)
1621  {
1622  UTimer tmpT;
1623  if(_optimizedPoses.size() && _memory->isIncremental())
1624  {
1625  //Search for latest node having GPS linked to current signature not too far.
1626  std::map<int, float> nearestIds = graph::getNodesInRadius(signature->id(), _optimizedPoses, _localRadius);
1627  for(std::map<int, float>::reverse_iterator iter=nearestIds.rbegin(); iter!=nearestIds.rend() && iter->first>0; ++iter)
1628  {
1629  const Signature * s = _memory->getSignature(iter->first);
1630  UASSERT(s!=0);
1631  if(s->sensorData().gps().stamp() > 0.0)
1632  {
1633  originGPS = s->sensorData().gps();
1634  const Transform & sPose = _optimizedPoses.at(s->id());
1635  Transform localToENU(0,0,(float)((-(originGPS.bearing()-90))*M_PI/180.0) - sPose.theta());
1636  originOffsetENU = localToENU * (sPose.rotation()*(sPose.inverse()*_optimizedPoses.at(signature->id())));
1637  break;
1638  }
1639  }
1640  }
1641  //else if(!_memory->isIncremental()) // TODO, how can we estimate current GPS position in localization?
1642  //{
1643  //}
1644  }
1645  if(originGPS.stamp() > 0.0)
1646  {
1647  // no need to save it if it is in localization mode
1648  _gpsGeocentricCache.insert(std::make_pair(signature->id(), std::make_pair(originGPS.toGeodeticCoords().toGeocentric_WGS84(), originOffsetENU)));
1649  }
1650  }
1651 
1652  for(std::map<int, double>::const_iterator iter=_memory->getWorkingMem().begin();
1653  iter!=_memory->getWorkingMem().end();
1654  ++iter)
1655  {
1656  if(iter->first > 0)
1657  {
1658  const Signature * s = _memory->getSignature(iter->first);
1659  UASSERT(s!=0);
1660  if(s->getWeight() != -1) // ignore intermediate nodes
1661  {
1662  bool accept = true;
1663  if(originGPS.stamp()>0.0)
1664  {
1665  std::map<int, std::pair<cv::Point3d, Transform> >::iterator cacheIter = _gpsGeocentricCache.find(s->id());
1666  if(cacheIter == _gpsGeocentricCache.end())
1667  {
1668  GPS gps = s->sensorData().gps();
1669  Transform offsetENU = Transform::getIdentity();
1670  if(gps.stamp()==0.0)
1671  {
1672  _memory->getGPS(s->id(), gps, offsetENU, false);
1673  }
1674  if(gps.stamp() > 0.0)
1675  {
1676  cacheIter = _gpsGeocentricCache.insert(
1677  std::make_pair(s->id(),
1678  std::make_pair(gps.toGeodeticCoords().toGeocentric_WGS84(), offsetENU))).first;
1679  }
1680  }
1681 
1682 
1683  if(cacheIter != _gpsGeocentricCache.end())
1684  {
1685  std::map<int, std::pair<cv::Point3d, Transform> >::iterator originIter = _gpsGeocentricCache.find(signature->id());
1686  UASSERT(originIter != _gpsGeocentricCache.end());
1687  cv::Point3d relativePose = GeodeticCoords::Geocentric_WGS84ToENU_WGS84(cacheIter->second.first, originIter->second.first, originGPS.toGeodeticCoords());
1688  const double & error = originGPS.error();
1689  const Transform & offsetENU = cacheIter->second.second;
1690  relativePose.x += offsetENU.x() - originOffsetENU.x();
1691  relativePose.y += offsetENU.y() - originOffsetENU.y();
1692  relativePose.z += offsetENU.z() - originOffsetENU.z();
1693  // ignore altitude if difference is under GPS error
1694  if(relativePose.z>error)
1695  {
1696  relativePose.z -= error;
1697  }
1698  else if(relativePose.z < -error)
1699  {
1700  relativePose.z += error;
1701  }
1702  else
1703  {
1704  relativePose.z = 0;
1705  }
1706  accept = uNormSquared(relativePose.x, relativePose.y, relativePose.z) < _localRadius*_localRadius;
1707  }
1708  }
1709 
1710  if(accept)
1711  {
1712  signaturesToCompare.push_back(iter->first);
1713  }
1714  }
1715  }
1716  else
1717  {
1718  // virtual signature should be added
1719  signaturesToCompare.push_back(iter->first);
1720  }
1721  }
1722 
1723  rawLikelihood = _memory->computeLikelihood(signature, signaturesToCompare);
1724 
1725  // Adjust the likelihood (with mean and std dev)
1726  likelihood = rawLikelihood;
1727  this->adjustLikelihood(likelihood);
1728 
1729  timeLikelihoodCalculation = timer.ticks();
1730  ULOGGER_INFO("timeLikelihoodCalculation=%fs",timeLikelihoodCalculation);
1731 
1732  //============================================================
1733  // Apply the Bayes filter
1734  // Posterior = Likelihood x Prior
1735  //============================================================
1736  ULOGGER_INFO("getting posterior...");
1737 
1738  // Compute the posterior
1739  posterior = _bayesFilter->computePosterior(_memory, likelihood);
1740  timePosteriorCalculation = timer.ticks();
1741  ULOGGER_INFO("timePosteriorCalculation=%fs",timePosteriorCalculation);
1742 
1743  // For statistics, copy weights
1745  {
1746  weights = _memory->getWeights();
1747  }
1748 
1749  //============================================================
1750  // Select the highest hypothesis
1751  //============================================================
1752  ULOGGER_INFO("creating hypotheses...");
1753  if(posterior.size())
1754  {
1755  for(std::map<int, float>::const_reverse_iterator iter = posterior.rbegin(); iter != posterior.rend(); ++iter)
1756  {
1757  if(iter->first > 0 && iter->second > _highestHypothesis.second)
1758  {
1759  _highestHypothesis = *iter;
1760  }
1761  }
1762  // With the virtual place, use sum of LC probabilities (1 - virtual place hypothesis).
1763  _highestHypothesis.second = 1-posterior.begin()->second;
1764  }
1765  timeHypothesesCreation = timer.ticks();
1766  ULOGGER_INFO("Highest hypothesis=%d, value=%f, timeHypothesesCreation=%fs", _highestHypothesis.first, _highestHypothesis.second, timeHypothesesCreation);
1767 
1768  if(_highestHypothesis.first > 0)
1769  {
1770  float loopThr = _loopThr;
1772  graph::filterLinks(signature->getLinks(), Link::kSelfRefLink).size() == 0 && // alone in the current map
1773  _memory->getWorkingMem().size()>1 && // should have an old map (beside virtual signature)
1774  (int)_memory->getWorkingMem().size()<=_memory->getMaxStMemSize() &&
1775  _rgbdSlamMode)
1776  {
1777  // If the map is very small (under STM size) and we need to find
1778  // a loop closure before continuing the map or localizing,
1779  // use the best hypothesis directly.
1780  loopThr = 0.0f;
1781  }
1782 
1783  // Loop closure Threshold
1784  // When _loopThr=0, accept loop closure if the hypothesis is over
1785  // the virtual (new) place hypothesis.
1786  if(_highestHypothesis.second >= loopThr)
1787  {
1788  rejectedGlobalLoopClosure = true;
1789  if(posterior.size() <= 2 && loopThr>0.0f)
1790  {
1791  // Ignore loop closure if there is only one loop closure hypothesis
1792  UDEBUG("rejected hypothesis: single hypothesis");
1793  }
1795  {
1796  UWARN("rejected hypothesis: by epipolar geometry");
1797  }
1798  else if(_loopRatio > 0.0f && lastHighestHypothesis.second && _highestHypothesis.second < _loopRatio*lastHighestHypothesis.second)
1799  {
1800  UWARN("rejected hypothesis: not satisfying hypothesis ratio (%f < %f * %f)",
1801  _highestHypothesis.second, _loopRatio, lastHighestHypothesis.second);
1802  }
1803  else if(_loopRatio > 0.0f && lastHighestHypothesis.second == 0)
1804  {
1805  UWARN("rejected hypothesis: last closure hypothesis is null (loop ratio is on)");
1806  }
1807  else
1808  {
1810  rejectedGlobalLoopClosure = false;
1811  }
1812 
1813  timeHypothesesValidation = timer.ticks();
1814  ULOGGER_INFO("timeHypothesesValidation=%fs",timeHypothesesValidation);
1815  }
1816  else if(_highestHypothesis.second < _loopRatio*lastHighestHypothesis.second)
1817  {
1818  // Used for Precision-Recall computation.
1819  // When analyzing logs, it's convenient to know
1820  // if the hypothesis would be rejected if T_loop would be lower.
1821  rejectedGlobalLoopClosure = true;
1822  UDEBUG("rejected hypothesis: under loop ratio %f < %f", _highestHypothesis.second, _loopRatio*lastHighestHypothesis.second);
1823  }
1824 
1825  //for statistic...
1826  hypothesisRatio = _loopClosureHypothesis.second>0?_highestHypothesis.second/_loopClosureHypothesis.second:0;
1827  }
1828  } // if(_memory->getWorkingMemSize())
1829  }// !isBadSignature
1830  else if(!signature->isBadSignature() && (smallDisplacement || tooFastMovement))
1831  {
1832  _highestHypothesis = lastHighestHypothesis;
1833  }
1834 
1835  //============================================================
1836  // Before retrieval, make sure the trash has finished
1837  //============================================================
1839  timeEmptyingTrash = _memory->getDbSavingTime();
1840  timeJoiningTrash = timer.ticks();
1841  ULOGGER_INFO("Time emptying memory trash = %fs, joining (actual overhead) = %fs", timeEmptyingTrash, timeJoiningTrash);
1842 
1843  //============================================================
1844  // RETRIEVAL 1/3 : Loop closure neighbors reactivation
1845  //============================================================
1846  int retrievalId = _highestHypothesis.first;
1847  std::list<int> reactivatedIds;
1848  double timeGetNeighborsTimeDb = 0.0;
1849  double timeGetNeighborsSpaceDb = 0.0;
1850  int immunizedGlobally = 0;
1851  int immunizedLocally = 0;
1852  int maxLocalLocationsImmunized = 0;
1853  if(_maxTimeAllowed != 0 || _maxMemoryAllowed != 0)
1854  {
1855  // with memory management, we have to immunize some nodes
1856  maxLocalLocationsImmunized = _localImmunizationRatio * float(_memory->getWorkingMem().size());
1857  }
1858  // no need to do retrieval or immunization of locations if memory management
1859  // is disabled and all nodes are in WM
1860  if(!(_memory->allNodesInWM() && maxLocalLocationsImmunized == 0))
1861  {
1862  if(retrievalId > 0)
1863  {
1864  //Load neighbors
1865  ULOGGER_INFO("Retrieving locations... around id=%d", retrievalId);
1866  int neighborhoodSize = (int)_bayesFilter->getPredictionLC().size()-1;
1867  UASSERT(neighborhoodSize >= 0);
1868  ULOGGER_DEBUG("margin=%d maxRetieved=%d", neighborhoodSize, _maxRetrieved);
1869 
1870  UTimer timeGetN;
1871  unsigned int nbLoadedFromDb = 0;
1872  std::set<int> reactivatedIdsSet;
1873  std::map<int, int> neighbors;
1874  int nbDirectNeighborsInDb = 0;
1875 
1876  // priority in time
1877  // Direct neighbors TIME
1878  ULOGGER_DEBUG("In TIME");
1879  neighbors = _memory->getNeighborsId(retrievalId,
1880  neighborhoodSize,
1881  _maxRetrieved,
1882  true,
1883  true,
1884  false,
1885  true,
1886  std::set<int>(),
1887  &timeGetNeighborsTimeDb);
1888  ULOGGER_DEBUG("neighbors of %d in time = %d", retrievalId, (int)neighbors.size());
1889  //Priority to locations near in time (direct neighbor) then by space (loop closure)
1890  bool firstPassDone = false; // just to avoid checking to STM after the first pass
1891  int m = 0;
1892  while(m < neighborhoodSize)
1893  {
1894  std::set<int> idsSorted;
1895  for(std::map<int, int>::iterator iter=neighbors.begin(); iter!=neighbors.end();)
1896  {
1897  if(!firstPassDone && _memory->isInSTM(iter->first))
1898  {
1899  neighbors.erase(iter++);
1900  }
1901  else if(iter->second == m)
1902  {
1903  if(reactivatedIdsSet.find(iter->first) == reactivatedIdsSet.end())
1904  {
1905  idsSorted.insert(iter->first);
1906  reactivatedIdsSet.insert(iter->first);
1907 
1908  if(m == 1 && _memory->getSignature(iter->first) == 0)
1909  {
1910  ++nbDirectNeighborsInDb;
1911  }
1912 
1913  //immunized locations in the neighborhood from being transferred
1914  if(immunizedLocations.insert(iter->first).second)
1915  {
1916  ++immunizedGlobally;
1917  }
1918 
1919  //UDEBUG("nt=%d m=%d immunized=1", iter->first, iter->second);
1920  }
1921  neighbors.erase(iter++);
1922  }
1923  else
1924  {
1925  ++iter;
1926  }
1927  }
1928  firstPassDone = true;
1929  reactivatedIds.insert(reactivatedIds.end(), idsSorted.rbegin(), idsSorted.rend());
1930  ++m;
1931  }
1932 
1933  // neighbors SPACE, already added direct neighbors will be ignored
1934  ULOGGER_DEBUG("In SPACE");
1935  neighbors = _memory->getNeighborsId(retrievalId,
1936  neighborhoodSize,
1937  _maxRetrieved,
1938  true,
1939  false,
1940  false,
1941  false,
1942  std::set<int>(),
1943  &timeGetNeighborsSpaceDb);
1944  ULOGGER_DEBUG("neighbors of %d in space = %d", retrievalId, (int)neighbors.size());
1945  firstPassDone = false;
1946  m = 0;
1947  while(m < neighborhoodSize)
1948  {
1949  std::set<int> idsSorted;
1950  for(std::map<int, int>::iterator iter=neighbors.begin(); iter!=neighbors.end();)
1951  {
1952  if(!firstPassDone && _memory->isInSTM(iter->first))
1953  {
1954  neighbors.erase(iter++);
1955  }
1956  else if(iter->second == m)
1957  {
1958  if(reactivatedIdsSet.find(iter->first) == reactivatedIdsSet.end())
1959  {
1960  idsSorted.insert(iter->first);
1961  reactivatedIdsSet.insert(iter->first);
1962 
1963  if(m == 1 && _memory->getSignature(iter->first) == 0)
1964  {
1965  ++nbDirectNeighborsInDb;
1966  }
1967  //UDEBUG("nt=%d m=%d", iter->first, iter->second);
1968  }
1969  neighbors.erase(iter++);
1970  }
1971  else
1972  {
1973  ++iter;
1974  }
1975  }
1976  firstPassDone = true;
1977  reactivatedIds.insert(reactivatedIds.end(), idsSorted.rbegin(), idsSorted.rend());
1978  ++m;
1979  }
1980  ULOGGER_INFO("neighborhoodSize=%d, "
1981  "reactivatedIds.size=%d, "
1982  "nbLoadedFromDb=%d, "
1983  "nbDirectNeighborsInDb=%d, "
1984  "time=%fs (%fs %fs)",
1985  neighborhoodSize,
1986  reactivatedIds.size(),
1987  (int)nbLoadedFromDb,
1988  nbDirectNeighborsInDb,
1989  timeGetN.ticks(),
1990  timeGetNeighborsTimeDb,
1991  timeGetNeighborsSpaceDb);
1992 
1993  }
1994  }
1995 
1996  //============================================================
1997  // RETRIEVAL 2/3 : Update planned path and get next nodes to retrieve
1998  //============================================================
1999  std::list<int> retrievalLocalIds;
2000  if(_rgbdSlamMode)
2001  {
2002  // Priority on locations on the planned path
2003  if(_path.size())
2004  {
2005  updateGoalIndex();
2006 
2007  float distanceSoFar = 0.0f;
2008  // immunize all nodes after current node and
2009  // retrieve nodes after current node in the maximum radius from the current node
2010  for(unsigned int i=_pathCurrentIndex; i<_path.size(); ++i)
2011  {
2012  if(_localRadius > 0.0f && i != _pathCurrentIndex)
2013  {
2014  distanceSoFar += _path[i-1].second.getDistance(_path[i].second);
2015  }
2016 
2017  if(distanceSoFar <= _localRadius)
2018  {
2019  if(_memory->getSignature(_path[i].first) != 0)
2020  {
2021  if(immunizedLocations.insert(_path[i].first).second)
2022  {
2023  ++immunizedLocally;
2024  }
2025  UDEBUG("Path immunization: node %d (dist=%fm)", _path[i].first, distanceSoFar);
2026  }
2027  else if(retrievalLocalIds.size() < _maxLocalRetrieved)
2028  {
2029  UINFO("retrieval of node %d on path (dist=%fm)", _path[i].first, distanceSoFar);
2030  retrievalLocalIds.push_back(_path[i].first);
2031  // retrieved locations are automatically immunized
2032  }
2033  }
2034  else
2035  {
2036  UDEBUG("Stop on node %d (dist=%fm > %fm)",
2037  _path[i].first, distanceSoFar, _localRadius);
2038  break;
2039  }
2040  }
2041  }
2042 
2043  if(!(_memory->allNodesInWM() && maxLocalLocationsImmunized == 0))
2044  {
2045  // immunize the path from the nearest local location to the current location
2046  if(immunizedLocally < maxLocalLocationsImmunized &&
2047  _memory->isIncremental()) // Can only work in mapping mode
2048  {
2049  std::map<int ,Transform> poses;
2050  // remove poses from STM
2051  for(std::map<int, Transform>::iterator iter=_optimizedPoses.begin(); iter!=_optimizedPoses.end(); ++iter)
2052  {
2053  if(iter->first > 0 && !_memory->isInSTM(iter->first))
2054  {
2055  poses.insert(*iter);
2056  }
2057  }
2058  int nearestId = graph::findNearestNode(poses, _optimizedPoses.at(signature->id()));
2059 
2060  if(nearestId > 0 &&
2061  (_localRadius==0 ||
2062  _optimizedPoses.at(signature->id()).getDistance(_optimizedPoses.at(nearestId)) < _localRadius))
2063  {
2064  std::multimap<int, int> links;
2065  for(std::multimap<int, Link>::iterator iter=_constraints.begin(); iter!=_constraints.end(); ++iter)
2066  {
2067  if(uContains(_optimizedPoses, iter->second.from()) && uContains(_optimizedPoses, iter->second.to()))
2068  {
2069  links.insert(std::make_pair(iter->second.from(), iter->second.to()));
2070  links.insert(std::make_pair(iter->second.to(), iter->second.from())); // <->
2071  }
2072  }
2073 
2074  std::list<std::pair<int, Transform> > path = graph::computePath(_optimizedPoses, links, nearestId, signature->id());
2075  if(path.size() == 0)
2076  {
2077  UWARN("Could not compute a path between %d and %d", nearestId, signature->id());
2078  }
2079  else
2080  {
2081  for(std::list<std::pair<int, Transform> >::iterator iter=path.begin();
2082  iter!=path.end();
2083  ++iter)
2084  {
2085  if(iter->first>0)
2086  {
2087  if(immunizedLocally >= maxLocalLocationsImmunized)
2088  {
2089  // set 20 to avoid this warning when starting mapping
2090  if(maxLocalLocationsImmunized > 20 && _someNodesHaveBeenTransferred)
2091  {
2092  UWARN("Could not immunize the whole local path (%d) between "
2093  "%d and %d (max location immunized=%d). You may want "
2094  "to increase RGBD/LocalImmunizationRatio (current=%f (%d of WM=%d)) "
2095  "to be able to immunize longer paths.",
2096  (int)path.size(),
2097  nearestId,
2098  signature->id(),
2099  maxLocalLocationsImmunized,
2101  maxLocalLocationsImmunized,
2102  (int)_memory->getWorkingMem().size());
2103  }
2104  break;
2105  }
2106  else if(!_memory->isInSTM(iter->first))
2107  {
2108  if(immunizedLocations.insert(iter->first).second)
2109  {
2110  ++immunizedLocally;
2111  }
2112  //UDEBUG("local node %d on path immunized=1", iter->first);
2113  }
2114  }
2115  }
2116  }
2117  }
2118  }
2119 
2120  // retrieval based on the nodes close the the nearest pose in WM
2121  // immunize closest nodes
2122  std::map<int, float> nearNodes = graph::getNodesInRadius(signature->id(), _optimizedPoses, _localRadius);
2123  // sort by distance
2124  std::multimap<float, int> nearNodesByDist;
2125  for(std::map<int, float>::iterator iter=nearNodes.lower_bound(1); iter!=nearNodes.end(); ++iter)
2126  {
2127  nearNodesByDist.insert(std::make_pair(iter->second, iter->first));
2128  }
2129  UINFO("near nodes=%d, max local immunized=%d, ratio=%f WM=%d",
2130  (int)nearNodesByDist.size(),
2131  maxLocalLocationsImmunized,
2133  (int)_memory->getWorkingMem().size());
2134  for(std::multimap<float, int>::iterator iter=nearNodesByDist.begin();
2135  iter!=nearNodesByDist.end() && (retrievalLocalIds.size() < _maxLocalRetrieved || immunizedLocally < maxLocalLocationsImmunized);
2136  ++iter)
2137  {
2138  const Signature * s = _memory->getSignature(iter->second);
2139  if(s!=0)
2140  {
2141  // If there is a change of direction, better to be retrieving
2142  // ALL nearest signatures than only newest neighbors
2143  const std::multimap<int, Link> & links = s->getLinks();
2144  for(std::multimap<int, Link>::const_reverse_iterator jter=links.rbegin();
2145  jter!=links.rend() && retrievalLocalIds.size() < _maxLocalRetrieved;
2146  ++jter)
2147  {
2148  if(_memory->getSignature(jter->first) == 0)
2149  {
2150  UINFO("retrieval of node %d on local map", jter->first);
2151  retrievalLocalIds.push_back(jter->first);
2152  }
2153  }
2154  if(!_memory->isInSTM(s->id()) && immunizedLocally < maxLocalLocationsImmunized)
2155  {
2156  if(immunizedLocations.insert(s->id()).second)
2157  {
2158  ++immunizedLocally;
2159  }
2160  //UDEBUG("local node %d (%f m) immunized=1", iter->second, iter->first);
2161  }
2162  }
2163  }
2164  // well, if the maximum retrieved is not reached, look for neighbors in database
2165  if(retrievalLocalIds.size() < _maxLocalRetrieved)
2166  {
2167  std::set<int> retrievalLocalIdsSet(retrievalLocalIds.begin(), retrievalLocalIds.end());
2168  for(std::list<int>::iterator iter=retrievalLocalIds.begin();
2169  iter!=retrievalLocalIds.end() && retrievalLocalIds.size() < _maxLocalRetrieved;
2170  ++iter)
2171  {
2172  std::map<int, int> ids = _memory->getNeighborsId(*iter, 2, _maxLocalRetrieved - (unsigned int)retrievalLocalIds.size() + 1, true, false);
2173  for(std::map<int, int>::reverse_iterator jter=ids.rbegin();
2174  jter!=ids.rend() && retrievalLocalIds.size() < _maxLocalRetrieved;
2175  ++jter)
2176  {
2177  if(_memory->getSignature(jter->first) == 0 &&
2178  retrievalLocalIdsSet.find(jter->first) == retrievalLocalIdsSet.end())
2179  {
2180  UINFO("retrieval of node %d on local map", jter->first);
2181  retrievalLocalIds.push_back(jter->first);
2182  retrievalLocalIdsSet.insert(jter->first);
2183  }
2184  }
2185  }
2186  }
2187 
2188  // update Age of the close signatures (oldest the farthest)
2189  for(std::multimap<float, int>::reverse_iterator iter=nearNodesByDist.rbegin(); iter!=nearNodesByDist.rend(); ++iter)
2190  {
2191  _memory->updateAge(iter->second);
2192  }
2193 
2194  // insert them first to make sure they are loaded.
2195  reactivatedIds.insert(reactivatedIds.begin(), retrievalLocalIds.begin(), retrievalLocalIds.end());
2196  }
2197 
2198  //============================================================
2199  // RETRIEVAL 3/3 : Load signatures from the database
2200  //============================================================
2201  if(reactivatedIds.size())
2202  {
2203  // Not important if the loop closure hypothesis don't have all its neighbors loaded,
2204  // only a loop closure link is added...
2205  signaturesRetrieved = _memory->reactivateSignatures(
2206  reactivatedIds,
2207  _maxRetrieved+(unsigned int)retrievalLocalIds.size(), // add path retrieved
2208  timeRetrievalDbAccess);
2209 
2210  ULOGGER_INFO("retrieval of %d (db time = %fs)", (int)signaturesRetrieved.size(), timeRetrievalDbAccess);
2211 
2212  timeRetrievalDbAccess += timeGetNeighborsTimeDb + timeGetNeighborsSpaceDb;
2213  UINFO("total timeRetrievalDbAccess=%fs", timeRetrievalDbAccess);
2214 
2215  // Immunize just retrieved signatures
2216  immunizedLocations.insert(signaturesRetrieved.begin(), signaturesRetrieved.end());
2217  }
2218  timeReactivations = timer.ticks();
2219  ULOGGER_INFO("timeReactivations=%fs", timeReactivations);
2220  }
2221 
2222  //============================================================
2223  // Landmark
2224  //============================================================
2225  int landmarkDetected = 0;
2226  bool rejectedLandmark = false;
2227  std::set<int> landmarkDetectedNodesRef;
2228  if(!signature->getLandmarks().empty())
2229  {
2230  for(std::map<int, Link>::const_iterator iter=signature->getLandmarks().begin(); iter!=signature->getLandmarks().end(); ++iter)
2231  {
2232  if(uContains(_memory->getLandmarksInvertedIndex(), iter->first) &&
2233  _memory->getLandmarksInvertedIndex().find(iter->first)->second.size()>1)
2234  {
2235  landmarkDetected = iter->first;
2236  landmarkDetectedNodesRef = _memory->getLandmarksInvertedIndex().find(iter->first)->second;
2237  UINFO("Landmark %d observed again! Seen the first time by node %d.", -iter->first, *landmarkDetectedNodesRef.begin());
2238  break;
2239  }
2240  }
2241  }
2242 
2243  //============================================================
2244  // Proximity detections
2245  //============================================================
2246  std::list<std::pair<int, int> > loopClosureLinksAdded;
2247  int loopClosureVisualInliers = 0; // for statistics
2248  float loopClosureVisualInliersRatio = 0.0f;
2249  int loopClosureVisualMatches = 0;
2250  float loopClosureLinearVariance = 0.0f;
2251  float loopClosureAngularVariance = 0.0f;
2252  float loopClosureVisualInliersMeanDist = 0;
2253  float loopClosureVisualInliersDistribution = 0;
2254 
2255  int proximityDetectionsAddedVisually = 0;
2256  int proximityDetectionsAddedByICPOnly = 0;
2257  int lastProximitySpaceClosureId = 0;
2258  int proximitySpacePaths = 0;
2259  int localVisualPathsChecked = 0;
2260  int localScanPathsChecked = 0;
2261  int loopIdSuppressedByProximity = 0;
2262 
2263  if(_proximityBySpace &&
2264  _localRadius > 0 &&
2265  _rgbdSlamMode &&
2266  signature->getWeight() >= 0) // not an intermediate node
2267  {
2268  if(_graphOptimizer->iterations() == 0)
2269  {
2270  UWARN("Cannot do local loop closure detection in space if graph optimization is disabled!");
2271  }
2272  else if(_memory->isIncremental() || landmarkDetected == 0)
2273  {
2274  // In localization mode, no need to check local loop
2275  // closures if we are already localized by a landmark.
2276 
2277  // don't do it if it is a small displacement unless the previous signature didn't have a loop closure
2278  // don't do it if there is a too fast movement
2279  if((!smallDisplacement || _memory->getLoopClosureLinks(previousId, false).size() == 0) && !tooFastMovement)
2280  {
2281 
2282  //============================================================
2283  // LOCAL LOOP CLOSURE SPACE
2284  //============================================================
2285 
2286  //
2287  // 1) compare visually with nearest locations
2288  //
2289  UDEBUG("Proximity detection (local loop closure in SPACE using matching images, local radius=%fm)", _localRadius);
2290  std::map<int, float> nearestIds;
2292  {
2294  }
2295  else
2296  {
2297  nearestIds = graph::getNodesInRadius(signature->id(), _optimizedPoses, _localRadius);
2298  }
2299  UDEBUG("nearestIds=%d/%d", (int)nearestIds.size(), (int)_optimizedPoses.size());
2300  std::map<int, Transform> nearestPoses;
2301  for(std::map<int, float>::iterator iter=nearestIds.lower_bound(1); iter!=nearestIds.end(); ++iter)
2302  {
2303  if(_memory->getStMem().find(iter->first) == _memory->getStMem().end())
2304  {
2305  nearestPoses.insert(std::make_pair(iter->first, _optimizedPoses.at(iter->first)));
2306  }
2307  }
2308  UDEBUG("nearestPoses=%d", (int)nearestPoses.size());
2309 
2310  // segment poses by paths, only one detection per path, landmarks are ignored
2311  std::map<int, std::map<int, Transform> > nearestPathsNotSorted = getPaths(nearestPoses, _optimizedPoses.at(signature->id()), _proximityMaxGraphDepth);
2312  UDEBUG("got %d paths", (int)nearestPathsNotSorted.size());
2313  // sort nearest paths by highest likelihood (if two have same likelihood, sort by id)
2314  std::map<NearestPathKey, std::map<int, Transform> > nearestPaths;
2315  for(std::map<int, std::map<int, Transform> >::const_iterator iter=nearestPathsNotSorted.begin();iter!=nearestPathsNotSorted.end(); ++iter)
2316  {
2317  const std::map<int, Transform> & path = iter->second;
2318  float highestLikelihood = 0.0f;
2319  int highestLikelihoodId = iter->first;
2320  for(std::map<int, Transform>::const_iterator jter=path.begin(); jter!=path.end(); ++jter)
2321  {
2322  float v = uValue(likelihood, jter->first, 0.0f);
2323  if(v > highestLikelihood)
2324  {
2325  highestLikelihood = v;
2326  highestLikelihoodId = jter->first;
2327  }
2328  }
2329  nearestPaths.insert(std::make_pair(NearestPathKey(highestLikelihood, highestLikelihoodId), path));
2330  }
2331  UDEBUG("nearestPaths=%d proximityMaxPaths=%d", (int)nearestPaths.size(), _proximityMaxPaths);
2332 
2333  float proximityFilteringRadius = _proximityFilteringRadius;
2334  if(_maxLoopClosureDistance>0.0f && (proximityFilteringRadius <= 0.0f || _maxLoopClosureDistance<proximityFilteringRadius))
2335  {
2336  proximityFilteringRadius = _maxLoopClosureDistance;
2337  }
2338  for(std::map<NearestPathKey, std::map<int, Transform> >::const_reverse_iterator iter=nearestPaths.rbegin();
2339  iter!=nearestPaths.rend() &&
2340  (_memory->isIncremental() || lastProximitySpaceClosureId == 0) &&
2341  (_proximityMaxPaths <= 0 || localVisualPathsChecked < _proximityMaxPaths);
2342  ++iter)
2343  {
2344  std::map<int, Transform> path = iter->second;
2345  UASSERT(path.size());
2346 
2347  //find the nearest pose on the path looking in the same direction
2348  path.insert(std::make_pair(signature->id(), _optimizedPoses.at(signature->id())));
2349  path = graph::getPosesInRadius(signature->id(), path, _localRadius, _proximityAngle);
2350  //take the one with highest likelihood if not null
2351  int nearestId = 0;
2352  if(iter->first.likelihood > 0.0f &&
2353  path.find(iter->first.id)!=path.end())
2354  {
2355  nearestId = iter->first.id;
2356  }
2357  else
2358  {
2359  nearestId = rtabmap::graph::findNearestNode(path, _optimizedPoses.at(signature->id()));
2360  }
2361 
2362  if(nearestId > 0)
2363  {
2364  // nearest pose must not be linked to current location and enough close
2365  if(!signature->hasLink(nearestId) &&
2366  (proximityFilteringRadius <= 0.0f ||
2367  _optimizedPoses.at(signature->id()).getDistanceSquared(_optimizedPoses.at(nearestId)) < proximityFilteringRadius*proximityFilteringRadius))
2368  {
2369  ++localVisualPathsChecked;
2370  RegistrationInfo info;
2371  Transform guess;
2373  {
2374  // Use odometry as guess so that correspondences can be computed by projection
2375  guess = _optimizedPoses.at(nearestId).inverse()*_optimizedPoses.at(signature->id());
2376  } //else: guess is null to make sure visual correspondences are globally computed
2377  Transform transform = _memory->computeTransform(nearestId, signature->id(), guess, &info);
2378  if(!transform.isNull())
2379  {
2380  transform = transform.inverse();
2381  if(proximityFilteringRadius <= 0 || transform.getNormSquared() <= proximityFilteringRadius*proximityFilteringRadius)
2382  {
2383  UINFO("[Visual] Add local loop closure in SPACE (%d->%d) %s",
2384  signature->id(),
2385  nearestId,
2386  transform.prettyPrint().c_str());
2387  UASSERT(info.covariance.at<double>(0,0) > 0.0 && info.covariance.at<double>(5,5) > 0.0);
2388  cv::Mat information = getInformation(info.covariance);
2389  _memory->addLink(Link(signature->id(), nearestId, Link::kLocalSpaceClosure, transform, information));
2390  loopClosureLinksAdded.push_back(std::make_pair(signature->id(), nearestId));
2391 
2392  //for statistics
2393  loopClosureVisualInliersMeanDist = info.inliersMeanDistance;
2394  loopClosureVisualInliersDistribution = info.inliersDistribution;
2395 
2396  ++proximityDetectionsAddedVisually;
2397  lastProximitySpaceClosureId = nearestId;
2398 
2399  loopClosureVisualInliers = info.inliers;
2400  loopClosureVisualInliersRatio = info.inliersRatio;
2401  loopClosureVisualMatches = info.matches;
2402 
2403  loopClosureLinearVariance = 1.0/information.at<double>(0,0);
2404  loopClosureAngularVariance = 1.0/information.at<double>(5,5);
2405 
2406  if(_loopClosureHypothesis.first>0 &&
2407  nearestIds.find(_loopClosureHypothesis.first)!=nearestIds.end())
2408  {
2409  UDEBUG("Proximity detection on %d is close to loop closure %d, ignoring loop closure transform estimation...",
2410  nearestId, _loopClosureHypothesis.first);
2411  // In localization mode, avoid transform
2412  // computation on the global loop closure if a visual proximity
2413  // one has been detected close (inside proximity radius) to that hypothesis.
2414  loopIdSuppressedByProximity = _loopClosureHypothesis.first;
2415  _loopClosureHypothesis.first = 0;
2416  }
2417  }
2418  else
2419  {
2420  UWARN("Ignoring local loop closure with %d because resulting "
2421  "transform is too large!? (%fm > %fm)",
2422  nearestId, transform.getNorm(), proximityFilteringRadius);
2423  }
2424  }
2425  }
2426  }
2427  }
2428 
2429  timeProximityBySpaceVisualDetection = timer.ticks();
2430  ULOGGER_INFO("timeProximityBySpaceVisualDetection=%fs", timeProximityBySpaceVisualDetection);
2431 
2432  //
2433  // 2) compare locally with nearest locations by scan matching
2434  //
2435  UDEBUG("Proximity detection (local loop closure in SPACE with scan matching)");
2436  if( _proximityMaxNeighbors <= 0)
2437  {
2438  UDEBUG("Proximity by scan matching is disabled (%s=%d).", Parameters::kRGBDProximityPathMaxNeighbors().c_str(), _proximityMaxNeighbors);
2439  }
2440  else if(!signature->sensorData().laserScanCompressed().isEmpty() &&
2441  (_memory->isIncremental() || lastProximitySpaceClosureId == 0))
2442  {
2443  // In localization mode, no need to check local loop
2444  // closures if we are already localized by at least one
2445  // local visual closure above.
2446 
2447  proximitySpacePaths = (int)nearestPaths.size();
2448  for(std::map<NearestPathKey, std::map<int, Transform> >::const_reverse_iterator iter=nearestPaths.rbegin();
2449  iter!=nearestPaths.rend() &&
2450  (_memory->isIncremental() || lastProximitySpaceClosureId == 0) &&
2451  (_proximityMaxPaths <= 0 || localScanPathsChecked < _proximityMaxPaths);
2452  ++iter)
2453  {
2454  std::map<int, Transform> path = iter->second; // should contain only nodes (no landmarks)
2455  UASSERT(path.size());
2456  UASSERT(path.begin()->first > 0);
2457 
2458  //find the nearest pose on the path
2459  int nearestId = rtabmap::graph::findNearestNode(path, _optimizedPoses.at(signature->id()));
2460  UASSERT(nearestId > 0);
2461  //UDEBUG("Path %d (size=%d) distance=%fm", nearestId, (int)path.size(), _optimizedPoses.at(signature->id()).getDistance(_optimizedPoses.at(nearestId)));
2462 
2463  // nearest pose must be close and not linked to current location
2464  if(!signature->hasLink(nearestId))
2465  {
2467  {
2468  std::map<int, Transform> filteredPath;
2469  int i=0;
2470  std::map<int, Transform>::iterator nearestIdIter = path.find(nearestId);
2471  // "_proximityMaxNeighbors-1" means that if _proximityMaxNeighbors=1,
2472  // only nearest node on the path is taken (no scan merging). Useful to find
2473  // proximity detection between only 2 nodes with 360x360 lidar scans.
2474  for(std::map<int, Transform>::iterator iter=nearestIdIter; iter!=path.end() && i<=_proximityMaxNeighbors-1; ++iter, ++i)
2475  {
2476  filteredPath.insert(*iter);
2477  }
2478  i=1;
2479  for(std::map<int, Transform>::reverse_iterator iter(nearestIdIter); iter!=path.rend() && i<=_proximityMaxNeighbors-1; ++iter, ++i)
2480  {
2481  filteredPath.insert(*iter);
2482  }
2483  path = filteredPath;
2484  }
2485 
2486  // Assemble scans in the path and do ICP only
2487  std::map<int, Transform> optimizedLocalPath;
2489  {
2490  //optimize the path's poses locally
2491  cv::Mat covariance;
2492  path = optimizeGraph(nearestId, uKeysSet(path), std::map<int, Transform>(), false, covariance);
2493  // transform local poses in optimized graph referential
2494  UASSERT(uContains(path, nearestId));
2495  Transform t = _optimizedPoses.at(nearestId) * path.at(nearestId).inverse();
2496 
2497  for(std::map<int, Transform>::iterator jter=path.lower_bound(1); jter!=path.end(); ++jter)
2498  {
2499  optimizedLocalPath.insert(std::make_pair(jter->first, t * jter->second));
2500  }
2501  }
2502  else
2503  {
2504  optimizedLocalPath = path;
2505  }
2506 
2507  std::map<int, Transform> filteredPath;
2508  if(optimizedLocalPath.size() > 2 && proximityFilteringRadius > 0.0f)
2509  {
2510  // path filtering
2511  filteredPath = graph::radiusPosesFiltering(optimizedLocalPath, proximityFilteringRadius, 0, true);
2512  // make sure the current pose is still here
2513  filteredPath.insert(*optimizedLocalPath.find(nearestId));
2514  }
2515  else
2516  {
2517  filteredPath = optimizedLocalPath;
2518  }
2519 
2520  if(filteredPath.size() > 0)
2521  {
2522  // add current node to poses
2523  filteredPath.insert(std::make_pair(signature->id(), _optimizedPoses.at(signature->id())));
2524  //The nearest will be the reference for a loop closure transform
2525  if(signature->getLinks().find(nearestId) == signature->getLinks().end())
2526  {
2527  ++localScanPathsChecked;
2528  RegistrationInfo info;
2529  Transform transform = _memory->computeIcpTransformMulti(signature->id(), nearestId, filteredPath, &info);
2530  if(!transform.isNull())
2531  {
2532  UINFO("[Scan matching] Add local loop closure in SPACE (%d->%d) %s",
2533  signature->id(),
2534  nearestId,
2535  transform.prettyPrint().c_str());
2536 
2537  cv::Mat scanMatchingIds;
2539  {
2540  std::stringstream stream;
2541  stream << "SCANS:";
2542  for(std::map<int, Transform>::iterator iter=optimizedLocalPath.begin(); iter!=optimizedLocalPath.end(); ++iter)
2543  {
2544  if(iter != optimizedLocalPath.begin())
2545  {
2546  stream << ";";
2547  }
2548  stream << uNumber2Str(iter->first);
2549  }
2550  std::string scansStr = stream.str();
2551  scanMatchingIds = cv::Mat(1, int(scansStr.size()+1), CV_8SC1, (void *)scansStr.c_str());
2552  scanMatchingIds = compressData2(scanMatchingIds); // compressed
2553  }
2554 
2555  // set Identify covariance for laser scan matching only
2556  UASSERT(info.covariance.at<double>(0,0) > 0.0 && info.covariance.at<double>(5,5) > 0.0);
2557  _memory->addLink(Link(signature->id(), nearestId, Link::kLocalSpaceClosure, transform, getInformation(info.covariance)/100.0, scanMatchingIds));
2558  loopClosureLinksAdded.push_back(std::make_pair(signature->id(), nearestId));
2559 
2560  ++proximityDetectionsAddedByICPOnly;
2561 
2562  // no local loop closure added visually
2563  if(proximityDetectionsAddedVisually == 0)
2564  {
2565  lastProximitySpaceClosureId = nearestId;
2566  }
2567  }
2568  else
2569  {
2570  UINFO("Local scan matching rejected: %s", info.rejectedMsg.c_str());
2571  }
2572  }
2573  }
2574  }
2575  else
2576  {
2577  //UDEBUG("Path %d ignored", nearestId);
2578  }
2579  }
2580  }
2581  }
2582  }
2583  }
2584  timeProximityBySpaceDetection = timer.ticks();
2585  ULOGGER_INFO("timeProximityBySpaceDetection=%fs", timeProximityBySpaceDetection);
2586 
2587  //=============================================================
2588  // Global loop closure detection
2589  // (updated: place this after retrieval to be sure that neighbors of the loop closure are in RAM)
2590  //=============================================================
2591  if(_loopClosureHypothesis.first>0)
2592  {
2593  //Compute transform if metric data are present
2594  Transform transform;
2595  RegistrationInfo info;
2596  info.covariance = cv::Mat::eye(6,6,CV_64FC1);
2597  if(_rgbdSlamMode)
2598  {
2599  transform = _memory->computeTransform(_loopClosureHypothesis.first, signature->id(), Transform(), &info);
2600  loopClosureVisualInliersMeanDist = info.inliersMeanDistance;
2601  loopClosureVisualInliersDistribution = info.inliersDistribution;
2602 
2603  loopClosureVisualInliers = info.inliers;
2604  loopClosureVisualInliersRatio = info.inliersRatio;
2605  loopClosureVisualMatches = info.matches;
2606  rejectedGlobalLoopClosure = transform.isNull();
2607  if(rejectedGlobalLoopClosure)
2608  {
2609  UWARN("Rejected loop closure %d -> %d: %s",
2610  _loopClosureHypothesis.first, signature->id(), info.rejectedMsg.c_str());
2611  }
2612  else if(_maxLoopClosureDistance>0.0f && transform.getNorm() > _maxLoopClosureDistance)
2613  {
2614  rejectedGlobalLoopClosure = true;
2615  UWARN("Rejected localization %d -> %d because distance to map (%fm) is over %s=%fm.",
2616  _loopClosureHypothesis.first, signature->id(), transform.getNorm(), Parameters::kRGBDMaxLoopClosureDistance().c_str(), _maxLoopClosureDistance);
2617  }
2618  else
2619  {
2620  transform = transform.inverse();
2621  }
2622  }
2623  if(!rejectedGlobalLoopClosure)
2624  {
2625  // Make the new one the parent of the old one
2626  UASSERT(info.covariance.at<double>(0,0) > 0.0 && info.covariance.at<double>(5,5) > 0.0);
2627  cv::Mat information = getInformation(info.covariance);
2628  loopClosureLinearVariance = 1.0/information.at<double>(0,0);
2629  loopClosureAngularVariance = 1.0/information.at<double>(5,5);
2630  rejectedGlobalLoopClosure = !_memory->addLink(Link(signature->id(), _loopClosureHypothesis.first, Link::kGlobalClosure, transform, information));
2631  if(!rejectedGlobalLoopClosure)
2632  {
2633  loopClosureLinksAdded.push_back(std::make_pair(signature->id(), _loopClosureHypothesis.first));
2634  }
2635  }
2636 
2637  if(rejectedGlobalLoopClosure)
2638  {
2639  _loopClosureHypothesis.first = 0;
2640  }
2641  }
2642 
2643  timeAddLoopClosureLink = timer.ticks();
2644  ULOGGER_INFO("timeAddLoopClosureLink=%fs", timeAddLoopClosureLink);
2645 
2646  //============================================================
2647  // Add virtual links if a path is activated
2648  //============================================================
2649  if(_path.size())
2650  {
2651  // Add a virtual loop closure link to keep the path linked to local map
2652  if( signature->id() != _path[_pathCurrentIndex].first &&
2653  !signature->hasLink(_path[_pathCurrentIndex].first))
2654  {
2655  UASSERT(uContains(_optimizedPoses, signature->id()));
2657  Transform virtualLoop = _optimizedPoses.at(signature->id()).inverse() * _optimizedPoses.at(_path[_pathCurrentIndex].first);
2658 
2659  if(_localRadius == 0.0f || virtualLoop.getNorm() < _localRadius)
2660  {
2661  _memory->addLink(Link(signature->id(), _path[_pathCurrentIndex].first, Link::kVirtualClosure, virtualLoop, cv::Mat::eye(6,6,CV_64FC1)*0.01)); // set high variance
2662  }
2663  else
2664  {
2665  UERROR("Virtual link larger than local radius (%fm > %fm). Aborting the plan!",
2666  virtualLoop.getNorm(), _localRadius);
2667  this->clearPath(-1);
2668  }
2669  }
2670  }
2671 
2672  //============================================================
2673  // Optimize map graph
2674  //============================================================
2675  float maxLinearError = 0.0f;
2676  float maxLinearErrorRatio = 0.0f;
2677  float maxAngularError = 0.0f;
2678  float maxAngularErrorRatio = 0.0f;
2679  double optimizationError = 0.0;
2680  int optimizationIterations = 0;
2681  cv::Mat localizationCovariance;
2682  Transform previousMapCorrection;
2683  if(_rgbdSlamMode
2684  &&
2685  (_loopClosureHypothesis.first>0 ||
2686  lastProximitySpaceClosureId>0 || // can be different map of the current one
2687  statistics_.reducedIds().size() ||
2688  (signature->hasLink(signature->id(), Link::kPosePrior) && !_graphOptimizer->priorsIgnored()) || // prior edge
2689  (signature->hasLink(signature->id(), Link::kGravity) && _graphOptimizer->gravitySigma()>0.0f && !_memory->isOdomGravityUsed()) || // gravity edge
2690  proximityDetectionsInTimeFound>0 ||
2691  landmarkDetected!=0 ||
2692  signaturesRetrieved.size()) // can be different map of the current one
2693  &&
2694  (_memory->isIncremental() ||
2695  // In localization mode, the new node should be linked to another node or a landmark already in the working memory
2696  graph::filterLinks(signature->getLinks(), Link::kSelfRefLink).size() ||
2697  landmarkDetected!=0))
2698  {
2699  UASSERT(uContains(_optimizedPoses, signature->id()));
2700 
2701  //used in localization mode: filter virtual links
2702  std::multimap<int, Link> localizationLinks = graph::filterLinks(signature->getLinks(), Link::kVirtualClosure);
2703  localizationLinks = graph::filterLinks(localizationLinks, Link::kSelfRefLink);
2704  if(landmarkDetected!=0 && !_memory->isIncremental() && _optimizedPoses.find(landmarkDetected)!=_optimizedPoses.end())
2705  {
2706  UASSERT(uContains(signature->getLandmarks(), landmarkDetected));
2707  localizationLinks.insert(std::make_pair(landmarkDetected, signature->getLandmarks().at(landmarkDetected)));
2708  }
2709 
2710  // Note that in localization mode, we don't re-optimize the graph
2711  // if:
2712  // 1- there are no signatures retrieved,
2713  // 2- we are relocalizing on a node already in the optimized graph
2714  if(!_memory->isIncremental() &&
2715  signaturesRetrieved.size() == 0 &&
2716  localizationLinks.size() &&
2717  uContains(_optimizedPoses, localizationLinks.begin()->first))
2718  {
2719  bool rejectLocalization = false;
2720  if(!_odomCachePoses.empty() && _optimizationMaxError > 0.0f)
2721  {
2722  // Verify if the new localization is valid by checking if there is
2723  // not too much deformation in odometry poses since previous localization
2724  std::map<int, Transform>::iterator iter = _optimizedPoses.find(_odomCacheConstraints.find(_odomCachePoses.begin()->first)->second.to());
2725  Transform optPoseRefA;
2726  Transform optPoseRefB;
2727  if(iter != _optimizedPoses.end())
2728  {
2729  optPoseRefA = iter->second * _odomCacheConstraints.find(_odomCachePoses.begin()->first)->second.transform().inverse();
2730  }
2731  iter = _optimizedPoses.find(localizationLinks.begin()->first);
2732  if(iter != _optimizedPoses.end())
2733  {
2734  optPoseRefB = iter->second * localizationLinks.begin()->second.transform().inverse();
2735  }
2736  if(optPoseRefA.isNull() || optPoseRefB.isNull())
2737  {
2738  UWARN("Both optimized pose references are null! Flushing cached odometry poses. Localization won't be verified.");
2739  _odomCachePoses.clear();
2740  _constraints.clear();
2741  }
2742  else
2743  {
2744  std::multimap<int, Link> constraints = _odomCacheConstraints;
2745  constraints.erase(_odomCachePoses.begin()->first);
2746  constraints.insert(std::make_pair(_odomCachePoses.begin()->first,
2747  Link(_odomCachePoses.begin()->first, signature->id(), Link::kVirtualClosure,
2748  optPoseRefA.inverse() * optPoseRefB, cv::Mat::eye(6,6,CV_64FC1)*100)));
2749 
2750  std::map<int, Transform> optPoses = _graphOptimizer->optimize(signature->id(), _odomCachePoses, constraints);
2751 
2752  if(optPoses.empty())
2753  {
2754  UWARN("Optimization failed, rejecting localization!");
2755  rejectLocalization = true;
2756  }
2757  else
2758  {
2759  UINFO("Compute max graph errors...");
2760  const Link * maxLinearLink = 0;
2761  const Link * maxAngularLink = 0;
2763  optPoses,
2764  constraints,
2765  maxLinearErrorRatio,
2766  maxAngularErrorRatio,
2767  maxLinearError,
2768  maxAngularError,
2769  &maxLinearLink,
2770  &maxAngularLink);
2771  if(maxLinearLink == 0 && maxAngularLink==0)
2772  {
2773  UWARN("Could not compute graph errors! Wrong loop closures could be accepted!");
2774  }
2775 
2776  if(maxLinearLink)
2777  {
2778  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()));
2779  if(maxLinearErrorRatio > _optimizationMaxError)
2780  {
2781  UWARN("Rejecting localization (%d <-> %d) in this "
2782  "iteration because a wrong loop closure has been "
2783  "detected after graph optimization, resulting in "
2784  "a maximum graph error ratio of %f (edge %d->%d, type=%d, abs error=%f m, stddev=%f). The "
2785  "maximum error ratio parameter \"%s\" is %f of std deviation.",
2786  loopClosureLinksAdded.front().first,
2787  loopClosureLinksAdded.front().second,
2788  maxLinearErrorRatio,
2789  maxLinearLink->from(),
2790  maxLinearLink->to(),
2791  maxLinearLink->type(),
2792  maxLinearError,
2793  sqrt(maxLinearLink->transVariance()),
2794  Parameters::kRGBDOptimizeMaxError().c_str(),
2796  rejectLocalization = true;
2797  }
2798  }
2799  if(maxAngularLink)
2800  {
2801  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()));
2802  if(maxAngularErrorRatio > _optimizationMaxError)
2803  {
2804  UWARN("Rejecting localization (%d <-> %d) in this "
2805  "iteration because a wrong loop closure has been "
2806  "detected after graph optimization, resulting in "
2807  "a maximum graph error ratio of %f (edge %d->%d, type=%d, abs error=%f deg, stddev=%f). The "
2808  "maximum error ratio parameter \"%s\" is %f of std deviation.",
2809  loopClosureLinksAdded.front().first,
2810  loopClosureLinksAdded.front().second,
2811  maxAngularErrorRatio,
2812  maxAngularLink->from(),
2813  maxAngularLink->to(),
2814  maxAngularLink->type(),
2815  maxAngularError*180.0f/CV_PI,
2816  sqrt(maxAngularLink->rotVariance()),
2817  Parameters::kRGBDOptimizeMaxError().c_str(),
2819  rejectLocalization = true;
2820  }
2821  }
2822  }
2823  }
2824  }
2825 
2826  if(!rejectLocalization)
2827  {
2828  // If there are no signatures retrieved, we don't
2829  // need to re-optimize the graph. Just update the last
2830  // position if OptimizeFromGraphEnd=false or transform the
2831  // whole graph if OptimizeFromGraphEnd=true
2832  UINFO("Localization without map optimization");
2834  {
2835  // update all previous nodes
2836  // Normally _mapCorrection should be identity, but if _optimizeFromGraphEnd
2837  // parameters just changed state, we should put back all poses without map correction.
2838  Transform oldPose = _optimizedPoses.at(localizationLinks.begin()->first);
2839  Transform mapCorrectionInv = _mapCorrection.inverse();
2840  Transform u = signature->getPose() * localizationLinks.begin()->second.transform();
2841  if(_graphOptimizer->isSlam2d())
2842  {
2843  // in case of 3d landmarks, transform constraint to 2D
2844  u = u.to3DoF();
2845  }
2846  else if(_graphOptimizer->gravitySigma() > 0)
2847  {
2848  // Adjust transform with gravity
2849  Transform transform = localizationLinks.begin()->second.transform();
2850  int loopId = localizationLinks.begin()->first;
2851  if(loopId < 0)
2852  {
2853  //For landmarks, use transform against other node looking the landmark
2854  // (because we don't assume that landmarks are aligned with gravity)
2855  int landmarkId = loopId;
2856  UASSERT(!landmarkDetectedNodesRef.empty());
2857  loopId = *landmarkDetectedNodesRef.begin();
2858  const Signature * loopS = _memory->getSignature(loopId);
2859  transform = transform * loopS->getLandmarks().at(landmarkId).transform().inverse();
2860  UASSERT(_optimizedPoses.find(loopId) != _optimizedPoses.end());
2861  oldPose = _optimizedPoses.at(loopId);
2862  }
2863 
2864  const Signature * loopS = _memory->getSignature(loopId);
2865  UASSERT(loopS !=0);
2866  std::multimap<int, Link>::const_iterator iterGravityLoop = graph::findLink(loopS->getLinks(), loopS->id(), loopS->id(), false, Link::kGravity);
2867  std::multimap<int, Link>::const_iterator iterGravitySign = graph::findLink(signature->getLinks(), signature->id(), signature->id(), false, Link::kGravity);
2868  if(iterGravityLoop!=loopS->getLinks().end() &&
2869  iterGravitySign!=signature->getLinks().end())
2870  {
2871  float roll,pitch,yaw;
2872  iterGravityLoop->second.transform().getEulerAngles(roll, pitch, yaw);
2873  Transform targetRotation = iterGravitySign->second.transform().rotation()*transform.rotation();
2874  targetRotation = Transform(0,0,0,roll,pitch,targetRotation.theta());
2875  Transform error = transform.rotation().inverse() * iterGravitySign->second.transform().rotation().inverse() * targetRotation;
2876  transform *= error;
2877 
2878  u = signature->getPose() * transform;
2879  }
2880  else
2881  {
2882  UWARN("Gravity link not found for %d and/or %d, localization won't be corrected with gravity.", loopId, signature->id());
2883  }
2884  }
2885  Transform up = u * oldPose.inverse();
2886  for(std::map<int, Transform>::iterator iter=_optimizedPoses.begin(); iter!=_optimizedPoses.end(); ++iter)
2887  {
2888  iter->second = mapCorrectionInv * up * iter->second;
2889  }
2890  _optimizedPoses.at(signature->id()) = signature->getPose();
2891  }
2892  else
2893  {
2894  Transform newPose = _optimizedPoses.at(localizationLinks.begin()->first) * localizationLinks.begin()->second.transform().inverse();
2895  UDEBUG("newPose=%s", newPose.prettyPrint().c_str());
2896  if(_graphOptimizer->isSlam2d())
2897  {
2898  // in case of 3d landmarks, transform constraint to 2D
2899  newPose = newPose.to3DoF();
2900  UDEBUG("newPose 2D=%s", newPose.prettyPrint().c_str());
2901  }
2902  else if(_graphOptimizer->gravitySigma() > 0)
2903  {
2904  // Adjust transform with gravity
2905  Transform transform = localizationLinks.begin()->second.transform();
2906  int loopId = localizationLinks.begin()->first;
2907  if(loopId < 0)
2908  {
2909  //For landmarks, use transform against other node looking the landmark
2910  // (because we don't assume that landmarks are aligned with gravity)
2911  int landmarkId = loopId;
2912  UASSERT(!landmarkDetectedNodesRef.empty());
2913  loopId = *landmarkDetectedNodesRef.begin();
2914  const Signature * loopS = _memory->getSignature(loopId);
2915  transform = transform * loopS->getLandmarks().at(landmarkId).transform().inverse();
2916  }
2917 
2918  const Signature * loopS = _memory->getSignature(loopId);
2919  UASSERT(loopS !=0);
2920  std::multimap<int, Link>::const_iterator iterGravityLoop = graph::findLink(loopS->getLinks(), loopS->id(), loopS->id(), false, Link::kGravity);
2921  std::multimap<int, Link>::const_iterator iterGravitySign = graph::findLink(signature->getLinks(), signature->id(), signature->id(), false, Link::kGravity);
2922  if(iterGravityLoop!=loopS->getLinks().end() &&
2923  iterGravitySign!=signature->getLinks().end())
2924  {
2925  float roll,pitch,yaw;
2926  iterGravityLoop->second.transform().getEulerAngles(roll, pitch, yaw);
2927  Transform targetRotation = iterGravitySign->second.transform().rotation()*transform.rotation();
2928  targetRotation = Transform(0,0,0,roll,pitch,targetRotation.theta());
2929  Transform error = transform.rotation().inverse() * iterGravitySign->second.transform().rotation().inverse() * targetRotation;
2930  transform *= error;
2931 
2932  newPose = _optimizedPoses.at(loopId) * transform.inverse();
2933  UDEBUG("newPose gravity=%s", newPose.prettyPrint().c_str());
2934  }
2935  else
2936  {
2937  UWARN("Gravity link not found for %d and/or %d, localization won't be corrected with gravity.", loopId, signature->id());
2938  }
2939  }
2940  _optimizedPoses.at(signature->id()) = newPose;
2941  }
2942  localizationCovariance = localizationLinks.begin()->second.infMatrix().inv();
2943 
2944  _odomCachePoses.clear();
2945  _odomCacheConstraints.clear();
2946  if(_optimizationMaxError > 0.0f && _maxOdomCacheSize > 0)
2947  {
2948  _odomCachePoses.insert(std::make_pair(signature->id(), signature->getPose()));
2949  _odomCacheConstraints.insert(std::make_pair(signature->id(), localizationLinks.begin()->second));
2950  }
2951  }
2952  else
2953  {
2954  _loopClosureHypothesis.first = 0;
2955  lastProximitySpaceClosureId = 0;
2956  rejectedGlobalLoopClosure = true;
2957  rejectedLandmark = true;
2958  }
2959  }
2960  else
2961  {
2962  UINFO("Update map correction");
2963  std::map<int, Transform> poses = _optimizedPoses;
2964 
2965  // if _optimizeFromGraphEnd parameter just changed state, don't use optimized poses as guess
2967  {
2968  UWARN("Optimization: clearing guess poses as %s has changed state, now %s",
2969  Parameters::kRGBDOptimizeFromGraphEnd().c_str(), _optimizeFromGraphEnd?"true":"false");
2970  poses.clear();
2972  }
2973 
2974  std::multimap<int, Link> constraints;
2975  cv::Mat covariance;
2976  optimizeCurrentMap(signature->id(), false, poses, covariance, &constraints, &optimizationError, &optimizationIterations);
2977 
2978  // Check added loop closures have broken the graph
2979  // (in case of wrong loop closures).
2980  bool updateConstraints = true;
2981  if(poses.empty())
2982  {
2983  UWARN("Graph optimization failed! Rejecting last loop closures added.");
2984  for(std::list<std::pair<int, int> >::iterator iter=loopClosureLinksAdded.begin(); iter!=loopClosureLinksAdded.end(); ++iter)
2985  {
2986  _memory->removeLink(iter->first, iter->second);
2987  UWARN("Loop closure %d->%d rejected!", iter->first, iter->second);
2988  }
2989  updateConstraints = false;
2990  _loopClosureHypothesis.first = 0;
2991  lastProximitySpaceClosureId = 0;
2992  rejectedGlobalLoopClosure = true;
2993  rejectedLandmark = true;
2994  }
2995  else if(_memory->isIncremental() &&
2996  _optimizationMaxError > 0.0f &&
2997  loopClosureLinksAdded.size() &&
2998  optimizationIterations > 0 &&
2999  constraints.size())
3000  {
3001  UINFO("Compute max graph errors...");
3002  const Link * maxLinearLink = 0;
3003  const Link * maxAngularLink = 0;
3005  poses,
3006  constraints,
3007  maxLinearErrorRatio,
3008  maxAngularErrorRatio,
3009  maxLinearError,
3010  maxAngularError,
3011  &maxLinearLink,
3012  &maxAngularLink);
3013  if(maxLinearLink == 0 && maxAngularLink==0)
3014  {
3015  UWARN("Could not compute graph errors! Wrong loop closures could be accepted!");
3016  }
3017 
3018  bool reject = false;
3019  if(maxLinearLink)
3020  {
3021  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()));
3022  if(maxLinearErrorRatio > _optimizationMaxError)
3023  {
3024  UWARN("Rejecting all added loop closures (%d, first is %d <-> %d) in this "
3025  "iteration because a wrong loop closure has been "
3026  "detected after graph optimization, resulting in "
3027  "a maximum graph error ratio of %f (edge %d->%d, type=%d, abs error=%f m, stddev=%f). The "
3028  "maximum error ratio parameter \"%s\" is %f of std deviation.",
3029  (int)loopClosureLinksAdded.size(),
3030  loopClosureLinksAdded.front().first,
3031  loopClosureLinksAdded.front().second,
3032  maxLinearErrorRatio,
3033  maxLinearLink->from(),
3034  maxLinearLink->to(),
3035  maxLinearLink->type(),
3036  maxLinearError,
3037  sqrt(maxLinearLink->transVariance()),
3038  Parameters::kRGBDOptimizeMaxError().c_str(),
3040  reject = true;
3041  }
3042  }
3043  if(maxAngularLink)
3044  {
3045  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()));
3046  if(maxAngularErrorRatio > _optimizationMaxError)
3047  {
3048  UWARN("Rejecting all added loop closures (%d, first is %d <-> %d) in this "
3049  "iteration because a wrong loop closure has been "
3050  "detected after graph optimization, resulting in "
3051  "a maximum graph error ratio of %f (edge %d->%d, type=%d, abs error=%f deg, stddev=%f). The "
3052  "maximum error ratio parameter \"%s\" is %f of std deviation.",
3053  (int)loopClosureLinksAdded.size(),
3054  loopClosureLinksAdded.front().first,
3055  loopClosureLinksAdded.front().second,
3056  maxAngularErrorRatio,
3057  maxAngularLink->from(),
3058  maxAngularLink->to(),
3059  maxAngularLink->type(),
3060  maxAngularError*180.0f/CV_PI,
3061  sqrt(maxAngularLink->rotVariance()),
3062  Parameters::kRGBDOptimizeMaxError().c_str(),
3064  reject = true;
3065  }
3066  }
3067 
3068  if(reject)
3069  {
3070  for(std::list<std::pair<int, int> >::iterator iter=loopClosureLinksAdded.begin(); iter!=loopClosureLinksAdded.end(); ++iter)
3071  {
3072  _memory->removeLink(iter->first, iter->second);
3073  UWARN("Loop closure %d->%d rejected!", iter->first, iter->second);
3074  }
3075  updateConstraints = false;
3076  _loopClosureHypothesis.first = 0;
3077  lastProximitySpaceClosureId = 0;
3078  rejectedGlobalLoopClosure = true;
3079  rejectedLandmark = true;
3080  }
3081  }
3082 
3083  if(updateConstraints)
3084  {
3085  UINFO("Updated local map (old size=%d, new size=%d)", (int)_optimizedPoses.size(), (int)poses.size());
3086  _optimizedPoses = poses;
3087  _constraints = constraints;
3088  localizationCovariance = covariance;
3089  }
3090  }
3091 
3092  // Update map correction, it should be identify when optimizing from the last node
3093  UASSERT(_optimizedPoses.find(signature->id()) != _optimizedPoses.end());
3094  if(fakeOdom && _mapCorrectionBackup.isNull())
3095  {
3097  }
3098  previousMapCorrection = _mapCorrection;
3099  _mapCorrection = _optimizedPoses.at(signature->id()) * signature->getPose().inverse();
3100  _lastLocalizationPose = _optimizedPoses.at(signature->id()); // update
3102  {
3103  bool hasPrior = signature->hasLink(signature->id());
3105  {
3106  for(std::multimap<int, Link>::reverse_iterator iter=_constraints.rbegin(); !hasPrior && iter!=_constraints.rend(); ++iter)
3107  {
3108  if(iter->second.type() == Link::kPosePrior)
3109  {
3110  hasPrior = true;
3111  }
3112  }
3113  }
3114  if((!hasPrior || _graphOptimizer->priorsIgnored()) && _graphOptimizer->gravitySigma()==0.0f)
3115  {
3116  UERROR("Map correction should be identity when optimizing from the last node. T=%s", _mapCorrection.prettyPrint().c_str());
3117  }
3118  }
3119  }
3120  int newLocId = _loopClosureHypothesis.first>0?_loopClosureHypothesis.first:lastProximitySpaceClosureId>0?lastProximitySpaceClosureId:0;
3121  _lastLocalizationNodeId = newLocId!=0?newLocId:_lastLocalizationNodeId;
3122  if(newLocId==0 && landmarkDetected!=0)
3123  {
3124  std::map<int, std::set<int> >::const_iterator iter = _memory->getLandmarksInvertedIndex().find(landmarkDetected);
3125  if(iter!=_memory->getLandmarksInvertedIndex().end())
3126  {
3127  if(iter->second.size() && *iter->second.begin()!=signature->id())
3128  {
3129  _lastLocalizationNodeId = *iter->second.begin();
3130  }
3131  }
3132  }
3133 
3134  timeMapOptimization = timer.ticks();
3135  ULOGGER_INFO("timeMapOptimization=%fs", timeMapOptimization);
3136 
3137  //============================================================
3138  // Prepare statistics
3139  //============================================================
3140  // Data used for the statistics event and for the log files
3141  int dictionarySize = 0;
3142  int refWordsCount = 0;
3143  int refUniqueWordsCount = 0;
3144  int lcHypothesisReactivated = 0;
3145  float rehearsalValue = uValue(statistics_.data(), Statistics::kMemoryRehearsal_sim(), 0.0f);
3146  int rehearsalMaxId = (int)uValue(statistics_.data(), Statistics::kMemoryRehearsal_merged(), 0.0f);
3147  sLoop = _memory->getSignature(_loopClosureHypothesis.first?_loopClosureHypothesis.first:lastProximitySpaceClosureId?lastProximitySpaceClosureId:_highestHypothesis.first);
3148  if(sLoop)
3149  {
3150  lcHypothesisReactivated = sLoop->isSaved()?1.0f:0.0f;
3151  }
3152  dictionarySize = (int)_memory->getVWDictionary()->getVisualWords().size();
3153  refWordsCount = (int)signature->getWords().size();
3154  refUniqueWordsCount = (int)uUniqueKeys(signature->getWords()).size();
3155 
3156  // Posterior is empty if a bad signature is detected
3157  float vpHypothesis = posterior.size()?posterior.at(Memory::kIdVirtual):0.0f;
3158 
3159  // prepare statistics
3161  {
3162  ULOGGER_INFO("sending stats...");
3163  statistics_.setRefImageId(_memory->getLastSignatureId()); // Use last id from Memory (in case of rehearsal)
3164  statistics_.setRefImageMapId(signature->mapId());
3165  statistics_.setStamp(data.stamp());
3167  {
3170  ULOGGER_INFO("Loop closure detected! With id=%d", _loopClosureHypothesis.first);
3171  }
3172  if(_publishStats)
3173  {
3174  ULOGGER_INFO("send all stats...");
3176 
3177  statistics_.addStatistic(Statistics::kLoopAccepted_hypothesis_id(), _loopClosureHypothesis.first);
3178  statistics_.addStatistic(Statistics::kLoopSuppressed_hypothesis_id(), loopIdSuppressedByProximity);
3179  statistics_.addStatistic(Statistics::kLoopHighest_hypothesis_id(), _highestHypothesis.first);
3180  statistics_.addStatistic(Statistics::kLoopHighest_hypothesis_value(), _highestHypothesis.second);
3181  statistics_.addStatistic(Statistics::kLoopHypothesis_reactivated(), lcHypothesisReactivated);
3182  statistics_.addStatistic(Statistics::kLoopVp_hypothesis(), vpHypothesis);
3183  statistics_.addStatistic(Statistics::kLoopReactivate_id(), retrievalId);
3184  statistics_.addStatistic(Statistics::kLoopHypothesis_ratio(), hypothesisRatio);
3185  statistics_.addStatistic(Statistics::kLoopVisual_inliers(), loopClosureVisualInliers);
3186  statistics_.addStatistic(Statistics::kLoopVisual_inliers_ratio(), loopClosureVisualInliersRatio);
3187  statistics_.addStatistic(Statistics::kLoopVisual_matches(), loopClosureVisualMatches);
3188  statistics_.addStatistic(Statistics::kLoopLinear_variance(), loopClosureLinearVariance);
3189  statistics_.addStatistic(Statistics::kLoopAngular_variance(), loopClosureAngularVariance);
3190  statistics_.addStatistic(Statistics::kLoopLast_id(), _memory->getLastGlobalLoopClosureId());
3191  statistics_.addStatistic(Statistics::kLoopOptimization_max_error(), maxLinearError);
3192  statistics_.addStatistic(Statistics::kLoopOptimization_max_error_ratio(), maxLinearErrorRatio);
3193  statistics_.addStatistic(Statistics::kLoopOptimization_error(), optimizationError);
3194  statistics_.addStatistic(Statistics::kLoopOptimization_iterations(), optimizationIterations);
3195  statistics_.addStatistic(Statistics::kLoopLandmark_detected(), -landmarkDetected);
3196  statistics_.addStatistic(Statistics::kLoopLandmark_detected_node_ref(), landmarkDetectedNodesRef.empty()?0:*landmarkDetectedNodesRef.begin());
3197  statistics_.addStatistic(Statistics::kLoopVisual_inliers_mean_dist(), loopClosureVisualInliersMeanDist);
3198  statistics_.addStatistic(Statistics::kLoopVisual_inliers_distribution(), loopClosureVisualInliersDistribution);
3199 
3200  statistics_.addStatistic(Statistics::kProximityTime_detections(), proximityDetectionsInTimeFound);
3201  statistics_.addStatistic(Statistics::kProximitySpace_detections_added_visually(), proximityDetectionsAddedVisually);
3202  statistics_.addStatistic(Statistics::kProximitySpace_detections_added_icp_only(), proximityDetectionsAddedByICPOnly);
3203  statistics_.addStatistic(Statistics::kProximitySpace_paths(), proximitySpacePaths);
3204  statistics_.addStatistic(Statistics::kProximitySpace_visual_paths_checked(), localVisualPathsChecked);
3205  statistics_.addStatistic(Statistics::kProximitySpace_scan_paths_checked(), localScanPathsChecked);
3206  statistics_.addStatistic(Statistics::kProximitySpace_last_detection_id(), lastProximitySpaceClosureId);
3207  statistics_.setProximityDetectionId(lastProximitySpaceClosureId);
3208  statistics_.setProximityDetectionMapId(_memory->getMapId(lastProximitySpaceClosureId));
3209 
3210  int loopId = _loopClosureHypothesis.first>0?_loopClosureHypothesis.first:lastProximitySpaceClosureId;
3211  statistics_.addStatistic(Statistics::kLoopId(), loopId);
3212  statistics_.addStatistic(Statistics::kLoopMap_id(), (loopId>0 && sLoop)?sLoop->mapId():-1);
3213 
3214  float x,y,z,roll,pitch,yaw;
3215  if(_loopClosureHypothesis.first || lastProximitySpaceClosureId)
3216  {
3217  // Loop closure transform
3218  UASSERT(sLoop);
3219  std::multimap<int, Link>::const_iterator loopIter = sLoop->getLinks().find(signature->id());
3220  UASSERT(loopIter!=sLoop->getLinks().end());
3221  UINFO("Set loop closure transform = %s", loopIter->second.transform().prettyPrint().c_str());
3222  statistics_.setLoopClosureTransform(loopIter->second.transform());
3223 
3224  statistics_.addStatistic(Statistics::kLoopVisual_words(), sLoop->getWords().size());
3225  statistics_.addStatistic(Statistics::kLoopDistance_since_last_loc(), _distanceTravelledSinceLastLocalization);
3227 
3228  // if ground truth exists, compute localization error
3229  if(!sLoop->getGroundTruthPose().isNull() && !signature->getGroundTruthPose().isNull())
3230  {
3231  Transform transformGT = sLoop->getGroundTruthPose().inverse() * signature->getGroundTruthPose();
3232  Transform error = loopIter->second.transform().inverse() * transformGT;
3233  statistics_.addStatistic(Statistics::kGtLocalization_linear_error(), error.getNorm());
3234  statistics_.addStatistic(Statistics::kGtLocalization_angular_error(), error.getAngle(1,0,0)*180/M_PI);
3235  }
3236 
3237  statistics_.addStatistic(Statistics::kLoopMapToOdom_norm(), _mapCorrection.getNorm());
3238  statistics_.addStatistic(Statistics::kLoopMapToOdom_angle(), _mapCorrection.getAngle()*180.0f/M_PI);
3239  _mapCorrection.getTranslationAndEulerAngles(x, y, z, roll, pitch, yaw);
3240  statistics_.addStatistic(Statistics::kLoopMapToOdom_x(), x);
3241  statistics_.addStatistic(Statistics::kLoopMapToOdom_y(), y);
3242  statistics_.addStatistic(Statistics::kLoopMapToOdom_z(), z);
3243  statistics_.addStatistic(Statistics::kLoopMapToOdom_roll(), roll*180.0f/M_PI);
3244  statistics_.addStatistic(Statistics::kLoopMapToOdom_pitch(), pitch*180.0f/M_PI);
3245  statistics_.addStatistic(Statistics::kLoopMapToOdom_yaw(), yaw*180.0f/M_PI);
3246 
3247  // Odom correction (actual odometry pose change), ignore correction from first localization
3248  if(!odomPose.isNull() && !previousMapCorrection.isNull() && !previousMapCorrection.isIdentity())
3249  {
3250  Transform odomCorrection = (previousMapCorrection*odomPose).inverse()*_mapCorrection*odomPose;
3251  statistics_.addStatistic(Statistics::kLoopOdom_correction_norm(), odomCorrection.getNorm());
3252  statistics_.addStatistic(Statistics::kLoopOdom_correction_angle(), odomCorrection.getAngle()*180.0f/M_PI);
3253  odomCorrection.getTranslationAndEulerAngles(x, y, z, roll, pitch, yaw);
3254  statistics_.addStatistic(Statistics::kLoopOdom_correction_x(), x);
3255  statistics_.addStatistic(Statistics::kLoopOdom_correction_y(), y);
3256  statistics_.addStatistic(Statistics::kLoopOdom_correction_z(), z);
3257  statistics_.addStatistic(Statistics::kLoopOdom_correction_roll(), roll*180.0f/M_PI);
3258  statistics_.addStatistic(Statistics::kLoopOdom_correction_pitch(), pitch*180.0f/M_PI);
3259  statistics_.addStatistic(Statistics::kLoopOdom_correction_yaw(), yaw*180.0f/M_PI);
3260 
3261  _odomCorrectionAcc[0]+=x;
3262  _odomCorrectionAcc[1]+=y;
3263  _odomCorrectionAcc[2]+=z;
3266  _odomCorrectionAcc[5]+=yaw;
3267  Transform odomCorrectionAcc(
3268  _odomCorrectionAcc[0],
3269  _odomCorrectionAcc[1],
3270  _odomCorrectionAcc[2],
3271  _odomCorrectionAcc[3],
3272  _odomCorrectionAcc[4],
3273  _odomCorrectionAcc[5]);
3274 
3275  statistics_.addStatistic(Statistics::kLoopOdom_correction_acc_norm(), odomCorrectionAcc.getNorm());
3276  statistics_.addStatistic(Statistics::kLoopOdom_correction_acc_angle(), odomCorrectionAcc.getAngle()*180.0f/M_PI);
3277  odomCorrectionAcc.getTranslationAndEulerAngles(x, y, z, roll, pitch, yaw);
3278  statistics_.addStatistic(Statistics::kLoopOdom_correction_acc_x(), x);
3279  statistics_.addStatistic(Statistics::kLoopOdom_correction_acc_y(), y);
3280  statistics_.addStatistic(Statistics::kLoopOdom_correction_acc_z(), z);
3281  statistics_.addStatistic(Statistics::kLoopOdom_correction_acc_roll(), roll*180.0f/M_PI);
3282  statistics_.addStatistic(Statistics::kLoopOdom_correction_acc_pitch(), pitch*180.0f/M_PI);
3283  statistics_.addStatistic(Statistics::kLoopOdom_correction_acc_yaw(), yaw*180.0f/M_PI);
3284  }
3285  }
3287  {
3288  _lastLocalizationPose.getTranslationAndEulerAngles(x, y, z, roll, pitch, yaw);
3289  statistics_.addStatistic(Statistics::kLoopMapToBase_x(), x);
3290  statistics_.addStatistic(Statistics::kLoopMapToBase_y(), y);
3291  statistics_.addStatistic(Statistics::kLoopMapToBase_z(), z);
3292  statistics_.addStatistic(Statistics::kLoopMapToBase_roll(), roll*180.0f/M_PI);
3293  statistics_.addStatistic(Statistics::kLoopMapToBase_pitch(), pitch*180.0f/M_PI);
3294  statistics_.addStatistic(Statistics::kLoopMapToBase_yaw(), yaw*180.0f/M_PI);
3295  UINFO("Localization pose = %s", _lastLocalizationPose.prettyPrint().c_str());
3296  }
3297 
3299  UINFO("Set map correction = %s", _mapCorrection.prettyPrint().c_str());
3300  statistics_.setLocalizationCovariance(localizationCovariance);
3301 
3302  // timings...
3303  statistics_.addStatistic(Statistics::kTimingMemory_update(), timeMemoryUpdate*1000);
3304  statistics_.addStatistic(Statistics::kTimingNeighbor_link_refining(), timeNeighborLinkRefining*1000);
3305  statistics_.addStatistic(Statistics::kTimingProximity_by_time(), timeProximityByTimeDetection*1000);
3306  statistics_.addStatistic(Statistics::kTimingProximity_by_space_visual(), timeProximityBySpaceVisualDetection*1000);
3307  statistics_.addStatistic(Statistics::kTimingProximity_by_space(), timeProximityBySpaceDetection*1000);
3308  statistics_.addStatistic(Statistics::kTimingReactivation(), timeReactivations*1000);
3309  statistics_.addStatistic(Statistics::kTimingAdd_loop_closure_link(), timeAddLoopClosureLink*1000);
3310  statistics_.addStatistic(Statistics::kTimingMap_optimization(), timeMapOptimization*1000);
3311  statistics_.addStatistic(Statistics::kTimingLikelihood_computation(), timeLikelihoodCalculation*1000);
3312  statistics_.addStatistic(Statistics::kTimingPosterior_computation(), timePosteriorCalculation*1000);
3313  statistics_.addStatistic(Statistics::kTimingHypotheses_creation(), timeHypothesesCreation*1000);
3314  statistics_.addStatistic(Statistics::kTimingHypotheses_validation(), timeHypothesesValidation*1000);
3315  statistics_.addStatistic(Statistics::kTimingCleaning_neighbors(), timeCleaningNeighbors*1000);
3316 
3317  // retrieval
3318  statistics_.addStatistic(Statistics::kMemorySignatures_retrieved(), (float)signaturesRetrieved.size());
3319 
3320  // Feature specific parameters
3321  statistics_.addStatistic(Statistics::kKeypointDictionary_size(), dictionarySize);
3322  statistics_.addStatistic(Statistics::kKeypointCurrent_frame(), refWordsCount);
3323  statistics_.addStatistic(Statistics::kKeypointIndexed_words(), _memory->getVWDictionary()->getIndexedWordsCount());
3324  statistics_.addStatistic(Statistics::kKeypointIndex_memory_usage(), _memory->getVWDictionary()->getIndexMemoryUsed());
3325 
3326  //Epipolar geometry constraint
3327  statistics_.addStatistic(Statistics::kLoopRejectedHypothesis(), rejectedGlobalLoopClosure?1.0f:0);
3328 
3329  statistics_.addStatistic(Statistics::kMemorySmall_movement(), smallDisplacement?1.0f:0);
3330  statistics_.addStatistic(Statistics::kMemoryDistance_travelled(), _distanceTravelled);
3331  statistics_.addStatistic(Statistics::kMemoryFast_movement(), tooFastMovement?1.0f:0);
3332  if(_publishRAMUsage)
3333  {
3334  UTimer ramTimer;
3335  statistics_.addStatistic(Statistics::kMemoryRAM_usage(), UProcessInfo::getMemoryUsage()/(1024*1024));
3336  long estimatedMemoryUsage = sizeof(Rtabmap);
3337  estimatedMemoryUsage += _optimizedPoses.size() * (sizeof(int) + sizeof(Transform) + 12 * sizeof(float) + sizeof(std::map<int, Transform>::iterator)) + sizeof(std::map<int, Transform>);
3338  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>);
3339  estimatedMemoryUsage += _memory->getMemoryUsed();
3340  estimatedMemoryUsage += _bayesFilter->getMemoryUsed();
3341  estimatedMemoryUsage += _parameters.size()*(sizeof(std::string)*2+sizeof(ParametersMap::iterator)) + sizeof(ParametersMap);
3342  statistics_.addStatistic(Statistics::kMemoryRAM_estimated(), (float)(estimatedMemoryUsage/(1024*1024)));//MB
3343  statistics_.addStatistic(Statistics::kTimingRAM_estimation(), ramTimer.ticks()*1000);
3344  }
3345 
3347  {
3348  // Child count by parent signature on the root of the memory ... for statistics
3349  statistics_.setWeights(weights);
3350  if(_publishPdf)
3351  {
3352  statistics_.setPosterior(posterior);
3353  }
3354  if(_publishLikelihood)
3355  {
3356  statistics_.setLikelihood(likelihood);
3357  statistics_.setRawLikelihood(rawLikelihood);
3358  }
3359  }
3360 
3362 
3363  // Path
3364  if(_path.size())
3365  {
3368  }
3369  }
3370 
3371  timeStatsCreation = timer.ticks();
3372  ULOGGER_INFO("Time creating stats = %f...", timeStatsCreation);
3373  }
3374 
3375  Signature lastSignatureData(signature->id());
3376  Transform lastSignatureLocalizedPose;
3377  if(_optimizedPoses.find(signature->id()) != _optimizedPoses.end() &&
3378  graph::filterLinks(signature->getLinks(), Link::kSelfRefLink).size())
3379  {
3380  // only if localized set it
3381  lastSignatureLocalizedPose = _optimizedPoses.at(signature->id());
3382  }
3384  {
3385  lastSignatureData = *signature;
3386  }
3387  if(!_rawDataKept)
3388  {
3389  _memory->removeRawData(signature->id(), true, !_neighborLinkRefining && !_proximityBySpace, true);
3390  }
3391 
3392  // remove last signature if the memory is not incremental or is a bad signature (if bad signatures are ignored)
3393  int signatureRemoved = _memory->cleanup();
3394  if(signatureRemoved)
3395  {
3396  signaturesRemoved.push_back(signatureRemoved);
3397  }
3398 
3399  // If this option activated, add new nodes only if there are linked with a previous map.
3400  // Used when rtabmap is first started, it will wait a
3401  // global loop closure detection before starting the new map,
3402  // otherwise it deletes the current node.
3403  if(signatureRemoved != lastSignatureData.id())
3404  {
3406  _memory->isIncremental() && // only in mapping mode
3407  graph::filterLinks(signature->getLinks(), Link::kSelfRefLink).size() == 0 && // alone in the current map
3408  (landmarkDetected == 0 || rejectedLandmark) && // if we re not seeing a landmark from a previous map
3409  _memory->getWorkingMem().size()>=2) // The working memory should not be empty (beside virtual signature)
3410  {
3411  UWARN("Ignoring location %d because a global loop closure is required before starting a new map!",
3412  signature->id());
3413  signaturesRemoved.push_back(signature->id());
3414  _memory->deleteLocation(signature->id());
3415  }
3416  else if(_startNewMapOnGoodSignature &&
3417  (signature->getLandmarks().empty() && signature->isBadSignature()) &&
3418  graph::filterLinks(signature->getLinks(), Link::kSelfRefLink).size() == 0) // alone in the current map
3419  {
3420  UWARN("Ignoring location %d because a good signature (with enough features or with a landmark detected) is required before starting a new map!",
3421  signature->id());
3422  signaturesRemoved.push_back(signature->id());
3423  _memory->deleteLocation(signature->id());
3424  }
3425  else if((smallDisplacement || tooFastMovement) && _loopClosureHypothesis.first == 0 && lastProximitySpaceClosureId == 0)
3426  {
3427  // Don't delete the location if a loop closure is detected
3428  UINFO("Ignoring location %d because the displacement is too small! (d=%f a=%f)",
3429  signature->id(), _rgbdLinearUpdate, _rgbdAngularUpdate);
3430  // If there is a too small displacement, remove the node
3431  signaturesRemoved.push_back(signature->id());
3432  _memory->deleteLocation(signature->id());
3433  }
3434  else
3435  {
3436  _memory->saveLocationData(signature->id());
3437  }
3438  }
3439  else if(!_memory->isIncremental() &&
3440  (smallDisplacement || tooFastMovement) &&
3441  _loopClosureHypothesis.first == 0 &&
3442  lastProximitySpaceClosureId == 0)
3443  {
3444  _odomCachePoses.erase(signatureRemoved);
3445  _odomCacheConstraints.erase(signatureRemoved);
3446  }
3447 
3448  // Pass this point signature should not be used, since it could have been transferred...
3449  signature = 0;
3450 
3451  timeMemoryCleanup = timer.ticks();
3452  ULOGGER_INFO("timeMemoryCleanup = %fs... %d signatures removed", timeMemoryCleanup, (int)signaturesRemoved.size());
3453 
3454 
3455 
3456  //============================================================
3457  // TRANSFER
3458  //============================================================
3459  // If time allowed for the detection exceeds the limit of
3460  // real-time, move the oldest signature with less frequency
3461  // entry (from X oldest) from the short term memory to the
3462  // long term memory.
3463  //============================================================
3464  double totalTime = timerTotal.ticks();
3465  ULOGGER_INFO("Total time processing = %fs...", totalTime);
3466  if((_maxTimeAllowed != 0 && totalTime*1000>_maxTimeAllowed) ||
3468  {
3469  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);
3470  immunizedLocations.insert(_lastLocalizationNodeId); // keep the latest localization in working memory
3471  std::list<int> transferred = _memory->forget(immunizedLocations);
3472  signaturesRemoved.insert(signaturesRemoved.end(), transferred.begin(), transferred.end());
3473  if(!_someNodesHaveBeenTransferred && transferred.size())
3474  {
3475  _someNodesHaveBeenTransferred = true; // only used to hide a warning on close nodes immunization
3476  }
3477  }
3478  _lastProcessTime = totalTime;
3479 
3480  // cleanup cached gps values
3481  for(std::list<int>::iterator iter=signaturesRemoved.begin(); iter!=signaturesRemoved.end() && _gpsGeocentricCache.size(); ++iter)
3482  {
3483  _gpsGeocentricCache.erase(*iter);
3484  }
3485 
3486  //Remove optimized poses from signatures transferred
3487  if(signaturesRemoved.size() && (_optimizedPoses.size() || _constraints.size()))
3488  {
3489  //refresh the local map because some transferred nodes may have broken the tree
3490  int id = 0;
3491  if(!_memory->isIncremental() && (_lastLocalizationNodeId > 0 || _path.size()))
3492  {
3493  if(_path.size())
3494  {
3495  // priority on node on the path
3496  UASSERT(_pathCurrentIndex < _path.size());
3497  UASSERT_MSG(uContains(_optimizedPoses, _path.at(_pathCurrentIndex).first), uFormat("id=%d", _path.at(_pathCurrentIndex).first).c_str());
3498  id = _path.at(_pathCurrentIndex).first;
3499  UDEBUG("Refresh local map from %d", id);
3500  }
3501  else
3502  {
3504  {
3506  UDEBUG("Refresh local map from %d", id);
3507  }
3508  else
3509  {
3511  }
3512  }
3513  }
3514  else if(_memory->isIncremental() &&
3515  _optimizedPoses.size() &&
3517  {
3518  id = _memory->getLastWorkingSignature()->id();
3519  UDEBUG("Refresh local map from %d", id);
3520  }
3521  UDEBUG("id=%d _optimizedPoses=%d", id, (int)_optimizedPoses.size());
3522  if(id > 0)
3523  {
3524  if(_lastLocalizationNodeId != 0)
3525  {
3527  }
3528  UASSERT_MSG(_memory->getSignature(id) != 0, uFormat("id=%d", id).c_str());
3529 
3530  if(signaturesRemoved.size() == 1 && signaturesRemoved.front() == lastSignatureData.id())
3531  {
3532  UDEBUG("Detected that only last signature has been removed");
3533  int lastId = signaturesRemoved.front();
3534  _optimizedPoses.erase(lastId);
3535  for(std::multimap<int, Link>::iterator iter=_constraints.find(lastId); iter!=_constraints.end() && iter->first==lastId;++iter)
3536  {
3537  if(iter->second.to() != iter->second.from())
3538  {
3539  std::multimap<int, Link>::iterator jter = graph::findLink(_constraints, iter->second.to(), iter->second.from(), false);
3540  if(jter != _constraints.end())
3541  {
3542  _constraints.erase(jter);
3543  }
3544  }
3545  }
3546  _constraints.erase(lastId);
3547  }
3548  else
3549  {
3550 
3551  std::map<int, int> ids = _memory->getNeighborsId(id, 0, 0, true);
3552  for(std::map<int, Transform>::iterator iter=_optimizedPoses.begin(); iter!=_optimizedPoses.end();)
3553  {
3554  if(iter->first > 0 && !uContains(ids, iter->first))
3555  {
3556  UDEBUG("Removed %d from local map", iter->first);
3557  UASSERT(iter->first != _lastLocalizationNodeId);
3558  _optimizedPoses.erase(iter++);
3559  }
3560  else
3561  {
3562  ++iter;
3563  }
3564  }
3565  for(std::multimap<int, Link>::iterator iter=_constraints.begin(); iter!=_constraints.end();)
3566  {
3567  if(iter->first > 0 && (!uContains(ids, iter->second.from()) || !uContains(ids, iter->second.to())))
3568  {
3569  _constraints.erase(iter++);
3570  }
3571  else
3572  {
3573  ++iter;
3574  }
3575  }
3576  }
3577  }
3578  else
3579  {
3580  _optimizedPoses.clear();
3581  _constraints.clear();
3582  }
3583  }
3584  // just some verifications to make sure that planning path is still in the local map!
3585  if(_path.size())
3586  {
3587  UASSERT(_pathCurrentIndex < _path.size());
3588  UASSERT(_pathGoalIndex < _path.size());
3589  UASSERT_MSG(uContains(_optimizedPoses, _path.at(_pathCurrentIndex).first), uFormat("local map size=%d, id=%d", (int)_optimizedPoses.size(), _path.at(_pathCurrentIndex).first).c_str());
3590  UASSERT_MSG(uContains(_optimizedPoses, _path.at(_pathGoalIndex).first), uFormat("local map size=%d, id=%d", (int)_optimizedPoses.size(), _path.at(_pathGoalIndex).first).c_str());
3591  }
3592 
3593 
3594  timeRealTimeLimitReachedProcess = timer.ticks();
3595  ULOGGER_INFO("Time limit reached processing = %f...", timeRealTimeLimitReachedProcess);
3596 
3597  //==============================================================
3598  // Finalize statistics and log files
3599  //==============================================================
3600  int localGraphSize = 0;
3601  if(_publishStats)
3602  {
3603  statistics_.addStatistic(Statistics::kTimingStatistics_creation(), timeStatsCreation*1000);
3604  statistics_.addStatistic(Statistics::kTimingTotal(), totalTime*1000);
3605  statistics_.addStatistic(Statistics::kTimingForgetting(), timeRealTimeLimitReachedProcess*1000);
3606  statistics_.addStatistic(Statistics::kTimingJoining_trash(), timeJoiningTrash*1000);
3607  statistics_.addStatistic(Statistics::kTimingEmptying_trash(), timeEmptyingTrash*1000);
3608  statistics_.addStatistic(Statistics::kTimingMemory_cleanup(), timeMemoryCleanup*1000);
3609 
3610  // Transfer
3611  statistics_.addStatistic(Statistics::kMemorySignatures_removed(), signaturesRemoved.size());
3612  statistics_.addStatistic(Statistics::kMemoryImmunized_globally(), immunizedGlobally);
3613  statistics_.addStatistic(Statistics::kMemoryImmunized_locally(), immunizedLocally);
3614  statistics_.addStatistic(Statistics::kMemoryImmunized_locally_max(), maxLocalLocationsImmunized);
3615 
3616  // place after transfer because the memory/local graph may have changed
3617  statistics_.addStatistic(Statistics::kMemoryWorking_memory_size(), _memory->getWorkingMem().size());
3618  statistics_.addStatistic(Statistics::kMemoryShort_time_memory_size(), _memory->getStMem().size());
3619  statistics_.addStatistic(Statistics::kMemoryDatabase_memory_used(), _memory->getDatabaseMemoryUsed());
3620 
3621  // Set local graph
3622  std::map<int, Transform> poses;
3623  std::multimap<int, Link> constraints;
3624  if(!_rgbdSlamMode)
3625  {
3626  UDEBUG("");
3627  // no optimization on appearance-only mode, create a local graph
3628  std::map<int, int> ids = _memory->getNeighborsId(lastSignatureData.id(), 0, 0, true);
3629  _memory->getMetricConstraints(uKeysSet(ids), poses, constraints, false);
3630  }
3631  else // RGBD-SLAM mode
3632  {
3633  poses = _optimizedPoses;
3634  constraints = _constraints;
3635  }
3636  UDEBUG("");
3638  {
3639  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);
3640  statistics_.setLastSignatureData(lastSignatureData);
3641  }
3642  else
3643  {
3644  // only copy node info
3645  Signature nodeInfo(
3646  lastSignatureData.id(),
3647  lastSignatureData.mapId(),
3648  lastSignatureData.getWeight(),
3649  lastSignatureData.getStamp(),
3650  lastSignatureData.getLabel(),
3651  lastSignatureData.getPose(),
3652  lastSignatureData.getGroundTruthPose());
3653  const std::vector<float> & v = lastSignatureData.getVelocity();
3654  if(v.size() == 6)
3655  {
3656  nodeInfo.setVelocity(v[0], v[1], v[2], v[3], v[4], v[5]);
3657  }
3658  nodeInfo.sensorData().setGPS(lastSignatureData.sensorData().gps());
3659  nodeInfo.sensorData().setEnvSensors(lastSignatureData.sensorData().envSensors());
3661  }
3662  UDEBUG("");
3663  localGraphSize = (int)poses.size();
3664  if(!lastSignatureLocalizedPose.isNull())
3665  {
3666  poses.insert(std::make_pair(lastSignatureData.id(), lastSignatureLocalizedPose)); // in case we are in localization
3667  }
3668  statistics_.setPoses(poses);
3669  statistics_.setConstraints(constraints);
3670 
3671  statistics_.addStatistic(Statistics::kMemoryLocal_graph_size(), poses.size());
3672 
3673  if(_computeRMSE && _memory->getGroundTruths().size())
3674  {
3675  UDEBUG("Computing RMSE...");
3676  float translational_rmse = 0.0f;
3677  float translational_mean = 0.0f;
3678  float translational_median = 0.0f;
3679  float translational_std = 0.0f;
3680  float translational_min = 0.0f;
3681  float translational_max = 0.0f;
3682  float rotational_rmse = 0.0f;
3683  float rotational_mean = 0.0f;
3684  float rotational_median = 0.0f;
3685  float rotational_std = 0.0f;
3686  float rotational_min = 0.0f;
3687  float rotational_max = 0.0f;
3688 
3691  poses,
3692  translational_rmse,
3693  translational_mean,
3694  translational_median,
3695  translational_std,
3696  translational_min,
3697  translational_max,
3698  rotational_rmse,
3699  rotational_mean,
3700  rotational_median,
3701  rotational_std,
3702  rotational_min,
3703  rotational_max);
3704 
3705  statistics_.addStatistic(Statistics::kGtTranslational_rmse(), translational_rmse);
3706  statistics_.addStatistic(Statistics::kGtTranslational_mean(), translational_mean);
3707  statistics_.addStatistic(Statistics::kGtTranslational_median(), translational_median);
3708  statistics_.addStatistic(Statistics::kGtTranslational_std(), translational_std);
3709  statistics_.addStatistic(Statistics::kGtTranslational_min(), translational_min);
3710  statistics_.addStatistic(Statistics::kGtTranslational_max(), translational_max);
3711  statistics_.addStatistic(Statistics::kGtRotational_rmse(), rotational_rmse);
3712  statistics_.addStatistic(Statistics::kGtRotational_mean(), rotational_mean);
3713  statistics_.addStatistic(Statistics::kGtRotational_median(), rotational_median);
3714  statistics_.addStatistic(Statistics::kGtRotational_std(), rotational_std);
3715  statistics_.addStatistic(Statistics::kGtRotational_min(), rotational_min);
3716  statistics_.addStatistic(Statistics::kGtRotational_max(), rotational_max);
3717  UDEBUG("Computing RMSE...done!");
3718  }
3719 
3720  std::vector<int> ids;
3721  ids.reserve(_memory->getWorkingMem().size() + _memory->getStMem().size());
3722  for(std::set<int>::const_iterator iter=_memory->getStMem().begin(); iter!=_memory->getStMem().end(); ++iter)
3723  {
3724  ids.push_back(*iter);
3725  }
3726  for(std::map<int, double>::const_iterator iter=_memory->getWorkingMem().lower_bound(0); iter!=_memory->getWorkingMem().end(); ++iter)
3727  {
3728  ids.push_back(iter->first);
3729  }
3730  statistics_.setWmState(ids);
3731  UDEBUG("wmState=%d", (int)ids.size());
3732  }
3733 
3734  //Save statistics to database
3736  {
3738  }
3739 
3740  //Start trashing
3741  UDEBUG("Empty trash...");
3742  _memory->emptyTrash();
3743 
3744  // Log info...
3745  // TODO : use a specific class which will handle the RtabmapEvent
3746  if(_foutFloat && _foutInt)
3747  {
3748  UDEBUG("Logging...");
3749  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",
3750  totalTime,
3751  timeMemoryUpdate,
3752  timeReactivations,
3753  timeLikelihoodCalculation,
3754  timePosteriorCalculation,
3755  timeHypothesesCreation,
3756  timeHypothesesValidation,
3757  timeRealTimeLimitReachedProcess,
3758  timeStatsCreation,
3759  _highestHypothesis.second,
3760  0.0f,
3761  0.0f,
3762  0.0f,
3763  0.0f,
3764  0.0f,
3765  vpHypothesis,
3766  timeJoiningTrash,
3767  rehearsalValue,
3768  timeEmptyingTrash,
3769  timeRetrievalDbAccess,
3770  timeAddLoopClosureLink,
3771  timeMemoryCleanup,
3772  timeNeighborLinkRefining,
3773  timeProximityByTimeDetection,
3774  timeProximityBySpaceDetection,
3775  timeMapOptimization);
3776  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",
3777  _loopClosureHypothesis.first,
3778  _highestHypothesis.first,
3779  (int)signaturesRemoved.size(),
3780  0,
3781  refWordsCount,
3782  dictionarySize,
3783  int(_memory->getWorkingMem().size()),
3784  rejectedGlobalLoopClosure?1:0,
3785  0,
3786  0,
3787  int(signaturesRetrieved.size()),
3788  lcHypothesisReactivated,
3789  refUniqueWordsCount,
3790  retrievalId,
3791  0,
3792  rehearsalMaxId,
3793  rehearsalMaxId>0?1:0,
3794  localGraphSize,
3795  data.id(),
3799  {
3800  _bufferedLogsF.push_back(logF);
3801  _bufferedLogsI.push_back(logI);
3802  }
3803  else
3804  {
3805  if(_foutFloat)
3806  {
3807  fprintf(_foutFloat, "%s", logF.c_str());
3808  }
3809  if(_foutInt)
3810  {
3811  fprintf(_foutInt, "%s", logI.c_str());
3812  }
3813  }
3814  UINFO("Time logging = %f...", timer.ticks());
3815  //ULogger::flush();
3816  }
3817  timeFinalizingStatistics = timer.ticks();
3818  UDEBUG("End process, timeFinalizingStatistics=%fs", timeFinalizingStatistics);
3819  if(_publishStats)
3820  {
3821  statistics_.addStatistic(Statistics::kTimingFinalizing_statistics(), timeFinalizingStatistics*1000);
3822  }
3823 
3824  return true;
3825 }
3826 
3827 // SETTERS
3828 void Rtabmap::setTimeThreshold(float maxTimeAllowed)
3829 {
3830  //must be positive, 0 mean inf time allowed (no time limit)
3831  _maxTimeAllowed = maxTimeAllowed;
3832  if(_maxTimeAllowed < 0)
3833  {
3834  ULOGGER_WARN("maxTimeAllowed < 0, then setting it to 0 (inf).");
3835  _maxTimeAllowed = 0;
3836  }
3837  else if(_maxTimeAllowed > 0.0f && _maxTimeAllowed < 1.0f)
3838  {
3839  ULOGGER_WARN("Time threshold set to %fms, it is not in seconds!", _maxTimeAllowed);
3840  }
3841 }
3842 
3843 void Rtabmap::setWorkingDirectory(std::string path)
3844 {
3845  if(!path.empty() && UDirectory::exists(path))
3846  {
3847  ULOGGER_DEBUG("Comparing new working directory path \"%s\" with \"%s\"", path.c_str(), _wDir.c_str());
3848  if(path.compare(_wDir) != 0)
3849  {
3850  if (_foutFloat || _foutInt)
3851  {
3852  UWARN("Working directory has been changed from \"%s\" with \"%s\", new log files will be created.",
3853  path.c_str(), _wDir.c_str());
3854  }
3855  _wDir = path;
3856  setupLogFiles();
3857  }
3858  }
3859  else if(path.empty())
3860  {
3861  _wDir.clear();
3862  setupLogFiles();
3863  }
3864  else
3865  {
3866  ULOGGER_ERROR("Directory \"%s\" doesn't exist!", path.c_str());
3867  }
3868 }
3869 
3871 {
3872  if(_memory && _memory->getStMem().find(getLastLocationId())!=_memory->getStMem().end())
3873  {
3874  std::multimap<int, Link> links = _memory->getLinks(getLastLocationId(), false);
3875  bool linksRemoved = false;
3876  for(std::multimap<int, Link>::iterator iter = links.begin(); iter!=links.end(); ++iter)
3877  {
3878  if(iter->second.type() == Link::kGlobalClosure ||
3879  iter->second.type() == Link::kLocalSpaceClosure ||
3880  iter->second.type() == Link::kLocalTimeClosure ||
3881  iter->second.type() == Link::kUserClosure)
3882  {
3883  _memory->removeLink(iter->second.from(), iter->second.to());
3884  std::multimap<int, Link>::iterator jter = graph::findLink(_constraints, iter->second.from(), iter->second.to(), true);
3885  if(jter!=_constraints.end())
3886  {
3887  _constraints.erase(jter);
3888  // second time if link is also inverted
3889  jter = graph::findLink(_constraints, iter->second.from(), iter->second.to(), true);
3890  if(jter!=_constraints.end())
3891  {
3892  _constraints.erase(jter);
3893  }
3894  }
3895  linksRemoved = true;
3896  }
3897  }
3898 
3899  if(linksRemoved)
3900  {
3901  _loopClosureHypothesis.first = 0;
3902 
3903  // we have to re-optimize the graph without the rejected links
3904  if(_memory->isIncremental() && _optimizedPoses.size())
3905  {
3906  UINFO("Update graph");
3907  std::map<int, Transform> poses = _optimizedPoses;
3908  std::multimap<int, Link> constraints;
3909  cv::Mat covariance;
3910  optimizeCurrentMap(getLastLocationId(), false, poses, covariance, &constraints);
3911 
3912  if(poses.empty())
3913  {
3914  UWARN("Graph optimization failed after removing loop closure links from last location!");
3915  }
3916  else
3917  {
3918  UINFO("Updated local map (old size=%d, new size=%d)", (int)_optimizedPoses.size(), (int)poses.size());
3919  _optimizedPoses = poses;
3920  _constraints = constraints;
3922  }
3923  }
3924  }
3925  }
3926 }
3927 
3929 {
3930  if(_memory && _memory->getStMem().size())
3931  {
3932  int lastId = *_memory->getStMem().rbegin();
3933  _memory->deleteLocation(lastId);
3934  // we have to re-optimize the graph without the deleted location
3935  if(_memory->isIncremental() && _optimizedPoses.size())
3936  {
3937  UINFO("Update graph");
3938  _optimizedPoses.erase(lastId);
3939  std::map<int, Transform> poses = _optimizedPoses;
3940  //remove all constraints with last localization id
3941  for(std::multimap<int, Link>::iterator iter=_constraints.begin(); iter!=_constraints.end();)
3942  {
3943  if(iter->second.from() == lastId || iter->second.to() == lastId)
3944  {
3945  _constraints.erase(iter++);
3946  }
3947  else
3948  {
3949  ++iter;
3950  }
3951  }
3952 
3953  if(poses.empty())
3954  {
3956  }
3957  else
3958  {
3959  std::multimap<int, Link> constraints;
3960  cv::Mat covariance;
3961  optimizeCurrentMap(_memory->getLastWorkingSignature()->id(), false, poses, covariance, &constraints);
3962 
3963  if(poses.empty())
3964  {
3965  UWARN("Graph optimization failed after deleting the last location!");
3966  }
3967  else
3968  {
3969  _optimizedPoses = poses;
3970  _constraints = constraints;
3972  }
3973  }
3974  }
3975  }
3976 }
3977 
3978 void Rtabmap::setOptimizedPoses(const std::map<int, Transform> & poses)
3979 {
3980  _optimizedPoses = poses;
3981 }
3982 
3983 void Rtabmap::dumpData() const
3984 {
3985  UDEBUG("");
3986  if(_memory)
3987  {
3988  if(this->getWorkingDir().empty())
3989  {
3990  UERROR("Working directory not set.");
3991  }
3992  else
3993  {
3994  _memory->dumpMemory(this->getWorkingDir());
3995  }
3996  }
3997 }
3998 
3999 // fromId must be in _memory and in _optimizedPoses
4000 // Get poses in front of the robot, return optimized poses
4001 std::map<int, Transform> Rtabmap::getForwardWMPoses(
4002  int fromId,
4003  int maxNearestNeighbors,
4004  float radius,
4005  int maxGraphDepth // 0 means ignore
4006  ) const
4007 {
4008  std::map<int, Transform> poses;
4009  if(_memory && fromId > 0)
4010  {
4011  UDEBUG("");
4012  const Signature * fromS = _memory->getSignature(fromId);
4013  UASSERT(fromS != 0);
4014  UASSERT(_optimizedPoses.find(fromId) != _optimizedPoses.end());
4015 
4016  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
4017  cloud->resize(_optimizedPoses.size());
4018  std::vector<int> ids(_optimizedPoses.size());
4019  int oi = 0;
4020  const std::set<int> & stm = _memory->getStMem();
4021  //get distances
4022  std::map<int, float> foundIds;
4023  if(_memory->isIncremental())
4024  {
4025  foundIds = _memory->getNeighborsIdRadius(fromId, radius, _optimizedPoses, maxGraphDepth);
4026  }
4027  else
4028  {
4029  foundIds = graph::getNodesInRadius(fromId, _optimizedPoses, radius);
4030  }
4031 
4032  float radiusSqrd = radius * radius;
4033  for(std::map<int, Transform>::const_iterator iter = _optimizedPoses.begin(); iter!=_optimizedPoses.end(); ++iter)
4034  {
4035  if(iter->first != fromId)
4036  {
4037  if(stm.find(iter->first) == stm.end() &&
4038  uContains(foundIds, iter->first) &&
4039  (radiusSqrd==0 || foundIds.at(iter->first) <= radiusSqrd))
4040  {
4041  (*cloud)[oi] = pcl::PointXYZ(iter->second.x(), iter->second.y(), iter->second.z());
4042  ids[oi++] = iter->first;
4043  }
4044  }
4045  }
4046 
4047  cloud->resize(oi);
4048  ids.resize(oi);
4049 
4050  Transform fromT = _optimizedPoses.at(fromId);
4051 
4052  if(cloud->size())
4053  {
4054  //if(cloud->size())
4055  //{
4056  // pcl::io::savePCDFile("radiusPoses.pcd", *cloud);
4057  // UWARN("Saved radiusPoses.pcd");
4058  //}
4059 
4060  //filter poses in front of the fromId
4061  float x,y,z, roll,pitch,yaw;
4062  fromT.getTranslationAndEulerAngles(x,y,z, roll,pitch,yaw);
4063 
4064  pcl::CropBox<pcl::PointXYZ> cropbox;
4065  cropbox.setInputCloud(cloud);
4066  cropbox.setMin(Eigen::Vector4f(-1, -radius, -999999, 0));
4067  cropbox.setMax(Eigen::Vector4f(radius, radius, 999999, 0));
4068  cropbox.setRotation(Eigen::Vector3f(roll, pitch, yaw));
4069  cropbox.setTranslation(Eigen::Vector3f(x, y, z));
4070  cropbox.setRotation(Eigen::Vector3f(roll,pitch,yaw));
4071  pcl::IndicesPtr indices(new std::vector<int>());
4072  cropbox.filter(*indices);
4073 
4074  //if(indices->size())
4075  //{
4076  // pcl::io::savePCDFile("radiusCrop.pcd", *cloud, *indices);
4077  // UWARN("Saved radiusCrop.pcd");
4078  //}
4079 
4080  if(indices->size())
4081  {
4082  pcl::search::KdTree<pcl::PointXYZ>::Ptr kdTree(new pcl::search::KdTree<pcl::PointXYZ>);
4083  kdTree->setInputCloud(cloud, indices);
4084  std::vector<int> ind;
4085  std::vector<float> dist;
4086  pcl::PointXYZ pt(fromT.x(), fromT.y(), fromT.z());
4087  kdTree->radiusSearch(pt, radius, ind, dist, maxNearestNeighbors);
4088  //pcl::PointCloud<pcl::PointXYZ> inliers;
4089  for(unsigned int i=0; i<ind.size(); ++i)
4090  {
4091  if(ind[i] >=0)
4092  {
4093  Transform tmp = _optimizedPoses.find(ids[ind[i]])->second;
4094  //inliers.push_back(pcl::PointXYZ(tmp.x(), tmp.y(), tmp.z()));
4095  UDEBUG("Inlier %d: %s", ids[ind[i]], tmp.prettyPrint().c_str());
4096  poses.insert(std::make_pair(ids[ind[i]], tmp));
4097  }
4098  }
4099 
4100  //if(inliers.size())
4101  //{
4102  // pcl::io::savePCDFile("radiusInliers.pcd", inliers);
4103  //}
4104  //if(nearestId >0)
4105  //{
4106  // pcl::PointCloud<pcl::PointXYZ> c;
4107  // Transform ct = _optimizedPoses.find(nearestId)->second;
4108  // c.push_back(pcl::PointXYZ(ct.x(), ct.y(), ct.z()));
4109  // pcl::io::savePCDFile("radiusNearestPt.pcd", c);
4110  //}
4111  }
4112  }
4113  }
4114  return poses;
4115 }
4116 
4117 std::map<int, std::map<int, Transform> > Rtabmap::getPaths(const std::map<int, Transform> & posesIn, const Transform & target, int maxGraphDepth) const
4118 {
4119  std::map<int, std::map<int, Transform> > paths;
4120  std::set<int> nodesSet;
4121  std::map<int, Transform> poses;
4122  for(std::map<int, Transform>::const_iterator iter=posesIn.lower_bound(1); iter!=posesIn.end(); ++iter)
4123  {
4124  nodesSet.insert(iter->first);
4125  poses.insert(*iter);
4126  }
4127  if(_memory && nodesSet.size() && !target.isNull())
4128  {
4129  double e0=0,e1=0,e2=0,e3=0,e4=0;
4130  UTimer t;
4131  e0 = t.ticks();
4132  // Segment poses connected only by neighbor links
4133  while(poses.size())
4134  {
4135  std::map<int, Transform> path;
4136  // select nearest pose and iterate neighbors from there
4137  int nearestId = rtabmap::graph::findNearestNode(poses, target);
4138 
4139  e1+=t.ticks();
4140 
4141  if(nearestId == 0)
4142  {
4143  UWARN("Nearest id of %s in %d poses is 0 !? Returning empty path.", target.prettyPrint().c_str(), (int)poses.size());
4144  break;
4145  }
4146  std::map<int, int> ids = _memory->getNeighborsId(nearestId, maxGraphDepth, 0, true, true, true, true, nodesSet);
4147 
4148  e2+=t.ticks();
4149 
4150  for(std::map<int, int>::iterator iter=ids.begin(); iter!=ids.end(); ++iter)
4151  {
4152  std::map<int, Transform>::iterator jter = poses.find(iter->first);
4153  if(jter != poses.end())
4154  {
4155  bool valid = path.empty();
4156  if(!valid)
4157  {
4158  // make sure it has a neighbor added to path
4159  std::multimap<int, Link> links = _memory->getNeighborLinks(iter->first);
4160  for(std::multimap<int, Link>::iterator kter=links.begin(); kter!=links.end() && !valid; ++kter)
4161  {
4162  valid = path.find(kter->first) != path.end();
4163  }
4164  }
4165 
4166  if(valid)
4167  {
4168  //UDEBUG("%d <- %d", nearestId, jter->first);
4169  path.insert(*jter);
4170  poses.erase(jter);
4171  }
4172  }
4173  }
4174 
4175  e3+=t.ticks();
4176 
4177  if (path.size())
4178  {
4179  if (maxGraphDepth > 0 && !_memory->isGraphReduced() && (int)path.size() > maxGraphDepth * 2 + 1)
4180  {
4181  UWARN("%s=Off but path(%d) > maxGraphDepth(%d)*2+1, nearestId=%d ids=%d. Is reduce graph activated before?",
4182  Parameters::kMemReduceGraph().c_str(), (int)path.size(), maxGraphDepth, nearestId, (int)ids.size());
4183  }
4184  paths.insert(std::make_pair(nearestId, path));
4185  }
4186  else
4187  {
4188  UWARN(uFormat("path.size()=0!? nearestId=%d ids=%d, aborting...", (int)path.size(), nearestId, (int)ids.size()).c_str());
4189  break;
4190  }
4191 
4192  e4+=t.ticks();
4193  }
4194  UDEBUG("e0=%fs e1=%fs e2=%fs e3=%fs e4=%fs", e0, e1, e2, e3, e4);
4195  }
4196  return paths;
4197 }
4198 
4200  int id,
4201  bool lookInDatabase,
4202  std::map<int, Transform> & optimizedPoses,
4203  cv::Mat & covariance,
4204  std::multimap<int, Link> * constraints,
4205  double * error,
4206  int * iterationsDone) const
4207 {
4208  //Optimize the map
4209  UINFO("Optimize map: around location %d (lookInDatabase=%s)", id, lookInDatabase?"true":"false");
4210  if(_memory && id > 0)
4211  {
4212  UTimer timer;
4213  std::map<int, int> ids = _memory->getNeighborsId(id, 0, lookInDatabase?-1:0, true, false);
4214  if(!_optimizeFromGraphEnd && ids.size() > 1)
4215  {
4216  id = ids.begin()->first;
4217  }
4218  UINFO("get %d ids time %f s", (int)ids.size(), timer.ticks());
4219 
4220  std::map<int, Transform> poses = Rtabmap::optimizeGraph(id, uKeysSet(ids), optimizedPoses, lookInDatabase, covariance, constraints, error, iterationsDone);
4221  UINFO("optimize time %f s", timer.ticks());
4222 
4223  if(poses.size())
4224  {
4225  optimizedPoses = poses;
4226 
4227  if(_memory->getSignature(id) && uContains(optimizedPoses, id))
4228  {
4229  Transform t = optimizedPoses.at(id) * _memory->getSignature(id)->getPose().inverse();
4230  UINFO("Correction (from node %d) %s", id, t.prettyPrint().c_str());
4231  }
4232  }
4233  else
4234  {
4235  UWARN("Failed to optimize the graph! returning empty optimized poses...");
4236  optimizedPoses.clear();
4237  if(constraints)
4238  {
4239  constraints->clear();
4240  }
4241  }
4242  }
4243 }
4244 
4245 std::map<int, Transform> Rtabmap::optimizeGraph(
4246  int fromId,
4247  const std::set<int> & ids,
4248  const std::map<int, Transform> & guessPoses,
4249  bool lookInDatabase,
4250  cv::Mat & covariance,
4251  std::multimap<int, Link> * constraints,
4252  double * error,
4253  int * iterationsDone) const
4254 {
4255  UTimer timer;
4256  std::map<int, Transform> optimizedPoses;
4257  std::map<int, Transform> poses;
4258  std::multimap<int, Link> edgeConstraints;
4259  UDEBUG("ids=%d", (int)ids.size());
4260  _memory->getMetricConstraints(ids, poses, edgeConstraints, lookInDatabase, !_graphOptimizer->landmarksIgnored());
4261  UINFO("get constraints (ids=%d, %d poses, %d edges) time %f s", (int)ids.size(), (int)poses.size(), (int)edgeConstraints.size(), timer.ticks());
4262 
4263  if(_graphOptimizer->iterations() > 0)
4264  {
4265  for(std::map<int, Transform>::iterator iter=poses.begin(); iter!=poses.end(); ++iter)
4266  {
4267  // Apply guess poses (if some)
4268  std::map<int, Transform>::const_iterator foundGuess = guessPoses.find(iter->first);
4269  if(foundGuess!=guessPoses.end())
4270  {
4271  iter->second = foundGuess->second;
4272  }
4273  }
4274  }
4275 
4276 
4278  if(_graphOptimizer->iterations() == 0)
4279  {
4280  // Optimization disabled! Return not optimized poses.
4281  optimizedPoses = poses;
4282  if(constraints)
4283  {
4284  *constraints = edgeConstraints;
4285  }
4286  }
4287  else
4288  {
4289  bool hasLandmarks = edgeConstraints.begin()->first < 0;
4290  if(poses.size() != guessPoses.size() || hasLandmarks)
4291  {
4292  UDEBUG("recompute poses using only links (robust to multi-session)");
4293  std::map<int, Transform> posesOut;
4294  std::multimap<int, Link> edgeConstraintsOut;
4295  _graphOptimizer->getConnectedGraph(fromId, poses, edgeConstraints, posesOut, edgeConstraintsOut);
4296  optimizedPoses = _graphOptimizer->optimize(fromId, posesOut, edgeConstraintsOut, covariance, 0, error, iterationsDone);
4297  if(constraints)
4298  {
4299  *constraints = edgeConstraintsOut;
4300  }
4301  }
4302  else
4303  {
4304  UDEBUG("use input guess poses");
4305  optimizedPoses = _graphOptimizer->optimize(fromId, poses, edgeConstraints, covariance, 0, error, iterationsDone);
4306  if(constraints)
4307  {
4308  *constraints = edgeConstraints;
4309  }
4310  }
4311 
4312  if(!poses.empty() && optimizedPoses.empty())
4313  {
4314  UWARN("Optimization has failed (poses=%d, guess=%d, links=%d)...",
4315  (int)poses.size(), (int)guessPoses.size(), (int)edgeConstraints.size());
4316  }
4317  }
4318 
4319  UINFO("Optimization time %f s", timer.ticks());
4320 
4321  return optimizedPoses;
4322 }
4323 
4324 void Rtabmap::adjustLikelihood(std::map<int, float> & likelihood) const
4325 {
4326  ULOGGER_DEBUG("likelihood.size()=%d", likelihood.size());
4327  UTimer timer;
4328  timer.start();
4329  if(likelihood.size()==0)
4330  {
4331  return;
4332  }
4333 
4334  // Use only non-null values (ignore virtual place)
4335  std::list<float> values;
4336  bool likelihoodNullValuesIgnored = true;
4337  for(std::map<int, float>::iterator iter = ++likelihood.begin(); iter!=likelihood.end(); ++iter)
4338  {
4339  if((iter->second >= 0 && !likelihoodNullValuesIgnored) ||
4340  (iter->second > 0 && likelihoodNullValuesIgnored))
4341  {
4342  values.push_back(iter->second);
4343  }
4344  }
4345  UDEBUG("values.size=%d", values.size());
4346 
4347  float mean = uMean(values);
4348  float stdDev = std::sqrt(uVariance(values, mean));
4349 
4350 
4351  //Adjust likelihood with mean and standard deviation (see Angeli phd)
4352  float epsilon = 0.0001;
4353  float max = 0.0f;
4354  int maxId = 0;
4355  for(std::map<int, float>::iterator iter=++likelihood.begin(); iter!= likelihood.end(); ++iter)
4356  {
4357  float value = iter->second;
4358  if(value > mean+stdDev && mean)
4359  {
4360  iter->second = (value-(stdDev-epsilon))/mean;
4361  if(value > max)
4362  {
4363  max = value;
4364  maxId = iter->first;
4365  }
4366  }
4367  else if(value == 1.0f && stdDev == 0)
4368  {
4369  iter->second = 1.0f;
4370  if(value > max)
4371  {
4372  max = value;
4373  maxId = iter->first;
4374  }
4375  }
4376  else
4377  {
4378  iter->second = 1.0f;
4379  }
4380  }
4381 
4382  if(stdDev > epsilon && max)
4383  {
4384  likelihood.begin()->second = mean/stdDev + 1.0f;
4385  }
4386  else
4387  {
4388  likelihood.begin()->second = 2.0f; //2 * std dev
4389  }
4390 
4391  double time = timer.ticks();
4392  UDEBUG("mean=%f, stdDev=%f, max=%f, maxId=%d, time=%fs", mean, stdDev, max, maxId, time);
4393 }
4394 
4396 {
4397  if(_memory && _bayesFilter)
4398  {
4399  if(this->getWorkingDir().empty())
4400  {
4401  UERROR("Working directory not set.");
4402  return;
4403  }
4404  std::list<int> signaturesToCompare;
4405  for(std::map<int, double>::const_iterator iter=_memory->getWorkingMem().begin();
4406  iter!=_memory->getWorkingMem().end();
4407  ++iter)
4408  {
4409  if(iter->first > 0)
4410  {
4411  const Signature * s = _memory->getSignature(iter->first);
4412  UASSERT(s!=0);
4413  if(s->getWeight() != -1) // ignore intermediate nodes
4414  {
4415  signaturesToCompare.push_back(iter->first);
4416  }
4417  }
4418  else
4419  {
4420  // virtual signature should be added
4421  signaturesToCompare.push_back(iter->first);
4422  }
4423  }
4424  cv::Mat prediction = _bayesFilter->generatePrediction(_memory, uListToVector(signaturesToCompare));
4425 
4426  FILE* fout = 0;
4427  std::string fileName = this->getWorkingDir() + "/DumpPrediction.txt";
4428  #ifdef _MSC_VER
4429  fopen_s(&fout, fileName.c_str(), "w");
4430  #else
4431  fout = fopen(fileName.c_str(), "w");
4432  #endif
4433 
4434  if(fout)
4435  {
4436  for(int i=0; i<prediction.rows; ++i)
4437  {
4438  for(int j=0; j<prediction.cols; ++j)
4439  {
4440  fprintf(fout, "%f ",((float*)prediction.data)[j + i*prediction.cols]);
4441  }
4442  fprintf(fout, "\n");
4443  }
4444  fclose(fout);
4445  }
4446  }
4447  else
4448  {
4449  UWARN("Memory and/or the Bayes filter are not created");
4450  }
4451 }
4452 
4453 Signature Rtabmap::getSignatureCopy(int id, bool images, bool scan, bool userData, bool occupancyGrid, bool withWords, bool withGlobalDescriptors) const
4454 {
4455  Signature s;
4456  if(_memory)
4457  {
4458  Transform odomPoseLocal;
4459  int weight = -1;
4460  int mapId = -1;
4461  std::string label;
4462  double stamp = 0;
4463  Transform groundTruth;
4464  std::vector<float> velocity;
4465  GPS gps;
4466  EnvSensors sensors;
4467  _memory->getNodeInfo(id, odomPoseLocal, mapId, weight, label, stamp, groundTruth, velocity, gps, sensors, true);
4468  SensorData data;
4469  data.setId(id);
4470  if(images || scan || userData || occupancyGrid)
4471  {
4472  data = _memory->getNodeData(id, images, scan, userData, occupancyGrid);
4473  }
4474  if(!images && withWords)
4475  {
4476  std::vector<CameraModel> models;
4477  StereoCameraModel stereoModel;
4478  _memory->getNodeCalibration(id, models, stereoModel);
4479  data.setCameraModels(models);
4480  data.setStereoCameraModel(stereoModel);
4481  }
4482 
4483  s=Signature(id,
4484  mapId,
4485  weight,
4486  stamp,
4487  label,
4488  odomPoseLocal,
4489  groundTruth,
4490  data);
4491 
4492  std::multimap<int, Link> links = _memory->getLinks(id, true, true);
4493  for(std::multimap<int, Link>::iterator iter=links.begin(); iter!=links.end(); ++iter)
4494  {
4495  if(iter->second.type() == Link::kLandmark)
4496  {
4497  s.addLandmark(iter->second);
4498  }
4499  else
4500  {
4501  s.addLink(iter->second);
4502  }
4503  }
4504 
4505  if(withWords || withGlobalDescriptors)
4506  {
4507  std::multimap<int, int> words;
4508  std::vector<cv::KeyPoint> wordsKpts;
4509  std::vector<cv::Point3f> words3;
4510  cv::Mat wordsDescriptors;
4511  std::vector<rtabmap::GlobalDescriptor> globalDescriptors;
4512  _memory->getNodeWordsAndGlobalDescriptors(id, words, wordsKpts, words3, wordsDescriptors, globalDescriptors);
4513  if(withWords)
4514  {
4515  s.setWords(words, wordsKpts, words3, wordsDescriptors);
4516  }
4517  if(withGlobalDescriptors)
4518  {
4519  s.sensorData().setGlobalDescriptors(globalDescriptors);
4520  }
4521  }
4522  if(velocity.size()==6)
4523  {
4524  s.setVelocity(velocity[0], velocity[1], velocity[2], velocity[3], velocity[4], velocity[5]);
4525  }
4526  s.sensorData().setGPS(gps);
4527  s.sensorData().setEnvSensors(sensors);
4528  }
4529  return s;
4530 }
4531 
4532 void Rtabmap::get3DMap(
4533  std::map<int, Signature> & signatures,
4534  std::map<int, Transform> & poses,
4535  std::multimap<int, Link> & constraints,
4536  bool optimized,
4537  bool global) const
4538 {
4539  UDEBUG("");
4540  return getGraph(poses, constraints, optimized, global, &signatures, true, true, true, true);
4541 }
4542 
4544  std::map<int, Transform> & poses,
4545  std::multimap<int, Link> & constraints,
4546  bool optimized,
4547  bool global,
4548  std::map<int, Signature> * signatures,
4549  bool withImages,
4550  bool withScan,
4551  bool withUserData,
4552  bool withGrid,
4553  bool withWords,
4554  bool withGlobalDescriptors) const
4555 {
4557  {
4558  if(_rgbdSlamMode)
4559  {
4560  if(optimized)
4561  {
4562  poses = _optimizedPoses; // guess
4563  cv::Mat covariance;
4564  this->optimizeCurrentMap(_memory->getLastWorkingSignature()->id(), global, poses, covariance, &constraints);
4565  }
4566  else
4567  {
4568  std::map<int, int> ids = _memory->getNeighborsId(_memory->getLastWorkingSignature()->id(), 0, global?-1:0, true);
4569  _memory->getMetricConstraints(uKeysSet(ids), poses, constraints, global);
4570  }
4571  }
4572  else
4573  {
4574  // no optimization on appearance-only mode
4575  std::map<int, int> ids = _memory->getNeighborsId(_memory->getLastWorkingSignature()->id(), 0, global?-1:0, true);
4576  _memory->getMetricConstraints(uKeysSet(ids), poses, constraints, global);
4577  }
4578 
4579  if(signatures)
4580  {
4581  // Get data
4582  std::set<int> ids = uKeysSet(_memory->getWorkingMem()); // WM
4583 
4584  //remove virtual signature
4585  ids.erase(Memory::kIdVirtual);
4586 
4587  ids.insert(_memory->getStMem().begin(), _memory->getStMem().end()); // STM + WM
4588  if(global)
4589  {
4590  ids = _memory->getAllSignatureIds(); // STM + WM + LTM, ignoreChildren=true
4591  }
4592 
4593  for(std::set<int>::iterator iter = ids.begin(); iter!=ids.end(); ++iter)
4594  {
4595  signatures->insert(std::make_pair(*iter, getSignatureCopy(*iter, withImages, withScan, withUserData, withGrid, withWords, withGlobalDescriptors)));
4596  }
4597  }
4598  }
4599  else if(_memory && (_memory->getStMem().size() || _memory->getWorkingMem().size() > 1))
4600  {
4601  UERROR("Last working signature is null!?");
4602  }
4603  else if(_memory == 0)
4604  {
4605  UWARN("Memory not initialized...");
4606  }
4607 }
4608 
4609 std::map<int, Transform> Rtabmap::getNodesInRadius(const Transform & pose, float radius)
4610 {
4611  return graph::getPosesInRadius(pose, _optimizedPoses, radius<=0?_localRadius:radius);
4612 }
4613 
4614 std::map<int, Transform> Rtabmap::getNodesInRadius(int nodeId, float radius)
4615 {
4616  UDEBUG("nodeId=%d, radius=%f", nodeId, radius);
4617  std::map<int, Transform> nearNodes;
4618  if(nodeId==0 && !_optimizedPoses.empty())
4619  {
4620  nodeId = _optimizedPoses.rbegin()->first;
4621  }
4622  if(_optimizedPoses.find(nodeId) != _optimizedPoses.end())
4623  {
4624  nearNodes = graph::getPosesInRadius(nodeId, _optimizedPoses, radius<=0?_localRadius:radius);
4625  }
4626  return nearNodes;
4627 }
4628 
4630  float clusterRadius,
4631  float clusterAngle,
4632  int iterations,
4633  bool intraSession,
4634  bool interSession,
4635  const ProgressState * processState)
4636 {
4637  UASSERT(iterations>0);
4638 
4639  if(_graphOptimizer->iterations() <= 0)
4640  {
4641  UERROR("Cannot detect more loop closures if graph optimization iterations = 0");
4642  return -1;
4643  }
4644  if(!_rgbdSlamMode)
4645  {
4646  UERROR("Detecting more loop closures can be done only in RGBD-SLAM mode.");
4647  return -1;
4648  }
4649  if(!intraSession && !interSession)
4650  {
4651  UERROR("Intra and/or inter session argument should be true.");
4652  return -1;
4653  }
4654 
4655  std::list<Link> loopClosuresAdded;
4656  std::multimap<int, int> checkedLoopClosures;
4657 
4658  std::map<int, Transform> posesToCheckLoopClosures;
4659  std::map<int, Transform> poses;
4660  std::multimap<int, Link> links;
4661  std::map<int, Signature> signatures; // some signatures may be in LTM, get them all
4662  this->getGraph(poses, links, true, true, &signatures);
4663 
4664  std::map<int, int> mapIds;
4665  UDEBUG("remove all invalid or intermediate nodes, fill mapIds");
4666  for(std::map<int, Transform>::iterator iter=poses.upper_bound(0); iter!=poses.end();++iter)
4667  {
4668  if(signatures.at(iter->first).getWeight() >= 0)
4669  {
4670  posesToCheckLoopClosures.insert(*iter);
4671  mapIds.insert(std::make_pair(iter->first, signatures.at(iter->first).mapId()));
4672  }
4673  }
4674 
4675  for(int n=0; n<iterations; ++n)
4676  {
4677  UINFO("Looking for more loop closures, clustering poses... (iteration=%d/%d, radius=%f m angle=%f rad)",
4678  n+1, iterations, clusterRadius, clusterAngle);
4679 
4680  std::multimap<int, int> clusters = graph::radiusPosesClustering(
4681  posesToCheckLoopClosures,
4682  clusterRadius,
4683  clusterAngle);
4684 
4685  UINFO("Looking for more loop closures, clustering poses... found %d clusters.", (int)clusters.size());
4686 
4687  int i=0;
4688  std::set<int> addedLinks;
4689  for(std::multimap<int, int>::iterator iter=clusters.begin(); iter!= clusters.end(); ++iter, ++i)
4690  {
4691  if(processState && processState->isCanceled())
4692  {
4693  return -1;
4694  break;
4695  }
4696 
4697  int from = iter->first;
4698  int to = iter->second;
4699  if(from > to)
4700  {
4701  from = iter->second;
4702  to = iter->first;
4703  }
4704 
4705  int mapIdFrom = uValue(mapIds, from, 0);
4706  int mapIdTo = uValue(mapIds, to, 0);
4707 
4708  if((interSession && mapIdFrom != mapIdTo) ||
4709  (intraSession && mapIdFrom == mapIdTo))
4710  {
4711 
4712  bool alreadyChecked = false;
4713  for(std::multimap<int, int>::iterator jter = checkedLoopClosures.lower_bound(from);
4714  !alreadyChecked && jter!=checkedLoopClosures.end() && jter->first == from;
4715  ++jter)
4716  {
4717  if(to == jter->second)
4718  {
4719  alreadyChecked = true;
4720  }
4721  }
4722 
4723  if(!alreadyChecked)
4724  {
4725  // only add new links and one per cluster per iteration
4726  if(addedLinks.find(from) == addedLinks.end() &&
4727  addedLinks.find(to) == addedLinks.end() &&
4728  rtabmap::graph::findLink(links, from, to) == links.end())
4729  {
4730  checkedLoopClosures.insert(std::make_pair(from, to));
4731 
4732  UASSERT(signatures.find(from) != signatures.end());
4733  UASSERT(signatures.find(to) != signatures.end());
4734 
4735  Transform guess;
4736  if(_proximityOdomGuess && uContains(poses, from) && uContains(poses, to))
4737  {
4738  guess = poses.at(from).inverse() * poses.at(to);
4739  }
4740 
4741  RegistrationInfo info;
4742  // use signatures instead of IDs because some signatures may not be in WM
4743  Transform t = _memory->computeTransform(signatures.at(from), signatures.at(to), guess, &info);
4744 
4745  if(!t.isNull())
4746  {
4747  bool updateConstraints = true;
4748  if(_optimizationMaxError > 0.0f)
4749  {
4750  //optimize the graph to see if the new constraint is globally valid
4751 
4752  int fromId = from;
4753  int mapId = signatures.at(from).mapId();
4754  // use first node of the map containing from
4755  for(std::map<int, Signature>::iterator ster=signatures.begin(); ster!=signatures.end(); ++ster)
4756  {
4757  if(ster->second.mapId() == mapId)
4758  {
4759  fromId = ster->first;
4760  break;
4761  }
4762  }
4763  std::multimap<int, Link> linksIn = links;
4764  linksIn.insert(std::make_pair(from, Link(from, to, Link::kUserClosure, t, getInformation(info.covariance))));
4765  const Link * maxLinearLink = 0;
4766  const Link * maxAngularLink = 0;
4767  float maxLinearError = 0.0f;
4768  float maxAngularError = 0.0f;
4769  float maxLinearErrorRatio = 0.0f;
4770  float maxAngularErrorRatio = 0.0f;
4771  std::map<int, Transform> optimizedPoses;
4772  std::multimap<int, Link> links;
4773  UASSERT(poses.find(fromId) != poses.end());
4774  UASSERT_MSG(poses.find(from) != poses.end(), uFormat("id=%d poses=%d links=%d", from, (int)poses.size(), (int)links.size()).c_str());
4775  UASSERT_MSG(poses.find(to) != poses.end(), uFormat("id=%d poses=%d links=%d", to, (int)poses.size(), (int)links.size()).c_str());
4776  _graphOptimizer->getConnectedGraph(fromId, poses, linksIn, optimizedPoses, links);
4777  UASSERT(optimizedPoses.find(fromId) != optimizedPoses.end());
4778  UASSERT_MSG(optimizedPoses.find(from) != optimizedPoses.end(), uFormat("id=%d poses=%d links=%d", from, (int)optimizedPoses.size(), (int)links.size()).c_str());
4779  UASSERT_MSG(optimizedPoses.find(to) != optimizedPoses.end(), uFormat("id=%d poses=%d links=%d", to, (int)optimizedPoses.size(), (int)links.size()).c_str());
4780  UASSERT(graph::findLink(links, from, to) != links.end());
4781  optimizedPoses = _graphOptimizer->optimize(fromId, optimizedPoses, links);
4782  std::string msg;
4783  if(optimizedPoses.size())