Memory.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/utilite/UEventsManager.h>
29 #include <rtabmap/utilite/ULogger.h>
30 #include <rtabmap/utilite/UTimer.h>
31 #include <rtabmap/utilite/UConversion.h>
32 #include <rtabmap/utilite/UProcessInfo.h>
33 #include <rtabmap/utilite/UMath.h>
34 
35 #include "rtabmap/core/Memory.h"
36 #include "rtabmap/core/Signature.h"
37 #include "rtabmap/core/Parameters.h"
38 #include "rtabmap/core/RtabmapEvent.h"
39 #include "rtabmap/core/VWDictionary.h"
40 #include <rtabmap/core/EpipolarGeometry.h>
41 #include "rtabmap/core/VisualWord.h"
42 #include "rtabmap/core/Features2d.h"
43 #include "rtabmap/core/RegistrationIcp.h"
44 #include "rtabmap/core/Registration.h"
45 #include "rtabmap/core/RegistrationVis.h"
46 #include "rtabmap/core/DBDriver.h"
47 #include "rtabmap/core/util3d_features.h"
48 #include "rtabmap/core/util3d_filtering.h"
49 #include "rtabmap/core/util3d_correspondences.h"
50 #include "rtabmap/core/util3d_registration.h"
51 #include "rtabmap/core/util3d_surface.h"
52 #include "rtabmap/core/util3d_transforms.h"
53 #include "rtabmap/core/util3d_motion_estimation.h"
54 #include "rtabmap/core/util3d.h"
55 #include "rtabmap/core/util2d.h"
56 #include "rtabmap/core/Statistics.h"
57 #include "rtabmap/core/Compression.h"
58 #include "rtabmap/core/Graph.h"
59 #include "rtabmap/core/Stereo.h"
60 #include "rtabmap/core/OptimizerG2O.h"
61 #include <pcl/io/pcd_io.h>
62 #include <pcl/common/common.h>
63 #include <rtabmap/core/OccupancyGrid.h>
64 #include <opencv2/imgproc/types_c.h>
65 
66 namespace rtabmap {
67 
68 const int Memory::kIdStart = 0;
69 const int Memory::kIdVirtual = -1;
70 const int Memory::kIdInvalid = 0;
71 
72 Memory::Memory(const ParametersMap & parameters) :
73  _dbDriver(0),
74  _similarityThreshold(Parameters::defaultMemRehearsalSimilarity()),
75  _binDataKept(Parameters::defaultMemBinDataKept()),
76  _rawDescriptorsKept(Parameters::defaultMemRawDescriptorsKept()),
77  _saveDepth16Format(Parameters::defaultMemSaveDepth16Format()),
78  _notLinkedNodesKeptInDb(Parameters::defaultMemNotLinkedNodesKept()),
79  _saveIntermediateNodeData(Parameters::defaultMemIntermediateNodeDataKept()),
80  _incrementalMemory(Parameters::defaultMemIncrementalMemory()),
81  _reduceGraph(Parameters::defaultMemReduceGraph()),
82  _maxStMemSize(Parameters::defaultMemSTMSize()),
83  _recentWmRatio(Parameters::defaultMemRecentWmRatio()),
84  _transferSortingByWeightId(Parameters::defaultMemTransferSortingByWeightId()),
85  _idUpdatedToNewOneRehearsal(Parameters::defaultMemRehearsalIdUpdatedToNewOne()),
86  _generateIds(Parameters::defaultMemGenerateIds()),
87  _badSignaturesIgnored(Parameters::defaultMemBadSignaturesIgnored()),
88  _mapLabelsAdded(Parameters::defaultMemMapLabelsAdded()),
89  _depthAsMask(Parameters::defaultMemDepthAsMask()),
90  _imagePreDecimation(Parameters::defaultMemImagePreDecimation()),
91  _imagePostDecimation(Parameters::defaultMemImagePostDecimation()),
92  _compressionParallelized(Parameters::defaultMemCompressionParallelized()),
93  _laserScanDownsampleStepSize(Parameters::defaultMemLaserScanDownsampleStepSize()),
94  _laserScanVoxelSize(Parameters::defaultMemLaserScanVoxelSize()),
95  _laserScanNormalK(Parameters::defaultMemLaserScanNormalK()),
96  _laserScanNormalRadius(Parameters::defaultMemLaserScanNormalRadius()),
97  _reextractLoopClosureFeatures(Parameters::defaultRGBDLoopClosureReextractFeatures()),
98  _localBundleOnLoopClosure(Parameters::defaultRGBDLocalBundleOnLoopClosure()),
99  _rehearsalMaxDistance(Parameters::defaultRGBDLinearUpdate()),
100  _rehearsalMaxAngle(Parameters::defaultRGBDAngularUpdate()),
101  _rehearsalWeightIgnoredWhileMoving(Parameters::defaultMemRehearsalWeightIgnoredWhileMoving()),
102  _useOdometryFeatures(Parameters::defaultMemUseOdomFeatures()),
103  _createOccupancyGrid(Parameters::defaultRGBDCreateOccupancyGrid()),
104  _visMaxFeatures(Parameters::defaultVisMaxFeatures()),
105  _visCorType(Parameters::defaultVisCorType()),
106  _imagesAlreadyRectified(Parameters::defaultRtabmapImagesAlreadyRectified()),
107  _rectifyOnlyFeatures(Parameters::defaultRtabmapRectifyOnlyFeatures()),
108  _covOffDiagonalIgnored(Parameters::defaultMemCovOffDiagIgnored()),
109  _idCount(kIdStart),
110  _idMapCount(kIdStart),
111  _lastSignature(0),
112  _lastGlobalLoopClosureId(0),
113  _memoryChanged(false),
114  _linksChanged(false),
115  _signaturesAdded(0),
116 
117  _badSignRatio(Parameters::defaultKpBadSignRatio()),
118  _tfIdfLikelihoodUsed(Parameters::defaultKpTfIdfLikelihoodUsed()),
119  _parallelized(Parameters::defaultKpParallelized())
120 {
121  _feature2D = Feature2D::create(parameters);
122  _vwd = new VWDictionary(parameters);
123  _registrationPipeline = Registration::create(parameters);
124 
125  // for local scan matching, correspondences ratio should be two times higher as we expect more matches
126  float corRatio = Parameters::defaultIcpCorrespondenceRatio();
127  Parameters::parse(parameters, Parameters::kIcpCorrespondenceRatio(), corRatio);
128  ParametersMap paramsMulti = parameters;
129  paramsMulti.insert(ParametersPair(Parameters::kIcpCorrespondenceRatio(), uNumber2Str(corRatio>=0.5f?1.0f:corRatio*2.0f)));
130  if(corRatio >= 0.5)
131  {
132  UWARN( "%s is >=0.5, which sets correspondence ratio for proximity detection using "
133  "laser scans to 100% (2 x Ratio). You may lower the ratio to accept proximity "
134  "detection with not full scans overlapping.", Parameters::kIcpCorrespondenceRatio().c_str());
135  }
136  _registrationIcpMulti = new RegistrationIcp(paramsMulti);
137 
138  _occupancy = new OccupancyGrid(parameters);
139  this->parseParameters(parameters);
140 }
141 
142 bool Memory::init(const std::string & dbUrl, bool dbOverwritten, const ParametersMap & parameters, bool postInitClosingEvents)
143 {
144  if(postInitClosingEvents) UEventsManager::post(new RtabmapEventInit(RtabmapEventInit::kInitializing));
145 
146  UDEBUG("");
147  this->parseParameters(parameters);
148 
149  if(postInitClosingEvents) UEventsManager::post(new RtabmapEventInit("Clearing memory..."));
150  DBDriver * tmpDriver = 0;
151  if((!_memoryChanged && !_linksChanged) || dbOverwritten)
152  {
153  if(_dbDriver)
154  {
155  tmpDriver = _dbDriver;
156  _dbDriver = 0; // HACK for the clear() below to think that there is no db
157  }
158  }
159  else if(!_memoryChanged && _linksChanged)
160  {
161  _dbDriver->setTimestampUpdateEnabled(false); // update links only
162  }
163  this->clear();
164  if(postInitClosingEvents) UEventsManager::post(new RtabmapEventInit("Clearing memory, done!"));
165 
166  if(tmpDriver)
167  {
168  _dbDriver = tmpDriver;
169  }
170 
171  if(_dbDriver)
172  {
173  if(postInitClosingEvents) UEventsManager::post(new RtabmapEventInit("Closing database connection..."));
174  _dbDriver->closeConnection();
175  if(postInitClosingEvents) UEventsManager::post(new RtabmapEventInit("Closing database connection, done!"));
176  }
177 
178  if(_dbDriver == 0)
179  {
180  _dbDriver = DBDriver::create(parameters);
181  }
182 
183  bool success = true;
184  if(_dbDriver)
185  {
186  _dbDriver->setTimestampUpdateEnabled(true); // make sure that timestamp update is enabled (may be disabled above)
187  success = false;
188  if(postInitClosingEvents) UEventsManager::post(new RtabmapEventInit(std::string("Connecting to database \"") + dbUrl + "\"..."));
189  if(_dbDriver->openConnection(dbUrl, dbOverwritten))
190  {
191  success = true;
192  if(postInitClosingEvents) UEventsManager::post(new RtabmapEventInit(std::string("Connecting to database \"") + dbUrl + "\", done!"));
193  }
194  else
195  {
196  if(postInitClosingEvents) UEventsManager::post(new RtabmapEventInit(RtabmapEventInit::kError, std::string("Connecting to database ") + dbUrl + ", path is invalid!"));
197  }
198  }
199 
200  loadDataFromDb(postInitClosingEvents);
201 
202  if(postInitClosingEvents) UEventsManager::post(new RtabmapEventInit(RtabmapEventInit::kInitialized));
203 
204  return success;
205 }
206 
207 void Memory::loadDataFromDb(bool postInitClosingEvents)
208 {
209  if(_dbDriver && _dbDriver->isConnected())
210  {
211  bool loadAllNodesInWM = Parameters::defaultMemInitWMWithAllNodes();
212  Parameters::parse(parameters_, Parameters::kMemInitWMWithAllNodes(), loadAllNodesInWM);
213 
214  // Load the last working memory...
215  std::list<Signature*> dbSignatures;
216 
217  if(loadAllNodesInWM)
218  {
219  if(postInitClosingEvents) UEventsManager::post(new RtabmapEventInit(std::string("Loading all nodes to WM...")));
220  std::set<int> ids;
221  _dbDriver->getAllNodeIds(ids, true);
222  _dbDriver->loadSignatures(std::list<int>(ids.begin(), ids.end()), dbSignatures);
223  }
224  else
225  {
226  // load previous session working memory
227  if(postInitClosingEvents) UEventsManager::post(new RtabmapEventInit(std::string("Loading last nodes to WM...")));
228  _dbDriver->loadLastNodes(dbSignatures);
229  }
230  for(std::list<Signature*>::reverse_iterator iter=dbSignatures.rbegin(); iter!=dbSignatures.rend(); ++iter)
231  {
232  // ignore bad signatures
233  if(!((*iter)->isBadSignature() && _badSignaturesIgnored))
234  {
235  // insert all in WM
236  // Note: it doesn't make sense to keep last STM images
237  // of the last session in the new STM because they can be
238  // only linked with the ones of the current session by
239  // global loop closures.
240  _signatures.insert(std::pair<int, Signature *>((*iter)->id(), *iter));
241  _workingMem.insert(std::make_pair((*iter)->id(), UTimer::now()));
242  }
243  else
244  {
245  delete *iter;
246  }
247  }
248  if(postInitClosingEvents) UEventsManager::post(new RtabmapEventInit(std::string("Loading nodes to WM, done! (") + uNumber2Str(int(_workingMem.size() + _stMem.size())) + " loaded)"));
249 
250  // Assign the last signature
251  if(_stMem.size()>0)
252  {
253  _lastSignature = uValue(_signatures, *_stMem.rbegin(), (Signature*)0);
254  }
255  else if(_workingMem.size()>0)
256  {
257  _lastSignature = uValue(_signatures, _workingMem.rbegin()->first, (Signature*)0);
258  }
259 
260  // Last id
261  _dbDriver->getLastNodeId(_idCount);
262  _idMapCount = _lastSignature?_lastSignature->mapId()+1:kIdStart;
263 
264  // Now load the dictionary if we have a connection
265  if(postInitClosingEvents) UEventsManager::post(new RtabmapEventInit("Loading dictionary..."));
266  UDEBUG("Loading dictionary...");
267  if(loadAllNodesInWM)
268  {
269  UDEBUG("load all referenced words in working memory");
270  // load all referenced words in working memory
271  std::set<int> wordIds;
272  const std::map<int, Signature *> & signatures = this->getSignatures();
273  for(std::map<int, Signature *>::const_iterator i=signatures.begin(); i!=signatures.end(); ++i)
274  {
275  const std::multimap<int, cv::KeyPoint> & words = i->second->getWords();
276  std::list<int> keys = uUniqueKeys(words);
277  for(std::list<int>::iterator iter=keys.begin(); iter!=keys.end(); ++iter)
278  {
279  if(*iter > 0)
280  {
281  wordIds.insert(*iter);
282  }
283  }
284  }
285 
286  UDEBUG("load words %d", (int)wordIds.size());
287  if(_vwd->isIncremental())
288  {
289  if(wordIds.size())
290  {
291  std::list<VisualWord*> words;
292  _dbDriver->loadWords(wordIds, words);
293  for(std::list<VisualWord*>::iterator iter = words.begin(); iter!=words.end(); ++iter)
294  {
295  _vwd->addWord(*iter);
296  }
297  // Get Last word id
298  int id = 0;
299  _dbDriver->getLastWordId(id);
300  _vwd->setLastWordId(id);
301  }
302  }
303  else
304  {
305  _dbDriver->load(_vwd, false);
306  }
307  }
308  else
309  {
310  UDEBUG("load words");
311  // load the last dictionary
312  _dbDriver->load(_vwd, _vwd->isIncremental());
313  }
314  UDEBUG("%d words loaded!", _vwd->getUnusedWordsSize());
315  _vwd->update();
316  if(postInitClosingEvents) UEventsManager::post(new RtabmapEventInit(uFormat("Loading dictionary, done! (%d words)", (int)_vwd->getUnusedWordsSize())));
317 
318  if(postInitClosingEvents) UEventsManager::post(new RtabmapEventInit(std::string("Adding word references...")));
319  // Enable loaded signatures
320  const std::map<int, Signature *> & signatures = this->getSignatures();
321  for(std::map<int, Signature *>::const_iterator i=signatures.begin(); i!=signatures.end(); ++i)
322  {
323  Signature * s = this->_getSignature(i->first);
324  UASSERT(s != 0);
325 
326  const std::multimap<int, cv::KeyPoint> & words = s->getWords();
327  if(words.size())
328  {
329  UDEBUG("node=%d, word references=%d", s->id(), words.size());
330  for(std::multimap<int, cv::KeyPoint>::const_iterator iter = words.begin(); iter!=words.end(); ++iter)
331  {
332  if(iter->first > 0)
333  {
334  _vwd->addWordRef(iter->first, i->first);
335  }
336  }
337  s->setEnabled(true);
338  }
339  }
340  if(postInitClosingEvents) UEventsManager::post(new RtabmapEventInit(uFormat("Adding word references, done! (%d)", _vwd->getTotalActiveReferences())));
341 
342  if(_vwd->getUnusedWordsSize() && _vwd->isIncremental())
343  {
344  UWARN("_vwd->getUnusedWordsSize() must be empty... size=%d", _vwd->getUnusedWordsSize());
345  }
346  UDEBUG("Total word references added = %d", _vwd->getTotalActiveReferences());
347 
348  if(_lastSignature == 0)
349  {
350  // Memory is empty, save parameters
351  ParametersMap parameters = Parameters::getDefaultParameters();
352  uInsert(parameters, parameters_);
353  parameters.erase(Parameters::kRtabmapWorkingDirectory()); // don't save working directory as it is machine dependent
354  UDEBUG("");
355  _dbDriver->addInfoAfterRun(0, 0, 0, 0, 0, parameters);
356  }
357  }
358  else
359  {
360  _idCount = kIdStart;
361  _idMapCount = kIdStart;
362  }
363 
364  _workingMem.insert(std::make_pair(kIdVirtual, 0));
365 
366  UDEBUG("ids start with %d", _idCount+1);
367  UDEBUG("map ids start with %d", _idMapCount);
368 }
369 
370 void Memory::close(bool databaseSaved, bool postInitClosingEvents, const std::string & ouputDatabasePath)
371 {
372  UINFO("databaseSaved=%d, postInitClosingEvents=%d", databaseSaved?1:0, postInitClosingEvents?1:0);
373  if(postInitClosingEvents) UEventsManager::post(new RtabmapEventInit(RtabmapEventInit::kClosing));
374 
375  bool databaseNameChanged = false;
376  if(databaseSaved && _dbDriver)
377  {
378  databaseNameChanged = ouputDatabasePath.size() && _dbDriver->getUrl().size() && _dbDriver->getUrl().compare(ouputDatabasePath) != 0?true:false;
379  }
380 
381  if(!databaseSaved || (!_memoryChanged && !_linksChanged && !databaseNameChanged))
382  {
383  if(postInitClosingEvents) UEventsManager::post(new RtabmapEventInit(uFormat("No changes added to database.")));
384 
385  UINFO("No changes added to database.");
386  if(_dbDriver)
387  {
388  if(postInitClosingEvents) UEventsManager::post(new RtabmapEventInit(uFormat("Closing database \"%s\"...", _dbDriver->getUrl().c_str())));
389  _dbDriver->closeConnection(false, ouputDatabasePath);
390  delete _dbDriver;
391  _dbDriver = 0;
392  if(postInitClosingEvents) UEventsManager::post(new RtabmapEventInit("Closing database, done!"));
393  }
394  if(postInitClosingEvents) UEventsManager::post(new RtabmapEventInit("Clearing memory..."));
395  this->clear();
396  if(postInitClosingEvents) UEventsManager::post(new RtabmapEventInit("Clearing memory, done!"));
397  }
398  else
399  {
400  UINFO("Saving memory...");
401  if(postInitClosingEvents) UEventsManager::post(new RtabmapEventInit("Saving memory..."));
402  if(!_memoryChanged && _linksChanged && _dbDriver)
403  {
404  // don't update the time stamps!
405  UDEBUG("");
406  _dbDriver->setTimestampUpdateEnabled(false);
407  }
408  this->clear();
409  if(_dbDriver)
410  {
411  _dbDriver->emptyTrashes();
412  if(postInitClosingEvents) UEventsManager::post(new RtabmapEventInit("Saving memory, done!"));
413  if(postInitClosingEvents) UEventsManager::post(new RtabmapEventInit(uFormat("Closing database \"%s\"...", _dbDriver->getUrl().c_str())));
414  _dbDriver->closeConnection(true, ouputDatabasePath);
415  delete _dbDriver;
416  _dbDriver = 0;
417  if(postInitClosingEvents) UEventsManager::post(new RtabmapEventInit("Closing database, done!"));
418  }
419  else
420  {
421  if(postInitClosingEvents) UEventsManager::post(new RtabmapEventInit("Saving memory, done!"));
422  }
423  }
424  if(postInitClosingEvents) UEventsManager::post(new RtabmapEventInit(RtabmapEventInit::kClosed));
425 }
426 
427 Memory::~Memory()
428 {
429  this->close();
430 
431  if(_dbDriver)
432  {
433  UWARN("Please call Memory::close() before");
434  }
435  delete _feature2D;
436  delete _vwd;
437  delete _registrationPipeline;
438  delete _registrationIcpMulti;
439  delete _occupancy;
440 }
441 
442 void Memory::parseParameters(const ParametersMap & parameters)
443 {
444  uInsert(parameters_, parameters);
445  ParametersMap params = parameters;
446 
447  UDEBUG("");
448  ParametersMap::const_iterator iter;
449 
450  Parameters::parse(params, Parameters::kMemBinDataKept(), _binDataKept);
451  Parameters::parse(params, Parameters::kMemRawDescriptorsKept(), _rawDescriptorsKept);
452  Parameters::parse(params, Parameters::kMemSaveDepth16Format(), _saveDepth16Format);
453  Parameters::parse(params, Parameters::kMemReduceGraph(), _reduceGraph);
454  Parameters::parse(params, Parameters::kMemNotLinkedNodesKept(), _notLinkedNodesKeptInDb);
455  Parameters::parse(params, Parameters::kMemIntermediateNodeDataKept(), _saveIntermediateNodeData);
456  Parameters::parse(params, Parameters::kMemRehearsalIdUpdatedToNewOne(), _idUpdatedToNewOneRehearsal);
457  Parameters::parse(params, Parameters::kMemGenerateIds(), _generateIds);
458  Parameters::parse(params, Parameters::kMemBadSignaturesIgnored(), _badSignaturesIgnored);
459  Parameters::parse(params, Parameters::kMemMapLabelsAdded(), _mapLabelsAdded);
460  Parameters::parse(params, Parameters::kMemRehearsalSimilarity(), _similarityThreshold);
461  Parameters::parse(params, Parameters::kMemRecentWmRatio(), _recentWmRatio);
462  Parameters::parse(params, Parameters::kMemTransferSortingByWeightId(), _transferSortingByWeightId);
463  Parameters::parse(params, Parameters::kMemSTMSize(), _maxStMemSize);
464  Parameters::parse(params, Parameters::kMemDepthAsMask(), _depthAsMask);
465  Parameters::parse(params, Parameters::kMemImagePreDecimation(), _imagePreDecimation);
466  Parameters::parse(params, Parameters::kMemImagePostDecimation(), _imagePostDecimation);
467  Parameters::parse(params, Parameters::kMemCompressionParallelized(), _compressionParallelized);
468  Parameters::parse(params, Parameters::kMemLaserScanDownsampleStepSize(), _laserScanDownsampleStepSize);
469  Parameters::parse(params, Parameters::kMemLaserScanVoxelSize(), _laserScanVoxelSize);
470  Parameters::parse(params, Parameters::kMemLaserScanNormalK(), _laserScanNormalK);
471  Parameters::parse(params, Parameters::kMemLaserScanNormalRadius(), _laserScanNormalRadius);
472  Parameters::parse(params, Parameters::kRGBDLoopClosureReextractFeatures(), _reextractLoopClosureFeatures);
473  Parameters::parse(params, Parameters::kRGBDLocalBundleOnLoopClosure(), _localBundleOnLoopClosure);
474  Parameters::parse(params, Parameters::kRGBDLinearUpdate(), _rehearsalMaxDistance);
475  Parameters::parse(params, Parameters::kRGBDAngularUpdate(), _rehearsalMaxAngle);
476  Parameters::parse(params, Parameters::kMemRehearsalWeightIgnoredWhileMoving(), _rehearsalWeightIgnoredWhileMoving);
477  Parameters::parse(params, Parameters::kMemUseOdomFeatures(), _useOdometryFeatures);
478  Parameters::parse(params, Parameters::kRGBDCreateOccupancyGrid(), _createOccupancyGrid);
479  Parameters::parse(params, Parameters::kVisMaxFeatures(), _visMaxFeatures);
480  Parameters::parse(params, Parameters::kVisCorType(), _visCorType);
481  if(_visCorType != 0)
482  {
483  UWARN("%s is not 0 (Features Matching), the only approach supported for loop closure transformation estimation. Setting to 0...",
484  Parameters::kVisCorType().c_str());
485  _visCorType = 0;
486  uInsert(parameters_, ParametersPair(Parameters::kVisCorType(), "0"));
487  uInsert(params, ParametersPair(Parameters::kVisCorType(), "0"));
488  }
489  Parameters::parse(params, Parameters::kRtabmapImagesAlreadyRectified(), _imagesAlreadyRectified);
490  Parameters::parse(params, Parameters::kRtabmapRectifyOnlyFeatures(), _rectifyOnlyFeatures);
491  Parameters::parse(params, Parameters::kMemCovOffDiagIgnored(), _covOffDiagonalIgnored);
492 
493 
494  UASSERT_MSG(_maxStMemSize >= 0, uFormat("value=%d", _maxStMemSize).c_str());
495  UASSERT_MSG(_similarityThreshold >= 0.0f && _similarityThreshold <= 1.0f, uFormat("value=%f", _similarityThreshold).c_str());
496  UASSERT_MSG(_recentWmRatio >= 0.0f && _recentWmRatio <= 1.0f, uFormat("value=%f", _recentWmRatio).c_str());
497  if(_imagePreDecimation == 0)
498  {
499  _imagePreDecimation = 1;
500  }
501  if(_imagePostDecimation == 0)
502  {
503  _imagePostDecimation = 1;
504  }
505  UASSERT(_rehearsalMaxDistance >= 0.0f);
506  UASSERT(_rehearsalMaxAngle >= 0.0f);
507 
508  if(_dbDriver)
509  {
510  _dbDriver->parseParameters(params);
511  }
512 
513  // Keypoint stuff
514  if(_vwd)
515  {
516  _vwd->parseParameters(params);
517  }
518 
519  Parameters::parse(params, Parameters::kKpTfIdfLikelihoodUsed(), _tfIdfLikelihoodUsed);
520  Parameters::parse(params, Parameters::kKpParallelized(), _parallelized);
521  Parameters::parse(params, Parameters::kKpBadSignRatio(), _badSignRatio);
522 
523  //Keypoint detector
524  UASSERT(_feature2D != 0);
525  Feature2D::Type detectorStrategy = Feature2D::kFeatureUndef;
526  if((iter=params.find(Parameters::kKpDetectorStrategy())) != params.end())
527  {
528  detectorStrategy = (Feature2D::Type)std::atoi((*iter).second.c_str());
529  }
530  if(detectorStrategy!=Feature2D::kFeatureUndef && detectorStrategy!=_feature2D->getType())
531  {
532  if(_vwd->getVisualWords().size())
533  {
534  UWARN("new detector strategy %d while the vocabulary is already created. This may give problems if feature descriptors are not the same type than the one used in the current vocabulary (a memory reset would be required if so).", int(detectorStrategy));
535  }
536  else
537  {
538  UINFO("new detector strategy %d.", int(detectorStrategy));
539  }
540  if(_feature2D)
541  {
542  delete _feature2D;
543  _feature2D = 0;
544  }
545 
546  _feature2D = Feature2D::create(detectorStrategy, parameters_);
547  }
548  else if(_feature2D)
549  {
550  _feature2D->parseParameters(params);
551  }
552 
553  Registration::Type regStrategy = Registration::kTypeUndef;
554  if((iter=params.find(Parameters::kRegStrategy())) != params.end())
555  {
556  regStrategy = (Registration::Type)std::atoi((*iter).second.c_str());
557  }
558  if(regStrategy!=Registration::kTypeUndef)
559  {
560  UDEBUG("new registration strategy %d", int(regStrategy));
561  if(_registrationPipeline)
562  {
563  delete _registrationPipeline;
564  _registrationPipeline = 0;
565  }
566 
567  _registrationPipeline = Registration::create(regStrategy, parameters_);
568  }
569  else if(_registrationPipeline)
570  {
571  _registrationPipeline->parseParameters(params);
572  }
573 
574  if(_registrationIcpMulti)
575  {
576  if(uContains(params, Parameters::kIcpCorrespondenceRatio()))
577  {
578  // for local scan matching, correspondences ratio should be two times higher as we expect more matches
579  float corRatio = Parameters::defaultIcpCorrespondenceRatio();
580  Parameters::parse(parameters, Parameters::kIcpCorrespondenceRatio(), corRatio);
581  ParametersMap paramsMulti = params;
582  paramsMulti.insert(ParametersPair(Parameters::kIcpCorrespondenceRatio(), uNumber2Str(corRatio>=0.5f?1.0f:corRatio*2.0f)));
583  if(corRatio >= 0.5)
584  {
585  UWARN( "%s is >=0.5, which sets correspondence ratio for proximity detection using "
586  "laser scans to 100% (2 x Ratio). You may lower the ratio to accept proximity "
587  "detection with not full scans overlapping.", Parameters::kIcpCorrespondenceRatio().c_str());
588  }
589  _registrationIcpMulti->parseParameters(paramsMulti);
590  }
591  else
592  {
593  _registrationIcpMulti->parseParameters(params);
594  }
595  }
596 
597  if(_occupancy)
598  {
599  _occupancy->parseParameters(params);
600  }
601 
602  // do this after all params are parsed
603  // SLAM mode vs Localization mode
604  iter = params.find(Parameters::kMemIncrementalMemory());
605  if(iter != params.end())
606  {
607  bool value = uStr2Bool(iter->second.c_str());
608  if(value == false && _incrementalMemory)
609  {
610  // From SLAM to localization, change map id
611  this->incrementMapId();
612 
613  // The easiest way to make sure that the mapping session is saved
614  // is to save the memory in the database and reload it.
615  if((_memoryChanged || _linksChanged) && _dbDriver)
616  {
617  UWARN("Switching from Mapping to Localization mode, the database will be saved and reloaded.");
618  bool memoryChanged = _memoryChanged;
619  bool linksChanged = _linksChanged;
620  this->clear();
621  _memoryChanged = memoryChanged;
622  _linksChanged = linksChanged;
623  this->loadDataFromDb(false);
624  UWARN("Switching from Mapping to Localization mode, the database is reloaded!");
625  }
626  }
627  _incrementalMemory = value;
628  }
629 
630  if(_useOdometryFeatures)
631  {
632  int visFeatureType = Parameters::defaultVisFeatureType();
633  int kpDetectorStrategy = Parameters::defaultKpDetectorStrategy();
634  Parameters::parse(parameters_, Parameters::kVisFeatureType(), visFeatureType);
635  Parameters::parse(parameters_, Parameters::kKpDetectorStrategy(), kpDetectorStrategy);
636  if(visFeatureType != kpDetectorStrategy)
637  {
638  UWARN("%s is enabled, but %s and %s parameters are not the same! Disabling %s...",
639  Parameters::kMemUseOdomFeatures().c_str(),
640  Parameters::kVisFeatureType().c_str(),
641  Parameters::kKpDetectorStrategy().c_str(),
642  Parameters::kMemUseOdomFeatures().c_str());
643  _useOdometryFeatures = false;
644  uInsert(parameters_, ParametersPair(Parameters::kMemUseOdomFeatures(), "false"));
645  }
646  }
647 }
648 
649 void Memory::preUpdate()
650 {
651  _signaturesAdded = 0;
652  if(_vwd->isIncremental())
653  {
654  this->cleanUnusedWords();
655  }
656  if(_vwd && !_parallelized)
657  {
658  //When parallelized, it is done in CreateSignature
659  _vwd->update();
660  }
661 }
662 
663 bool Memory::update(
664  const SensorData & data,
665  Statistics * stats)
666 {
667  return update(data, Transform(), cv::Mat(), std::vector<float>(), stats);
668 }
669 
670 bool Memory::update(
671  const SensorData & data,
672  const Transform & pose,
673  const cv::Mat & covariance,
674  const std::vector<float> & velocity,
675  Statistics * stats)
676 {
677  UDEBUG("");
678  UTimer timer;
679  UTimer totalTimer;
680  timer.start();
681  float t;
682 
683  //============================================================
684  // Pre update...
685  //============================================================
686  UDEBUG("pre-updating...");
687  this->preUpdate();
688  t=timer.ticks()*1000;
689  if(stats) stats->addStatistic(Statistics::kTimingMemPre_update(), t);
690  UDEBUG("time preUpdate=%f ms", t);
691 
692  //============================================================
693  // Create a signature with the image received.
694  //============================================================
695  Signature * signature = this->createSignature(data, pose, stats);
696  if (signature == 0)
697  {
698  UERROR("Failed to create a signature...");
699  return false;
700  }
701  if(velocity.size()==6)
702  {
703  signature->setVelocity(velocity[0], velocity[1], velocity[2], velocity[3], velocity[4], velocity[5]);
704  }
705 
706  t=timer.ticks()*1000;
707  if(stats) stats->addStatistic(Statistics::kTimingMemSignature_creation(), t);
708  UDEBUG("time creating signature=%f ms", t);
709 
710  // It will be added to the short-term memory, no need to delete it...
711  this->addSignatureToStm(signature, covariance);
712 
713  _lastSignature = signature;
714 
715  //============================================================
716  // Rehearsal step...
717  // Compare with the X last signatures. If different, add this
718  // signature like a parent to the memory tree, otherwise add
719  // it as a child to the similar signature.
720  //============================================================
721  if(_incrementalMemory)
722  {
723  if(_similarityThreshold < 1.0f)
724  {
725  this->rehearsal(signature, stats);
726  }
727  t=timer.ticks()*1000;
728  if(stats) stats->addStatistic(Statistics::kTimingMemRehearsal(), t);
729  UDEBUG("time rehearsal=%f ms", t);
730  }
731  else
732  {
733  if(_workingMem.size() <= 1)
734  {
735  UWARN("The working memory is empty and the memory is not "
736  "incremental (Mem/IncrementalMemory=False), no loop closure "
737  "can be detected! Please set Mem/IncrementalMemory=true to increase "
738  "the memory with new images or decrease the STM size (which is %d "
739  "including the new one added).", (int)_stMem.size());
740  }
741  }
742 
743  //============================================================
744  // Transfer the oldest signature of the short-term memory to the working memory
745  //============================================================
746  int notIntermediateNodesCount = 0;
747  for(std::set<int>::iterator iter=_stMem.begin(); iter!=_stMem.end(); ++iter)
748  {
749  const Signature * s = this->getSignature(*iter);
750  UASSERT(s != 0);
751  if(s->getWeight() >= 0)
752  {
753  ++notIntermediateNodesCount;
754  }
755  }
756  std::map<int, int> reducedIds;
757  while(_stMem.size() && _maxStMemSize>0 && notIntermediateNodesCount > _maxStMemSize)
758  {
759  int id = *_stMem.begin();
760  Signature * s = this->_getSignature(id);
761  UASSERT(s != 0);
762  if(s->getWeight() >= 0)
763  {
764  --notIntermediateNodesCount;
765  }
766 
767  int reducedTo = 0;
768  moveSignatureToWMFromSTM(id, &reducedTo);
769 
770  if(reducedTo > 0)
771  {
772  reducedIds.insert(std::make_pair(id, reducedTo));
773  }
774  }
775  if(stats) stats->setReducedIds(reducedIds);
776 
777  if(!_memoryChanged && _incrementalMemory)
778  {
779  _memoryChanged = true;
780  }
781 
782  UDEBUG("totalTimer = %fs", totalTimer.ticks());
783 
784  return true;
785 }
786 
787 void Memory::addSignatureToStm(Signature * signature, const cv::Mat & covariance)
788 {
789  UTimer timer;
790  // add signature on top of the short-term memory
791  if(signature)
792  {
793  UDEBUG("adding %d", signature->id());
794  // Update neighbors
795  if(_stMem.size())
796  {
797  if(_signatures.at(*_stMem.rbegin())->mapId() == signature->mapId())
798  {
799  Transform motionEstimate;
800  if(!signature->getPose().isNull() &&
801  !_signatures.at(*_stMem.rbegin())->getPose().isNull())
802  {
803  UASSERT(covariance.cols == 6 && covariance.rows == 6 && covariance.type() == CV_64FC1);
804  double maxAngVar = uMax3(covariance.at<double>(3,3), covariance.at<double>(4,4), covariance.at<double>(5,5));
805  if(maxAngVar != 1.0 && maxAngVar > 0.1)
806  {
807  UWARN("Very large angular variance (%f) detected! Please fix odometry "
808  "twist covariance, otherwise poor graph optimizations are "
809  "expected and wrong loop closure detections creating a lot "
810  "of errors in the map could be accepted.", maxAngVar);
811  }
812 
813  cv::Mat infMatrix;
814  if(_covOffDiagonalIgnored)
815  {
816  infMatrix = cv::Mat::zeros(6,6,CV_64FC1);
817  infMatrix.at<double>(0,0) = 1.0 / covariance.at<double>(0,0);
818  infMatrix.at<double>(1,1) = 1.0 / covariance.at<double>(1,1);
819  infMatrix.at<double>(2,2) = 1.0 / covariance.at<double>(2,2);
820  infMatrix.at<double>(3,3) = 1.0 / covariance.at<double>(3,3);
821  infMatrix.at<double>(4,4) = 1.0 / covariance.at<double>(4,4);
822  infMatrix.at<double>(5,5) = 1.0 / covariance.at<double>(5,5);
823  }
824  else
825  {
826  infMatrix = covariance.inv();
827  }
828  if((uIsFinite(covariance.at<double>(0,0)) && covariance.at<double>(0,0)>0.0) &&
829  !(uIsFinite(infMatrix.at<double>(0,0)) && infMatrix.at<double>(0,0)>0.0))
830  {
831  UERROR("Failed to invert the covariance matrix! Covariance matrix should be invertible!");
832  std::cout << "Covariance: " << covariance << std::endl;
833  infMatrix = cv::Mat::eye(6,6,CV_64FC1);
834  }
835  motionEstimate = _signatures.at(*_stMem.rbegin())->getPose().inverse() * signature->getPose();
836  _signatures.at(*_stMem.rbegin())->addLink(Link(*_stMem.rbegin(), signature->id(), Link::kNeighbor, motionEstimate, infMatrix));
837  signature->addLink(Link(signature->id(), *_stMem.rbegin(), Link::kNeighbor, motionEstimate.inverse(), infMatrix));
838  }
839  else
840  {
841  _signatures.at(*_stMem.rbegin())->addLink(Link(*_stMem.rbegin(), signature->id(), Link::kNeighbor, Transform()));
842  signature->addLink(Link(signature->id(), *_stMem.rbegin(), Link::kNeighbor, Transform()));
843  }
844  UDEBUG("Min STM id = %d", *_stMem.begin());
845  }
846  else
847  {
848  UDEBUG("Ignoring neighbor link between %d and %d because they are not in the same map! (%d vs %d)",
849  *_stMem.rbegin(), signature->id(),
850  _signatures.at(*_stMem.rbegin())->mapId(), signature->mapId());
851 
852  if(_mapLabelsAdded && isIncremental())
853  {
854  //Tag the first node of the map
855  std::string tag = uFormat("map%d", signature->mapId());
856  if(getSignatureIdByLabel(tag, false) == 0)
857  {
858  UINFO("Tagging node %d with label \"%s\"", signature->id(), tag.c_str());
859  signature->setLabel(tag);
860  }
861  }
862  }
863  }
864  else if(_mapLabelsAdded && isIncremental())
865  {
866  //Tag the first node of the map
867  std::string tag = uFormat("map%d", signature->mapId());
868  if(getSignatureIdByLabel(tag, false) == 0)
869  {
870  UINFO("Tagging node %d with label \"%s\"", signature->id(), tag.c_str());
871  signature->setLabel(tag);
872  }
873  }
874 
875  _signatures.insert(_signatures.end(), std::pair<int, Signature *>(signature->id(), signature));
876  _stMem.insert(_stMem.end(), signature->id());
877  ++_signaturesAdded;
878 
879  if(_vwd)
880  {
881  UDEBUG("%d words ref for the signature %d", signature->getWords().size(), signature->id());
882  }
883  if(signature->getWords().size())
884  {
885  signature->setEnabled(true);
886  }
887  }
888 
889  UDEBUG("time = %fs", timer.ticks());
890 }
891 
892 void Memory::addSignatureToWmFromLTM(Signature * signature)
893 {
894  if(signature)
895  {
896  UDEBUG("Inserting node %d in WM...", signature->id());
897  _workingMem.insert(std::make_pair(signature->id(), UTimer::now()));
898  _signatures.insert(std::pair<int, Signature*>(signature->id(), signature));
899  ++_signaturesAdded;
900  }
901  else
902  {
903  UERROR("Signature is null ?!?");
904  }
905 }
906 
907 void Memory::moveSignatureToWMFromSTM(int id, int * reducedTo)
908 {
909  UDEBUG("Inserting node %d from STM in WM...", id);
910  UASSERT(_stMem.find(id) != _stMem.end());
911  Signature * s = this->_getSignature(id);
912  UASSERT(s!=0);
913 
914  if(_reduceGraph)
915  {
916  bool merge = false;
917  const std::map<int, Link> & links = s->getLinks();
918  std::map<int, Link> neighbors;
919  for(std::map<int, Link>::const_iterator iter=links.begin(); iter!=links.end(); ++iter)
920  {
921  if(!merge)
922  {
923  merge = iter->second.to() < s->id() && // should be a parent->child link
924  iter->second.to() != iter->second.from() &&
925  iter->second.type() != Link::kNeighbor &&
926  iter->second.type() != Link::kNeighborMerged &&
927  iter->second.userDataCompressed().empty() &&
928  iter->second.type() != Link::kUndef &&
929  iter->second.type() != Link::kVirtualClosure;
930  if(merge)
931  {
932  UDEBUG("Reduce %d to %d", s->id(), iter->second.to());
933  if(reducedTo)
934  {
935  *reducedTo = iter->second.to();
936  }
937  }
938 
939  }
940  if(iter->second.type() == Link::kNeighbor)
941  {
942  neighbors.insert(*iter);
943  }
944  }
945  if(merge)
946  {
947  if(s->getLabel().empty())
948  {
949  for(std::map<int, Link>::const_iterator iter=links.begin(); iter!=links.end(); ++iter)
950  {
951  merge = true;
952  Signature * sTo = this->_getSignature(iter->first);
953  UASSERT(sTo!=0);
954  sTo->removeLink(s->id());
955  if(iter->second.type() != Link::kNeighbor &&
956  iter->second.type() != Link::kNeighborMerged &&
957  iter->second.type() != Link::kUndef)
958  {
959  // link to all neighbors
960  for(std::map<int, Link>::iterator jter=neighbors.begin(); jter!=neighbors.end(); ++jter)
961  {
962  if(!sTo->hasLink(jter->second.to()))
963  {
964  Link l = iter->second.inverse().merge(
965  jter->second,
966  iter->second.userDataCompressed().empty() && iter->second.type() != Link::kVirtualClosure?Link::kNeighborMerged:iter->second.type());
967  sTo->addLink(l);
968  Signature * sB = this->_getSignature(l.to());
969  UASSERT(sB!=0);
970  UASSERT(!sB->hasLink(l.to()));
971  sB->addLink(l.inverse());
972  }
973  }
974  }
975  }
976 
977  //remove neighbor links
978  std::map<int, Link> linksCopy = links;
979  for(std::map<int, Link>::iterator iter=linksCopy.begin(); iter!=linksCopy.end(); ++iter)
980  {
981  if(iter->second.type() == Link::kNeighbor ||
982  iter->second.type() == Link::kNeighborMerged)
983  {
984  s->removeLink(iter->first);
985  if(iter->second.type() == Link::kNeighbor)
986  {
987  if(_lastGlobalLoopClosureId == s->id())
988  {
989  _lastGlobalLoopClosureId = iter->first;
990  }
991  }
992  }
993  }
994 
995  this->moveToTrash(s, _notLinkedNodesKeptInDb);
996  s = 0;
997  }
998  }
999  }
1000  if(s != 0)
1001  {
1002  _workingMem.insert(_workingMem.end(), std::make_pair(*_stMem.begin(), UTimer::now()));
1003  _stMem.erase(*_stMem.begin());
1004  }
1005  // else already removed from STM/WM in moveToTrash()
1006 }
1007 
1008 const Signature * Memory::getSignature(int id) const
1009 {
1010  return _getSignature(id);
1011 }
1012 
1013 Signature * Memory::_getSignature(int id) const
1014 {
1015  return uValue(_signatures, id, (Signature*)0);
1016 }
1017 
1018 const VWDictionary * Memory::getVWDictionary() const
1019 {
1020  return _vwd;
1021 }
1022 
1023 std::map<int, Link> Memory::getNeighborLinks(
1024  int signatureId,
1025  bool lookInDatabase) const
1026 {
1027  std::map<int, Link> links;
1028  Signature * s = uValue(_signatures, signatureId, (Signature*)0);
1029  if(s)
1030  {
1031  const std::map<int, Link> & allLinks = s->getLinks();
1032  for(std::map<int, Link>::const_iterator iter = allLinks.begin(); iter!=allLinks.end(); ++iter)
1033  {
1034  if(iter->second.type() == Link::kNeighbor ||
1035  iter->second.type() == Link::kNeighborMerged)
1036  {
1037  links.insert(*iter);
1038  }
1039  }
1040  }
1041  else if(lookInDatabase && _dbDriver)
1042  {
1043  std::map<int, Link> neighbors;
1044  _dbDriver->loadLinks(signatureId, neighbors);
1045  for(std::map<int, Link>::iterator iter=neighbors.begin(); iter!=neighbors.end();)
1046  {
1047  if(iter->second.type() != Link::kNeighbor &&
1048  iter->second.type() != Link::kNeighborMerged)
1049  {
1050  neighbors.erase(iter++);
1051  }
1052  else
1053  {
1054  ++iter;
1055  }
1056  }
1057  }
1058  else
1059  {
1060  UWARN("Cannot find signature %d in memory", signatureId);
1061  }
1062  return links;
1063 }
1064 
1065 std::map<int, Link> Memory::getLoopClosureLinks(
1066  int signatureId,
1067  bool lookInDatabase) const
1068 {
1069  const Signature * s = this->getSignature(signatureId);
1070  std::map<int, Link> loopClosures;
1071  if(s)
1072  {
1073  const std::map<int, Link> & allLinks = s->getLinks();
1074  for(std::map<int, Link>::const_iterator iter = allLinks.begin(); iter!=allLinks.end(); ++iter)
1075  {
1076  if(iter->second.type() != Link::kNeighbor &&
1077  iter->second.type() != Link::kNeighborMerged &&
1078  iter->second.type() != Link::kPosePrior &&
1079  iter->second.type() != Link::kUndef)
1080  {
1081  loopClosures.insert(*iter);
1082  }
1083  }
1084  }
1085  else if(lookInDatabase && _dbDriver)
1086  {
1087  _dbDriver->loadLinks(signatureId, loopClosures);
1088  for(std::map<int, Link>::iterator iter=loopClosures.begin(); iter!=loopClosures.end();)
1089  {
1090  if(iter->second.type() == Link::kNeighbor ||
1091  iter->second.type() == Link::kNeighborMerged ||
1092  iter->second.type() == Link::kUndef )
1093  {
1094  loopClosures.erase(iter++);
1095  }
1096  else
1097  {
1098  ++iter;
1099  }
1100  }
1101  }
1102  return loopClosures;
1103 }
1104 
1105 std::map<int, Link> Memory::getLinks(
1106  int signatureId,
1107  bool lookInDatabase) const
1108 {
1109  std::map<int, Link> links;
1110  Signature * s = uValue(_signatures, signatureId, (Signature*)0);
1111  if(s)
1112  {
1113  links = s->getLinks();
1114  }
1115  else if(lookInDatabase && _dbDriver)
1116  {
1117  _dbDriver->loadLinks(signatureId, links, Link::kUndef);
1118  }
1119  else
1120  {
1121  UWARN("Cannot find signature %d in memory", signatureId);
1122  }
1123  return links;
1124 }
1125 
1126 std::multimap<int, Link> Memory::getAllLinks(bool lookInDatabase, bool ignoreNullLinks) const
1127 {
1128  std::multimap<int, Link> links;
1129 
1130  if(lookInDatabase && _dbDriver)
1131  {
1132  _dbDriver->getAllLinks(links, ignoreNullLinks);
1133  }
1134 
1135  for(std::map<int, Signature*>::const_iterator iter=_signatures.begin(); iter!=_signatures.end(); ++iter)
1136  {
1137  links.erase(iter->first);
1138  for(std::map<int, Link>::const_iterator jter=iter->second->getLinks().begin();
1139  jter!=iter->second->getLinks().end();
1140  ++jter)
1141  {
1142  if(!ignoreNullLinks || jter->second.isValid())
1143  {
1144  links.insert(std::make_pair(iter->first, jter->second));
1145  }
1146  }
1147  }
1148 
1149  return links;
1150 }
1151 
1152 
1153 // return map<Id,Margin>, including signatureId
1154 // maxCheckedInDatabase = -1 means no limit to check in database (default)
1155 // maxCheckedInDatabase = 0 means don't check in database
1156 std::map<int, int> Memory::getNeighborsId(
1157  int signatureId,
1158  int maxGraphDepth, // 0 means infinite margin
1159  int maxCheckedInDatabase, // default -1 (no limit)
1160  bool incrementMarginOnLoop, // default false
1161  bool ignoreLoopIds, // default false
1162  bool ignoreIntermediateNodes, // default false
1163  bool ignoreLocalSpaceLoopIds, // default false, ignored if ignoreLoopIds=true
1164  const std::set<int> & nodesSet,
1165  double * dbAccessTime
1166  ) const
1167 {
1168  UASSERT(maxGraphDepth >= 0);
1169  //UDEBUG("signatureId=%d, neighborsMargin=%d", signatureId, margin);
1170  if(dbAccessTime)
1171  {
1172  *dbAccessTime = 0;
1173  }
1174  std::map<int, int> ids;
1175  if(signatureId<=0)
1176  {
1177  return ids;
1178  }
1179  int nbLoadedFromDb = 0;
1180  std::list<int> curentMarginList;
1181  std::set<int> currentMargin;
1182  std::set<int> nextMargin;
1183  nextMargin.insert(signatureId);
1184  int m = 0;
1185  std::set<int> ignoredIds;
1186  while((maxGraphDepth == 0 || m < maxGraphDepth) && nextMargin.size())
1187  {
1188  // insert more recent first (priority to be loaded first from the database below if set)
1189  curentMarginList = std::list<int>(nextMargin.rbegin(), nextMargin.rend());
1190  nextMargin.clear();
1191 
1192  for(std::list<int>::iterator jter = curentMarginList.begin(); jter!=curentMarginList.end(); ++jter)
1193  {
1194  if(ids.find(*jter) == ids.end() && (nodesSet.empty() || nodesSet.find(*jter) != nodesSet.end()))
1195  {
1196  //UDEBUG("Added %d with margin %d", *jter, m);
1197  // Look up in STM/WM if all ids are here, if not... load them from the database
1198  const Signature * s = this->getSignature(*jter);
1199  std::map<int, Link> tmpLinks;
1200  const std::map<int, Link> * links = &tmpLinks;
1201  if(s)
1202  {
1203  if(!ignoreIntermediateNodes || s->getWeight() != -1)
1204  {
1205  ids.insert(std::pair<int, int>(*jter, m));
1206  }
1207  else
1208  {
1209  ignoredIds.insert(*jter);
1210  }
1211 
1212  links = &s->getLinks();
1213  }
1214  else if(maxCheckedInDatabase == -1 || (maxCheckedInDatabase > 0 && _dbDriver && nbLoadedFromDb < maxCheckedInDatabase))
1215  {
1216  ++nbLoadedFromDb;
1217  ids.insert(std::pair<int, int>(*jter, m));
1218 
1219  UTimer timer;
1220  _dbDriver->loadLinks(*jter, tmpLinks);
1221  if(dbAccessTime)
1222  {
1223  *dbAccessTime += timer.getElapsedTime();
1224  }
1225  }
1226 
1227  // links
1228  for(std::map<int, Link>::const_iterator iter=links->begin(); iter!=links->end(); ++iter)
1229  {
1230  if( !uContains(ids, iter->first) && ignoredIds.find(iter->first) == ignoredIds.end())
1231  {
1232  UASSERT(iter->second.type() != Link::kUndef);
1233  if(iter->second.type() == Link::kNeighbor ||
1234  iter->second.type() == Link::kNeighborMerged)
1235  {
1236  if(ignoreIntermediateNodes && s->getWeight()==-1)
1237  {
1238  // stay on the same margin
1239  if(currentMargin.insert(iter->first).second)
1240  {
1241  curentMarginList.push_back(iter->first);
1242  }
1243  }
1244  else
1245  {
1246  nextMargin.insert(iter->first);
1247  }
1248  }
1249  else if(!ignoreLoopIds && (!ignoreLocalSpaceLoopIds || iter->second.type()!=Link::kLocalSpaceClosure))
1250  {
1251  if(incrementMarginOnLoop)
1252  {
1253  nextMargin.insert(iter->first);
1254  }
1255  else
1256  {
1257  if(currentMargin.insert(iter->first).second)
1258  {
1259  curentMarginList.push_back(iter->first);
1260  }
1261  }
1262  }
1263  }
1264  }
1265  }
1266  }
1267  ++m;
1268  }
1269  return ids;
1270 }
1271 
1272 // return map<Id,sqrdDistance>, including signatureId
1273 std::map<int, float> Memory::getNeighborsIdRadius(
1274  int signatureId,
1275  float radius, // 0 means ignore radius
1276  const std::map<int, Transform> & optimizedPoses,
1277  int maxGraphDepth // 0 means infinite margin
1278  ) const
1279 {
1280  UASSERT(maxGraphDepth >= 0);
1281  UASSERT(uContains(optimizedPoses, signatureId));
1282  UASSERT(signatureId > 0);
1283  std::map<int, float> ids;
1284  std::map<int, float> checkedIds;
1285  std::list<int> curentMarginList;
1286  std::set<int> currentMargin;
1287  std::set<int> nextMargin;
1288  nextMargin.insert(signatureId);
1289  int m = 0;
1290  Transform referential = optimizedPoses.at(signatureId);
1291  UASSERT(!referential.isNull());
1292  float radiusSqrd = radius*radius;
1293  while((maxGraphDepth == 0 || m < maxGraphDepth) && nextMargin.size())
1294  {
1295  curentMarginList = std::list<int>(nextMargin.begin(), nextMargin.end());
1296  nextMargin.clear();
1297 
1298  for(std::list<int>::iterator jter = curentMarginList.begin(); jter!=curentMarginList.end(); ++jter)
1299  {
1300  if(checkedIds.find(*jter) == checkedIds.end())
1301  {
1302  //UDEBUG("Added %d with margin %d", *jter, m);
1303  // Look up in STM/WM if all ids are here, if not... load them from the database
1304  const Signature * s = this->getSignature(*jter);
1305  std::map<int, Link> tmpLinks;
1306  const std::map<int, Link> * links = &tmpLinks;
1307  if(s)
1308  {
1309  const Transform & t = optimizedPoses.at(*jter);
1310  UASSERT(!t.isNull());
1311  float distanceSqrd = referential.getDistanceSquared(t);
1312  if(radiusSqrd == 0 || distanceSqrd<radiusSqrd)
1313  {
1314  ids.insert(std::pair<int, float>(*jter,distanceSqrd));
1315  }
1316 
1317  links = &s->getLinks();
1318  }
1319 
1320  // links
1321  for(std::map<int, Link>::const_iterator iter=links->begin(); iter!=links->end(); ++iter)
1322  {
1323  if(!uContains(ids, iter->first) &&
1324  uContains(optimizedPoses, iter->first) &&
1325  iter->second.type()!=Link::kVirtualClosure)
1326  {
1327  nextMargin.insert(iter->first);
1328  }
1329  }
1330  }
1331  }
1332  ++m;
1333  }
1334  return ids;
1335 }
1336 
1337 int Memory::getNextId()
1338 {
1339  return ++_idCount;
1340 }
1341 
1342 int Memory::incrementMapId(std::map<int, int> * reducedIds)
1343 {
1344  //don't increment if there is no location in the current map
1345  const Signature * s = getLastWorkingSignature();
1346  if(s && s->mapId() == _idMapCount)
1347  {
1348  // New session! move all signatures from the STM to WM
1349  while(_stMem.size())
1350  {
1351  int reducedId = 0;
1352  int id = *_stMem.begin();
1353  moveSignatureToWMFromSTM(id, &reducedId);
1354  if(reducedIds && reducedId > 0)
1355  {
1356  reducedIds->insert(std::make_pair(id, reducedId));
1357  }
1358  }
1359 
1360  return ++_idMapCount;
1361  }
1362  return _idMapCount;
1363 }
1364 
1365 void Memory::updateAge(int signatureId)
1366 {
1367  std::map<int, double>::iterator iter=_workingMem.find(signatureId);
1368  if(iter!=_workingMem.end())
1369  {
1370  iter->second = UTimer::now();
1371  }
1372 }
1373 
1374 int Memory::getDatabaseMemoryUsed() const
1375 {
1376  int memoryUsed = 0;
1377  if(_dbDriver)
1378  {
1379  memoryUsed = _dbDriver->getMemoryUsed()/(1024*1024); //Byte to MB
1380  }
1381 
1382  return memoryUsed;
1383 }
1384 
1385 std::string Memory::getDatabaseVersion() const
1386 {
1387  std::string version = "0.0.0";
1388  if(_dbDriver)
1389  {
1390  version = _dbDriver->getDatabaseVersion();
1391  }
1392  return version;
1393 }
1394 
1395 std::string Memory::getDatabaseUrl() const
1396 {
1397  std::string url = "";
1398  if(_dbDriver)
1399  {
1400  url = _dbDriver->getUrl();
1401  }
1402  return url;
1403 }
1404 
1405 double Memory::getDbSavingTime() const
1406 {
1407  return _dbDriver?_dbDriver->getEmptyTrashesTime():0;
1408 }
1409 
1410 std::set<int> Memory::getAllSignatureIds() const
1411 {
1412  std::set<int> ids;
1413  if(_dbDriver)
1414  {
1415  _dbDriver->getAllNodeIds(ids);
1416  }
1417  for(std::map<int, Signature*>::const_iterator iter = _signatures.begin(); iter!=_signatures.end(); ++iter)
1418  {
1419  ids.insert(iter->first);
1420  }
1421  return ids;
1422 }
1423 
1424 void Memory::clear()
1425 {
1426  UDEBUG("");
1427 
1428  // empty the STM
1429  while(_stMem.size())
1430  {
1431  moveSignatureToWMFromSTM(*_stMem.begin());
1432  }
1433  if(_stMem.size() != 0)
1434  {
1435  ULOGGER_ERROR("_stMem must be empty here, size=%d", _stMem.size());
1436  }
1437  _stMem.clear();
1438 
1439  this->cleanUnusedWords();
1440 
1441  if(_dbDriver)
1442  {
1443  _dbDriver->emptyTrashes();
1444  _dbDriver->join();
1445  }
1446  if(_dbDriver)
1447  {
1448  // make sure time_enter in database is at least 1 second
1449  // after for the next stuf added to database
1450  uSleep(1500);
1451  }
1452 
1453  // Save some stats to the db, save only when the mem is not empty
1454  if(_dbDriver && (_stMem.size() || _workingMem.size()))
1455  {
1456  unsigned int memSize = (unsigned int)(_workingMem.size() + _stMem.size());
1457  if(_workingMem.size() && _workingMem.begin()->first < 0)
1458  {
1459  --memSize;
1460  }
1461 
1462  // this is only a safe check...not supposed to occur.
1463  UASSERT_MSG(memSize == _signatures.size(),
1464  uFormat("The number of signatures don't match! _workingMem=%d, _stMem=%d, _signatures=%d",
1465  _workingMem.size(), _stMem.size(), _signatures.size()).c_str());
1466 
1467  UDEBUG("Adding statistics after run...");
1468  if(_memoryChanged)
1469  {
1470  ParametersMap parameters = Parameters::getDefaultParameters();
1471  uInsert(parameters, parameters_);
1472  parameters.erase(Parameters::kRtabmapWorkingDirectory()); // don't save working directory as it is machine dependent
1473  UDEBUG("");
1474  _dbDriver->addInfoAfterRun(memSize,
1475  _lastSignature?_lastSignature->id():0,
1476  UProcessInfo::getMemoryUsage(),
1477  _dbDriver->getMemoryUsed(),
1478  (int)_vwd->getVisualWords().size(),
1479  parameters);
1480  }
1481  }
1482  UDEBUG("");
1483 
1484  //Get the tree root (parents)
1485  std::map<int, Signature*> mem = _signatures;
1486  for(std::map<int, Signature *>::iterator i=mem.begin(); i!=mem.end(); ++i)
1487  {
1488  if(i->second)
1489  {
1490  UDEBUG("deleting from the working and the short-term memory: %d", i->first);
1491  this->moveToTrash(i->second);
1492  }
1493  }
1494 
1495  if(_workingMem.size() != 0 && !(_workingMem.size() == 1 && _workingMem.begin()->first == kIdVirtual))
1496  {
1497  ULOGGER_ERROR("_workingMem must be empty here, size=%d", _workingMem.size());
1498  }
1499  _workingMem.clear();
1500  if(_signatures.size()!=0)
1501  {
1502  ULOGGER_ERROR("_signatures must be empty here, size=%d", _signatures.size());
1503  }
1504  _signatures.clear();
1505 
1506  UDEBUG("");
1507  // Wait until the db trash has finished cleaning the memory
1508  if(_dbDriver)
1509  {
1510  _dbDriver->emptyTrashes();
1511  }
1512  UDEBUG("");
1513  _lastSignature = 0;
1514  _lastGlobalLoopClosureId = 0;
1515  _idCount = kIdStart;
1516  _idMapCount = kIdStart;
1517  _memoryChanged = false;
1518  _linksChanged = false;
1519  _gpsOrigin = GPS();
1520  _rectCameraModels.clear();
1521  _rectStereoCameraModel = StereoCameraModel();
1522 
1523  if(_dbDriver)
1524  {
1525  _dbDriver->join(true);
1526  cleanUnusedWords();
1527  _dbDriver->emptyTrashes();
1528  }
1529  else
1530  {
1531  cleanUnusedWords();
1532  }
1533  if(_vwd)
1534  {
1535  _vwd->clear();
1536  }
1537  UDEBUG("");
1538 }
1539 
1545 std::map<int, float> Memory::computeLikelihood(const Signature * signature, const std::list<int> & ids)
1546 {
1547  if(!_tfIdfLikelihoodUsed)
1548  {
1549  UTimer timer;
1550  timer.start();
1551  std::map<int, float> likelihood;
1552 
1553  if(!signature)
1554  {
1555  ULOGGER_ERROR("The signature is null");
1556  return likelihood;
1557  }
1558  else if(ids.empty())
1559  {
1560  UWARN("ids list is empty");
1561  return likelihood;
1562  }
1563 
1564  for(std::list<int>::const_iterator iter = ids.begin(); iter!=ids.end(); ++iter)
1565  {
1566  float sim = 0.0f;
1567  if(*iter > 0)
1568  {
1569  const Signature * sB = this->getSignature(*iter);
1570  if(!sB)
1571  {
1572  UFATAL("Signature %d not found in WM ?!?", *iter);
1573  }
1574  sim = signature->compareTo(*sB);
1575  }
1576 
1577  likelihood.insert(likelihood.end(), std::pair<int, float>(*iter, sim));
1578  }
1579 
1580  UDEBUG("compute likelihood (similarity)... %f s", timer.ticks());
1581  return likelihood;
1582  }
1583  else
1584  {
1585  UTimer timer;
1586  timer.start();
1587  std::map<int, float> likelihood;
1588  std::map<int, float> calculatedWordsRatio;
1589 
1590  if(!signature)
1591  {
1592  ULOGGER_ERROR("The signature is null");
1593  return likelihood;
1594  }
1595  else if(ids.empty())
1596  {
1597  UWARN("ids list is empty");
1598  return likelihood;
1599  }
1600 
1601  for(std::list<int>::const_iterator iter = ids.begin(); iter!=ids.end(); ++iter)
1602  {
1603  likelihood.insert(likelihood.end(), std::pair<int, float>(*iter, 0.0f));
1604  }
1605 
1606  const std::list<int> & wordIds = uUniqueKeys(signature->getWords());
1607 
1608  float nwi; // nwi is the number of a specific word referenced by a place
1609  float ni; // ni is the total of words referenced by a place
1610  float nw; // nw is the number of places referenced by a specific word
1611  float N; // N is the total number of places
1612 
1613  float logNnw;
1614  const VisualWord * vw;
1615 
1616  N = this->getSignatures().size();
1617 
1618  if(N)
1619  {
1620  UDEBUG("processing... ");
1621  // Pour chaque mot dans la signature SURF
1622  for(std::list<int>::const_iterator i=wordIds.begin(); i!=wordIds.end(); ++i)
1623  {
1624  if(*i>0)
1625  {
1626  // "Inverted index" - Pour chaque endroit contenu dans chaque mot
1627  vw = _vwd->getWord(*i);
1628  UASSERT_MSG(vw!=0, uFormat("Word %d not found in dictionary!?", *i).c_str());
1629 
1630  const std::map<int, int> & refs = vw->getReferences();
1631  nw = refs.size();
1632  if(nw)
1633  {
1634  logNnw = log10(N/nw);
1635  if(logNnw)
1636  {
1637  for(std::map<int, int>::const_iterator j=refs.begin(); j!=refs.end(); ++j)
1638  {
1639  std::map<int, float>::iterator iter = likelihood.find(j->first);
1640  if(iter != likelihood.end())
1641  {
1642  nwi = j->second;
1643  ni = this->getNi(j->first);
1644  if(ni != 0)
1645  {
1646  //UDEBUG("%d, %f %f %f %f", vw->id(), logNnw, nwi, ni, ( nwi * logNnw ) / ni);
1647  iter->second += ( nwi * logNnw ) / ni;
1648  }
1649  }
1650  }
1651  }
1652  }
1653  }
1654  }
1655  }
1656 
1657  UDEBUG("compute likelihood (tf-idf) %f s", timer.ticks());
1658  return likelihood;
1659  }
1660 }
1661 
1662 // Weights of the signatures in the working memory <signature id, weight>
1663 std::map<int, int> Memory::getWeights() const
1664 {
1665  std::map<int, int> weights;
1666  for(std::map<int, double>::const_iterator iter=_workingMem.begin(); iter!=_workingMem.end(); ++iter)
1667  {
1668  if(iter->first > 0)
1669  {
1670  const Signature * s = this->getSignature(iter->first);
1671  if(!s)
1672  {
1673  UFATAL("Location %d must exist in memory", iter->first);
1674  }
1675  weights.insert(weights.end(), std::make_pair(iter->first, s->getWeight()));
1676  }
1677  else
1678  {
1679  weights.insert(weights.end(), std::make_pair(iter->first, -1));
1680  }
1681  }
1682  return weights;
1683 }
1684 
1685 std::list<int> Memory::forget(const std::set<int> & ignoredIds)
1686 {
1687  UDEBUG("");
1688  std::list<int> signaturesRemoved;
1689  if(this->isIncremental() &&
1690  _vwd->isIncremental() &&
1691  _vwd->getVisualWords().size() &&
1692  !_vwd->isIncrementalFlann())
1693  {
1694  // Note that when using incremental FLANN, the number of words
1695  // is not the biggest issue, so use the number of signatures instead
1696  // of the number of words
1697 
1698  int newWords = 0;
1699  int wordsRemoved = 0;
1700 
1701  // Get how many new words added for the last run...
1702  newWords = _vwd->getNotIndexedWordsCount();
1703 
1704  // So we need to remove at least "newWords" words from the
1705  // dictionary to respect the limit.
1706  while(wordsRemoved < newWords)
1707  {
1708  std::list<Signature *> signatures = this->getRemovableSignatures(1, ignoredIds);
1709  if(signatures.size())
1710  {
1711  Signature * s = dynamic_cast<Signature *>(signatures.front());
1712  if(s)
1713  {
1714  signaturesRemoved.push_back(s->id());
1715  this->moveToTrash(s);
1716  wordsRemoved = _vwd->getUnusedWordsSize();
1717  }
1718  else
1719  {
1720  break;
1721  }
1722  }
1723  else
1724  {
1725  break;
1726  }
1727  }
1728  UDEBUG("newWords=%d, wordsRemoved=%d", newWords, wordsRemoved);
1729  }
1730  else
1731  {
1732  UDEBUG("");
1733  // Remove one more than total added during the iteration
1734  int signaturesAdded = _signaturesAdded;
1735  std::list<Signature *> signatures = getRemovableSignatures(signaturesAdded+1, ignoredIds);
1736  for(std::list<Signature *>::iterator iter=signatures.begin(); iter!=signatures.end(); ++iter)
1737  {
1738  signaturesRemoved.push_back((*iter)->id());
1739  // When a signature is deleted, it notifies the memory
1740  // and it is removed from the memory list
1741  this->moveToTrash(*iter);
1742  }
1743  if((int)signatures.size() < signaturesAdded)
1744  {
1745  UWARN("Less signatures transferred (%d) than added (%d)! The working memory cannot decrease in size.",
1746  (int)signatures.size(), signaturesAdded);
1747  }
1748  else
1749  {
1750  UDEBUG("signaturesRemoved=%d, _signaturesAdded=%d", (int)signatures.size(), signaturesAdded);
1751  }
1752  }
1753  return signaturesRemoved;
1754 }
1755 
1756 
1757 int Memory::cleanup()
1758 {
1759  UDEBUG("");
1760  int signatureRemoved = 0;
1761 
1762  // bad signature
1763  if(_lastSignature && ((_lastSignature->isBadSignature() && _badSignaturesIgnored) || !_incrementalMemory))
1764  {
1765  if(_lastSignature->isBadSignature())
1766  {
1767  UDEBUG("Bad signature! %d", _lastSignature->id());
1768  }
1769  signatureRemoved = _lastSignature->id();
1770  moveToTrash(_lastSignature, _incrementalMemory);
1771  }
1772 
1773  return signatureRemoved;
1774 }
1775 
1776 void Memory::saveStatistics(const Statistics & statistics)
1777 {
1778  if(_dbDriver)
1779  {
1780  _dbDriver->addStatistics(statistics);
1781  }
1782 }
1783 
1784 void Memory::savePreviewImage(const cv::Mat & image) const
1785 {
1786  if(_dbDriver)
1787  {
1788  _dbDriver->savePreviewImage(image);
1789  }
1790 }
1791 cv::Mat Memory::loadPreviewImage() const
1792 {
1793  if(_dbDriver)
1794  {
1795  return _dbDriver->loadPreviewImage();
1796  }
1797  return cv::Mat();
1798 }
1799 
1800 void Memory::saveOptimizedPoses(const std::map<int, Transform> & optimizedPoses, const Transform & lastlocalizationPose) const
1801 {
1802  if(_dbDriver)
1803  {
1804  _dbDriver->saveOptimizedPoses(optimizedPoses, lastlocalizationPose);
1805  }
1806 }
1807 
1808 std::map<int, Transform> Memory::loadOptimizedPoses(Transform * lastlocalizationPose) const
1809 {
1810  if(_dbDriver)
1811  {
1812  return _dbDriver->loadOptimizedPoses(lastlocalizationPose);
1813  }
1814  return std::map<int, Transform>();
1815 }
1816 
1817 void Memory::save2DMap(const cv::Mat & map, float xMin, float yMin, float cellSize) const
1818 {
1819  if(_dbDriver)
1820  {
1821  _dbDriver->save2DMap(map, xMin, yMin, cellSize);
1822  }
1823 }
1824 
1825 cv::Mat Memory::load2DMap(float & xMin, float & yMin, float & cellSize) const
1826 {
1827  if(_dbDriver)
1828  {
1829  return _dbDriver->load2DMap(xMin, yMin, cellSize);
1830  }
1831  return cv::Mat();
1832 }
1833 
1834 void Memory::saveOptimizedMesh(
1835  const cv::Mat & cloud,
1836  const std::vector<std::vector<std::vector<unsigned int> > > & polygons,
1837 #if PCL_VERSION_COMPARE(>=, 1, 8, 0)
1838  const std::vector<std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > > & texCoords,
1839 #else
1840  const std::vector<std::vector<Eigen::Vector2f> > & texCoords,
1841 #endif
1842  const cv::Mat & textures) const
1843 {
1844  if(_dbDriver)
1845  {
1846  _dbDriver->saveOptimizedMesh(cloud, polygons, texCoords, textures);
1847  }
1848 }
1849 
1850 cv::Mat Memory::loadOptimizedMesh(
1851  std::vector<std::vector<std::vector<unsigned int> > > * polygons,
1852 #if PCL_VERSION_COMPARE(>=, 1, 8, 0)
1853  std::vector<std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > > * texCoords,
1854 #else
1855  std::vector<std::vector<Eigen::Vector2f> > * texCoords,
1856 #endif
1857  cv::Mat * textures) const
1858 {
1859  if(_dbDriver)
1860  {
1861  return _dbDriver->loadOptimizedMesh(polygons, texCoords, textures);
1862  }
1863  return cv::Mat();
1864 }
1865 
1866 void Memory::emptyTrash()
1867 {
1868  if(_dbDriver)
1869  {
1870  _dbDriver->emptyTrashes(true);
1871  }
1872 }
1873 
1874 void Memory::joinTrashThread()
1875 {
1876  if(_dbDriver)
1877  {
1878  UDEBUG("");
1879  _dbDriver->join();
1880  UDEBUG("");
1881  }
1882 }
1883 
1884 class WeightAgeIdKey
1885 {
1886 public:
1887  WeightAgeIdKey(int w, double a, int i) :
1888  weight(w),
1889  age(a),
1890  id(i){}
1891  bool operator<(const WeightAgeIdKey & k) const
1892  {
1893  if(weight < k.weight)
1894  {
1895  return true;
1896  }
1897  else if(weight == k.weight)
1898  {
1899  if(age < k.age)
1900  {
1901  return true;
1902  }
1903  else if(age == k.age)
1904  {
1905  if(id < k.id)
1906  {
1907  return true;
1908  }
1909  }
1910  }
1911  return false;
1912  }
1913  int weight, age, id;
1914 };
1915 std::list<Signature *> Memory::getRemovableSignatures(int count, const std::set<int> & ignoredIds)
1916 {
1917  //UDEBUG("");
1918  std::list<Signature *> removableSignatures;
1919  std::map<WeightAgeIdKey, Signature *> weightAgeIdMap;
1920 
1921  // Find the last index to check...
1922  UDEBUG("mem.size()=%d, ignoredIds.size()=%d", (int)_workingMem.size(), (int)ignoredIds.size());
1923 
1924  if(_workingMem.size())
1925  {
1926  int recentWmMaxSize = _recentWmRatio * float(_workingMem.size());
1927  bool recentWmImmunized = false;
1928  // look for the position of the lastLoopClosureId in WM
1929  int currentRecentWmSize = 0;
1930  if(_lastGlobalLoopClosureId > 0 && _stMem.find(_lastGlobalLoopClosureId) == _stMem.end())
1931  {
1932  // If set, it must be in WM
1933  std::map<int, double>::const_iterator iter = _workingMem.find(_lastGlobalLoopClosureId);
1934  while(iter != _workingMem.end())
1935  {
1936  ++currentRecentWmSize;
1937  ++iter;
1938  }
1939  if(currentRecentWmSize>1 && currentRecentWmSize < recentWmMaxSize)
1940  {
1941  recentWmImmunized = true;
1942  }
1943  else if(currentRecentWmSize == 0 && _workingMem.size() > 1)
1944  {
1945  UERROR("Last loop closure id not found in WM (%d)", _lastGlobalLoopClosureId);
1946  }
1947  UDEBUG("currentRecentWmSize=%d, recentWmMaxSize=%d, _recentWmRatio=%f, end recent wM = %d", currentRecentWmSize, recentWmMaxSize, _recentWmRatio, _lastGlobalLoopClosureId);
1948  }
1949 
1950  // Ignore neighbor of the last location in STM (for neighbor links redirection issue during Rehearsal).
1951  Signature * lastInSTM = 0;
1952  if(_stMem.size())
1953  {
1954  lastInSTM = _signatures.at(*_stMem.begin());
1955  }
1956 
1957  for(std::map<int, double>::const_iterator memIter = _workingMem.begin(); memIter != _workingMem.end(); ++memIter)
1958  {
1959  if( (recentWmImmunized && memIter->first > _lastGlobalLoopClosureId) ||
1960  memIter->first == _lastGlobalLoopClosureId)
1961  {
1962  // ignore recent memory
1963  }
1964  else if(memIter->first > 0 && ignoredIds.find(memIter->first) == ignoredIds.end() && (!lastInSTM || !lastInSTM->hasLink(memIter->first)))
1965  {
1966  Signature * s = this->_getSignature(memIter->first);
1967  if(s)
1968  {
1969  // Links must not be in STM to be removable, rehearsal issue
1970  bool foundInSTM = false;
1971  for(std::map<int, Link>::const_iterator iter = s->getLinks().begin(); iter!=s->getLinks().end(); ++iter)
1972  {
1973  if(_stMem.find(iter->first) != _stMem.end())
1974  {
1975  UDEBUG("Ignored %d because it has a link (%d) to STM", s->id(), iter->first);
1976  foundInSTM = true;
1977  break;
1978  }
1979  }
1980  if(!foundInSTM)
1981  {
1982  // less weighted signature priority to be transferred
1983  weightAgeIdMap.insert(std::make_pair(WeightAgeIdKey(s->getWeight(), _transferSortingByWeightId?0.0:memIter->second, s->id()), s));
1984  }
1985  }
1986  else
1987  {
1988  ULOGGER_ERROR("Not supposed to occur!!!");
1989  }
1990  }
1991  else
1992  {
1993  //UDEBUG("Ignoring id %d", memIter->first);
1994  }
1995  }
1996 
1997  int recentWmCount = 0;
1998  // make the list of removable signatures
1999  // Criteria : Weight -> ID
2000  UDEBUG("signatureMap.size()=%d _lastGlobalLoopClosureId=%d currentRecentWmSize=%d recentWmMaxSize=%d",
2001  (int)weightAgeIdMap.size(), _lastGlobalLoopClosureId, currentRecentWmSize, recentWmMaxSize);
2002  for(std::map<WeightAgeIdKey, Signature*>::iterator iter=weightAgeIdMap.begin();
2003  iter!=weightAgeIdMap.end();
2004  ++iter)
2005  {
2006  if(!recentWmImmunized)
2007  {
2008  UDEBUG("weight=%d, id=%d",
2009  iter->second->getWeight(),
2010  iter->second->id());
2011  removableSignatures.push_back(iter->second);
2012 
2013  if(_lastGlobalLoopClosureId && iter->second->id() > _lastGlobalLoopClosureId)
2014  {
2015  ++recentWmCount;
2016  if(currentRecentWmSize - recentWmCount < recentWmMaxSize)
2017  {
2018  UDEBUG("switched recentWmImmunized");
2019  recentWmImmunized = true;
2020  }
2021  }
2022  }
2023  else if(_lastGlobalLoopClosureId == 0 || iter->second->id() < _lastGlobalLoopClosureId)
2024  {
2025  UDEBUG("weight=%d, id=%d",
2026  iter->second->getWeight(),
2027  iter->second->id());
2028  removableSignatures.push_back(iter->second);
2029  }
2030  if(removableSignatures.size() >= (unsigned int)count)
2031  {
2032  break;
2033  }
2034  }
2035  }
2036  else
2037  {
2038  ULOGGER_WARN("not enough signatures to get an old one...");
2039  }
2040  return removableSignatures;
2041 }
2042 
2046 void Memory::moveToTrash(Signature * s, bool keepLinkedToGraph, std::list<int> * deletedWords)
2047 {
2048  UDEBUG("id=%d", s?s->id():0);
2049  if(s)
2050  {
2051  // If not saved to database or it is a bad signature (not saved), remove links!
2052  if(!keepLinkedToGraph || (!s->isSaved() && s->isBadSignature() && _badSignaturesIgnored))
2053  {
2054  UASSERT_MSG(this->isInSTM(s->id()),
2055  uFormat("Deleting location (%d) outside the "
2056  "STM is not implemented!", s->id()).c_str());
2057  const std::map<int, Link> & links = s->getLinks();
2058  for(std::map<int, Link>::const_iterator iter=links.begin(); iter!=links.end(); ++iter)
2059  {
2060  if(iter->second.from() != iter->second.to())
2061  {
2062  Signature * sTo = this->_getSignature(iter->first);
2063  // neighbor to s
2064  UASSERT_MSG(sTo!=0,
2065  uFormat("A neighbor (%d) of the deleted location %d is "
2066  "not found in WM/STM! Are you deleting a location "
2067  "outside the STM?", iter->first, s->id()).c_str());
2068 
2069  if(iter->first > s->id() && links.size()>1 && sTo->hasLink(s->id()))
2070  {
2071  UWARN("Link %d of %d is newer, removing neighbor link "
2072  "may split the map!",
2073  iter->first, s->id());
2074  }
2075 
2076  // child
2077  if(iter->second.type() == Link::kGlobalClosure && s->id() > sTo->id())
2078  {
2079  sTo->setWeight(sTo->getWeight() + s->getWeight()); // copy weight
2080  }
2081 
2082  sTo->removeLink(s->id());
2083  }
2084 
2085  }
2086  s->removeLinks(); // remove all links
2087  s->setWeight(0);
2088  s->setLabel(""); // reset label
2089  }
2090  else
2091  {
2092  // Make sure that virtual links are removed.
2093  // It should be called before the signature is
2094  // removed from _signatures below.
2095  removeVirtualLinks(s->id());
2096  }
2097 
2098  this->disableWordsRef(s->id());
2099  if(!keepLinkedToGraph && _vwd->isIncremental())
2100  {
2101  std::list<int> keys = uUniqueKeys(s->getWords());
2102  for(std::list<int>::const_iterator i=keys.begin(); i!=keys.end(); ++i)
2103  {
2104  // assume just removed word doesn't have any other references
2105  VisualWord * w = _vwd->getUnusedWord(*i);
2106  if(w)
2107  {
2108  std::vector<VisualWord*> wordToDelete;
2109  wordToDelete.push_back(w);
2110  _vwd->removeWords(wordToDelete);
2111  if(deletedWords)
2112  {
2113  deletedWords->push_back(w->id());
2114  }
2115  delete w;
2116  }
2117  }
2118  }
2119 
2120  _workingMem.erase(s->id());
2121  _stMem.erase(s->id());
2122  _signatures.erase(s->id());
2123  if(_signaturesAdded>0)
2124  {
2125  --_signaturesAdded;
2126  }
2127 
2128  if(_lastSignature == s)
2129  {
2130  _lastSignature = 0;
2131  if(_stMem.size())
2132  {
2133  _lastSignature = this->_getSignature(*_stMem.rbegin());
2134  }
2135  else if(_workingMem.size())
2136  {
2137  _lastSignature = this->_getSignature(_workingMem.rbegin()->first);
2138  }
2139  }
2140 
2141  if(_lastGlobalLoopClosureId == s->id())
2142  {
2143  _lastGlobalLoopClosureId = 0;
2144  }
2145 
2146  if( (_notLinkedNodesKeptInDb || keepLinkedToGraph || s->isSaved()) &&
2147  _dbDriver &&
2148  s->id()>0 &&
2149  (_incrementalMemory || s->isSaved()))
2150  {
2151  _dbDriver->asyncSave(s);
2152  }
2153  else
2154  {
2155  delete s;
2156  }
2157  }
2158 }
2159 
2160 int Memory::getLastSignatureId() const
2161 {
2162  return _idCount;
2163 }
2164 
2165 const Signature * Memory::getLastWorkingSignature() const
2166 {
2167  UDEBUG("");
2168  return _lastSignature;
2169 }
2170 
2171 int Memory::getSignatureIdByLabel(const std::string & label, bool lookInDatabase) const
2172 {
2173  UDEBUG("label=%s", label.c_str());
2174  int id = 0;
2175  if(label.size())
2176  {
2177  for(std::map<int, Signature*>::const_iterator iter=_signatures.begin(); iter!=_signatures.end(); ++iter)
2178  {
2179  UASSERT(iter->second != 0);
2180  if(iter->second->getLabel().compare(label) == 0)
2181  {
2182  id = iter->second->id();
2183  break;
2184  }
2185  }
2186  if(id == 0 && _dbDriver && lookInDatabase)
2187  {
2188  _dbDriver->getNodeIdByLabel(label, id);
2189  }
2190  }
2191  return id;
2192 }
2193 
2194 bool Memory::labelSignature(int id, const std::string & label)
2195 {
2196  // verify that this label is not used
2197  int idFound=getSignatureIdByLabel(label);
2198  if(idFound == 0 || idFound == id)
2199  {
2200  Signature * s = this->_getSignature(id);
2201  if(s)
2202  {
2203  s->setLabel(label);
2204  _linksChanged = s->isSaved(); // HACK to get label updated in Localization mode
2205  UWARN("Label \"%s\" set to node %d", label.c_str(), id);
2206  return true;
2207  }
2208  else if(_dbDriver)
2209  {
2210  std::list<int> ids;
2211  ids.push_back(id);
2212  std::list<Signature *> signatures;
2213  _dbDriver->loadSignatures(ids,signatures);
2214  if(signatures.size())
2215  {
2216  signatures.front()->setLabel(label);
2217  UWARN("Label \"%s\" set to node %d", label.c_str(), id);
2218  _dbDriver->asyncSave(signatures.front()); // move it again to trash
2219  return true;
2220  }
2221  }
2222  else
2223  {
2224  UERROR("Node %d not found, failed to set label \"%s\"!", id, label.c_str());
2225  }
2226  }
2227  else if(idFound)
2228  {
2229  UWARN("Node %d has already label \"%s\"", idFound, label.c_str());
2230  }
2231  return false;
2232 }
2233 
2234 std::map<int, std::string> Memory::getAllLabels() const
2235 {
2236  std::map<int, std::string> labels;
2237  for(std::map<int, Signature*>::const_iterator iter = _signatures.begin(); iter!=_signatures.end(); ++iter)
2238  {
2239  if(!iter->second->getLabel().empty())
2240  {
2241  labels.insert(std::make_pair(iter->first, iter->second->getLabel()));
2242  }
2243  }
2244  if(_dbDriver)
2245  {
2246  _dbDriver->getAllLabels(labels);
2247  }
2248  return labels;
2249 }
2250 
2251 bool Memory::setUserData(int id, const cv::Mat & data)
2252 {
2253  Signature * s = this->_getSignature(id);
2254  if(s)
2255  {
2256  s->sensorData().setUserData(data);
2257  return true;
2258  }
2259  else
2260  {
2261  UERROR("Node %d not found in RAM, failed to set user data (size=%d)!", id, data.total());
2262  }
2263  return false;
2264 }
2265 
2266 void Memory::deleteLocation(int locationId, std::list<int> * deletedWords)
2267 {
2268  UDEBUG("Deleting location %d", locationId);
2269  Signature * location = _getSignature(locationId);
2270  if(location)
2271  {
2272  this->moveToTrash(location, false, deletedWords);
2273  }
2274 }
2275 
2276 void Memory::saveLocationData(int locationId)
2277 {
2278  UDEBUG("Saving location data %d", locationId);
2279  Signature * location = _getSignature(locationId);
2280  if( location &&
2281  _dbDriver &&
2282  !_dbDriver->isInMemory() && // don't push in database if it is also in memory.
2283  location->id()>0 &&
2284  (_incrementalMemory && !location->isSaved()))
2285  {
2286  Signature * cpy = new Signature();
2287  *cpy = *location;
2288  _dbDriver->asyncSave(cpy);
2289 
2290  location->setSaved(true);
2291  location->sensorData().clearCompressedData();
2292  }
2293 }
2294 
2295 void Memory::removeLink(int oldId, int newId)
2296 {
2297  //this method assumes receiving oldId < newId, if not switch them
2298  Signature * oldS = this->_getSignature(oldId<newId?oldId:newId);
2299  Signature * newS = this->_getSignature(oldId<newId?newId:oldId);
2300  if(oldS && newS)
2301  {
2302  UINFO("removing link between location %d and %d", oldS->id(), newS->id());
2303 
2304  if(oldS->hasLink(newS->id()) && newS->hasLink(oldS->id()))
2305  {
2306  Link::Type type = oldS->getLinks().at(newS->id()).type();
2307  if(type == Link::kGlobalClosure && newS->getWeight() > 0)
2308  {
2309  // adjust the weight
2310  oldS->setWeight(oldS->getWeight()+1);
2311  newS->setWeight(newS->getWeight()>0?newS->getWeight()-1:0);
2312  }
2313 
2314 
2315  oldS->removeLink(newS->id());
2316  newS->removeLink(oldS->id());
2317 
2318  if(type!=Link::kVirtualClosure)
2319  {
2320  _linksChanged = true;
2321  }
2322 
2323  bool noChildrenAnymore = true;
2324  for(std::map<int, Link>::const_iterator iter=newS->getLinks().begin(); iter!=newS->getLinks().end(); ++iter)
2325  {
2326  if(iter->second.type() != Link::kNeighbor &&
2327  iter->second.type() != Link::kNeighborMerged &&
2328  iter->second.type() != Link::kPosePrior &&
2329  iter->first < newS->id())
2330  {
2331  noChildrenAnymore = false;
2332  break;
2333  }
2334  }
2335  if(noChildrenAnymore && newS->id() == _lastGlobalLoopClosureId)
2336  {
2337  _lastGlobalLoopClosureId = 0;
2338  }
2339  }
2340  else
2341  {
2342  UERROR("Signatures %d and %d don't have bidirectional link!", oldS->id(), newS->id());
2343  }
2344  }
2345  else
2346  {
2347  if(!newS)
2348  {
2349  UERROR("Signature %d is not in working memory... cannot remove link.", newS->id());
2350  }
2351  if(!oldS)
2352  {
2353  UERROR("Signature %d is not in working memory... cannot remove link.", oldS->id());
2354  }
2355  }
2356 }
2357 
2358 void Memory::removeRawData(int id, bool image, bool scan, bool userData)
2359 {
2360  UDEBUG("id=%d image=%d scan=%d userData=%d", id, image?1:0, scan?1:0, userData?1:0);
2361  Signature * s = this->_getSignature(id);
2362  if(s)
2363  {
2364  if(image && (!_reextractLoopClosureFeatures || !_registrationPipeline->isImageRequired()))
2365  {
2366  s->sensorData().setImageRaw(cv::Mat());
2367  s->sensorData().setDepthOrRightRaw(cv::Mat());
2368  }
2369  if(scan && !_registrationPipeline->isScanRequired())
2370  {
2371  LaserScan scan = s->sensorData().laserScanRaw();
2372  scan.clear();
2373  s->sensorData().setLaserScanRaw(scan);
2374  }
2375  if(userData && !_registrationPipeline->isUserDataRequired())
2376  {
2377  s->sensorData().setUserDataRaw(cv::Mat());
2378  }
2379  }
2380 }
2381 
2382 // compute transform fromId -> toId
2383 Transform Memory::computeTransform(
2384  int fromId,
2385  int toId,
2386  Transform guess,
2387  RegistrationInfo * info,
2388  bool useKnownCorrespondencesIfPossible)
2389 {
2390  Signature * fromS = this->_getSignature(fromId);
2391  Signature * toS = this->_getSignature(toId);
2392 
2393  Transform transform;
2394 
2395  if(fromS && toS)
2396  {
2397  transform = computeTransform(*fromS, *toS, guess, info, useKnownCorrespondencesIfPossible);
2398  }
2399  else
2400  {
2401  std::string msg = uFormat("Did not find nodes %d and/or %d", fromId, toId);
2402  if(info)
2403  {
2404  info->rejectedMsg = msg;
2405  }
2406  UWARN(msg.c_str());
2407  }
2408  return transform;
2409 }
2410 
2411 // compute transform fromId -> toId
2412 Transform Memory::computeTransform(
2413  Signature & fromS,
2414  Signature & toS,
2415  Transform guess,
2416  RegistrationInfo * info,
2417  bool useKnownCorrespondencesIfPossible) const
2418 {
2419  UDEBUG("");
2420  Transform transform;
2421 
2422  // make sure we have all data needed
2423  // load binary data from database if not in RAM (if image is already here, scan and userData should be or they are null)
2424  if((((_reextractLoopClosureFeatures || _visCorType==1) && _registrationPipeline->isImageRequired()) && fromS.sensorData().imageCompressed().empty()) ||
2425  (_registrationPipeline->isScanRequired() && fromS.sensorData().imageCompressed().empty() && fromS.sensorData().laserScanCompressed().isEmpty()) ||
2426  (_registrationPipeline->isUserDataRequired() && fromS.sensorData().imageCompressed().empty() && fromS.sensorData().userDataCompressed().empty()))
2427  {
2428  fromS.sensorData() = getNodeData(fromS.id());
2429  }
2430  if((((_reextractLoopClosureFeatures || _visCorType==1) && _registrationPipeline->isImageRequired()) && toS.sensorData().imageCompressed().empty()) ||
2431  (_registrationPipeline->isScanRequired() && toS.sensorData().imageCompressed().empty() && toS.sensorData().laserScanCompressed().isEmpty()) ||
2432  (_registrationPipeline->isUserDataRequired() && toS.sensorData().imageCompressed().empty() && toS.sensorData().userDataCompressed().empty()))
2433  {
2434  toS.sensorData() = getNodeData(toS.id());
2435  }
2436  // uncompress only what we need
2437  cv::Mat imgBuf, depthBuf, userBuf;
2438  LaserScan laserBuf;
2439  fromS.sensorData().uncompressData(
2440  ((_reextractLoopClosureFeatures || _visCorType==1) && _registrationPipeline->isImageRequired())?&imgBuf:0,
2441  ((_reextractLoopClosureFeatures || _visCorType==1) && _registrationPipeline->isImageRequired())?&depthBuf:0,
2442  _registrationPipeline->isScanRequired()?&laserBuf:0,
2443  _registrationPipeline->isUserDataRequired()?&userBuf:0);
2444  toS.sensorData().uncompressData(
2445  ((_reextractLoopClosureFeatures || _visCorType==1) && _registrationPipeline->isImageRequired())?&imgBuf:0,
2446  ((_reextractLoopClosureFeatures || _visCorType==1) && _registrationPipeline->isImageRequired())?&depthBuf:0,
2447  _registrationPipeline->isScanRequired()?&laserBuf:0,
2448  _registrationPipeline->isUserDataRequired()?&userBuf:0);
2449 
2450 
2451  // compute transform fromId -> toId
2452  std::vector<int> inliersV;
2453  if((_reextractLoopClosureFeatures && _registrationPipeline->isImageRequired()) ||
2454  (fromS.getWords().size() && toS.getWords().size()) ||
2455  (!guess.isNull() && !_registrationPipeline->isImageRequired()))
2456  {
2457  Signature tmpFrom = fromS;
2458  Signature tmpTo = toS;
2459 
2460  if(_reextractLoopClosureFeatures && _registrationPipeline->isImageRequired())
2461  {
2462  UDEBUG("");
2463  tmpFrom.setWords(std::multimap<int, cv::KeyPoint>());
2464  tmpFrom.setWords3(std::multimap<int, cv::Point3f>());
2465  tmpFrom.setWordsDescriptors(std::multimap<int, cv::Mat>());
2466  tmpFrom.sensorData().setFeatures(std::vector<cv::KeyPoint>(), std::vector<cv::Point3f>(), cv::Mat());
2467  tmpTo.setWords(std::multimap<int, cv::KeyPoint>());
2468  tmpTo.setWords3(std::multimap<int, cv::Point3f>());
2469  tmpTo.setWordsDescriptors(std::multimap<int, cv::Mat>());
2470  tmpTo.sensorData().setFeatures(std::vector<cv::KeyPoint>(), std::vector<cv::Point3f>(), cv::Mat());
2471  }
2472  else if(useKnownCorrespondencesIfPossible)
2473  {
2474  // This will make RegistrationVis bypassing the correspondences computation
2475  tmpFrom.setWordsDescriptors(std::multimap<int, cv::Mat>());
2476  tmpTo.setWordsDescriptors(std::multimap<int, cv::Mat>());
2477  }
2478 
2479  if(guess.isNull() && (!_registrationPipeline->isImageRequired() || _visCorType==1))
2480  {
2481  UDEBUG("");
2482  // no visual in the pipeline, make visual registration for guess
2483  // make sure feature matching is used instead of optical flow to compute the guess
2484  ParametersMap parameters = parameters_;
2485  uInsert(parameters, ParametersPair(Parameters::kVisCorType(), "0"));
2486  uInsert(parameters, ParametersPair(Parameters::kRegRepeatOnce(), "false"));
2487  RegistrationVis regVis(parameters);
2488  guess = regVis.computeTransformation(tmpFrom, tmpTo, guess, info);
2489  if(!guess.isNull())
2490  {
2491  transform = _registrationPipeline->computeTransformationMod(tmpFrom, tmpTo, guess, info);
2492  }
2493  }
2494  else if(_localBundleOnLoopClosure &&
2495  _registrationPipeline->isImageRequired() &&
2496  !_registrationPipeline->isScanRequired() &&
2497  !_registrationPipeline->isUserDataRequired() &&
2498  !tmpTo.getWordsDescriptors().empty() &&
2499  !tmpTo.getWords().empty() &&
2500  !tmpFrom.getWordsDescriptors().empty() &&
2501  !tmpFrom.getWords().empty() &&
2502  !tmpFrom.getWords3().empty())
2503  {
2504  std::multimap<int, cv::Point3f> words3DMap;
2505  std::multimap<int, cv::KeyPoint> wordsMap;
2506  std::multimap<int, cv::Mat> wordsDescriptorsMap;
2507 
2508  const std::map<int, Link> & links = fromS.getLinks();
2509  {
2510  const std::map<int, cv::Point3f> & words3 = uMultimapToMapUnique(fromS.getWords3());
2511  UDEBUG("fromS.getWords3()=%d uniques=%d", (int)fromS.getWords3().size(), (int)words3.size());
2512  for(std::map<int, cv::Point3f>::const_iterator jter=words3.begin(); jter!=words3.end(); ++jter)
2513  {
2514  if(util3d::isFinite(jter->second))
2515  {
2516  words3DMap.insert(*jter);
2517  wordsMap.insert(*fromS.getWords().find(jter->first));
2518  wordsDescriptorsMap.insert(*fromS.getWordsDescriptors().find(jter->first));
2519  }
2520  }
2521  }
2522  UDEBUG("words3DMap=%d", (int)words3DMap.size());
2523 
2524  for(std::map<int, Link>::const_iterator iter=links.begin(); iter!=links.end(); ++iter)
2525  {
2526  int id = iter->first;
2527  const Signature * s = this->getSignature(id);
2528  const std::map<int, cv::Point3f> & words3 = uMultimapToMapUnique(s->getWords3());
2529  for(std::map<int, cv::Point3f>::const_iterator jter=words3.begin(); jter!=words3.end(); ++jter)
2530  {
2531  if( jter->first > 0 &&
2532  util3d::isFinite(jter->second) &&
2533  words3DMap.find(jter->first) == words3DMap.end())
2534  {
2535  words3DMap.insert(std::make_pair(jter->first, util3d::transformPoint(jter->second, iter->second.transform())));
2536  wordsMap.insert(*s->getWords().find(jter->first));
2537  wordsDescriptorsMap.insert(*s->getWordsDescriptors().find(jter->first));
2538  }
2539  }
2540  }
2541  UDEBUG("words3DMap=%d", (int)words3DMap.size());
2542  Signature tmpFrom2(fromS.id());
2543  tmpFrom2.setWords3(words3DMap);
2544  tmpFrom2.setWords(wordsMap);
2545  tmpFrom2.setWordsDescriptors(wordsDescriptorsMap);
2546 
2547  transform = _registrationPipeline->computeTransformationMod(tmpFrom2, tmpTo, guess, info);
2548 
2549  if(!transform.isNull() && info)
2550  {
2551  std::map<int, cv::Point3f> points3DMap = uMultimapToMapUnique(tmpFrom2.getWords3());
2552  std::map<int, Transform> bundlePoses;
2553  std::multimap<int, Link> bundleLinks;
2554  std::map<int, CameraModel> bundleModels;
2555  std::map<int, std::map<int, cv::Point3f> > wordReferences;
2556 
2557  std::map<int, Link> links = fromS.getLinks();
2558  links.insert(std::make_pair(toS.id(), Link(fromS.id(), toS.id(), Link::kGlobalClosure, transform, info->covariance.inv())));
2559  links.insert(std::make_pair(fromS.id(), Link()));
2560 
2561  for(std::map<int, Link>::iterator iter=links.begin(); iter!=links.end(); ++iter)
2562  {
2563  int id = iter->first;
2564  const Signature * s;
2565  if(id == tmpTo.id())
2566  {
2567  s = &tmpTo; // reuse matched words
2568  }
2569  else
2570  {
2571  s = this->getSignature(id);
2572  }
2573  CameraModel model;
2574  if(s->sensorData().cameraModels().size() == 1 && s->sensorData().cameraModels().at(0).isValidForProjection())
2575  {
2576  model = s->sensorData().cameraModels()[0];
2577  }
2578  else if(s->sensorData().stereoCameraModel().isValidForProjection())
2579  {
2580  model = s->sensorData().stereoCameraModel().left();
2581  // Set Tx for stereo BA
2582  model = CameraModel(model.fx(),
2583  model.fy(),
2584  model.cx(),
2585  model.cy(),
2586  model.localTransform(),
2587  -s->sensorData().stereoCameraModel().baseline()*model.fx());
2588  }
2589  else
2590  {
2591  UFATAL("no valid camera model to use local bundle adjustment on loop closure!");
2592  }
2593  bundleModels.insert(std::make_pair(id, model));
2594  Transform invLocalTransform = model.localTransform().inverse();
2595  if(iter->second.isValid())
2596  {
2597  bundleLinks.insert(std::make_pair(iter->second.from(), iter->second));
2598  bundlePoses.insert(std::make_pair(id, iter->second.transform()));
2599  }
2600  else
2601  {
2602  bundlePoses.insert(std::make_pair(id, Transform::getIdentity()));
2603  }
2604  const std::map<int,cv::KeyPoint> & words = uMultimapToMapUnique(s->getWords());
2605  for(std::map<int, cv::KeyPoint>::const_iterator jter=words.begin(); jter!=words.end(); ++jter)
2606  {
2607  if(points3DMap.find(jter->first)!=points3DMap.end() &&
2608  (id == tmpTo.id() || jter->first > 0))
2609  {
2610  std::multimap<int, cv::Point3f>::const_iterator kter = s->getWords3().find(jter->first);
2611  cv::Point3f pt3d = util3d::transformPoint(kter->second, invLocalTransform);
2612  wordReferences.insert(std::make_pair(jter->first, std::map<int, cv::Point3f>()));
2613  wordReferences.at(jter->first).insert(std::make_pair(id, cv::Point3f(jter->second.pt.x, jter->second.pt.y, pt3d.z)));
2614  }
2615  }
2616  }
2617 
2618  UDEBUG("sba...start");
2619  // set root negative to fix all other poses
2620  std::set<int> sbaOutliers;
2621  UTimer bundleTimer;
2622  OptimizerG2O sba;
2623  UTimer bundleTime;
2624  bundlePoses = sba.optimizeBA(-toS.id(), bundlePoses, bundleLinks, bundleModels, points3DMap, wordReferences, &sbaOutliers);
2625  UDEBUG("sba...end");
2626 
2627  UDEBUG("bundleTime=%fs (poses=%d wordRef=%d outliers=%d)", bundleTime.ticks(), (int)bundlePoses.size(), (int)wordReferences.size(), (int)sbaOutliers.size());
2628 
2629  UDEBUG("Local Bundle Adjustment Before: %s", transform.prettyPrint().c_str());
2630  if(!bundlePoses.rbegin()->second.isNull())
2631  {
2632  if(sbaOutliers.size())
2633  {
2634  std::vector<int> newInliers(info->inliersIDs.size());
2635  int oi=0;
2636  for(unsigned int i=0; i<info->inliersIDs.size(); ++i)
2637  {
2638  if(sbaOutliers.find(info->inliersIDs[i]) == sbaOutliers.end())
2639  {
2640  newInliers[oi++] = info->inliersIDs[i];
2641  }
2642  }
2643  newInliers.resize(oi);
2644  UDEBUG("BA outliers ratio %f", float(sbaOutliers.size())/float(info->inliersIDs.size()));
2645  info->inliers = (int)newInliers.size();
2646  info->inliersIDs = newInliers;
2647  }
2648  if(info->inliers < _registrationPipeline->getMinVisualCorrespondences())
2649  {
2650  info->rejectedMsg = uFormat("Too low inliers after bundle adjustment: %d<%d", info->inliers, _registrationPipeline->getMinVisualCorrespondences());
2651  transform.setNull();
2652  }
2653  else
2654  {
2655  transform = bundlePoses.rbegin()->second;
2656  }
2657  }
2658  UDEBUG("Local Bundle Adjustment After : %s", transform.prettyPrint().c_str());
2659  }
2660  }
2661  else
2662  {
2663  transform = _registrationPipeline->computeTransformationMod(tmpFrom, tmpTo, guess, info);
2664  }
2665 
2666  if(!transform.isNull())
2667  {
2668  UDEBUG("");
2669  // verify if it is a 180 degree transform, well verify > 90
2670  float x,y,z, roll,pitch,yaw;
2671  if(guess.isNull())
2672  {
2673  transform.getTranslationAndEulerAngles(x,y,z, roll,pitch,yaw);
2674  }
2675  else
2676  {
2677  Transform guessError = guess.inverse() * transform;
2678  guessError.getTranslationAndEulerAngles(x,y,z, roll,pitch,yaw);
2679  }
2680  if(fabs(pitch) > CV_PI/2 ||
2681  fabs(yaw) > CV_PI/2)
2682  {
2683  transform.setNull();
2684  std::string msg = uFormat("Too large rotation detected! (pitch=%f, yaw=%f) max is %f",
2685  roll, pitch, yaw, CV_PI/2);
2686  UINFO(msg.c_str());
2687  if(info)
2688  {
2689  info->rejectedMsg = msg;
2690  }
2691  }
2692  }
2693  }
2694  return transform;
2695 }
2696 
2697 // compute transform fromId -> multiple toId
2698 Transform Memory::computeIcpTransformMulti(
2699  int fromId,
2700  int toId,
2701  const std::map<int, Transform> & poses,
2702  RegistrationInfo * info)
2703 {
2704  UASSERT(uContains(poses, fromId) && uContains(_signatures, fromId));
2705  UASSERT(uContains(poses, toId) && uContains(_signatures, toId));
2706 
2707  UDEBUG("%d -> %d, Guess=%s", fromId, toId, (poses.at(fromId).inverse() * poses.at(toId)).prettyPrint().c_str());
2708  if(ULogger::level() == ULogger::kDebug)
2709  {
2710  std::string ids;
2711  for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
2712  {
2713  if(iter->first != fromId)
2714  {
2715  ids += uNumber2Str(iter->first) + " ";
2716  }
2717  }
2718  UDEBUG("%d vs %s", fromId, ids.c_str());
2719  }
2720 
2721  // make sure that all laser scans are loaded
2722  std::list<Signature*> depthToLoad;
2723  for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
2724  {
2725  Signature * s = _getSignature(iter->first);
2726  UASSERT(s != 0);
2727  //if image is already here, scan should be or it is null
2728  if(s->sensorData().imageCompressed().empty() &&
2729  s->sensorData().laserScanCompressed().isEmpty())
2730  {
2731  depthToLoad.push_back(s);
2732  }
2733  }
2734  if(depthToLoad.size() && _dbDriver)
2735  {
2736  _dbDriver->loadNodeData(depthToLoad, false, true, false, false);
2737  }
2738 
2739  Signature * fromS = _getSignature(fromId);
2740  LaserScan fromScan;
2741  fromS->sensorData().uncompressData(0, 0, &fromScan);
2742 
2743  Signature * toS = _getSignature(toId);
2744  LaserScan toScan;
2745  toS->sensorData().uncompressData(0, 0, &toScan);
2746 
2747  Transform t;
2748  if(!fromScan.isEmpty() && !toScan.isEmpty())
2749  {
2750  Transform guess = poses.at(fromId).inverse() * poses.at(toId);
2751  float guessNorm = guess.getNorm();
2752  if(fromScan.maxRange() > 0.0f && toScan.maxRange() > 0.0f &&
2753  guessNorm > fromScan.maxRange() + toScan.maxRange())
2754  {
2755  // stop right known,it is impossible that scans overlay.
2756  UINFO("Too far scans between %d and %d to compute transformation: guessNorm=%f, scan range from=%f to=%f", fromId, toId, guessNorm, fromScan.maxRange(), toScan.maxRange());
2757  return t;
2758  }
2759 
2760  // Create a fake signature with all scans merged in oldId referential
2761  SensorData assembledData;
2762  Transform toPoseInv = poses.at(toId).inverse();
2763  std::string msg;
2764  int maxPoints = fromScan.size();
2765  pcl::PointCloud<pcl::PointXYZ>::Ptr assembledToClouds(new pcl::PointCloud<pcl::PointXYZ>);
2766  pcl::PointCloud<pcl::PointNormal>::Ptr assembledToNormalClouds(new pcl::PointCloud<pcl::PointNormal>);
2767  pcl::PointCloud<pcl::PointXYZI>::Ptr assembledToIClouds(new pcl::PointCloud<pcl::PointXYZI>);
2768  pcl::PointCloud<pcl::PointXYZINormal>::Ptr assembledToNormalIClouds(new pcl::PointCloud<pcl::PointXYZINormal>);
2769  for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
2770  {
2771  if(iter->first != fromId)
2772  {
2773  Signature * s = this->_getSignature(iter->first);
2774  if(!s->sensorData().laserScanCompressed().isEmpty())
2775  {
2776  LaserScan scan;
2777  s->sensorData().uncompressData(0, 0, &scan);
2778  if(!scan.isEmpty() && scan.format() == fromScan.format())
2779  {
2780  if(scan.hasIntensity())
2781  {
2782  if(scan.hasNormals())
2783  {
2784  *assembledToNormalIClouds += *util3d::laserScanToPointCloudINormal(scan,
2785  toPoseInv * iter->second * scan.localTransform());
2786  }
2787  else
2788  {
2789  *assembledToIClouds += *util3d::laserScanToPointCloudI(scan,
2790  toPoseInv * iter->second * scan.localTransform());
2791  }
2792  }
2793  else
2794  {
2795  if(scan.hasNormals())
2796  {
2797  *assembledToNormalClouds += *util3d::laserScanToPointCloudNormal(scan,
2798  toPoseInv * iter->second * scan.localTransform());
2799  }
2800  else
2801  {
2802  *assembledToClouds += *util3d::laserScanToPointCloud(scan,
2803  toPoseInv * iter->second * scan.localTransform());
2804  }
2805  }
2806 
2807  if(scan.size() > maxPoints)
2808  {
2809  maxPoints = scan.size();
2810  }
2811  }
2812  else if(!scan.isEmpty())
2813  {
2814  UWARN("Incompatible scan format %d vs %d", (int)fromScan.format(), (int)scan.format());
2815  }
2816  }
2817  else
2818  {
2819  UWARN("Depth2D not found for signature %d", iter->first);
2820  }
2821  }
2822  }
2823 
2824  cv::Mat assembledScan;
2825  if(assembledToNormalClouds->size())
2826  {
2827  assembledScan = fromScan.is2d()?util3d::laserScan2dFromPointCloud(*assembledToNormalClouds):util3d::laserScanFromPointCloud(*assembledToNormalClouds);
2828  }
2829  else if(assembledToClouds->size())
2830  {
2831  assembledScan = fromScan.is2d()?util3d::laserScan2dFromPointCloud(*assembledToClouds):util3d::laserScanFromPointCloud(*assembledToClouds);
2832  }
2833  else if(assembledToNormalIClouds->size())
2834  {
2835  assembledScan = fromScan.is2d()?util3d::laserScan2dFromPointCloud(*assembledToNormalIClouds):util3d::laserScanFromPointCloud(*assembledToNormalIClouds);
2836  }
2837  else if(assembledToIClouds->size())
2838  {
2839  assembledScan = fromScan.is2d()?util3d::laserScan2dFromPointCloud(*assembledToIClouds):util3d::laserScanFromPointCloud(*assembledToIClouds);
2840  }
2841 
2842  // scans are in base frame but for 2d scans, set the height so that correspondences matching works
2843  assembledData.setLaserScanRaw(
2844  LaserScan(assembledScan,
2845  fromScan.maxPoints()?fromScan.maxPoints():maxPoints,
2846  fromScan.maxRange(),
2847  fromScan.format(),
2848  fromScan.is2d()?Transform(0,0,fromScan.localTransform().z(),0,0,0):Transform::getIdentity()));
2849 
2850  t = _registrationIcpMulti->computeTransformation(fromS->sensorData(), assembledData, guess, info);
2851  }
2852 
2853  return t;
2854 }
2855 
2856 bool Memory::addLink(const Link & link, bool addInDatabase)
2857 {
2858  UASSERT(link.type() > Link::kNeighbor && link.type() != Link::kUndef);
2859 
2860  ULOGGER_INFO("to=%d, from=%d transform: %s var=%f", link.to(), link.from(), link.transform().prettyPrint().c_str(), link.transVariance());
2861  Signature * toS = _getSignature(link.to());
2862  Signature * fromS = _getSignature(link.from());
2863  if(toS && fromS)
2864  {
2865  if(toS->hasLink(link.from()))
2866  {
2867  // do nothing, already merged
2868  UINFO("already linked! to=%d, from=%d", link.to(), link.from());
2869  return true;
2870  }
2871 
2872  UDEBUG("Add link between %d and %d", toS->id(), fromS->id());
2873 
2874  toS->addLink(link.inverse());
2875  fromS->addLink(link);
2876 
2877  if(_incrementalMemory)
2878  {
2879  if(link.type()!=Link::kVirtualClosure)
2880  {
2881  _linksChanged = true;
2882 
2883  // update weight
2884  // ignore scan matching loop closures
2885  if(link.type() != Link::kLocalSpaceClosure ||
2886  link.userDataCompressed().empty())
2887  {
2888  _lastGlobalLoopClosureId = fromS->id()>toS->id()?fromS->id():toS->id();
2889 
2890  // update weights only if the memory is incremental
2891  // When reducing the graph, transfer weight to the oldest signature
2892  UASSERT(fromS->getWeight() >= 0 && toS->getWeight() >=0);
2893  if((_reduceGraph && fromS->id() < toS->id()) ||
2894  (!_reduceGraph && fromS->id() > toS->id()))
2895  {
2896  fromS->setWeight(fromS->getWeight() + toS->getWeight());
2897  toS->setWeight(0);
2898  }
2899  else
2900  {
2901  toS->setWeight(toS->getWeight() + fromS->getWeight());
2902  fromS->setWeight(0);
2903  }
2904  }
2905  }
2906  }
2907  }
2908  else if(!addInDatabase)
2909  {
2910  if(!fromS)
2911  {
2912  UERROR("from=%d, to=%d, Signature %d not found in working/st memories", link.from(), link.to(), link.from());
2913  }
2914  if(!toS)
2915  {
2916  UERROR("from=%d, to=%d, Signature %d not found in working/st memories", link.from(), link.to(), link.to());
2917  }
2918  return false;
2919  }
2920  else if(fromS)
2921  {
2922  UDEBUG("Add link between %d and %d (db)", link.from(), link.to());
2923  fromS->addLink(link);
2924  _dbDriver->addLink(link.inverse());
2925  }
2926  else if(toS)
2927  {
2928  UDEBUG("Add link between %d (db) and %d", link.from(), link.to());
2929  _dbDriver->addLink(link);
2930  toS->addLink(link.inverse());
2931  }
2932  else
2933  {
2934  UDEBUG("Add link between %d (db) and %d (db)", link.from(), link.to());
2935  _dbDriver->addLink(link);
2936  _dbDriver->addLink(link.inverse());
2937  }
2938  return true;
2939 }
2940 
2941 void Memory::updateLink(const Link & link, bool updateInDatabase)
2942 {
2943  Signature * fromS = this->_getSignature(link.from());
2944  Signature * toS = this->_getSignature(link.to());
2945 
2946  if(fromS && toS)
2947  {
2948  if(fromS->hasLink(link.to()) && toS->hasLink(link.from()))
2949  {
2950  Link::Type oldType = fromS->getLinks().at(link.to()).type();
2951 
2952  fromS->removeLink(link.to());
2953  toS->removeLink(link.from());
2954 
2955  fromS->addLink(link);
2956  toS->addLink(link.inverse());
2957 
2958  if(oldType!=Link::kVirtualClosure || link.type()!=Link::kVirtualClosure)
2959  {
2960  _linksChanged = true;
2961  }
2962  }
2963  else
2964  {
2965  UERROR("fromId=%d and toId=%d are not linked!", link.from(), link.to());
2966  }
2967  }
2968  else if(!updateInDatabase)
2969  {
2970  if(!fromS)
2971  {
2972  UERROR("from=%d, to=%d, Signature %d not found in working/st memories", link.from(), link.to(), link.from());
2973  }
2974  if(!toS)
2975  {
2976  UERROR("from=%d, to=%d, Signature %d not found in working/st memories", link.from(), link.to(), link.to());
2977  }
2978  }
2979  else if(fromS)
2980  {
2981  UDEBUG("Update link between %d and %d (db)", link.from(), link.to());
2982  fromS->removeLink(link.to());
2983  fromS->addLink(link);
2984  _dbDriver->updateLink(link.inverse());
2985  }
2986  else if(toS)
2987  {
2988  UDEBUG("Update link between %d (db) and %d", link.from(), link.to());
2989  toS->removeLink(link.from());
2990  toS->addLink(link.inverse());
2991  _dbDriver->updateLink(link);
2992  }
2993  else
2994  {
2995  UDEBUG("Update link between %d (db) and %d (db)", link.from(), link.to());
2996  _dbDriver->updateLink(link);
2997  _dbDriver->updateLink(link.inverse());
2998  }
2999 }
3000 
3001 void Memory::removeAllVirtualLinks()
3002 {
3003  UDEBUG("");
3004  for(std::map<int, Signature*>::iterator iter=_signatures.begin(); iter!=_signatures.end(); ++iter)
3005  {
3006  iter->second->removeVirtualLinks();
3007  }
3008 }
3009 
3010 void Memory::removeVirtualLinks(int signatureId)
3011 {
3012  UDEBUG("");
3013  Signature * s = this->_getSignature(signatureId);
3014  if(s)
3015  {
3016  const std::map<int, Link> & links = s->getLinks();
3017  for(std::map<int, Link>::const_iterator iter=links.begin(); iter!=links.end(); ++iter)
3018  {
3019  if(iter->second.type() == Link::kVirtualClosure)
3020  {
3021  Signature * sTo = this->_getSignature(iter->first);
3022  if(sTo)
3023  {
3024  sTo->removeLink(s->id());
3025  }
3026  else
3027  {
3028  UERROR("Link %d of %d not in WM/STM?!?", iter->first, s->id());
3029  }
3030  }
3031  }
3032  s->removeVirtualLinks();
3033  }
3034  else
3035  {
3036  UERROR("Signature %d not in WM/STM?!?", signatureId);
3037  }
3038 }
3039 
3040 void Memory::dumpMemory(std::string directory) const
3041 {
3042  UINFO("Dumping memory to directory \"%s\"", directory.c_str());
3043  this->dumpDictionary((directory+"/DumpMemoryWordRef.txt").c_str(), (directory+"/DumpMemoryWordDesc.txt").c_str());
3044  this->dumpSignatures((directory + "/DumpMemorySign.txt").c_str(), false);
3045  this->dumpSignatures((directory + "/DumpMemorySign3.txt").c_str(), true);
3046  this->dumpMemoryTree((directory + "/DumpMemoryTree.txt").c_str());
3047 }
3048 
3049 void Memory::dumpDictionary(const char * fileNameRef, const char * fileNameDesc) const
3050 {
3051  if(_vwd)
3052  {
3053  _vwd->exportDictionary(fileNameRef, fileNameDesc);
3054  }
3055 }
3056 
3057 void Memory::dumpSignatures(const char * fileNameSign, bool words3D) const
3058 {
3059  UDEBUG("");
3060  FILE* foutSign = 0;
3061 #ifdef _MSC_VER
3062  fopen_s(&foutSign, fileNameSign, "w");
3063 #else
3064  foutSign = fopen(fileNameSign, "w");
3065 #endif
3066 
3067  if(foutSign)
3068  {
3069  fprintf(foutSign, "SignatureID WordsID...\n");
3070  const std::map<int, Signature *> & signatures = this->getSignatures();
3071  for(std::map<int, Signature *>::const_iterator iter=signatures.begin(); iter!=signatures.end(); ++iter)
3072  {
3073  fprintf(foutSign, "%d ", iter->first);
3074  const Signature * ss = dynamic_cast<const Signature *>(iter->second);
3075  if(ss)
3076  {
3077  if(words3D)
3078  {
3079  const std::multimap<int, cv::Point3f> & ref = ss->getWords3();
3080  for(std::multimap<int, cv::Point3f>::const_iterator jter=ref.begin(); jter!=ref.end(); ++jter)
3081  {
3082  //show only valid point according to current parameters
3083  if(pcl::isFinite(jter->second) &&
3084  (jter->second.x != 0 || jter->second.y != 0 || jter->second.z != 0))
3085  {
3086  fprintf(foutSign, "%d ", (*jter).first);
3087  }
3088  }
3089  }
3090  else
3091  {
3092  const std::multimap<int, cv::KeyPoint> & ref = ss->getWords();
3093  for(std::multimap<int, cv::KeyPoint>::const_iterator jter=ref.begin(); jter!=ref.end(); ++jter)
3094  {
3095  fprintf(foutSign, "%d ", (*jter).first);
3096  }
3097  }
3098  }
3099  fprintf(foutSign, "\n");
3100  }
3101  fclose(foutSign);
3102  }
3103 }
3104 
3105 void Memory::dumpMemoryTree(const char * fileNameTree) const
3106 {
3107  UDEBUG("");
3108  FILE* foutTree = 0;
3109  #ifdef _MSC_VER
3110  fopen_s(&foutTree, fileNameTree, "w");
3111  #else
3112  foutTree = fopen(fileNameTree, "w");
3113  #endif
3114 
3115  if(foutTree)
3116  {
3117  fprintf(foutTree, "SignatureID Weight NbLoopClosureIds LoopClosureIds... NbChildLoopClosureIds ChildLoopClosureIds...\n");
3118 
3119  for(std::map<int, Signature *>::const_iterator i=_signatures.begin(); i!=_signatures.end(); ++i)
3120  {
3121  fprintf(foutTree, "%d %d", i->first, i->second->getWeight());
3122 
3123  std::map<int, Link> loopIds, childIds;
3124 
3125  for(std::map<int, Link>::const_iterator iter = i->second->getLinks().begin();
3126  iter!=i->second->getLinks().end();
3127  ++iter)
3128  {
3129  if(iter->second.type() != Link::kNeighbor &&
3130  iter->second.type() != Link::kNeighborMerged)
3131  {
3132  if(iter->first < i->first)
3133  {
3134  childIds.insert(*iter);
3135  }
3136  else if(iter->second.from() != iter->second.to())
3137  {
3138  loopIds.insert(*iter);
3139  }
3140  }
3141  }
3142 
3143  fprintf(foutTree, " %d", (int)loopIds.size());
3144  for(std::map<int, Link>::const_iterator j=loopIds.begin(); j!=loopIds.end(); ++j)
3145  {
3146  fprintf(foutTree, " %d", j->first);
3147  }
3148 
3149  fprintf(foutTree, " %d", (int)childIds.size());
3150  for(std::map<int, Link>::const_iterator j=childIds.begin(); j!=childIds.end(); ++j)
3151  {
3152  fprintf(foutTree, " %d", j->first);
3153  }
3154 
3155  fprintf(foutTree, "\n");
3156  }
3157 
3158  fclose(foutTree);
3159  }
3160 
3161 }
3162 
3163 void Memory::rehearsal(Signature * signature, Statistics * stats)
3164 {
3165  UTimer timer;
3166  if(signature->isBadSignature())
3167  {
3168  return;
3169  }
3170 
3171  //============================================================
3172  // Compare with the last (not intermediate node)
3173  //============================================================
3174  Signature * sB = 0;
3175  for(std::set<int>::reverse_iterator iter=_stMem.rbegin(); iter!=_stMem.rend(); ++iter)
3176  {
3177  Signature * s = this->_getSignature(*iter);
3178  UASSERT(s!=0);
3179  if(s->getWeight() >= 0 && s->id() != signature->id())
3180  {
3181  sB = s;
3182  break;
3183  }
3184  }
3185  if(sB)
3186  {
3187  int id = sB->id();
3188  UDEBUG("Comparing with signature (%d)...", id);
3189 
3190  float sim = signature->compareTo(*sB);
3191 
3192  int merged = 0;
3193  if(sim >= _similarityThreshold)
3194  {
3195  if(_incrementalMemory)
3196  {
3197  if(this->rehearsalMerge(id, signature->id()))
3198  {
3199  merged = id;
3200  }
3201  }
3202  else
3203  {
3204  signature->setWeight(signature->getWeight() + 1 + sB->getWeight());
3205  }
3206  }
3207 
3208  if(stats) stats->addStatistic(Statistics::kMemoryRehearsal_merged(), merged);
3209  if(stats) stats->addStatistic(Statistics::kMemoryRehearsal_sim(), sim);
3210  if(stats) stats->addStatistic(Statistics::kMemoryRehearsal_id(), sim >= _similarityThreshold?id:0);
3211  UDEBUG("merged=%d, sim=%f t=%fs", merged, sim, timer.ticks());
3212  }
3213  else
3214  {
3215  if(stats) stats->addStatistic(Statistics::kMemoryRehearsal_merged(), 0);
3216  if(stats) stats->addStatistic(Statistics::kMemoryRehearsal_sim(), 0);
3217  }
3218 }
3219 
3220 bool Memory::rehearsalMerge(int oldId, int newId)
3221 {
3222  ULOGGER_INFO("old=%d, new=%d", oldId, newId);
3223  Signature * oldS = _getSignature(oldId);
3224  Signature * newS = _getSignature(newId);
3225  if(oldS && newS && _incrementalMemory)
3226  {
3227  UASSERT_MSG(oldS->getWeight() >= 0 && newS->getWeight() >= 0, uFormat("%d %d", oldS->getWeight(), newS->getWeight()).c_str());
3228  std::map<int, Link>::const_iterator iter = oldS->getLinks().find(newS->id());
3229  if(iter != oldS->getLinks().end() &&
3230  iter->second.type() != Link::kNeighbor &&
3231  iter->second.type() != Link::kNeighborMerged &&
3232  iter->second.from() != iter->second.to())
3233  {
3234  // do nothing, already merged
3235  UWARN("already merged, old=%d, new=%d", oldId, newId);
3236  return false;
3237  }
3238  UASSERT(!newS->isSaved());
3239 
3240  UINFO("Rehearsal merging %d (w=%d) and %d (w=%d)",
3241  oldS->id(), oldS->getWeight(),
3242  newS->id(), newS->getWeight());
3243 
3244  bool fullMerge;
3245  bool intermediateMerge = false;
3246  if(!newS->getLinks().empty() && !newS->getLinks().begin()->second.transform().isNull())
3247  {
3248  // we are in metric SLAM mode:
3249  // 1) Normal merge if not moving AND has direct link
3250  // 2) Transform to intermediate node (weight = -1) if not moving AND hasn't direct link.
3251  float x,y,z, roll,pitch,yaw;
3252  newS->getLinks().begin()->second.transform().getTranslationAndEulerAngles(x,y,z, roll,pitch,yaw);
3253  bool isMoving = fabs(x) > _rehearsalMaxDistance ||
3254  fabs(y) > _rehearsalMaxDistance ||
3255  fabs(z) > _rehearsalMaxDistance ||
3256  fabs(roll) > _rehearsalMaxAngle ||
3257  fabs(pitch) > _rehearsalMaxAngle ||
3258  fabs(yaw) > _rehearsalMaxAngle;
3259  if(isMoving && _rehearsalWeightIgnoredWhileMoving)
3260  {
3261  UINFO("Rehearsal ignored because the robot has moved more than %f m or %f rad (\"Mem/RehearsalWeightIgnoredWhileMoving\"=true)",
3262  _rehearsalMaxDistance, _rehearsalMaxAngle);
3263  return false;
3264  }
3265  fullMerge = !isMoving && newS->hasLink(oldS->id());
3266  intermediateMerge = !isMoving && !newS->hasLink(oldS->id());
3267  }
3268  else
3269  {
3270  fullMerge = newS->hasLink(oldS->id()) && newS->getLinks().begin()->second.transform().isNull();
3271  }
3272 
3273  if(fullMerge)
3274  {
3275  //remove mutual links
3276  Link newToOldLink = newS->getLinks().at(oldS->id());
3277  oldS->removeLink(newId);
3278  newS->removeLink(oldId);
3279 
3280  if(_idUpdatedToNewOneRehearsal)
3281  {
3282  // redirect neighbor links
3283  const std::map<int, Link> & links = oldS->getLinks();
3284  for(std::map<int, Link>::const_iterator iter = links.begin(); iter!=links.end(); ++iter)
3285  {
3286  if(iter->second.from() != iter->second.to())
3287  {
3288  Link link = iter->second;
3289  Link mergedLink = newToOldLink.merge(link, link.type());
3290  UASSERT(mergedLink.from() == newS->id() && mergedLink.to() == link.to());
3291 
3292  Signature * s = this->_getSignature(link.to());
3293  if(s)
3294  {
3295  // modify neighbor "from"
3296  s->removeLink(oldS->id());
3297  s->addLink(mergedLink.inverse());
3298 
3299  newS->addLink(mergedLink);
3300  }
3301  else
3302  {
3303  UERROR("Didn't find neighbor %d of %d in RAM...", link.to(), oldS->id());
3304  }
3305  }
3306  }
3307  newS->setLabel(oldS->getLabel());
3308  oldS->setLabel("");
3309  oldS->removeLinks(); // remove all links
3310  oldS->addLink(Link(oldS->id(), newS->id(), Link::kGlobalClosure, Transform(), cv::Mat::eye(6,6,CV_64FC1))); // to keep track of the merged location
3311 
3312  // Set old image to new signature
3313  this->copyData(oldS, newS);
3314 
3315  // update weight
3316  newS->setWeight(newS->getWeight() + 1 + oldS->getWeight());
3317 
3318  if(_lastGlobalLoopClosureId == oldS->id())
3319  {
3320  _lastGlobalLoopClosureId = newS->id();
3321  }
3322  }
3323  else
3324  {
3325  newS->addLink(Link(newS->id(), oldS->id(), Link::kGlobalClosure, Transform() , cv::Mat::eye(6,6,CV_64FC1))); // to keep track of the merged location
3326 
3327  // update weight
3328  oldS->setWeight(newS->getWeight() + 1 + oldS->getWeight());
3329 
3330  if(_lastSignature == newS)
3331  {
3332  _lastSignature = oldS;
3333  }
3334  }
3335 
3336  // remove location
3337  moveToTrash(_idUpdatedToNewOneRehearsal?oldS:newS, _notLinkedNodesKeptInDb);
3338 
3339  return true;
3340  }
3341  else
3342  {
3343  // update only weights
3344  if(_idUpdatedToNewOneRehearsal)
3345  {
3346  // just update weight
3347  int w = oldS->getWeight()>=0?oldS->getWeight():0;
3348  newS->setWeight(w + newS->getWeight() + 1);
3349  oldS->setWeight(intermediateMerge?-1:0); // convert to intermediate node
3350 
3351  if(_lastGlobalLoopClosureId == oldS->id())
3352  {
3353  _lastGlobalLoopClosureId = newS->id();
3354  }
3355  }
3356  else // !_idUpdatedToNewOneRehearsal
3357  {
3358  int w = newS->getWeight()>=0?newS->getWeight():0;
3359  oldS->setWeight(w + oldS->getWeight() + 1);
3360  newS->setWeight(intermediateMerge?-1:0); // convert to intermediate node
3361  }
3362  }
3363  }
3364  else
3365  {
3366  if(!newS)
3367  {
3368  UERROR("newId=%d, oldId=%d, Signature %d not found in working/st memories", newId, oldId, newId);
3369  }
3370  if(!oldS)
3371  {
3372  UERROR("newId=%d, oldId=%d, Signature %d not found in working/st memories", newId, oldId, oldId);
3373  }
3374  }
3375  return false;
3376 }
3377 
3378 Transform Memory::getOdomPose(int signatureId, bool lookInDatabase) const
3379 {
3380  Transform pose, groundTruth;
3381  int mapId, weight;
3382  std::string label;
3383  double stamp;
3384  std::vector<float> velocity;
3385  GPS gps;
3386  getNodeInfo(signatureId, pose, mapId, weight, label, stamp, groundTruth, velocity, gps, lookInDatabase);
3387  return pose;
3388 }
3389 
3390 Transform Memory::getGroundTruthPose(int signatureId, bool lookInDatabase) const
3391 {
3392  Transform pose, groundTruth;
3393  int mapId, weight;
3394  std::string label;
3395  double stamp;
3396  std::vector<float> velocity;
3397  GPS gps;
3398  getNodeInfo(signatureId, pose, mapId, weight, label, stamp, groundTruth, velocity, gps, lookInDatabase);
3399  return groundTruth;
3400 }
3401 
3402 bool Memory::getNodeInfo(int signatureId,
3403  Transform & odomPose,
3404  int & mapId,
3405  int & weight,
3406  std::string & label,
3407  double & stamp,
3408  Transform & groundTruth,
3409  std::vector<float> & velocity,
3410  GPS & gps,
3411  bool lookInDatabase) const
3412 {
3413  const Signature * s = this->getSignature(signatureId);
3414  if(s)
3415  {
3416  odomPose = s->getPose();
3417  mapId = s->mapId();
3418  weight = s->getWeight();
3419  label = s->getLabel();
3420  stamp = s->getStamp();
3421  groundTruth = s->getGroundTruthPose();
3422  velocity = s->getVelocity();
3423  gps = s->sensorData().gps();
3424  return true;
3425  }
3426  else if(lookInDatabase && _dbDriver)
3427  {
3428  return _dbDriver->getNodeInfo(signatureId, odomPose, mapId, weight, label, stamp, groundTruth, velocity, gps);
3429  }
3430  return false;
3431 }
3432 
3433 cv::Mat Memory::getImageCompressed(int signatureId) const
3434 {
3435  cv::Mat image;
3436  const Signature * s = this->getSignature(signatureId);
3437  if(s)
3438  {
3439  image = s->sensorData().imageCompressed();
3440  }
3441  if(image.empty() && this->isBinDataKept() && _dbDriver)
3442  {
3443  SensorData data;
3444  _dbDriver->getNodeData(signatureId, data);
3445  image = data.imageCompressed();
3446  }
3447  return image;
3448 }
3449 
3450 SensorData Memory::getNodeData(int nodeId, bool uncompressedData) const
3451 {
3452  UDEBUG("nodeId=%d", nodeId);
3453  SensorData r;
3454  Signature * s = this->_getSignature(nodeId);
3455  if(s && !s->sensorData().imageCompressed().empty())
3456  {
3457  r = s->sensorData();
3458  }
3459  else if(_dbDriver)
3460  {
3461  // load from database
3462  _dbDriver->getNodeData(nodeId, r);
3463  }
3464 
3465  if(uncompressedData)
3466  {
3467  r.uncompressData();
3468  }
3469 
3470  return r;
3471 }
3472 
3473 void Memory::getNodeWords(int nodeId,
3474  std::multimap<int, cv::KeyPoint> & words,
3475  std::multimap<int, cv::Point3f> & words3,
3476  std::multimap<int, cv::Mat> & wordsDescriptors)
3477 {
3478  UDEBUG("nodeId=%d", nodeId);
3479  Signature * s = this->_getSignature(nodeId);
3480  if(s)
3481  {
3482  words = s->getWords();
3483  words3 = s->getWords3();
3484  wordsDescriptors = s->getWordsDescriptors();
3485  }
3486  else if(_dbDriver)
3487  {
3488  // load from database
3489  std::list<Signature*> signatures;
3490  std::list<int> ids;
3491  ids.push_back(nodeId);
3492  std::set<int> loadedFromTrash;
3493  _dbDriver->loadSignatures(ids, signatures, &loadedFromTrash);
3494  if(signatures.size())
3495  {
3496  words = signatures.front()->getWords();
3497  words3 = signatures.front()->getWords3();
3498  wordsDescriptors = signatures.front()->getWordsDescriptors();
3499  if(loadedFromTrash.size())
3500  {
3501  //put back
3502  _dbDriver->asyncSave(signatures.front());
3503  }
3504  else
3505  {
3506  delete signatures.front();
3507  }
3508  }
3509  }
3510 }
3511 
3512 void Memory::getNodeCalibration(int nodeId,
3513  std::vector<CameraModel> & models,
3514  StereoCameraModel & stereoModel)
3515 {
3516  UDEBUG("nodeId=%d", nodeId);
3517  Signature * s = this->_getSignature(nodeId);
3518  if(s)
3519  {
3520  models = s->sensorData().cameraModels();
3521  stereoModel = s->sensorData().stereoCameraModel();
3522  }
3523  else if(_dbDriver)
3524  {
3525  // load from database
3526  _dbDriver->getCalibration(nodeId, models, stereoModel);
3527  }
3528 }
3529 
3530 SensorData Memory::getSignatureDataConst(int locationId,
3531  bool images, bool scan, bool userData, bool occupancyGrid) const
3532 {
3533  UDEBUG("");
3534  SensorData r;
3535  const Signature * s = this->getSignature(locationId);
3536  if(s && (!s->sensorData().imageCompressed().empty() ||
3537  !s->sensorData().laserScanCompressed().isEmpty() ||
3538  !s->sensorData().userDataCompressed().empty() ||
3539  s->sensorData().gridCellSize() != 0.0f))
3540  {
3541  r = s->sensorData();
3542  }
3543  else if(_dbDriver)
3544  {
3545  // load from database
3546  _dbDriver->getNodeData(locationId, r, images, scan, userData, occupancyGrid);
3547  }
3548 
3549  return r;
3550 }
3551 
3552 void Memory::generateGraph(const std::string & fileName, const std::set<int> & ids)
3553 {
3554  if(!_dbDriver)
3555  {
3556  UERROR("A database must must loaded first...");
3557  return;
3558  }
3559 
3560  _dbDriver->generateGraph(fileName, ids, _signatures);
3561 }
3562 
3563 int Memory::getNi(int signatureId) const
3564 {
3565  int ni = 0;
3566  const Signature * s = this->getSignature(signatureId);
3567  if(s)
3568  {
3569  ni = (int)((Signature *)s)->getWords().size();
3570  }
3571  else
3572  {
3573  _dbDriver->getInvertedIndexNi(signatureId, ni);
3574  }
3575  return ni;
3576 }
3577 
3578 
3579 void Memory::copyData(const Signature * from, Signature * to)
3580 {
3581  UTimer timer;
3582  timer.start();
3583  if(from && to)
3584  {
3585  // words 2d
3586  this->disableWordsRef(to->id());
3587  to->setWords(from->getWords());
3588  std::list<int> id;
3589  id.push_back(to->id());
3590  this->enableWordsRef(id);
3591 
3592  if(from->isSaved() && _dbDriver)
3593  {
3594  _dbDriver->getNodeData(from->id(), to->sensorData());
3595  UDEBUG("Loaded image data from database");
3596  }
3597  else
3598  {
3599  to->sensorData() = (SensorData)from->sensorData();
3600  }
3601  to->sensorData().setId(to->id());
3602 
3603  to->setPose(from->getPose());
3604  to->setWords3(from->getWords3());
3605  to->setWordsDescriptors(from->getWordsDescriptors());
3606  }
3607  else
3608  {
3609  ULOGGER_ERROR("Can't merge the signatures because there are not same type.");
3610  }
3611  UDEBUG("Merging time = %fs", timer.ticks());
3612 }
3613 
3614 class PreUpdateThread : public UThreadNode
3615 {
3616 public:
3617  PreUpdateThread(VWDictionary * vwp) : _vwp(vwp) {}
3618  virtual ~PreUpdateThread() {}
3619 private:
3620  void mainLoop() {
3621  if(_vwp)
3622  {
3623  _vwp->update();
3624  }
3625  this->kill();
3626  }
3627  VWDictionary * _vwp;
3628 };
3629 
3630 Signature * Memory::createSignature(const SensorData & inputData, const Transform & pose, Statistics * stats)
3631 {
3632  UDEBUG("");
3633  SensorData data = inputData;
3634  UASSERT(data.imageRaw().empty() ||
3635  data.imageRaw().type() == CV_8UC1 ||
3636  data.imageRaw().type() == CV_8UC3);
3637  UASSERT_MSG(data.depthOrRightRaw().empty() ||
3638  ( ( data.depthOrRightRaw().type() == CV_16UC1 ||
3639  data.depthOrRightRaw().type() == CV_32FC1 ||
3640  data.depthOrRightRaw().type() == CV_8UC1)
3641  &&
3642  ( (data.imageRaw().empty() && data.depthOrRightRaw().type() != CV_8UC1) ||
3643  (data.imageRaw().rows % data.depthOrRightRaw().rows == 0 && data.imageRaw().cols % data.depthOrRightRaw().cols == 0))),
3644  uFormat("image=(%d/%d) depth=(%d/%d, type=%d [accepted=%d,%d,%d])",
3645  data.imageRaw().cols,
3646  data.imageRaw().rows,
3647  data.depthOrRightRaw().cols,
3648  data.depthOrRightRaw().rows,
3649  data.depthOrRightRaw().type(),
3650  CV_16UC1, CV_32FC1, CV_8UC1).c_str());
3651 
3652  if(!data.depthOrRightRaw().empty() &&
3653  data.cameraModels().size() == 0 &&
3654  !data.stereoCameraModel().isValidForProjection() &&
3655  !pose.isNull())
3656  {
3657  UERROR("Camera calibration not valid, calibrate your camera!");
3658  return 0;
3659  }
3660  UASSERT(_feature2D != 0);
3661 
3662  PreUpdateThread preUpdateThread(_vwd);
3663 
3664  UTimer timer;
3665  timer.start();
3666  float t;
3667  std::vector<cv::KeyPoint> keypoints;
3668  cv::Mat descriptors;
3669  bool isIntermediateNode = data.id() < 0 || data.imageRaw().empty();
3670  int id = data.id();
3671  if(_generateIds)
3672  {
3673  id = this->getNextId();
3674  }
3675  else
3676  {
3677  if(id <= 0)
3678  {
3679  UERROR("Received image ID is null. "
3680  "Please set parameter Mem/GenerateIds to \"true\" or "
3681  "make sure the input source provides image ids (seq).");
3682  return 0;
3683  }
3684  else if(id > _idCount)
3685  {
3686  _idCount = id;
3687  }
3688  else
3689  {
3690  UERROR("Id of acquired image (%d) is smaller than the last in memory (%d). "
3691  "Please set parameter Mem/GenerateIds to \"true\" or "
3692  "make sure the input source provides image ids (seq) over the last in "
3693  "memory, which is %d.",
3694  id,
3695  _idCount,
3696  _idCount);
3697  return 0;
3698  }
3699  }
3700 
3701  bool imagesRectified = _imagesAlreadyRectified;
3702  // Stereo must be always rectified because of the stereo correspondence approach
3703  if(!imagesRectified && !data.imageRaw().empty() && !(_rectifyOnlyFeatures && data.rightRaw().empty()))
3704  {
3705  // we assume that once rtabmap is receiving data, the calibration won't change over time
3706  if(data.cameraModels().size())
3707  {
3708  // Note that only RGB image is rectified, the depth image is assumed to be already registered to rectified RGB camera.
3709  UASSERT(int((data.imageRaw().cols/data.cameraModels().size())*data.cameraModels().size()) == data.imageRaw().cols);
3710  int subImageWidth = data.imageRaw().cols/data.cameraModels().size();
3711  cv::Mat rectifiedImages(data.imageRaw().size(), data.imageRaw().type());
3712  bool initRectMaps = _rectCameraModels.empty();
3713  if(initRectMaps)
3714  {
3715  _rectCameraModels.resize(data.cameraModels().size());
3716  }
3717  for(unsigned int i=0; i<data.cameraModels().size(); ++i)
3718  {
3719  if(data.cameraModels()[i].isValidForRectification())
3720  {
3721  if(initRectMaps)
3722  {
3723  _rectCameraModels[i] = data.cameraModels()[i];
3724  if(!_rectCameraModels[i].isRectificationMapInitialized())
3725  {
3726  UWARN("Initializing rectification maps for camera %d (only done for the first image received)...", i);
3727  _rectCameraModels[i].initRectificationMap();
3728  UWARN("Initializing rectification maps for camera %d (only done for the first image received)... done!", i);
3729  }
3730  }
3731  UASSERT(_rectCameraModels[i].imageWidth() == data.cameraModels()[i].imageWidth() &&
3732  _rectCameraModels[i].imageHeight() == data.cameraModels()[i].imageHeight());
3733  cv::Mat rectifiedImage = _rectCameraModels[i].rectifyImage(cv::Mat(data.imageRaw(), cv::Rect(subImageWidth*i, 0, subImageWidth, data.imageRaw().rows)));
3734  rectifiedImage.copyTo(cv::Mat(rectifiedImages, cv::Rect(subImageWidth*i, 0, subImageWidth, data.imageRaw().rows)));
3735  imagesRectified = true;
3736  }
3737  else
3738  {
3739  UERROR("Calibration for camera %d cannot be used to rectify the image. Make sure to do a "
3740  "full calibration. If images are already rectified, set %s parameter back to true.",
3741  (int)i,
3742  Parameters::kRtabmapImagesAlreadyRectified().c_str());
3743  return 0;
3744  }
3745  }
3746  data.setImageRaw(rectifiedImages);
3747  }
3748  else if(data.stereoCameraModel().isValidForRectification())
3749  {
3750  if(!_rectStereoCameraModel.isValidForRectification())
3751  {
3752  _rectStereoCameraModel = data.stereoCameraModel();
3753  if(!_rectStereoCameraModel.isRectificationMapInitialized())
3754  {
3755  UWARN("Initializing rectification maps (only done for the first image received)...");
3756  _rectStereoCameraModel.initRectificationMap();
3757  UWARN("Initializing rectification maps (only done for the first image received)...done!");
3758  }
3759  }
3760  UASSERT(_rectStereoCameraModel.left().imageWidth() == data.stereoCameraModel().left().imageWidth());
3761  UASSERT(_rectStereoCameraModel.left().imageHeight() == data.stereoCameraModel().left().imageHeight());
3762  data.setImageRaw(_rectStereoCameraModel.left().rectifyImage(data.imageRaw()));
3763  data.setDepthOrRightRaw(_rectStereoCameraModel.right().rectifyImage(data.rightRaw()));
3764  imagesRectified = true;
3765  }
3766  else
3767  {
3768  UERROR("Stereo calibration cannot be used to rectify images. Make sure to do a "
3769  "full stereo calibration. If images are already rectified, set %s parameter back to true.",
3770  Parameters::kRtabmapImagesAlreadyRectified().c_str());
3771  return 0;
3772  }
3773  t = timer.ticks();
3774  if(stats) stats->addStatistic(Statistics::kTimingMemRectification(), t*1000.0f);
3775  UDEBUG("time rectification = %fs", t);
3776  }
3777 
3778  int treeSize= int(_workingMem.size() + _stMem.size());
3779  int meanWordsPerLocation = _feature2D->getMaxFeatures()>0?_feature2D->getMaxFeatures():0;
3780  if(treeSize > 1)
3781  {
3782  meanWordsPerLocation = _vwd->getTotalActiveReferences() / (treeSize-1); // ignore virtual signature
3783  }
3784 
3785  if(_parallelized && !isIntermediateNode)
3786  {
3787  UDEBUG("Start dictionary update thread");
3788  preUpdateThread.start();
3789  }
3790 
3791  int preDecimation = 1;
3792  std::vector<cv::Point3f> keypoints3D;
3793  SensorData decimatedData;
3794  if(!_useOdometryFeatures || data.keypoints().empty() || (int)data.keypoints().size() != data.descriptors().rows)
3795  {
3796  if(_feature2D->getMaxFeatures() >= 0 && !data.imageRaw().empty() && !isIntermediateNode)
3797  {
3798  decimatedData = data;
3799  if(_imagePreDecimation > 1)
3800  {
3801  preDecimation = _imagePreDecimation;
3802  if(!decimatedData.rightRaw().empty() ||
3803  (decimatedData.depthRaw().rows == decimatedData.imageRaw().rows && decimatedData.depthRaw().cols == decimatedData.imageRaw().cols))
3804  {
3805  decimatedData.setDepthOrRightRaw(util2d::decimate(decimatedData.depthOrRightRaw(), _imagePreDecimation));
3806  }
3807  decimatedData.setImageRaw(util2d::decimate(decimatedData.imageRaw(), _imagePreDecimation));
3808  std::vector<CameraModel> cameraModels = decimatedData.cameraModels();
3809  for(unsigned int i=0; i<cameraModels.size(); ++i)
3810  {
3811  cameraModels[i] = cameraModels[i].scaled(1.0/double(_imagePreDecimation));
3812  }
3813  decimatedData.setCameraModels(cameraModels);
3814  StereoCameraModel stereoModel = decimatedData.stereoCameraModel();
3815  if(stereoModel.isValidForProjection())
3816  {
3817  stereoModel.scale(1.0/double(_imagePreDecimation));
3818  }
3819  decimatedData.setStereoCameraModel(stereoModel);
3820  }
3821 
3822  UINFO("Extract features");
3823  cv::Mat imageMono;
3824  if(decimatedData.imageRaw().channels() == 3)
3825  {
3826  cv::cvtColor(decimatedData.imageRaw(), imageMono, CV_BGR2GRAY);
3827  }
3828  else
3829  {
3830  imageMono = decimatedData.imageRaw();
3831  }
3832 
3833  cv::Mat depthMask;
3834  if(imagesRectified && !decimatedData.depthRaw().empty() && _depthAsMask)
3835  {
3836  if(imageMono.rows % decimatedData.depthRaw().rows == 0 &&
3837  imageMono.cols % decimatedData.depthRaw().cols == 0 &&
3838  imageMono.rows/decimatedData.depthRaw().rows == imageMono.cols/decimatedData.depthRaw().cols)
3839  {
3840  depthMask = util2d::interpolate(decimatedData.depthRaw(), imageMono.rows/decimatedData.depthRaw().rows, 0.1f);
3841  }
3842  }
3843 
3844  int oldMaxFeatures = _feature2D->getMaxFeatures();
3845  UDEBUG("rawDescriptorsKept=%d, pose=%d, maxFeatures=%d, visMaxFeatures=%d", _rawDescriptorsKept?1:0, pose.isNull()?0:1, _feature2D->getMaxFeatures(), _visMaxFeatures);
3846  ParametersMap tmpMaxFeatureParameter;
3847  if(_rawDescriptorsKept&&!pose.isNull()&&_feature2D->getMaxFeatures()>0&&_feature2D->getMaxFeatures()<_visMaxFeatures)
3848  {
3849  // The total extracted features should match the number of features used for transformation estimation
3850  UDEBUG("Changing temporary max features from %d to %d", _feature2D->getMaxFeatures(), _visMaxFeatures);
3851  tmpMaxFeatureParameter.insert(ParametersPair(Parameters::kKpMaxFeatures(), uNumber2Str(_visMaxFeatures)));
3852  _feature2D->parseParameters(tmpMaxFeatureParameter);
3853  }
3854 
3855  keypoints = _feature2D->generateKeypoints(
3856  imageMono,
3857  depthMask);
3858 
3859  if(tmpMaxFeatureParameter.size())
3860  {
3861  tmpMaxFeatureParameter.at(Parameters::kKpMaxFeatures()) = uNumber2Str(oldMaxFeatures);
3862  _feature2D->parseParameters(tmpMaxFeatureParameter); // reset back
3863  }
3864  t = timer.ticks();
3865  if(stats) stats->addStatistic(Statistics::kTimingMemKeypoints_detection(), t*1000.0f);
3866  UDEBUG("time keypoints (%d) = %fs", (int)keypoints.size(), t);
3867 
3868  descriptors = _feature2D->generateDescriptors(imageMono, keypoints);
3869  t = timer.ticks();
3870  if(stats) stats->addStatistic(Statistics::kTimingMemDescriptors_extraction(), t*1000.0f);
3871  UDEBUG("time descriptors (%d) = %fs", descriptors.rows, t);
3872 
3873  UDEBUG("ratio=%f, meanWordsPerLocation=%d", _badSignRatio, meanWordsPerLocation);
3874  if(descriptors.rows && descriptors.rows < _badSignRatio * float(meanWordsPerLocation))
3875  {
3876  descriptors = cv::Mat();
3877  }
3878  else
3879  {
3880  if(!imagesRectified && decimatedData.cameraModels().size())
3881  {
3882  std::vector<cv::KeyPoint> keypointsValid;
3883  keypointsValid.reserve(keypoints.size());
3884  cv::Mat descriptorsValid;
3885  descriptorsValid.reserve(descriptors.rows);
3886 
3887  //undistort keypoints before projection (RGB-D)
3888  if(decimatedData.cameraModels().size() == 1)
3889  {
3890  std::vector<cv::Point2f> pointsIn, pointsOut;
3891  cv::KeyPoint::convert(keypoints,pointsIn);
3892  if(decimatedData.cameraModels()[0].D_raw().cols == 6)
3893  {
3894 #if CV_MAJOR_VERSION > 2 or (CV_MAJOR_VERSION == 2 and (CV_MINOR_VERSION >4 or (CV_MINOR_VERSION == 4 and CV_SUBMINOR_VERSION >=10)))
3895  // Equidistant / FishEye
3896  // get only k parameters (k1,k2,p1,p2,k3,k4)
3897  cv::Mat D(1, 4, CV_64FC1);
3898  D.at<double>(0,0) = decimatedData.cameraModels()[0].D_raw().at<double>(0,1);
3899  D.at<double>(0,1) = decimatedData.cameraModels()[0].D_raw().at<double>(0,1);
3900  D.at<double>(0,2) = decimatedData.cameraModels()[0].D_raw().at<double>(0,4);
3901  D.at<double>(0,3) = decimatedData.cameraModels()[0].D_raw().at<double>(0,5);
3902  cv::fisheye::undistortPoints(pointsIn, pointsOut,
3903  decimatedData.cameraModels()[0].K_raw(),
3904  D,
3905  decimatedData.cameraModels()[0].R(),
3906  decimatedData.cameraModels()[0].P());
3907  }
3908  else
3909 #else
3910  UWARN("Too old opencv version (%d,%d,%d) to support fisheye model (min 2.4.10 required)!",
3911  CV_MAJOR_VERSION, CV_MINOR_VERSION, CV_SUBMINOR_VERSION);
3912  }
3913 #endif
3914  {
3915  //RadialTangential
3916  cv::undistortPoints(pointsIn, pointsOut,
3917  decimatedData.cameraModels()[0].K_raw(),
3918  decimatedData.cameraModels()[0].D_raw(),
3919  decimatedData.cameraModels()[0].R(),
3920  decimatedData.cameraModels()[0].P());
3921  }
3922  UASSERT(pointsOut.size() == keypoints.size());
3923  for(unsigned int i=0; i<pointsOut.size(); ++i)
3924  {
3925  if(pointsOut.at(i).x>=0 && pointsOut.at(i).x<decimatedData.cameraModels()[0].imageWidth() &&
3926  pointsOut.at(i).y>=0 && pointsOut.at(i).y<decimatedData.cameraModels()[0].imageHeight())
3927  {
3928  keypointsValid.push_back(keypoints.at(i));
3929  keypointsValid.back().pt.x = pointsOut.at(i).x;
3930  keypointsValid.back().pt.y = pointsOut.at(i).y;
3931  descriptorsValid.push_back(descriptors.row(i));
3932  }
3933  }
3934  }
3935  else
3936  {
3937  UASSERT(int((decimatedData.imageRaw().cols/decimatedData.cameraModels().size())*decimatedData.cameraModels().size()) == decimatedData.imageRaw().cols);
3938  float subImageWidth = decimatedData.imageRaw().cols/decimatedData.cameraModels().size();
3939  for(unsigned int i=0; i<keypoints.size(); ++i)
3940  {
3941  int cameraIndex = int(keypoints.at(i).pt.x / subImageWidth);
3942  UASSERT_MSG(cameraIndex >= 0 && cameraIndex < (int)decimatedData.cameraModels().size(),
3943  uFormat("cameraIndex=%d, models=%d, kpt.x=%f, subImageWidth=%f (Camera model image width=%d)",
3944  cameraIndex, (int)decimatedData.cameraModels().size(), keypoints[i].pt.x, subImageWidth, decimatedData.cameraModels()[0].imageWidth()).c_str());
3945 
3946  std::vector<cv::Point2f> pointsIn, pointsOut;
3947  pointsIn.push_back(cv::Point2f(keypoints.at(i).pt.x-subImageWidth*cameraIndex, keypoints.at(i).pt.y));
3948  if(decimatedData.cameraModels()[cameraIndex].D_raw().cols == 6)
3949  {
3950 #if CV_MAJOR_VERSION > 2 or (CV_MAJOR_VERSION == 2 and (CV_MINOR_VERSION >4 or (CV_MINOR_VERSION == 4 and CV_SUBMINOR_VERSION >=10)))
3951  // Equidistant / FishEye
3952  // get only k parameters (k1,k2,p1,p2,k3,k4)
3953  cv::Mat D(1, 4, CV_64FC1);
3954  D.at<double>(0,0) = decimatedData.cameraModels()[cameraIndex].D_raw().at<double>(0,1);
3955  D.at<double>(0,1) = decimatedData.cameraModels()[cameraIndex].D_raw().at<double>(0,1);
3956  D.at<double>(0,2) = decimatedData.cameraModels()[cameraIndex].D_raw().at<double>(0,4);
3957  D.at<double>(0,3) = decimatedData.cameraModels()[cameraIndex].D_raw().at<double>(0,5);
3958  cv::fisheye::undistortPoints(pointsIn, pointsOut,
3959  decimatedData.cameraModels()[cameraIndex].K_raw(),
3960  D,
3961  decimatedData.cameraModels()[cameraIndex].R(),
3962  decimatedData.cameraModels()[cameraIndex].P());
3963  }
3964  else
3965 #else
3966  UWARN("Too old opencv version (%d,%d,%d) to support fisheye model (min 2.4.10 required)!",
3967  CV_MAJOR_VERSION, CV_MINOR_VERSION, CV_SUBMINOR_VERSION);
3968  }
3969 #endif
3970  {
3971  //RadialTangential
3972  cv::undistortPoints(pointsIn, pointsOut,
3973  decimatedData.cameraModels()[cameraIndex].K_raw(),
3974  decimatedData.cameraModels()[cameraIndex].D_raw(),
3975  decimatedData.cameraModels()[cameraIndex].R(),
3976  decimatedData.cameraModels()[cameraIndex].P());
3977  }
3978 
3979  if(pointsOut[0].x>=0 && pointsOut[0].x<decimatedData.cameraModels()[cameraIndex].imageWidth() &&
3980  pointsOut[0].y>=0 && pointsOut[0].y<decimatedData.cameraModels()[cameraIndex].imageHeight())
3981  {
3982  keypointsValid.push_back(keypoints.at(i));
3983  keypointsValid.back().pt.x = pointsOut[0].x + subImageWidth*cameraIndex;
3984  keypointsValid.back().pt.y = pointsOut[0].y;
3985  descriptorsValid.push_back(descriptors.row(i));
3986  }
3987  }
3988  }
3989 
3990  keypoints = keypointsValid;
3991  descriptors = descriptorsValid;
3992 
3993  t = timer.ticks();
3994  if(stats) stats->addStatistic(Statistics::kTimingMemRectification(), t*1000.0f);
3995  UDEBUG("time rectification = %fs", t);
3996  }
3997 
3998  if((!decimatedData.depthRaw().empty() && decimatedData.cameraModels().size() && decimatedData.cameraModels()[0].isValidForProjection()) ||
3999  (!decimatedData.rightRaw().empty() && decimatedData.stereoCameraModel().isValidForProjection()))
4000  {
4001  keypoints3D = _feature2D->generateKeypoints3D(decimatedData, keypoints);
4002  t = timer.ticks();
4003  if(stats) stats->addStatistic(Statistics::kTimingMemKeypoints_3D(), t*1000.0f);
4004  UDEBUG("time keypoints 3D (%d) = %fs", (int)keypoints3D.size(), t);
4005  }
4006  }
4007  }
4008  else if(data.imageRaw().empty())
4009  {
4010  UDEBUG("Empty image, cannot extract features...");
4011  }
4012  else if(_feature2D->getMaxFeatures() < 0)
4013  {
4014  UDEBUG("_feature2D->getMaxFeatures()(%d<0) so don't extract any features...", _feature2D->getMaxFeatures());
4015  }
4016  else
4017  {
4018  UDEBUG("Intermediate node detected, don't extract features!");
4019  }
4020  }
4021  else if(_feature2D->getMaxFeatures() >= 0 && !isIntermediateNode)
4022  {
4023  UINFO("Use odometry features: kpts=%d 3d=%d desc=%d (dim=%d, type=%d)",
4024  (int)data.keypoints().size(),
4025  (int)data.keypoints3D().size(),
4026  data.descriptors().rows,
4027  data.descriptors().cols,
4028  data.descriptors().type());
4029  keypoints = data.keypoints();
4030  keypoints3D = data.keypoints3D();
4031  descriptors = data.descriptors().clone();
4032 
4033  UASSERT(descriptors.empty() || descriptors.rows == (int)keypoints.size());
4034  UASSERT(keypoints3D.empty() || keypoints3D.size() == keypoints.size());
4035 
4036  int maxFeatures = _rawDescriptorsKept&&!pose.isNull()&&_feature2D->getMaxFeatures()>0&&_feature2D->getMaxFeatures()<_visMaxFeatures?_visMaxFeatures:_feature2D->getMaxFeatures();
4037  if((int)keypoints.size() > maxFeatures)
4038  {
4039  _feature2D->limitKeypoints(keypoints, keypoints3D, descriptors, maxFeatures);
4040  }
4041  t = timer.ticks();
4042  if(stats) stats->addStatistic(Statistics::kTimingMemKeypoints_detection(), t*1000.0f);
4043  UDEBUG("time keypoints (%d) = %fs", (int)keypoints.size(), t);
4044 
4045  if(descriptors.empty())
4046  {
4047  cv::Mat imageMono;
4048  if(data.imageRaw().channels() == 3)
4049  {
4050  cv::cvtColor(data.imageRaw(), imageMono, CV_BGR2GRAY);
4051  }
4052  else
4053  {
4054  imageMono = data.imageRaw();
4055  }
4056 
4057  UASSERT_MSG(imagesRectified, "Cannot extract descriptors on not rectified image from keypoints which assumed to be undistorted");
4058 
4059  descriptors = _feature2D->generateDescriptors(imageMono, keypoints);
4060  }
4061  t = timer.ticks();
4062  if(stats) stats->addStatistic(Statistics::kTimingMemDescriptors_extraction(), t*1000.0f);
4063  UDEBUG("time descriptors (%d) = %fs", descriptors.rows, t);
4064 
4065  if(keypoints3D.empty() &&
4066  ((!data.depthRaw().empty() && data.cameraModels().size() && data.cameraModels()[0].isValidForProjection()) ||
4067  (!data.rightRaw().empty() && data.stereoCameraModel().isValidForProjection())))
4068  {
4069  keypoints3D = _feature2D->generateKeypoints3D(data, keypoints);
4070  if(_feature2D->getMinDepth() > 0.0f || _feature2D->getMaxDepth() > 0.0f)
4071  {
4072  UDEBUG("");
4073  //remove all keypoints/descriptors with no valid 3D points
4074  UASSERT((int)keypoints.size() == descriptors.rows &&
4075  keypoints3D.size() == keypoints.size());
4076  std::vector<cv::KeyPoint> validKeypoints(keypoints.size());
4077  std::vector<cv::Point3f> validKeypoints3D(keypoints.size());
4078  cv::Mat validDescriptors(descriptors.size(), descriptors.type());
4079 
4080  int oi=0;
4081  for(unsigned int i=0; i<keypoints3D.size(); ++i)
4082  {
4083  if(util3d::isFinite(keypoints3D[i]))
4084  {
4085  validKeypoints[oi] = keypoints[i];
4086  validKeypoints3D[oi] = keypoints3D[i];
4087  descriptors.row(i).copyTo(validDescriptors.row(oi));
4088  ++oi;
4089  }
4090  }
4091  UDEBUG("Removed %d invalid 3D points", (int)keypoints3D.size()-oi);
4092  validKeypoints.resize(oi);
4093  validKeypoints3D.resize(oi);
4094  keypoints = validKeypoints;
4095  keypoints3D = validKeypoints3D;
4096  descriptors = validDescriptors.rowRange(0, oi).clone();
4097  }
4098  }
4099  t = timer.ticks();
4100  if(stats) stats->addStatistic(Statistics::kTimingMemKeypoints_3D(), t*1000.0f);
4101  UDEBUG("time keypoints 3D (%d) = %fs", (int)keypoints3D.size(), t);
4102 
4103  UDEBUG("ratio=%f, meanWordsPerLocation=%d", _badSignRatio, meanWordsPerLocation);
4104  if(descriptors.rows && descriptors.rows < _badSignRatio * float(meanWordsPerLocation))
4105  {
4106  descriptors = cv::Mat();
4107  }
4108  }
4109 
4110  if(_parallelized)
4111  {
4112  UDEBUG("Joining dictionary update thread...");
4113  preUpdateThread.join(); // Wait the dictionary to be updated
4114  UDEBUG("Joining dictionary update thread... thread finished!");
4115  }
4116 
4117  t = timer.ticks();
4118  if(stats) stats->addStatistic(Statistics::kTimingMemJoining_dictionary_update(), t*1000.0f);
4119  if(_parallelized)
4120  {
4121  UDEBUG("time descriptor and memory update (%d of size=%d) = %fs", descriptors.rows, descriptors.cols, t);
4122  }
4123  else
4124  {
4125  UDEBUG("time descriptor (%d of size=%d) = %fs", descriptors.rows, descriptors.cols, t);
4126  }
4127 
4128  std::list<int> wordIds;
4129  if(descriptors.rows)
4130  {
4131  // In case the number of features we want to do quantization is lower
4132  // than extracted ones (that would be used for transform estimation)
4133  std::vector<bool> inliers;
4134  cv::Mat descriptorsForQuantization = descriptors;
4135  std::vector<int> quantizedToRawIndices;
4136  if(_feature2D->getMaxFeatures()>0 && descriptors.rows > _feature2D->getMaxFeatures())
4137  {
4138  UASSERT((int)keypoints.size() == descriptors.rows);
4139  Feature2D::limitKeypoints(keypoints, inliers, _feature2D->getMaxFeatures());
4140 
4141  descriptorsForQuantization = cv::Mat(_feature2D->getMaxFeatures(), descriptors.cols, descriptors.type());
4142  quantizedToRawIndices.resize(_feature2D->getMaxFeatures());
4143  unsigned int oi=0;
4144  UASSERT((int)inliers.size() == descriptors.rows);
4145  for(int k=0; k < descriptors.rows; ++k)
4146  {
4147  if(inliers[k])
4148  {
4149  UASSERT(oi < quantizedToRawIndices.size());
4150  if(descriptors.type() == CV_32FC1)
4151  {
4152  memcpy(descriptorsForQuantization.ptr<float>(oi), descriptors.ptr<float>(k), descriptors.cols*sizeof(float));
4153  }
4154  else
4155  {
4156  memcpy(descriptorsForQuantization.ptr<char>(oi), descriptors.ptr<char>(k), descriptors.cols*sizeof(char));
4157  }
4158  quantizedToRawIndices[oi] = k;
4159  ++oi;
4160  }
4161  }
4162  UASSERT((int)oi == _feature2D->getMaxFeatures());
4163  }
4164 
4165  // Quantization to vocabulary
4166  wordIds = _vwd->addNewWords(descriptorsForQuantization, id);
4167 
4168  // Set ID -1 to features not used for quantization
4169  if(wordIds.size() < keypoints.size())
4170  {
4171  std::vector<int> allWordIds;
4172  allWordIds.resize(keypoints.size(),-1);
4173  int i=0;
4174  for(std::list<int>::iterator iter=wordIds.begin(); iter!=wordIds.end(); ++iter)
4175  {
4176  allWordIds[quantizedToRawIndices[i]] = *iter;
4177  ++i;
4178  }
4179  int negIndex = -1;
4180  for(i=0; i<(int)allWordIds.size(); ++i)
4181  {
4182  if(allWordIds[i] < 0)
4183  {
4184  allWordIds[i] = negIndex--;
4185  }
4186  }
4187  wordIds = uVectorToList(allWordIds);
4188  }
4189 
4190  t = timer.ticks();
4191  if(stats) stats->addStatistic(Statistics::kTimingMemAdd_new_words(), t*1000.0f);
4192  UDEBUG("time addNewWords %fs indexed=%d not=%d", t, _vwd->getIndexedWordsCount(), _vwd->getNotIndexedWordsCount());
4193  }
4194  else if(id>0)
4195  {
4196  UDEBUG("id %d is a bad signature", id);
4197  }
4198 
4199  std::multimap<int, cv::KeyPoint> words;
4200  std::multimap<int, cv::Point3f> words3D;
4201  std::multimap<int, cv::Mat> wordsDescriptors;
4202  if(wordIds.size() > 0)
4203  {
4204  UASSERT(wordIds.size() == keypoints.size());
4205  UASSERT(keypoints3D.size() == 0 || keypoints3D.size() == wordIds.size());
4206  unsigned int i=0;
4207  float decimationRatio = float(preDecimation) / float(_imagePostDecimation);
4208  double log2value = log(double(preDecimation))/log(2.0);
4209  for(std::list<int>::iterator iter=wordIds.begin(); iter!=wordIds.end() && i < keypoints.size(); ++iter, ++i)
4210  {
4211  cv::KeyPoint kpt = keypoints[i];
4212  if(preDecimation != _imagePostDecimation)
4213  {
4214  // remap keypoints to final image size
4215  kpt.pt.x *= decimationRatio;
4216  kpt.pt.y *= decimationRatio;
4217  kpt.size *= decimationRatio;
4218  kpt.octave += log2value;
4219  }
4220  words.insert(std::pair<int, cv::KeyPoint>(*iter, kpt));
4221 
4222  if(keypoints3D.size())
4223  {
4224  words3D.insert(std::pair<int, cv::Point3f>(*iter, keypoints3D.at(i)));
4225  }
4226  if(_rawDescriptorsKept)
4227  {
4228  wordsDescriptors.insert(std::pair<int, cv::Mat>(*iter, descriptors.row(i).clone()));
4229  }
4230  }
4231  }
4232 
4233  cv::Mat image = data.imageRaw();
4234  cv::Mat depthOrRightImage = data.depthOrRightRaw();
4235  std::vector<CameraModel> cameraModels = data.cameraModels();
4236  StereoCameraModel stereoCameraModel = data.stereoCameraModel();
4237 
4238  // apply decimation?
4239  if(_imagePostDecimation > 1 && !isIntermediateNode)
4240  {
4241  if(_imagePostDecimation == preDecimation && decimatedData.isValid())
4242  {
4243  image = decimatedData.imageRaw();
4244  depthOrRightImage = decimatedData.depthOrRightRaw();
4245  cameraModels = decimatedData.cameraModels();
4246  stereoCameraModel = decimatedData.stereoCameraModel();
4247  }
4248  else
4249  {
4250  if(!data.rightRaw().empty() ||
4251  (data.depthRaw().rows == image.rows && data.depthRaw().cols == image.cols))
4252  {
4253  depthOrRightImage = util2d::decimate(depthOrRightImage, _imagePostDecimation);
4254  }
4255  image = util2d::decimate(image, _imagePostDecimation);
4256  for(unsigned int i=0; i<cameraModels.size(); ++i)
4257  {
4258  cameraModels[i] = cameraModels[i].scaled(1.0/double(_imagePostDecimation));
4259  }
4260  if(stereoCameraModel.isValidForProjection())
4261  {
4262  stereoCameraModel.scale(1.0/double(_imagePostDecimation));
4263  }
4264  }
4265 
4266  t = timer.ticks();
4267  if(stats) stats->addStatistic(Statistics::kTimingMemPost_decimation(), t*1000.0f);
4268  UDEBUG("time post-decimation = %fs", t);
4269  }
4270 
4271  bool triangulateWordsWithoutDepth = !_depthAsMask;
4272  if(!pose.isNull() &&
4273  cameraModels.size() == 1 &&
4274  words.size() &&
4275  (words3D.size() == 0 || (triangulateWordsWithoutDepth && words.size() == words3D.size())) &&
4276  _registrationPipeline->isImageRequired() &&
4277  _signatures.size() &&
4278  _signatures.rbegin()->second->mapId() == _idMapCount) // same map
4279  {
4280  UDEBUG("Generate 3D words using odometry");
4281  Signature * previousS = _signatures.rbegin()->second;
4282  if(previousS->getWords().size() > 8 && words.size() > 8 && !previousS->getPose().isNull())
4283  {
4284  UDEBUG("Previous pose(%d) = %s", previousS->id(), previousS->getPose().prettyPrint().c_str());
4285  UDEBUG("Current pose(%d) = %s", id, pose.prettyPrint().c_str());
4286  Transform cameraTransform = pose.inverse() * previousS->getPose();
4287 
4288  Signature cpPrevious(2);
4289  // IDs should be unique so that registration doesn't override them
4290  std::map<int, cv::KeyPoint> uniqueWords = uMultimapToMapUnique(previousS->getWords());
4291  std::map<int, cv::Mat> uniqueWordsDescriptors = uMultimapToMapUnique(previousS->getWordsDescriptors());
4292  cpPrevious.sensorData().setCameraModels(previousS->sensorData().cameraModels());
4293  cpPrevious.setWords(std::multimap<int, cv::KeyPoint>(uniqueWords.begin(), uniqueWords.end()));
4294  cpPrevious.setWordsDescriptors(std::multimap<int, cv::Mat>(uniqueWordsDescriptors.begin(), uniqueWordsDescriptors.end()));
4295  Signature cpCurrent(1);
4296  uniqueWords = uMultimapToMapUnique(words);
4297  uniqueWordsDescriptors = uMultimapToMapUnique(wordsDescriptors);
4298  cpCurrent.sensorData().setCameraModels(cameraModels);
4299  cpCurrent.setWords(std::multimap<int, cv::KeyPoint>(uniqueWords.begin(), uniqueWords.end()));
4300  cpCurrent.setWordsDescriptors(std::multimap<int, cv::Mat>(uniqueWordsDescriptors.begin(), uniqueWordsDescriptors.end()));
4301  // This will force comparing descriptors between both images directly
4302  Transform tmpt = _registrationPipeline->computeTransformationMod(cpCurrent, cpPrevious, cameraTransform);
4303  UDEBUG("t=%s", tmpt.prettyPrint().c_str());
4304 
4305  // compute 3D words by epipolar geometry with the previous signature
4306  std::map<int, cv::Point3f> inliers = util3d::generateWords3DMono(
4307  uMultimapToMapUnique(cpCurrent.getWords()),
4308  uMultimapToMapUnique(cpPrevious.getWords()),
4309  cameraModels[0],
4310  cameraTransform);
4311 
4312  UDEBUG("inliers=%d", (int)inliers.size());
4313 
4314  // words3D should have the same size than words if not empty
4315  float bad_point = std::numeric_limits<float>::quiet_NaN ();
4316  UASSERT(words3D.size() == 0 || words.size() == words3D.size());
4317  bool words3DWasEmpty = words3D.empty();
4318  int added3DPointsWithoutDepth = 0;
4319  for(std::multimap<int, cv::KeyPoint>::const_iterator iter=words.begin(); iter!=words.end(); ++iter)
4320  {
4321  std::map<int, cv::Point3f>::iterator jter=inliers.find(iter->first);
4322  std::multimap<int, cv::Point3f>::iterator iter3D = words3D.find(iter->first);
4323  if(iter3D == words3D.end())
4324  {
4325  if(jter != inliers.end())
4326  {
4327  words3D.insert(std::make_pair(iter->first, jter->second));
4328  ++added3DPointsWithoutDepth;
4329  }
4330  else
4331  {
4332  words3D.insert(std::make_pair(iter->first, cv::Point3f(bad_point,bad_point,bad_point)));
4333  }
4334  }
4335  else if(!util3d::isFinite(iter3D->second) && jter != inliers.end())
4336  {
4337  iter3D->second = jter->second;
4338  ++added3DPointsWithoutDepth;
4339  }
4340  else if(words3DWasEmpty && jter == inliers.end())
4341  {
4342  // duplicate
4343  words3D.insert(std::make_pair(iter->first, cv::Point3f(bad_point,bad_point,bad_point)));
4344  }
4345  }
4346  UDEBUG("added3DPointsWithoutDepth=%d", added3DPointsWithoutDepth);
4347  if(stats) stats->addStatistic(Statistics::kMemoryTriangulated_points(), (float)added3DPointsWithoutDepth);
4348 
4349  t = timer.ticks();
4350  UASSERT(words3D.size() == words.size());
4351  if(stats) stats->addStatistic(Statistics::kTimingMemKeypoints_3D_motion(), t*1000.0f);
4352  UDEBUG("time keypoints 3D by motion (%d) = %fs", (int)words3D.size(), t);
4353  }
4354  }
4355 
4356  // Filter the laser scan?
4357  LaserScan laserScan = data.laserScanRaw();
4358  if(!isIntermediateNode && laserScan.size())
4359  {
4360  if(laserScan.maxRange() == 0.0f)
4361  {
4362  bool id2d = laserScan.is2d();
4363  float maxRange = 0.0f;
4364  for(int i=0; i<laserScan.size(); ++i)
4365  {
4366  const float * ptr = laserScan.data().ptr<float>(0, i);
4367  float r;
4368  if(id2d)
4369  {
4370  r = ptr[0]*ptr[0] + ptr[1]*ptr[1];
4371  }
4372  else
4373  {
4374  r = ptr[0]*ptr[0] + ptr[1]*ptr[1] + ptr[2]*ptr[2];
4375  }
4376  if(r>maxRange)
4377  {
4378  maxRange = r;
4379  }
4380  }
4381  if(maxRange > 0.0f)
4382  {
4383  laserScan=LaserScan(laserScan.data(), laserScan.maxPoints(), sqrt(maxRange), laserScan.format(), laserScan.localTransform());
4384  }
4385  }
4386 
4387  laserScan = util3d::commonFiltering(laserScan,
4388  _laserScanDownsampleStepSize,
4389  0,
4390  0,
4391  _laserScanVoxelSize,
4392  _laserScanNormalK,
4393  _laserScanNormalRadius);
4394  t = timer.ticks();
4395  if(stats) stats->addStatistic(Statistics::kTimingMemScan_filtering(), t*1000.0f);
4396  UDEBUG("time normals scan = %fs", t);
4397  }
4398 
4399  Signature * s;
4400  if(this->isBinDataKept() && (!isIntermediateNode || _saveIntermediateNodeData))
4401  {
4402  UDEBUG("Bin data kept: rgb=%d, depth=%d, scan=%d, userData=%d",
4403  image.empty()?0:1,
4404  depthOrRightImage.empty()?0:1,
4405  laserScan.isEmpty()?0:1,
4406  data.userDataRaw().empty()?0:1);
4407 
4408  std::vector<unsigned char> imageBytes;
4409  std::vector<unsigned char> depthBytes;
4410 
4411  if(_saveDepth16Format && !depthOrRightImage.empty() && depthOrRightImage.type() == CV_32FC1)
4412  {
4413  UWARN("Save depth data to 16 bits format: depth type detected is 32FC1, use 16UC1 depth format to avoid this conversion (or set parameter \"Mem/SaveDepth16Format\"=false to use 32bits format).");
4414  depthOrRightImage = util2d::cvtDepthFromFloat(depthOrRightImage);
4415  }
4416 
4417  cv::Mat compressedImage;
4418  cv::Mat compressedDepth;
4419  cv::Mat compressedScan;
4420  cv::Mat compressedUserData;
4421  if(_compressionParallelized)
4422  {
4423  rtabmap::CompressionThread ctImage(image, std::string(".jpg"));
4424  rtabmap::CompressionThread ctDepth(depthOrRightImage, std::string(".png"));
4425  rtabmap::CompressionThread ctLaserScan(laserScan.data());
4426  rtabmap::CompressionThread ctUserData(data.userDataRaw());
4427  if(!image.empty())
4428  {
4429  ctImage.start();
4430  }
4431  if(!depthOrRightImage.empty())
4432  {
4433  ctDepth.start();
4434  }
4435  if(!laserScan.isEmpty())
4436  {
4437  ctLaserScan.start();
4438  }
4439  if(!data.userDataRaw().empty())
4440  {
4441  ctUserData.start();
4442  }
4443  ctImage.join();
4444  ctDepth.join();
4445  ctLaserScan.join();
4446  ctUserData.join();
4447 
4448  compressedImage = ctImage.getCompressedData();
4449  compressedDepth = ctDepth.getCompressedData();
4450  compressedScan = ctLaserScan.getCompressedData();
4451  compressedUserData = ctUserData.getCompressedData();
4452  }
4453  else
4454  {
4455  compressedImage = compressImage2(image, std::string(".jpg"));
4456  compressedDepth = compressImage2(depthOrRightImage, depthOrRightImage.type() == CV_32FC1 || depthOrRightImage.type() == CV_16UC1?std::string(".png"):std::string(".jpg"));
4457  compressedScan = compressData2(laserScan.data());
4458  compressedUserData = compressData2(data.userDataRaw());
4459  }
4460 
4461  s = new Signature(id,
4462  _idMapCount,
4463  isIntermediateNode?-1:0, // tag intermediate nodes as weight=-1
4464  data.stamp(),
4465  "",
4466  pose,
4467  data.groundTruth(),
4468  stereoCameraModel.isValidForProjection()?
4469  SensorData(
4470  LaserScan(compressedScan, laserScan.maxPoints(), laserScan.maxRange(), laserScan.format(), laserScan.localTransform()),
4471  compressedImage,
4472  compressedDepth,
4473  stereoCameraModel,
4474  id,
4475  0,
4476  compressedUserData):
4477  SensorData(
4478  LaserScan(compressedScan, laserScan.maxPoints(), laserScan.maxRange(), laserScan.format(), laserScan.localTransform()),
4479  compressedImage,
4480  compressedDepth,
4481  cameraModels,
4482  id,
4483  0,
4484  compressedUserData));
4485  }
4486  else
4487  {
4488  UDEBUG("Bin data kept: scan=%d, userData=%d",
4489  laserScan.isEmpty()?0:1,
4490  data.userDataRaw().empty()?0:1);
4491 
4492  // just compress user data and laser scan (scans can be used for local scan matching)
4493  cv::Mat compressedScan;
4494  cv::Mat compressedUserData;
4495  if(_compressionParallelized)
4496  {
4497  rtabmap::CompressionThread ctUserData(data.userDataRaw());
4498  rtabmap::CompressionThread ctLaserScan(laserScan.data());
4499  if(!data.userDataRaw().empty() && !isIntermediateNode)
4500  {
4501  ctUserData.start();
4502  }
4503  if(!laserScan.isEmpty() && !isIntermediateNode)
4504  {
4505  ctLaserScan.start();
4506  }
4507  ctUserData.join();
4508  ctLaserScan.join();
4509 
4510  compressedScan = ctLaserScan.getCompressedData();
4511  compressedUserData = ctUserData.getCompressedData();
4512  }
4513  else
4514  {
4515  compressedScan = compressData2(laserScan.data());
4516  compressedUserData = compressData2(data.userDataRaw());
4517  }
4518 
4519  s = new Signature(id,
4520  _idMapCount,
4521  isIntermediateNode?-1:0, // tag intermediate nodes as weight=-1
4522  data.stamp(),
4523  "",
4524  pose,
4525  data.groundTruth(),
4526  stereoCameraModel.isValidForProjection()?
4527  SensorData(
4528  LaserScan(compressedScan, laserScan.maxPoints(), laserScan.maxRange(), laserScan.format(), laserScan.localTransform()),
4529  cv::Mat(),
4530  cv::Mat(),
4531  stereoCameraModel,
4532  id,
4533  0,
4534  compressedUserData):
4535  SensorData(
4536  LaserScan(compressedScan, laserScan.maxPoints(), laserScan.maxRange(), laserScan.format(), laserScan.localTransform()),
4537  cv::Mat(),
4538  cv::Mat(),
4539  cameraModels,
4540  id,
4541  0,
4542  compressedUserData));
4543  }
4544 
4545  s->setWords(words);
4546  s->setWords3(words3D);
4547  s->setWordsDescriptors(wordsDescriptors);
4548 
4549  // set raw data
4550  s->sensorData().setImageRaw(image);
4551  s->sensorData().setDepthOrRightRaw(depthOrRightImage);
4552  s->sensorData().setLaserScanRaw(laserScan);
4553  s->sensorData().setUserDataRaw(data.userDataRaw());
4554 
4555  s->sensorData().setGroundTruth(data.groundTruth());
4556  s->sensorData().setGPS(data.gps());
4557 
4558  t = timer.ticks();
4559  if(stats) stats->addStatistic(Statistics::kTimingMemCompressing_data(), t*1000.0f);
4560  UDEBUG("time compressing data (id=%d) %fs", id, t);
4561  if(words.size())
4562  {
4563  s->setEnabled(true); // All references are already activated in the dictionary at this point (see _vwd->addNewWords())
4564  }
4565 
4566  // Occupancy grid map stuff
4567  cv::Mat ground, obstacles, empty;
4568  float cellSize = 0.0f;
4569  cv::Point3f viewPoint(0,0,0);
4570  if(_createOccupancyGrid && !data.depthOrRightRaw().empty() && !isIntermediateNode)
4571  {
4572  _occupancy->createLocalMap(*s, ground, obstacles, empty, viewPoint);
4573  cellSize = _occupancy->getCellSize();
4574 
4575  t = timer.ticks();
4576  if(stats) stats->addStatistic(Statistics::kTimingMemOccupancy_grid(), t*1000.0f);
4577  UDEBUG("time grid map = %fs", t);
4578  }
4579  s->sensorData().setOccupancyGrid(ground, obstacles, empty, cellSize, viewPoint);
4580 
4581  // prior
4582  if(!isIntermediateNode)
4583  {
4584  if(!data.globalPose().isNull() && data.globalPoseCovariance().cols==6 && data.globalPoseCovariance().rows==6 && data.globalPoseCovariance().cols==CV_64FC1)
4585  {
4586  s->addLink(Link(s->id(), s->id(), Link::kPosePrior, data.globalPose(), data.globalPoseCovariance().inv()));
4587 
4588  /*if(data.gps().stamp() > 0.0)
4589  {
4590  UWARN("GPS constraint ignored as global pose is also set.");
4591  }*/
4592  }
4593  else if(data.gps().stamp() > 0.0)
4594  {
4595  // TODO: What kind of covariance should we set to have decent gtsam and g2o results!?
4596  /*if(_gpsOrigin.stamp() <= 0.0)
4597  {
4598  _gpsOrigin = data.gps();
4599  }
4600  cv::Point3f pt = data.gps().toGeodeticCoords().toENU_WGS84(_gpsOrigin.toGeodeticCoords());
4601  Transform gpsPose(pt.x, pt.y, pose.z(), 0, 0, -(data.gps().bearing()-90.0)*180.0/M_PI);
4602  cv::Mat gpsInfMatrix = cv::Mat::eye(6,6,CV_64FC1)*0.00000001;
4603  if(data.gps().error() > 0.0)
4604  {
4605  // only set x, y as we don't know variance for other degrees of freedom.
4606  gpsInfMatrix.at<double>(0,0) = gpsInfMatrix.at<double>(1,1) = 0.1;
4607  gpsInfMatrix.at<double>(2,2) = 100000;
4608  s->addLink(Link(s->id(), s->id(), Link::kPosePrior, gpsPose, gpsInfMatrix));
4609  }
4610  else
4611  {
4612  UERROR("Invalid GPS error value (%f m), must be > 0 m.", data.gps().error());
4613  }*/
4614  }
4615  }
4616 
4617  return s;
4618 }
4619 
4620 void Memory::disableWordsRef(int signatureId)
4621 {
4622  UDEBUG("id=%d", signatureId);
4623 
4624  Signature * ss = this->_getSignature(signatureId);
4625  if(ss && ss->isEnabled())
4626  {
4627  const std::multimap<int, cv::KeyPoint> & words = ss->getWords();
4628  const std::list<int> & keys = uUniqueKeys(words);
4629  int count = _vwd->getTotalActiveReferences();
4630  // First remove all references
4631  for(std::list<int>::const_iterator i=keys.begin(); i!=keys.end(); ++i)
4632  {
4633  _vwd->removeAllWordRef(*i, signatureId);
4634  }
4635 
4636  count -= _vwd->getTotalActiveReferences();
4637  ss->setEnabled(false);
4638  UDEBUG("%d words total ref removed from signature %d... (total active ref = %d)", count, ss->id(), _vwd->getTotalActiveReferences());
4639  }
4640 }
4641 
4642 void Memory::cleanUnusedWords()
4643 {
4644  std::vector<VisualWord*> removedWords = _vwd->getUnusedWords();
4645  UDEBUG("Removing %d words (dictionary size=%d)...", removedWords.size(), _vwd->getVisualWords().size());
4646  if(removedWords.size())
4647  {
4648  // remove them from the dictionary
4649  _vwd->removeWords(removedWords);
4650 
4651  for(unsigned int i=0; i<removedWords.size(); ++i)
4652  {
4653  if(_dbDriver)
4654  {
4655  _dbDriver->asyncSave(removedWords[i]);
4656  }
4657  else
4658  {
4659  delete removedWords[i];
4660  }
4661  }
4662  }
4663 }
4664 
4665 void Memory::enableWordsRef(const std::list<int> & signatureIds)
4666 {
4667  UDEBUG("size=%d", signatureIds.size());
4668  UTimer timer;
4669  timer.start();
4670 
4671  std::map<int, int> refsToChange; //<oldWordId, activeWordId>
4672 
4673  std::set<int> oldWordIds;
4674  std::list<Signature *> surfSigns;
4675  for(std::list<int>::const_iterator i=signatureIds.begin(); i!=signatureIds.end(); ++i)
4676  {
4677  Signature * ss = dynamic_cast<Signature *>(this->_getSignature(*i));
4678  if(ss && !ss->isEnabled())
4679  {
4680  surfSigns.push_back(ss);
4681  std::list<int> uniqueKeys = uUniqueKeys(ss->getWords());
4682 
4683  //Find words in the signature which they are not in the current dictionary
4684  for(std::list<int>::const_iterator k=uniqueKeys.begin(); k!=uniqueKeys.end(); ++k)
4685  {
4686  if(*k>0 && _vwd->getWord(*k) == 0 && _vwd->getUnusedWord(*k) == 0)
4687  {
4688  oldWordIds.insert(oldWordIds.end(), *k);
4689  }
4690  }
4691  }
4692  }
4693 
4694  if(!_vwd->isIncremental() && oldWordIds.size())
4695  {
4696  UWARN("Dictionary is fixed, but some words retrieved have not been found!?");
4697  }
4698 
4699  UDEBUG("oldWordIds.size()=%d, getOldIds time=%fs", oldWordIds.size(), timer.ticks());
4700 
4701  // the words were deleted, so try to math it with an active word
4702  std::list<VisualWord *> vws;
4703  if(oldWordIds.size() && _dbDriver)
4704  {
4705  // get the descriptors
4706  _dbDriver->loadWords(oldWordIds, vws);
4707  }
4708  UDEBUG("loading words(%d) time=%fs", oldWordIds.size(), timer.ticks());
4709 
4710 
4711  if(vws.size())
4712  {
4713  //Search in the dictionary
4714  std::vector<int> vwActiveIds = _vwd->findNN(vws);
4715  UDEBUG("find active ids (number=%d) time=%fs", vws.size(), timer.ticks());
4716  int i=0;
4717  for(std::list<VisualWord *>::iterator iterVws=vws.begin(); iterVws!=vws.end(); ++iterVws)
4718  {
4719  if(vwActiveIds[i] > 0)
4720  {
4721  //UDEBUG("Match found %d with %d", (*iterVws)->id(), vwActiveIds[i]);
4722  refsToChange.insert(refsToChange.end(), std::pair<int, int>((*iterVws)->id(), vwActiveIds[i]));
4723  if((*iterVws)->isSaved())
4724  {
4725  delete (*iterVws);
4726  }
4727  else if(_dbDriver)
4728  {
4729  _dbDriver->asyncSave(*iterVws);
4730  }
4731  }
4732  else
4733  {
4734  //add to dictionary
4735  _vwd->addWord(*iterVws); // take ownership
4736  }
4737  ++i;
4738  }
4739  UDEBUG("Added %d to dictionary, time=%fs", vws.size()-refsToChange.size(), timer.ticks());
4740 
4741  //update the global references map and update the signatures reactivated
4742  for(std::map<int, int>::const_iterator iter=refsToChange.begin(); iter != refsToChange.end(); ++iter)
4743  {
4744  //uInsert(_wordRefsToChange, (const std::pair<int, int>)*iter); // This will be used to change references in the database
4745  for(std::list<Signature *>::iterator j=surfSigns.begin(); j!=surfSigns.end(); ++j)
4746  {
4747  (*j)->changeWordsRef(iter->first, iter->second);
4748  }
4749  }
4750  UDEBUG("changing ref, total=%d, time=%fs", refsToChange.size(), timer.ticks());
4751  }
4752 
4753  int count = _vwd->getTotalActiveReferences();
4754 
4755  // Reactivate references and signatures
4756  for(std::list<Signature *>::iterator j=surfSigns.begin(); j!=surfSigns.end(); ++j)
4757  {
4758  const std::vector<int> & keys = uKeys((*j)->getWords());
4759  if(keys.size())
4760  {
4761  // Add all references
4762  for(unsigned int i=0; i<keys.size(); ++i)
4763  {
4764  if(keys.at(i)>0)
4765  {
4766  _vwd->addWordRef(keys.at(i), (*j)->id());
4767  }
4768  }
4769  (*j)->setEnabled(true);
4770  }
4771  }
4772 
4773  count = _vwd->getTotalActiveReferences() - count;
4774  UDEBUG("%d words total ref added from %d signatures, time=%fs...", count, surfSigns.size(), timer.ticks());
4775 }
4776 
4777 std::set<int> Memory::reactivateSignatures(const std::list<int> & ids, unsigned int maxLoaded, double & timeDbAccess)
4778 {
4779  // get the signatures, if not in the working memory, they
4780  // will be loaded from the database in an more efficient way
4781  // than how it is done in the Memory
4782 
4783  UDEBUG("");
4784  UTimer timer;
4785  std::list<int> idsToLoad;
4786  std::map<int, int>::iterator wmIter;
4787  for(std::list<int>::const_iterator i=ids.begin(); i!=ids.end(); ++i)
4788  {
4789  if(!this->getSignature(*i) && !uContains(idsToLoad, *i))
4790  {
4791  if(!maxLoaded || idsToLoad.size() < maxLoaded)
4792  {
4793  idsToLoad.push_back(*i);
4794  UINFO("Loading location %d from database...", *i);
4795  }
4796  }
4797  }
4798 
4799  UDEBUG("idsToLoad = %d", idsToLoad.size());
4800 
4801  std::list<Signature *> reactivatedSigns;
4802  if(_dbDriver)
4803  {
4804  _dbDriver->loadSignatures(idsToLoad, reactivatedSigns);
4805  }
4806  timeDbAccess = timer.getElapsedTime();
4807  std::list<int> idsLoaded;
4808  for(std::list<Signature *>::iterator i=reactivatedSigns.begin(); i!=reactivatedSigns.end(); ++i)
4809  {
4810  idsLoaded.push_back((*i)->id());
4811  //append to working memory
4812  this->addSignatureToWmFromLTM(*i);
4813  }
4814  this->enableWordsRef(idsLoaded);
4815  UDEBUG("time = %fs", timer.ticks());
4816  return std::set<int>(idsToLoad.begin(), idsToLoad.end());
4817 }
4818 
4819 // return all non-null poses
4820 // return unique links between nodes (for neighbors: old->new, for loops: parent->child)
4821 void Memory::getMetricConstraints(
4822  const std::set<int> & ids,
4823  std::map<int, Transform> & poses,
4824  std::multimap<int, Link> & links,
4825  bool lookInDatabase)
4826 {
4827  UDEBUG("");
4828  for(std::set<int>::const_iterator iter=ids.begin(); iter!=ids.end(); ++iter)
4829  {
4830  Transform pose = getOdomPose(*iter, lookInDatabase);
4831  if(!pose.isNull())
4832  {
4833  poses.insert(std::make_pair(*iter, pose));
4834  }
4835  }
4836 
4837  for(std::set<int>::const_iterator iter=ids.begin(); iter!=ids.end(); ++iter)
4838  {
4839  if(uContains(poses, *iter))
4840  {
4841  std::map<int, Link> tmpLinks = getLinks(*iter, lookInDatabase);
4842  for(std::map<int, Link>::iterator jter=tmpLinks.begin(); jter!=tmpLinks.end(); ++jter)
4843  {
4844  if( jter->second.isValid() &&
4845  uContains(poses, jter->first) &&
4846  graph::findLink(links, *iter, jter->first) == links.end())
4847  {
4848  if(!lookInDatabase &&
4849  (jter->second.type() == Link::kNeighbor ||
4850  jter->second.type() == Link::kNeighborMerged))
4851  {
4852  Link link = jter->second;
4853  const Signature * s = this->getSignature(jter->first);
4854  UASSERT(s!=0);
4855  while(s && s->getWeight() == -1)
4856  {
4857  // skip to next neighbor, well we assume that bad signatures
4858  // are only linked by max 2 neighbor links.
4859  std::map<int, Link> n = this->getNeighborLinks(s->id(), false);
4860  UASSERT(n.size() <= 2);
4861  std::map<int, Link>::iterator uter = n.upper_bound(s->id());
4862  if(uter != n.end())
4863  {
4864  const Signature * s2 = this->getSignature(uter->first);
4865  if(s2)
4866  {
4867  link = link.merge(uter->second, uter->second.type());
4868  poses.erase(s->id());
4869  s = s2;
4870  }
4871 
4872  }
4873  else
4874  {
4875  break;
4876  }
4877  }
4878 
4879  links.insert(std::make_pair(*iter, link));
4880  }
4881  else
4882  {
4883  links.insert(std::make_pair(*iter, jter->second));
4884  }
4885  }
4886  }
4887  }
4888  }
4889 }
4890 
4891 } // namespace rtabmap
rtabmap::Signature::getWords3
const std::multimap< int, cv::Point3f > & getWords3() const
Definition: Signature.h:128
glm::log
GLM_FUNC_DECL genType log(genType const &x)
rtabmap::util3d::laserScanFromPointCloud
LaserScan RTABMAP_EXP laserScanFromPointCloud(const pcl::PCLPointCloud2 &cloud, bool filterNaNs=true)
Definition: util3d.cpp:1266
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)
rtabmap::Registration::isUserDataRequired
bool isUserDataRequired() const
Definition: Registration.cpp:109
rtabmap::util3d::isFinite
bool RTABMAP_EXP isFinite(const cv::Point3f &pt)
Definition: util3d.cpp:3035
rtabmap::CameraModel::imageWidth
int imageWidth() const
Definition: CameraModel.h:113
rtabmap::Memory::loadOptimizedMesh
cv::Mat loadOptimizedMesh(std::vector< std::vector< std::vector< unsigned int > > > *polygons=0, std::vector< std::vector< Eigen::Vector2f > > *texCoords=0, cv::Mat *textures=0) const
Definition: Memory.cpp:1850
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::Memory::loadPreviewImage
cv::Mat loadPreviewImage() const
Definition: Memory.cpp:1791
rtabmap::Memory::_occupancy
OccupancyGrid * _occupancy
Definition: Memory.h:333
rtabmap::DBDriver::getLastNodeId
void getLastNodeId(int &id) const
Definition: DBDriver.cpp:879
rtabmap::Registration::getMinVisualCorrespondences
int getMinVisualCorrespondences() const
Definition: Registration.cpp:129
rtabmap::Memory::labelSignature
bool labelSignature(int id, const std::string &label)
Definition: Memory.cpp:2194
rtabmap::DBDriver::loadPreviewImage
cv::Mat loadPreviewImage() const
Definition: DBDriver.cpp:1056
rtabmap::DBDriver::loadOptimizedMesh
cv::Mat loadOptimizedMesh(std::vector< std::vector< std::vector< unsigned int > > > *polygons=0, std::vector< std::vector< Eigen::Vector2f > > *texCoords=0, cv::Mat *textures=0) const
Definition: DBDriver.cpp:1108
uMultimapToMapUnique
std::map< K, V > uMultimapToMapUnique(const std::multimap< K, V > &m)
Definition: UStl.h:505
rtabmap::Memory::_vwd
VWDictionary * _vwd
Definition: Memory.h:324
rtabmap::util3d::generateWords3DMono
std::map< int, cv::Point3f > RTABMAP_EXP generateWords3DMono(const std::map< int, cv::KeyPoint > &kpts, const std::map< int, cv::KeyPoint > &previousKpts, const CameraModel &cameraModel, Transform &cameraTransform, int pnpIterations=100, float pnpReprojError=8.0f, int pnpFlags=0, int pnpRefineIterations=1, float ransacParam1=3.0f, float ransacParam2=0.99f, const std::map< int, cv::Point3f > &refGuess3D=std::map< int, cv::Point3f >(), double *variance=0)
Definition: util3d_features.cpp:206
rtabmap::DBDriver::asyncSave
void asyncSave(Signature *s)
Definition: DBDriver.cpp:382
UTimer
Definition: UTimer.h:46
rtabmap::Feature2D::limitKeypoints
static void limitKeypoints(std::vector< cv::KeyPoint > &keypoints, int maxKeypoints)
Definition: Features2d.cpp:205
rtabmap::Memory::_lastSignature
Signature * _lastSignature
Definition: Memory.h:310
rtabmap::Memory::addSignatureToWmFromLTM
void addSignatureToWmFromLTM(Signature *signature)
Definition: Memory.cpp:892
rtabmap::Feature2D::getType
virtual Feature2D::Type getType() const =0
UThread::start
void start()
Definition: UThread.cpp:122
rtabmap::Memory::~Memory
virtual ~Memory()
Definition: Memory.cpp:427
rtabmap::SensorData::setOccupancyGrid
void setOccupancyGrid(const cv::Mat &ground, const cv::Mat &obstacles, const cv::Mat &empty, float cellSize, const cv::Point3f &viewPoint)
Definition: SensorData.cpp:482
rtabmap::Memory::_mapLabelsAdded
bool _mapLabelsAdded
Definition: Memory.h:286
rtabmap::Memory::_similarityThreshold
float _similarityThreshold
Definition: Memory.h:272
rtabmap::Memory::_visMaxFeatures
int _visMaxFeatures
Definition: Memory.h:302
rtabmap::Transform::prettyPrint
std::string prettyPrint() const
Definition: Transform.cpp:274
rtabmap::OccupancyGrid::getCellSize
float getCellSize() const
Definition: OccupancyGrid.h:58
UEventsManager::post
static void post(UEvent *event, bool async=true, const UEventsSender *sender=0)
Definition: UEventsManager.cpp:54
rtabmap::util3d::transformPoint
cv::Point3f RTABMAP_EXP transformPoint(const cv::Point3f &pt, const Transform &transform)
Definition: util3d_transforms.cpp:193
rtabmap::Feature2D::generateKeypoints3D
std::vector< cv::Point3f > generateKeypoints3D(const SensorData &data, const std::vector< cv::KeyPoint > &keypoints) const
Definition: Features2d.cpp:612
rtabmap::DBDriver::saveOptimizedPoses
void saveOptimizedPoses(const std::map< int, Transform > &optimizedPoses, const Transform &lastlocalizationPose) const
Definition: DBDriver.cpp:1064
rtabmap::LaserScan::format
Format format() const
Definition: LaserScan.h:66
uKeys
std::vector< K > uKeys(const std::multimap< K, V > &mm)
Definition: UStl.h:67
rtabmap::Transform::getDistanceSquared
float getDistanceSquared(const Transform &t) const
Definition: Transform.cpp:246
rtabmap::DBDriver::addInfoAfterRun
void addInfoAfterRun(int stMemSize, int lastSignAdded, int processMemUsed, int databaseMemUsed, int dictionarySize, const ParametersMap &parameters) const
Definition: DBDriver.cpp:992
rtabmap::Memory::getGroundTruthPose
Transform getGroundTruthPose(int signatureId, bool lookInDatabase=false) const
Definition: Memory.cpp:3390
rtabmap::SensorData::laserScanCompressed
const LaserScan & laserScanCompressed() const
Definition: SensorData.h:159
rtabmap::Signature::getGroundTruthPose
const Transform & getGroundTruthPose() const
Definition: Signature.h:131
rtabmap::Memory::_signaturesAdded
int _signaturesAdded
Definition: Memory.h:314
rtabmap::LaserScan::hasNormals
bool hasNormals() const
Definition: LaserScan.h:73
rtabmap::Signature::mapId
int mapId() const
Definition: Signature.h:71
rtabmap::Signature::getWords
const std::multimap< int, cv::KeyPoint > & getWords() const
Definition: Signature.h:108
rtabmap::Signature::setWords3
void setWords3(const std::multimap< int, cv::Point3f > &words3)
Definition: Signature.h:115
rtabmap::Memory::_imagesAlreadyRectified
bool _imagesAlreadyRectified
Definition: Memory.h:304
f
f
rtabmap::Memory::getLinks
std::map< int, Link > getLinks(int signatureId, bool lookInDatabase=false) const
Definition: Memory.cpp:1105
rtabmap::Feature2D::generateDescriptors
cv::Mat generateDescriptors(const cv::Mat &image, std::vector< cv::KeyPoint > &keypoints) const
Definition: Features2d.cpp:596
rtabmap::Signature::getVelocity
const std::vector< float > & getVelocity() const
Definition: Signature.h:132
rtabmap::Feature2D::parseParameters
virtual void parseParameters(const ParametersMap &parameters)
Definition: Features2d.cpp:341
rtabmap::LaserScan::maxRange
float maxRange() const
Definition: LaserScan.h:65
rtabmap::Memory::removeVirtualLinks
void removeVirtualLinks(int signatureId)
Definition: Memory.cpp:3010
ULOGGER_INFO
#define ULOGGER_INFO(...)
Definition: ULogger.h:54
rtabmap::DBDriver::loadLinks
void loadLinks(int signatureId, std::map< int, Link > &links, Link::Type type=Link::kUndef) const
Definition: DBDriver.cpp:763
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::Signature::setWords
void setWords(const std::multimap< int, cv::KeyPoint > &words)
Definition: Signature.cpp:231
util3d_motion_estimation.h
rtabmap::Registration::computeTransformationMod
Transform computeTransformationMod(Signature &from, Signature &to, Transform guess=Transform::getIdentity(), RegistrationInfo *info=0) const
Definition: Registration.cpp:188
rtabmap::Memory::getSignature
const Signature * getSignature(int id) const
Definition: Memory.cpp:1008
rtabmap::Registration::computeTransformation
Transform computeTransformation(const Signature &from, const Signature &to, Transform guess=Transform::getIdentity(), RegistrationInfo *info=0) const
Definition: Registration.cpp:166
rtabmap::Memory::_reduceGraph
bool _reduceGraph
Definition: Memory.h:279
glm::sqrt
GLM_FUNC_DECL vecType< T, P > sqrt(vecType< T, P > const &x)
rtabmap::Memory::getDatabaseUrl
std::string getDatabaseUrl() const
Definition: Memory.cpp:1395
rtabmap::DBDriver::getAllNodeIds
void getAllNodeIds(std::set< int > &ids, bool ignoreChildren=false, bool ignoreBadSignatures=false) const
Definition: DBDriver.cpp:813
rtabmap::Memory::_gpsOrigin
GPS _gpsOrigin
Definition: Memory.h:315
rtabmap::VWDictionary::update
virtual void update()
Definition: VWDictionary.cpp:348
rtabmap::Feature2D::getMinDepth
float getMinDepth() const
Definition: Features2d.h:144
rtabmap::Memory::_stMem
std::set< int > _stMem
Definition: Memory.h:320
rtabmap::ParametersPair
std::pair< std::string, std::string > ParametersPair
Definition: Parameters.h:42
rtabmap::Transform::getIdentity
static Transform getIdentity()
Definition: Transform.cpp:364
uStr2Bool
bool UTILITE_EXP uStr2Bool(const char *str)
Definition: UConversion.cpp:155
rtabmap::Memory::_transferSortingByWeightId
bool _transferSortingByWeightId
Definition: Memory.h:282
rtabmap::Memory::generateGraph
void generateGraph(const std::string &fileName, const std::set< int > &ids=std::set< int >())
Definition: Memory.cpp:3552
rtabmap::Memory::_tfIdfLikelihoodUsed
bool _tfIdfLikelihoodUsed
Definition: Memory.h:326
rtabmap::Feature2D::generateKeypoints
std::vector< cv::KeyPoint > generateKeypoints(const cv::Mat &image, const cv::Mat &mask=cv::Mat()) const
Definition: Features2d.cpp:497
rtabmap::Statistics::setReducedIds
void setReducedIds(const std::map< int, int > &reducedIds)
Definition: Statistics.h:197
rtabmap::WeightAgeIdKey::operator<
bool operator<(const WeightAgeIdKey &k) const
Definition: Memory.cpp:1891
rtabmap::Memory::getNodeData
SensorData getNodeData(int nodeId, bool uncompressedData=false) const
Definition: Memory.cpp:3450
rtabmap::Memory::_linksChanged
bool _linksChanged
Definition: Memory.h:313
rtabmap::DBDriver::getAllLinks
void getAllLinks(std::multimap< int, Link > &links, bool ignoreNullLinks=true) const
Definition: DBDriver.cpp:852
rtabmap::LaserScan::isEmpty
bool isEmpty() const
Definition: LaserScan.h:69
rtabmap::Memory::createSignature
Signature * createSignature(const SensorData &data, const Transform &pose, Statistics *stats=0)
Definition: Memory.cpp:3630
rtabmap::SensorData::descriptors
const cv::Mat & descriptors() const
Definition: SensorData.h:229
rtabmap::Transform::getNorm
float getNorm() const
Definition: Transform.cpp:231
rtabmap::Memory::_maxStMemSize
int _maxStMemSize
Definition: Memory.h:280
rtabmap::Memory::_getSignature
Signature * _getSignature(int id) const
Definition: Memory.cpp:1013
rtabmap::Memory::_compressionParallelized
bool _compressionParallelized
Definition: Memory.h:290
rtabmap::DBDriver::getNodeData
void getNodeData(int signatureId, SensorData &data, bool images=true, bool scan=true, bool userData=true, bool occupancyGrid=true) const
Definition: DBDriver.cpp:642
rtabmap::VWDictionary::addNewWords
virtual std::list< int > addNewWords(const cv::Mat &descriptors, int signatureId)
Definition: VWDictionary.cpp:634
rtabmap::Memory::getImageCompressed
cv::Mat getImageCompressed(int signatureId) const
Definition: Memory.cpp:3433
rtabmap::ParametersMap
std::map< std::string, std::string > ParametersMap
Definition: Parameters.h:41
rtabmap::Signature::removeVirtualLinks
void removeVirtualLinks()
Definition: Signature.cpp:165
rtabmap::OptimizerG2O::optimizeBA
virtual std::map< int, Transform > optimizeBA(int rootId, const std::map< int, Transform > &poses, const std::multimap< int, Link > &links, const std::map< int, CameraModel > &models, std::map< int, cv::Point3f > &points3DMap, const std::map< int, std::map< int, cv::Point3f > > &wordReferences, std::set< int > *outliers=0)
Definition: OptimizerG2O.cpp:851
rtabmap::VisualWord::getReferences
const std::map< int, int > & getReferences() const
Definition: VisualWord.h:50
rtabmap::DBDriver::loadWords
void loadWords(const std::set< int > &wordIds, std::list< VisualWord * > &vws)
Definition: DBDriver.cpp:580
rtabmap::SensorData::gridCellSize
float gridCellSize() const
Definition: SensorData.h:223
rtabmap::Memory::close
void close(bool databaseSaved=true, bool postInitClosingEvents=false, const std::string &ouputDatabasePath="")
Definition: Memory.cpp:370
rtabmap::LaserScan::maxPoints
int maxPoints() const
Definition: LaserScan.h:64
rtabmap::SensorData::keypoints
const std::vector< cv::KeyPoint > & keypoints() const
Definition: SensorData.h:227
rtabmap::Signature::getWordsDescriptors
const std::multimap< int, cv::Mat > & getWordsDescriptors() const
Definition: Signature.h:111
rtabmap::LaserScan::is2d
bool is2d() const
Definition: LaserScan.h:72
rtabmap::SensorData::setCameraModels
void setCameraModels(const std::vector< CameraModel > &models)
Definition: SensorData.h:168
UMath.h
Basic mathematics functions.
rtabmap::VWDictionary::addWordRef
void addWordRef(int wordId, int signatureId)
Definition: VWDictionary.cpp:603
rtabmap::DBDriver::load
void load(VWDictionary *dictionary, bool lastStateOnly=true) const
Definition: DBDriver.cpp:514
rtabmap::SensorData::setGPS
void setGPS(const GPS &gps)
Definition: SensorData.h:238
rtabmap::RegistrationInfo::inliersIDs
std::vector< int > inliersIDs
Definition: RegistrationInfo.h:70
rtabmap::SensorData::setId
void setId(int id)
Definition: SensorData.h:153
rtabmap::Memory::Memory
Memory(const ParametersMap &parameters=ParametersMap())
Definition: Memory.cpp:72
UConversion.h
Some conversion functions.
rtabmap::DBDriver::getUrl
const std::string & getUrl() const
Definition: DBDriver.h:72
rtabmap::Memory::_registrationPipeline
Registration * _registrationPipeline
Definition: Memory.h:330
rtabmap::Memory::getNodeCalibration
void getNodeCalibration(int nodeId, std::vector< CameraModel > &models, StereoCameraModel &stereoModel)
Definition: Memory.cpp:3512
rtabmap::DBDriver::getNodeInfo
bool getNodeInfo(int signatureId, Transform &pose, int &mapId, int &weight, std::string &label, double &stamp, Transform &groundTruthPose, std::vector< float > &velocity, GPS &gps) const
Definition: DBDriver.cpp:727
rtabmap::VWDictionary::getWord
const VisualWord * getWord(int id) const
Definition: VWDictionary.cpp:1272
rtabmap::VWDictionary::exportDictionary
void exportDictionary(const char *fileNameReferences, const char *fileNameDescriptors) const
Definition: VWDictionary.cpp:1316
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::Memory::getDatabaseVersion
std::string getDatabaseVersion() const
Definition: Memory.cpp:1385
rtabmap::LaserScan::size
int size() const
Definition: LaserScan.h:70
rtabmap::Memory::dumpDictionary
void dumpDictionary(const char *fileNameRef, const char *fileNameDesc) const
Definition: Memory.cpp:3049
rtabmap::Memory::_dbDriver
DBDriver * _dbDriver
Definition: Memory.h:267
rtabmap::SensorData::imageRaw
const cv::Mat & imageRaw() const
Definition: SensorData.h:161
rtabmap::DBDriver::generateGraph
void generateGraph(const std::string &fileName, const std::set< int > &ids=std::set< int >(), const std::map< int, Signature * > &otherSignatures=std::map< int, Signature * >())
Definition: DBDriver.cpp:1123
rtabmap::SensorData::setUserData
void setUserData(const cv::Mat &userData)
Definition: SensorData.cpp:452
rtabmap::VWDictionary::getIndexedWordsCount
unsigned int getIndexedWordsCount() const
Definition: VWDictionary.cpp:338
rtabmap::Memory::cleanUnusedWords
void cleanUnusedWords()
Definition: Memory.cpp:4642
rtabmap::Memory::_reextractLoopClosureFeatures
bool _reextractLoopClosureFeatures
Definition: Memory.h:295
rtabmap::VisualWord::id
int id() const
Definition: VisualWord.h:48
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::Signature::setEnabled
void setEnabled(bool enabled)
Definition: Signature.h:107
UFATAL
#define UFATAL(...)
UTimer::getElapsedTime
double getElapsedTime()
Definition: UTimer.cpp:90
rtabmap::util3d::laserScanToPointCloud
pcl::PointCloud< pcl::PointXYZ >::Ptr RTABMAP_EXP laserScanToPointCloud(const LaserScan &laserScan, const Transform &transform=Transform())
Definition: util3d.cpp:2328
rtabmap::SensorData::setUserDataRaw
void setUserDataRaw(const cv::Mat &userDataRaw)
Definition: SensorData.cpp:436
rtabmap::Feature2D::getMaxFeatures
int getMaxFeatures() const
Definition: Features2d.h:143
util3d_registration.h
rtabmap::Feature2D::getMaxDepth
float getMaxDepth() const
Definition: Features2d.h:145
rtabmap::Memory::_imagePostDecimation
int _imagePostDecimation
Definition: Memory.h:289
rtabmap::DBDriver::isConnected
bool isConnected() const
Definition: DBDriver.cpp:100
rtabmap::SensorData::rightRaw
cv::Mat rightRaw() const
Definition: SensorData.h:173
rtabmap::DBDriver::getAllLabels
void getAllLabels(std::map< int, std::string > &labels) const
Definition: DBDriver.cpp:973
rtabmap::SensorData::setImageRaw
void setImageRaw(const cv::Mat &imageRaw)
Definition: SensorData.h:164
rtabmap::Memory::saveLocationData
void saveLocationData(int locationId)
Definition: Memory.cpp:2276
rtabmap::PreUpdateThread::PreUpdateThread
PreUpdateThread(VWDictionary *vwp)
Definition: Memory.cpp:3617
rtabmap::Memory::dumpSignatures
virtual void dumpSignatures(const char *fileNameSign, bool words3D) const
Definition: Memory.cpp:3057
rtabmap::VWDictionary::getUnusedWords
std::vector< VisualWord * > getUnusedWords() const
Definition: VWDictionary.cpp:1282
rtabmap::Memory::_depthAsMask
bool _depthAsMask
Definition: Memory.h:287
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::_parallelized
bool _parallelized
Definition: Memory.h:328
rtabmap::Memory::savePreviewImage
void savePreviewImage(const cv::Mat &image) const
Definition: Memory.cpp:1784
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::util2d::interpolate
cv::Mat RTABMAP_EXP interpolate(const cv::Mat &image, int factor, float depthErrorRatio=0.02f)
Definition: util2d.cpp:1245
rtabmap::DBDriver::getEmptyTrashesTime
double getEmptyTrashesTime() const
Definition: DBDriver.h:80
util3d_correspondences.h
rtabmap::util3d::laserScan2dFromPointCloud
cv::Mat RTABMAP_EXP laserScan2dFromPointCloud(const pcl::PointCloud< pcl::PointXYZ > &cloud, const Transform &transform=Transform(), bool filterNaNs=true)
Definition: util3d.cpp:2051
rtabmap::Signature::getLabel
const std::string & getLabel() const
Definition: Signature.h:77
uIsFinite
bool uIsFinite(const T &value)
Definition: UMath.h:55
rtabmap::Memory::moveToTrash
void moveToTrash(Signature *s, bool keepLinkedToGraph=true, std::list< int > *deletedWords=0)
Definition: Memory.cpp:2046
rtabmap::Signature::getStamp
double getStamp() const
Definition: Signature.h:79
rtabmap::DBDriver::getNodeIdByLabel
void getNodeIdByLabel(const std::string &label, int &id) const
Definition: DBDriver.cpp:938
rtabmap::DBDriver::openConnection
bool openConnection(const std::string &url, bool overwritten=false)
Definition: DBDriver.cpp:86
UASSERT
#define UASSERT(condition)
rtabmap::VWDictionary::setLastWordId
void setLastWordId(int id)
Definition: VWDictionary.h:79
rtabmap::Memory::reactivateSignatures
std::set< int > reactivateSignatures(const std::list< int > &ids, unsigned int maxLoaded, double &timeDbAccess)
Definition: Memory.cpp:4777
rtabmap::Memory::_memoryChanged
bool _memoryChanged
Definition: Memory.h:312
rtabmap::StereoCameraModel::left
const CameraModel & left() const
Definition: StereoCameraModel.h:119
rtabmap::VWDictionary::parseParameters
virtual void parseParameters(const ParametersMap &parameters)
Definition: VWDictionary.cpp:86
rtabmap::VWDictionary::isIncrementalFlann
bool isIncrementalFlann() const
Definition: VWDictionary.h:89
rtabmap::OccupancyGrid::createLocalMap
void createLocalMap(const Signature &node, cv::Mat &groundCells, cv::Mat &obstacleCells, cv::Mat &emptyCells, cv::Point3f &viewPoint) const
Definition: OccupancyGrid.cpp:282
rtabmap::CameraModel::cx
double cx() const
Definition: CameraModel.h:97
rtabmap::Memory::_recentWmRatio
float _recentWmRatio
Definition: Memory.h:281
uMax3
T uMax3(const T &a, const T &b, const T &c)
Definition: UMath.h:80
rtabmap::Signature::isEnabled
bool isEnabled() const
Definition: Signature.h:106
rtabmap::Feature2D::create
static Feature2D * create(const ParametersMap &parameters=ParametersMap())
Definition: Features2d.cpp:405
rtabmap::Signature::setWeight
void setWeight(int weight)
Definition: Signature.h:73
rtabmap::DBDriver::loadOptimizedPoses
std::map< int, Transform > loadOptimizedPoses(Transform *lastlocalizationPose) const
Definition: DBDriver.cpp:1070
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
rtabmap::Memory::_badSignRatio
float _badSignRatio
Definition: Memory.h:326
rtabmap::Memory::dumpMemory
virtual void dumpMemory(std::string directory) const
Definition: Memory.cpp:3040
UASSERT_MSG
#define UASSERT_MSG(condition, msg_str)
Definition: ULogger.h:67
rtabmap::CameraModel::fy
double fy() const
Definition: CameraModel.h:96
rtabmap::Memory::_rawDescriptorsKept
bool _rawDescriptorsKept
Definition: Memory.h:274
rtabmap::SensorData::clearCompressedData
void clearCompressedData()
Definition: SensorData.h:251
rtabmap::DBDriver::closeConnection
void closeConnection(bool save=true, const std::string &outputUrl="")
Definition: DBDriver.cpp:64
rtabmap::Memory::_imagePreDecimation
int _imagePreDecimation
Definition: Memory.h:288
rtabmap::util3d::laserScanToPointCloudINormal
pcl::PointCloud< pcl::PointXYZINormal >::Ptr RTABMAP_EXP laserScanToPointCloudINormal(const LaserScan &laserScan, const Transform &transform=Transform(), float intensity=0.0f)
Definition: util3d.cpp:2416
rtabmap::DBDriver::loadSignatures
void loadSignatures(const std::list< int > &ids, std::list< Signature * > &signatures, std::set< int > *loadedFromTrash=0)
Definition: DBDriver.cpp:528
rtabmap::Memory::update
bool update(const SensorData &data, Statistics *stats=0)
Definition: Memory.cpp:663
rtabmap::Memory::_laserScanDownsampleStepSize
float _laserScanDownsampleStepSize
Definition: Memory.h:291
rtabmap::Memory::rehearsal
void rehearsal(Signature *signature, Statistics *stats=0)
Definition: Memory.cpp:3163
rtabmap::util2d::decimate
cv::Mat RTABMAP_EXP decimate(const cv::Mat &image, int d)
Definition: util2d.cpp:1196
util3d_filtering.h
rtabmap::Memory::joinTrashThread
void joinTrashThread()
Definition: Memory.cpp:1874
rtabmap::Memory::getSignatureIdByLabel
int getSignatureIdByLabel(const std::string &label, bool lookInDatabase=true) const
Definition: Memory.cpp:2171
rtabmap::Memory::getSignatures
const std::map< int, Signature * > & getSignatures() const
Definition: Memory.h:252
rtabmap::Registration::isScanRequired
bool isScanRequired() const
Definition: Registration.cpp:99
rtabmap::Memory::disableWordsRef
void disableWordsRef(int signatureId)
Definition: Memory.cpp:4620
rtabmap::DBDriver::load2DMap
cv::Mat load2DMap(float &xMin, float &yMin, float &cellSize) const
Definition: DBDriver.cpp:1085
rtabmap::VWDictionary::findNN
std::vector< int > findNN(const std::list< VisualWord * > &vws) const
Definition: VWDictionary.cpp:936
rtabmap::Registration::parseParameters
virtual void parseParameters(const ParametersMap &parameters)
Definition: Registration.cpp:78
rtabmap::DBDriver::getCalibration
bool getCalibration(int signatureId, std::vector< CameraModel > &models, StereoCameraModel &stereoModel) const
Definition: DBDriver.cpp:677
rtabmap::Signature::setWordsDescriptors
void setWordsDescriptors(const std::multimap< int, cv::Mat > &descriptors)
Definition: Signature.h:112
rtabmap::Memory::_laserScanNormalK
int _laserScanNormalK
Definition: Memory.h:293
rtabmap::Memory::computeTransform
Transform computeTransform(Signature &fromS, Signature &toS, Transform guess, RegistrationInfo *info=0, bool useKnownCorrespondencesIfPossible=false) const
Definition: Memory.cpp:2412
uVectorToList
std::list< V > uVectorToList(const std::vector< V > &v)
Definition: UStl.h:486
rtabmap::WeightAgeIdKey::WeightAgeIdKey
WeightAgeIdKey(int w, double a, int i)
Definition: Memory.cpp:1887
rtabmap::Memory::_localBundleOnLoopClosure
bool _localBundleOnLoopClosure
Definition: Memory.h:296
rtabmap::Memory::save2DMap
void save2DMap(const cv::Mat &map, float xMin, float yMin, float cellSize) const
Definition: Memory.cpp:1817
rtabmap::Signature::setVelocity
void setVelocity(float vx, float vy, float vz, float vroll, float vpitch, float vyaw)
Definition: Signature.h:118
rtabmap::Memory::_registrationIcpMulti
RegistrationIcp * _registrationIcpMulti
Definition: Memory.h:331
rtabmap::VWDictionary::clear
void clear(bool printWarningsIfNotEmpty=true)
Definition: VWDictionary.cpp:561
rtabmap::DBDriver::updateLink
void updateLink(const Link &link)
Definition: DBDriver.cpp:477
rtabmap::Signature::id
int id() const
Definition: Signature.h:70
rtabmap::LaserScan::localTransform
Transform localTransform() const
Definition: LaserScan.h:67
rtabmap::DBDriver::getMemoryUsed
long getMemoryUsed() const
Definition: DBDriver.cpp:110
rtabmap::Memory::kIdStart
static const int kIdStart
Definition: Memory.h:64
rtabmap::Memory::emptyTrash
void emptyTrash()
Definition: Memory.cpp:1866
rtabmap::Memory::_createOccupancyGrid
bool _createOccupancyGrid
Definition: Memory.h:301
rtabmap::DBDriver::loadNodeData
void loadNodeData(std::list< Signature * > &signatures, bool images=true, bool scan=true, bool userData=true, bool occupancyGrid=true) const
Definition: DBDriver.cpp:622
rtabmap::SensorData::gps
const GPS & gps() const
Definition: SensorData.h:242
rtabmap::DBDriver::saveOptimizedMesh
void saveOptimizedMesh(const cv::Mat &cloud, const std::vector< std::vector< std::vector< unsigned int > > > &polygons=std::vector< std::vector< std::vector< unsigned int > > >(), const std::vector< std::vector< Eigen::Vector2f > > &texCoords=std::vector< std::vector< Eigen::Vector2f > >(), const cv::Mat &textures=cv::Mat()) const
Definition: DBDriver.cpp:1093
rtabmap::Memory::forget
std::list< int > forget(const std::set< int > &ignoredIds=std::set< int >())
Definition: Memory.cpp:1685
rtabmap::Memory::getSignatureDataConst
SensorData getSignatureDataConst(int locationId, bool images=true, bool scan=true, bool userData=true, bool occupancyGrid=true) const
Definition: Memory.cpp:3530
rtabmap::SensorData::depthOrRightRaw
const cv::Mat & depthOrRightRaw() const
Definition: SensorData.h:162
UTimer::start
void start()
Definition: UTimer.cpp:80
rtabmap::Memory::_visCorType
int _visCorType
Definition: Memory.h:303
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::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::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
util3d_transforms.h
rtabmap::Memory::moveSignatureToWMFromSTM
void moveSignatureToWMFromSTM(int id, int *reducedTo=0)
Definition: Memory.cpp:907
rtabmap::Memory::getRemovableSignatures
std::list< Signature * > getRemovableSignatures(int count, const std::set< int > &ignoredIds=std::set< int >())
Definition: Memory.cpp:1915
rtabmap::Memory::_laserScanVoxelSize
float _laserScanVoxelSize
Definition: Memory.h:292
rtabmap::Memory::_rectifyOnlyFeatures
bool _rectifyOnlyFeatures
Definition: Memory.h:305
rtabmap::DBDriver::addStatistics
void addStatistics(const Statistics &statistics) const
Definition: DBDriver.cpp:1042
uContains
bool uContains(const std::list< V > &list, const V &value)
Definition: UStl.h:409
rtabmap::CameraModel::cy
double cy() const
Definition: CameraModel.h:98
rtabmap::Signature::setPose
void setPose(const Transform &pose)
Definition: Signature.h:116
rtabmap::Signature::setSaved
void setSaved(bool saved)
Definition: Signature.h:93
rtabmap::Signature::setLabel
void setLabel(const std::string &label)
Definition: Signature.h:76
rtabmap::DBDriver::create
static DBDriver * create(const ParametersMap &parameters=ParametersMap())
Definition: DBDriver.cpp:41
rtabmap::Memory::getVWDictionary
const VWDictionary * getVWDictionary() const
Definition: Memory.cpp:1018
rtabmap::Memory::_saveIntermediateNodeData
bool _saveIntermediateNodeData
Definition: Memory.h:277
rtabmap::Signature::compareTo
float compareTo(const Signature &signature) const
Definition: Signature.cpp:180
rtabmap::Memory::_rectCameraModels
std::vector< CameraModel > _rectCameraModels
Definition: Memory.h:316
rtabmap::PreUpdateThread::_vwp
VWDictionary * _vwp
Definition: Memory.cpp:3627
rtabmap::CameraModel::rectifyImage
cv::Mat rectifyImage(const cv::Mat &raw, int interpolation=cv::INTER_LINEAR) const
Definition: CameraModel.cpp:537
rtabmap::Memory::enableWordsRef
void enableWordsRef(const std::list< int > &signatureIds)
Definition: Memory.cpp:4665
rtabmap::Memory::incrementMapId
int incrementMapId(std::map< int, int > *reducedIds=0)
Definition: Memory.cpp:1342
rtabmap::SensorData::userDataCompressed
const cv::Mat & userDataCompressed() const
Definition: SensorData.h:206
false
#define false
Definition: ConvertUTF.c:56
uSleep
void uSleep(unsigned int ms)
Definition: Posix/UThreadC.h:23
rtabmap::Memory::getLastSignatureId
int getLastSignatureId() const
Definition: Memory.cpp:2160
rtabmap::Memory::removeAllVirtualLinks
void removeAllVirtualLinks()
Definition: Memory.cpp:3001
rtabmap::Memory::_rectStereoCameraModel
StereoCameraModel _rectStereoCameraModel
Definition: Memory.h:317
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::Memory::_notLinkedNodesKeptInDb
bool _notLinkedNodesKeptInDb
Definition: Memory.h:276
rtabmap::SensorData::cameraModels
const std::vector< CameraModel > & cameraModels() const
Definition: SensorData.h:193
rtabmap::Signature::getPose
const Transform & getPose() const
Definition: Signature.h:129
rtabmap::Memory::parseParameters
virtual void parseParameters(const ParametersMap &parameters)
Definition: Memory.cpp:442
rtabmap::Memory::_feature2D
Feature2D * _feature2D
Definition: Memory.h:325
rtabmap::Registration::create
static Registration * create(const ParametersMap &parameters)
Definition: Registration.cpp:38
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::Memory::_badSignaturesIgnored
bool _badSignaturesIgnored
Definition: Memory.h:285
rtabmap::Memory::_useOdometryFeatures
bool _useOdometryFeatures
Definition: Memory.h:300
UDEBUG
#define UDEBUG(...)
rtabmap::CameraModel::fx
double fx() const
Definition: CameraModel.h:95
rtabmap::Memory::kIdVirtual
static const int kIdVirtual
Definition: Memory.h:65
rtabmap::Signature::sensorData
SensorData & sensorData()
Definition: Signature.h:134
rtabmap::VWDictionary::removeWords
void removeWords(const std::vector< VisualWord * > &words)
Definition: VWDictionary.cpp:1292
rtabmap::StereoCameraModel::isValidForRectification
bool isValidForRectification() const
Definition: StereoCameraModel.h:86
rtabmap::StereoCameraModel::right
const CameraModel & right() const
Definition: StereoCameraModel.h:120
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
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::Memory::_generateIds
bool _generateIds
Definition: Memory.h:284
rtabmap::Memory::_rehearsalMaxDistance
float _rehearsalMaxDistance
Definition: Memory.h:297
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::CompressionThread::getCompressedData
const cv::Mat & getCompressedData() const
Definition: Compression.h:61
rtabmap::Memory::rehearsalMerge
bool rehearsalMerge(int oldId, int newId)
Definition: Memory.cpp:3220
rtabmap::Memory::preUpdate
void preUpdate()
Definition: Memory.cpp:649
rtabmap::util3d::laserScanToPointCloudNormal
pcl::PointCloud< pcl::PointNormal >::Ptr RTABMAP_EXP laserScanToPointCloudNormal(const LaserScan &laserScan, const Transform &transform=Transform())
Definition: util3d.cpp:2346
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
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::DBDriver::savePreviewImage
void savePreviewImage(const cv::Mat &image) const
Definition: DBDriver.cpp:1049
UERROR
#define UERROR(...)
rtabmap::Memory::_saveDepth16Format
bool _saveDepth16Format
Definition: Memory.h:275
rtabmap::Memory::saveStatistics
void saveStatistics(const Statistics &statistics)
Definition: Memory.cpp:1776
ULOGGER_WARN
#define ULOGGER_WARN(...)
Definition: ULogger.h:55
rtabmap::Memory::saveOptimizedPoses
void saveOptimizedPoses(const std::map< int, Transform > &optimizedPoses, const Transform &lastlocalizationPose) const
Definition: Memory.cpp:1800
ULogger.h
ULogger class and convenient macros.
rtabmap::SensorData::stereoCameraModel
const StereoCameraModel & stereoCameraModel() const
Definition: SensorData.h:194
rtabmap::Memory::load2DMap
cv::Mat load2DMap(float &xMin, float &yMin, float &cellSize) const
Definition: Memory.cpp:1825
rtabmap::Memory::isBinDataKept
bool isBinDataKept() const
Definition: Memory.h:156
UWARN
#define UWARN(...)
rtabmap::VWDictionary::getNotIndexedWordsCount
unsigned int getNotIndexedWordsCount() const
Definition: VWDictionary.h:82
rtabmap::VWDictionary::removeAllWordRef
void removeAllWordRef(int wordId, int signatureId)
Definition: VWDictionary.cpp:620
rtabmap::Registration::isImageRequired
bool isImageRequired() const
Definition: Registration.cpp:89
rtabmap::Memory::_idUpdatedToNewOneRehearsal
bool _idUpdatedToNewOneRehearsal
Definition: Memory.h:283
UTimer::ticks
double ticks()
Definition: UTimer.cpp:110
rtabmap::Memory::_incrementalMemory
bool _incrementalMemory
Definition: Memory.h:278
UTimer::now
static double now()
Definition: UTimer.cpp:73
rtabmap::DBDriver::setTimestampUpdateEnabled
void setTimestampUpdateEnabled(bool enabled)
Definition: DBDriver.h:81
glm::yaw
GLM_FUNC_DECL T yaw(detail::tquat< T, P > const &x)
rtabmap::Memory::memoryChanged
bool memoryChanged() const
Definition: Memory.h:199
rtabmap::SensorData::imageCompressed
const cv::Mat & imageCompressed() const
Definition: SensorData.h:157
rtabmap::Memory::computeLikelihood
std::map< int, float > computeLikelihood(const Signature *signature, const std::list< int > &ids)
Definition: Memory.cpp:1545
rtabmap::DBDriver::getDatabaseVersion
std::string getDatabaseVersion() const
Definition: DBDriver.cpp:275
rtabmap::Memory::getWeights
std::map< int, int > getWeights() const
Definition: Memory.cpp:1663
rtabmap::Memory::_laserScanNormalRadius
float _laserScanNormalRadius
Definition: Memory.h:294
rtabmap::VWDictionary::getUnusedWordsSize
unsigned int getUnusedWordsSize() const
Definition: VWDictionary.h:98
rtabmap::DBDriver::loadLastNodes
void loadLastNodes(std::list< Signature * > &signatures) const
Definition: DBDriver.cpp:521
rtabmap::OccupancyGrid::parseParameters
void parseParameters(const ParametersMap &parameters)
Definition: OccupancyGrid.cpp:92
rtabmap::Signature::addLink
void addLink(const Link &link)
Definition: Signature.cpp:119
util3d_features.h
rtabmap::SensorData::setGroundTruth
void setGroundTruth(const Transform &pose)
Definition: SensorData.h:231
rtabmap::util2d::cvtDepthFromFloat
cv::Mat RTABMAP_EXP cvtDepthFromFloat(const cv::Mat &depth32F)
Definition: util2d.cpp:887
rtabmap::Transform::inverse
Transform inverse() const
Definition: Transform.cpp:169
rtabmap::Memory::getAllLinks
std::multimap< int, Link > getAllLinks(bool lookInDatabase, bool ignoreNullLinks=true) const
Definition: Memory.cpp:1126
rtabmap::Memory::getAllSignatureIds
std::set< int > getAllSignatureIds() const
Definition: Memory.cpp:1410
rtabmap::util3d::commonFiltering
LaserScan RTABMAP_EXP commonFiltering(const LaserScan &scan, int downsamplingStep, float rangeMin=0.0f, float rangeMax=0.0f, float voxelSize=0.0f, int normalK=0, float normalRadius=0.0f, bool forceGroundNormalsUp=false)
Definition: util3d_filtering.cpp:74
rtabmap::Memory::parameters_
ParametersMap parameters_
Definition: Memory.h:271
rtabmap::Memory::isIncremental
bool isIncremental() const
Definition: Memory.h:200
rtabmap::Memory::_binDataKept
bool _binDataKept
Definition: Memory.h:273
rtabmap::Memory::updateAge
void updateAge(int signatureId)
Definition: Memory.cpp:1365
rtabmap::Memory::saveOptimizedMesh
void saveOptimizedMesh(const cv::Mat &cloud, const std::vector< std::vector< std::vector< unsigned int > > > &polygons=std::vector< std::vector< std::vector< unsigned int > > >(), const std::vector< std::vector< Eigen::Vector2f > > &texCoords=std::vector< std::vector< Eigen::Vector2f > >(), const cv::Mat &textures=cv::Mat()) const
Definition: Memory.cpp:1834
rtabmap::Memory::getOdomPose
Transform getOdomPose(int signatureId, bool lookInDatabase=false) const
Definition: Memory.cpp:3378
rtabmap::util3d::laserScanToPointCloudI
pcl::PointCloud< pcl::PointXYZI >::Ptr RTABMAP_EXP laserScanToPointCloudI(const LaserScan &laserScan, const Transform &transform=Transform(), float intensity=0.0f)
Definition: util3d.cpp:2381
rtabmap::Memory::_signatures
std::map< int, Signature * > _signatures
Definition: Memory.h:319
UThread::join
void join(bool killFirst=false)
Definition: UThread.cpp:85
rtabmap::Signature::removeLink
void removeLink(int idTo)
Definition: Signature.cpp:155
rtabmap::VWDictionary::addWord
virtual void addWord(VisualWord *vw)
Definition: VWDictionary.cpp:1251
rtabmap::Memory::dumpMemoryTree
void dumpMemoryTree(const char *fileNameTree) const
Definition: Memory.cpp:3105
rtabmap::DBDriver::addLink
void addLink(const Link &link)
Definition: DBDriver.cpp:467
rtabmap::Memory::_rehearsalWeightIgnoredWhileMoving
bool _rehearsalWeightIgnoredWhileMoving
Definition: Memory.h:299
rtabmap::Memory::_covOffDiagonalIgnored
bool _covOffDiagonalIgnored
Definition: Memory.h:306
ULOGGER_ERROR
#define ULOGGER_ERROR(...)
Definition: ULogger.h:56
rtabmap::VWDictionary::getUnusedWord
VisualWord * getUnusedWord(int id) const
Definition: VWDictionary.cpp:1277
rtabmap::Signature::isBadSignature
bool isBadSignature() const
Definition: Signature.cpp:246
rtabmap::Memory::copyData
void copyData(const Signature *from, Signature *to)
Definition: Memory.cpp:3579
rtabmap::VWDictionary::getTotalActiveReferences
int getTotalActiveReferences() const
Definition: VWDictionary.h:84
rtabmap::DBDriver::isInMemory
virtual bool isInMemory() const
Definition: DBDriver.h:71
rtabmap::SensorData::setStereoCameraModel
void setStereoCameraModel(const StereoCameraModel &stereoCameraModel)
Definition: SensorData.h:169
rtabmap::VWDictionary::isIncremental
bool isIncremental() const
Definition: VWDictionary.h:88
rtabmap::Memory::_rehearsalMaxAngle
float _rehearsalMaxAngle
Definition: Memory.h:298
rtabmap::DBDriver::getInvertedIndexNi
void getInvertedIndexNi(int signatureId, int &ni) const
Definition: DBDriver.cpp:918
rtabmap::SensorData::setDepthOrRightRaw
void setDepthOrRightRaw(const cv::Mat &depthOrImageRaw)
Definition: SensorData.h:165
rtabmap::Memory::_lastGlobalLoopClosureId
int _lastGlobalLoopClosureId
Definition: Memory.h:311
rtabmap::DBDriver::save2DMap
void save2DMap(const cv::Mat &map, float xMin, float yMin, float cellSize) const
Definition: DBDriver.cpp:1078
rtabmap::Memory::computeIcpTransformMulti
Transform computeIcpTransformMulti(int newId, int oldId, const std::map< int, Transform > &poses, RegistrationInfo *info=0)
Definition: Memory.cpp:2698
rtabmap::Memory::addSignatureToStm
void addSignatureToStm(Signature *signature, const cv::Mat &covariance)
Definition: Memory.cpp:787
rtabmap::PreUpdateThread::~PreUpdateThread
virtual ~PreUpdateThread()
Definition: Memory.cpp:3618
rtabmap::Transform::getTranslationAndEulerAngles
void getTranslationAndEulerAngles(float &x, float &y, float &z, float &roll, float &pitch, float &yaw) const
Definition: Transform.cpp:206
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::RegistrationIcp::parseParameters
virtual void parseParameters(const ParametersMap &parameters)
Definition: RegistrationIcp.cpp:396
rtabmap::Memory::setUserData
bool setUserData(int id, const cv::Mat &data)
Definition: Memory.cpp:2251
rtabmap::CameraModel::imageHeight
int imageHeight() const
Definition: CameraModel.h:114
rtabmap::DBDriver::getLastWordId
void getLastWordId(int &id) const
Definition: DBDriver.cpp:903
rtabmap::DBDriver::parseParameters
virtual void parseParameters(const ParametersMap &parameters)
Definition: DBDriver.cpp:60
rtabmap::Memory::_workingMem
std::map< int, double > _workingMem
Definition: Memory.h:321
rtabmap::Memory::getNi
int getNi(int signatureId) const
Definition: Memory.cpp:3563
uValue
V uValue(const std::map< K, V > &m, const K &key, const V &defaultValue=V())
Definition: UStl.h:240
rtabmap::DBDriver::emptyTrashes
void emptyTrashes(bool async=false)
Definition: DBDriver.cpp:311
rtabmap::SensorData::setFeatures
void setFeatures(const std::vector< cv::KeyPoint > &keypoints, const std::vector< cv::Point3f > &keypoints3D, const cv::Mat &descriptors)
Definition: SensorData.cpp:828
rtabmap::Memory::getDbSavingTime
double getDbSavingTime() const
Definition: Memory.cpp:1405
rtabmap::Memory::loadDataFromDb
void loadDataFromDb(bool postInitClosingEvents)
Definition: Memory.cpp:207
rtabmap::SensorData::depthRaw
cv::Mat depthRaw() const
Definition: SensorData.h:172
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::LaserScan::hasIntensity
bool hasIntensity() const
Definition: LaserScan.h:75
rtabmap::Memory::getDatabaseMemoryUsed
int getDatabaseMemoryUsed() const
Definition: Memory.cpp:1374
rtabmap::Memory::_idMapCount
int _idMapCount
Definition: Memory.h:309
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::CameraModel::localTransform
const Transform & localTransform() const
Definition: CameraModel.h:109
util3d_surface.h
rtabmap::SensorData::setLaserScanRaw
void setLaserScanRaw(const LaserScan &laserScanRaw)
Definition: SensorData.h:166
rtabmap::compressImage2
cv::Mat RTABMAP_EXP compressImage2(const cv::Mat &image, const std::string &format=".png")
Definition: Compression.cpp:120
UINFO
#define UINFO(...)

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