tools/Reprocess/main.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/core/Rtabmap.h>
29 #include <rtabmap/core/DBDriver.h>
30 #include <rtabmap/core/DBReader.h>
31 #ifdef RTABMAP_OCTOMAP
32 #include <rtabmap/core/OctoMap.h>
33 #endif
35 #include <rtabmap/core/Graph.h>
36 #include <rtabmap/core/Memory.h>
38 #include <rtabmap/utilite/UFile.h>
40 #include <rtabmap/utilite/UTimer.h>
41 #include <rtabmap/utilite/UStl.h>
42 #include <rtabmap/utilite/UMath.h>
43 #include <stdio.h>
44 #include <string.h>
45 #include <stdlib.h>
46 #include <pcl/io/pcd_io.h>
47 #include <signal.h>
48 
49 using namespace rtabmap;
50 
51 void showUsage()
52 {
53  printf("\nUsage:\n"
54  " rtabmap-reprocess [options] \"input.db\" \"output.db\"\n"
55  " rtabmap-reprocess [options] \"input1.db;input2.db;input3.db\" \"output.db\"\n"
56  " For the second example, only parameters from the first database are used.\n"
57  " If Mem/IncrementalMemory is false, RTAB-Map is initialized with the first input database.\n"
58  " To see warnings when loop closures are rejected, add \"--uwarn\" argument.\n"
59  " Options:\n"
60  " -r Use database stamps as input rate.\n"
61  " -skip # Skip # frames after each processed frame (default 0=don't skip any frames).\n"
62  " -c \"path.ini\" Configuration file, overwriting parameters read \n"
63  " from the database. If custom parameters are also set as \n"
64  " arguments, they overwrite those in config file and the database.\n"
65  " -start # Start from this node ID.\n"
66  " -stop # Last node to process.\n"
67  " -g2 Assemble 2D occupancy grid map and save it to \"[output]_map.pgm\".\n"
68  " -g3 Assemble 3D cloud map and save it to \"[output]_map.pcd\".\n"
69  " -o2 Assemble OctoMap 2D projection and save it to \"[output]_octomap.pgm\".\n"
70  " -o3 Assemble OctoMap 3D cloud and save it to \"[output]_octomap.pcd\".\n"
71  " -p Save odometry and localization poses (*.g2o).\n"
72  " -scan_from_depth Generate scans from depth images (overwrite previous\n"
73  " scans if they exist).\n"
74  " -scan_downsample # Downsample input scans.\n"
75  " -scan_range_min #.# Filter input scans with minimum range (m).\n"
76  " -scan_range_max #.# Filter input scans with maximum range (m).\n"
77  " -scan_voxel_size #.# Voxel filter input scans (m).\n"
78  " -scan_normal_k # Compute input scan normals (k-neighbors approach).\n"
79  " -scan_normal_radius #.# Compute input scan normals (radius(m)-neighbors approach).\n\n"
80  "%s\n"
81  "\n", Parameters::showUsage());
82  exit(1);
83 }
84 
85 // catch ctrl-c
86 bool g_loopForever = true;
87 void sighandler(int sig)
88 {
89  printf("\nSignal %d caught...\n", sig);
90  g_loopForever = false;
91 }
92 
93 int loopCount = 0;
94 int proxCount = 0;
96 int totalFrames = 0;
98 std::vector<float> previousLocalizationDistances;
99 std::vector<float> odomDistances;
100 std::vector<float> localizationVariations;
101 std::vector<float> localizationAngleVariations;
102 std::vector<float> localizationTime;
103 std::map<int, Transform> odomTrajectoryPoses;
104 std::multimap<int, Link> odomTrajectoryLinks;
105 std::map<int, Transform> localizationPoses;
106 bool exportPoses = false;
107 int sessionCount = 0;
108 void showLocalizationStats(const std::string & outputDatabasePath)
109 {
110  printf("Total localizations on previous session = %d/%d (Loop=%d, Prox=%d, In Motion=%d/%d)\n", loopCount+proxCount, totalFrames, loopCount, proxCount, loopCountMotion, totalFramesMotion);
111  {
112  float m = uMean(localizationTime);
113  float var = uVariance(localizationTime, m);
114  float stddev = -1;
115  if(var>0)
116  {
117  stddev = sqrt(var);
118  }
119  printf("Average localization time = %f ms (stddev=%f ms)\n", m, stddev);
120  }
121  if(localizationVariations.size()>=2)
122  {
123  //ignore first localization
124  localizationVariations = std::vector<float>(++localizationVariations.begin(), localizationVariations.end());
125  localizationAngleVariations = std::vector<float>(++localizationAngleVariations.begin(), localizationAngleVariations.end());
126 
127  float m = uMean(localizationVariations);
129  float var = uVariance(localizationVariations, m);
130  float stddev = -1;
131  if(var>0)
132  {
133  stddev = sqrt(var);
134  }
135  float mA = uMean(localizationAngleVariations);
136  float maxA = uMax(localizationAngleVariations);
137  float varA = uVariance(localizationAngleVariations, mA);
138  float stddevA = -1;
139  if(varA>0)
140  {
141  stddevA = sqrt(varA);
142  }
143  printf("Average localization variations = %f m, %f deg (stddev=%f m, %f deg) (max=%f m, %f deg)\n", m, mA, stddev, stddevA, max, maxA);
144  }
145  if(!previousLocalizationDistances.empty())
146  {
149  float stddev = -1;
150  if(var>0)
151  {
152  stddev = sqrt(var);
153  }
154  printf("Average distance from previous localization = %f m (stddev=%f m)\n", m, stddev);
155  }
156  if(!odomDistances.empty())
157  {
158  float m = uMean(odomDistances);
159  float var = uVariance(odomDistances, m);
160  float stddev = -1;
161  if(var>0)
162  {
163  stddev = sqrt(var);
164  }
165  printf("Average odometry distances = %f m (stddev=%f m)\n", m, stddev);
166  }
167 
168  if(exportPoses)
169  {
170  std::string outputPath = outputDatabasePath.substr(0, outputDatabasePath.size()-3);
171  std::string oName = outputPath+uFormat("_session_%d_odom.g2o", sessionCount);
172  std::string lName = outputPath+uFormat("_session_%d_loc.g2o", sessionCount);
175  printf("Exported %s and %s\n", oName.c_str(), lName.c_str());
176  }
177 
178  loopCount = 0;
179  proxCount = 0;
180  totalFrames = 0;
181  loopCountMotion = 0;
182  totalFramesMotion = 0;
184  odomDistances.clear();
185  localizationVariations.clear();
187  localizationTime.clear();
188  odomTrajectoryPoses.clear();
189  odomTrajectoryLinks.clear();
190  localizationPoses.clear();
191  ++sessionCount;
192 }
193 
194 int main(int argc, char * argv[])
195 {
196  signal(SIGABRT, &sighandler);
197  signal(SIGTERM, &sighandler);
198  signal(SIGINT, &sighandler);
199 
202 
203  ParametersMap customParameters = Parameters::parseArguments(argc, argv);
204 
205  if(argc < 3)
206  {
207  showUsage();
208  }
209 
210  bool assemble2dMap = false;
211  bool assemble3dMap = false;
212  bool assemble2dOctoMap = false;
213  bool assemble3dOctoMap = false;
214  bool useDatabaseRate = false;
215  int startId = 0;
216  int stopId = 0;
217  int framesToSkip = 0;
218  bool scanFromDepth = false;
219  int scanDecimation = 1;
220  float scanRangeMin = 0.0f;
221  float scanRangeMax = 0.0f;
222  float scanVoxelSize = 0;
223  int scanNormalK = 0;
224  float scanNormalRadius = 0.0f;
225  ParametersMap configParameters;
226  for(int i=1; i<argc-2; ++i)
227  {
228  if(strcmp(argv[i], "-r") == 0 || strcmp(argv[i], "--r") == 0)
229  {
230  useDatabaseRate = true;
231  printf("Using database stamps as input rate.\n");
232  }
233  else if (strcmp(argv[i], "-c") == 0 || strcmp(argv[i], "--c") == 0)
234  {
235  ++i;
236  if (i < argc - 2 && UFile::exists(argv[i]) && UFile::getExtension(argv[i]).compare("ini") == 0)
237  {
238  Parameters::readINI(argv[i], configParameters);
239  printf("Using %d parameters from config file \"%s\"\n", (int)configParameters.size(), argv[i]);
240  }
241  else if(i < argc - 2)
242  {
243  printf("Config file \"%s\" is not valid or doesn't exist!\n", argv[i]);
244  showUsage();
245  }
246  else
247  {
248  printf("Config file is not set!\n");
249  showUsage();
250  }
251  }
252  else if (strcmp(argv[i], "-start") == 0 || strcmp(argv[i], "--start") == 0)
253  {
254  ++i;
255  if(i < argc - 2)
256  {
257  startId = atoi(argv[i]);
258  printf("Start at node ID = %d.\n", startId);
259  }
260  else
261  {
262  printf("-start option requires a value\n");
263  showUsage();
264  }
265  }
266  else if (strcmp(argv[i], "-stop") == 0 || strcmp(argv[i], "--stop") == 0)
267  {
268  ++i;
269  if(i < argc - 2)
270  {
271  stopId = atoi(argv[i]);
272  printf("Stop at node ID = %d.\n", stopId);
273  }
274  else
275  {
276  printf("-stop option requires a value\n");
277  showUsage();
278  }
279  }
280  else if (strcmp(argv[i], "-skip") == 0 || strcmp(argv[i], "--skip") == 0)
281  {
282  ++i;
283  if(i < argc - 2)
284  {
285  framesToSkip = atoi(argv[i]);
286  printf("Will skip %d frames.\n", framesToSkip);
287  }
288  else
289  {
290  printf("-skip option requires a value\n");
291  showUsage();
292  }
293  }
294  else if(strcmp(argv[i], "-p") == 0 || strcmp(argv[i], "--p") == 0)
295  {
296  exportPoses = true;
297  printf("Odometry trajectory and localization poses will be exported in g2o format (-p option).\n");
298  }
299  else if(strcmp(argv[i], "-g2") == 0 || strcmp(argv[i], "--g2") == 0)
300  {
301  assemble2dMap = true;
302  printf("2D occupancy grid will be assembled (-g2 option).\n");
303  }
304  else if(strcmp(argv[i], "-g3") == 0 || strcmp(argv[i], "--g3") == 0)
305  {
306  assemble3dMap = true;
307  printf("3D cloud map will be assembled (-g3 option).\n");
308  }
309  else if(strcmp(argv[i], "-o2") == 0 || strcmp(argv[i], "--o2") == 0)
310  {
311 #ifdef RTABMAP_OCTOMAP
312  assemble2dOctoMap = true;
313  printf("OctoMap will be assembled (-o2 option).\n");
314 #else
315  printf("RTAB-Map is not built with OctoMap support, cannot set -o2 option!\n");
316 #endif
317  }
318  else if(strcmp(argv[i], "-o3") == 0 || strcmp(argv[i], "--o3") == 0)
319  {
320 #ifdef RTABMAP_OCTOMAP
321  assemble3dOctoMap = true;
322  printf("OctoMap will be assembled (-o3 option).\n");
323 #else
324  printf("RTAB-Map is not built with OctoMap support, cannot set -o3 option!\n");
325 #endif
326  }
327  else if (strcmp(argv[i], "-scan_from_depth") == 0 || strcmp(argv[i], "--scan_from_depth") == 0)
328  {
329  scanFromDepth = true;
330  }
331  else if (strcmp(argv[i], "-scan_downsample") == 0 || strcmp(argv[i], "--scan_downsample") == 0 ||
332  strcmp(argv[i], "-scan_decimation") == 0 || strcmp(argv[i], "--scan_decimation") == 0)
333  {
334  ++i;
335  if(i < argc - 2)
336  {
337  scanDecimation = atoi(argv[i]);
338  printf("Scan from depth decimation = %d.\n", scanDecimation);
339  }
340  else
341  {
342  printf("-scan_downsample option requires 1 value\n");
343  showUsage();
344  }
345  }
346  else if (strcmp(argv[i], "-scan_range_min") == 0 || strcmp(argv[i], "--scan_range_min") == 0)
347  {
348  ++i;
349  if(i < argc - 2)
350  {
351  scanRangeMin = atof(argv[i]);
352  printf("Scan range min = %f m.\n", scanRangeMin);
353  }
354  else
355  {
356  printf("-scan_range_min option requires 1 value\n");
357  showUsage();
358  }
359  }
360  else if (strcmp(argv[i], "-scan_range_max") == 0 || strcmp(argv[i], "--scan_range_max") == 0)
361  {
362  ++i;
363  if(i < argc - 2)
364  {
365  scanRangeMax = atof(argv[i]);
366  printf("Scan range max = %f m.\n", scanRangeMax);
367  }
368  else
369  {
370  printf("-scan_range_max option requires 1 value\n");
371  showUsage();
372  }
373  }
374  else if (strcmp(argv[i], "-scan_voxel_size") == 0 || strcmp(argv[i], "--scan_voxel_size") == 0)
375  {
376  ++i;
377  if(i < argc - 2)
378  {
379  scanVoxelSize = atof(argv[i]);
380  printf("Scan voxel size = %f m.\n", scanVoxelSize);
381  }
382  else
383  {
384  printf("-scan_voxel_size option requires 1 value\n");
385  showUsage();
386  }
387  }
388  else if (strcmp(argv[i], "-scan_normal_k") == 0 || strcmp(argv[i], "--scan_normal_k") == 0)
389  {
390  ++i;
391  if(i < argc - 2)
392  {
393  scanNormalK = atoi(argv[i]);
394  printf("Scan normal k = %d.\n", scanNormalK);
395  }
396  else
397  {
398  printf("-scan_normal_k option requires 1 value\n");
399  showUsage();
400  }
401  }
402  else if (strcmp(argv[i], "-scan_normal_radius") == 0 || strcmp(argv[i], "--scan_normal_radius") == 0)
403  {
404  ++i;
405  if(i < argc - 2)
406  {
407  scanNormalRadius = atof(argv[i]);
408  printf("Scan normal radius = %f m.\n", scanNormalRadius);
409  }
410  else
411  {
412  printf("-scan_normal_radius option requires 1 value\n");
413  showUsage();
414  }
415  }
416  }
417 
418  std::string inputDatabasePath = uReplaceChar(argv[argc-2], '~', UDirectory::homeDir());
419  std::string outputDatabasePath = uReplaceChar(argv[argc-1], '~', UDirectory::homeDir());
420 
421  std::list<std::string> databases = uSplit(inputDatabasePath, ';');
422  if (databases.empty())
423  {
424  printf("No input database \"%s\" detected!\n", inputDatabasePath.c_str());
425  return -1;
426  }
427  for (std::list<std::string>::iterator iter = databases.begin(); iter != databases.end(); ++iter)
428  {
429  if (!UFile::exists(*iter))
430  {
431  printf("Input database \"%s\" doesn't exist!\n", iter->c_str());
432  return -1;
433  }
434 
435  if (UFile::getExtension(*iter).compare("db") != 0)
436  {
437  printf("File \"%s\" is not a database format (*.db)!\n", iter->c_str());
438  return -1;
439  }
440  }
441 
442  if(UFile::getExtension(outputDatabasePath).compare("db") != 0)
443  {
444  printf("File \"%s\" is not a database format (*.db)!\n", outputDatabasePath.c_str());
445  return -1;
446  }
447 
448  if(UFile::exists(outputDatabasePath))
449  {
450  UFile::erase(outputDatabasePath);
451  }
452 
453  // Get parameters of the first database
454  DBDriver * dbDriver = DBDriver::create();
455  if(!dbDriver->openConnection(databases.front(), false))
456  {
457  printf("Failed opening input database!\n");
458  delete dbDriver;
459  return -1;
460  }
461 
462  ParametersMap parameters = dbDriver->getLastParameters();
463  std::string targetVersion = dbDriver->getDatabaseVersion();
464  parameters.insert(ParametersPair(Parameters::kDbTargetVersion(), targetVersion));
465  if(parameters.empty())
466  {
467  printf("WARNING: Failed getting parameters from database, reprocessing will be done with default parameters! Database version may be too old (%s).\n", dbDriver->getDatabaseVersion().c_str());
468  }
469  if(customParameters.size())
470  {
471  printf("Custom parameters:\n");
472  for(ParametersMap::iterator iter=customParameters.begin(); iter!=customParameters.end(); ++iter)
473  {
474  printf(" %s\t= %s\n", iter->first.c_str(), iter->second.c_str());
475  }
476  }
477  if((configParameters.find(Parameters::kKpDetectorStrategy())!=configParameters.end() ||
478  configParameters.find(Parameters::kVisFeatureType())!=configParameters.end() ||
479  customParameters.find(Parameters::kKpDetectorStrategy())!=customParameters.end() ||
480  customParameters.find(Parameters::kVisFeatureType())!=customParameters.end()) &&
481  configParameters.find(Parameters::kMemUseOdomFeatures())==configParameters.end() &&
482  customParameters.find(Parameters::kMemUseOdomFeatures())==customParameters.end())
483  {
484  bool useOdomFeatures = Parameters::defaultMemUseOdomFeatures();
485  Parameters::parse(parameters, Parameters::kMemUseOdomFeatures(), useOdomFeatures);
486  if(useOdomFeatures)
487  {
488  printf("[Warning] %s and/or %s are overwritten but parameter %s is true in the opened database. "
489  "Setting it to false for convenience to use the new selected feature detector. Set %s "
490  "explicitly to suppress this warning.\n",
491  Parameters::kKpDetectorStrategy().c_str(),
492  Parameters::kVisFeatureType().c_str(),
493  Parameters::kMemUseOdomFeatures().c_str(),
494  Parameters::kMemUseOdomFeatures().c_str());
495  uInsert(parameters, ParametersPair(Parameters::kMemUseOdomFeatures(), "false"));
496  }
497  }
498  uInsert(parameters, configParameters);
499  uInsert(parameters, customParameters);
500 
501  bool incrementalMemory = Parameters::defaultMemIncrementalMemory();
502  Parameters::parse(parameters, Parameters::kMemIncrementalMemory(), incrementalMemory);
503  Parameters::parse(parameters, Parameters::kDbTargetVersion(), targetVersion);
504 
505  int totalIds = 0;
506  std::set<int> ids;
507  dbDriver->getAllNodeIds(ids);
508  if(ids.empty())
509  {
510  printf("Input database doesn't have any nodes saved in it.\n");
511  dbDriver->closeConnection(false);
512  delete dbDriver;
513  return -1;
514  }
515  if(!(!incrementalMemory && databases.size() > 1))
516  {
517  totalIds = ids.size();
518  }
519  dbDriver->closeConnection(false);
520 
521  // Count remaining ids in the other databases
522  for (std::list<std::string>::iterator iter = ++databases.begin(); iter != databases.end(); ++iter)
523  {
524  if (!dbDriver->openConnection(*iter, false))
525  {
526  printf("Failed opening input database!\n");
527  delete dbDriver;
528  return -1;
529  }
530  ids.clear();
531  dbDriver->getAllNodeIds(ids);
532  totalIds += ids.size();
533  dbDriver->closeConnection(false);
534  }
535  delete dbDriver;
536  dbDriver = 0;
537 
538  if(framesToSkip)
539  {
540  totalIds/=framesToSkip+1;
541  }
542 
543  std::string workingDirectory = UDirectory::getDir(outputDatabasePath);
544  printf("Set working directory to \"%s\".\n", workingDirectory.c_str());
545  if(!targetVersion.empty())
546  {
547  printf("Target database version: \"%s\" (set explicitly --%s \"\" to output with latest version.\n", targetVersion.c_str(), Parameters::kDbTargetVersion().c_str());
548  }
549  uInsert(parameters, ParametersPair(Parameters::kRtabmapWorkingDirectory(), workingDirectory));
550  uInsert(parameters, ParametersPair(Parameters::kRtabmapPublishStats(), "true")); // to log status below
551 
552  if(!incrementalMemory && databases.size() > 1)
553  {
554  UFile::copy(databases.front(), outputDatabasePath);
555  printf("Parameter \"%s\" is set to false, initializing RTAB-Map with \"%s\" for localization...\n", Parameters::kMemIncrementalMemory().c_str(), databases.front().c_str());
556  databases.pop_front();
557  inputDatabasePath = uJoin(databases, ";");
558  }
559 
561  rtabmap.init(parameters, outputDatabasePath);
562 
563  bool rgbdEnabled = Parameters::defaultRGBDEnabled();
564  Parameters::parse(parameters, Parameters::kRGBDEnabled(), rgbdEnabled);
565  bool odometryIgnored = !rgbdEnabled;
566  DBReader * dbReader = new DBReader(inputDatabasePath, useDatabaseRate?-1:0, odometryIgnored, false, false, startId, -1, stopId);
567  dbReader->init();
568 
569  OccupancyGrid grid(parameters);
570  grid.setCloudAssembling(assemble3dMap);
571 #ifdef RTABMAP_OCTOMAP
572  OctoMap octomap(parameters);
573 #endif
574 
575  float linearUpdate = Parameters::defaultRGBDLinearUpdate();
576  float angularUpdate = Parameters::defaultRGBDAngularUpdate();
577  Parameters::parse(parameters, Parameters::kRGBDLinearUpdate(), linearUpdate);
578  Parameters::parse(parameters, Parameters::kRGBDAngularUpdate(), angularUpdate);
579 
580  printf("Reprocessing data of \"%s\"...\n", inputDatabasePath.c_str());
581  std::map<std::string, float> globalMapStats;
582  int processed = 0;
583  CameraInfo info;
584  SensorData data = dbReader->takeImage(&info);
585  CameraThread camThread(dbReader, parameters); // take ownership of dbReader
586  camThread.setScanParameters(scanFromDepth, scanDecimation, scanRangeMin, scanRangeMax, scanVoxelSize, scanNormalK, scanNormalRadius);
587  if(scanFromDepth)
588  {
589  data.setLaserScan(LaserScan());
590  }
591  camThread.postUpdate(&data, &info);
592  Transform lastLocalizationOdomPose = info.odomPose;
593  bool inMotion = true;
594  while(data.isValid() && g_loopForever)
595  {
596  UTimer iterationTime;
597  std::string status;
598  if(!odometryIgnored && info.odomPose.isNull())
599  {
600  printf("Skipping node %d as it doesn't have odometry pose set.\n", data.id());
601  }
602  else
603  {
604  if(!odometryIgnored && !info.odomCovariance.empty() && info.odomCovariance.at<double>(0,0)>=9999)
605  {
606  printf("High variance detected, triggering a new map...\n");
607  if(!incrementalMemory && processed>0)
608  {
609  showLocalizationStats(outputDatabasePath);
610  lastLocalizationOdomPose = info.odomPose;
611  }
612  rtabmap.triggerNewMap();
613  inMotion = true;
614  }
615  UTimer t;
616  if(!rtabmap.process(data, info.odomPose, info.odomCovariance, info.odomVelocity, globalMapStats))
617  {
618  printf("Failed processing node %d.\n", data.id());
619  globalMapStats.clear();
620  }
621  else if(assemble2dMap || assemble3dMap || assemble2dOctoMap || assemble3dOctoMap)
622  {
623  globalMapStats.clear();
624  double timeRtabmap = t.ticks();
625  double timeUpdateInit = 0.0;
626  double timeUpdateGrid = 0.0;
627 #ifdef RTABMAP_OCTOMAP
628  double timeUpdateOctoMap = 0.0;
629 #endif
630  const rtabmap::Statistics & stats = rtabmap.getStatistics();
631  if(stats.poses().size() && stats.getLastSignatureData().id())
632  {
633  int id = stats.poses().rbegin()->first;
634  if(id == stats.getLastSignatureData().id() &&
635  stats.getLastSignatureData().sensorData().gridCellSize() > 0.0f)
636  {
637  bool updateGridMap = false;
638  bool updateOctoMap = false;
639  if((assemble2dMap || assemble3dMap) && grid.addedNodes().find(id) == grid.addedNodes().end())
640  {
641  updateGridMap = true;
642  }
643 #ifdef RTABMAP_OCTOMAP
644  if((assemble2dOctoMap || assemble3dOctoMap) && octomap.addedNodes().find(id) == octomap.addedNodes().end())
645  {
646  updateOctoMap = true;
647  }
648 #endif
649  if(updateGridMap || updateOctoMap)
650  {
651  cv::Mat ground, obstacles, empty;
652  stats.getLastSignatureData().sensorData().uncompressDataConst(0, 0, 0, 0, &ground, &obstacles, &empty);
653 
654  timeUpdateInit = t.ticks();
655 
656  if(updateGridMap)
657  {
658  grid.addToCache(id, ground, obstacles, empty);
659  grid.update(stats.poses());
660  timeUpdateGrid = t.ticks() + timeUpdateInit;
661  }
662 #ifdef RTABMAP_OCTOMAP
663  if(updateOctoMap)
664  {
665  const cv::Point3f & viewpoint = stats.getLastSignatureData().sensorData().gridViewPoint();
666  octomap.addToCache(id, ground, obstacles, empty, viewpoint);
667  octomap.update(stats.poses());
668  timeUpdateOctoMap = t.ticks() + timeUpdateInit;
669  }
670 #endif
671  }
672  }
673  }
674 
675  globalMapStats.insert(std::make_pair(std::string("GlobalGrid/GridUpdate/ms"), timeUpdateGrid*1000.0f));
676 #ifdef RTABMAP_OCTOMAP
677  //Simulate publishing
678  double timePub2dOctoMap = 0.0;
679  double timePub3dOctoMap = 0.0;
680  if(assemble2dOctoMap)
681  {
682  float xMin, yMin, size;
683  octomap.createProjectionMap(xMin, yMin, size);
684  timePub2dOctoMap = t.ticks();
685  }
686  if(assemble3dOctoMap)
687  {
688  octomap.createCloud();
689  timePub3dOctoMap = t.ticks();
690  }
691 
692  globalMapStats.insert(std::make_pair(std::string("GlobalGrid/OctoMapUpdate/ms"), timeUpdateOctoMap*1000.0f));
693  globalMapStats.insert(std::make_pair(std::string("GlobalGrid/OctoMapProjection/ms"), timePub2dOctoMap*1000.0f));
694  globalMapStats.insert(std::make_pair(std::string("GlobalGrid/OctomapToCloud/ms"), timePub3dOctoMap*1000.0f));
695  globalMapStats.insert(std::make_pair(std::string("GlobalGrid/TotalWithRtabmap/ms"), (timeUpdateGrid+timeUpdateOctoMap+timePub2dOctoMap+timePub3dOctoMap+timeRtabmap)*1000.0f));
696 #else
697  globalMapStats.insert(std::make_pair(std::string("GlobalGrid/TotalWithRtabmap/ms"), (timeUpdateGrid+timeRtabmap)*1000.0f));
698 #endif
699  }
700  }
701 
702  const rtabmap::Statistics & stats = rtabmap.getStatistics();
703  int refId = stats.refImageId();
704  int loopId = stats.loopClosureId() > 0? stats.loopClosureId(): stats.proximityDetectionId() > 0?stats.proximityDetectionId() :0;
705  int landmarkId = (int)uValue(stats.data(), rtabmap::Statistics::kLoopLandmark_detected(), 0.0f);
706  int refMapId = stats.refImageMapId();
707  ++totalFrames;
708 
709  if(inMotion)
710  {
712  }
713  if (loopId>0)
714  {
715  if(stats.loopClosureId()>0)
716  {
717  ++loopCount;
718  }
719  else
720  {
721  ++proxCount;
722  }
723  if(inMotion)
724  {
725  ++loopCountMotion;
726  }
727  int loopMapId = stats.loopClosureId() > 0? stats.loopClosureMapId(): stats.proximityDetectionMapId();
728  printf("Processed %d/%d nodes [id=%d map=%d]... %dms %s on %d [%d]\n", ++processed, totalIds, refId, refMapId, int(iterationTime.ticks() * 1000), stats.loopClosureId() > 0?"Loop":"Prox", loopId, loopMapId);
729  }
730  else if(landmarkId != 0)
731  {
732  printf("Processed %d/%d nodes [id=%d map=%d]... %dms Loop on landmark %d\n", ++processed, totalIds, refId, refMapId, int(iterationTime.ticks() * 1000), landmarkId);
733  }
734  else
735  {
736  printf("Processed %d/%d nodes [id=%d map=%d]... %dms\n", ++processed, totalIds, refId, refMapId, int(iterationTime.ticks() * 1000));
737  }
738 
739  // Here we accumulate statistics about distance from last localization
740  if(!incrementalMemory &&
741  !lastLocalizationOdomPose.isNull() &&
742  !info.odomPose.isNull())
743  {
744  if(loopId>0 || landmarkId != 0)
745  {
746  previousLocalizationDistances.push_back(lastLocalizationOdomPose.getDistance(info.odomPose));
747  lastLocalizationOdomPose = info.odomPose;
748  }
749  }
750  if(!incrementalMemory)
751  {
752  float totalTime = uValue(stats.data(), rtabmap::Statistics::kTimingTotal(), 0.0f);
753  localizationTime.push_back(totalTime);
754  if(stats.data().find(Statistics::kLoopOdom_correction_norm()) != stats.data().end())
755  {
756  localizationVariations.push_back(stats.data().at(Statistics::kLoopOdom_correction_norm()));
757  localizationAngleVariations.push_back(stats.data().at(Statistics::kLoopOdom_correction_angle()));
758  }
759 
760  if(exportPoses && !info.odomPose.isNull())
761  {
762  if(!odomTrajectoryPoses.empty())
763  {
764  int previousId = odomTrajectoryPoses.rbegin()->first;
765  odomTrajectoryLinks.insert(std::make_pair(previousId, Link(previousId, refId, Link::kNeighbor, odomTrajectoryPoses.rbegin()->second.inverse()*info.odomPose, info.odomCovariance)));
766  }
767  odomTrajectoryPoses.insert(std::make_pair(refId, info.odomPose));
768  localizationPoses.insert(std::make_pair(refId, stats.mapCorrection()*info.odomPose));
769  }
770  }
771 
772  Transform odomPose = info.odomPose;
773 
774  if(framesToSkip>0)
775  {
776  int skippedFrames = framesToSkip;
777  while(skippedFrames-- > 0)
778  {
779  data = dbReader->takeImage(&info);
780  if(!odometryIgnored && !info.odomCovariance.empty() && info.odomCovariance.at<double>(0,0)>=9999)
781  {
782  printf("High variance detected, triggering a new map...\n");
783  if(!incrementalMemory && processed>0)
784  {
785  showLocalizationStats(outputDatabasePath);
786  lastLocalizationOdomPose = info.odomPose;
787  }
788  rtabmap.triggerNewMap();
789  }
790  }
791  }
792 
793  data = dbReader->takeImage(&info);
794  if(scanFromDepth)
795  {
796  data.setLaserScan(LaserScan());
797  }
798  camThread.postUpdate(&data, &info);
799 
800  inMotion = true;
801  if(!incrementalMemory &&
802  !odomPose.isNull() &&
803  !info.odomPose.isNull())
804  {
805  float distance = odomPose.getDistance(info.odomPose);
806  float angle = (odomPose.inverse()*info.odomPose).getAngle();
807  odomDistances.push_back(distance);
808  if(distance < linearUpdate && angle <= angularUpdate)
809  {
810  inMotion = false;
811  }
812  }
813  }
814 
815  int databasesMerged = 0;
816  if(!incrementalMemory)
817  {
818  showLocalizationStats(outputDatabasePath);
819  }
820  else
821  {
822  printf("Total loop closures = %d (Loop=%d, Prox=%d, In Motion=%d/%d)\n", loopCount+proxCount, loopCount, proxCount, loopCountMotion, totalFramesMotion);
823 
824  if(databases.size()>1)
825  {
826  std::map<int, Transform> poses;
827  std::multimap<int, Link> constraints;
828  rtabmap.getGraph(poses, constraints, 0, 1, 0, false, false, false, false, false, false);
829  std::set<int> mapIds;
830  for(std::map<int, Transform>::iterator iter=poses.begin(); iter!=poses.end(); ++iter)
831  {
832  int id;
833  if((id=rtabmap.getMemory()->getMapId(iter->first, true))>=0)
834  {
835  mapIds.insert(id);
836  }
837  }
838  databasesMerged = mapIds.size();
839  }
840  }
841 
842  printf("Closing database \"%s\"...\n", outputDatabasePath.c_str());
843  rtabmap.close(true);
844  printf("Closing database \"%s\"... done!\n", outputDatabasePath.c_str());
845 
846  if(assemble2dMap)
847  {
848  std::string outputPath = outputDatabasePath.substr(0, outputDatabasePath.size()-3) + "_map.pgm";
849  float xMin,yMin;
850  cv::Mat map = grid.getMap(xMin, yMin);
851  if(!map.empty())
852  {
853  cv::Mat map8U(map.rows, map.cols, CV_8U);
854  //convert to gray scaled map
855  for (int i = 0; i < map.rows; ++i)
856  {
857  for (int j = 0; j < map.cols; ++j)
858  {
859  char v = map.at<char>(i, j);
860  unsigned char gray;
861  if(v == 0)
862  {
863  gray = 178;
864  }
865  else if(v == 100)
866  {
867  gray = 0;
868  }
869  else // -1
870  {
871  gray = 89;
872  }
873  map8U.at<unsigned char>(i, j) = gray;
874  }
875  }
876  if(cv::imwrite(outputPath, map8U))
877  {
878  printf("Saving occupancy grid \"%s\"... done!\n", outputPath.c_str());
879  }
880  else
881  {
882  printf("Saving occupancy grid \"%s\"... failed!\n", outputPath.c_str());
883  }
884  }
885  else
886  {
887  printf("2D map is empty! Cannot save it!\n");
888  }
889  }
890  if(assemble3dMap)
891  {
892  std::string outputPath = outputDatabasePath.substr(0, outputDatabasePath.size()-3) + "_obstacles.pcd";
893  if(pcl::io::savePCDFileBinary(outputPath, *grid.getMapObstacles()) == 0)
894  {
895  printf("Saving 3d obstacles \"%s\"... done!\n", outputPath.c_str());
896  }
897  else
898  {
899  printf("Saving 3d obstacles \"%s\"... failed!\n", outputPath.c_str());
900  }
901  if(grid.getMapGround()->size())
902  {
903  outputPath = outputDatabasePath.substr(0, outputDatabasePath.size()-3) + "_ground.pcd";
904  if(pcl::io::savePCDFileBinary(outputPath, *grid.getMapGround()) == 0)
905  {
906  printf("Saving 3d ground \"%s\"... done!\n", outputPath.c_str());
907  }
908  else
909  {
910  printf("Saving 3d ground \"%s\"... failed!\n", outputPath.c_str());
911  }
912  }
913  if(grid.getMapEmptyCells()->size())
914  {
915  outputPath = outputDatabasePath.substr(0, outputDatabasePath.size()-3) + "_empty.pcd";
916  if(pcl::io::savePCDFileBinary(outputPath, *grid.getMapEmptyCells()) == 0)
917  {
918  printf("Saving 3d empty cells \"%s\"... done!\n", outputPath.c_str());
919  }
920  else
921  {
922  printf("Saving 3d empty cells \"%s\"... failed!\n", outputPath.c_str());
923  }
924  }
925  }
926 #ifdef RTABMAP_OCTOMAP
927  if(assemble2dOctoMap)
928  {
929  std::string outputPath = outputDatabasePath.substr(0, outputDatabasePath.size()-3) + "_octomap.pgm";
930  float xMin,yMin,cellSize;
931  cv::Mat map = octomap.createProjectionMap(xMin, yMin, cellSize);
932  if(!map.empty())
933  {
934  cv::Mat map8U(map.rows, map.cols, CV_8U);
935  //convert to gray scaled map
936  for (int i = 0; i < map.rows; ++i)
937  {
938  for (int j = 0; j < map.cols; ++j)
939  {
940  char v = map.at<char>(i, j);
941  unsigned char gray;
942  if(v == 0)
943  {
944  gray = 178;
945  }
946  else if(v == 100)
947  {
948  gray = 0;
949  }
950  else // -1
951  {
952  gray = 89;
953  }
954  map8U.at<unsigned char>(i, j) = gray;
955  }
956  }
957  if(cv::imwrite(outputPath, map8U))
958  {
959  printf("Saving octomap 2D projection \"%s\"... done!\n", outputPath.c_str());
960  }
961  else
962  {
963  printf("Saving octomap 2D projection \"%s\"... failed!\n", outputPath.c_str());
964  }
965  }
966  else
967  {
968  printf("OctoMap 2D projection map is empty! Cannot save it!\n");
969  }
970  }
971  if(assemble3dOctoMap)
972  {
973  std::string outputPath = outputDatabasePath.substr(0, outputDatabasePath.size()-3) + "_octomap_occupied.pcd";
974  std::vector<int> obstacles, emptySpace, ground;
975  pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = octomap.createCloud(0, &obstacles, &emptySpace, &ground);
976  if(pcl::io::savePCDFile(outputPath, *cloud, obstacles, true) == 0)
977  {
978  printf("Saving obstacles cloud \"%s\"... done!\n", outputPath.c_str());
979  }
980  else
981  {
982  printf("Saving obstacles cloud \"%s\"... failed!\n", outputPath.c_str());
983  }
984  if(ground.size())
985  {
986  outputPath = outputDatabasePath.substr(0, outputDatabasePath.size()-3) + "_octomap_ground.pcd";
987  if(pcl::io::savePCDFile(outputPath, *cloud, ground, true) == 0)
988  {
989  printf("Saving empty space cloud \"%s\"... done!\n", outputPath.c_str());
990  }
991  else
992  {
993  printf("Saving empty space cloud \"%s\"... failed!\n", outputPath.c_str());
994  }
995  }
996  if(emptySpace.size())
997  {
998  outputPath = outputDatabasePath.substr(0, outputDatabasePath.size()-3) + "_octomap_empty.pcd";
999  if(pcl::io::savePCDFile(outputPath, *cloud, emptySpace, true) == 0)
1000  {
1001  printf("Saving empty space cloud \"%s\"... done!\n", outputPath.c_str());
1002  }
1003  else
1004  {
1005  printf("Saving empty space cloud \"%s\"... failed!\n", outputPath.c_str());
1006  }
1007  }
1008  }
1009 #endif
1010 
1011  return databasesMerged;
1012 }
static std::string homeDir()
Definition: UDirectory.cpp:355
T uVariance(const T *v, unsigned int size, T meanV)
Definition: UMath.h:491
static bool parse(const ParametersMap &parameters, const std::string &key, bool &value)
Definition: Parameters.cpp:476
bool update(const std::map< int, Transform > &poses)
const pcl::PointCloud< pcl::PointXYZRGB >::Ptr & getMapEmptyCells() const
void uncompressDataConst(cv::Mat *imageRaw, cv::Mat *depthOrRightRaw, LaserScan *laserScanRaw=0, cv::Mat *userDataRaw=0, cv::Mat *groundCellsRaw=0, cv::Mat *obstacleCellsRaw=0, cv::Mat *emptyCellsRaw=0) const
Definition: SensorData.cpp:579
cv::Mat odomCovariance
Definition: CameraInfo.h:70
Definition: UTimer.h:46
T uMean(const T *v, unsigned int size)
Definition: UMath.h:401
bool exportPoses
virtual bool init(const std::string &calibrationFolder=".", const std::string &cameraName="")
Definition: DBReader.cpp:112
int proxCount
void setLaserScan(const LaserScan &laserScan, bool clearPreviousData=true)
Definition: SensorData.cpp:337
f
int refImageMapId() const
Definition: Statistics.h:258
static const char * showUsage()
Definition: Parameters.cpp:548
std::vector< float > localizationAngleVariations
GLM_FUNC_DECL vecType< T, P > sqrt(vecType< T, P > const &x)
void getAllNodeIds(std::set< int > &ids, bool ignoreChildren=false, bool ignoreBadSignatures=false) const
Definition: DBDriver.cpp:876
int loopClosureMapId() const
Definition: Statistics.h:260
static std::string getDir(const std::string &filePath)
Definition: UDirectory.cpp:273
int proximityDetectionId() const
Definition: Statistics.h:261
std::pair< std::string, std::string > ParametersPair
Definition: Parameters.h:44
const Signature & getLastSignatureData() const
Definition: Statistics.h:265
static ParametersMap parseArguments(int argc, char *argv[], bool onlyParameters=false)
Definition: Parameters.cpp:575
static int erase(const std::string &filePath)
Definition: UFile.cpp:58
int main(int argc, char *argv[])
std::map< std::string, std::string > ParametersMap
Definition: Parameters.h:43
GLM_FUNC_DECL genType::value_type distance(genType const &p0, genType const &p1)
float gridCellSize() const
Definition: SensorData.h:245
void init(const ParametersMap &parameters, const std::string &databasePath="", bool loadDatabaseParameters=false)
Definition: Rtabmap.cpp:292
Basic mathematics functions.
GLM_FUNC_DECL T angle(detail::tquat< T, P > const &x)
std::vector< float > odomVelocity
Definition: CameraInfo.h:71
std::string getExtension()
Definition: UFile.h:140
static void setLevel(ULogger::Level level)
Definition: ULogger.h:339
std::vector< float > localizationVariations
int triggerNewMap()
Definition: Rtabmap.cpp:737
int totalFramesMotion
void getGraph(std::map< int, Transform > &poses, std::multimap< int, Link > &constraints, bool optimized, bool global, std::map< int, Signature > *signatures=0, bool withImages=false, bool withScan=false, bool withUserData=false, bool withGrid=false, bool withWords=true, bool withGlobalDescriptors=true) const
Definition: Rtabmap.cpp:4543
std::list< std::string > uSplit(const std::string &str, char separator= ' ')
Definition: UStl.h:566
std::vector< float > odomDistances
bool openConnection(const std::string &url, bool overwritten=false)
Definition: DBDriver.cpp:86
bool isValid() const
Definition: SensorData.h:137
bool update(const std::map< int, Transform > &poses)
Definition: OctoMap.cpp:382
SensorData takeImage(CameraInfo *info=0)
Definition: Camera.cpp:72
Wrappers of STL for convenient functions.
const std::map< std::string, float > & data() const
Definition: Statistics.h:282
void addToCache(int nodeId, const pcl::PointCloud< pcl::PointXYZRGB >::Ptr &ground, const pcl::PointCloud< pcl::PointXYZRGB >::Ptr &obstacles, const pcl::PointXYZ &viewPoint)
Definition: OctoMap.cpp:348
std::string uJoin(const std::list< std::string > &strings, const std::string &separator="")
Definition: UStl.h:603
const std::map< int, Transform > & addedNodes() const
Definition: OctoMap.h:175
void close(bool databaseSaved=true, const std::string &ouputDatabasePath="")
Definition: Rtabmap.cpp:392
bool isNull() const
Definition: Transform.cpp:107
std::vector< float > previousLocalizationDistances
void closeConnection(bool save=true, const std::string &outputUrl="")
Definition: DBDriver.cpp:64
void addToCache(int nodeId, const cv::Mat &ground, const cv::Mat &obstacles, const cv::Mat &empty)
void setScanParameters(bool fromDepth, int downsampleStep=1, float rangeMin=0.0f, float rangeMax=0.0f, float voxelSize=0.0f, int normalsK=0, int normalsRadius=0.0f, float groundNormalsUp=0.0f)
const Statistics & getStatistics() const
Definition: Rtabmap.cpp:699
static void setType(Type type, const std::string &fileName=kDefaultLogFileName, bool append=true)
Definition: ULogger.cpp:176
bool process(const SensorData &data, Transform odomPose, const cv::Mat &odomCovariance=cv::Mat::eye(6, 6, CV_64FC1), const std::vector< float > &odomVelocity=std::vector< float >(), const std::map< std::string, float > &externalStats=std::map< std::string, float >())
Main loop of rtabmap.
Definition: Rtabmap.cpp:992
static void copy(const std::string &from, const std::string &to)
Definition: UFile.cpp:97
int proximityDetectionMapId() const
Definition: Statistics.h:262
std::string UTILITE_EXP uReplaceChar(const std::string &str, char before, char after)
Definition: UConversion.cpp:32
int id() const
Definition: Signature.h:70
int id() const
Definition: SensorData.h:155
int loopClosureId() const
Definition: Statistics.h:259
const pcl::PointCloud< pcl::PointXYZRGB >::Ptr & getMapGround() const
int totalFrames
bool RTABMAP_EXP exportPoses(const std::string &filePath, int format, const std::map< int, Transform > &poses, const std::multimap< int, Link > &constraints=std::multimap< int, Link >(), const std::map< int, double > &stamps=std::map< int, double >(), const ParametersMap &parameters=ParametersMap())
Definition: Graph.cpp:54
static DBDriver * create(const ParametersMap &parameters=ParametersMap())
Definition: DBDriver.cpp:41
bool g_loopForever
int loopCount
T uMax(const T *v, unsigned int size, unsigned int &index)
Definition: UMath.h:94
const std::map< int, Transform > & poses() const
Definition: Statistics.h:267
void sighandler(int sig)
pcl::PointCloud< pcl::PointXYZRGB >::Ptr createCloud(unsigned int treeDepth=0, std::vector< int > *obstacleIndices=0, std::vector< int > *emptyIndices=0, std::vector< int > *groundIndices=0, bool originalRefPoints=true, std::vector< int > *frontierIndices=0, std::vector< double > *cloudProb=0) const
Definition: OctoMap.cpp:890
float getDistance(const Transform &t) const
Definition: Transform.cpp:262
GLM_FUNC_DECL genType max(genType const &x, genType const &y)
SensorData & sensorData()
Definition: Signature.h:137
bool exists()
Definition: UFile.h:104
void postUpdate(SensorData *data, CameraInfo *info=0) const
void showLocalizationStats(const std::string &outputDatabasePath)
ParametersMap getLastParameters() const
Definition: DBDriver.cpp:239
std::vector< float > localizationTime
const Memory * getMemory() const
Definition: Rtabmap.h:146
double ticks()
Definition: UTimer.cpp:117
std::string getDatabaseVersion() const
Definition: DBDriver.cpp:275
std::map< int, Transform > odomTrajectoryPoses
static void readINI(const std::string &configFile, ParametersMap &parameters, bool modifiedOnly=false)
std::map< int, Transform > localizationPoses
Transform inverse() const
Definition: Transform.cpp:178
int sessionCount
cv::Mat createProjectionMap(float &xMin, float &yMin, float &gridCellSize, float minGridSize=0.0f, unsigned int treeDepth=0)
Definition: OctoMap.cpp:1053
const pcl::PointCloud< pcl::PointXYZRGB >::Ptr & getMapObstacles() const
const Transform & mapCorrection() const
Definition: Statistics.h:269
std::multimap< int, Link > odomTrajectoryLinks
Transform odomPose
Definition: CameraInfo.h:69
void showUsage()
int loopCountMotion
std::string UTILITE_EXP uFormat(const char *fmt,...)
int refImageId() const
Definition: Statistics.h:257
cv::Mat getMap(float &xMin, float &yMin) const
const std::map< int, Transform > & addedNodes() const
Definition: OccupancyGrid.h:65
const cv::Point3f & gridViewPoint() const
Definition: SensorData.h:246
V uValue(const std::map< K, V > &m, const K &key, const V &defaultValue=V())
Definition: UStl.h:240
void setCloudAssembling(bool enabled)
void uInsert(std::map< K, V > &map, const std::pair< K, V > &pair)
Definition: UStl.h:443
int getMapId(int id, bool lookInDatabase=false) const
Definition: Memory.cpp:3776


rtabmap
Author(s): Mathieu Labbe
autogenerated on Mon Dec 14 2020 03:34:59