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

rtabmap
Author(s): Mathieu Labbe
autogenerated on Mon Apr 28 2025 02:45:57