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

rtabmap
Author(s): Mathieu Labbe
autogenerated on Thu Jul 25 2024 02:50:12