tools/KittiDataset/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/Odometry.h>
29 #include "rtabmap/core/Rtabmap.h"
30 #include "rtabmap/core/CameraStereo.h"
31 #include "rtabmap/core/CameraThread.h"
32 #include "rtabmap/core/Graph.h"
33 #include "rtabmap/core/OdometryInfo.h"
34 #include "rtabmap/core/OdometryEvent.h"
35 #include "rtabmap/core/Memory.h"
36 #include "rtabmap/core/util3d_registration.h"
37 #include "rtabmap/utilite/UConversion.h"
38 #include "rtabmap/utilite/UDirectory.h"
39 #include "rtabmap/utilite/UFile.h"
40 #include "rtabmap/utilite/UMath.h"
41 #include "rtabmap/utilite/UStl.h"
42 #include "rtabmap/utilite/UProcessInfo.h"
43 #include <pcl/common/common.h>
44 #include <stdio.h>
45 #include <signal.h>
46 
47 using namespace rtabmap;
48 
49 void showUsage()
50 {
51  printf("\nUsage:\n"
52  "rtabmap-kitti_dataset [options] path\n"
53  " path Folder of the sequence (e.g., \"~/KITTI/dataset/sequences/07\")\n"
54  " containing least calib.txt, times.txt, image_0 and image_1 folders.\n"
55  " Optional image_2, image_3 and velodyne folders.\n"
56  " --output Output directory. By default, results are saved in \"path\".\n"
57  " --output_name Output database name (default \"rtabmap\").\n"
58  " --gt \"path\" Ground truth path (e.g., ~/KITTI/devkit/cpp/data/odometry/poses/07.txt)\n"
59  " --quiet Don't show log messages and iteration updates.\n"
60  " --color Use color images for stereo (image_2 and image_3 folders).\n"
61  " --height Add car's height to camera local transform (1.67m).\n"
62  " --disp Generate full disparity.\n"
63  " --exposure_comp Do exposure compensation between left and right images.\n"
64  " --scan Include velodyne scan in node's data (use --scan_only to ignore image data).\n"
65  " --scan_step # Scan downsample step (default=1).\n"
66  " --scan_voxel #.# Scan voxel size (default 0.5 m).\n"
67  " --scan_k Scan normal K (default 0).\n"
68  " --scan_radius Scan normal radius (default 0).\n\n"
69  "%s\n"
70  "Example:\n\n"
71  " $ rtabmap-kitti_dataset \\\n"
72  " --Rtabmap/PublishRAMUsage true\\\n"
73  " --Rtabmap/DetectionRate 2\\\n"
74  " --Rtabmap/CreateIntermediateNodes true\\\n"
75  " --RGBD/LinearUpdate 0\\\n"
76  " --GFTT/QualityLevel 0.01\\\n"
77  " --GFTT/MinDistance 7\\\n"
78  " --OdomF2M/MaxSize 3000\\\n"
79  " --Mem/STMSize 30\\\n"
80  " --Kp/MaxFeatures 750\\\n"
81  " --Vis/MaxFeatures 1500\\\n"
82  " --gt \"~/KITTI/devkit/cpp/data/odometry/poses/07.txt\"\\\n"
83  " ~/KITTI/dataset/sequences/07\n\n", rtabmap::Parameters::showUsage());
84  exit(1);
85 }
86 
87 // catch ctrl-c
88 bool g_forever = true;
89 void sighandler(int sig)
90 {
91  printf("\nSignal %d caught...\n", sig);
92  g_forever = false;
93 }
94 
95 int main(int argc, char * argv[])
96 {
97  signal(SIGABRT, &sighandler);
98  signal(SIGTERM, &sighandler);
99  signal(SIGINT, &sighandler);
100 
101  ULogger::setType(ULogger::kTypeConsole);
102  ULogger::setLevel(ULogger::kWarning);
103 
104  ParametersMap parameters;
105  std::string path;
106  std::string output;
107  std::string outputName = "rtabmap";
108  std::string seq;
109  bool color = false;
110  bool height = false;
111  bool scan = false;
112  bool disp = false;
113  bool exposureCompensation = false;
114  int scanStep = 1;
115  float scanVoxel = 0.5f;
116  int scanNormalK = 0;
117  float scanNormalRadius = 0.0f;
118  bool scanOnly = false;
119  std::string gtPath;
120  bool quiet = false;
121  if(argc < 2)
122  {
123  showUsage();
124  }
125  else
126  {
127  for(int i=1; i<argc; ++i)
128  {
129  if(std::strcmp(argv[i], "--output") == 0)
130  {
131  output = argv[++i];
132  }
133  else if(std::strcmp(argv[i], "--output_name") == 0)
134  {
135  outputName = argv[++i];
136  }
137  else if(std::strcmp(argv[i], "--quiet") == 0)
138  {
139  quiet = true;
140  }
141  else if(std::strcmp(argv[i], "--scan_step") == 0)
142  {
143  scanStep = atoi(argv[++i]);
144  if(scanStep <= 0)
145  {
146  printf("scan_step should be > 0\n");
147  showUsage();
148  }
149  }
150  else if(std::strcmp(argv[i], "--scan_voxel") == 0)
151  {
152  scanVoxel = atof(argv[++i]);
153  if(scanVoxel < 0.0f)
154  {
155  printf("scan_voxel should be >= 0.0\n");
156  showUsage();
157  }
158  }
159  else if(std::strcmp(argv[i], "--scan_k") == 0)
160  {
161  scanNormalK = atoi(argv[++i]);
162  if(scanNormalK < 0)
163  {
164  printf("scanNormalK should be >= 0\n");
165  showUsage();
166  }
167  }
168  else if(std::strcmp(argv[i], "--scan_radius") == 0)
169  {
170  scanNormalRadius = atof(argv[++i]);
171  if(scanNormalRadius < 0.0f)
172  {
173  printf("scanNormalRadius should be >= 0\n");
174  showUsage();
175  }
176  }
177  else if(std::strcmp(argv[i], "--scan_only") == 0)
178  {
179  scan = scanOnly = true;
180  }
181  else if(std::strcmp(argv[i], "--gt") == 0)
182  {
183  gtPath = argv[++i];
184  }
185  else if(std::strcmp(argv[i], "--color") == 0)
186  {
187  color = true;
188  }
189  else if(std::strcmp(argv[i], "--height") == 0)
190  {
191  height = true;
192  }
193  else if(std::strcmp(argv[i], "--scan") == 0)
194  {
195  scan = true;
196  }
197  else if(std::strcmp(argv[i], "--disp") == 0)
198  {
199  disp = true;
200  }
201  else if(std::strcmp(argv[i], "--exposure_comp") == 0)
202  {
203  exposureCompensation = true;
204  }
205  }
206  parameters = Parameters::parseArguments(argc, argv);
207  path = argv[argc-1];
208  path = uReplaceChar(path, '~', UDirectory::homeDir());
209  path = uReplaceChar(path, '\\', '/');
210  if(output.empty())
211  {
212  output = path;
213  }
214  else
215  {
216  output = uReplaceChar(output, '~', UDirectory::homeDir());
217  UDirectory::makeDir(output);
218  }
219  parameters.insert(ParametersPair(Parameters::kRtabmapWorkingDirectory(), output));
220  parameters.insert(ParametersPair(Parameters::kRtabmapPublishRAMUsage(), "true"));
221  }
222 
223  seq = uSplit(path, '/').back();
224  if(seq.empty() || !(uStr2Int(seq)>=0 && uStr2Int(seq)<=21))
225  {
226  UWARN("Sequence number \"%s\" should be between 0 and 21 (official KITTI datasets).", seq.c_str());
227  seq.clear();
228  }
229  std::string pathLeftImages = path+(color?"/image_2":"/image_0");
230  std::string pathRightImages = path+(color?"/image_3":"/image_1");
231  std::string pathCalib = path+"/calib.txt";
232  std::string pathTimes = path+"/times.txt";
233  std::string pathScan;
234 
235  printf("Paths:\n"
236  " Sequence number: %s\n"
237  " Sequence path: %s\n"
238  " Output: %s\n"
239  " Output name: %s\n"
240  " left images: %s\n"
241  " right images: %s\n"
242  " calib.txt: %s\n"
243  " times.txt: %s\n",
244  seq.c_str(),
245  path.c_str(),
246  output.c_str(),
247  outputName.c_str(),
248  pathLeftImages.c_str(),
249  pathRightImages.c_str(),
250  pathCalib.c_str(),
251  pathTimes.c_str());
252  if(!gtPath.empty())
253  {
254  gtPath = uReplaceChar(gtPath, '~', UDirectory::homeDir());
255  gtPath = uReplaceChar(gtPath, '\\', '/');
256  if(!UFile::exists(gtPath))
257  {
258  UWARN("Ground truth file path doesn't exist: \"%s\", benchmark values won't be computed.", gtPath.c_str());
259  gtPath.clear();
260  }
261  else
262  {
263  printf(" Ground Truth: %s\n", gtPath.c_str());
264  }
265  }
266  printf(" Exposure Compensation: %s\n", exposureCompensation?"true":"false");
267  printf(" Disparity: %s\n", disp?"true":"false");
268  if(scan)
269  {
270  pathScan = path+"/velodyne";
271  printf(" Scan: %s\n", pathScan.c_str());
272  printf(" Scan only: %s\n", scanOnly?"true":"false");
273  printf(" Scan step: %d\n", scanStep);
274  printf(" Scan voxel: %fm\n", scanVoxel);
275  printf(" Scan normal k: %d\n", scanNormalK);
276  printf(" Scan normal radius: %f\n", scanNormalRadius);
277  }
278 
279  // convert calib.txt to rtabmap format (yaml)
280  FILE * pFile = 0;
281  pFile = fopen(pathCalib.c_str(),"r");
282  if(!pFile)
283  {
284  UERROR("Cannot open calibration file \"%s\"", pathCalib.c_str());
285  return -1;
286  }
287  cv::Mat_<double> P0(3,4);
288  cv::Mat_<double> P1(3,4);
289  cv::Mat_<double> P2(3,4);
290  cv::Mat_<double> P3(3,4);
291  if(fscanf (pFile, "%*s %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf",
292  &P0(0, 0), &P0(0, 1), &P0(0, 2), &P0(0, 3),
293  &P0(1, 0), &P0(1, 1), &P0(1, 2), &P0(1, 3),
294  &P0(2, 0), &P0(2, 1), &P0(2, 2), &P0(2, 3)) != 12)
295  {
296  UERROR("Failed to parse calibration file \"%s\"", pathCalib.c_str());
297  return -1;
298  }
299  if(fscanf (pFile, "%*s %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf",
300  &P1(0, 0), &P1(0, 1), &P1(0, 2), &P1(0, 3),
301  &P1(1, 0), &P1(1, 1), &P1(1, 2), &P1(1, 3),
302  &P1(2, 0), &P1(2, 1), &P1(2, 2), &P1(2, 3)) != 12)
303  {
304  UERROR("Failed to parse calibration file \"%s\"", pathCalib.c_str());
305  return -1;
306  }
307  if(fscanf (pFile, "%*s %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf",
308  &P2(0, 0), &P2(0, 1), &P2(0, 2), &P2(0, 3),
309  &P2(1, 0), &P2(1, 1), &P2(1, 2), &P2(1, 3),
310  &P2(2, 0), &P2(2, 1), &P2(2, 2), &P2(2, 3)) != 12)
311  {
312  UERROR("Failed to parse calibration file \"%s\"", pathCalib.c_str());
313  return -1;
314  }
315  if(fscanf (pFile, "%*s %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf",
316  &P3(0, 0), &P3(0, 1), &P3(0, 2), &P3(0, 3),
317  &P3(1, 0), &P3(1, 1), &P3(1, 2), &P3(1, 3),
318  &P3(2, 0), &P3(2, 1), &P3(2, 2), &P3(2, 3)) != 12)
319  {
320  UERROR("Failed to parse calibration file \"%s\"", pathCalib.c_str());
321  return -1;
322  }
323  fclose (pFile);
324  // get image size
325  UDirectory dir(pathLeftImages);
326  std::string firstImage = dir.getNextFileName();
327  cv::Mat image = cv::imread(dir.getNextFilePath());
328  if(image.empty())
329  {
330  UERROR("Failed to read first image of \"%s\"", firstImage.c_str());
331  return -1;
332  }
333 
334  StereoCameraModel model(outputName+"_calib",
335  image.size(), P0.colRange(0,3), cv::Mat(), cv::Mat(), P0,
336  image.size(), P1.colRange(0,3), cv::Mat(), cv::Mat(), P1,
337  cv::Mat(), cv::Mat(), cv::Mat(), cv::Mat());
338  if(!model.save(output, true))
339  {
340  UERROR("Could not save calibration!");
341  return -1;
342  }
343  printf("Saved calibration \"%s\" to \"%s\"\n", (outputName+"_calib").c_str(), output.c_str());
344 
345  if(!parameters.empty())
346  {
347  printf("Parameters:\n");
348  for(ParametersMap::iterator iter=parameters.begin(); iter!=parameters.end(); ++iter)
349  {
350  printf(" %s=%s\n", iter->first.c_str(), iter->second.c_str());
351  }
352  }
353 
354  printf("RTAB-Map version: %s\n", RTABMAP_VERSION);
355 
356  if(quiet)
357  {
358  ULogger::setLevel(ULogger::kError);
359  }
360 
361  // We use CameraThread only to use postUpdate() method
362  Transform opticalRotation(0,0,1,0, -1,0,0,color?-0.06:0, 0,-1,0,height?1.67:0.0);
363  Camera * camera = 0;
364  if(scanOnly)
365  {
366  camera = new CameraImages(""); // Scan path is set below
367  }
368  else
369  {
370  camera = new CameraStereoImages(
371  pathLeftImages,
372  pathRightImages,
373  false, // assume that images are already rectified
374  0.0f);
375  }
376  CameraThread cameraThread(camera, parameters);
377  ((CameraImages*)cameraThread.camera())->setTimestamps(false, pathTimes, false);
378  if(exposureCompensation)
379  {
380  cameraThread.setStereoExposureCompensation(true);
381  }
382  if(disp)
383  {
384  cameraThread.setStereoToDepth(true);
385  }
386  if(!gtPath.empty())
387  {
388  ((CameraImages*)cameraThread.camera())->setGroundTruthPath(gtPath, 2);
389  }
390  if(!pathScan.empty())
391  {
392  ((CameraImages*)cameraThread.camera())->setScanPath(
393  pathScan,
394  130000,
395  Transform(-0.27f, 0.0f, 0.08+(height?1.67f:0.0f), 0.0f, 0.0f, 0.0f));
396  cameraThread.setScanParameters(
397  false,
398  scanStep,
399  0,
400  0,
401  scanVoxel,
402  scanNormalK,
403  scanNormalRadius,
404  0.8f);
405  }
406 
407  float detectionRate = Parameters::defaultRtabmapDetectionRate();
408  bool intermediateNodes = Parameters::defaultRtabmapCreateIntermediateNodes();
409  int odomStrategy = Parameters::defaultOdomStrategy();
410  Parameters::parse(parameters, Parameters::kOdomStrategy(), odomStrategy);
411  Parameters::parse(parameters, Parameters::kRtabmapDetectionRate(), detectionRate);
412  Parameters::parse(parameters, Parameters::kRtabmapCreateIntermediateNodes(), intermediateNodes);
413 
414  // assuming source is 10 Hz
415  int mapUpdate = detectionRate>0?10 / detectionRate:1;
416  if(mapUpdate < 1)
417  {
418  mapUpdate = 1;
419  }
420 
421  std::string databasePath = output+"/"+outputName+".db";
422  UFile::erase(databasePath);
423  if(cameraThread.camera()->init(output, outputName+"_calib"))
424  {
425  int totalImages = (int)((CameraStereoImages*)cameraThread.camera())->filenames().size();
426 
427  printf("Processing %d images...\n", totalImages);
428 
429  ParametersMap odomParameters = parameters;
430  odomParameters.erase(Parameters::kRtabmapPublishRAMUsage()); // as odometry is in the same process than rtabmap, don't get RAM usage in odometry.
431  Odometry * odom = Odometry::create(odomParameters);
432  Rtabmap rtabmap;
433  rtabmap.init(parameters, databasePath);
434 
435  UTimer totalTime;
436  UTimer timer;
437  CameraInfo cameraInfo;
438  SensorData data = cameraThread.camera()->takeImage(&cameraInfo);
439  int iteration = 0;
440 
442  // Processing dataset begin
444  cv::Mat covariance;
445  int odomKeyFrames = 0;
446  while(data.isValid() && g_forever)
447  {
448  cameraThread.postUpdate(&data, &cameraInfo);
449  cameraInfo.timeTotal = timer.ticks();
450 
451 
452  OdometryInfo odomInfo;
453  Transform pose = odom->process(data, &odomInfo);
454  float speed = 0.0f;
455  if(odomInfo.interval>0.0)
456  speed = odomInfo.transform.x()/odomInfo.interval*3.6;
457  if(odomInfo.keyFrameAdded)
458  {
459  ++odomKeyFrames;
460  }
461 
462  if(odomStrategy == 2)
463  {
464  //special case for FOVIS, set covariance 1 if 9999 is detected
465  if(!odomInfo.reg.covariance.empty() && odomInfo.reg.covariance.at<double>(0,0) >= 9999)
466  {
467  odomInfo.reg.covariance = cv::Mat::eye(6,6,CV_64FC1);
468  }
469  }
470 
471  bool processData = true;
472  if(iteration % mapUpdate != 0)
473  {
474  // set negative id so rtabmap will detect it as an intermediate node
475  data.setId(-1);
476  data.setFeatures(std::vector<cv::KeyPoint>(), std::vector<cv::Point3f>(), cv::Mat());// remove features
477  processData = intermediateNodes;
478  }
479  if(covariance.empty() || odomInfo.reg.covariance.at<double>(0,0) > covariance.at<double>(0,0))
480  {
481  covariance = odomInfo.reg.covariance;
482  }
483 
484  timer.restart();
485  if(processData)
486  {
487  std::map<std::string, float> externalStats;
488  // save camera statistics to database
489  externalStats.insert(std::make_pair("Camera/BilateralFiltering/ms", cameraInfo.timeBilateralFiltering*1000.0f));
490  externalStats.insert(std::make_pair("Camera/Capture/ms", cameraInfo.timeCapture*1000.0f));
491  externalStats.insert(std::make_pair("Camera/Disparity/ms", cameraInfo.timeDisparity*1000.0f));
492  externalStats.insert(std::make_pair("Camera/ImageDecimation/ms", cameraInfo.timeImageDecimation*1000.0f));
493  externalStats.insert(std::make_pair("Camera/Mirroring/ms", cameraInfo.timeMirroring*1000.0f));
494  externalStats.insert(std::make_pair("Camera/ExposureCompensation/ms", cameraInfo.timeStereoExposureCompensation*1000.0f));
495  externalStats.insert(std::make_pair("Camera/ScanFromDepth/ms", cameraInfo.timeScanFromDepth*1000.0f));
496  externalStats.insert(std::make_pair("Camera/TotalTime/ms", cameraInfo.timeTotal*1000.0f));
497  externalStats.insert(std::make_pair("Camera/UndistortDepth/ms", cameraInfo.timeUndistortDepth*1000.0f));
498  // save odometry statistics to database
499  externalStats.insert(std::make_pair("Odometry/LocalBundle/ms", odomInfo.localBundleTime*1000.0f));
500  externalStats.insert(std::make_pair("Odometry/LocalBundleConstraints/", odomInfo.localBundleConstraints));
501  externalStats.insert(std::make_pair("Odometry/LocalBundleOutliers/", odomInfo.localBundleOutliers));
502  externalStats.insert(std::make_pair("Odometry/TotalTime/ms", odomInfo.timeEstimation*1000.0f));
503  externalStats.insert(std::make_pair("Odometry/Registration/ms", odomInfo.reg.totalTime*1000.0f));
504  externalStats.insert(std::make_pair("Odometry/Speed/kph", speed));
505  externalStats.insert(std::make_pair("Odometry/Inliers/", odomInfo.reg.inliers));
506  externalStats.insert(std::make_pair("Odometry/Features/", odomInfo.features));
507  externalStats.insert(std::make_pair("Odometry/DistanceTravelled/m", odomInfo.distanceTravelled));
508  externalStats.insert(std::make_pair("Odometry/KeyFrameAdded/", odomInfo.keyFrameAdded));
509  externalStats.insert(std::make_pair("Odometry/LocalKeyFrames/", odomInfo.localKeyFrames));
510  externalStats.insert(std::make_pair("Odometry/LocalMapSize/", odomInfo.localMapSize));
511  externalStats.insert(std::make_pair("Odometry/LocalScanMapSize/", odomInfo.localScanMapSize));
512 
513  OdometryEvent e(SensorData(), Transform(), odomInfo);
514  rtabmap.process(data, pose, covariance, e.velocity(), externalStats);
515  covariance = cv::Mat();
516  }
517 
518  ++iteration;
519  if(!quiet || iteration == totalImages)
520  {
521  double slamTime = timer.ticks();
522 
523  float rmse = -1;
524  if(rtabmap.getStatistics().data().find(Statistics::kGtTranslational_rmse()) != rtabmap.getStatistics().data().end())
525  {
526  rmse = rtabmap.getStatistics().data().at(Statistics::kGtTranslational_rmse());
527  }
528 
529  if(data.keypoints().size() == 0 && data.laserScanRaw().size())
530  {
531  if(rmse >= 0.0f)
532  {
533  //printf("Iteration %d/%d: speed=%dkm/h camera=%dms, odom(quality=%f, kfs=%d)=%dms, slam=%dms, rmse=%fm, noise stddev=%fm %frad",
534  // iteration, totalImages, int(speed), int(cameraInfo.timeTotal*1000.0f), odomInfo.reg.icpInliersRatio, odomKeyFrames, int(odomInfo.timeEstimation*1000.0f), int(slamTime*1000.0f), rmse, sqrt(odomInfo.reg.covariance.at<double>(0,0)), sqrt(odomInfo.reg.covariance.at<double>(3,3)));
535  printf("Iteration %d/%d: speed=%dkm/h camera=%dms, odom(quality=%f, kfs=%d)=%dms, slam=%dms, rmse=%fm",
536  iteration, totalImages, int(speed), int(cameraInfo.timeTotal*1000.0f), odomInfo.reg.icpInliersRatio, odomKeyFrames, int(odomInfo.timeEstimation*1000.0f), int(slamTime*1000.0f), rmse);
537  }
538  else
539  {
540  printf("Iteration %d/%d: speed=%dkm/h camera=%dms, odom(quality=%f, kfs=%d)=%dms, slam=%dms",
541  iteration, totalImages, int(speed), int(cameraInfo.timeTotal*1000.0f), odomInfo.reg.icpInliersRatio, odomKeyFrames, int(odomInfo.timeEstimation*1000.0f), int(slamTime*1000.0f));
542  }
543  }
544  else
545  {
546  if(rmse >= 0.0f)
547  {
548  //printf("Iteration %d/%d: speed=%dkm/h camera=%dms, odom(quality=%d/%d, kfs=%d)=%dms, slam=%dms, rmse=%fm, noise stddev=%fm %frad",
549  // iteration, totalImages, int(speed), int(cameraInfo.timeTotal*1000.0f), odomInfo.reg.inliers, odomInfo.features, odomKeyFrames, int(odomInfo.timeEstimation*1000.0f), int(slamTime*1000.0f), rmse, sqrt(odomInfo.reg.covariance.at<double>(0,0)), sqrt(odomInfo.reg.covariance.at<double>(3,3)));
550  printf("Iteration %d/%d: speed=%dkm/h camera=%dms, odom(quality=%d/%d, kfs=%d)=%dms, slam=%dms, rmse=%fm",
551  iteration, totalImages, int(speed), int(cameraInfo.timeTotal*1000.0f), odomInfo.reg.inliers, odomInfo.features, odomKeyFrames, int(odomInfo.timeEstimation*1000.0f), int(slamTime*1000.0f), rmse);
552  }
553  else
554  {
555  printf("Iteration %d/%d: speed=%dkm/h camera=%dms, odom(quality=%d/%d, kfs=%d)=%dms, slam=%dms",
556  iteration, totalImages, int(speed), int(cameraInfo.timeTotal*1000.0f), odomInfo.reg.inliers, odomInfo.features, odomKeyFrames, int(odomInfo.timeEstimation*1000.0f), int(slamTime*1000.0f));
557  }
558  }
559  if(processData && rtabmap.getLoopClosureId()>0)
560  {
561  printf(" *");
562  }
563  printf("\n");
564  }
565  else if(iteration % (totalImages/10) == 0)
566  {
567  printf(".");
568  fflush(stdout);
569  }
570 
571  cameraInfo = CameraInfo();
572  timer.restart();
573  data = cameraThread.camera()->takeImage(&cameraInfo);
574  }
575  delete odom;
576  printf("Total time=%fs\n", totalTime.ticks());
578  // Processing dataset end
580 
581  // Save trajectory
582  printf("Saving trajectory ...\n");
583  std::map<int, Transform> poses;
584  std::multimap<int, Link> links;
585  rtabmap.getGraph(poses, links, true, true);
586  std::string pathTrajectory = output+"/"+outputName+"_poses.txt";
587  if(poses.size() && graph::exportPoses(pathTrajectory, 2, poses, links))
588  {
589  printf("Saving %s... done!\n", pathTrajectory.c_str());
590  }
591  else
592  {
593  printf("Saving %s... failed!\n", pathTrajectory.c_str());
594  }
595 
596  if(!gtPath.empty())
597  {
598  // Log ground truth statistics
599  std::map<int, Transform> groundTruth;
600 
601  for(std::map<int, Transform>::const_iterator iter=poses.begin(); iter!=poses.end(); ++iter)
602  {
603  Transform o, gtPose;
604  int m,w;
605  std::string l;
606  double s;
607  std::vector<float> v;
608  GPS gps;
609  EnvSensors sensors;
610  rtabmap.getMemory()->getNodeInfo(iter->first, o, m, w, l, s, gtPose, v, gps, sensors, true);
611  if(!gtPose.isNull())
612  {
613  groundTruth.insert(std::make_pair(iter->first, gtPose));
614  }
615  }
616 
617  // compute KITTI statistics
618  float t_err = 0.0f;
619  float r_err = 0.0f;
620  graph::calcKittiSequenceErrors(uValues(groundTruth), uValues(poses), t_err, r_err);
621  printf("Ground truth comparison:\n");
622  printf(" KITTI t_err = %f %%\n", t_err);
623  printf(" KITTI r_err = %f deg/m\n", r_err);
624 
625  // compute RMSE statistics
626  float translational_rmse = 0.0f;
627  float translational_mean = 0.0f;
628  float translational_median = 0.0f;
629  float translational_std = 0.0f;
630  float translational_min = 0.0f;
631  float translational_max = 0.0f;
632  float rotational_rmse = 0.0f;
633  float rotational_mean = 0.0f;
634  float rotational_median = 0.0f;
635  float rotational_std = 0.0f;
636  float rotational_min = 0.0f;
637  float rotational_max = 0.0f;
638  graph::calcRMSE(
639  groundTruth,
640  poses,
641  translational_rmse,
642  translational_mean,
643  translational_median,
644  translational_std,
645  translational_min,
646  translational_max,
647  rotational_rmse,
648  rotational_mean,
649  rotational_median,
650  rotational_std,
651  rotational_min,
652  rotational_max);
653 
654  printf(" translational_rmse= %f m\n", translational_rmse);
655  printf(" rotational_rmse= %f deg\n", rotational_rmse);
656 
657  pFile = 0;
658  std::string pathErrors = output+"/"+outputName+"_rmse.txt";
659  pFile = fopen(pathErrors.c_str(),"w");
660  if(!pFile)
661  {
662  UERROR("could not save RMSE results to \"%s\"", pathErrors.c_str());
663  }
664  fprintf(pFile, "Ground truth comparison:\n");
665  fprintf(pFile, " KITTI t_err = %f %%\n", t_err);
666  fprintf(pFile, " KITTI r_err = %f deg/m\n", r_err);
667  fprintf(pFile, " translational_rmse= %f\n", translational_rmse);
668  fprintf(pFile, " translational_mean= %f\n", translational_mean);
669  fprintf(pFile, " translational_median= %f\n", translational_median);
670  fprintf(pFile, " translational_std= %f\n", translational_std);
671  fprintf(pFile, " translational_min= %f\n", translational_min);
672  fprintf(pFile, " translational_max= %f\n", translational_max);
673  fprintf(pFile, " rotational_rmse= %f\n", rotational_rmse);
674  fprintf(pFile, " rotational_mean= %f\n", rotational_mean);
675  fprintf(pFile, " rotational_median= %f\n", rotational_median);
676  fprintf(pFile, " rotational_std= %f\n", rotational_std);
677  fprintf(pFile, " rotational_min= %f\n", rotational_min);
678  fprintf(pFile, " rotational_max= %f\n", rotational_max);
679  fclose(pFile);
680  }
681  }
682  else
683  {
684  UERROR("Camera init failed!");
685  }
686 
687  printf("Saving rtabmap database (with all statistics) to \"%s\"\n", (output+"/"+outputName+".db").c_str());
688  printf("Do:\n"
689  " $ rtabmap-databaseViewer %s\n\n", (output+"/"+outputName+".db").c_str());
690 
691  return 0;
692 }
rtabmap::Memory::getNodeInfo
bool getNodeInfo(int signatureId, Transform &odomPose, int &mapId, int &weight, std::string &label, double &stamp, Transform &groundTruth, std::vector< float > &velocity, GPS &gps, EnvSensors &sensors, bool lookInDatabase=false) const
Definition: Memory.cpp:4021
uStr2Int
int UTILITE_EXP uStr2Int(const std::string &str)
Definition: UConversion.cpp:125
UDirectory::homeDir
static std::string homeDir()
Definition: UDirectory.cpp:355
rtabmap::Parameters::parse
static bool parse(const ParametersMap &parameters, const std::string &key, bool &value)
Definition: Parameters.cpp:497
UTimer::restart
double restart()
Definition: UTimer.h:94
UTimer
Definition: UTimer.h:46
sighandler
void sighandler(int sig)
Definition: tools/KittiDataset/main.cpp:89
UDirectory::makeDir
static bool makeDir(const std::string &dirPath)
Definition: UDirectory.cpp:333
rtabmap::CameraThread::setStereoExposureCompensation
void setStereoExposureCompensation(bool enabled)
Definition: CameraThread.h:80
f
f
rtabmap::Parameters::showUsage
static const char * showUsage()
Definition: Parameters.cpp:569
rtabmap::Rtabmap::getStatistics
const Statistics & getStatistics() const
Definition: Rtabmap.cpp:830
rtabmap::ParametersPair
std::pair< std::string, std::string > ParametersPair
Definition: Parameters.h:44
data
data
rtabmap::Parameters::parseArguments
static ParametersMap parseArguments(int argc, char *argv[], bool onlyParameters=false)
Definition: Parameters.cpp:596
UFile::erase
static int erase(const std::string &filePath)
Definition: UFile.cpp:58
glm::e
GLM_FUNC_DECL genType e()
rtabmap::ParametersMap
std::map< std::string, std::string > ParametersMap
Definition: Parameters.h:43
rtabmap::SensorData::keypoints
const std::vector< cv::KeyPoint > & keypoints() const
Definition: SensorData.h:270
rtabmap::Rtabmap::init
void init(const ParametersMap &parameters, const std::string &databasePath="", bool loadDatabaseParameters=false)
Definition: Rtabmap.cpp:310
UMath.h
Basic mathematics functions.
rtabmap::SensorData::setId
void setId(int id)
Definition: SensorData.h:175
UConversion.h
Some conversion functions.
ULogger::setLevel
static void setLevel(ULogger::Level level)
Definition: ULogger.h:339
rtabmap::Odometry::process
Transform process(SensorData &data, OdometryInfo *info=0)
Definition: Odometry.cpp:291
util3d_registration.h
showUsage
void showUsage()
Definition: tools/KittiDataset/main.cpp:49
rtabmap::Statistics::data
const std::map< std::string, float > & data() const
Definition: Statistics.h:295
rtabmap::Camera::init
virtual bool init(const std::string &calibrationFolder=".", const std::string &cameraName="")=0
rtabmap::Camera::takeImage
SensorData takeImage(CameraInfo *info=0)
Definition: Camera.cpp:72
UStl.h
Wrappers of STL for convenient functions.
uSplit
std::list< std::string > uSplit(const std::string &str, char separator=' ')
Definition: UStl.h:566
rtabmap::CameraInfo::timeStereoExposureCompensation
float timeStereoExposureCompensation
Definition: CameraInfo.h:63
g_forever
bool g_forever
Definition: tools/KittiDataset/main.cpp:88
rtabmap::Odometry::create
static Odometry * create(const ParametersMap &parameters=ParametersMap())
Definition: Odometry.cpp:57
rtabmap::CameraThread::setScanParameters
void setScanParameters(bool fromDepth, int downsampleStep=1, float rangeMin=0.0f, float rangeMax=0.0f, float voxelSize=0.0f, int normalsK=0, int normalsRadius=0.0f, float groundNormalsUp=0.0f)
Definition: CameraThread.cpp:217
rtabmap::Rtabmap::getMemory
const Memory * getMemory() const
Definition: Rtabmap.h:147
rtabmap::graph::calcRMSE
Transform RTABMAP_EXP calcRMSE(const std::map< int, Transform > &groundTruth, const std::map< int, Transform > &poses, float &translational_rmse, float &translational_mean, float &translational_median, float &translational_std, float &translational_min, float &translational_max, float &rotational_rmse, float &rotational_mean, float &rotational_median, float &rotational_std, float &rotational_min, float &rotational_max)
Definition: Graph.cpp:759
ULogger::setType
static void setType(Type type, const std::string &fileName=kDefaultLogFileName, bool append=true)
Definition: ULogger.cpp:176
rtabmap::Rtabmap::process
bool process(const SensorData &data, Transform odomPose, const cv::Mat &odomCovariance=cv::Mat::eye(6, 6, CV_64FC1), const std::vector< float > &odomVelocity=std::vector< float >(), const std::map< std::string, float > &externalStats=std::map< std::string, float >())
Main loop of rtabmap.
Definition: Rtabmap.cpp:1151
uReplaceChar
std::string UTILITE_EXP uReplaceChar(const std::string &str, char before, char after)
Definition: UConversion.cpp:33
rtabmap::graph::exportPoses
bool RTABMAP_EXP exportPoses(const std::string &filePath, int format, const std::map< int, Transform > &poses, const std::multimap< int, Link > &constraints=std::multimap< int, Link >(), const std::map< int, double > &stamps=std::map< int, double >(), const ParametersMap &parameters=ParametersMap())
Definition: Graph.cpp:54
rtabmap::OdometryEvent::velocity
std::vector< float > velocity() const
Definition: OdometryEvent.h:73
rtabmap::Transform::isNull
bool isNull() const
Definition: Transform.cpp:107
rtabmap::CameraThread::postUpdate
void postUpdate(SensorData *data, CameraInfo *info=0) const
Definition: CameraThread.cpp:350
uValues
std::vector< V > uValues(const std::multimap< K, V > &mm)
Definition: UStl.h:100
UDirectory::getNextFilePath
std::string getNextFilePath()
Definition: UDirectory.cpp:232
rtabmap::CameraThread::setStereoToDepth
void setStereoToDepth(bool enabled)
Definition: CameraThread.h:83
rtabmap::SensorData::laserScanRaw
const LaserScan & laserScanRaw() const
Definition: SensorData.h:185
UDirectory::getNextFileName
std::string getNextFileName()
Definition: UDirectory.cpp:221
UFile::exists
bool exists()
Definition: UFile.h:104
UERROR
#define UERROR(...)
rtabmap_oanet.argv
argv
Definition: rtabmap_oanet.py:12
UWARN
#define UWARN(...)
rtabmap::Rtabmap::getGraph
void getGraph(std::map< int, Transform > &poses, std::multimap< int, Link > &constraints, bool optimized, bool global, std::map< int, Signature > *signatures=0, bool withImages=false, bool withScan=false, bool withUserData=false, bool withGrid=false, bool withWords=true, bool withGlobalDescriptors=true) const
Definition: Rtabmap.cpp:5189
rtabmap::CameraInfo::timeImageDecimation
float timeImageDecimation
Definition: CameraInfo.h:64
UTimer::ticks
double ticks()
Definition: UTimer.cpp:117
trace.model
model
Definition: trace.py:4
rtabmap::OdometryInfo::reg
RegistrationInfo reg
Definition: OdometryInfo.h:97
rtabmap::EnvSensors
std::map< EnvSensor::Type, EnvSensor > EnvSensors
Definition: EnvSensor.h:81
main
int main(int argc, char *argv[])
Definition: tools/KittiDataset/main.cpp:95
rtabmap::CameraInfo::timeBilateralFiltering
float timeBilateralFiltering
Definition: CameraInfo.h:67
rtabmap::SensorData::isValid
bool isValid() const
Definition: SensorData.h:156
rtabmap::LaserScan::size
int size() const
Definition: LaserScan.h:126
rtabmap::SensorData::setFeatures
void setFeatures(const std::vector< cv::KeyPoint > &keypoints, const std::vector< cv::Point3f > &keypoints3D, const cv::Mat &descriptors)
Definition: SensorData.cpp:801
rtabmap::Rtabmap::getLoopClosureId
int getLoopClosureId() const
Definition: Rtabmap.h:128
rtabmap::graph::calcKittiSequenceErrors
void RTABMAP_EXP calcKittiSequenceErrors(const std::vector< Transform > &poses_gt, const std::vector< Transform > &poses_result, float &t_err, float &r_err)
Definition: Graph.cpp:649
rtabmap::CameraInfo::timeUndistortDepth
float timeUndistortDepth
Definition: CameraInfo.h:66

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