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  return transform;
3190 }
3191 
3192 Transform Memory::computeIcpTransform(
3193  const Signature & fromS,
3194  const Signature & toS,
3195  Transform guess,
3196  RegistrationInfo * info) const
3197 {
3198  UDEBUG("%d -> %d, Guess=%s", fromS.id(), toS.id(), guess.prettyPrint().c_str());
3199 
3200  Signature tmpFrom = fromS;
3201  Signature tmpTo = toS;
3202  return _registrationIcpMulti->computeTransformation(tmpFrom.sensorData(), tmpTo.sensorData(), guess, info);
3203 }
3204 
3205 // compute transform fromId -> multiple toId
3206 Transform Memory::computeIcpTransformMulti(
3207  int fromId,
3208  int toId,
3209  const std::map<int, Transform> & poses,
3210  RegistrationInfo * info)
3211 {
3212  UASSERT(uContains(poses, fromId) && uContains(_signatures, fromId));
3213  UASSERT(uContains(poses, toId) && uContains(_signatures, toId));
3214 
3215  UDEBUG("%d -> %d, Guess=%s", fromId, toId, (poses.at(fromId).inverse() * poses.at(toId)).prettyPrint().c_str());
3216  if(ULogger::level() == ULogger::kDebug)
3217  {
3218  std::string ids;
3219  for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
3220  {
3221  if(iter->first != fromId)
3222  {
3223  ids += uNumber2Str(iter->first) + " ";
3224  }
3225  }
3226  UDEBUG("%d vs %s", fromId, ids.c_str());
3227  }
3228 
3229  // make sure that all laser scans are loaded
3230  std::list<Signature*> scansToLoad;
3231  for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
3232  {
3233  Signature * s = _getSignature(iter->first);
3234  UASSERT_MSG(s != 0, uFormat("id=%d", iter->first).c_str());
3235  //if image is already here, scan should be or it is null
3236  if(s->sensorData().imageCompressed().empty() &&
3237  s->sensorData().laserScanCompressed().isEmpty())
3238  {
3239  scansToLoad.push_back(s);
3240  }
3241  }
3242  if(scansToLoad.size() && _dbDriver)
3243  {
3244  _dbDriver->loadNodeData(scansToLoad, false, true, false, false);
3245  }
3246 
3247  Signature * fromS = _getSignature(fromId);
3248  LaserScan fromScan;
3249  fromS->sensorData().uncompressData(0, 0, &fromScan);
3250 
3251  Signature * toS = _getSignature(toId);
3252  LaserScan toScan;
3253  toS->sensorData().uncompressData(0, 0, &toScan);
3254 
3255  Transform t;
3256  if(!fromScan.isEmpty() && !toScan.isEmpty())
3257  {
3258  Transform guess = poses.at(toId).inverse() * poses.at(fromId);
3259  float guessNorm = guess.getNorm();
3260  if(fromScan.rangeMax() > 0.0f && toScan.rangeMax() > 0.0f &&
3261  guessNorm > fromScan.rangeMax() + toScan.rangeMax())
3262  {
3263  // stop right known,it is impossible that scans overlay.
3264  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());
3265  return t;
3266  }
3267 
3268  // Create a fake signature with all scans merged in oldId referential
3269  Transform toPoseInv = poses.at(toId).inverse();
3270  std::string msg;
3271  int maxPoints = fromScan.size();
3272  pcl::PointCloud<pcl::PointXYZ>::Ptr assembledToClouds(new pcl::PointCloud<pcl::PointXYZ>);
3273  pcl::PointCloud<pcl::PointNormal>::Ptr assembledToNormalClouds(new pcl::PointCloud<pcl::PointNormal>);
3274  pcl::PointCloud<pcl::PointXYZI>::Ptr assembledToIClouds(new pcl::PointCloud<pcl::PointXYZI>);
3275  pcl::PointCloud<pcl::PointXYZINormal>::Ptr assembledToNormalIClouds(new pcl::PointCloud<pcl::PointXYZINormal>);
3276  pcl::PointCloud<pcl::PointXYZRGB>::Ptr assembledToRGBClouds(new pcl::PointCloud<pcl::PointXYZRGB>);
3277  pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr assembledToNormalRGBClouds(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
3278  UDEBUG("maxPoints from(%d) = %d", fromId, maxPoints);
3279  for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
3280  {
3281  if(iter->first != fromId)
3282  {
3283  Signature * s = this->_getSignature(iter->first);
3284  if(!s->sensorData().laserScanCompressed().isEmpty())
3285  {
3286  LaserScan scan;
3287  s->sensorData().uncompressData(0, 0, &scan);
3288  if(!scan.isEmpty() && scan.format() == toScan.format())
3289  {
3290  if(scan.hasIntensity())
3291  {
3292  if(scan.hasNormals())
3293  {
3294  *assembledToNormalIClouds += *util3d::laserScanToPointCloudINormal(scan,
3295  toPoseInv * iter->second * scan.localTransform());
3296  }
3297  else
3298  {
3299  *assembledToIClouds += *util3d::laserScanToPointCloudI(scan,
3300  toPoseInv * iter->second * scan.localTransform());
3301  }
3302  }
3303  else if(scan.hasRGB())
3304  {
3305  if(scan.hasNormals())
3306  {
3307  *assembledToNormalRGBClouds += *util3d::laserScanToPointCloudRGBNormal(scan,
3308  toPoseInv * iter->second * scan.localTransform());
3309  }
3310  else
3311  {
3312  *assembledToRGBClouds += *util3d::laserScanToPointCloudRGB(scan,
3313  toPoseInv * iter->second * scan.localTransform());
3314  }
3315  }
3316  else
3317  {
3318  if(scan.hasNormals())
3319  {
3320  *assembledToNormalClouds += *util3d::laserScanToPointCloudNormal(scan,
3321  toPoseInv * iter->second * scan.localTransform());
3322  }
3323  else
3324  {
3325  *assembledToClouds += *util3d::laserScanToPointCloud(scan,
3326  toPoseInv * iter->second * scan.localTransform());
3327  }
3328  }
3329 
3330  if(scan.size() > maxPoints)
3331  {
3332  UDEBUG("maxPoints scan(%d) = %d", iter->first, (int)scan.size());
3333  maxPoints = scan.size();
3334  }
3335  }
3336  else if(!scan.isEmpty())
3337  {
3338  UWARN("Incompatible scan format %s vs %s", toScan.formatName().c_str(), scan.formatName().c_str());
3339  }
3340  }
3341  else
3342  {
3343  UWARN("Depth2D not found for signature %d", iter->first);
3344  }
3345  }
3346  }
3347 
3348  LaserScan assembledScan;
3349  if(assembledToNormalClouds->size())
3350  {
3351  assembledScan = fromScan.is2d()?util3d::laserScan2dFromPointCloud(*assembledToNormalClouds):util3d::laserScanFromPointCloud(*assembledToNormalClouds);
3352  }
3353  else if(assembledToClouds->size())
3354  {
3355  assembledScan = fromScan.is2d()?util3d::laserScan2dFromPointCloud(*assembledToClouds):util3d::laserScanFromPointCloud(*assembledToClouds);
3356  }
3357  else if(assembledToNormalIClouds->size())
3358  {
3359  assembledScan = fromScan.is2d()?util3d::laserScan2dFromPointCloud(*assembledToNormalIClouds):util3d::laserScanFromPointCloud(*assembledToNormalIClouds);
3360  }
3361  else if(assembledToIClouds->size())
3362  {
3363  assembledScan = fromScan.is2d()?util3d::laserScan2dFromPointCloud(*assembledToIClouds):util3d::laserScanFromPointCloud(*assembledToIClouds);
3364  }
3365  else if(assembledToNormalRGBClouds->size())
3366  {
3367  if(fromScan.is2d())
3368  {
3369  UERROR("Cannot handle 2d scan with RGB format.");
3370  }
3371  else
3372  {
3373  assembledScan = util3d::laserScanFromPointCloud(*assembledToNormalRGBClouds);
3374  }
3375  }
3376  else if(assembledToRGBClouds->size())
3377  {
3378  if(fromScan.is2d())
3379  {
3380  UERROR("Cannot handle 2d scan with RGB format.");
3381  }
3382  else
3383  {
3384  assembledScan = util3d::laserScanFromPointCloud(*assembledToRGBClouds);
3385  }
3386  }
3387  UDEBUG("assembledScan=%d points", assembledScan.size());
3388 
3389  // scans are in base frame but for 2d scans, set the height so that correspondences matching works
3390  SensorData assembledData;
3391  assembledData.setLaserScan(
3392  LaserScan(assembledScan,
3393  maxPoints,
3394  fromScan.rangeMax(),
3395  fromScan.is2d()?Transform(0,0,fromScan.localTransform().z(),0,0,0):Transform::getIdentity()));
3396 
3397  t = _registrationIcpMulti->computeTransformation(assembledData, fromS->sensorData(), guess, info);
3398  if(!t.isNull())
3399  {
3400  t = t.inverse();
3401  }
3402  }
3403 
3404  return t;
3405 }
3406 
3407 bool Memory::addLink(const Link & link, bool addInDatabase)
3408 {
3409  UASSERT(link.type() > Link::kNeighbor && link.type() != Link::kUndef);
3410 
3411  ULOGGER_INFO("to=%d, from=%d transform: %s var=%f", link.to(), link.from(), link.transform().prettyPrint().c_str(), link.transVariance(false));
3412  Signature * toS = _getSignature(link.to());
3413  Signature * fromS = _getSignature(link.from());
3414  if(toS && fromS)
3415  {
3416  if(toS->hasLink(link.from()))
3417  {
3418  // do nothing, already merged
3419  UINFO("already linked! to=%d, from=%d", link.to(), link.from());
3420  return true;
3421  }
3422 
3423  UDEBUG("Add link between %d and %d", toS->id(), fromS->id());
3424 
3425  toS->addLink(link.inverse());
3426  fromS->addLink(link);
3427 
3428  if(_incrementalMemory)
3429  {
3430  if(link.type()!=Link::kVirtualClosure)
3431  {
3432  _linksChanged = true;
3433 
3434  // update weight
3435  // ignore scan matching loop closures
3436  if(link.type() != Link::kLocalSpaceClosure ||
3437  link.userDataCompressed().empty())
3438  {
3439  _lastGlobalLoopClosureId = fromS->id()>toS->id()?fromS->id():toS->id();
3440 
3441  // update weights only if the memory is incremental
3442  // When reducing the graph, transfer weight to the oldest signature
3443  UASSERT(fromS->getWeight() >= 0 && toS->getWeight() >=0);
3444  if((_reduceGraph && fromS->id() < toS->id()) ||
3445  (!_reduceGraph && fromS->id() > toS->id()))
3446  {
3447  fromS->setWeight(fromS->getWeight() + toS->getWeight());
3448  toS->setWeight(0);
3449  }
3450  else
3451  {
3452  toS->setWeight(toS->getWeight() + fromS->getWeight());
3453  fromS->setWeight(0);
3454  }
3455  }
3456  }
3457  }
3458  }
3459  else if(!addInDatabase)
3460  {
3461  if(!fromS)
3462  {
3463  UERROR("from=%d, to=%d, Signature %d not found in working/st memories", link.from(), link.to(), link.from());
3464  }
3465  if(!toS)
3466  {
3467  UERROR("from=%d, to=%d, Signature %d not found in working/st memories", link.from(), link.to(), link.to());
3468  }
3469  return false;
3470  }
3471  else if(fromS)
3472  {
3473  UDEBUG("Add link between %d and %d (db)", link.from(), link.to());
3474  fromS->addLink(link);
3475  _dbDriver->addLink(link.inverse());
3476  }
3477  else if(toS)
3478  {
3479  UDEBUG("Add link between %d (db) and %d", link.from(), link.to());
3480  _dbDriver->addLink(link);
3481  toS->addLink(link.inverse());
3482  }
3483  else
3484  {
3485  UDEBUG("Add link between %d (db) and %d (db)", link.from(), link.to());
3486  _dbDriver->addLink(link);
3487  _dbDriver->addLink(link.inverse());
3488  }
3489  return true;
3490 }
3491 
3492 void Memory::updateLink(const Link & link, bool updateInDatabase)
3493 {
3494  Signature * fromS = this->_getSignature(link.from());
3495  Signature * toS = this->_getSignature(link.to());
3496 
3497  if(fromS && toS)
3498  {
3499  if(fromS->hasLink(link.to()) && toS->hasLink(link.from()))
3500  {
3501  Link::Type oldType = fromS->getLinks().find(link.to())->second.type();
3502 
3503  fromS->removeLink(link.to());
3504  toS->removeLink(link.from());
3505 
3506  fromS->addLink(link);
3507  toS->addLink(link.inverse());
3508 
3509  if(oldType!=Link::kVirtualClosure || link.type()!=Link::kVirtualClosure)
3510  {
3511  _linksChanged = true;
3512  }
3513  }
3514  else
3515  {
3516  UERROR("fromId=%d and toId=%d are not linked!", link.from(), link.to());
3517  }
3518  }
3519  else if(!updateInDatabase)
3520  {
3521  if(!fromS)
3522  {
3523  UERROR("from=%d, to=%d, Signature %d not found in working/st memories", link.from(), link.to(), link.from());
3524  }
3525  if(!toS)
3526  {
3527  UERROR("from=%d, to=%d, Signature %d not found in working/st memories", link.from(), link.to(), link.to());
3528  }
3529  }
3530  else if(fromS)
3531  {
3532  UDEBUG("Update link between %d and %d (db)", link.from(), link.to());
3533  fromS->removeLink(link.to());
3534  fromS->addLink(link);
3535  _dbDriver->updateLink(link.inverse());
3536  }
3537  else if(toS)
3538  {
3539  UDEBUG("Update link between %d (db) and %d", link.from(), link.to());
3540  toS->removeLink(link.from());
3541  toS->addLink(link.inverse());
3542  _dbDriver->updateLink(link);
3543  }
3544  else
3545  {
3546  UDEBUG("Update link between %d (db) and %d (db)", link.from(), link.to());
3547  _dbDriver->updateLink(link);
3548  _dbDriver->updateLink(link.inverse());
3549  }
3550 }
3551 
3552 void Memory::removeAllVirtualLinks()
3553 {
3554  UDEBUG("");
3555  for(std::map<int, Signature*>::iterator iter=_signatures.begin(); iter!=_signatures.end(); ++iter)
3556  {
3557  iter->second->removeVirtualLinks();
3558  }
3559 }
3560 
3561 void Memory::removeVirtualLinks(int signatureId)
3562 {
3563  UDEBUG("");
3564  Signature * s = this->_getSignature(signatureId);
3565  if(s)
3566  {
3567  const std::multimap<int, Link> & links = s->getLinks();
3568  for(std::multimap<int, Link>::const_iterator iter=links.begin(); iter!=links.end(); ++iter)
3569  {
3570  if(iter->second.type() == Link::kVirtualClosure)
3571  {
3572  Signature * sTo = this->_getSignature(iter->first);
3573  if(sTo)
3574  {
3575  sTo->removeLink(s->id());
3576  }
3577  else
3578  {
3579  UERROR("Link %d of %d not in WM/STM?!?", iter->first, s->id());
3580  }
3581  }
3582  }
3583  s->removeVirtualLinks();
3584  }
3585  else
3586  {
3587  UERROR("Signature %d not in WM/STM?!?", signatureId);
3588  }
3589 }
3590 
3591 void Memory::dumpMemory(std::string directory) const
3592 {
3593  UINFO("Dumping memory to directory \"%s\"", directory.c_str());
3594  this->dumpDictionary((directory+"/DumpMemoryWordRef.txt").c_str(), (directory+"/DumpMemoryWordDesc.txt").c_str());
3595  this->dumpSignatures((directory + "/DumpMemorySign.txt").c_str(), false);
3596  this->dumpSignatures((directory + "/DumpMemorySign3.txt").c_str(), true);
3597  this->dumpMemoryTree((directory + "/DumpMemoryTree.txt").c_str());
3598 }
3599 
3600 void Memory::dumpDictionary(const char * fileNameRef, const char * fileNameDesc) const
3601 {
3602  if(_vwd)
3603  {
3604  _vwd->exportDictionary(fileNameRef, fileNameDesc);
3605  }
3606 }
3607 
3608 void Memory::dumpSignatures(const char * fileNameSign, bool words3D) const
3609 {
3610  UDEBUG("");
3611  FILE* foutSign = 0;
3612 #ifdef _MSC_VER
3613  fopen_s(&foutSign, fileNameSign, "w");
3614 #else
3615  foutSign = fopen(fileNameSign, "w");
3616 #endif
3617 
3618  if(foutSign)
3619  {
3620  fprintf(foutSign, "SignatureID WordsID...\n");
3621  const std::map<int, Signature *> & signatures = this->getSignatures();
3622  for(std::map<int, Signature *>::const_iterator iter=signatures.begin(); iter!=signatures.end(); ++iter)
3623  {
3624  fprintf(foutSign, "%d ", iter->first);
3625  const Signature * ss = dynamic_cast<const Signature *>(iter->second);
3626  if(ss)
3627  {
3628  if(words3D)
3629  {
3630  if(!ss->getWords3().empty())
3631  {
3632  const std::multimap<int, int> & ref = ss->getWords();
3633  for(std::multimap<int, int>::const_iterator jter=ref.begin(); jter!=ref.end(); ++jter)
3634  {
3635  const cv::Point3f & pt = ss->getWords3()[jter->second];
3636  //show only valid point according to current parameters
3637  if(pcl::isFinite(pt) &&
3638  (pt.x != 0 || pt.y != 0 || pt.z != 0))
3639  {
3640  fprintf(foutSign, "%d ", (*jter).first);
3641  }
3642  }
3643  }
3644  }
3645  else
3646  {
3647  const std::multimap<int, int> & ref = ss->getWords();
3648  for(std::multimap<int, int>::const_iterator jter=ref.begin(); jter!=ref.end(); ++jter)
3649  {
3650  fprintf(foutSign, "%d ", (*jter).first);
3651  }
3652  }
3653  }
3654  fprintf(foutSign, "\n");
3655  }
3656  fclose(foutSign);
3657  }
3658 }
3659 
3660 void Memory::dumpMemoryTree(const char * fileNameTree) const
3661 {
3662  UDEBUG("");
3663  FILE* foutTree = 0;
3664  #ifdef _MSC_VER
3665  fopen_s(&foutTree, fileNameTree, "w");
3666  #else
3667  foutTree = fopen(fileNameTree, "w");
3668  #endif
3669 
3670  if(foutTree)
3671  {
3672  fprintf(foutTree, "SignatureID Weight NbLoopClosureIds LoopClosureIds... NbChildLoopClosureIds ChildLoopClosureIds...\n");
3673 
3674  for(std::map<int, Signature *>::const_iterator i=_signatures.begin(); i!=_signatures.end(); ++i)
3675  {
3676  fprintf(foutTree, "%d %d", i->first, i->second->getWeight());
3677 
3678  std::map<int, Link> loopIds, childIds;
3679 
3680  for(std::map<int, Link>::const_iterator iter = i->second->getLinks().begin();
3681  iter!=i->second->getLinks().end();
3682  ++iter)
3683  {
3684  if(iter->second.type() != Link::kNeighbor &&
3685  iter->second.type() != Link::kNeighborMerged)
3686  {
3687  if(iter->first < i->first)
3688  {
3689  childIds.insert(*iter);
3690  }
3691  else if(iter->second.from() != iter->second.to())
3692  {
3693  loopIds.insert(*iter);
3694  }
3695  }
3696  }
3697 
3698  fprintf(foutTree, " %d", (int)loopIds.size());
3699  for(std::map<int, Link>::const_iterator j=loopIds.begin(); j!=loopIds.end(); ++j)
3700  {
3701  fprintf(foutTree, " %d", j->first);
3702  }
3703 
3704  fprintf(foutTree, " %d", (int)childIds.size());
3705  for(std::map<int, Link>::const_iterator j=childIds.begin(); j!=childIds.end(); ++j)
3706  {
3707  fprintf(foutTree, " %d", j->first);
3708  }
3709 
3710  fprintf(foutTree, "\n");
3711  }
3712 
3713  fclose(foutTree);
3714  }
3715 
3716 }
3717 
3718 unsigned long Memory::getMemoryUsed() const
3719 {
3720  unsigned long memoryUsage = sizeof(Memory);
3721  memoryUsage += _signatures.size() * (sizeof(int)+sizeof(std::map<int, Signature *>::iterator)) + sizeof(std::map<int, Signature *>);
3722  for(std::map<int, Signature*>::const_iterator iter=_signatures.begin(); iter!=_signatures.end(); ++iter)
3723  {
3724  memoryUsage += iter->second->getMemoryUsed(true);
3725  }
3726  if(_vwd)
3727  {
3728  memoryUsage += _vwd->getMemoryUsed();
3729  }
3730  memoryUsage += _stMem.size() * (sizeof(int)+sizeof(std::set<int>::iterator)) + sizeof(std::set<int>);
3731  memoryUsage += _workingMem.size() * (sizeof(int)+sizeof(double)+sizeof(std::map<int, double>::iterator)) + sizeof(std::map<int, double>);
3732  memoryUsage += _groundTruths.size() * (sizeof(int)+sizeof(Transform)+12*sizeof(float) + sizeof(std::map<int, Transform>::iterator)) + sizeof(std::map<int, Transform>);
3733  memoryUsage += _labels.size() * (sizeof(int)+sizeof(std::string) + sizeof(std::map<int, std::string>::iterator)) + sizeof(std::map<int, std::string>);
3734  for(std::map<int, std::string>::const_iterator iter=_labels.begin(); iter!=_labels.end(); ++iter)
3735  {
3736  memoryUsage+=iter->second.size();
3737  }
3738  memoryUsage += _landmarksIndex.size() * (sizeof(int)+sizeof(std::set<int>) + sizeof(std::map<int, std::set<int> >::iterator)) + sizeof(std::map<int, std::set<int> >);
3739  for(std::map<int, std::set<int> >::const_iterator iter=_landmarksIndex.begin(); iter!=_landmarksIndex.end(); ++iter)
3740  {
3741  memoryUsage+=iter->second.size()*(sizeof(int)+sizeof(std::set<int>::iterator)) + sizeof(std::set<int>);
3742  }
3743  memoryUsage += parameters_.size()*(sizeof(std::string)*2+sizeof(ParametersMap::iterator)) + sizeof(ParametersMap);
3744  memoryUsage += sizeof(Feature2D) + _feature2D->getParameters().size()*(sizeof(std::string)*2+sizeof(ParametersMap::iterator)) + sizeof(ParametersMap);
3745  memoryUsage += sizeof(Registration);
3746  memoryUsage += sizeof(RegistrationIcp);
3747  memoryUsage += sizeof(LocalGridMaker);
3748  memoryUsage += sizeof(MarkerDetector);
3749  memoryUsage += sizeof(DBDriver);
3750 
3751  return memoryUsage;
3752 }
3753 
3754 void Memory::rehearsal(Signature * signature, Statistics * stats)
3755 {
3756  UTimer timer;
3757  if(signature->isBadSignature())
3758  {
3759  return;
3760  }
3761 
3762  //============================================================
3763  // Compare with the last (not intermediate node)
3764  //============================================================
3765  Signature * sB = 0;
3766  for(std::set<int>::reverse_iterator iter=_stMem.rbegin(); iter!=_stMem.rend(); ++iter)
3767  {
3768  Signature * s = this->_getSignature(*iter);
3769  UASSERT(s!=0);
3770  if(s->getWeight() >= 0 && s->id() != signature->id())
3771  {
3772  sB = s;
3773  break;
3774  }
3775  }
3776  if(sB)
3777  {
3778  int id = sB->id();
3779  UDEBUG("Comparing with signature (%d)...", id);
3780 
3781  float sim = signature->compareTo(*sB);
3782 
3783  int merged = 0;
3784  if(sim >= _similarityThreshold)
3785  {
3786  if(_incrementalMemory)
3787  {
3788  if(this->rehearsalMerge(id, signature->id()))
3789  {
3790  merged = id;
3791  }
3792  }
3793  else
3794  {
3795  signature->setWeight(signature->getWeight() + 1 + sB->getWeight());
3796  }
3797  }
3798 
3799  if(stats) stats->addStatistic(Statistics::kMemoryRehearsal_merged(), merged);
3800  if(stats) stats->addStatistic(Statistics::kMemoryRehearsal_sim(), sim);
3801  if(stats) stats->addStatistic(Statistics::kMemoryRehearsal_id(), sim >= _similarityThreshold?id:0);
3802  UDEBUG("merged=%d, sim=%f t=%fs", merged, sim, timer.ticks());
3803  }
3804  else
3805  {
3806  if(stats) stats->addStatistic(Statistics::kMemoryRehearsal_merged(), 0);
3807  if(stats) stats->addStatistic(Statistics::kMemoryRehearsal_sim(), 0);
3808  }
3809 }
3810 
3811 bool Memory::rehearsalMerge(int oldId, int newId)
3812 {
3813  ULOGGER_INFO("old=%d, new=%d", oldId, newId);
3814  Signature * oldS = _getSignature(oldId);
3815  Signature * newS = _getSignature(newId);
3816  if(oldS && newS && _incrementalMemory)
3817  {
3818  UASSERT_MSG(oldS->getWeight() >= 0 && newS->getWeight() >= 0, uFormat("%d %d", oldS->getWeight(), newS->getWeight()).c_str());
3819  std::map<int, Link>::const_iterator iter = oldS->getLinks().find(newS->id());
3820  if(iter != oldS->getLinks().end() &&
3821  iter->second.type() != Link::kNeighbor &&
3822  iter->second.type() != Link::kNeighborMerged &&
3823  iter->second.from() != iter->second.to())
3824  {
3825  // do nothing, already merged
3826  UWARN("already merged, old=%d, new=%d", oldId, newId);
3827  return false;
3828  }
3829  UASSERT(!newS->isSaved());
3830 
3831  UINFO("Rehearsal merging %d (w=%d) and %d (w=%d)",
3832  oldS->id(), oldS->getWeight(),
3833  newS->id(), newS->getWeight());
3834 
3835  bool fullMerge;
3836  bool intermediateMerge = false;
3837  if(!newS->getLinks().empty() && !newS->getLinks().begin()->second.transform().isNull())
3838  {
3839  // we are in metric SLAM mode:
3840  // 1) Normal merge if not moving AND has direct link
3841  // 2) Transform to intermediate node (weight = -1) if not moving AND hasn't direct link.
3842  float x,y,z, roll,pitch,yaw;
3843  newS->getLinks().begin()->second.transform().getTranslationAndEulerAngles(x,y,z, roll,pitch,yaw);
3844  bool isMoving = fabs(x) > _rehearsalMaxDistance ||
3845  fabs(y) > _rehearsalMaxDistance ||
3846  fabs(z) > _rehearsalMaxDistance ||
3847  fabs(roll) > _rehearsalMaxAngle ||
3848  fabs(pitch) > _rehearsalMaxAngle ||
3849  fabs(yaw) > _rehearsalMaxAngle;
3850  if(isMoving && _rehearsalWeightIgnoredWhileMoving)
3851  {
3852  UINFO("Rehearsal ignored because the robot has moved more than %f m or %f rad (\"Mem/RehearsalWeightIgnoredWhileMoving\"=true)",
3853  _rehearsalMaxDistance, _rehearsalMaxAngle);
3854  return false;
3855  }
3856  fullMerge = !isMoving && newS->hasLink(oldS->id());
3857  intermediateMerge = !isMoving && !newS->hasLink(oldS->id());
3858  }
3859  else
3860  {
3861  fullMerge = newS->hasLink(oldS->id()) && newS->getLinks().begin()->second.transform().isNull();
3862  }
3863  UDEBUG("fullMerge=%s intermediateMerge=%s _idUpdatedToNewOneRehearsal=%s",
3864  fullMerge?"true":"false",
3865  intermediateMerge?"true":"false",
3866  _idUpdatedToNewOneRehearsal?"true":"false");
3867 
3868  if(fullMerge)
3869  {
3870  //remove mutual links
3871  Link newToOldLink = newS->getLinks().find(oldS->id())->second;
3872  oldS->removeLink(newId);
3873  newS->removeLink(oldId);
3874 
3875  if(_idUpdatedToNewOneRehearsal)
3876  {
3877  // redirect neighbor links
3878  const std::multimap<int, Link> & links = oldS->getLinks();
3879  for(std::multimap<int, Link>::const_iterator iter = links.begin(); iter!=links.end(); ++iter)
3880  {
3881  if(iter->second.from() != iter->second.to())
3882  {
3883  Link link = iter->second;
3884  Link mergedLink = newToOldLink.merge(link, link.type());
3885  UASSERT(mergedLink.from() == newS->id() && mergedLink.to() == link.to());
3886 
3887  Signature * s = this->_getSignature(link.to());
3888  if(s)
3889  {
3890  // modify neighbor "from"
3891  s->removeLink(oldS->id());
3892  s->addLink(mergedLink.inverse());
3893 
3894  newS->addLink(mergedLink);
3895  }
3896  else
3897  {
3898  UERROR("Didn't find neighbor %d of %d in RAM...", link.to(), oldS->id());
3899  }
3900  }
3901  }
3902  newS->setLabel(oldS->getLabel());
3903  oldS->setLabel("");
3904  oldS->removeLinks(); // remove all links
3905  oldS->addLink(Link(oldS->id(), newS->id(), Link::kGlobalClosure, Transform(), cv::Mat::eye(6,6,CV_64FC1))); // to keep track of the merged location
3906 
3907  // Set old image to new signature
3908  this->copyData(oldS, newS);
3909 
3910  // update weight
3911  newS->setWeight(newS->getWeight() + 1 + oldS->getWeight());
3912 
3913  if(_lastGlobalLoopClosureId == oldS->id())
3914  {
3915  _lastGlobalLoopClosureId = newS->id();
3916  }
3917  oldS->setWeight(-9);
3918  }
3919  else
3920  {
3921  newS->addLink(Link(newS->id(), oldS->id(), Link::kGlobalClosure, Transform() , cv::Mat::eye(6,6,CV_64FC1))); // to keep track of the merged location
3922 
3923  // update weight
3924  oldS->setWeight(newS->getWeight() + 1 + oldS->getWeight());
3925 
3926  if(_lastSignature == newS)
3927  {
3928  _lastSignature = oldS;
3929  }
3930  newS->setWeight(-9);
3931  }
3932  UDEBUG("New weights: %d->%d %d->%d", oldS->id(), oldS->getWeight(), newS->id(), oldS->getWeight());
3933 
3934  // remove location
3935  moveToTrash(_idUpdatedToNewOneRehearsal?oldS:newS, _notLinkedNodesKeptInDb);
3936 
3937  return true;
3938  }
3939  else
3940  {
3941  // update only weights
3942  if(_idUpdatedToNewOneRehearsal)
3943  {
3944  // just update weight
3945  int w = oldS->getWeight()>=0?oldS->getWeight():0;
3946  newS->setWeight(w + newS->getWeight() + 1);
3947  oldS->setWeight(intermediateMerge?-1:0); // convert to intermediate node
3948 
3949  if(_lastGlobalLoopClosureId == oldS->id())
3950  {
3951  _lastGlobalLoopClosureId = newS->id();
3952  }
3953  }
3954  else // !_idUpdatedToNewOneRehearsal
3955  {
3956  int w = newS->getWeight()>=0?newS->getWeight():0;
3957  oldS->setWeight(w + oldS->getWeight() + 1);
3958  newS->setWeight(intermediateMerge?-1:0); // convert to intermediate node
3959  }
3960  }
3961  }
3962  else
3963  {
3964  if(!newS)
3965  {
3966  UERROR("newId=%d, oldId=%d, Signature %d not found in working/st memories", newId, oldId, newId);
3967  }
3968  if(!oldS)
3969  {
3970  UERROR("newId=%d, oldId=%d, Signature %d not found in working/st memories", newId, oldId, oldId);
3971  }
3972  }
3973  return false;
3974 }
3975 
3976 int Memory::getMapId(int signatureId, bool lookInDatabase) const
3977 {
3978  Transform pose, groundTruth;
3979  int mapId = -1, weight;
3980  std::string label;
3981  double stamp;
3982  std::vector<float> velocity;
3983  GPS gps;
3984  EnvSensors sensors;
3985  getNodeInfo(signatureId, pose, mapId, weight, label, stamp, groundTruth, velocity, gps, sensors, lookInDatabase);
3986  return mapId;
3987 }
3988 
3989 Transform Memory::getOdomPose(int signatureId, bool lookInDatabase) const
3990 {
3991  Transform pose, groundTruth;
3992  int mapId, weight;
3993  std::string label;
3994  double stamp;
3995  std::vector<float> velocity;
3996  GPS gps;
3997  EnvSensors sensors;
3998  getNodeInfo(signatureId, pose, mapId, weight, label, stamp, groundTruth, velocity, gps, sensors, lookInDatabase);
3999  return pose;
4000 }
4001 
4002 Transform Memory::getGroundTruthPose(int signatureId, bool lookInDatabase) const
4003 {
4004  Transform pose, groundTruth;
4005  int mapId, weight;
4006  std::string label;
4007  double stamp;
4008  std::vector<float> velocity;
4009  GPS gps;
4010  EnvSensors sensors;
4011  getNodeInfo(signatureId, pose, mapId, weight, label, stamp, groundTruth, velocity, gps, sensors, lookInDatabase);
4012  return groundTruth;
4013 }
4014 
4015 void Memory::getGPS(int id, GPS & gps, Transform & offsetENU, bool lookInDatabase, int maxGraphDepth) const
4016 {
4017  gps = GPS();
4018  offsetENU=Transform::getIdentity();
4019 
4020  Transform odomPose, groundTruth;
4021  int mapId, weight;
4022  std::string label;
4023  double stamp;
4024  std::vector<float> velocity;
4025  EnvSensors sensors;
4026  getNodeInfo(id, odomPose, mapId, weight, label, stamp, groundTruth, velocity, gps, sensors, lookInDatabase);
4027 
4028  if(gps.stamp() == 0.0)
4029  {
4030  // Search for the nearest node with GPS, and compute its relative transform in ENU coordinates
4031 
4032  std::map<int, int> nearestIds;
4033  nearestIds = getNeighborsId(id, maxGraphDepth, lookInDatabase?-1:0, true, false, true);
4034  std::multimap<int, int> nearestIdsSorted;
4035  for(std::map<int, int>::iterator iter=nearestIds.begin(); iter!=nearestIds.end(); ++iter)
4036  {
4037  nearestIdsSorted.insert(std::make_pair(iter->second, iter->first));
4038  }
4039 
4040  for(std::map<int, int>::iterator iter=nearestIdsSorted.begin(); iter!=nearestIdsSorted.end(); ++iter)
4041  {
4042  const Signature * s = getSignature(iter->second);
4043  UASSERT(s!=0);
4044  if(s->sensorData().gps().stamp() > 0.0)
4045  {
4046  std::list<std::pair<int, Transform> > path = graph::computePath(s->id(), id, this, lookInDatabase);
4047  if(path.size() >= 2)
4048  {
4049  gps = s->sensorData().gps();
4050  Transform localToENU(0,0,(float)((-(gps.bearing()-90))*M_PI/180.0) - s->getPose().theta());
4051  offsetENU = localToENU*(s->getPose().rotation()*path.rbegin()->second);
4052  break;
4053  }
4054  else
4055  {
4056  UWARN("Failed to find path %d -> %d", s->id(), id);
4057  }
4058  }
4059  }
4060  }
4061 }
4062 
4063 bool Memory::getNodeInfo(int signatureId,
4064  Transform & odomPose,
4065  int & mapId,
4066  int & weight,
4067  std::string & label,
4068  double & stamp,
4069  Transform & groundTruth,
4070  std::vector<float> & velocity,
4071  GPS & gps,
4072  EnvSensors & sensors,
4073  bool lookInDatabase) const
4074 {
4075  const Signature * s = this->getSignature(signatureId);
4076  if(s)
4077  {
4078  odomPose = s->getPose().clone();
4079  mapId = s->mapId();
4080  weight = s->getWeight();
4081  label = std::string(s->getLabel());
4082  stamp = s->getStamp();
4083  groundTruth = s->getGroundTruthPose().clone();
4084  velocity = std::vector<float>(s->getVelocity());
4085  gps = GPS(s->sensorData().gps());
4086  sensors = EnvSensors(s->sensorData().envSensors());
4087  return true;
4088  }
4089  else if(lookInDatabase && _dbDriver)
4090  {
4091  return _dbDriver->getNodeInfo(signatureId, odomPose, mapId, weight, label, stamp, groundTruth, velocity, gps, sensors);
4092  }
4093  return false;
4094 }
4095 
4096 cv::Mat Memory::getImageCompressed(int signatureId) const
4097 {
4098  cv::Mat image;
4099  const Signature * s = this->getSignature(signatureId);
4100  if(s)
4101  {
4102  image = s->sensorData().imageCompressed();
4103  }
4104  if(image.empty() && this->isBinDataKept() && _dbDriver)
4105  {
4106  SensorData data;
4107  _dbDriver->getNodeData(signatureId, data);
4108  image = data.imageCompressed();
4109  }
4110  return image;
4111 }
4112 
4113 SensorData Memory::getNodeData(int locationId, bool images, bool scan, bool userData, bool occupancyGrid) const
4114 {
4115  //UDEBUG("");
4116  SensorData r;
4117  const Signature * s = this->getSignature(locationId);
4118  if(s && (!s->isSaved() ||
4119  ((!images || !s->sensorData().imageCompressed().empty()) &&
4120  (!scan || !s->sensorData().laserScanCompressed().isEmpty()) &&
4121  (!userData || !s->sensorData().userDataCompressed().empty()) &&
4122  (!occupancyGrid || s->sensorData().gridCellSize() != 0.0f))))
4123  {
4124  r = s->sensorData();
4125  if(!images)
4126  {
4127  r.setRGBDImage(cv::Mat(), cv::Mat(), std::vector<CameraModel>());
4128  }
4129  if(!scan)
4130  {
4131  r.setLaserScan(LaserScan());
4132  }
4133  if(!userData)
4134  {
4135  r.setUserData(cv::Mat());
4136  }
4137  if(!occupancyGrid)
4138  {
4139  r.setOccupancyGrid(cv::Mat(), cv::Mat(), cv::Mat(), 0, cv::Point3f());
4140  }
4141  }
4142  else if(_dbDriver)
4143  {
4144  // load from database
4145  _dbDriver->getNodeData(locationId, r, images, scan, userData, occupancyGrid);
4146  }
4147 
4148  return r;
4149 }
4150 
4151 void Memory::getNodeWordsAndGlobalDescriptors(int nodeId,
4152  std::multimap<int, int> & words,
4153  std::vector<cv::KeyPoint> & wordsKpts,
4154  std::vector<cv::Point3f> & words3,
4155  cv::Mat & wordsDescriptors,
4156  std::vector<GlobalDescriptor> & globalDescriptors) const
4157 {
4158  //UDEBUG("nodeId=%d", nodeId);
4159  Signature * s = this->_getSignature(nodeId);
4160  if(s)
4161  {
4162  words = s->getWords();
4163  wordsKpts = s->getWordsKpts();
4164  words3 = s->getWords3();
4165  wordsDescriptors = s->getWordsDescriptors();
4166  globalDescriptors = s->sensorData().globalDescriptors();
4167  }
4168  else if(_dbDriver)
4169  {
4170  // load from database
4171  std::list<Signature*> signatures;
4172  std::list<int> ids;
4173  ids.push_back(nodeId);
4174  std::set<int> loadedFromTrash;
4175  _dbDriver->loadSignatures(ids, signatures, &loadedFromTrash);
4176  if(signatures.size())
4177  {
4178  words = signatures.front()->getWords();
4179  wordsKpts = signatures.front()->getWordsKpts();
4180  words3 = signatures.front()->getWords3();
4181  wordsDescriptors = signatures.front()->getWordsDescriptors();
4182  globalDescriptors = signatures.front()->sensorData().globalDescriptors();
4183  if(loadedFromTrash.size())
4184  {
4185  //put back
4186  _dbDriver->asyncSave(signatures.front());
4187  }
4188  else
4189  {
4190  delete signatures.front();
4191  }
4192  }
4193  }
4194 }
4195 
4196 void Memory::getNodeCalibration(int nodeId,
4197  std::vector<CameraModel> & models,
4198  std::vector<StereoCameraModel> & stereoModels) const
4199 {
4200  //UDEBUG("nodeId=%d", nodeId);
4201  Signature * s = this->_getSignature(nodeId);
4202  if(s)
4203  {
4204  models = s->sensorData().cameraModels();
4205  stereoModels = s->sensorData().stereoCameraModels();
4206  }
4207  else if(_dbDriver)
4208  {
4209  // load from database
4210  _dbDriver->getCalibration(nodeId, models, stereoModels);
4211  }
4212 }
4213 
4214 void Memory::generateGraph(const std::string & fileName, const std::set<int> & ids)
4215 {
4216  if(!_dbDriver)
4217  {
4218  UERROR("A database must must loaded first...");
4219  return;
4220  }
4221 
4222  _dbDriver->generateGraph(fileName, ids, _signatures);
4223 }
4224 
4225 int Memory::cleanupLocalGrids(
4226  const std::map<int, Transform> & poses,
4227  const cv::Mat & map,
4228  float xMin,
4229  float yMin,
4230  float cellSize,
4231  int cropRadius,
4232  bool filterScans)
4233 {
4234  if(!_dbDriver)
4235  {
4236  UERROR("A database must be loaded first...");
4237  return -1;
4238  }
4239 
4240  if(poses.empty() || poses.lower_bound(1) == poses.end())
4241  {
4242  UERROR("Empty poses?!");
4243  return -1;
4244  }
4245  if(map.empty())
4246  {
4247  UERROR("Map is empty!");
4248  return -1;
4249  }
4250  UASSERT(cropRadius>=0);
4251  UASSERT(cellSize>0.0f);
4252 
4253  int maxPoses = 0;
4254  for(std::map<int, Transform>::const_iterator iter=poses.lower_bound(1); iter!=poses.end(); ++iter)
4255  {
4256  ++maxPoses;
4257  }
4258 
4259  UINFO("Processing %d grids...", maxPoses);
4260  int processedGrids = 1;
4261  int gridsScansModified = 0;
4262  for(std::map<int, Transform>::const_iterator iter=poses.lower_bound(1); iter!=poses.end(); ++iter, ++processedGrids)
4263  {
4264  // local grid
4265  cv::Mat gridGround;
4266  cv::Mat gridObstacles;
4267  cv::Mat gridEmpty;
4268 
4269  // scan
4270  SensorData data = this->getNodeData(iter->first, false, true, false, true);
4271  LaserScan scan;
4272  data.uncompressData(0,0,&scan,0,&gridGround,&gridObstacles,&gridEmpty);
4273 
4274  if(!gridObstacles.empty())
4275  {
4276  UASSERT(data.gridCellSize() == cellSize);
4277  cv::Mat filtered = cv::Mat(1, gridObstacles.cols, gridObstacles.type());
4278  int oi = 0;
4279  for(int i=0; i<gridObstacles.cols; ++i)
4280  {
4281  const float * ptr = gridObstacles.ptr<float>(0, i);
4282  cv::Point3f pt(ptr[0], ptr[1], gridObstacles.channels()==2?0:ptr[2]);
4283  pt = util3d::transformPoint(pt, iter->second);
4284 
4285  int x = int((pt.x - xMin) / cellSize + 0.5f);
4286  int y = int((pt.y - yMin) / cellSize + 0.5f);
4287 
4288  if(x>=0 && x<map.cols &&
4289  y>=0 && y<map.rows)
4290  {
4291  bool obstacleDetected = false;
4292 
4293  for(int j=-cropRadius; j<=cropRadius && !obstacleDetected; ++j)
4294  {
4295  for(int k=-cropRadius; k<=cropRadius && !obstacleDetected; ++k)
4296  {
4297  if(x+j>=0 && x+j<map.cols &&
4298  y+k>=0 && y+k<map.rows &&
4299  map.at<unsigned char>(y+k,x+j) == 100)
4300  {
4301  obstacleDetected = true;
4302  }
4303  }
4304  }
4305 
4306  if(map.at<unsigned char>(y,x) != 0 || obstacleDetected)
4307  {
4308  // Verify that we don't have an obstacle on neighbor cells
4309  cv::Mat(gridObstacles, cv::Range::all(), cv::Range(i,i+1)).copyTo(cv::Mat(filtered, cv::Range::all(), cv::Range(oi,oi+1)));
4310  ++oi;
4311  }
4312  }
4313  }
4314 
4315  if(oi != gridObstacles.cols)
4316  {
4317  UINFO("Grid id=%d (%d/%d) filtered %d -> %d", iter->first, processedGrids, maxPoses, gridObstacles.cols, oi);
4318  gridsScansModified += 1;
4319 
4320  // update
4321  Signature * s = this->_getSignature(iter->first);
4322  cv::Mat newObstacles = cv::Mat(filtered, cv::Range::all(), cv::Range(0, oi));
4323  bool modifyDb = true;
4324  if(s)
4325  {
4326  s->sensorData().setOccupancyGrid(gridGround, newObstacles, gridEmpty, cellSize, data.gridViewPoint());
4327  if(!s->isSaved())
4328  {
4329  // not saved in database yet
4330  modifyDb = false;
4331  }
4332  }
4333  if(modifyDb)
4334  {
4335  _dbDriver->updateOccupancyGrid(iter->first,
4336  gridGround,
4337  newObstacles,
4338  gridEmpty,
4339  cellSize,
4340  data.gridViewPoint());
4341  }
4342  }
4343  }
4344 
4345  if(filterScans && !scan.isEmpty())
4346  {
4347  Transform mapToScan = iter->second * scan.localTransform();
4348 
4349  cv::Mat filtered = cv::Mat(1, scan.size(), scan.dataType());
4350  int oi = 0;
4351  for(int i=0; i<scan.size(); ++i)
4352  {
4353  const float * ptr = scan.data().ptr<float>(0, i);
4354  cv::Point3f pt(ptr[0], ptr[1], scan.is2d()?0:ptr[2]);
4355  pt = util3d::transformPoint(pt, mapToScan);
4356 
4357  int x = int((pt.x - xMin) / cellSize + 0.5f);
4358  int y = int((pt.y - yMin) / cellSize + 0.5f);
4359 
4360  if(x>=0 && x<map.cols &&
4361  y>=0 && y<map.rows)
4362  {
4363  bool obstacleDetected = false;
4364 
4365  for(int j=-cropRadius; j<=cropRadius && !obstacleDetected; ++j)
4366  {
4367  for(int k=-cropRadius; k<=cropRadius && !obstacleDetected; ++k)
4368  {
4369  if(x+j>=0 && x+j<map.cols &&
4370  y+k>=0 && y+k<map.rows &&
4371  map.at<unsigned char>(y+k,x+j) == 100)
4372  {
4373  obstacleDetected = true;
4374  }
4375  }
4376  }
4377 
4378  if(map.at<unsigned char>(y,x) != 0 || obstacleDetected)
4379  {
4380  // Verify that we don't have an obstacle on neighbor cells
4381  cv::Mat(scan.data(), cv::Range::all(), cv::Range(i,i+1)).copyTo(cv::Mat(filtered, cv::Range::all(), cv::Range(oi,oi+1)));
4382  ++oi;
4383  }
4384  }
4385  }
4386 
4387  if(oi != scan.size())
4388  {
4389  UINFO("Scan id=%d (%d/%d) filtered %d -> %d", iter->first, processedGrids, maxPoses, (int)scan.size(), oi);
4390  gridsScansModified += 1;
4391 
4392  // update
4393  if(scan.angleIncrement()!=0)
4394  {
4395  // copy meta data
4396  scan = LaserScan(
4397  cv::Mat(filtered, cv::Range::all(), cv::Range(0, oi)),
4398  scan.format(),
4399  scan.rangeMin(),
4400  scan.rangeMax(),
4401  scan.angleMin(),
4402  scan.angleMax(),
4403  scan.angleIncrement(),
4404  scan.localTransform());
4405  }
4406  else
4407  {
4408  // copy meta data
4409  scan = LaserScan(
4410  cv::Mat(filtered, cv::Range::all(), cv::Range(0, oi)),
4411  scan.maxPoints(),
4412  scan.rangeMax(),
4413  scan.format(),
4414  scan.localTransform());
4415  }
4416 
4417  // update
4418  Signature * s = this->_getSignature(iter->first);
4419  bool modifyDb = true;
4420  if(s)
4421  {
4422  s->sensorData().setLaserScan(scan, true);
4423  if(!s->isSaved())
4424  {
4425  // not saved in database yet
4426  modifyDb = false;
4427  }
4428  }
4429  if(modifyDb)
4430  {
4431  _dbDriver->updateLaserScan(iter->first, scan);
4432  }
4433 
4434  }
4435  }
4436  }
4437  return gridsScansModified;
4438 }
4439 
4440 int Memory::getNi(int signatureId) const
4441 {
4442  int ni = 0;
4443  const Signature * s = this->getSignature(signatureId);
4444  if(s)
4445  {
4446  ni = (int)((Signature *)s)->getWords().size();
4447  }
4448  else
4449  {
4450  _dbDriver->getInvertedIndexNi(signatureId, ni);
4451  }
4452  return ni;
4453 }
4454 
4455 
4456 void Memory::copyData(const Signature * from, Signature * to)
4457 {
4458  UTimer timer;
4459  timer.start();
4460  if(from && to)
4461  {
4462  // words 2d
4463  this->disableWordsRef(to->id());
4464  to->setWords(from->getWords(), from->getWordsKpts(), from->getWords3(), from->getWordsDescriptors());
4465  std::list<int> id;
4466  id.push_back(to->id());
4467  this->enableWordsRef(id);
4468 
4469  if(from->isSaved() && _dbDriver)
4470  {
4471  _dbDriver->getNodeData(from->id(), to->sensorData());
4472  UDEBUG("Loaded image data from database");
4473  }
4474  else
4475  {
4476  to->sensorData() = (SensorData)from->sensorData();
4477  }
4478  to->sensorData().setId(to->id());
4479 
4480  to->setPose(from->getPose());
4481  }
4482  else
4483  {
4484  ULOGGER_ERROR("Can't merge the signatures because there are not same type.");
4485  }
4486  UDEBUG("Merging time = %fs", timer.ticks());
4487 }
4488 
4489 class PreUpdateThread : public UThreadNode
4490 {
4491 public:
4492  PreUpdateThread(VWDictionary * vwp) : _vwp(vwp) {}
4493  virtual ~PreUpdateThread() {}
4494 private:
4495  void mainLoop() {
4496  if(_vwp)
4497  {
4498  _vwp->update();
4499  }
4500  this->kill();
4501  }
4502  VWDictionary * _vwp;
4503 };
4504 
4505 Signature * Memory::createSignature(const SensorData & inputData, const Transform & pose, Statistics * stats)
4506 {
4507  UDEBUG("");
4508  SensorData data = inputData;
4509  UASSERT(data.imageRaw().empty() ||
4510  data.imageRaw().type() == CV_8UC1 ||
4511  data.imageRaw().type() == CV_8UC3);
4512  UASSERT_MSG(data.depthOrRightRaw().empty() ||
4513  ( ( data.depthOrRightRaw().type() == CV_16UC1 ||
4514  data.depthOrRightRaw().type() == CV_32FC1 ||
4515  data.depthOrRightRaw().type() == CV_8UC1)
4516  &&
4517  ( (data.imageRaw().empty() && data.depthOrRightRaw().type() != CV_8UC1) ||
4518  (data.depthOrRightRaw().rows <= data.imageRaw().rows && data.depthOrRightRaw().cols <= data.imageRaw().cols))),
4519  uFormat("image=(%d/%d, type=%d, [accepted=%d,%d]) depth=(%d/%d, type=%d [accepted=%d(depth mm),%d(depth m),%d(stereo)]). "
4520  "For stereo, left and right images should be same size. "
4521  "For RGB-D, depth can be X times smaller than RGB (where X is an integer).",
4522  data.imageRaw().cols,
4523  data.imageRaw().rows,
4524  data.imageRaw().type(),
4525  CV_8UC1,
4526  CV_8UC3,
4527  data.depthOrRightRaw().cols,
4528  data.depthOrRightRaw().rows,
4529  data.depthOrRightRaw().type(),
4530  CV_16UC1, CV_32FC1, CV_8UC1).c_str());
4531 
4532  if(!data.depthOrRightRaw().empty() &&
4533  data.cameraModels().empty() &&
4534  data.stereoCameraModels().empty() &&
4535  !pose.isNull())
4536  {
4537  UERROR("No camera calibration found, calibrate your camera!");
4538  return 0;
4539  }
4540  UASSERT(_feature2D != 0);
4541 
4542  PreUpdateThread preUpdateThread(_vwd);
4543 
4544  UTimer timer;
4545  timer.start();
4546  float t;
4547  std::vector<cv::KeyPoint> keypoints;
4548  cv::Mat descriptors;
4549  bool isIntermediateNode = data.id() < 0 || (data.imageRaw().empty() && data.keypoints().empty() && data.laserScanRaw().empty());
4550  int id = data.id();
4551  if(_generateIds)
4552  {
4553  id = this->getNextId();
4554  }
4555  else
4556  {
4557  if(id <= 0)
4558  {
4559  UERROR("Received image ID is null. "
4560  "Please set parameter Mem/GenerateIds to \"true\" or "
4561  "make sure the input source provides image ids (seq).");
4562  return 0;
4563  }
4564  else if(id > _idCount)
4565  {
4566  _idCount = id;
4567  }
4568  else
4569  {
4570  UERROR("Id of acquired image (%d) is smaller than the last in memory (%d). "
4571  "Please set parameter Mem/GenerateIds to \"true\" or "
4572  "make sure the input source provides image ids (seq) over the last in "
4573  "memory, which is %d.",
4574  id,
4575  _idCount,
4576  _idCount);
4577  return 0;
4578  }
4579  }
4580 
4581  bool imagesRectified = _imagesAlreadyRectified;
4582  // Stereo must be always rectified because of the stereo correspondence approach
4583  if(!imagesRectified && !data.imageRaw().empty() && !(_rectifyOnlyFeatures && data.rightRaw().empty()))
4584  {
4585  // we assume that once rtabmap is receiving data, the calibration won't change over time
4586  if(data.cameraModels().size())
4587  {
4588  UDEBUG("Monocular rectification");
4589  // Note that only RGB image is rectified, the depth image is assumed to be already registered to rectified RGB camera.
4590  UASSERT(int((data.imageRaw().cols/data.cameraModels().size())*data.cameraModels().size()) == data.imageRaw().cols);
4591  int subImageWidth = data.imageRaw().cols/data.cameraModels().size();
4592  cv::Mat rectifiedImages(data.imageRaw().size(), data.imageRaw().type());
4593  bool initRectMaps = _rectCameraModels.empty();
4594  if(initRectMaps)
4595  {
4596  _rectCameraModels.resize(data.cameraModels().size());
4597  }
4598  for(unsigned int i=0; i<data.cameraModels().size(); ++i)
4599  {
4600  if(data.cameraModels()[i].isValidForRectification())
4601  {
4602  if(initRectMaps)
4603  {
4604  _rectCameraModels[i] = data.cameraModels()[i];
4605  if(!_rectCameraModels[i].isRectificationMapInitialized())
4606  {
4607  UWARN("Initializing rectification maps for camera %d (only done for the first image received)...", i);
4608  _rectCameraModels[i].initRectificationMap();
4609  UWARN("Initializing rectification maps for camera %d (only done for the first image received)... done!", i);
4610  }
4611  }
4612  UASSERT(_rectCameraModels[i].imageWidth() == data.cameraModels()[i].imageWidth() &&
4613  _rectCameraModels[i].imageHeight() == data.cameraModels()[i].imageHeight());
4614  cv::Mat rectifiedImage = _rectCameraModels[i].rectifyImage(cv::Mat(data.imageRaw(), cv::Rect(subImageWidth*i, 0, subImageWidth, data.imageRaw().rows)));
4615  rectifiedImage.copyTo(cv::Mat(rectifiedImages, cv::Rect(subImageWidth*i, 0, subImageWidth, data.imageRaw().rows)));
4616  imagesRectified = true;
4617  }
4618  else
4619  {
4620  UERROR("Calibration for camera %d cannot be used to rectify the image. Make sure to do a "
4621  "full calibration. If images are already rectified, set %s parameter back to true.",
4622  (int)i,
4623  Parameters::kRtabmapImagesAlreadyRectified().c_str());
4624  std::cout << data.cameraModels()[i] << std::endl;
4625  return 0;
4626  }
4627  }
4628  data.setRGBDImage(rectifiedImages, data.depthOrRightRaw(), data.cameraModels());
4629  }
4630  else if(data.stereoCameraModels().size())
4631  {
4632  UDEBUG("Stereo rectification");
4633  UASSERT(int((data.imageRaw().cols/data.stereoCameraModels().size())*data.stereoCameraModels().size()) == data.imageRaw().cols);
4634  int subImageWidth = data.imageRaw().cols/data.stereoCameraModels().size();
4635  UASSERT(subImageWidth == data.rightRaw().cols/(int)data.stereoCameraModels().size());
4636  cv::Mat rectifiedLefts(data.imageRaw().size(), data.imageRaw().type());
4637  cv::Mat rectifiedRights(data.rightRaw().size(), data.rightRaw().type());
4638  bool initRectMaps = _rectStereoCameraModels.empty();
4639  if(initRectMaps)
4640  {
4641  _rectStereoCameraModels.resize(data.stereoCameraModels().size());
4642  }
4643 
4644  for(unsigned int i=0; i<data.stereoCameraModels().size(); ++i)
4645  {
4646  if(data.stereoCameraModels()[i].isValidForRectification())
4647  {
4648  if(initRectMaps)
4649  {
4650  _rectStereoCameraModels[i] = data.stereoCameraModels()[i];
4651  if(!_rectStereoCameraModels[i].isRectificationMapInitialized())
4652  {
4653  UWARN("Initializing rectification maps (only done for the first image received)...");
4654  _rectStereoCameraModels[i].initRectificationMap();
4655  UWARN("Initializing rectification maps (only done for the first image received)...done!");
4656  }
4657  }
4658  UASSERT(_rectStereoCameraModels[i].left().imageWidth() == data.stereoCameraModels()[i].left().imageWidth());
4659  UASSERT(_rectStereoCameraModels[i].left().imageHeight() == data.stereoCameraModels()[i].left().imageHeight());
4660 
4661  cv::Mat rectifiedLeft = _rectStereoCameraModels[i].left().rectifyImage(cv::Mat(data.imageRaw(), cv::Rect(subImageWidth*i, 0, subImageWidth, data.imageRaw().rows)));
4662  cv::Mat rectifiedRight = _rectStereoCameraModels[i].right().rectifyImage(cv::Mat(data.rightRaw(), cv::Rect(subImageWidth*i, 0, subImageWidth, data.rightRaw().rows)));
4663  rectifiedLeft.copyTo(cv::Mat(rectifiedLefts, cv::Rect(subImageWidth*i, 0, subImageWidth, data.imageRaw().rows)));
4664  rectifiedRight.copyTo(cv::Mat(rectifiedRights, cv::Rect(subImageWidth*i, 0, subImageWidth, data.rightRaw().rows)));
4665  imagesRectified = true;
4666  }
4667  else
4668  {
4669  UERROR("Calibration for camera %d cannot be used to rectify the image. Make sure to do a "
4670  "full calibration. If images are already rectified, set %s parameter back to true.",
4671  (int)i,
4672  Parameters::kRtabmapImagesAlreadyRectified().c_str());
4673  std::cout << data.stereoCameraModels()[i] << std::endl;
4674  return 0;
4675  }
4676  }
4677 
4678  data.setStereoImage(
4679  rectifiedLefts,
4680  rectifiedRights,
4681  data.stereoCameraModels());
4682  }
4683  else
4684  {
4685  UERROR("Stereo calibration cannot be used to rectify images. Make sure to do a "
4686  "full stereo calibration. If images are already rectified, set %s parameter back to true.",
4687  Parameters::kRtabmapImagesAlreadyRectified().c_str());
4688  return 0;
4689  }
4690  t = timer.ticks();
4691  if(stats) stats->addStatistic(Statistics::kTimingMemRectification(), t*1000.0f);
4692  UDEBUG("time rectification = %fs", t);
4693  }
4694 
4695  int treeSize= int(_workingMem.size() + _stMem.size());
4696  int meanWordsPerLocation = _feature2D->getMaxFeatures()>0?_feature2D->getMaxFeatures():0;
4697  if(treeSize > 1)
4698  {
4699  meanWordsPerLocation = _vwd->getTotalActiveReferences() / (treeSize-1); // ignore virtual signature
4700  }
4701 
4702  if(_parallelized && !isIntermediateNode)
4703  {
4704  UDEBUG("Start dictionary update thread");
4705  preUpdateThread.start();
4706  }
4707 
4708  if(_rotateImagesUpsideUp && !data.imageRaw().empty() && !data.cameraModels().empty())
4709  {
4710  // Currently stereo is not supported
4711  UASSERT(int((data.imageRaw().cols/data.cameraModels().size())*data.cameraModels().size()) == data.imageRaw().cols);
4712  int subInputImageWidth = data.imageRaw().cols/data.cameraModels().size();
4713  int subInputDepthWidth = data.depthRaw().cols/data.cameraModels().size();
4714  int subOutputImageWidth = 0;
4715  int subOutputDepthWidth = 0;
4716  cv::Mat rotatedColorImages;
4717  cv::Mat rotatedDepthImages;
4718  std::vector<CameraModel> rotatedCameraModels;
4719  bool allOutputSizesAreOkay = true;
4720  for(size_t i=0; i<data.cameraModels().size(); ++i)
4721  {
4722  UDEBUG("Rotating camera %ld", i);
4723  cv::Mat rgb = cv::Mat(data.imageRaw(), cv::Rect(subInputImageWidth*i, 0, subInputImageWidth, data.imageRaw().rows));
4724  cv::Mat depth = !data.depthRaw().empty()?cv::Mat(data.depthRaw(), cv::Rect(subInputDepthWidth*i, 0, subInputDepthWidth, data.depthRaw().rows)):cv::Mat();
4725  CameraModel model = data.cameraModels()[i];
4726  util2d::rotateImagesUpsideUpIfNecessary(model, rgb, depth);
4727  if(rotatedColorImages.empty())
4728  {
4729  rotatedColorImages = cv::Mat(cv::Size(rgb.cols * data.cameraModels().size(), rgb.rows), rgb.type());
4730  subOutputImageWidth = rgb.cols;;
4731  if(!depth.empty())
4732  {
4733  rotatedDepthImages = cv::Mat(cv::Size(depth.cols * data.cameraModels().size(), depth.rows), depth.type());
4734  subOutputDepthWidth = depth.cols;
4735  }
4736  }
4737  else if(rgb.cols != subOutputImageWidth || depth.cols != subOutputDepthWidth ||
4738  rgb.rows != rotatedColorImages.rows || depth.rows != rotatedDepthImages.rows)
4739  {
4740  UWARN("Rotated image for camera index %d (rgb=%dx%d depth=%dx%d) doesn't tally "
4741  "with the first camera (rgb=%dx%d, depth=%dx%d). Aborting upside up rotation, "
4742  "will use original image orientation. Set parameter %s to false to avoid "
4743  "this warning.",
4744  i,
4745  rgb.cols, rgb.rows,
4746  depth.cols, depth.rows,
4747  subOutputImageWidth, rotatedColorImages.rows,
4748  subOutputDepthWidth, rotatedDepthImages.rows,
4749  Parameters::kMemRotateImagesUpsideUp().c_str());
4750  allOutputSizesAreOkay = false;
4751  break;
4752  }
4753  rgb.copyTo(cv::Mat(rotatedColorImages, cv::Rect(subOutputImageWidth*i, 0, subOutputImageWidth, rgb.rows)));
4754  if(!depth.empty())
4755  {
4756  depth.copyTo(cv::Mat(rotatedDepthImages, cv::Rect(subOutputDepthWidth*i, 0, subOutputDepthWidth, depth.rows)));
4757  }
4758  rotatedCameraModels.push_back(model);
4759  }
4760  if(allOutputSizesAreOkay)
4761  {
4762  data.setRGBDImage(rotatedColorImages, rotatedDepthImages, rotatedCameraModels);
4763 
4764  // Clear any features to avoid confusion with the rotated cameras.
4765  if(!data.keypoints().empty() || !data.keypoints3D().empty() || !data.descriptors().empty())
4766  {
4767  if(_useOdometryFeatures)
4768  {
4769  static bool warned = false;
4770  if(!warned)
4771  {
4772  UWARN("Because parameter %s is enabled, parameter %s is inhibited as "
4773  "features have to be regenerated. To avoid this warning, set "
4774  "explicitly %s to false. This message is only "
4775  "printed once.",
4776  Parameters::kMemRotateImagesUpsideUp().c_str(),
4777  Parameters::kMemUseOdomFeatures().c_str(),
4778  Parameters::kMemUseOdomFeatures().c_str());
4779  warned = true;
4780  }
4781  }
4782  data.setFeatures(std::vector<cv::KeyPoint>(), std::vector<cv::Point3f>(), cv::Mat());
4783  }
4784  }
4785  }
4786  else if(_rotateImagesUpsideUp)
4787  {
4788  static bool warned = false;
4789  if(!warned)
4790  {
4791  UWARN("Parameter %s can only be used with RGB-only or RGB-D cameras. "
4792  "Ignoring upside up rotation. This message is only printed once.",
4793  Parameters::kMemRotateImagesUpsideUp().c_str());
4794  warned = true;
4795  }
4796  }
4797 
4798  unsigned int preDecimation = 1;
4799  std::vector<cv::Point3f> keypoints3D;
4800  SensorData decimatedData;
4801  UDEBUG("Received kpts=%d kpts3D=%d, descriptors=%d _useOdometryFeatures=%s",
4802  (int)data.keypoints().size(), (int)data.keypoints3D().size(), data.descriptors().rows, _useOdometryFeatures?"true":"false");
4803  if(!_useOdometryFeatures ||
4804  data.keypoints().empty() ||
4805  (int)data.keypoints().size() != data.descriptors().rows ||
4806  (_feature2D->getType() == Feature2D::kFeatureOrbOctree && data.descriptors().empty()))
4807  {
4808  if(_feature2D->getMaxFeatures() >= 0 && !data.imageRaw().empty() && !isIntermediateNode)
4809  {
4810  decimatedData = data;
4811  if(_imagePreDecimation > 1)
4812  {
4813  preDecimation = _imagePreDecimation;
4814  int decimationDepth = _imagePreDecimation;
4815  if( !data.cameraModels().empty() &&
4816  data.cameraModels()[0].imageHeight()>0 &&
4817  data.cameraModels()[0].imageWidth()>0)
4818  {
4819  // decimate from RGB image size
4820  int targetSize = data.cameraModels()[0].imageHeight() / _imagePreDecimation;
4821  if(targetSize >= data.depthRaw().rows)
4822  {
4823  decimationDepth = 1;
4824  }
4825  else
4826  {
4827  decimationDepth = (int)ceil(float(data.depthRaw().rows) / float(targetSize));
4828  }
4829  }
4830  UDEBUG("decimation rgbOrLeft(rows=%d)=%d, depthOrRight(rows=%d)=%d", data.imageRaw().rows, _imagePreDecimation, data.depthOrRightRaw().rows, decimationDepth);
4831 
4832  std::vector<CameraModel> cameraModels = decimatedData.cameraModels();
4833  for(unsigned int i=0; i<cameraModels.size(); ++i)
4834  {
4835  cameraModels[i] = cameraModels[i].scaled(1.0/double(_imagePreDecimation));
4836  }
4837  if(!cameraModels.empty())
4838  {
4839  decimatedData.setRGBDImage(
4840  util2d::decimate(decimatedData.imageRaw(), _imagePreDecimation),
4841  util2d::decimate(decimatedData.depthOrRightRaw(), decimationDepth),
4842  cameraModels);
4843  }
4844 
4845  std::vector<StereoCameraModel> stereoCameraModels = decimatedData.stereoCameraModels();
4846  for(unsigned int i=0; i<stereoCameraModels.size(); ++i)
4847  {
4848  stereoCameraModels[i].scale(1.0/double(_imagePreDecimation));
4849  }
4850  if(!stereoCameraModels.empty())
4851  {
4852  decimatedData.setStereoImage(
4853  util2d::decimate(decimatedData.imageRaw(), _imagePreDecimation),
4854  util2d::decimate(decimatedData.depthOrRightRaw(), _imagePreDecimation),
4855  stereoCameraModels);
4856  }
4857  }
4858 
4859  UINFO("Extract features");
4860  cv::Mat imageMono;
4861  if(decimatedData.imageRaw().channels() == 3)
4862  {
4863  cv::cvtColor(decimatedData.imageRaw(), imageMono, CV_BGR2GRAY);
4864  }
4865  else
4866  {
4867  imageMono = decimatedData.imageRaw();
4868  }
4869 
4870  cv::Mat depthMask;
4871  if(imagesRectified && !decimatedData.depthRaw().empty() && _depthAsMask)
4872  {
4873  if(imageMono.rows % decimatedData.depthRaw().rows == 0 &&
4874  imageMono.cols % decimatedData.depthRaw().cols == 0 &&
4875  imageMono.rows/decimatedData.depthRaw().rows == imageMono.cols/decimatedData.depthRaw().cols)
4876  {
4877  depthMask = util2d::interpolate(decimatedData.depthRaw(), imageMono.rows/decimatedData.depthRaw().rows, 0.1f);
4878  }
4879  else
4880  {
4881  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).",
4882  Parameters::kMemDepthAsMask().c_str(),
4883  imageMono.cols, imageMono.rows,
4884  decimatedData.depthRaw().cols, decimatedData.depthRaw().rows,
4885  Parameters::kMemImagePreDecimation().c_str(), _imagePreDecimation);
4886  }
4887  }
4888 
4889  bool useProvided3dPoints = false;
4890  if(_useOdometryFeatures && !data.keypoints().empty())
4891  {
4892  UDEBUG("Using provided keypoints (%d)", (int)data.keypoints().size());
4893  keypoints = data.keypoints();
4894 
4895  useProvided3dPoints = keypoints.size() == data.keypoints3D().size();
4896 
4897  // A: Adjust keypoint position so that descriptors are correctly extracted
4898  // B: In case we provided corresponding 3D features
4899  if(_imagePreDecimation > 1 || useProvided3dPoints)
4900  {
4901  float decimationRatio = 1.0f / float(_imagePreDecimation);
4902  double log2value = log(double(_imagePreDecimation))/log(2.0);
4903  for(unsigned int i=0; i < keypoints.size(); ++i)
4904  {
4905  cv::KeyPoint & kpt = keypoints[i];
4906  if(_imagePreDecimation > 1)
4907  {
4908  kpt.pt.x *= decimationRatio;
4909  kpt.pt.y *= decimationRatio;
4910  kpt.size *= decimationRatio;
4911  kpt.octave += log2value;
4912  }
4913  if(useProvided3dPoints)
4914  {
4915  keypoints[i].class_id = i;
4916  }
4917  }
4918  }
4919  }
4920  else
4921  {
4922  int oldMaxFeatures = _feature2D->getMaxFeatures();
4923  UDEBUG("rawDescriptorsKept=%d, pose=%d, maxFeatures=%d, visMaxFeatures=%d", _rawDescriptorsKept?1:0, pose.isNull()?0:1, _feature2D->getMaxFeatures(), _visMaxFeatures);
4924  ParametersMap tmpMaxFeatureParameter;
4925  if(_rawDescriptorsKept&&!pose.isNull()&&_feature2D->getMaxFeatures()>0&&_feature2D->getMaxFeatures()<_visMaxFeatures)
4926  {
4927  // The total extracted features should match the number of features used for transformation estimation
4928  UDEBUG("Changing temporary max features from %d to %d", _feature2D->getMaxFeatures(), _visMaxFeatures);
4929  tmpMaxFeatureParameter.insert(ParametersPair(Parameters::kKpMaxFeatures(), uNumber2Str(_visMaxFeatures)));
4930  _feature2D->parseParameters(tmpMaxFeatureParameter);
4931  }
4932 
4933  keypoints = _feature2D->generateKeypoints(
4934  imageMono,
4935  depthMask);
4936 
4937  if(tmpMaxFeatureParameter.size())
4938  {
4939  tmpMaxFeatureParameter.at(Parameters::kKpMaxFeatures()) = uNumber2Str(oldMaxFeatures);
4940  _feature2D->parseParameters(tmpMaxFeatureParameter); // reset back
4941  }
4942  t = timer.ticks();
4943  if(stats) stats->addStatistic(Statistics::kTimingMemKeypoints_detection(), t*1000.0f);
4944  UDEBUG("time keypoints (%d) = %fs", (int)keypoints.size(), t);
4945  }
4946 
4947  descriptors = _feature2D->generateDescriptors(imageMono, keypoints);
4948  t = timer.ticks();
4949  if(stats) stats->addStatistic(Statistics::kTimingMemDescriptors_extraction(), t*1000.0f);
4950  UDEBUG("time descriptors (%d) = %fs", descriptors.rows, t);
4951 
4952  UDEBUG("ratio=%f, meanWordsPerLocation=%d", _badSignRatio, meanWordsPerLocation);
4953  if(descriptors.rows && descriptors.rows < _badSignRatio * float(meanWordsPerLocation))
4954  {
4955  descriptors = cv::Mat();
4956  }
4957  else
4958  {
4959  if(!imagesRectified && decimatedData.cameraModels().size())
4960  {
4961  UASSERT_MSG((int)keypoints.size() == descriptors.rows, uFormat("%d vs %d", (int)keypoints.size(), descriptors.rows).c_str());
4962  std::vector<cv::KeyPoint> keypointsValid;
4963  keypointsValid.reserve(keypoints.size());
4964  cv::Mat descriptorsValid;
4965  descriptorsValid.reserve(descriptors.rows);
4966 
4967  //undistort keypoints before projection (RGB-D)
4968  if(decimatedData.cameraModels().size() == 1)
4969  {
4970  std::vector<cv::Point2f> pointsIn, pointsOut;
4971  cv::KeyPoint::convert(keypoints,pointsIn);
4972  if(decimatedData.cameraModels()[0].D_raw().cols == 6)
4973  {
4974 #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)))
4975  // Equidistant / FishEye
4976  // get only k parameters (k1,k2,p1,p2,k3,k4)
4977  cv::Mat D(1, 4, CV_64FC1);
4978  D.at<double>(0,0) = decimatedData.cameraModels()[0].D_raw().at<double>(0,0);
4979  D.at<double>(0,1) = decimatedData.cameraModels()[0].D_raw().at<double>(0,1);
4980  D.at<double>(0,2) = decimatedData.cameraModels()[0].D_raw().at<double>(0,4);
4981  D.at<double>(0,3) = decimatedData.cameraModels()[0].D_raw().at<double>(0,5);
4982  cv::fisheye::undistortPoints(pointsIn, pointsOut,
4983  decimatedData.cameraModels()[0].K_raw(),
4984  D,
4985  decimatedData.cameraModels()[0].R(),
4986  decimatedData.cameraModels()[0].P());
4987  }
4988  else
4989 #else
4990  UWARN("Too old opencv version (%d,%d,%d) to support fisheye model (min 2.4.10 required)!",
4991  CV_MAJOR_VERSION, CV_MINOR_VERSION, CV_SUBMINOR_VERSION);
4992  }
4993 #endif
4994  {
4995  //RadialTangential
4996  cv::undistortPoints(pointsIn, pointsOut,
4997  decimatedData.cameraModels()[0].K_raw(),
4998  decimatedData.cameraModels()[0].D_raw(),
4999  decimatedData.cameraModels()[0].R(),
5000  decimatedData.cameraModels()[0].P());
5001  }
5002  UASSERT(pointsOut.size() == keypoints.size());
5003  for(unsigned int i=0; i<pointsOut.size(); ++i)
5004  {
5005  if(pointsOut.at(i).x>=0 && pointsOut.at(i).x<decimatedData.cameraModels()[0].imageWidth() &&
5006  pointsOut.at(i).y>=0 && pointsOut.at(i).y<decimatedData.cameraModels()[0].imageHeight())
5007  {
5008  keypointsValid.push_back(keypoints.at(i));
5009  keypointsValid.back().pt.x = pointsOut.at(i).x;
5010  keypointsValid.back().pt.y = pointsOut.at(i).y;
5011  descriptorsValid.push_back(descriptors.row(i));
5012  }
5013  }
5014  }
5015  else
5016  {
5017  UASSERT(int((decimatedData.imageRaw().cols/decimatedData.cameraModels().size())*decimatedData.cameraModels().size()) == decimatedData.imageRaw().cols);
5018  float subImageWidth = decimatedData.imageRaw().cols/decimatedData.cameraModels().size();
5019  for(unsigned int i=0; i<keypoints.size(); ++i)
5020  {
5021  int cameraIndex = int(keypoints.at(i).pt.x / subImageWidth);
5022  UASSERT_MSG(cameraIndex >= 0 && cameraIndex < (int)decimatedData.cameraModels().size(),
5023  uFormat("cameraIndex=%d, models=%d, kpt.x=%f, subImageWidth=%f (Camera model image width=%d)",
5024  cameraIndex, (int)decimatedData.cameraModels().size(), keypoints[i].pt.x, subImageWidth, decimatedData.cameraModels()[0].imageWidth()).c_str());
5025 
5026  std::vector<cv::Point2f> pointsIn, pointsOut;
5027  pointsIn.push_back(cv::Point2f(keypoints.at(i).pt.x-subImageWidth*cameraIndex, keypoints.at(i).pt.y));
5028  if(decimatedData.cameraModels()[cameraIndex].D_raw().cols == 6)
5029  {
5030 #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)))
5031  // Equidistant / FishEye
5032  // get only k parameters (k1,k2,p1,p2,k3,k4)
5033  cv::Mat D(1, 4, CV_64FC1);
5034  D.at<double>(0,0) = decimatedData.cameraModels()[cameraIndex].D_raw().at<double>(0,0);
5035  D.at<double>(0,1) = decimatedData.cameraModels()[cameraIndex].D_raw().at<double>(0,1);
5036  D.at<double>(0,2) = decimatedData.cameraModels()[cameraIndex].D_raw().at<double>(0,4);
5037  D.at<double>(0,3) = decimatedData.cameraModels()[cameraIndex].D_raw().at<double>(0,5);
5038  cv::fisheye::undistortPoints(pointsIn, pointsOut,
5039  decimatedData.cameraModels()[cameraIndex].K_raw(),
5040  D,
5041  decimatedData.cameraModels()[cameraIndex].R(),
5042  decimatedData.cameraModels()[cameraIndex].P());
5043  }
5044  else
5045 #else
5046  UWARN("Too old opencv version (%d,%d,%d) to support fisheye model (min 2.4.10 required)!",
5047  CV_MAJOR_VERSION, CV_MINOR_VERSION, CV_SUBMINOR_VERSION);
5048  }
5049 #endif
5050  {
5051  //RadialTangential
5052  cv::undistortPoints(pointsIn, pointsOut,
5053  decimatedData.cameraModels()[cameraIndex].K_raw(),
5054  decimatedData.cameraModels()[cameraIndex].D_raw(),
5055  decimatedData.cameraModels()[cameraIndex].R(),
5056  decimatedData.cameraModels()[cameraIndex].P());
5057  }
5058 
5059  if(pointsOut[0].x>=0 && pointsOut[0].x<decimatedData.cameraModels()[cameraIndex].imageWidth() &&
5060  pointsOut[0].y>=0 && pointsOut[0].y<decimatedData.cameraModels()[cameraIndex].imageHeight())
5061  {
5062  keypointsValid.push_back(keypoints.at(i));
5063  keypointsValid.back().pt.x = pointsOut[0].x + subImageWidth*cameraIndex;
5064  keypointsValid.back().pt.y = pointsOut[0].y;
5065  descriptorsValid.push_back(descriptors.row(i));
5066  }
5067  }
5068  }
5069 
5070  keypoints = keypointsValid;
5071  descriptors = descriptorsValid;
5072 
5073  t = timer.ticks();
5074  if(stats) stats->addStatistic(Statistics::kTimingMemRectification(), t*1000.0f);
5075  UDEBUG("time rectification = %fs", t);
5076  }
5077 
5078  if(useProvided3dPoints && keypoints.size() != data.keypoints3D().size())
5079  {
5080  UDEBUG("Using provided 3d points (%d->%d)", (int)data.keypoints3D().size(), (int)keypoints.size());
5081  keypoints3D.resize(keypoints.size());
5082  for(size_t i=0; i<keypoints.size(); ++i)
5083  {
5084  UASSERT(keypoints[i].class_id < (int)data.keypoints3D().size());
5085  keypoints3D[i] = data.keypoints3D()[keypoints[i].class_id];
5086  }
5087  }
5088  else if(useProvided3dPoints && keypoints.size() == data.keypoints3D().size())
5089  {
5090  UDEBUG("Using provided 3d points (%d)", (int)data.keypoints3D().size());
5091  keypoints3D = data.keypoints3D();
5092  }
5093  else if((!decimatedData.depthRaw().empty() && decimatedData.cameraModels().size() && decimatedData.cameraModels()[0].isValidForProjection()) ||
5094  (!decimatedData.rightRaw().empty() && decimatedData.stereoCameraModels().size() && decimatedData.stereoCameraModels()[0].isValidForProjection()))
5095  {
5096  keypoints3D = _feature2D->generateKeypoints3D(decimatedData, keypoints);
5097  t = timer.ticks();
5098  if(stats) stats->addStatistic(Statistics::kTimingMemKeypoints_3D(), t*1000.0f);
5099  UDEBUG("time keypoints 3D (%d) = %fs", (int)keypoints3D.size(), t);
5100  }
5101  if(depthMask.empty() && (_feature2D->getMinDepth() > 0.0f || _feature2D->getMaxDepth() > 0.0f))
5102  {
5103  _feature2D->filterKeypointsByDepth(keypoints, descriptors, keypoints3D, _feature2D->getMinDepth(), _feature2D->getMaxDepth());
5104  }
5105  }
5106  }
5107  else if(data.imageRaw().empty())
5108  {
5109  UDEBUG("Empty image, cannot extract features...");
5110  }
5111  else if(_feature2D->getMaxFeatures() < 0)
5112  {
5113  UDEBUG("_feature2D->getMaxFeatures()(%d<0) so don't extract any features...", _feature2D->getMaxFeatures());
5114  }
5115  else
5116  {
5117  UDEBUG("Intermediate node detected, don't extract features!");
5118  }
5119  }
5120  else if(_feature2D->getMaxFeatures() >= 0 && !isIntermediateNode)
5121  {
5122  UINFO("Use odometry features: kpts=%d 3d=%d desc=%d (dim=%d, type=%d)",
5123  (int)data.keypoints().size(),
5124  (int)data.keypoints3D().size(),
5125  data.descriptors().rows,
5126  data.descriptors().cols,
5127  data.descriptors().type());
5128  keypoints = data.keypoints();
5129  keypoints3D = data.keypoints3D();
5130  descriptors = data.descriptors().clone();
5131 
5132  UASSERT(descriptors.empty() || descriptors.rows == (int)keypoints.size());
5133  UASSERT(keypoints3D.empty() || keypoints3D.size() == keypoints.size());
5134 
5135  int maxFeatures = _rawDescriptorsKept&&!pose.isNull()&&_feature2D->getMaxFeatures()>0&&_feature2D->getMaxFeatures()<_visMaxFeatures?_visMaxFeatures:_feature2D->getMaxFeatures();
5136  bool ssc = _rawDescriptorsKept&&!pose.isNull()&&_feature2D->getMaxFeatures()>0&&_feature2D->getMaxFeatures()<_visMaxFeatures?_visSSC:_feature2D->getSSC();
5137  if((int)keypoints.size() > maxFeatures)
5138  {
5139  if(data.cameraModels().size()==1 || data.stereoCameraModels().size()==1)
5140  _feature2D->limitKeypoints(keypoints, keypoints3D, descriptors, maxFeatures, data.cameraModels().size()?data.cameraModels()[0].imageSize():data.stereoCameraModels()[0].left().imageSize(), ssc);
5141  else
5142  _feature2D->limitKeypoints(keypoints, keypoints3D, descriptors, maxFeatures);
5143  }
5144  t = timer.ticks();
5145  if(stats) stats->addStatistic(Statistics::kTimingMemKeypoints_detection(), t*1000.0f);
5146  UDEBUG("time keypoints (%d) = %fs", (int)keypoints.size(), t);
5147 
5148  if(descriptors.empty())
5149  {
5150  cv::Mat imageMono;
5151  if(data.imageRaw().channels() == 3)
5152  {
5153  cv::cvtColor(data.imageRaw(), imageMono, CV_BGR2GRAY);
5154  }
5155  else
5156  {
5157  imageMono = data.imageRaw();
5158  }
5159 
5160  UASSERT_MSG(imagesRectified, "Cannot extract descriptors on not rectified image from keypoints which assumed to be undistorted");
5161  descriptors = _feature2D->generateDescriptors(imageMono, keypoints);
5162  }
5163  else if(!imagesRectified && !data.cameraModels().empty())
5164  {
5165  std::vector<cv::KeyPoint> keypointsValid;
5166  keypointsValid.reserve(keypoints.size());
5167  cv::Mat descriptorsValid;
5168  descriptorsValid.reserve(descriptors.rows);
5169  std::vector<cv::Point3f> keypoints3DValid;
5170  keypoints3DValid.reserve(keypoints3D.size());
5171 
5172  //undistort keypoints before projection (RGB-D)
5173  if(data.cameraModels().size() == 1)
5174  {
5175  std::vector<cv::Point2f> pointsIn, pointsOut;
5176  cv::KeyPoint::convert(keypoints,pointsIn);
5177  if(data.cameraModels()[0].D_raw().cols == 6)
5178  {
5179 #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)))
5180  // Equidistant / FishEye
5181  // get only k parameters (k1,k2,p1,p2,k3,k4)
5182  cv::Mat D(1, 4, CV_64FC1);
5183  D.at<double>(0,0) = data.cameraModels()[0].D_raw().at<double>(0,0);
5184  D.at<double>(0,1) = data.cameraModels()[0].D_raw().at<double>(0,1);
5185  D.at<double>(0,2) = data.cameraModels()[0].D_raw().at<double>(0,4);
5186  D.at<double>(0,3) = data.cameraModels()[0].D_raw().at<double>(0,5);
5187  cv::fisheye::undistortPoints(pointsIn, pointsOut,
5188  data.cameraModels()[0].K_raw(),
5189  D,
5190  data.cameraModels()[0].R(),
5191  data.cameraModels()[0].P());
5192  }
5193  else
5194 #else
5195  UWARN("Too old opencv version (%d,%d,%d) to support fisheye model (min 2.4.10 required)!",
5196  CV_MAJOR_VERSION, CV_MINOR_VERSION, CV_SUBMINOR_VERSION);
5197  }
5198 #endif
5199  {
5200  //RadialTangential
5201  cv::undistortPoints(pointsIn, pointsOut,
5202  data.cameraModels()[0].K_raw(),
5203  data.cameraModels()[0].D_raw(),
5204  data.cameraModels()[0].R(),
5205  data.cameraModels()[0].P());
5206  }
5207  UASSERT(pointsOut.size() == keypoints.size());
5208  for(unsigned int i=0; i<pointsOut.size(); ++i)
5209  {
5210  if(pointsOut.at(i).x>=0 && pointsOut.at(i).x<data.cameraModels()[0].imageWidth() &&
5211  pointsOut.at(i).y>=0 && pointsOut.at(i).y<data.cameraModels()[0].imageHeight())
5212  {
5213  keypointsValid.push_back(keypoints.at(i));
5214  keypointsValid.back().pt.x = pointsOut.at(i).x;
5215  keypointsValid.back().pt.y = pointsOut.at(i).y;
5216  descriptorsValid.push_back(descriptors.row(i));
5217  if(!keypoints3D.empty())
5218  {
5219  keypoints3DValid.push_back(keypoints3D.at(i));
5220  }
5221  }
5222  }
5223  }
5224  else
5225  {
5226  float subImageWidth;
5227  if(!data.imageRaw().empty())
5228  {
5229  UASSERT(int((data.imageRaw().cols/data.cameraModels().size())*data.cameraModels().size()) == data.imageRaw().cols);
5230  subImageWidth = data.imageRaw().cols/data.cameraModels().size();
5231  }
5232  else
5233  {
5234  UASSERT(data.cameraModels()[0].imageWidth()>0);
5235  subImageWidth = data.cameraModels()[0].imageWidth();
5236  }
5237 
5238  for(unsigned int i=0; i<keypoints.size(); ++i)
5239  {
5240  int cameraIndex = int(keypoints.at(i).pt.x / subImageWidth);
5241  UASSERT_MSG(cameraIndex >= 0 && cameraIndex < (int)data.cameraModels().size(),
5242  uFormat("cameraIndex=%d, models=%d, kpt.x=%f, subImageWidth=%f (Camera model image width=%d)",
5243  cameraIndex, (int)data.cameraModels().size(), keypoints[i].pt.x, subImageWidth, data.cameraModels()[0].imageWidth()).c_str());
5244 
5245  std::vector<cv::Point2f> pointsIn, pointsOut;
5246  pointsIn.push_back(cv::Point2f(keypoints.at(i).pt.x-subImageWidth*cameraIndex, keypoints.at(i).pt.y));
5247  if(data.cameraModels()[cameraIndex].D_raw().cols == 6)
5248  {
5249 #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)))
5250  // Equidistant / FishEye
5251  // get only k parameters (k1,k2,p1,p2,k3,k4)
5252  cv::Mat D(1, 4, CV_64FC1);
5253  D.at<double>(0,0) = data.cameraModels()[cameraIndex].D_raw().at<double>(0,0);
5254  D.at<double>(0,1) = data.cameraModels()[cameraIndex].D_raw().at<double>(0,1);
5255  D.at<double>(0,2) = data.cameraModels()[cameraIndex].D_raw().at<double>(0,4);
5256  D.at<double>(0,3) = data.cameraModels()[cameraIndex].D_raw().at<double>(0,5);
5257  cv::fisheye::undistortPoints(pointsIn, pointsOut,
5258  data.cameraModels()[cameraIndex].K_raw(),
5259  D,
5260  data.cameraModels()[cameraIndex].R(),
5261  data.cameraModels()[cameraIndex].P());
5262  }
5263  else
5264 #else
5265  UWARN("Too old opencv version (%d,%d,%d) to support fisheye model (min 2.4.10 required)!",
5266  CV_MAJOR_VERSION, CV_MINOR_VERSION, CV_SUBMINOR_VERSION);
5267  }
5268 #endif
5269  {
5270  //RadialTangential
5271  cv::undistortPoints(pointsIn, pointsOut,
5272  data.cameraModels()[cameraIndex].K_raw(),
5273  data.cameraModels()[cameraIndex].D_raw(),
5274  data.cameraModels()[cameraIndex].R(),
5275  data.cameraModels()[cameraIndex].P());
5276  }
5277 
5278  if(pointsOut[0].x>=0 && pointsOut[0].x<data.cameraModels()[cameraIndex].imageWidth() &&
5279  pointsOut[0].y>=0 && pointsOut[0].y<data.cameraModels()[cameraIndex].imageHeight())
5280  {
5281  keypointsValid.push_back(keypoints.at(i));
5282  keypointsValid.back().pt.x = pointsOut[0].x + subImageWidth*cameraIndex;
5283  keypointsValid.back().pt.y = pointsOut[0].y;
5284  descriptorsValid.push_back(descriptors.row(i));
5285  if(!keypoints3D.empty())
5286  {
5287  keypoints3DValid.push_back(keypoints3D.at(i));
5288  }
5289  }
5290  }
5291  }
5292 
5293  keypoints = keypointsValid;
5294  descriptors = descriptorsValid;
5295  keypoints3D = keypoints3DValid;
5296 
5297  t = timer.ticks();
5298  if(stats) stats->addStatistic(Statistics::kTimingMemRectification(), t*1000.0f);
5299  UDEBUG("time rectification = %fs", t);
5300  }
5301  t = timer.ticks();
5302  if(stats) stats->addStatistic(Statistics::kTimingMemDescriptors_extraction(), t*1000.0f);
5303  UDEBUG("time descriptors (%d) = %fs", descriptors.rows, t);
5304 
5305  if(keypoints3D.empty() &&
5306  ((!data.depthRaw().empty() && data.cameraModels().size() && data.cameraModels()[0].isValidForProjection()) ||
5307  (!data.rightRaw().empty() && data.stereoCameraModels().size() && data.stereoCameraModels()[0].isValidForProjection())))
5308  {
5309  keypoints3D = _feature2D->generateKeypoints3D(data, keypoints);
5310  }
5311  if(_feature2D->getMinDepth() > 0.0f || _feature2D->getMaxDepth() > 0.0f)
5312  {
5313  _feature2D->filterKeypointsByDepth(keypoints, descriptors, keypoints3D, _feature2D->getMinDepth(), _feature2D->getMaxDepth());
5314  }
5315  t = timer.ticks();
5316  if(stats) stats->addStatistic(Statistics::kTimingMemKeypoints_3D(), t*1000.0f);
5317  UDEBUG("time keypoints 3D (%d) = %fs", (int)keypoints3D.size(), t);
5318 
5319  UDEBUG("ratio=%f, meanWordsPerLocation=%d", _badSignRatio, meanWordsPerLocation);
5320  if(descriptors.rows && descriptors.rows < _badSignRatio * float(meanWordsPerLocation))
5321  {
5322  descriptors = cv::Mat();
5323  }
5324  }
5325 
5326  if(_parallelized)
5327  {
5328  UDEBUG("Joining dictionary update thread...");
5329  preUpdateThread.join(); // Wait the dictionary to be updated
5330  UDEBUG("Joining dictionary update thread... thread finished!");
5331  }
5332 
5333  t = timer.ticks();
5334  if(stats) stats->addStatistic(Statistics::kTimingMemJoining_dictionary_update(), t*1000.0f);
5335  if(_parallelized)
5336  {
5337  UDEBUG("time descriptor and memory update (%d of size=%d) = %fs", descriptors.rows, descriptors.cols, t);
5338  }
5339  else
5340  {
5341  UDEBUG("time descriptor (%d of size=%d) = %fs", descriptors.rows, descriptors.cols, t);
5342  }
5343 
5344  std::list<int> wordIds;
5345  if(descriptors.rows)
5346  {
5347  // In case the number of features we want to do quantization is lower
5348  // than extracted ones (that would be used for transform estimation)
5349  std::vector<bool> inliers;
5350  cv::Mat descriptorsForQuantization = descriptors;
5351  std::vector<int> quantizedToRawIndices;
5352  if(_feature2D->getMaxFeatures()>0 && descriptors.rows > _feature2D->getMaxFeatures())
5353  {
5354  UASSERT((int)keypoints.size() == descriptors.rows);
5355  int inliersCount = 0;
5356  if((_feature2D->getGridRows() > 1 || _feature2D->getGridCols() > 1) &&
5357  (decimatedData.cameraModels().size()==1 || decimatedData.stereoCameraModels().size()==1 ||
5358  data.cameraModels().size()==1 || data.stereoCameraModels().size()==1))
5359  {
5360  Feature2D::limitKeypoints(keypoints, inliers, _feature2D->getMaxFeatures(),
5361  decimatedData.cameraModels().size()?decimatedData.cameraModels()[0].imageSize():
5362  decimatedData.stereoCameraModels().size()?decimatedData.stereoCameraModels()[0].left().imageSize():
5363  data.cameraModels().size()?data.cameraModels()[0].imageSize():data.stereoCameraModels()[0].left().imageSize(),
5364  _feature2D->getGridRows(), _feature2D->getGridCols(), _feature2D->getSSC());
5365  }
5366  else
5367  {
5368  if(_feature2D->getGridRows() > 1 || _feature2D->getGridCols() > 1)
5369  {
5370  UWARN("Ignored %s and %s parameters as they cannot be used for multi-cameras setup or uncalibrated camera.",
5371  Parameters::kKpGridCols().c_str(), Parameters::kKpGridRows().c_str());
5372  }
5373  if(decimatedData.cameraModels().size()==1 || decimatedData.stereoCameraModels().size()==1 ||
5374  data.cameraModels().size()==1 || data.stereoCameraModels().size()==1)
5375  {
5376  Feature2D::limitKeypoints(keypoints, inliers, _feature2D->getMaxFeatures(),
5377  decimatedData.cameraModels().size()?decimatedData.cameraModels()[0].imageSize():
5378  decimatedData.stereoCameraModels().size()?decimatedData.stereoCameraModels()[0].left().imageSize():
5379  data.cameraModels().size()?data.cameraModels()[0].imageSize():data.stereoCameraModels()[0].left().imageSize(),
5380  _feature2D->getSSC());
5381  }
5382  else
5383  {
5384  Feature2D::limitKeypoints(keypoints, inliers, _feature2D->getMaxFeatures());
5385  }
5386  }
5387  for(size_t i=0; i<inliers.size(); ++i)
5388  {
5389  if(inliers[i])
5390  ++inliersCount;
5391  }
5392 
5393  descriptorsForQuantization = cv::Mat(inliersCount, descriptors.cols, descriptors.type());
5394  quantizedToRawIndices.resize(inliersCount);
5395  unsigned int oi=0;
5396  UASSERT((int)inliers.size() == descriptors.rows);
5397  for(int k=0; k < descriptors.rows; ++k)
5398  {
5399  if(inliers[k])
5400  {
5401  UASSERT(oi < quantizedToRawIndices.size());
5402  if(descriptors.type() == CV_32FC1)
5403  {
5404  memcpy(descriptorsForQuantization.ptr<float>(oi), descriptors.ptr<float>(k), descriptors.cols*sizeof(float));
5405  }
5406  else
5407  {
5408  memcpy(descriptorsForQuantization.ptr<char>(oi), descriptors.ptr<char>(k), descriptors.cols*sizeof(char));
5409  }
5410  quantizedToRawIndices[oi] = k;
5411  ++oi;
5412  }
5413  }
5414  UASSERT_MSG((int)oi == inliersCount,
5415  uFormat("oi=%d inliersCount=%d (maxFeatures=%d, grid=%dx%d)",
5416  oi, inliersCount, _feature2D->getMaxFeatures(), _feature2D->getGridCols(), _feature2D->getGridRows()).c_str());
5417  }
5418 
5419  // Quantization to vocabulary
5420  wordIds = _vwd->addNewWords(descriptorsForQuantization, id);
5421 
5422  // Set ID -1 to features not used for quantization
5423  if(wordIds.size() < keypoints.size())
5424  {
5425  std::vector<int> allWordIds;
5426  allWordIds.resize(keypoints.size(),-1);
5427  int i=0;
5428  for(std::list<int>::iterator iter=wordIds.begin(); iter!=wordIds.end(); ++iter)
5429  {
5430  allWordIds[quantizedToRawIndices[i]] = *iter;
5431  ++i;
5432  }
5433  int negIndex = -1;
5434  for(i=0; i<(int)allWordIds.size(); ++i)
5435  {
5436  if(allWordIds[i] < 0)
5437  {
5438  allWordIds[i] = negIndex--;
5439  }
5440  }
5441  wordIds = uVectorToList(allWordIds);
5442  }
5443 
5444  t = timer.ticks();
5445  if(stats) stats->addStatistic(Statistics::kTimingMemAdd_new_words(), t*1000.0f);
5446  UDEBUG("time addNewWords %fs indexed=%d not=%d", t, _vwd->getIndexedWordsCount(), _vwd->getNotIndexedWordsCount());
5447  }
5448  else if(id>0)
5449  {
5450  UDEBUG("id %d is a bad signature", id);
5451  }
5452 
5453  std::multimap<int, int> words;
5454  std::vector<cv::KeyPoint> wordsKpts;
5455  std::vector<cv::Point3f> words3D;
5456  cv::Mat wordsDescriptors;
5457  int words3DValid = 0;
5458  if(wordIds.size() > 0)
5459  {
5460  UASSERT(wordIds.size() == keypoints.size());
5461  UASSERT(keypoints3D.size() == 0 || keypoints3D.size() == wordIds.size());
5462  unsigned int i=0;
5463  float decimationRatio = float(preDecimation) / float(_imagePostDecimation);
5464  double log2value = log(double(preDecimation))/log(2.0);
5465  for(std::list<int>::iterator iter=wordIds.begin(); iter!=wordIds.end() && i < keypoints.size(); ++iter, ++i)
5466  {
5467  cv::KeyPoint kpt = keypoints[i];
5468  if(preDecimation != _imagePostDecimation)
5469  {
5470  // remap keypoints to final image size
5471  kpt.pt.x *= decimationRatio;
5472  kpt.pt.y *= decimationRatio;
5473  kpt.size *= decimationRatio;
5474  kpt.octave += log2value;
5475  }
5476  words.insert(std::make_pair(*iter, words.size()));
5477  wordsKpts.push_back(kpt);
5478 
5479  if(keypoints3D.size())
5480  {
5481  words3D.push_back(keypoints3D.at(i));
5482  if(util3d::isFinite(keypoints3D.at(i)))
5483  {
5484  ++words3DValid;
5485  }
5486  }
5487  if(_rawDescriptorsKept)
5488  {
5489  wordsDescriptors.push_back(descriptors.row(i));
5490  }
5491  }
5492  }
5493 
5494  Landmarks landmarks = data.landmarks();
5495  if(_detectMarkers && !isIntermediateNode && !data.imageRaw().empty())
5496  {
5497  UDEBUG("Detecting markers...");
5498  if(landmarks.empty())
5499  {
5500  std::vector<CameraModel> models = data.cameraModels();
5501  if(models.empty())
5502  {
5503  for(size_t i=0; i<data.stereoCameraModels().size(); ++i)
5504  {
5505  models.push_back(data.stereoCameraModels()[i].left());
5506  }
5507  }
5508 
5509  if(!models.empty() && models[0].isValidForProjection())
5510  {
5511  std::map<int, MarkerInfo> markers = _markerDetector->detect(data.imageRaw(), models, data.depthRaw(), _landmarksSize);
5512 
5513  for(std::map<int, MarkerInfo>::iterator iter=markers.begin(); iter!=markers.end(); ++iter)
5514  {
5515  if(iter->first <= 0)
5516  {
5517  UERROR("Invalid marker received! IDs should be > 0 (it is %d). Ignoring this marker.", iter->first);
5518  continue;
5519  }
5520  cv::Mat covariance = cv::Mat::eye(6,6,CV_64FC1);
5521  covariance(cv::Range(0,3), cv::Range(0,3)) *= _markerLinVariance;
5522  covariance(cv::Range(3,6), cv::Range(3,6)) *= _markerAngVariance;
5523  landmarks.insert(std::make_pair(iter->first, Landmark(iter->first, iter->second.length(), iter->second.pose(), covariance)));
5524  }
5525  UDEBUG("Markers detected = %d", (int)markers.size());
5526  }
5527  else
5528  {
5529  UWARN("No valid camera calibration for marker detection");
5530  }
5531  }
5532  else
5533  {
5534  UWARN("Input data has already landmarks, cannot do marker detection.");
5535  }
5536  t = timer.ticks();
5537  if(stats) stats->addStatistic(Statistics::kTimingMemMarkers_detection(), t*1000.0f);
5538  UDEBUG("time markers detection = %fs", t);
5539  }
5540 
5541  cv::Mat image = data.imageRaw();
5542  cv::Mat depthOrRightImage = data.depthOrRightRaw();
5543  std::vector<CameraModel> cameraModels = data.cameraModels();
5544  std::vector<StereoCameraModel> stereoCameraModels = data.stereoCameraModels();
5545 
5546  // apply decimation?
5547  if(_imagePostDecimation > 1 && !isIntermediateNode)
5548  {
5549  if(_imagePostDecimation == preDecimation && decimatedData.isValid())
5550  {
5551  image = decimatedData.imageRaw();
5552  depthOrRightImage = decimatedData.depthOrRightRaw();
5553  cameraModels = decimatedData.cameraModels();
5554  stereoCameraModels = decimatedData.stereoCameraModels();
5555  }
5556  else
5557  {
5558  int decimationDepth = _imagePreDecimation;
5559  if( !data.cameraModels().empty() &&
5560  data.cameraModels()[0].imageHeight()>0 &&
5561  data.cameraModels()[0].imageWidth()>0)
5562  {
5563  // decimate from RGB image size
5564  int targetSize = data.cameraModels()[0].imageHeight() / _imagePreDecimation;
5565  if(targetSize >= data.depthRaw().rows)
5566  {
5567  decimationDepth = 1;
5568  }
5569  else
5570  {
5571  decimationDepth = (int)ceil(float(data.depthRaw().rows) / float(targetSize));
5572  }
5573  }
5574  UDEBUG("decimation rgbOrLeft(rows=%d)=%d, depthOrRight(rows=%d)=%d", data.imageRaw().rows, _imagePostDecimation, data.depthOrRightRaw().rows, decimationDepth);
5575 
5576  depthOrRightImage = util2d::decimate(depthOrRightImage, decimationDepth);
5577  image = util2d::decimate(image, _imagePostDecimation);
5578  for(unsigned int i=0; i<cameraModels.size(); ++i)
5579  {
5580  cameraModels[i] = cameraModels[i].scaled(1.0/double(_imagePostDecimation));
5581  }
5582  for(unsigned int i=0; i<stereoCameraModels.size(); ++i)
5583  {
5584  stereoCameraModels[i].scale(1.0/double(_imagePostDecimation));
5585  }
5586  }
5587 
5588  if(!image.empty() && (depthOrRightImage.cols > image.cols || depthOrRightImage.rows > image.rows))
5589  {
5590  UWARN("Depth image is bigger than RGB image after post decimation, %s=%d is too high! RGB=%dx%d, depth=%dx%d",
5591  Parameters::kMemImagePostDecimation().c_str(), _imagePostDecimation,
5592  image.cols, image.rows, depthOrRightImage.cols, depthOrRightImage.rows);
5593  }
5594 
5595  t = timer.ticks();
5596  if(stats) stats->addStatistic(Statistics::kTimingMemPost_decimation(), t*1000.0f);
5597  UDEBUG("time post-decimation = %fs", t);
5598  }
5599 
5600  if(_stereoFromMotion &&
5601  !pose.isNull() &&
5602  cameraModels.size() == 1 &&
5603  words.size() &&
5604  (words3D.size() == 0 || (words.size() == words3D.size() && words3DValid!=(int)words3D.size())) &&
5605  _registrationPipeline->isImageRequired() &&
5606  _signatures.size() &&
5607  _signatures.rbegin()->second->mapId() == _idMapCount) // same map
5608  {
5609  UDEBUG("Generate 3D words using odometry (%s=true and words3DValid=%d/%d)",
5610  Parameters::kMemStereoFromMotion().c_str(), words3DValid, (int)words3D.size());
5611  Signature * previousS = _signatures.rbegin()->second;
5612  if(previousS->getWords().size() > 8 && words.size() > 8 && !previousS->getPose().isNull())
5613  {
5614  UDEBUG("Previous pose(%d) = %s", previousS->id(), previousS->getPose().prettyPrint().c_str());
5615  UDEBUG("Current pose(%d) = %s", id, pose.prettyPrint().c_str());
5616  Transform cameraTransform = pose.inverse() * previousS->getPose();
5617 
5618  Signature cpPrevious(2);
5619  // IDs should be unique so that registration doesn't override them
5620  std::map<int, int> uniqueWordsOld = uMultimapToMapUnique(previousS->getWords());
5621  std::vector<cv::KeyPoint> uniqueWordsKpts;
5622  cv::Mat uniqueWordsDescriptors;
5623  std::multimap<int, int> uniqueWords;
5624  for(std::map<int, int>::iterator iter=uniqueWordsOld.begin(); iter!=uniqueWordsOld.end(); ++iter)
5625  {
5626  uniqueWords.insert(std::make_pair(iter->first, uniqueWords.size()));
5627  uniqueWordsKpts.push_back(previousS->getWordsKpts()[iter->second]);
5628  uniqueWordsDescriptors.push_back(previousS->getWordsDescriptors().row(iter->second));
5629  }
5630  cpPrevious.sensorData().setCameraModels(previousS->sensorData().cameraModels());
5631  cpPrevious.setWords(uniqueWords, uniqueWordsKpts, std::vector<cv::Point3f>(), uniqueWordsDescriptors);
5632  Signature cpCurrent(1);
5633  uniqueWordsOld = uMultimapToMapUnique(words);
5634  uniqueWordsKpts.clear();
5635  uniqueWordsDescriptors = cv::Mat();
5636  uniqueWords.clear();
5637  for(std::map<int, int>::iterator iter=uniqueWordsOld.begin(); iter!=uniqueWordsOld.end(); ++iter)
5638  {
5639  uniqueWords.insert(std::make_pair(iter->first, uniqueWords.size()));
5640  uniqueWordsKpts.push_back(wordsKpts[iter->second]);
5641  uniqueWordsDescriptors.push_back(wordsDescriptors.row(iter->second));
5642  }
5643  cpCurrent.sensorData().setCameraModels(cameraModels);
5644  // This will force comparing descriptors between both images directly
5645  cpCurrent.setWords(uniqueWords, uniqueWordsKpts, std::vector<cv::Point3f>(), uniqueWordsDescriptors);
5646 
5647  // The following is used only to re-estimate the correspondences, the returned transform is ignored
5648  Transform tmpt;
5649  RegistrationVis reg(parameters_);
5650  if(_registrationPipeline->isScanRequired())
5651  {
5652  // If icp is used, remove it to just do visual registration
5653  RegistrationVis vis(parameters_);
5654  tmpt = vis.computeTransformationMod(cpCurrent, cpPrevious, cameraTransform);
5655  }
5656  else
5657  {
5658  tmpt = _registrationPipeline->computeTransformationMod(cpCurrent, cpPrevious, cameraTransform);
5659  }
5660  UDEBUG("t=%s", tmpt.prettyPrint().c_str());
5661 
5662  // compute 3D words by epipolar geometry with the previous signature using odometry motion
5663  std::map<int, int> currentUniqueWords = uMultimapToMapUnique(cpCurrent.getWords());
5664  std::map<int, int> previousUniqueWords = uMultimapToMapUnique(cpPrevious.getWords());
5665  std::map<int, cv::KeyPoint> currentWords;
5666  std::map<int, cv::KeyPoint> previousWords;
5667  for(std::map<int, int>::iterator iter=currentUniqueWords.begin(); iter!=currentUniqueWords.end(); ++iter)
5668  {
5669  currentWords.insert(std::make_pair(iter->first, cpCurrent.getWordsKpts()[iter->second]));
5670  }
5671  for(std::map<int, int>::iterator iter=previousUniqueWords.begin(); iter!=previousUniqueWords.end(); ++iter)
5672  {
5673  previousWords.insert(std::make_pair(iter->first, cpPrevious.getWordsKpts()[iter->second]));
5674  }
5675  std::map<int, cv::Point3f> inliers = util3d::generateWords3DMono(
5676  currentWords,
5677  previousWords,
5678  cameraModels[0],
5679  cameraTransform);
5680 
5681  UDEBUG("inliers=%d", (int)inliers.size());
5682 
5683  // words3D should have the same size than words if not empty
5684  float bad_point = std::numeric_limits<float>::quiet_NaN ();
5685  UASSERT(words3D.size() == 0 || words.size() == words3D.size());
5686  bool words3DWasEmpty = words3D.empty();
5687  int added3DPointsWithoutDepth = 0;
5688  for(std::multimap<int, int>::const_iterator iter=words.begin(); iter!=words.end(); ++iter)
5689  {
5690  std::map<int, cv::Point3f>::iterator jter=inliers.find(iter->first);
5691  if(words3DWasEmpty)
5692  {
5693  if(jter != inliers.end())
5694  {
5695  words3D.push_back(jter->second);
5696  ++added3DPointsWithoutDepth;
5697  }
5698  else
5699  {
5700  words3D.push_back(cv::Point3f(bad_point,bad_point,bad_point));
5701  }
5702  }
5703  else if(!util3d::isFinite(words3D[iter->second]) && jter != inliers.end())
5704  {
5705  words3D[iter->second] = jter->second;
5706  ++added3DPointsWithoutDepth;
5707  }
5708  }
5709  UDEBUG("added3DPointsWithoutDepth=%d", added3DPointsWithoutDepth);
5710  if(stats) stats->addStatistic(Statistics::kMemoryTriangulated_points(), (float)added3DPointsWithoutDepth);
5711 
5712  t = timer.ticks();
5713  UASSERT(words3D.size() == words.size());
5714  if(stats) stats->addStatistic(Statistics::kTimingMemKeypoints_3D_motion(), t*1000.0f);
5715  UDEBUG("time keypoints 3D by motion (%d) = %fs", (int)words3D.size(), t);
5716  }
5717  }
5718 
5719  // Filter the laser scan?
5720  LaserScan laserScan = data.laserScanRaw();
5721  if(!isIntermediateNode && laserScan.size())
5722  {
5723  if(laserScan.rangeMax() == 0.0f)
5724  {
5725  bool id2d = laserScan.is2d();
5726  float maxRange = 0.0f;
5727  for(int i=0; i<laserScan.size(); ++i)
5728  {
5729  const float * ptr = laserScan.data().ptr<float>(0, i);
5730  float r;
5731  if(id2d)
5732  {
5733  r = ptr[0]*ptr[0] + ptr[1]*ptr[1];
5734  }
5735  else
5736  {
5737  r = ptr[0]*ptr[0] + ptr[1]*ptr[1] + ptr[2]*ptr[2];
5738  }
5739  if(r>maxRange)
5740  {
5741  maxRange = r;
5742  }
5743  }
5744  if(maxRange > 0.0f)
5745  {
5746  laserScan=LaserScan(laserScan.data(), laserScan.maxPoints(), sqrt(maxRange), laserScan.format(), laserScan.localTransform());
5747  }
5748  }
5749 
5750  laserScan = util3d::commonFiltering(laserScan,
5751  _laserScanDownsampleStepSize,
5752  0,
5753  0,
5754  _laserScanVoxelSize,
5755  _laserScanNormalK,
5756  _laserScanNormalRadius,
5757  _laserScanGroundNormalsUp);
5758  t = timer.ticks();
5759  if(stats) stats->addStatistic(Statistics::kTimingMemScan_filtering(), t*1000.0f);
5760  UDEBUG("time normals scan = %fs", t);
5761  }
5762 
5763  Signature * s;
5764  if(this->isBinDataKept() && (!isIntermediateNode || _saveIntermediateNodeData))
5765  {
5766  UDEBUG("Bin data kept: rgb=%d, depth=%d, scan=%d, userData=%d",
5767  image.empty()?0:1,
5768  depthOrRightImage.empty()?0:1,
5769  laserScan.isEmpty()?0:1,
5770  data.userDataRaw().empty()?0:1);
5771 
5772  std::vector<unsigned char> imageBytes;
5773  std::vector<unsigned char> depthBytes;
5774 
5775  if(_saveDepth16Format && !depthOrRightImage.empty() && depthOrRightImage.type() == CV_32FC1)
5776  {
5777  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).");
5778  depthOrRightImage = util2d::cvtDepthFromFloat(depthOrRightImage);
5779  }
5780 
5781  cv::Mat compressedImage;
5782  cv::Mat compressedDepth;
5783  cv::Mat compressedScan;
5784  cv::Mat compressedUserData;
5785  if(_compressionParallelized)
5786  {
5787  rtabmap::CompressionThread ctImage(image, _rgbCompressionFormat);
5788  rtabmap::CompressionThread ctDepth(depthOrRightImage, depthOrRightImage.type() == CV_32FC1 || depthOrRightImage.type() == CV_16UC1?std::string(".png"):_rgbCompressionFormat);
5789  rtabmap::CompressionThread ctLaserScan(laserScan.data());
5790  rtabmap::CompressionThread ctUserData(data.userDataRaw());
5791  if(!image.empty())
5792  {
5793  ctImage.start();
5794  }
5795  if(!depthOrRightImage.empty())
5796  {
5797  ctDepth.start();
5798  }
5799  if(!laserScan.isEmpty())
5800  {
5801  ctLaserScan.start();
5802  }
5803  if(!data.userDataRaw().empty())
5804  {
5805  ctUserData.start();
5806  }
5807  ctImage.join();
5808  ctDepth.join();
5809  ctLaserScan.join();
5810  ctUserData.join();
5811 
5812  compressedImage = ctImage.getCompressedData();
5813  compressedDepth = ctDepth.getCompressedData();
5814  compressedScan = ctLaserScan.getCompressedData();
5815  compressedUserData = ctUserData.getCompressedData();
5816  }
5817  else
5818  {
5819  compressedImage = compressImage2(image, _rgbCompressionFormat);
5820  compressedDepth = compressImage2(depthOrRightImage, depthOrRightImage.type() == CV_32FC1 || depthOrRightImage.type() == CV_16UC1?std::string(".png"):_rgbCompressionFormat);
5821  compressedScan = compressData2(laserScan.data());
5822  compressedUserData = compressData2(data.userDataRaw());
5823  }
5824 
5825  s = new Signature(id,
5826  _idMapCount,
5827  isIntermediateNode?-1:0, // tag intermediate nodes as weight=-1
5828  data.stamp(),
5829  "",
5830  pose,
5831  data.groundTruth(),
5832  !stereoCameraModels.empty()?
5833  SensorData(
5834  laserScan.angleIncrement() == 0.0f?
5835  LaserScan(compressedScan,
5836  laserScan.maxPoints(),
5837  laserScan.rangeMax(),
5838  laserScan.format(),
5839  laserScan.localTransform()):
5840  LaserScan(compressedScan,
5841  laserScan.format(),
5842  laserScan.rangeMin(),
5843  laserScan.rangeMax(),
5844  laserScan.angleMin(),
5845  laserScan.angleMax(),
5846  laserScan.angleIncrement(),
5847  laserScan.localTransform()),
5848  compressedImage,
5849  compressedDepth,
5850  stereoCameraModels,
5851  id,
5852  0,
5853  compressedUserData):
5854  SensorData(
5855  laserScan.angleIncrement() == 0.0f?
5856  LaserScan(compressedScan,
5857  laserScan.maxPoints(),
5858  laserScan.rangeMax(),
5859  laserScan.format(),
5860  laserScan.localTransform()):
5861  LaserScan(compressedScan,
5862  laserScan.format(),
5863  laserScan.rangeMin(),
5864  laserScan.rangeMax(),
5865  laserScan.angleMin(),
5866  laserScan.angleMax(),
5867  laserScan.angleIncrement(),
5868  laserScan.localTransform()),
5869  compressedImage,
5870  compressedDepth,
5871  cameraModels,
5872  id,
5873  0,
5874  compressedUserData));
5875  }
5876  else
5877  {
5878  UDEBUG("Bin data kept: scan=%d, userData=%d",
5879  laserScan.isEmpty()?0:1,
5880  data.userDataRaw().empty()?0:1);
5881 
5882  // just compress user data and laser scan (scans can be used for local scan matching)
5883  cv::Mat compressedScan;
5884  cv::Mat compressedUserData;
5885  if(_compressionParallelized)
5886  {
5887  rtabmap::CompressionThread ctUserData(data.userDataRaw());
5888  rtabmap::CompressionThread ctLaserScan(laserScan.data());
5889  if(!data.userDataRaw().empty() && !isIntermediateNode)
5890  {
5891  ctUserData.start();
5892  }
5893  if(!laserScan.isEmpty() && !isIntermediateNode)
5894  {
5895  ctLaserScan.start();
5896  }
5897  ctUserData.join();
5898  ctLaserScan.join();
5899 
5900  compressedScan = ctLaserScan.getCompressedData();
5901  compressedUserData = ctUserData.getCompressedData();
5902  }
5903  else
5904  {
5905  compressedScan = compressData2(laserScan.data());
5906  compressedUserData = compressData2(data.userDataRaw());
5907  }
5908 
5909  s = new Signature(id,
5910  _idMapCount,
5911  isIntermediateNode?-1:0, // tag intermediate nodes as weight=-1
5912  data.stamp(),
5913  "",
5914  pose,
5915  data.groundTruth(),
5916  !stereoCameraModels.empty()?
5917  SensorData(
5918  laserScan.angleIncrement() == 0.0f?
5919  LaserScan(compressedScan,
5920  laserScan.maxPoints(),
5921  laserScan.rangeMax(),
5922  laserScan.format(),
5923  laserScan.localTransform()):
5924  LaserScan(compressedScan,
5925  laserScan.format(),
5926  laserScan.rangeMin(),
5927  laserScan.rangeMax(),
5928  laserScan.angleMin(),
5929  laserScan.angleMax(),
5930  laserScan.angleIncrement(),
5931  laserScan.localTransform()),
5932  cv::Mat(),
5933  cv::Mat(),
5934  stereoCameraModels,
5935  id,
5936  0,
5937  compressedUserData):
5938  SensorData(
5939  laserScan.angleIncrement() == 0.0f?
5940  LaserScan(compressedScan,
5941  laserScan.maxPoints(),
5942  laserScan.rangeMax(),
5943  laserScan.format(),
5944  laserScan.localTransform()):
5945  LaserScan(compressedScan,
5946  laserScan.format(),
5947  laserScan.rangeMin(),
5948  laserScan.rangeMax(),
5949  laserScan.angleMin(),
5950  laserScan.angleMax(),
5951  laserScan.angleIncrement(),
5952  laserScan.localTransform()),
5953  cv::Mat(),
5954  cv::Mat(),
5955  cameraModels,
5956  id,
5957  0,
5958  compressedUserData));
5959  }
5960 
5961  s->setWords(words, wordsKpts,
5962  _reextractLoopClosureFeatures?std::vector<cv::Point3f>():words3D,
5963  _reextractLoopClosureFeatures?cv::Mat():wordsDescriptors);
5964 
5965  // set raw data
5966  if(!cameraModels.empty())
5967  {
5968  s->sensorData().setRGBDImage(image, depthOrRightImage, cameraModels, false);
5969  }
5970  else
5971  {
5972  s->sensorData().setStereoImage(image, depthOrRightImage, stereoCameraModels, false);
5973  }
5974  s->sensorData().setLaserScan(laserScan, false);
5975  s->sensorData().setUserData(data.userDataRaw(), false);
5976 
5977  UDEBUG("data.groundTruth() =%s", data.groundTruth().prettyPrint().c_str());
5978  UDEBUG("data.gps() =%s", data.gps().stamp()?"true":"false");
5979  UDEBUG("data.envSensors() =%d", (int)data.envSensors().size());
5980  UDEBUG("data.globalDescriptors()=%d", (int)data.globalDescriptors().size());
5981  s->sensorData().setGroundTruth(data.groundTruth());
5982  s->sensorData().setGPS(data.gps());
5983  s->sensorData().setEnvSensors(data.envSensors());
5984 
5985  std::vector<GlobalDescriptor> globalDescriptors = data.globalDescriptors();
5986  if(_globalDescriptorExtractor)
5987  {
5988  GlobalDescriptor gdescriptor = _globalDescriptorExtractor->extract(inputData);
5989  if(!gdescriptor.data().empty())
5990  {
5991  globalDescriptors.push_back(gdescriptor);
5992  }
5993  }
5994  s->sensorData().setGlobalDescriptors(globalDescriptors);
5995 
5996  t = timer.ticks();
5997  if(stats) stats->addStatistic(Statistics::kTimingMemCompressing_data(), t*1000.0f);
5998  UDEBUG("time compressing data (id=%d) %fs", id, t);
5999  if(words.size())
6000  {
6001  s->setEnabled(true); // All references are already activated in the dictionary at this point (see _vwd->addNewWords())
6002  }
6003 
6004  // Occupancy grid map stuff
6005  if(_createOccupancyGrid && !isIntermediateNode)
6006  {
6007  if( (_localMapMaker->isGridFromDepth() && !data.depthOrRightRaw().empty()) ||
6008  (!_localMapMaker->isGridFromDepth() && !data.laserScanRaw().empty()))
6009  {
6010  cv::Mat ground, obstacles, empty;
6011  float cellSize = 0.0f;
6012  cv::Point3f viewPoint(0,0,0);
6013  _localMapMaker->createLocalMap(*s, ground, obstacles, empty, viewPoint);
6014  cellSize = _localMapMaker->getCellSize();
6015  s->sensorData().setOccupancyGrid(ground, obstacles, empty, cellSize, viewPoint);
6016 
6017  t = timer.ticks();
6018  if(stats) stats->addStatistic(Statistics::kTimingMemOccupancy_grid(), t*1000.0f);
6019  UDEBUG("time grid map = %fs", t);
6020  }
6021  else if(data.gridCellSize() != 0.0f)
6022  {
6023  s->sensorData().setOccupancyGrid(
6024  data.gridGroundCellsRaw(),
6025  data.gridObstacleCellsRaw(),
6026  data.gridEmptyCellsRaw(),
6027  data.gridCellSize(),
6028  data.gridViewPoint());
6029  }
6030  }
6031 
6032  // prior
6033  if(!data.globalPose().isNull() && data.globalPoseCovariance().cols==6 && data.globalPoseCovariance().rows==6 && data.globalPoseCovariance().cols==CV_64FC1)
6034  {
6035  s->addLink(Link(s->id(), s->id(), Link::kPosePrior, data.globalPose(), data.globalPoseCovariance().inv()));
6036  UDEBUG("Added global pose prior: %s", data.globalPose().prettyPrint().c_str());
6037 
6038  if(data.gps().stamp() > 0.0)
6039  {
6040  UWARN("GPS constraint ignored as global pose is also set.");
6041  }
6042  }
6043  else if(data.gps().stamp() > 0.0)
6044  {
6045  if(uIsFinite(data.gps().altitude()) &&
6046  uIsFinite(data.gps().latitude()) &&
6047  uIsFinite(data.gps().longitude()) &&
6048  uIsFinite(data.gps().bearing()) &&
6049  uIsFinite(data.gps().error()) &&
6050  data.gps().error() > 0.0)
6051  {
6052  if(_gpsOrigin.stamp() <= 0.0)
6053  {
6054  _gpsOrigin = data.gps();
6055  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());
6056  }
6057  cv::Point3f pt = data.gps().toGeodeticCoords().toENU_WGS84(_gpsOrigin.toGeodeticCoords());
6058  Transform gpsPose(pt.x, pt.y, pose.z(), 0, 0, -(data.gps().bearing()-90.0)*M_PI/180.0);
6059  cv::Mat gpsInfMatrix = cv::Mat::eye(6,6,CV_64FC1)/9999.0; // variance not used >= 9999
6060 
6061  UDEBUG("Added GPS prior: x=%f y=%f z=%f yaw=%f", gpsPose.x(), gpsPose.y(), gpsPose.z(), gpsPose.theta());
6062  // only set x, y as we don't know variance for other degrees of freedom.
6063  gpsInfMatrix.at<double>(0,0) = gpsInfMatrix.at<double>(1,1) = 1.0/data.gps().error();
6064  gpsInfMatrix.at<double>(2,2) = 1; // z variance is set to avoid issues with g2o and gtsam requiring a prior on Z
6065  s->addLink(Link(s->id(), s->id(), Link::kPosePrior, gpsPose, gpsInfMatrix));
6066  }
6067  else
6068  {
6069  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());
6070  }
6071  }
6072 
6073  // IMU / Gravity constraint
6074  if(_useOdometryGravity && !pose.isNull())
6075  {
6076  s->addLink(Link(s->id(), s->id(), Link::kGravity, pose.rotation()));
6077  UDEBUG("Added gravity constraint from odom pose: %s", pose.rotation().prettyPrint().c_str());
6078  }
6079  else if(!data.imu().localTransform().isNull() &&
6080  (data.imu().orientation()[0] != 0 ||
6081  data.imu().orientation()[1] != 0 ||
6082  data.imu().orientation()[2] != 0 ||
6083  data.imu().orientation()[3] != 0))
6084 
6085  {
6086  Transform orientation(0,0,0, data.imu().orientation()[0], data.imu().orientation()[1], data.imu().orientation()[2], data.imu().orientation()[3]);
6087  // orientation includes roll and pitch but not yaw in local transform
6088  orientation= Transform(0,0,data.imu().localTransform().theta()) * orientation * data.imu().localTransform().rotation().inverse();
6089 
6090  s->addLink(Link(s->id(), s->id(), Link::kGravity, orientation));
6091  UDEBUG("Added gravity constraint: %s", orientation.prettyPrint().c_str());
6092  }
6093 
6094  //landmarks
6095  for(Landmarks::const_iterator iter = landmarks.begin(); iter!=landmarks.end(); ++iter)
6096  {
6097  if(iter->second.id() > 0)
6098  {
6099  int landmarkId = -iter->first;
6100  cv::Mat landmarkSize;
6101  if(iter->second.size() > 0.0f)
6102  {
6103  landmarkSize = cv::Mat(1,1,CV_32FC1);
6104  landmarkSize.at<float>(0,0) = iter->second.size();
6105 
6106  std::pair<std::map<int, float>::iterator, bool> inserted=_landmarksSize.insert(std::make_pair(iter->first, iter->second.size()));
6107  if(!inserted.second)
6108  {
6109  if(inserted.first->second != landmarkSize.at<float>(0,0))
6110  {
6111  UWARN("Trying to update landmark size buffer for landmark %d with size=%f but "
6112  "it has already a different size set. Keeping old size (%f).",
6113  -landmarkId, inserted.first->second, landmarkSize.at<float>(0,0));
6114  }
6115  }
6116 
6117  }
6118 
6119  Transform landmarkPose = iter->second.pose();
6120  if(_registrationPipeline->force3DoF())
6121  {
6122  // For 2D slam, make sure the landmark z axis is up
6123  rtabmap::Transform tx = landmarkPose.rotation() * rtabmap::Transform(1,0,0,0,0,0);
6124  rtabmap::Transform ty = landmarkPose.rotation() * rtabmap::Transform(0,1,0,0,0,0);
6125  if(fabs(tx.z()) > 0.9)
6126  {
6127  landmarkPose*=rtabmap::Transform(0,0,0,0,(tx.z()>0?1:-1)*M_PI/2,0);
6128  }
6129  else if(fabs(ty.z()) > 0.9)
6130  {
6131  landmarkPose*=rtabmap::Transform(0,0,0,(ty.z()>0?-1:1)*M_PI/2,0,0);
6132  }
6133  }
6134 
6135  Link landmark(s->id(), landmarkId, Link::kLandmark, landmarkPose, iter->second.covariance().inv(), landmarkSize);
6136  s->addLandmark(landmark);
6137 
6138  // Update landmark index
6139  std::map<int, std::set<int> >::iterator nter = _landmarksIndex.find(landmarkId);
6140  if(nter!=_landmarksIndex.end())
6141  {
6142  nter->second.insert(s->id());
6143  }
6144  else
6145  {
6146  std::set<int> tmp;
6147  tmp.insert(s->id());
6148  _landmarksIndex.insert(std::make_pair(landmarkId, tmp));
6149  }
6150  }
6151  else
6152  {
6153  UERROR("Invalid landmark received! IDs should be > 0 (it is %d). Ignoring this landmark.", iter->second.id());
6154  }
6155  }
6156 
6157  return s;
6158 }
6159 
6160 void Memory::disableWordsRef(int signatureId)
6161 {
6162  UDEBUG("id=%d", signatureId);
6163 
6164  Signature * ss = this->_getSignature(signatureId);
6165  if(ss && ss->isEnabled())
6166  {
6167  const std::multimap<int, int> & words = ss->getWords();
6168  const std::list<int> & keys = uUniqueKeys(words);
6169  int count = _vwd->getTotalActiveReferences();
6170  // First remove all references
6171  for(std::list<int>::const_iterator i=keys.begin(); i!=keys.end(); ++i)
6172  {
6173  _vwd->removeAllWordRef(*i, signatureId);
6174  }
6175 
6176  count -= _vwd->getTotalActiveReferences();
6177  ss->setEnabled(false);
6178  UDEBUG("%d words total ref removed from signature %d... (total active ref = %d)", count, ss->id(), _vwd->getTotalActiveReferences());
6179  }
6180 }
6181 
6182 void Memory::cleanUnusedWords()
6183 {
6184  std::vector<VisualWord*> removedWords = _vwd->getUnusedWords();
6185  UDEBUG("Removing %d words (dictionary size=%d)...", removedWords.size(), _vwd->getVisualWords().size());
6186  if(removedWords.size())
6187  {
6188  // remove them from the dictionary
6189  _vwd->removeWords(removedWords);
6190 
6191  for(unsigned int i=0; i<removedWords.size(); ++i)
6192  {
6193  if(_dbDriver)
6194  {
6195  _dbDriver->asyncSave(removedWords[i]);
6196  }
6197  else
6198  {
6199  delete removedWords[i];
6200  }
6201  }
6202  }
6203 }
6204 
6205 void Memory::enableWordsRef(const std::list<int> & signatureIds)
6206 {
6207  UDEBUG("size=%d", signatureIds.size());
6208  UTimer timer;
6209  timer.start();
6210 
6211  std::map<int, int> refsToChange; //<oldWordId, activeWordId>
6212 
6213  std::set<int> oldWordIds;
6214  std::list<Signature *> surfSigns;
6215  for(std::list<int>::const_iterator i=signatureIds.begin(); i!=signatureIds.end(); ++i)
6216  {
6217  Signature * ss = dynamic_cast<Signature *>(this->_getSignature(*i));
6218  if(ss && !ss->isEnabled())
6219  {
6220  surfSigns.push_back(ss);
6221  std::list<int> uniqueKeys = uUniqueKeys(ss->getWords());
6222 
6223  //Find words in the signature which they are not in the current dictionary
6224  for(std::list<int>::const_iterator k=uniqueKeys.begin(); k!=uniqueKeys.end(); ++k)
6225  {
6226  if(*k>0 && _vwd->getWord(*k) == 0 && _vwd->getUnusedWord(*k) == 0)
6227  {
6228  oldWordIds.insert(oldWordIds.end(), *k);
6229  }
6230  }
6231  }
6232  }
6233 
6234  if(!_vwd->isIncremental() && oldWordIds.size())
6235  {
6236  UWARN("Dictionary is fixed, but some words retrieved have not been found!?");
6237  }
6238 
6239  UDEBUG("oldWordIds.size()=%d, getOldIds time=%fs", oldWordIds.size(), timer.ticks());
6240 
6241  // the words were deleted, so try to math it with an active word
6242  std::list<VisualWord *> vws;
6243  if(oldWordIds.size() && _dbDriver)
6244  {
6245  // get the descriptors
6246  _dbDriver->loadWords(oldWordIds, vws);
6247  }
6248  UDEBUG("loading words(%d) time=%fs", oldWordIds.size(), timer.ticks());
6249 
6250 
6251  if(vws.size())
6252  {
6253  //Search in the dictionary
6254  std::vector<int> vwActiveIds = _vwd->findNN(vws);
6255  UDEBUG("find active ids (number=%d) time=%fs", vws.size(), timer.ticks());
6256  int i=0;
6257  for(std::list<VisualWord *>::iterator iterVws=vws.begin(); iterVws!=vws.end(); ++iterVws)
6258  {
6259  if(vwActiveIds[i] > 0)
6260  {
6261  //UDEBUG("Match found %d with %d", (*iterVws)->id(), vwActiveIds[i]);
6262  refsToChange.insert(refsToChange.end(), std::pair<int, int>((*iterVws)->id(), vwActiveIds[i]));
6263  if((*iterVws)->isSaved())
6264  {
6265  delete (*iterVws);
6266  }
6267  else if(_dbDriver)
6268  {
6269  _dbDriver->asyncSave(*iterVws);
6270  }
6271  }
6272  else
6273  {
6274  //add to dictionary
6275  _vwd->addWord(*iterVws); // take ownership
6276  }
6277  ++i;
6278  }
6279  UDEBUG("Added %d to dictionary, time=%fs", vws.size()-refsToChange.size(), timer.ticks());
6280 
6281  //update the global references map and update the signatures reactivated
6282  for(std::map<int, int>::const_iterator iter=refsToChange.begin(); iter != refsToChange.end(); ++iter)
6283  {
6284  //uInsert(_wordRefsToChange, (const std::pair<int, int>)*iter); // This will be used to change references in the database
6285  for(std::list<Signature *>::iterator j=surfSigns.begin(); j!=surfSigns.end(); ++j)
6286  {
6287  (*j)->changeWordsRef(iter->first, iter->second);
6288  }
6289  }
6290  UDEBUG("changing ref, total=%d, time=%fs", refsToChange.size(), timer.ticks());
6291  }
6292 
6293  int count = _vwd->getTotalActiveReferences();
6294 
6295  // Reactivate references and signatures
6296  for(std::list<Signature *>::iterator j=surfSigns.begin(); j!=surfSigns.end(); ++j)
6297  {
6298  const std::vector<int> & keys = uKeys((*j)->getWords());
6299  if(keys.size())
6300  {
6301  // Add all references
6302  for(unsigned int i=0; i<keys.size(); ++i)
6303  {
6304  if(keys.at(i)>0)
6305  {
6306  _vwd->addWordRef(keys.at(i), (*j)->id());
6307  }
6308  }
6309  (*j)->setEnabled(true);
6310  }
6311  }
6312 
6313  count = _vwd->getTotalActiveReferences() - count;
6314  UDEBUG("%d words total ref added from %d signatures, time=%fs...", count, surfSigns.size(), timer.ticks());
6315 }
6316 
6317 std::set<int> Memory::reactivateSignatures(const std::list<int> & ids, unsigned int maxLoaded, double & timeDbAccess)
6318 {
6319  // get the signatures, if not in the working memory, they
6320  // will be loaded from the database in an more efficient way
6321  // than how it is done in the Memory
6322 
6323  UDEBUG("");
6324  UTimer timer;
6325  std::list<int> idsToLoad;
6326  std::map<int, int>::iterator wmIter;
6327  for(std::list<int>::const_iterator i=ids.begin(); i!=ids.end(); ++i)
6328  {
6329  if(!this->getSignature(*i) && !uContains(idsToLoad, *i))
6330  {
6331  if(!maxLoaded || idsToLoad.size() < maxLoaded)
6332  {
6333  idsToLoad.push_back(*i);
6334  UINFO("Loading location %d from database...", *i);
6335  }
6336  }
6337  }
6338 
6339  UDEBUG("idsToLoad = %d", idsToLoad.size());
6340 
6341  std::list<Signature *> reactivatedSigns;
6342  if(_dbDriver)
6343  {
6344  _dbDriver->loadSignatures(idsToLoad, reactivatedSigns);
6345  }
6346  timeDbAccess = timer.getElapsedTime();
6347  std::list<int> idsLoaded;
6348  for(std::list<Signature *>::iterator i=reactivatedSigns.begin(); i!=reactivatedSigns.end(); ++i)
6349  {
6350  if(!(*i)->getLandmarks().empty())
6351  {
6352  // Update landmark indexes
6353  for(std::map<int, Link>::const_iterator iter = (*i)->getLandmarks().begin(); iter!=(*i)->getLandmarks().end(); ++iter)
6354  {
6355  int landmarkId = iter->first;
6356  UASSERT(landmarkId < 0);
6357 
6358  cv::Mat landmarkSize = iter->second.uncompressUserDataConst();
6359  if(!landmarkSize.empty() && landmarkSize.type() == CV_32FC1 && landmarkSize.total()==1)
6360  {
6361  std::pair<std::map<int, float>::iterator, bool> inserted=_landmarksSize.insert(std::make_pair(-landmarkId, landmarkSize.at<float>(0,0)));
6362  if(!inserted.second)
6363  {
6364  if(inserted.first->second != landmarkSize.at<float>(0,0))
6365  {
6366  UWARN("Trying to update landmark size buffer for landmark %d with size=%f but "
6367  "it has already a different size set. Keeping old size (%f).",
6368  -landmarkId, inserted.first->second, landmarkSize.at<float>(0,0));
6369  }
6370  }
6371  }
6372 
6373  std::map<int, std::set<int> >::iterator nter = _landmarksIndex.find(landmarkId);
6374  if(nter!=_landmarksIndex.end())
6375  {
6376  nter->second.insert((*i)->id());
6377  }
6378  else
6379  {
6380  std::set<int> tmp;
6381  tmp.insert((*i)->id());
6382  _landmarksIndex.insert(std::make_pair(landmarkId, tmp));
6383  }
6384  }
6385  }
6386 
6387  idsLoaded.push_back((*i)->id());
6388  //append to working memory
6389  this->addSignatureToWmFromLTM(*i);
6390  }
6391  this->enableWordsRef(idsLoaded);
6392  UDEBUG("time = %fs", timer.ticks());
6393  return std::set<int>(idsToLoad.begin(), idsToLoad.end());
6394 }
6395 
6396 // return all non-null poses
6397 // return unique links between nodes (for neighbors: old->new, for loops: parent->child)
6398 void Memory::getMetricConstraints(
6399  const std::set<int> & ids,
6400  std::map<int, Transform> & poses,
6401  std::multimap<int, Link> & links,
6402  bool lookInDatabase,
6403  bool landmarksAdded)
6404 {
6405  UDEBUG("");
6406  for(std::set<int>::const_iterator iter=ids.begin(); iter!=ids.end(); ++iter)
6407  {
6408  Transform pose = getOdomPose(*iter, lookInDatabase);
6409  if(!pose.isNull())
6410  {
6411  poses.insert(std::make_pair(*iter, pose));
6412  }
6413  }
6414 
6415  for(std::set<int>::const_iterator iter=ids.begin(); iter!=ids.end(); ++iter)
6416  {
6417  if(uContains(poses, *iter))
6418  {
6419  std::multimap<int, Link> tmpLinks = getLinks(*iter, lookInDatabase, true);
6420  for(std::multimap<int, Link>::iterator jter=tmpLinks.begin(); jter!=tmpLinks.end(); ++jter)
6421  {
6422  std::multimap<int, Link>::iterator addedLinksIterator = graph::findLink(links, *iter, jter->first);
6423  if( jter->second.isValid() &&
6424  (addedLinksIterator == links.end() || addedLinksIterator->second.from()==addedLinksIterator->second.to()) &&
6425  (uContains(poses, jter->first) || (landmarksAdded && jter->second.type() == Link::kLandmark)))
6426  {
6427  if(!lookInDatabase &&
6428  (jter->second.type() == Link::kNeighbor ||
6429  jter->second.type() == Link::kNeighborMerged))
6430  {
6431  const Signature * s = this->getSignature(jter->first);
6432  UASSERT(s!=0);
6433  if(s->getWeight() == -1)
6434  {
6435  Link link = jter->second;
6436  while(s && s->getWeight() == -1)
6437  {
6438  // skip to next neighbor, well we assume that bad signatures
6439  // are only linked by max 2 neighbor links.
6440  std::multimap<int, Link> n = this->getNeighborLinks(s->id(), false);
6441  UASSERT(n.size() <= 2);
6442  std::multimap<int, Link>::iterator uter = n.upper_bound(s->id());
6443  if(uter != n.end())
6444  {
6445  const Signature * s2 = this->getSignature(uter->first);
6446  if(s2)
6447  {
6448  link = link.merge(uter->second, uter->second.type());
6449  poses.erase(s->id());
6450  s = s2;
6451  }
6452 
6453  }
6454  else
6455  {
6456  break;
6457  }
6458  }
6459  links.insert(std::make_pair(*iter, link));
6460  }
6461  else
6462  {
6463  links.insert(std::make_pair(*iter, jter->second));
6464  }
6465  }
6466  else if(jter->second.type() != Link::kLandmark)
6467  {
6468  links.insert(std::make_pair(*iter, jter->second));
6469  }
6470  else if(landmarksAdded)
6471  {
6472  if(!uContains(poses, jter->first))
6473  {
6474  poses.insert(std::make_pair(jter->first, poses.at(*iter) * jter->second.transform()));
6475  }
6476  links.insert(std::make_pair(jter->first, jter->second.inverse()));
6477  }
6478  }
6479  }
6480  }
6481  }
6482 }
6483 
6484 } // 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:4225
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:4096
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:569
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:4495
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:85
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:728
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:3561
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:3407
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:4063
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:6182
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:4493
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:119
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:108
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:3608
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:3989
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::util2d::rotateImagesUpsideUpIfNecessary
void 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:2282
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:4015
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:833
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:500
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:3754
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:225
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:4214
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:4505
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:4196
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:896
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:6160
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:3192
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:4489
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
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:4502
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:204
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:508
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:3552
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:206
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:3600
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:106
z
z
rtabmap::DBDriver::getDatabaseVersion
std::string getDatabaseVersion() const
Definition: DBDriver.cpp:275
rtabmap::Memory::getNi
int getNi(int signatureId) const
Definition: Memory.cpp:4440
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:108
x
x
m
Matrix3f m
UThread::start
void start()
Definition: UThread.cpp:122
rtabmap::Feature2D::getMaxDepth
float getMaxDepth() const
Definition: Features2d.h:207
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:4113
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:1024
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:849
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:3660
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:6205
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:4456
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:3591
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:1252
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:3718
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:4151
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:3976
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:4492
OptimizerG2O.h
rtabmap::Memory::updateLink
void updateLink(const Link &link, bool updateInDatabase=false)
Definition: Memory.cpp:3492
rtabmap::Memory::computeIcpTransformMulti
Transform computeIcpTransformMulti(int newId, int oldId, const std::map< int, Transform > &poses, RegistrationInfo *info=0)
Definition: Memory.cpp:3206
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:3811
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:1756
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:4002
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:119

rtabmap
Author(s): Mathieu Labbe
autogenerated on Thu Jul 25 2024 02:50:13