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

rtabmap
Author(s): Mathieu Labbe
autogenerated on Thu Feb 13 2025 03:44:56