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
34 #include <rtabmap/core/OccupancyGrid.h>
35 #include <rtabmap/core/Graph.h>
36 #include <rtabmap/core/Memory.h>
37 #include <rtabmap/core/CameraThread.h>
38 #include <rtabmap/core/Odometry.h>
39 #include <rtabmap/core/OdometryInfo.h>
40 #include <rtabmap/utilite/UFile.h>
41 #include <rtabmap/utilite/UDirectory.h>
42 #include <rtabmap/utilite/UTimer.h>
43 #include <rtabmap/utilite/UStl.h>
44 #include <rtabmap/utilite/UMath.h>
45 #include <stdio.h>
46 #include <string.h>
47 #include <stdlib.h>
48 #include <pcl/io/pcd_io.h>
49 #include <signal.h>
50 
51 using namespace rtabmap;
52 
53 void showUsage()
54 {
55  printf("\nUsage:\n"
56  " rtabmap-reprocess [options] \"input.db\" \"output.db\"\n"
57  " rtabmap-reprocess [options] \"input1.db;input2.db;input3.db\" \"output.db\"\n"
58  "\n"
59  " For the second example, only parameters from the first database are used.\n"
60  " If Mem/IncrementalMemory is false, RTAB-Map is initialized with the first input database,\n"
61  " then localization-only is done with next databases against the first one.\n"
62  " To see warnings when loop closures are rejected, add \"--uwarn\" argument.\n"
63  " To upgrade version of an old database to newest version:\n"
64  " rtabmap-reprocess --Db/TargetVersion \"\" \"input.db\" \"output.db\"\n"
65  "\n"
66  " Options:\n"
67  " -r Use database stamps as input rate.\n"
68  " -skip # Skip # frames after each processed frame (default 0=don't skip any frames).\n"
69  " -c \"path.ini\" Configuration file, overwriting parameters read \n"
70  " from the database. If custom parameters are also set as \n"
71  " arguments, they overwrite those in config file and the database.\n"
72  " -default Input database's parameters are ignored, using default ones instead.\n"
73  " -odom Recompute odometry. See \"Odom/\" parameters with --params. If -skip option\n"
74  " is used, it will be applied to odometry frames, not rtabmap frames. Multi-session\n"
75  " cannot be detected in this mode (assuming the database contains continuous frames\n"
76  " of a single session).\n"
77  " -start # Start from this node ID.\n"
78  " -stop # Last node to process.\n"
79  " -start_s # Start from this map session ID.\n"
80  " -stop_s # Last map session to process.\n"
81  " -a Append mode: if Mem/IncrementalMemory is true, RTAB-Map is initialized with the first input database,\n"
82  " then next databases are reprocessed on top of the first one.\n"
83  " -cam # Camera index to stream. Ignored if a database doesn't contain multi-camera data.\n"
84  " -nolandmark Don't republish landmarks contained in input database.\n"
85  " -pub_loops Republish loop closures contained in input database.\n"
86  " -loc_null On localization mode, reset localization pose to null and map correction to identity between sessions.\n"
87  " -gt When reprocessing a single database, load its original optimized graph, then \n"
88  " set it as ground truth for output database. If there was a ground truth in the input database, it will be ignored.\n"
89  " -g2 Assemble 2D occupancy grid map and save it to \"[output]_map.pgm\". Use with -db to save in database.\n"
90  " -g3 Assemble 3D cloud map and save it to \"[output]_map.pcd\".\n"
91  " -o2 Assemble OctoMap 2D projection and save it to \"[output]_octomap.pgm\". Use with -db to save in database.\n"
92  " -o3 Assemble OctoMap 3D cloud and save it to \"[output]_octomap.pcd\".\n"
93  " -db Save assembled 2D occupancy grid in database instead of a file.\n"
94  " -p Save odometry and localization poses (*.g2o).\n"
95  " -scan_from_depth Generate scans from depth images (overwrite previous\n"
96  " scans if they exist).\n"
97  " -scan_downsample # Downsample input scans.\n"
98  " -scan_range_min #.# Filter input scans with minimum range (m).\n"
99  " -scan_range_max #.# Filter input scans with maximum range (m).\n"
100  " -scan_voxel_size #.# Voxel filter input scans (m).\n"
101  " -scan_normal_k # Compute input scan normals (k-neighbors approach).\n"
102  " -scan_normal_radius #.# Compute input scan normals (radius(m)-neighbors approach).\n\n"
103  "%s\n"
104  "\n", Parameters::showUsage());
105  exit(1);
106 }
107 
108 // catch ctrl-c
109 bool g_loopForever = true;
110 void sighandler(int sig)
111 {
112  printf("\nSignal %d caught...\n", sig);
113  g_loopForever = false;
114 }
115 
116 int loopCount = 0;
117 int proxCount = 0;
118 int loopCountMotion = 0;
119 int totalFrames = 0;
120 int totalFramesMotion = 0;
121 std::vector<float> previousLocalizationDistances;
122 std::vector<float> odomDistances;
123 std::vector<float> localizationVariations;
124 std::vector<float> localizationAngleVariations;
125 std::vector<float> localizationTime;
126 std::map<int, Transform> odomTrajectoryPoses;
127 std::multimap<int, Link> odomTrajectoryLinks;
128 std::map<int, Transform> localizationPoses;
129 bool exportPoses = false;
130 int sessionCount = 0;
131 void showLocalizationStats(const std::string & outputDatabasePath)
132 {
133  printf("Total localizations on previous session = %d/%d (Loop=%d, Prox=%d, In Motion=%d/%d)\n", loopCount+proxCount, totalFrames, loopCount, proxCount, loopCountMotion, totalFramesMotion);
134  {
135  float m = uMean(localizationTime);
136  float var = uVariance(localizationTime, m);
137  float stddev = -1;
138  if(var>0)
139  {
140  stddev = sqrt(var);
141  }
142  printf("Average localization time = %f ms (stddev=%f ms)\n", m, stddev);
143  }
144  if(localizationVariations.size()>=2)
145  {
146  //ignore first localization
147  localizationVariations = std::vector<float>(++localizationVariations.begin(), localizationVariations.end());
148  localizationAngleVariations = std::vector<float>(++localizationAngleVariations.begin(), localizationAngleVariations.end());
149 
150  float m = uMean(localizationVariations);
151  float max = uMax(localizationVariations);
152  float var = uVariance(localizationVariations, m);
153  float stddev = -1;
154  if(var>0)
155  {
156  stddev = sqrt(var);
157  }
158  float mA = uMean(localizationAngleVariations);
159  float maxA = uMax(localizationAngleVariations);
160  float varA = uVariance(localizationAngleVariations, mA);
161  float stddevA = -1;
162  if(varA>0)
163  {
164  stddevA = sqrt(varA);
165  }
166  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);
167  }
168  if(!previousLocalizationDistances.empty())
169  {
170  float m = uMean(previousLocalizationDistances);
171  float var = uVariance(previousLocalizationDistances, m);
172  float stddev = -1;
173  if(var>0)
174  {
175  stddev = sqrt(var);
176  }
177  printf("Average distance from previous localization = %f m (stddev=%f m)\n", m, stddev);
178  }
179  if(!odomDistances.empty())
180  {
181  float m = uMean(odomDistances);
182  float var = uVariance(odomDistances, m);
183  float stddev = -1;
184  if(var>0)
185  {
186  stddev = sqrt(var);
187  }
188  printf("Average odometry distances = %f m (stddev=%f m)\n", m, stddev);
189  }
190 
191  if(exportPoses)
192  {
193  std::string outputPath = outputDatabasePath.substr(0, outputDatabasePath.size()-3);
194  std::string oName = outputPath+uFormat("_session_%d_odom.g2o", sessionCount);
195  std::string lName = outputPath+uFormat("_session_%d_loc.g2o", sessionCount);
196  graph::exportPoses(oName, 4, odomTrajectoryPoses, odomTrajectoryLinks);
197  graph::exportPoses(lName, 4, localizationPoses, odomTrajectoryLinks);
198  printf("Exported %s and %s\n", oName.c_str(), lName.c_str());
199  }
200 
201  loopCount = 0;
202  proxCount = 0;
203  totalFrames = 0;
204  loopCountMotion = 0;
205  totalFramesMotion = 0;
206  previousLocalizationDistances.clear();
207  odomDistances.clear();
208  localizationVariations.clear();
209  localizationAngleVariations.clear();
210  localizationTime.clear();
211  odomTrajectoryPoses.clear();
212  odomTrajectoryLinks.clear();
213  localizationPoses.clear();
214  ++sessionCount;
215 }
216 
217 int main(int argc, char * argv[])
218 {
219  signal(SIGABRT, &sighandler);
220  signal(SIGTERM, &sighandler);
221  signal(SIGINT, &sighandler);
222 
223  ULogger::setType(ULogger::kTypeConsole);
224  ULogger::setLevel(ULogger::kError);
225 
226  ParametersMap customParameters = Parameters::parseArguments(argc, argv);
227 
228  if(argc < 3)
229  {
230  showUsage();
231  }
232 
233  bool save2DMap = false;
234  bool assemble2dMap = false;
235  bool assemble3dMap = false;
236  bool assemble2dOctoMap = false;
237  bool assemble3dOctoMap = false;
238  bool useDatabaseRate = false;
239  bool useDefaultParameters = false;
240  bool recomputeOdometry = false;
241  int startId = 0;
242  int stopId = 0;
243  int startMapId = 0;
244  int stopMapId = -1;
245  bool appendMode = false;
246  int cameraIndex = -1;
247  int framesToSkip = 0;
248  bool ignoreLandmarks = false;
249  bool republishLoopClosures = false;
250  bool locNull = false;
251  bool originalGraphAsGT = false;
252  bool scanFromDepth = false;
253  int scanDecimation = 1;
254  float scanRangeMin = 0.0f;
255  float scanRangeMax = 0.0f;
256  float scanVoxelSize = 0;
257  int scanNormalK = 0;
258  float scanNormalRadius = 0.0f;
259  ParametersMap configParameters;
260  for(int i=1; i<argc-2; ++i)
261  {
262  if(strcmp(argv[i], "-r") == 0 || strcmp(argv[i], "--r") == 0)
263  {
264  useDatabaseRate = true;
265  printf("Using database stamps as input rate.\n");
266  }
267  else if (strcmp(argv[i], "-c") == 0 || strcmp(argv[i], "--c") == 0)
268  {
269  ++i;
270  if (i < argc - 2 && UFile::exists(argv[i]) && UFile::getExtension(argv[i]).compare("ini") == 0)
271  {
272  Parameters::readINI(argv[i], configParameters);
273  printf("Using %d parameters from config file \"%s\"\n", (int)configParameters.size(), argv[i]);
274  }
275  else if(i < argc - 2)
276  {
277  printf("Config file \"%s\" is not valid or doesn't exist!\n", argv[i]);
278  showUsage();
279  }
280  else
281  {
282  printf("Config file is not set!\n");
283  showUsage();
284  }
285  }
286  else if(strcmp(argv[i], "-default") == 0 || strcmp(argv[i], "--default") == 0)
287  {
288  useDefaultParameters = true;
289  printf("Using default parameters.\n");
290  }
291  else if(strcmp(argv[i], "-odom") == 0 || strcmp(argv[i], "--odom") == 0)
292  {
293  recomputeOdometry = true;
294  }
295  else if (strcmp(argv[i], "-start") == 0 || strcmp(argv[i], "--start") == 0)
296  {
297  ++i;
298  if(i < argc - 2)
299  {
300  startId = atoi(argv[i]);
301  printf("Start at node ID = %d.\n", startId);
302  }
303  else
304  {
305  printf("-start option requires a value\n");
306  showUsage();
307  }
308  }
309  else if (strcmp(argv[i], "-stop") == 0 || strcmp(argv[i], "--stop") == 0)
310  {
311  ++i;
312  if(i < argc - 2)
313  {
314  stopId = atoi(argv[i]);
315  printf("Stop at node ID = %d.\n", stopId);
316  }
317  else
318  {
319  printf("-stop option requires a value\n");
320  showUsage();
321  }
322  }
323  else if (strcmp(argv[i], "-start_s") == 0 || strcmp(argv[i], "--start_s") == 0)
324  {
325  ++i;
326  if(i < argc - 2)
327  {
328  startMapId = atoi(argv[i]);
329  printf("Start at map session ID = %d.\n", startMapId);
330  }
331  else
332  {
333  printf("-start_s option requires a value\n");
334  showUsage();
335  }
336  }
337  else if (strcmp(argv[i], "-stop_s") == 0 || strcmp(argv[i], "--stop_s") == 0)
338  {
339  ++i;
340  if(i < argc - 2)
341  {
342  stopMapId = atoi(argv[i]);
343  printf("Stop at map session ID = %d.\n", stopMapId);
344  }
345  else
346  {
347  printf("-stop option requires a value\n");
348  showUsage();
349  }
350  }
351  else if (strcmp(argv[i], "-a") == 0 || strcmp(argv[i], "--a") == 0)
352  {
353  appendMode = true;
354  printf("Append mode enabled (initialize with first database then reprocess next ones)\n");
355  }
356  else if (strcmp(argv[i], "-cam") == 0 || strcmp(argv[i], "--cam") == 0)
357  {
358  ++i;
359  if(i < argc - 2)
360  {
361  cameraIndex = atoi(argv[i]);
362  printf("Camera index = %d.\n", cameraIndex);
363  }
364  else
365  {
366  printf("-cam option requires a value\n");
367  showUsage();
368  }
369  }
370  else if (strcmp(argv[i], "-skip") == 0 || strcmp(argv[i], "--skip") == 0)
371  {
372  ++i;
373  if(i < argc - 2)
374  {
375  framesToSkip = atoi(argv[i]);
376  printf("Will skip %d frames.\n", framesToSkip);
377  }
378  else
379  {
380  printf("-skip option requires a value\n");
381  showUsage();
382  }
383  }
384  else if(strcmp(argv[i], "-nolandmark") == 0 || strcmp(argv[i], "--nolandmark") == 0)
385  {
386  ignoreLandmarks = true;
387  printf("Ignoring landmarks from input database (-nolandmark option).\n");
388  }
389  else if(strcmp(argv[i], "-pub_loops") == 0 || strcmp(argv[i], "--pub_loops") == 0)
390  {
391  republishLoopClosures = true;
392  printf("Republish loop closures from input database (-pub_loops option).\n");
393  }
394  else if(strcmp(argv[i], "-loc_null") == 0 || strcmp(argv[i], "--loc_null") == 0)
395  {
396  locNull = true;
397  printf("In localization mode, when restarting a new session, the current localization pose is set to null (-loc_null option).\n");
398  }
399  else if(strcmp(argv[i], "-gt") == 0 || strcmp(argv[i], "--gt") == 0)
400  {
401  originalGraphAsGT = true;
402  printf("Original graph is used as ground truth for output database (-gt option).\n");
403  }
404  else if(strcmp(argv[i], "-p") == 0 || strcmp(argv[i], "--p") == 0)
405  {
406  exportPoses = true;
407  printf("Odometry trajectory and localization poses will be exported in g2o format (-p option).\n");
408  }
409  else if(strcmp(argv[i], "-db") == 0 || strcmp(argv[i], "--db") == 0)
410  {
411  save2DMap = true;
412  printf("2D occupancy grid will be saved in database (-db option).\n");
413  }
414  else if(strcmp(argv[i], "-g2") == 0 || strcmp(argv[i], "--g2") == 0)
415  {
416  assemble2dMap = true;
417  printf("2D occupancy grid will be assembled (-g2 option).\n");
418  }
419  else if(strcmp(argv[i], "-g3") == 0 || strcmp(argv[i], "--g3") == 0)
420  {
421  assemble3dMap = true;
422  printf("3D cloud map will be assembled (-g3 option).\n");
423  }
424  else if(strcmp(argv[i], "-o2") == 0 || strcmp(argv[i], "--o2") == 0)
425  {
426 #ifdef RTABMAP_OCTOMAP
427  assemble2dOctoMap = true;
428  printf("OctoMap will be assembled (-o2 option).\n");
429 #else
430  printf("RTAB-Map is not built with OctoMap support, cannot set -o2 option!\n");
431 #endif
432  }
433  else if(strcmp(argv[i], "-o3") == 0 || strcmp(argv[i], "--o3") == 0)
434  {
435 #ifdef RTABMAP_OCTOMAP
436  assemble3dOctoMap = true;
437  printf("OctoMap will be assembled (-o3 option).\n");
438 #else
439  printf("RTAB-Map is not built with OctoMap support, cannot set -o3 option!\n");
440 #endif
441  }
442  else if (strcmp(argv[i], "-scan_from_depth") == 0 || strcmp(argv[i], "--scan_from_depth") == 0)
443  {
444  scanFromDepth = true;
445  }
446  else if (strcmp(argv[i], "-scan_downsample") == 0 || strcmp(argv[i], "--scan_downsample") == 0 ||
447  strcmp(argv[i], "-scan_decimation") == 0 || strcmp(argv[i], "--scan_decimation") == 0)
448  {
449  ++i;
450  if(i < argc - 2)
451  {
452  scanDecimation = atoi(argv[i]);
453  printf("Scan from depth decimation = %d.\n", scanDecimation);
454  }
455  else
456  {
457  printf("-scan_downsample option requires 1 value\n");
458  showUsage();
459  }
460  }
461  else if (strcmp(argv[i], "-scan_range_min") == 0 || strcmp(argv[i], "--scan_range_min") == 0)
462  {
463  ++i;
464  if(i < argc - 2)
465  {
466  scanRangeMin = atof(argv[i]);
467  printf("Scan range min = %f m.\n", scanRangeMin);
468  }
469  else
470  {
471  printf("-scan_range_min option requires 1 value\n");
472  showUsage();
473  }
474  }
475  else if (strcmp(argv[i], "-scan_range_max") == 0 || strcmp(argv[i], "--scan_range_max") == 0)
476  {
477  ++i;
478  if(i < argc - 2)
479  {
480  scanRangeMax = atof(argv[i]);
481  printf("Scan range max = %f m.\n", scanRangeMax);
482  }
483  else
484  {
485  printf("-scan_range_max option requires 1 value\n");
486  showUsage();
487  }
488  }
489  else if (strcmp(argv[i], "-scan_voxel_size") == 0 || strcmp(argv[i], "--scan_voxel_size") == 0)
490  {
491  ++i;
492  if(i < argc - 2)
493  {
494  scanVoxelSize = atof(argv[i]);
495  printf("Scan voxel size = %f m.\n", scanVoxelSize);
496  }
497  else
498  {
499  printf("-scan_voxel_size option requires 1 value\n");
500  showUsage();
501  }
502  }
503  else if (strcmp(argv[i], "-scan_normal_k") == 0 || strcmp(argv[i], "--scan_normal_k") == 0)
504  {
505  ++i;
506  if(i < argc - 2)
507  {
508  scanNormalK = atoi(argv[i]);
509  printf("Scan normal k = %d.\n", scanNormalK);
510  }
511  else
512  {
513  printf("-scan_normal_k option requires 1 value\n");
514  showUsage();
515  }
516  }
517  else if (strcmp(argv[i], "-scan_normal_radius") == 0 || strcmp(argv[i], "--scan_normal_radius") == 0)
518  {
519  ++i;
520  if(i < argc - 2)
521  {
522  scanNormalRadius = atof(argv[i]);
523  printf("Scan normal radius = %f m.\n", scanNormalRadius);
524  }
525  else
526  {
527  printf("-scan_normal_radius option requires 1 value\n");
528  showUsage();
529  }
530  }
531  }
532 
533  std::string inputDatabasePath = uReplaceChar(argv[argc-2], '~', UDirectory::homeDir());
534  std::string outputDatabasePath = uReplaceChar(argv[argc-1], '~', UDirectory::homeDir());
535 
536  std::list<std::string> databases = uSplit(inputDatabasePath, ';');
537  if (databases.empty())
538  {
539  printf("No input database \"%s\" detected!\n", inputDatabasePath.c_str());
540  return -1;
541  }
542  for (std::list<std::string>::iterator iter = databases.begin(); iter != databases.end(); ++iter)
543  {
544  if (!UFile::exists(*iter))
545  {
546  printf("Input database \"%s\" doesn't exist!\n", iter->c_str());
547  return -1;
548  }
549 
550  if (UFile::getExtension(*iter).compare("db") != 0)
551  {
552  printf("File \"%s\" is not a database format (*.db)!\n", iter->c_str());
553  return -1;
554  }
555  }
556 
557  if(UFile::getExtension(outputDatabasePath).compare("db") != 0)
558  {
559  printf("File \"%s\" is not a database format (*.db)!\n", outputDatabasePath.c_str());
560  return -1;
561  }
562 
563  if(UFile::exists(outputDatabasePath))
564  {
565  UFile::erase(outputDatabasePath);
566  }
567 
568  // Get parameters of the first database
569  DBDriver * dbDriver = DBDriver::create();
570  if(!dbDriver->openConnection(databases.front(), false))
571  {
572  printf("Failed opening input database!\n");
573  delete dbDriver;
574  return -1;
575  }
576 
577  ParametersMap parameters;
578  std::string targetVersion;
579  if(!useDefaultParameters)
580  {
581  parameters = dbDriver->getLastParameters();
582  targetVersion = dbDriver->getDatabaseVersion();
583  parameters.insert(ParametersPair(Parameters::kDbTargetVersion(), targetVersion));
584  if(parameters.empty())
585  {
586  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());
587  }
588  }
589 
590  if(customParameters.size())
591  {
592  printf("Custom parameters:\n");
593  for(ParametersMap::iterator iter=customParameters.begin(); iter!=customParameters.end(); ++iter)
594  {
595  printf(" %s\t= %s\n", iter->first.c_str(), iter->second.c_str());
596  }
597  }
598 
599  bool useOdomFeatures = Parameters::defaultMemUseOdomFeatures();
600  if((configParameters.find(Parameters::kKpDetectorStrategy())!=configParameters.end() ||
601  configParameters.find(Parameters::kVisFeatureType())!=configParameters.end() ||
602  customParameters.find(Parameters::kKpDetectorStrategy())!=customParameters.end() ||
603  customParameters.find(Parameters::kVisFeatureType())!=customParameters.end()) &&
604  configParameters.find(Parameters::kMemUseOdomFeatures())==configParameters.end() &&
605  customParameters.find(Parameters::kMemUseOdomFeatures())==customParameters.end())
606  {
607  Parameters::parse(parameters, Parameters::kMemUseOdomFeatures(), useOdomFeatures);
608  if(useOdomFeatures)
609  {
610  printf("[Warning] %s and/or %s are overwritten but parameter %s is true in the opened database. "
611  "Setting it to false for convenience to use the new selected feature detector. Set %s "
612  "explicitly to suppress this warning.\n",
613  Parameters::kKpDetectorStrategy().c_str(),
614  Parameters::kVisFeatureType().c_str(),
615  Parameters::kMemUseOdomFeatures().c_str(),
616  Parameters::kMemUseOdomFeatures().c_str());
617  uInsert(parameters, ParametersPair(Parameters::kMemUseOdomFeatures(), "false"));
618  useOdomFeatures = false;
619  }
620  }
621 
622  if(useOdomFeatures && databases.size() > 1 &&
623  configParameters.find(Parameters::kMemUseOdomFeatures())==configParameters.end() &&
624  customParameters.find(Parameters::kMemUseOdomFeatures())==customParameters.end())
625  {
626  printf("[Warning] Parameter %s is set to false for convenience as "
627  "there are more than one input database (which could "
628  "contain different features). Set %s "
629  "explicitly to suppress this warning.\n",
630  Parameters::kMemUseOdomFeatures().c_str(),
631  Parameters::kMemUseOdomFeatures().c_str());
632  useOdomFeatures = false;
633  }
634 
635  if(republishLoopClosures)
636  {
637  if(databases.size() > 1)
638  {
639  printf("[Warning] \"pub_loops\" option cannot be used with multiple databases input. "
640  "Disabling \"pub_loops\" to avoid mismatched loop closue ids.\n");
641  republishLoopClosures = false;
642  }
643  else
644  {
645  bool generateIds = Parameters::defaultMemGenerateIds();
646  Parameters::parse(parameters, Parameters::kMemGenerateIds(), generateIds);
647  Parameters::parse(configParameters, Parameters::kMemGenerateIds(), generateIds);
648  Parameters::parse(customParameters, Parameters::kMemGenerateIds(), generateIds);
649  if(generateIds)
650  {
651  if(configParameters.find(Parameters::kMemGenerateIds())!=configParameters.end() ||
652  customParameters.find(Parameters::kMemGenerateIds())!=customParameters.end())
653  {
654  printf("[Warning] \"pub_loops\" option is used but parameter %s is set to true in custom arguments. "
655  "Disabling \"pub_loops\" to avoid mismatched loop closure ids.\n",
656  Parameters::kMemGenerateIds().c_str());
657  republishLoopClosures = false;
658  }
659  else
660  {
661  printf("[Warning] \"pub_loops\" option is used but parameter %s is true in the opened database. "
662  "Setting parameter %s to false for convenience so that republished loop closure ids match.\n",
663  Parameters::kMemGenerateIds().c_str(),
664  Parameters::kMemGenerateIds().c_str());
665  uInsert(parameters, ParametersPair(Parameters::kMemGenerateIds(), "false"));
666  }
667  }
668  }
669  }
670  uInsert(parameters, configParameters);
671  uInsert(parameters, customParameters);
672 
673  bool incrementalMemory = Parameters::defaultMemIncrementalMemory();
674  Parameters::parse(parameters, Parameters::kMemIncrementalMemory(), incrementalMemory);
675  Parameters::parse(parameters, Parameters::kDbTargetVersion(), targetVersion);
676  bool intermediateNodes = Parameters::defaultRtabmapCreateIntermediateNodes();
677  Parameters::parse(parameters, Parameters::kRtabmapCreateIntermediateNodes(), intermediateNodes);
678 
679  int totalIds = 0;
680  std::set<int> ids;
681  dbDriver->getAllNodeIds(ids, false, false, !intermediateNodes);
682  if(ids.empty())
683  {
684  printf("Input database doesn't have any nodes saved in it.\n");
685  dbDriver->closeConnection(false);
686  delete dbDriver;
687  return -1;
688  }
689  if(!((!incrementalMemory || appendMode) && databases.size() > 1))
690  {
691  totalIds = ids.size();
692  }
693 
694  std::map<int, Transform> gt;
695  if(databases.size() == 1 && originalGraphAsGT)
696  {
697  gt = dbDriver->loadOptimizedPoses();
698  }
699 
700  dbDriver->closeConnection(false);
701 
702  // Count remaining ids in the other databases
703  for (std::list<std::string>::iterator iter = ++databases.begin(); iter != databases.end(); ++iter)
704  {
705  if (!dbDriver->openConnection(*iter, false))
706  {
707  printf("Failed opening input database!\n");
708  delete dbDriver;
709  return -1;
710  }
711  ids.clear();
712  dbDriver->getAllNodeIds(ids, false, false, !intermediateNodes);
713  totalIds += ids.size();
714  dbDriver->closeConnection(false);
715  }
716  delete dbDriver;
717  dbDriver = 0;
718 
719  if(framesToSkip)
720  {
721  totalIds/=framesToSkip+1;
722  }
723 
724  std::string workingDirectory = UDirectory::getDir(outputDatabasePath);
725  printf("Set working directory to \"%s\".\n", workingDirectory.c_str());
726  if(!targetVersion.empty())
727  {
728  printf("Target database version: \"%s\" (set explicitly --%s \"\" to output with latest version.\n", targetVersion.c_str(), Parameters::kDbTargetVersion().c_str());
729  }
730  uInsert(parameters, ParametersPair(Parameters::kRtabmapWorkingDirectory(), workingDirectory));
731  uInsert(parameters, ParametersPair(Parameters::kRtabmapPublishStats(), "true")); // to log status below
732 
733  if((!incrementalMemory || appendMode ) && databases.size() > 1)
734  {
735  UFile::copy(databases.front(), outputDatabasePath);
736  if(!incrementalMemory)
737  {
738  printf("Parameter \"%s\" is set to false, initializing RTAB-Map with \"%s\" for localization...\n", Parameters::kMemIncrementalMemory().c_str(), databases.front().c_str());
739  }
740  databases.pop_front();
741  inputDatabasePath = uJoin(databases, ";");
742  }
743 
744  Rtabmap rtabmap;
745  rtabmap.init(parameters, outputDatabasePath);
746 
747  if(!incrementalMemory && locNull)
748  {
749  rtabmap.setInitialPose(Transform());
750  }
751 
752  bool rgbdEnabled = Parameters::defaultRGBDEnabled();
753  Parameters::parse(parameters, Parameters::kRGBDEnabled(), rgbdEnabled);
754  bool odometryIgnored = !rgbdEnabled;
755 
756  DBReader * dbReader = new DBReader(
757  inputDatabasePath,
758  useDatabaseRate?-1:0,
759  odometryIgnored,
760  false,
761  false,
762  startId,
763  cameraIndex,
764  stopId,
765  !intermediateNodes,
766  ignoreLandmarks,
767  !useOdomFeatures,
768  startMapId,
769  stopMapId);
770 
771  dbReader->init();
772 
773  OccupancyGrid grid(parameters);
774  grid.setCloudAssembling(assemble3dMap);
775 #ifdef RTABMAP_OCTOMAP
776  OctoMap octomap(parameters);
777 #endif
778 
779  float linearUpdate = Parameters::defaultRGBDLinearUpdate();
780  float angularUpdate = Parameters::defaultRGBDAngularUpdate();
781  Parameters::parse(parameters, Parameters::kRGBDLinearUpdate(), linearUpdate);
782  Parameters::parse(parameters, Parameters::kRGBDAngularUpdate(), angularUpdate);
783 
784  Odometry * odometry = 0;
785  float rtabmapUpdateRate = Parameters::defaultRtabmapDetectionRate();
786  double lastUpdateStamp = 0;
787  if(recomputeOdometry)
788  {
789  if(odometryIgnored)
790  {
791  printf("odom option is set but %s parameter is false, odometry won't be recomputed...\n", Parameters::kRGBDEnabled().c_str());
792  recomputeOdometry = false;
793  }
794  else
795  {
796  printf("Odometry will be recomputed (odom option is set)\n");
797  Parameters::parse(parameters, Parameters::kRtabmapDetectionRate(), rtabmapUpdateRate);
798  if(rtabmapUpdateRate!=0)
799  {
800  rtabmapUpdateRate = 1.0f/rtabmapUpdateRate;
801  }
802  odometry = Odometry::create(parameters);
803  }
804  }
805 
806  printf("Reprocessing data of \"%s\"...\n", inputDatabasePath.c_str());
807  std::map<std::string, float> globalMapStats;
808  int processed = 0;
809  CameraInfo info;
810  SensorData data = dbReader->takeImage(&info);
811  CameraThread camThread(dbReader, parameters); // take ownership of dbReader
812  camThread.setScanParameters(scanFromDepth, scanDecimation, scanRangeMin, scanRangeMax, scanVoxelSize, scanNormalK, scanNormalRadius);
813  if(scanFromDepth)
814  {
815  data.setLaserScan(LaserScan());
816  }
817  camThread.postUpdate(&data, &info);
818  Transform lastLocalizationOdomPose = info.odomPose;
819  bool inMotion = true;
820  while(data.isValid() && g_loopForever)
821  {
822  if(recomputeOdometry)
823  {
824  OdometryInfo odomInfo;
825  Transform pose = odometry->process(data, &odomInfo);
826  printf("Processed %d/%d frames (visual=%d/%d lidar=%f lost=%s)... odometry = %dms\n",
827  processed+1,
828  totalIds,
829  odomInfo.reg.inliers,
830  odomInfo.reg.matches,
831  odomInfo.reg.icpInliersRatio,
832  odomInfo.lost?"true":"false",
833  int(odomInfo.timeEstimation * 1000));
834  if(lastUpdateStamp > 0.0 && data.stamp() < lastUpdateStamp + rtabmapUpdateRate)
835  {
836  if(framesToSkip>0)
837  {
838  int skippedFrames = framesToSkip;
839  while(skippedFrames-- > 0)
840  {
841  ++processed;
842  data = dbReader->takeImage();
843  }
844  }
845 
846  data = dbReader->takeImage(&info);
847  if(scanFromDepth)
848  {
849  data.setLaserScan(LaserScan());
850  }
851  camThread.postUpdate(&data, &info);
852  ++processed;
853  continue;
854  }
855  info.odomPose = pose;
856  info.odomCovariance = odomInfo.reg.covariance;
857  lastUpdateStamp = data.stamp();
858  }
859 
860  UTimer iterationTime;
861  std::string status;
862  if(!odometryIgnored && info.odomPose.isNull())
863  {
864  printf("Skipping node %d as it doesn't have odometry pose set.\n", data.id());
865  }
866  else
867  {
868  if(!odometryIgnored && !info.odomCovariance.empty() && info.odomCovariance.at<double>(0,0)>=9999)
869  {
870  printf("High variance detected, triggering a new map...\n");
871  if(!incrementalMemory && processed>0)
872  {
873  showLocalizationStats(outputDatabasePath);
874  lastLocalizationOdomPose = info.odomPose;
875  }
876  rtabmap.triggerNewMap();
877  if(!incrementalMemory && locNull)
878  {
879  rtabmap.setInitialPose(Transform());
880  }
881  inMotion = true;
882  }
883 
884  if(originalGraphAsGT)
885  {
886  data.setGroundTruth(gt.find(data.id()) != gt.end()?gt.at(data.id()):Transform());
887  }
888 
889  UTimer t;
890  if(!rtabmap.process(data, info.odomPose, info.odomCovariance, info.odomVelocity, globalMapStats))
891  {
892  printf("Failed processing node %d.\n", data.id());
893  globalMapStats.clear();
894  }
895  else
896  {
897  if(republishLoopClosures && dbReader->driver())
898  {
899  std::multimap<int, Link> links;
900  dbReader->driver()->loadLinks(data.id(), links);
901  for(std::multimap<int, Link>::iterator iter=links.begin(); iter!=links.end(); ++iter)
902  {
903  if((iter->second.type() == Link::kGlobalClosure ||
904  iter->second.type() == Link::kLocalSpaceClosure ||
905  iter->second.type() == Link::kLocalTimeClosure ||
906  iter->second.type() == Link::kUserClosure) &&
907  iter->second.to() < data.id())
908  {
909  if(!iter->second.transform().isNull() &&
910  rtabmap.getMemory()->getWorkingMem().find(iter->second.to()) != rtabmap.getMemory()->getWorkingMem().end() &&
911  rtabmap.addLink(iter->second))
912  {
913  printf("Added link %d->%d from input database.\n", iter->second.from(), iter->second.to());
914  }
915  }
916  }
917  }
918 
919  if(assemble2dMap || assemble3dMap || assemble2dOctoMap || assemble3dOctoMap)
920  {
921  globalMapStats.clear();
922  double timeRtabmap = t.ticks();
923  double timeUpdateInit = 0.0;
924  double timeUpdateGrid = 0.0;
925 #ifdef RTABMAP_OCTOMAP
926  double timeUpdateOctoMap = 0.0;
927 #endif
928  const rtabmap::Statistics & stats = rtabmap.getStatistics();
929  if(stats.poses().size() && stats.getLastSignatureData().id())
930  {
931  int id = stats.poses().rbegin()->first;
932  if(id == stats.getLastSignatureData().id() &&
933  stats.getLastSignatureData().sensorData().gridCellSize() > 0.0f)
934  {
935  bool updateGridMap = false;
936  bool updateOctoMap = false;
937  if((assemble2dMap || assemble3dMap) && grid.addedNodes().find(id) == grid.addedNodes().end())
938  {
939  updateGridMap = true;
940  }
941 #ifdef RTABMAP_OCTOMAP
942  if((assemble2dOctoMap || assemble3dOctoMap) && octomap.addedNodes().find(id) == octomap.addedNodes().end())
943  {
944  updateOctoMap = true;
945  }
946 #endif
947  if(updateGridMap || updateOctoMap)
948  {
949  cv::Mat ground, obstacles, empty;
950  stats.getLastSignatureData().sensorData().uncompressDataConst(0, 0, 0, 0, &ground, &obstacles, &empty);
951 
952  timeUpdateInit = t.ticks();
953 
954  if(updateGridMap)
955  {
956  grid.addToCache(id, ground, obstacles, empty);
957  grid.update(stats.poses());
958  timeUpdateGrid = t.ticks() + timeUpdateInit;
959  }
960 #ifdef RTABMAP_OCTOMAP
961  if(updateOctoMap)
962  {
963  const cv::Point3f & viewpoint = stats.getLastSignatureData().sensorData().gridViewPoint();
964  octomap.addToCache(id, ground, obstacles, empty, viewpoint);
965  octomap.update(stats.poses());
966  timeUpdateOctoMap = t.ticks() + timeUpdateInit;
967  }
968 #endif
969  }
970  }
971  }
972 
973  globalMapStats.insert(std::make_pair(std::string("GlobalGrid/GridUpdate/ms"), timeUpdateGrid*1000.0f));
974 #ifdef RTABMAP_OCTOMAP
975  //Simulate publishing
976  double timePub2dOctoMap = 0.0;
977  double timePub3dOctoMap = 0.0;
978  if(assemble2dOctoMap)
979  {
980  float xMin, yMin, size;
981  octomap.createProjectionMap(xMin, yMin, size);
982  timePub2dOctoMap = t.ticks();
983  }
984  if(assemble3dOctoMap)
985  {
986  octomap.createCloud();
987  timePub3dOctoMap = t.ticks();
988  }
989 
990  globalMapStats.insert(std::make_pair(std::string("GlobalGrid/OctoMapUpdate/ms"), timeUpdateOctoMap*1000.0f));
991  globalMapStats.insert(std::make_pair(std::string("GlobalGrid/OctoMapProjection/ms"), timePub2dOctoMap*1000.0f));
992  globalMapStats.insert(std::make_pair(std::string("GlobalGrid/OctomapToCloud/ms"), timePub3dOctoMap*1000.0f));
993  globalMapStats.insert(std::make_pair(std::string("GlobalGrid/TotalWithRtabmap/ms"), (timeUpdateGrid+timeUpdateOctoMap+timePub2dOctoMap+timePub3dOctoMap+timeRtabmap)*1000.0f));
994 #else
995  globalMapStats.insert(std::make_pair(std::string("GlobalGrid/TotalWithRtabmap/ms"), (timeUpdateGrid+timeRtabmap)*1000.0f));
996 #endif
997  }
998  }
999  }
1000 
1001  const rtabmap::Statistics & stats = rtabmap.getStatistics();
1002  int refId = stats.refImageId();
1003  int loopId = stats.loopClosureId() > 0? stats.loopClosureId(): stats.proximityDetectionId() > 0?stats.proximityDetectionId() :0;
1004  int landmarkId = (int)uValue(stats.data(), rtabmap::Statistics::kLoopLandmark_detected(), 0.0f);
1005  int refMapId = stats.refImageMapId();
1006  ++totalFrames;
1007 
1008  if(inMotion)
1009  {
1010  ++totalFramesMotion;
1011  }
1012  if (loopId>0)
1013  {
1014  if(stats.loopClosureId()>0)
1015  {
1016  ++loopCount;
1017  }
1018  else
1019  {
1020  ++proxCount;
1021  }
1022  if(inMotion)
1023  {
1024  ++loopCountMotion;
1025  }
1026  int loopMapId = stats.loopClosureId() > 0? stats.loopClosureMapId(): stats.proximityDetectionMapId();
1027  printf("Processed %d/%d nodes [id=%d map=%d opt_graph=%d]... %dms %s on %d [%d]\n", ++processed, totalIds, refId, refMapId, int(stats.poses().size()), int(iterationTime.ticks() * 1000), stats.loopClosureId() > 0?"Loop":"Prox", loopId, loopMapId);
1028  }
1029  else if(landmarkId != 0)
1030  {
1031  printf("Processed %d/%d nodes [id=%d map=%d opt_graph=%d]... %dms Loop on landmark %d\n", ++processed, totalIds, refId, refMapId, int(stats.poses().size()), int(iterationTime.ticks() * 1000), landmarkId);
1032  }
1033  else
1034  {
1035  printf("Processed %d/%d nodes [id=%d map=%d opt_graph=%d]... %dms\n", ++processed, totalIds, refId, refMapId, int(stats.poses().size()), int(iterationTime.ticks() * 1000));
1036  }
1037 
1038  // Here we accumulate statistics about distance from last localization
1039  if(!incrementalMemory &&
1040  !lastLocalizationOdomPose.isNull() &&
1041  !info.odomPose.isNull())
1042  {
1043  if(loopId>0 || landmarkId != 0)
1044  {
1045  previousLocalizationDistances.push_back(lastLocalizationOdomPose.getDistance(info.odomPose));
1046  lastLocalizationOdomPose = info.odomPose;
1047  }
1048  }
1049  if(!incrementalMemory)
1050  {
1051  float totalTime = uValue(stats.data(), rtabmap::Statistics::kTimingTotal(), 0.0f);
1052  localizationTime.push_back(totalTime);
1053  if(stats.data().find(Statistics::kLoopOdom_correction_norm()) != stats.data().end())
1054  {
1055  localizationVariations.push_back(stats.data().at(Statistics::kLoopOdom_correction_norm()));
1056  localizationAngleVariations.push_back(stats.data().at(Statistics::kLoopOdom_correction_angle()));
1057  }
1058 
1059  if(exportPoses && !info.odomPose.isNull())
1060  {
1061  if(!odomTrajectoryPoses.empty())
1062  {
1063  int previousId = odomTrajectoryPoses.rbegin()->first;
1064  odomTrajectoryLinks.insert(std::make_pair(previousId, Link(previousId, refId, Link::kNeighbor, odomTrajectoryPoses.rbegin()->second.inverse()*info.odomPose, info.odomCovariance)));
1065  }
1066  odomTrajectoryPoses.insert(std::make_pair(refId, info.odomPose));
1067  localizationPoses.insert(std::make_pair(refId, stats.mapCorrection()*info.odomPose));
1068  }
1069  }
1070 
1071  Transform odomPose = info.odomPose;
1072 
1073  if(framesToSkip>0 && !recomputeOdometry)
1074  {
1075  int skippedFrames = framesToSkip;
1076  while(skippedFrames-- > 0)
1077  {
1078  processed++;
1079  data = dbReader->takeImage(&info);
1080  if(!odometryIgnored && !info.odomCovariance.empty() && info.odomCovariance.at<double>(0,0)>=9999)
1081  {
1082  printf("High variance detected, triggering a new map...\n");
1083  if(!incrementalMemory && processed>0)
1084  {
1085  showLocalizationStats(outputDatabasePath);
1086  lastLocalizationOdomPose = info.odomPose;
1087  }
1088  rtabmap.triggerNewMap();
1089  }
1090  }
1091  }
1092 
1093  data = dbReader->takeImage(&info);
1094  if(scanFromDepth)
1095  {
1096  data.setLaserScan(LaserScan());
1097  }
1098  camThread.postUpdate(&data, &info);
1099 
1100  inMotion = true;
1101  if(!incrementalMemory &&
1102  !odomPose.isNull() &&
1103  !info.odomPose.isNull())
1104  {
1105  float distance = odomPose.getDistance(info.odomPose);
1106  float angle = (odomPose.inverse()*info.odomPose).getAngle();
1107  odomDistances.push_back(distance);
1108  if(distance < linearUpdate && angle <= angularUpdate)
1109  {
1110  inMotion = false;
1111  }
1112  }
1113  }
1114 
1115  int databasesMerged = 0;
1116  if(!incrementalMemory)
1117  {
1118  showLocalizationStats(outputDatabasePath);
1119  }
1120  else
1121  {
1122  printf("Total loop closures = %d (Loop=%d, Prox=%d, In Motion=%d/%d)\n", loopCount+proxCount, loopCount, proxCount, loopCountMotion, totalFramesMotion);
1123 
1124  if(databases.size()>1)
1125  {
1126  std::map<int, Transform> poses;
1127  std::multimap<int, Link> constraints;
1128  rtabmap.getGraph(poses, constraints, 0, 1, 0, false, false, false, false, false, false);
1129  std::set<int> mapIds;
1130  for(std::map<int, Transform>::iterator iter=poses.begin(); iter!=poses.end(); ++iter)
1131  {
1132  int id;
1133  if((id=rtabmap.getMemory()->getMapId(iter->first, true))>=0)
1134  {
1135  mapIds.insert(id);
1136  }
1137  }
1138  databasesMerged = mapIds.size();
1139  }
1140  }
1141 
1142  printf("Closing database \"%s\"...\n", outputDatabasePath.c_str());
1143  rtabmap.close(true);
1144  printf("Closing database \"%s\"... done!\n", outputDatabasePath.c_str());
1145 
1146  delete odometry;
1147 
1148  if(assemble2dMap)
1149  {
1150  std::string outputPath = outputDatabasePath.substr(0, outputDatabasePath.size()-3) + "_map.pgm";
1151  float xMin,yMin;
1152  cv::Mat map = grid.getMap(xMin, yMin);
1153  if(!map.empty())
1154  {
1155  if(save2DMap)
1156  {
1157  DBDriver * driver = DBDriver::create();
1158  if(driver->openConnection(outputDatabasePath))
1159  {
1160  driver->save2DMap(map, xMin, yMin, grid.getCellSize());
1161  printf("Saving occupancy grid to database... done!\n");
1162  }
1163  delete driver;
1164  }
1165  else
1166  {
1167  cv::Mat map8U(map.rows, map.cols, CV_8U);
1168  //convert to gray scaled map
1169  for (int i = 0; i < map.rows; ++i)
1170  {
1171  for (int j = 0; j < map.cols; ++j)
1172  {
1173  char v = map.at<char>(i, j);
1174  unsigned char gray;
1175  if(v == 0)
1176  {
1177  gray = 178;
1178  }
1179  else if(v == 100)
1180  {
1181  gray = 0;
1182  }
1183  else // -1
1184  {
1185  gray = 89;
1186  }
1187  map8U.at<unsigned char>(i, j) = gray;
1188  }
1189  }
1190 
1191  if(cv::imwrite(outputPath, map8U))
1192  {
1193  printf("Saving occupancy grid \"%s\"... done!\n", outputPath.c_str());
1194  }
1195  else
1196  {
1197  printf("Saving occupancy grid \"%s\"... failed!\n", outputPath.c_str());
1198  }
1199  }
1200  }
1201  else
1202  {
1203  printf("2D map is empty! Cannot save it!\n");
1204  }
1205  }
1206  if(assemble3dMap)
1207  {
1208  std::string outputPath = outputDatabasePath.substr(0, outputDatabasePath.size()-3) + "_obstacles.pcd";
1209  if(pcl::io::savePCDFileBinary(outputPath, *grid.getMapObstacles()) == 0)
1210  {
1211  printf("Saving 3d obstacles \"%s\"... done!\n", outputPath.c_str());
1212  }
1213  else
1214  {
1215  printf("Saving 3d obstacles \"%s\"... failed!\n", outputPath.c_str());
1216  }
1217  if(grid.getMapGround()->size())
1218  {
1219  outputPath = outputDatabasePath.substr(0, outputDatabasePath.size()-3) + "_ground.pcd";
1220  if(pcl::io::savePCDFileBinary(outputPath, *grid.getMapGround()) == 0)
1221  {
1222  printf("Saving 3d ground \"%s\"... done!\n", outputPath.c_str());
1223  }
1224  else
1225  {
1226  printf("Saving 3d ground \"%s\"... failed!\n", outputPath.c_str());
1227  }
1228  }
1229  if(grid.getMapEmptyCells()->size())
1230  {
1231  outputPath = outputDatabasePath.substr(0, outputDatabasePath.size()-3) + "_empty.pcd";
1232  if(pcl::io::savePCDFileBinary(outputPath, *grid.getMapEmptyCells()) == 0)
1233  {
1234  printf("Saving 3d empty cells \"%s\"... done!\n", outputPath.c_str());
1235  }
1236  else
1237  {
1238  printf("Saving 3d empty cells \"%s\"... failed!\n", outputPath.c_str());
1239  }
1240  }
1241  }
1242 #ifdef RTABMAP_OCTOMAP
1243  if(assemble2dOctoMap)
1244  {
1245  std::string outputPath = outputDatabasePath.substr(0, outputDatabasePath.size()-3) + "_octomap.pgm";
1246  float xMin,yMin,cellSize;
1247  cv::Mat map = octomap.createProjectionMap(xMin, yMin, cellSize);
1248  if(!map.empty())
1249  {
1250  if(save2DMap)
1251  {
1252  DBDriver * driver = DBDriver::create();
1253  if(driver->openConnection(outputDatabasePath))
1254  {
1255  driver->save2DMap(map, xMin, yMin, cellSize);
1256  printf("Saving occupancy grid to database... done!\n");
1257  }
1258  delete driver;
1259  }
1260  else
1261  {
1262  cv::Mat map8U(map.rows, map.cols, CV_8U);
1263  //convert to gray scaled map
1264  for (int i = 0; i < map.rows; ++i)
1265  {
1266  for (int j = 0; j < map.cols; ++j)
1267  {
1268  char v = map.at<char>(i, j);
1269  unsigned char gray;
1270  if(v == 0)
1271  {
1272  gray = 178;
1273  }
1274  else if(v == 100)
1275  {
1276  gray = 0;
1277  }
1278  else // -1
1279  {
1280  gray = 89;
1281  }
1282  map8U.at<unsigned char>(i, j) = gray;
1283  }
1284  }
1285  if(cv::imwrite(outputPath, map8U))
1286  {
1287  printf("Saving octomap 2D projection \"%s\"... done!\n", outputPath.c_str());
1288  }
1289  else
1290  {
1291  printf("Saving octomap 2D projection \"%s\"... failed!\n", outputPath.c_str());
1292  }
1293  }
1294  }
1295  else
1296  {
1297  printf("OctoMap 2D projection map is empty! Cannot save it!\n");
1298  }
1299  }
1300  if(assemble3dOctoMap)
1301  {
1302  std::string outputPath = outputDatabasePath.substr(0, outputDatabasePath.size()-3) + "_octomap_occupied.pcd";
1303  std::vector<int> obstacles, emptySpace, ground;
1304  pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = octomap.createCloud(0, &obstacles, &emptySpace, &ground);
1305  if(pcl::io::savePCDFile(outputPath, *cloud, obstacles, true) == 0)
1306  {
1307  printf("Saving obstacles cloud \"%s\"... done!\n", outputPath.c_str());
1308  }
1309  else
1310  {
1311  printf("Saving obstacles cloud \"%s\"... failed!\n", outputPath.c_str());
1312  }
1313  if(ground.size())
1314  {
1315  outputPath = outputDatabasePath.substr(0, outputDatabasePath.size()-3) + "_octomap_ground.pcd";
1316  if(pcl::io::savePCDFile(outputPath, *cloud, ground, true) == 0)
1317  {
1318  printf("Saving empty space cloud \"%s\"... done!\n", outputPath.c_str());
1319  }
1320  else
1321  {
1322  printf("Saving empty space cloud \"%s\"... failed!\n", outputPath.c_str());
1323  }
1324  }
1325  if(emptySpace.size())
1326  {
1327  outputPath = outputDatabasePath.substr(0, outputDatabasePath.size()-3) + "_octomap_empty.pcd";
1328  if(pcl::io::savePCDFile(outputPath, *cloud, emptySpace, true) == 0)
1329  {
1330  printf("Saving empty space cloud \"%s\"... done!\n", outputPath.c_str());
1331  }
1332  else
1333  {
1334  printf("Saving empty space cloud \"%s\"... failed!\n", outputPath.c_str());
1335  }
1336  }
1337  }
1338 #endif
1339 
1340  return databasesMerged;
1341 }
UDirectory::homeDir
static std::string homeDir()
Definition: UDirectory.cpp:355
uVariance
T uVariance(const T *v, unsigned int size, T meanV)
Definition: UMath.h:491
rtabmap::Parameters::parse
static bool parse(const ParametersMap &parameters, const std::string &key, bool &value)
Definition: Parameters.cpp:497
rtabmap::OccupancyGrid::update
bool update(const std::map< int, Transform > &poses)
Definition: OccupancyGrid.cpp:782
rtabmap::CameraInfo::odomCovariance
cv::Mat odomCovariance
Definition: CameraInfo.h:70
UTimer
Definition: UTimer.h:46
rtabmap::DBDriver::save2DMap
void save2DMap(const cv::Mat &map, float xMin, float yMin, float cellSize) const
Definition: DBDriver.cpp:1195
uMean
T uMean(const T *v, unsigned int size)
Definition: UMath.h:401
rtabmap::OctoMap::createCloud
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:1054
rtabmap::OccupancyGrid::getCellSize
float getCellSize() const
Definition: OccupancyGrid.h:58
exportPoses
bool exportPoses
Definition: tools/Reprocess/main.cpp:129
rtabmap::DBReader::init
virtual bool init(const std::string &calibrationFolder=".", const std::string &cameraName="")
Definition: DBReader.cpp:142
proxCount
int proxCount
Definition: tools/Reprocess/main.cpp:117
rtabmap::SensorData::setLaserScan
void setLaserScan(const LaserScan &laserScan, bool clearPreviousData=true)
Definition: SensorData.cpp:381
f
f
rtabmap::Parameters::showUsage
static const char * showUsage()
Definition: Parameters.cpp:569
localizationAngleVariations
std::vector< float > localizationAngleVariations
Definition: tools/Reprocess/main.cpp:124
glm::sqrt
GLM_FUNC_DECL vecType< T, P > sqrt(vecType< T, P > const &x)
rtabmap::Rtabmap::getStatistics
const Statistics & getStatistics() const
Definition: Rtabmap.cpp:830
UDirectory::getDir
static std::string getDir(const std::string &filePath)
Definition: UDirectory.cpp:273
rtabmap::ParametersPair
std::pair< std::string, std::string > ParametersPair
Definition: Parameters.h:44
rtabmap::Rtabmap::setInitialPose
void setInitialPose(const Transform &initialPose)
Definition: Rtabmap.cpp:840
rtabmap::OccupancyGrid::getMapObstacles
const pcl::PointCloud< pcl::PointXYZRGB >::Ptr & getMapObstacles() const
Definition: OccupancyGrid.h:104
data
data
rtabmap::DBReader::driver
const DBDriver * driver() const
Definition: DBReader.h:84
rtabmap::Parameters::parseArguments
static ParametersMap parseArguments(int argc, char *argv[], bool onlyParameters=false)
Definition: Parameters.cpp:596
UFile::erase
static int erase(const std::string &filePath)
Definition: UFile.cpp:58
rtabmap::OccupancyGrid::getMapEmptyCells
const pcl::PointCloud< pcl::PointXYZRGB >::Ptr & getMapEmptyCells() const
Definition: OccupancyGrid.h:105
main
int main(int argc, char *argv[])
Definition: tools/Reprocess/main.cpp:217
rtabmap::ParametersMap
std::map< std::string, std::string > ParametersMap
Definition: Parameters.h:43
glm::distance
GLM_FUNC_DECL genType::value_type distance(genType const &p0, genType const &p1)
rtabmap::Rtabmap::init
void init(const ParametersMap &parameters, const std::string &databasePath="", bool loadDatabaseParameters=false)
Definition: Rtabmap.cpp:310
UMath.h
Basic mathematics functions.
glm::angle
GLM_FUNC_DECL T angle(detail::tquat< T, P > const &x)
rtabmap::CameraInfo::odomVelocity
std::vector< float > odomVelocity
Definition: CameraInfo.h:71
rtabmap::DBDriver::loadLinks
void loadLinks(int signatureId, std::multimap< int, Link > &links, Link::Type type=Link::kUndef) const
Definition: DBDriver.cpp:815
UFile::getExtension
std::string getExtension()
Definition: UFile.h:140
ULogger::setLevel
static void setLevel(ULogger::Level level)
Definition: ULogger.h:339
localizationVariations
std::vector< float > localizationVariations
Definition: tools/Reprocess/main.cpp:123
rtabmap::Rtabmap::triggerNewMap
int triggerNewMap()
Definition: Rtabmap.cpp:868
totalFramesMotion
int totalFramesMotion
Definition: tools/Reprocess/main.cpp:120
rtabmap::Statistics::loopClosureMapId
int loopClosureMapId() const
Definition: Statistics.h:270
rtabmap::Odometry::process
Transform process(SensorData &data, OdometryInfo *info=0)
Definition: Odometry.cpp:291
rtabmap::Statistics::poses
const std::map< int, Transform > & poses() const
Definition: Statistics.h:278
rtabmap::DBDriver::loadOptimizedPoses
std::map< int, Transform > loadOptimizedPoses(Transform *lastlocalizationPose=0) const
Definition: DBDriver.cpp:1187
rtabmap::SensorData::gridCellSize
float gridCellSize() const
Definition: SensorData.h:266
rtabmap::Statistics::data
const std::map< std::string, float > & data() const
Definition: Statistics.h:295
rtabmap::Statistics::loopClosureId
int loopClosureId() const
Definition: Statistics.h:269
odomDistances
std::vector< float > odomDistances
Definition: tools/Reprocess/main.cpp:122
rtabmap::DBDriver::openConnection
bool openConnection(const std::string &url, bool overwritten=false)
Definition: DBDriver.cpp:86
rtabmap::OctoMap::update
bool update(const std::map< int, Transform > &poses)
Definition: OctoMap.cpp:512
rtabmap::Camera::takeImage
SensorData takeImage(CameraInfo *info=0)
Definition: Camera.cpp:72
UStl.h
Wrappers of STL for convenient functions.
rtabmap::Statistics::proximityDetectionMapId
int proximityDetectionMapId() const
Definition: Statistics.h:272
uSplit
std::list< std::string > uSplit(const std::string &str, char separator=' ')
Definition: UStl.h:566
rtabmap::OctoMap::addToCache
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:363
uJoin
std::string uJoin(const std::list< std::string > &strings, const std::string &separator="")
Definition: UStl.h:603
rtabmap::SensorData::id
int id() const
Definition: SensorData.h:174
rtabmap::Rtabmap::close
void close(bool databaseSaved=true, const std::string &ouputDatabasePath="")
Definition: Rtabmap.cpp:453
rtabmap::OctoMap::addedNodes
const std::map< int, Transform > & addedNodes() const
Definition: OctoMap.h:178
rtabmap::Odometry::create
static Odometry * create(const ParametersMap &parameters=ParametersMap())
Definition: Odometry.cpp:57
previousLocalizationDistances
std::vector< float > previousLocalizationDistances
Definition: tools/Reprocess/main.cpp:121
rtabmap::DBDriver::closeConnection
void closeConnection(bool save=true, const std::string &outputUrl="")
Definition: DBDriver.cpp:64
rtabmap::OccupancyGrid::addToCache
void addToCache(int nodeId, const cv::Mat &ground, const cv::Mat &obstacles, const cv::Mat &empty)
Definition: OccupancyGrid.cpp:767
rtabmap::CameraThread::setScanParameters
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)
Definition: CameraThread.cpp:217
rtabmap::Rtabmap::getMemory
const Memory * getMemory() const
Definition: Rtabmap.h:147
rtabmap::DBDriver::getAllNodeIds
void getAllNodeIds(std::set< int > &ids, bool ignoreChildren=false, bool ignoreBadSignatures=false, bool ignoreIntermediateNodes=false) const
Definition: DBDriver.cpp:876
rtabmap::Statistics::mapCorrection
const Transform & mapCorrection() const
Definition: Statistics.h:280
ULogger::setType
static void setType(Type type, const std::string &fileName=kDefaultLogFileName, bool append=true)
Definition: ULogger.cpp:176
rtabmap::Rtabmap::addLink
bool addLink(const Link &link)
Definition: Rtabmap.cpp:5743
rtabmap::Rtabmap::process
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:1151
UFile::copy
static void copy(const std::string &from, const std::string &to)
Definition: UFile.cpp:97
rtabmap::OccupancyGrid::getMap
cv::Mat getMap(float &xMin, float &yMin) const
Definition: OccupancyGrid.cpp:692
uReplaceChar
std::string UTILITE_EXP uReplaceChar(const std::string &str, char before, char after)
Definition: UConversion.cpp:33
totalFrames
int totalFrames
Definition: tools/Reprocess/main.cpp:119
rtabmap::graph::exportPoses
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
rtabmap::DBDriver::create
static DBDriver * create(const ParametersMap &parameters=ParametersMap())
Definition: DBDriver.cpp:41
rtabmap::Transform::getDistance
float getDistance(const Transform &t) const
Definition: Transform.cpp:283
rtabmap::Transform::isNull
bool isNull() const
Definition: Transform.cpp:107
g_loopForever
bool g_loopForever
Definition: tools/Reprocess/main.cpp:109
loopCount
int loopCount
Definition: tools/Reprocess/main.cpp:116
uMax
T uMax(const T *v, unsigned int size, unsigned int &index)
Definition: UMath.h:94
rtabmap::OccupancyGrid::addedNodes
const std::map< int, Transform > & addedNodes() const
Definition: OccupancyGrid.h:65
sighandler
void sighandler(int sig)
Definition: tools/Reprocess/main.cpp:110
rtabmap::SensorData::gridViewPoint
const cv::Point3f & gridViewPoint() const
Definition: SensorData.h:267
rtabmap::CameraThread::postUpdate
void postUpdate(SensorData *data, CameraInfo *info=0) const
Definition: CameraThread.cpp:350
rtabmap::Memory::getWorkingMem
const std::map< int, double > & getWorkingMem() const
Definition: Memory.h:148
glm::max
GLM_FUNC_DECL genType max(genType const &x, genType const &y)
rtabmap::Signature::sensorData
SensorData & sensorData()
Definition: Signature.h:137
UFile::exists
bool exists()
Definition: UFile.h:104
rtabmap::Memory::getMapId
int getMapId(int id, bool lookInDatabase=false) const
Definition: Memory.cpp:3934
showLocalizationStats
void showLocalizationStats(const std::string &outputDatabasePath)
Definition: tools/Reprocess/main.cpp:131
rtabmap_oanet.argv
argv
Definition: rtabmap_oanet.py:12
rtabmap::Statistics::refImageId
int refImageId() const
Definition: Statistics.h:267
localizationTime
std::vector< float > localizationTime
Definition: tools/Reprocess/main.cpp:125
rtabmap::Signature::id
int id() const
Definition: Signature.h:70
rtabmap::SensorData::stamp
double stamp() const
Definition: SensorData.h:176
rtabmap::Statistics::getLastSignatureData
const Signature & getLastSignatureData() const
Definition: Statistics.h:275
rtabmap::Rtabmap::getGraph
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:5189
rtabmap::DBDriver::getLastParameters
ParametersMap getLastParameters() const
Definition: DBDriver.cpp:239
UTimer::ticks
double ticks()
Definition: UTimer.cpp:117
odomTrajectoryPoses
std::map< int, Transform > odomTrajectoryPoses
Definition: tools/Reprocess/main.cpp:126
rtabmap::Parameters::readINI
static void readINI(const std::string &configFile, ParametersMap &parameters, bool modifiedOnly=false)
Definition: Parameters.cpp:1161
localizationPoses
std::map< int, Transform > localizationPoses
Definition: tools/Reprocess/main.cpp:128
rtabmap::Statistics::refImageMapId
int refImageMapId() const
Definition: Statistics.h:268
rtabmap::OdometryInfo::reg
RegistrationInfo reg
Definition: OdometryInfo.h:97
rtabmap::SensorData::setGroundTruth
void setGroundTruth(const Transform &pose)
Definition: SensorData.h:279
rtabmap::DBDriver::getDatabaseVersion
std::string getDatabaseVersion() const
Definition: DBDriver.cpp:275
rtabmap::OccupancyGrid::getMapGround
const pcl::PointCloud< pcl::PointXYZRGB >::Ptr & getMapGround() const
Definition: OccupancyGrid.h:103
sessionCount
int sessionCount
Definition: tools/Reprocess/main.cpp:130
rtabmap::OctoMap::createProjectionMap
cv::Mat createProjectionMap(float &xMin, float &yMin, float &gridCellSize, float minGridSize=0.0f, unsigned int treeDepth=0)
Definition: OctoMap.cpp:1218
rtabmap::Statistics::proximityDetectionId
int proximityDetectionId() const
Definition: Statistics.h:271
odomTrajectoryLinks
std::multimap< int, Link > odomTrajectoryLinks
Definition: tools/Reprocess/main.cpp:127
rtabmap::SensorData::isValid
bool isValid() const
Definition: SensorData.h:156
rtabmap::CameraInfo::odomPose
Transform odomPose
Definition: CameraInfo.h:69
showUsage
void showUsage()
Definition: tools/Reprocess/main.cpp:53
loopCountMotion
int loopCountMotion
Definition: tools/Reprocess/main.cpp:118
uFormat
std::string UTILITE_EXP uFormat(const char *fmt,...)
Definition: UConversion.cpp:365
rtabmap::SensorData::uncompressDataConst
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:623
uValue
V uValue(const std::map< K, V > &m, const K &key, const V &defaultValue=V())
Definition: UStl.h:240
rtabmap::Transform::inverse
Transform inverse() const
Definition: Transform.cpp:178
rtabmap::OccupancyGrid::setCloudAssembling
void setCloudAssembling(bool enabled)
Definition: OccupancyGrid.cpp:274
uInsert
void uInsert(std::map< K, V > &map, const std::pair< K, V > &pair)
Definition: UStl.h:443

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