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

rtabmap
Author(s): Mathieu Labbe
autogenerated on Sun Dec 1 2024 03:42:49