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

rtabmap
Author(s): Mathieu Labbe
autogenerated on Mon Dec 14 2020 03:34:59