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/Graph.h"
32 #include "rtabmap/core/OdometryInfo.h"
33 #include "rtabmap/core/OdometryEvent.h"
34 #include "rtabmap/core/Memory.h"
35 #include "rtabmap/core/util3d_registration.h"
36 #include "rtabmap/utilite/UConversion.h"
37 #include "rtabmap/utilite/UDirectory.h"
38 #include "rtabmap/utilite/UFile.h"
39 #include "rtabmap/utilite/UMath.h"
40 #include "rtabmap/utilite/UStl.h"
41 #include "rtabmap/utilite/UProcessInfo.h"
42 #include <pcl/common/common.h>
43 #include <rtabmap/core/SensorCaptureThread.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  SensorCaptureThread 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  SensorCaptureInfo cameraInfo;
438  SensorData data = cameraThread.camera()->takeData(&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/HistogramEqualization/ms", cameraInfo.timeHistogramEqualization*1000.0f));
495  externalStats.insert(std::make_pair("Camera/ExposureCompensation/ms", cameraInfo.timeStereoExposureCompensation*1000.0f));
496  externalStats.insert(std::make_pair("Camera/ScanFromDepth/ms", cameraInfo.timeScanFromDepth*1000.0f));
497  externalStats.insert(std::make_pair("Camera/TotalTime/ms", cameraInfo.timeTotal*1000.0f));
498  externalStats.insert(std::make_pair("Camera/UndistortDepth/ms", cameraInfo.timeUndistortDepth*1000.0f));
499  // save odometry statistics to database
500  externalStats.insert(std::make_pair("Odometry/LocalBundle/ms", odomInfo.localBundleTime*1000.0f));
501  externalStats.insert(std::make_pair("Odometry/LocalBundleConstraints/", odomInfo.localBundleConstraints));
502  externalStats.insert(std::make_pair("Odometry/LocalBundleOutliers/", odomInfo.localBundleOutliers));
503  externalStats.insert(std::make_pair("Odometry/TotalTime/ms", odomInfo.timeEstimation*1000.0f));
504  externalStats.insert(std::make_pair("Odometry/Registration/ms", odomInfo.reg.totalTime*1000.0f));
505  externalStats.insert(std::make_pair("Odometry/Speed/kph", speed));
506  externalStats.insert(std::make_pair("Odometry/Inliers/", odomInfo.reg.inliers));
507  externalStats.insert(std::make_pair("Odometry/Features/", odomInfo.features));
508  externalStats.insert(std::make_pair("Odometry/DistanceTravelled/m", odomInfo.distanceTravelled));
509  externalStats.insert(std::make_pair("Odometry/KeyFrameAdded/", odomInfo.keyFrameAdded));
510  externalStats.insert(std::make_pair("Odometry/LocalKeyFrames/", odomInfo.localKeyFrames));
511  externalStats.insert(std::make_pair("Odometry/LocalMapSize/", odomInfo.localMapSize));
512  externalStats.insert(std::make_pair("Odometry/LocalScanMapSize/", odomInfo.localScanMapSize));
513 
514  OdometryEvent e(SensorData(), Transform(), odomInfo);
515  rtabmap.process(data, pose, covariance, e.velocity(), externalStats);
516  covariance = cv::Mat();
517  }
518 
519  ++iteration;
520  if(!quiet || iteration == totalImages)
521  {
522  double slamTime = timer.ticks();
523 
524  float rmse = -1;
525  if(rtabmap.getStatistics().data().find(Statistics::kGtTranslational_rmse()) != rtabmap.getStatistics().data().end())
526  {
527  rmse = rtabmap.getStatistics().data().at(Statistics::kGtTranslational_rmse());
528  }
529 
530  if(data.keypoints().size() == 0 && data.laserScanRaw().size())
531  {
532  if(rmse >= 0.0f)
533  {
534  //printf("Iteration %d/%d: speed=%dkm/h camera=%dms, odom(quality=%f, kfs=%d)=%dms, slam=%dms, rmse=%fm, noise stddev=%fm %frad",
535  // 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)));
536  printf("Iteration %d/%d: speed=%dkm/h camera=%dms, odom(quality=%f, kfs=%d)=%dms, slam=%dms, rmse=%fm",
537  iteration, totalImages, int(speed), int(cameraInfo.timeTotal*1000.0f), odomInfo.reg.icpInliersRatio, odomKeyFrames, int(odomInfo.timeEstimation*1000.0f), int(slamTime*1000.0f), rmse);
538  }
539  else
540  {
541  printf("Iteration %d/%d: speed=%dkm/h camera=%dms, odom(quality=%f, kfs=%d)=%dms, slam=%dms",
542  iteration, totalImages, int(speed), int(cameraInfo.timeTotal*1000.0f), odomInfo.reg.icpInliersRatio, odomKeyFrames, int(odomInfo.timeEstimation*1000.0f), int(slamTime*1000.0f));
543  }
544  }
545  else
546  {
547  if(rmse >= 0.0f)
548  {
549  //printf("Iteration %d/%d: speed=%dkm/h camera=%dms, odom(quality=%d/%d, kfs=%d)=%dms, slam=%dms, rmse=%fm, noise stddev=%fm %frad",
550  // 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)));
551  printf("Iteration %d/%d: speed=%dkm/h camera=%dms, odom(quality=%d/%d, kfs=%d)=%dms, slam=%dms, rmse=%fm",
552  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);
553  }
554  else
555  {
556  printf("Iteration %d/%d: speed=%dkm/h camera=%dms, odom(quality=%d/%d, kfs=%d)=%dms, slam=%dms",
557  iteration, totalImages, int(speed), int(cameraInfo.timeTotal*1000.0f), odomInfo.reg.inliers, odomInfo.features, odomKeyFrames, int(odomInfo.timeEstimation*1000.0f), int(slamTime*1000.0f));
558  }
559  }
560  if(processData && rtabmap.getLoopClosureId()>0)
561  {
562  printf(" *");
563  }
564  printf("\n");
565  }
566  else if(iteration % (totalImages/10) == 0)
567  {
568  printf(".");
569  fflush(stdout);
570  }
571 
572  cameraInfo = SensorCaptureInfo();
573  timer.restart();
574  data = cameraThread.camera()->takeData(&cameraInfo);
575  }
576  delete odom;
577  printf("Total time=%fs\n", totalTime.ticks());
579  // Processing dataset end
581 
582  // Save trajectory
583  printf("Saving trajectory ...\n");
584  std::map<int, Transform> poses;
585  std::multimap<int, Link> links;
586  rtabmap.getGraph(poses, links, true, true);
587  std::string pathTrajectory = output+"/"+outputName+"_poses.txt";
588  if(poses.size() && graph::exportPoses(pathTrajectory, 2, poses, links))
589  {
590  printf("Saving %s... done!\n", pathTrajectory.c_str());
591  }
592  else
593  {
594  printf("Saving %s... failed!\n", pathTrajectory.c_str());
595  }
596 
597  if(!gtPath.empty())
598  {
599  // Log ground truth statistics
600  std::map<int, Transform> groundTruth;
601 
602  for(std::map<int, Transform>::const_iterator iter=poses.begin(); iter!=poses.end(); ++iter)
603  {
604  Transform o, gtPose;
605  int m,w;
606  std::string l;
607  double s;
608  std::vector<float> v;
609  GPS gps;
610  EnvSensors sensors;
611  rtabmap.getMemory()->getNodeInfo(iter->first, o, m, w, l, s, gtPose, v, gps, sensors, true);
612  if(!gtPose.isNull())
613  {
614  groundTruth.insert(std::make_pair(iter->first, gtPose));
615  }
616  }
617 
618  // compute KITTI statistics
619  float t_err = 0.0f;
620  float r_err = 0.0f;
621  graph::calcKittiSequenceErrors(uValues(groundTruth), uValues(poses), t_err, r_err);
622  printf("Ground truth comparison:\n");
623  printf(" KITTI t_err = %f %%\n", t_err);
624  printf(" KITTI r_err = %f deg/m\n", r_err);
625 
626  // compute RMSE statistics
627  float translational_rmse = 0.0f;
628  float translational_mean = 0.0f;
629  float translational_median = 0.0f;
630  float translational_std = 0.0f;
631  float translational_min = 0.0f;
632  float translational_max = 0.0f;
633  float rotational_rmse = 0.0f;
634  float rotational_mean = 0.0f;
635  float rotational_median = 0.0f;
636  float rotational_std = 0.0f;
637  float rotational_min = 0.0f;
638  float rotational_max = 0.0f;
639  graph::calcRMSE(
640  groundTruth,
641  poses,
642  translational_rmse,
643  translational_mean,
644  translational_median,
645  translational_std,
646  translational_min,
647  translational_max,
648  rotational_rmse,
649  rotational_mean,
650  rotational_median,
651  rotational_std,
652  rotational_min,
653  rotational_max);
654 
655  printf(" translational_rmse= %f m\n", translational_rmse);
656  printf(" rotational_rmse= %f deg\n", rotational_rmse);
657 
658  pFile = 0;
659  std::string pathErrors = output+"/"+outputName+"_rmse.txt";
660  pFile = fopen(pathErrors.c_str(),"w");
661  if(!pFile)
662  {
663  UERROR("could not save RMSE results to \"%s\"", pathErrors.c_str());
664  }
665  fprintf(pFile, "Ground truth comparison:\n");
666  fprintf(pFile, " KITTI t_err = %f %%\n", t_err);
667  fprintf(pFile, " KITTI r_err = %f deg/m\n", r_err);
668  fprintf(pFile, " translational_rmse= %f\n", translational_rmse);
669  fprintf(pFile, " translational_mean= %f\n", translational_mean);
670  fprintf(pFile, " translational_median= %f\n", translational_median);
671  fprintf(pFile, " translational_std= %f\n", translational_std);
672  fprintf(pFile, " translational_min= %f\n", translational_min);
673  fprintf(pFile, " translational_max= %f\n", translational_max);
674  fprintf(pFile, " rotational_rmse= %f\n", rotational_rmse);
675  fprintf(pFile, " rotational_mean= %f\n", rotational_mean);
676  fprintf(pFile, " rotational_median= %f\n", rotational_median);
677  fprintf(pFile, " rotational_std= %f\n", rotational_std);
678  fprintf(pFile, " rotational_min= %f\n", rotational_min);
679  fprintf(pFile, " rotational_max= %f\n", rotational_max);
680  fclose(pFile);
681  }
682  }
683  else
684  {
685  UERROR("Camera init failed!");
686  }
687 
688  printf("Saving rtabmap database (with all statistics) to \"%s\"\n", (output+"/"+outputName+".db").c_str());
689  printf("Do:\n"
690  " $ rtabmap-databaseViewer %s\n\n", (output+"/"+outputName+".db").c_str());
691 
692  return 0;
693 }
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
w
RowVector3d w
int
int
rtabmap::OdometryEvent
Definition: OdometryEvent.h:39
rtabmap::Odometry::create
static Odometry * create(const ParametersMap &parameters=ParametersMap())
Definition: Odometry.cpp:58
rtabmap::CameraImages
Definition: CameraImages.h:39
rtabmap::OdometryInfo::localMapSize
int localMapSize
Definition: OdometryInfo.h:101
rtabmap::StereoCameraModel
Definition: StereoCameraModel.h:35
UFile::erase
static int erase(const std::string &filePath)
Definition: UFile.cpp:58
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
UDirectory::makeDir
static bool makeDir(const std::string &dirPath)
Definition: UDirectory.cpp:333
timer
s
RealScalar s
rtabmap::OdometryInfo::localBundleTime
float localBundleTime
Definition: OdometryInfo.h:106
rtabmap::Parameters::parseArguments
static ParametersMap parseArguments(int argc, char *argv[], bool onlyParameters=false)
Definition: Parameters.cpp:599
UProcessInfo.h
rtabmap::OdometryInfo::keyFrameAdded
bool keyFrameAdded
Definition: OdometryInfo.h:109
rtabmap::OdometryInfo::timeEstimation
float timeEstimation
Definition: OdometryInfo.h:111
UDirectory.h
ULogger::kTypeConsole
@ kTypeConsole
Definition: ULogger.h:244
rtabmap::OdometryInfo::features
int features
Definition: OdometryInfo.h:100
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::GPS
Definition: GPS.h:35
CameraStereo.h
timer::restart
void restart()
rtabmap::SensorCaptureInfo::timeHistogramEqualization
float timeHistogramEqualization
Definition: SensorCaptureInfo.h:69
rtabmap::SensorCaptureInfo::timeCapture
float timeCapture
Definition: SensorCaptureInfo.h:63
rtabmap::OdometryInfo::localKeyFrames
int localKeyFrames
Definition: OdometryInfo.h:103
rtabmap::ParametersMap
std::map< std::string, std::string > ParametersMap
Definition: Parameters.h:43
stdout
stdout
rtabmap::graph::calcRMSE
Transform RTABMAP_CORE_EXPORT 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
rtabmap::Transform::isNull
bool isNull() const
Definition: Transform.cpp:107
UMath.h
Basic mathematics functions.
rtabmap::OdometryInfo::transform
Transform transform
Definition: OdometryInfo.h:115
rtabmap::Parameters::parse
static bool parse(const ParametersMap &parameters, const std::string &key, bool &value)
Definition: Parameters.cpp:500
rtabmap::SensorCaptureInfo
Definition: SensorCaptureInfo.h:36
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
rtabmap::SensorCapture::init
virtual bool init(const std::string &calibrationFolder=".", const std::string &cameraName="")=0
rtabmap::graph::calcKittiSequenceErrors
void RTABMAP_CORE_EXPORT calcKittiSequenceErrors(const std::vector< Transform > &poses_gt, const std::vector< Transform > &poses_result, float &t_err, float &r_err)
Definition: Graph.cpp:649
rtabmap::SensorCaptureInfo::timeScanFromDepth
float timeScanFromDepth
Definition: SensorCaptureInfo.h:70
seq
ArithmeticSequence< FirstTypeDerived, symbolic::AddExpr< symbolic::AddExpr< LastTypeDerived, symbolic::NegateExpr< FirstTypeDerived > >, symbolic::ValueExpr< internal::FixedInt< 1 > > > > seq(const symbolic::BaseExpr< FirstTypeDerived > &f, const symbolic::BaseExpr< LastTypeDerived > &l)
data
int data[]
rtabmap::CameraStereoImages
Definition: CameraStereoImages.h:39
UConversion.h
Some conversion functions.
UDirectory::getNextFilePath
std::string getNextFilePath()
Definition: UDirectory.cpp:232
sighandler
void sighandler(int sig)
Definition: tools/KittiDataset/main.cpp:89
rtabmap::OdometryInfo::localBundleConstraints
int localBundleConstraints
Definition: OdometryInfo.h:105
rtabmap::OdometryInfo::localScanMapSize
int localScanMapSize
Definition: OdometryInfo.h:102
Rtabmap.h
rtabmap::Odometry::process
Transform process(SensorData &data, OdometryInfo *info=0)
Definition: Odometry.cpp:298
rtabmap::Transform::x
float & x()
Definition: Transform.h:92
ULogger::kWarning
@ kWarning
Definition: ULogger.h:252
rtabmap::SensorCaptureThread::setStereoExposureCompensation
void setStereoExposureCompensation(bool enabled)
Definition: SensorCaptureThread.h:124
rtabmap::RegistrationInfo::covariance
cv::Mat covariance
Definition: RegistrationInfo.h:75
rtabmap::SensorCapture::takeData
SensorData takeData(SensorCaptureInfo *info=0)
Definition: SensorCapture.cpp:64
rtabmap::RegistrationInfo::icpInliersRatio
float icpInliersRatio
Definition: RegistrationInfo.h:90
rtabmap::RegistrationInfo::inliers
int inliers
Definition: RegistrationInfo.h:80
rtabmap_netvlad.argv
argv
Definition: rtabmap_netvlad.py:15
m
Matrix3f m
ULogger::setType
static void setType(Type type, const std::string &fileName=kDefaultLogFileName, bool append=true)
Definition: ULogger.cpp:176
rtabmap::SensorCaptureInfo::timeBilateralFiltering
float timeBilateralFiltering
Definition: SensorCaptureInfo.h:72
OdometryEvent.h
rtabmap::Camera
Definition: Camera.h:43
rtabmap::SensorCaptureInfo::timeTotal
float timeTotal
Definition: SensorCaptureInfo.h:73
rtabmap::SensorCaptureThread::setStereoToDepth
void setStereoToDepth(bool enabled)
Definition: SensorCaptureThread.h:128
rtabmap::SensorCaptureInfo::timeImageDecimation
float timeImageDecimation
Definition: SensorCaptureInfo.h:68
path
path
UWARN
#define UWARN(...)
rtabmap::Parameters::showUsage
static const char * showUsage()
Definition: Parameters.cpp:572
f
Point2(* f)(const Point3 &, OptionalJacobian< 2, 3 >)
uReplaceChar
std::string UTILITE_EXPORT uReplaceChar(const std::string &str, char before, char after)
Definition: UConversion.cpp:33
e
Array< double, 1, 3 > e(1./3., 0.5, 2.)
g_forever
bool g_forever
Definition: tools/KittiDataset/main.cpp:88
rtabmap::RegistrationInfo::totalTime
double totalTime
Definition: RegistrationInfo.h:77
rtabmap::Transform
Definition: Transform.h:41
Memory.h
UTimer::ticks
double ticks()
Definition: UTimer.cpp:117
uSplit
std::list< std::string > uSplit(const std::string &str, char separator=' ')
Definition: UStl.h:564
rtabmap::EnvSensors
std::map< EnvSensor::Type, EnvSensor > EnvSensors
Definition: EnvSensor.h:81
uStr2Int
int UTILITE_EXPORT uStr2Int(const std::string &str)
Definition: UConversion.cpp:125
rtabmap::OdometryInfo::distanceTravelled
float distanceTravelled
Definition: OdometryInfo.h:120
Graph.h
UDirectory::getNextFileName
std::string getNextFileName()
Definition: UDirectory.cpp:221
iter
iterator iter(handle obj)
rtabmap::SensorCaptureInfo::timeDisparity
float timeDisparity
Definition: SensorCaptureInfo.h:65
util3d_registration.h
camera
Camera camera(Pose3(Rot3().retract(Vector3(0.1, 0.2, 0.3)), Point3(0, 5, 0)), Cal3Bundler0(1, 0, 0))
c_str
const char * c_str(Args &&...args)
UStl.h
Wrappers of STL for convenient functions.
showUsage
void showUsage()
Definition: tools/KittiDataset/main.cpp:49
rtabmap::SensorCaptureInfo::timeStereoExposureCompensation
float timeStereoExposureCompensation
Definition: SensorCaptureInfo.h:67
rtabmap::SensorCaptureInfo::timeMirroring
float timeMirroring
Definition: SensorCaptureInfo.h:66
rtabmap::OdometryInfo::interval
double interval
Definition: OdometryInfo.h:114
v
Array< int, Dynamic, 1 > v
rtabmap::Odometry
Definition: Odometry.h:42
rtabmap::OdometryInfo::reg
RegistrationInfo reg
Definition: OdometryInfo.h:99
UTimer
Definition: UTimer.h:46
rtabmap::OdometryInfo
Definition: OdometryInfo.h:40
P0
const Point3 P0(0, 0, 0)
main
int main(int argc, char *argv[])
Definition: tools/KittiDataset/main.cpp:95
rtabmap::Rtabmap
Definition: Rtabmap.h:54
uValues
std::vector< V > uValues(const std::multimap< K, V > &mm)
Definition: UStl.h:100
rtabmap::SensorCaptureThread::camera
Camera * camera()
Definition: SensorCaptureThread.h:169
rtabmap::SensorCaptureInfo::timeUndistortDepth
float timeUndistortDepth
Definition: SensorCaptureInfo.h:71
UFile.h
rtabmap
Definition: CameraARCore.cpp:35
UDirectory
Definition: UDirectory.h:34
UFile::exists
bool exists()
Definition: UFile.h:104
UERROR
#define UERROR(...)
trace.model
model
Definition: trace.py:4
i
int i
rtabmap::OdometryInfo::localBundleOutliers
int localBundleOutliers
Definition: OdometryInfo.h:104

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