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

rtabmap
Author(s): Mathieu Labbe
autogenerated on Wed Jun 5 2019 22:41:32