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

rtabmap
Author(s): Mathieu Labbe
autogenerated on Mon Jan 23 2023 03:37:29