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

rtabmap
Author(s): Mathieu Labbe
autogenerated on Wed Jun 5 2019 22:41:31