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