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