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.\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  Transform(-0.27f, 0.0f, 0.08+(height?1.67f:0.0f), 0.0f, 0.0f, 0.0f));
383  cameraThread.setScanParameters(
384  false,
385  scanStep,
386  0,
387  0,
388  scanVoxel,
389  scanNormalK,
390  scanNormalRadius,
391  true);
392  }
393 
394  float detectionRate = Parameters::defaultRtabmapDetectionRate();
395  bool intermediateNodes = Parameters::defaultRtabmapCreateIntermediateNodes();
396  int odomStrategy = Parameters::defaultOdomStrategy();
397  Parameters::parse(parameters, Parameters::kOdomStrategy(), odomStrategy);
398  Parameters::parse(parameters, Parameters::kRtabmapDetectionRate(), detectionRate);
399  Parameters::parse(parameters, Parameters::kRtabmapCreateIntermediateNodes(), intermediateNodes);
400 
401  // assuming source is 10 Hz
402  int mapUpdate = detectionRate>0?10 / detectionRate:1;
403  if(mapUpdate < 1)
404  {
405  mapUpdate = 1;
406  }
407 
408  std::string databasePath = output+"/"+outputName+".db";
409  UFile::erase(databasePath);
410  if(cameraThread.camera()->init(output, outputName+"_calib"))
411  {
412  int totalImages = (int)((CameraStereoImages*)cameraThread.camera())->filenames().size();
413 
414  printf("Processing %d images...\n", totalImages);
415 
416  ParametersMap odomParameters = parameters;
417  odomParameters.erase(Parameters::kRtabmapPublishRAMUsage()); // as odometry is in the same process than rtabmap, don't get RAM usage in odometry.
418  Odometry * odom = Odometry::create(odomParameters);
419  Rtabmap rtabmap;
420  rtabmap.init(parameters, databasePath);
421 
422  UTimer totalTime;
423  UTimer timer;
424  CameraInfo cameraInfo;
425  SensorData data = cameraThread.camera()->takeImage(&cameraInfo);
426  int iteration = 0;
427 
429  // Processing dataset begin
431  cv::Mat covariance;
432  int odomKeyFrames = 0;
433  while(data.isValid() && g_forever)
434  {
435  cameraThread.postUpdate(&data, &cameraInfo);
436  cameraInfo.timeTotal = timer.ticks();
437 
438 
439  OdometryInfo odomInfo;
440  Transform pose = odom->process(data, &odomInfo);
441  float speed = 0.0f;
442  if(odomInfo.interval>0.0)
443  speed = odomInfo.transform.x()/odomInfo.interval*3.6;
444  if(odomInfo.keyFrameAdded)
445  {
446  ++odomKeyFrames;
447  }
448 
449  if(odomStrategy == 2)
450  {
451  //special case for FOVIS, set covariance 1 if 9999 is detected
452  if(!odomInfo.reg.covariance.empty() && odomInfo.reg.covariance.at<double>(0,0) >= 9999)
453  {
454  odomInfo.reg.covariance = cv::Mat::eye(6,6,CV_64FC1);
455  }
456  }
457 
458  bool processData = true;
459  if(iteration % mapUpdate != 0)
460  {
461  // set negative id so rtabmap will detect it as an intermediate node
462  data.setId(-1);
463  data.setFeatures(std::vector<cv::KeyPoint>(), std::vector<cv::Point3f>(), cv::Mat());// remove features
464  processData = intermediateNodes;
465  }
466  if(covariance.empty() || odomInfo.reg.covariance.at<double>(0,0) > covariance.at<double>(0,0))
467  {
468  covariance = odomInfo.reg.covariance;
469  }
470 
471  timer.restart();
472  if(processData)
473  {
474  std::map<std::string, float> externalStats;
475  // save camera statistics to database
476  externalStats.insert(std::make_pair("Camera/BilateralFiltering/ms", cameraInfo.timeBilateralFiltering*1000.0f));
477  externalStats.insert(std::make_pair("Camera/Capture/ms", cameraInfo.timeCapture*1000.0f));
478  externalStats.insert(std::make_pair("Camera/Disparity/ms", cameraInfo.timeDisparity*1000.0f));
479  externalStats.insert(std::make_pair("Camera/ImageDecimation/ms", cameraInfo.timeImageDecimation*1000.0f));
480  externalStats.insert(std::make_pair("Camera/Mirroring/ms", cameraInfo.timeMirroring*1000.0f));
481  externalStats.insert(std::make_pair("Camera/ExposureCompensation/ms", cameraInfo.timeStereoExposureCompensation*1000.0f));
482  externalStats.insert(std::make_pair("Camera/ScanFromDepth/ms", cameraInfo.timeScanFromDepth*1000.0f));
483  externalStats.insert(std::make_pair("Camera/TotalTime/ms", cameraInfo.timeTotal*1000.0f));
484  externalStats.insert(std::make_pair("Camera/UndistortDepth/ms", cameraInfo.timeUndistortDepth*1000.0f));
485  // save odometry statistics to database
486  externalStats.insert(std::make_pair("Odometry/LocalBundle/ms", odomInfo.localBundleTime*1000.0f));
487  externalStats.insert(std::make_pair("Odometry/LocalBundleConstraints/", odomInfo.localBundleConstraints));
488  externalStats.insert(std::make_pair("Odometry/LocalBundleOutliers/", odomInfo.localBundleOutliers));
489  externalStats.insert(std::make_pair("Odometry/TotalTime/ms", odomInfo.timeEstimation*1000.0f));
490  externalStats.insert(std::make_pair("Odometry/Registration/ms", odomInfo.reg.totalTime*1000.0f));
491  externalStats.insert(std::make_pair("Odometry/Speed/kph", speed));
492  externalStats.insert(std::make_pair("Odometry/Inliers/", odomInfo.reg.inliers));
493  externalStats.insert(std::make_pair("Odometry/Features/", odomInfo.features));
494  externalStats.insert(std::make_pair("Odometry/DistanceTravelled/m", odomInfo.distanceTravelled));
495  externalStats.insert(std::make_pair("Odometry/KeyFrameAdded/", odomInfo.keyFrameAdded));
496  externalStats.insert(std::make_pair("Odometry/LocalKeyFrames/", odomInfo.localKeyFrames));
497  externalStats.insert(std::make_pair("Odometry/LocalMapSize/", odomInfo.localMapSize));
498  externalStats.insert(std::make_pair("Odometry/LocalScanMapSize/", odomInfo.localScanMapSize));
499 
500  OdometryEvent e(SensorData(), Transform(), odomInfo);
501  rtabmap.process(data, pose, covariance, e.velocity(), externalStats);
502  covariance = cv::Mat();
503  }
504 
505  ++iteration;
506  if(!quiet || iteration == totalImages)
507  {
508  double slamTime = timer.ticks();
509 
510  float rmse = -1;
511  if(rtabmap.getStatistics().data().find(Statistics::kGtTranslational_rmse()) != rtabmap.getStatistics().data().end())
512  {
513  rmse = rtabmap.getStatistics().data().at(Statistics::kGtTranslational_rmse());
514  }
515 
516  if(data.keypoints().size() == 0 && data.laserScanRaw().size())
517  {
518  if(rmse >= 0.0f)
519  {
520  //printf("Iteration %d/%d: speed=%dkm/h camera=%dms, odom(quality=%f, kfs=%d)=%dms, slam=%dms, rmse=%fm, noise stddev=%fm %frad",
521  // 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)));
522  printf("Iteration %d/%d: speed=%dkm/h camera=%dms, odom(quality=%f, kfs=%d)=%dms, slam=%dms, rmse=%fm",
523  iteration, totalImages, int(speed), int(cameraInfo.timeTotal*1000.0f), odomInfo.reg.icpInliersRatio, odomKeyFrames, int(odomInfo.timeEstimation*1000.0f), int(slamTime*1000.0f), rmse);
524  }
525  else
526  {
527  printf("Iteration %d/%d: speed=%dkm/h camera=%dms, odom(quality=%f, kfs=%d)=%dms, slam=%dms",
528  iteration, totalImages, int(speed), int(cameraInfo.timeTotal*1000.0f), odomInfo.reg.icpInliersRatio, odomKeyFrames, int(odomInfo.timeEstimation*1000.0f), int(slamTime*1000.0f));
529  }
530  }
531  else
532  {
533  if(rmse >= 0.0f)
534  {
535  //printf("Iteration %d/%d: speed=%dkm/h camera=%dms, odom(quality=%d/%d, kfs=%d)=%dms, slam=%dms, rmse=%fm, noise stddev=%fm %frad",
536  // 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)));
537  printf("Iteration %d/%d: speed=%dkm/h camera=%dms, odom(quality=%d/%d, kfs=%d)=%dms, slam=%dms, rmse=%fm",
538  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);
539  }
540  else
541  {
542  printf("Iteration %d/%d: speed=%dkm/h camera=%dms, odom(quality=%d/%d, kfs=%d)=%dms, slam=%dms",
543  iteration, totalImages, int(speed), int(cameraInfo.timeTotal*1000.0f), odomInfo.reg.inliers, odomInfo.features, odomKeyFrames, int(odomInfo.timeEstimation*1000.0f), int(slamTime*1000.0f));
544  }
545  }
546  if(processData && rtabmap.getLoopClosureId()>0)
547  {
548  printf(" *");
549  }
550  printf("\n");
551  }
552  else if(iteration % (totalImages/10) == 0)
553  {
554  printf(".");
555  fflush(stdout);
556  }
557 
558  cameraInfo = CameraInfo();
559  timer.restart();
560  data = cameraThread.camera()->takeImage(&cameraInfo);
561  }
562  delete odom;
563  printf("Total time=%fs\n", totalTime.ticks());
565  // Processing dataset end
567 
568  // Save trajectory
569  printf("Saving trajectory ...\n");
570  std::map<int, Transform> poses;
571  std::multimap<int, Link> links;
572  rtabmap.getGraph(poses, links, true, true);
573  std::string pathTrajectory = output+"/"+outputName+"_poses.txt";
574  if(poses.size() && graph::exportPoses(pathTrajectory, 2, poses, links))
575  {
576  printf("Saving %s... done!\n", pathTrajectory.c_str());
577  }
578  else
579  {
580  printf("Saving %s... failed!\n", pathTrajectory.c_str());
581  }
582 
583  if(!gtPath.empty())
584  {
585  // Log ground truth statistics
586  std::map<int, Transform> groundTruth;
587 
588  for(std::map<int, Transform>::const_iterator iter=poses.begin(); iter!=poses.end(); ++iter)
589  {
590  Transform o, gtPose;
591  int m,w;
592  std::string l;
593  double s;
594  std::vector<float> v;
595  GPS gps;
596  EnvSensors sensors;
597  rtabmap.getMemory()->getNodeInfo(iter->first, o, m, w, l, s, gtPose, v, gps, sensors, true);
598  if(!gtPose.isNull())
599  {
600  groundTruth.insert(std::make_pair(iter->first, gtPose));
601  }
602  }
603 
604  // compute KITTI statistics
605  float t_err = 0.0f;
606  float r_err = 0.0f;
607  graph::calcKittiSequenceErrors(uValues(groundTruth), uValues(poses), t_err, r_err);
608  printf("Ground truth comparison:\n");
609  printf(" KITTI t_err = %f %%\n", t_err);
610  printf(" KITTI r_err = %f deg/m\n", r_err);
611 
612  // compute RMSE statistics
613  float translational_rmse = 0.0f;
614  float translational_mean = 0.0f;
615  float translational_median = 0.0f;
616  float translational_std = 0.0f;
617  float translational_min = 0.0f;
618  float translational_max = 0.0f;
619  float rotational_rmse = 0.0f;
620  float rotational_mean = 0.0f;
621  float rotational_median = 0.0f;
622  float rotational_std = 0.0f;
623  float rotational_min = 0.0f;
624  float rotational_max = 0.0f;
625  graph::calcRMSE(
626  groundTruth,
627  poses,
628  translational_rmse,
629  translational_mean,
630  translational_median,
631  translational_std,
632  translational_min,
633  translational_max,
634  rotational_rmse,
635  rotational_mean,
636  rotational_median,
637  rotational_std,
638  rotational_min,
639  rotational_max);
640 
641  printf(" translational_rmse= %f m\n", translational_rmse);
642  printf(" rotational_rmse= %f deg\n", rotational_rmse);
643 
644  pFile = 0;
645  std::string pathErrors = output+"/"+outputName+"_rmse.txt";
646  pFile = fopen(pathErrors.c_str(),"w");
647  if(!pFile)
648  {
649  UERROR("could not save RMSE results to \"%s\"", pathErrors.c_str());
650  }
651  fprintf(pFile, "Ground truth comparison:\n");
652  fprintf(pFile, " KITTI t_err = %f %%\n", t_err);
653  fprintf(pFile, " KITTI r_err = %f deg/m\n", r_err);
654  fprintf(pFile, " translational_rmse= %f\n", translational_rmse);
655  fprintf(pFile, " translational_mean= %f\n", translational_mean);
656  fprintf(pFile, " translational_median= %f\n", translational_median);
657  fprintf(pFile, " translational_std= %f\n", translational_std);
658  fprintf(pFile, " translational_min= %f\n", translational_min);
659  fprintf(pFile, " translational_max= %f\n", translational_max);
660  fprintf(pFile, " rotational_rmse= %f\n", rotational_rmse);
661  fprintf(pFile, " rotational_mean= %f\n", rotational_mean);
662  fprintf(pFile, " rotational_median= %f\n", rotational_median);
663  fprintf(pFile, " rotational_std= %f\n", rotational_std);
664  fprintf(pFile, " rotational_min= %f\n", rotational_min);
665  fprintf(pFile, " rotational_max= %f\n", rotational_max);
666  fclose(pFile);
667  }
668  }
669  else
670  {
671  UERROR("Camera init failed!");
672  }
673 
674  printf("Saving rtabmap database (with all statistics) to \"%s\"\n", (output+"/"+outputName+".db").c_str());
675  printf("Do:\n"
676  " $ rtabmap-databaseViewer %s\n\n", (output+"/"+outputName+".db").c_str());
677 
678  return 0;
679 }
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:476
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:64
f
f
rtabmap::Parameters::showUsage
static const char * showUsage()
Definition: Parameters.cpp:548
rtabmap::ParametersPair
std::pair< std::string, std::string > ParametersPair
Definition: Parameters.h:44
rtabmap::Parameters::parseArguments
static ParametersMap parseArguments(int argc, char *argv[], bool onlyParameters=false)
Definition: Parameters.cpp:575
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::Rtabmap::init
void init(const ParametersMap &parameters, const std::string &databasePath="", bool loadDatabaseParameters=false)
Definition: Rtabmap.cpp:292
rtabmap::SensorData::keypoints
const std::vector< cv::KeyPoint > & keypoints() const
Definition: SensorData.h:249
UMath.h
Basic mathematics functions.
rtabmap::SensorData::setId
void setId(int id)
Definition: SensorData.h:156
UConversion.h
Some conversion functions.
rtabmap::SensorData::laserScanRaw
const LaserScan & laserScanRaw() const
Definition: SensorData.h:166
rtabmap::LaserScan::size
int size() const
Definition: LaserScan.h:102
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:278
util3d_registration.h
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:4543
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:137
rtabmap::Camera::takeImage
SensorData takeImage(CameraInfo *info=0)
Definition: Camera.cpp:72
UStl.h
Wrappers of STL for convenient functions.
rtabmap::Statistics::data
const std::map< std::string, float > & data() const
Definition: Statistics.h:282
rtabmap::CameraInfo::timeStereoExposureCompensation
float timeStereoExposureCompensation
Definition: CameraInfo.h:63
g_forever
bool g_forever
Definition: tools/KittiDataset/main.cpp:88
rtabmap::Transform::isNull
bool isNull() const
Definition: Transform.cpp:107
rtabmap::Odometry::create
static Odometry * create(const ParametersMap &parameters=ParametersMap())
Definition: Odometry.cpp:54
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:132
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:3863
rtabmap::Rtabmap::getLoopClosureId
int getLoopClosureId() const
Definition: Rtabmap.h:127
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:737
rtabmap::Rtabmap::getStatistics
const Statistics & getStatistics() const
Definition: Rtabmap.cpp:699
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:992
uReplaceChar
std::string UTILITE_EXP uReplaceChar(const std::string &str, char before, char after)
Definition: UConversion.cpp:32
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
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:67
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:218
UERROR
#define UERROR(...)
rtabmap_oanet.argv
argv
Definition: rtabmap_oanet.py:12
UWARN
#define UWARN(...)
rtabmap::Rtabmap::getMemory
const Memory * getMemory() const
Definition: Rtabmap.h:146
rtabmap::CameraInfo::timeImageDecimation
float timeImageDecimation
Definition: CameraInfo.h:64
UTimer::ticks
double ticks()
Definition: UTimer.cpp:117
rtabmap::OdometryInfo::reg
RegistrationInfo reg
Definition: OdometryInfo.h:96
rtabmap::OdometryEvent::velocity
std::vector< float > velocity() const
Definition: OdometryEvent.h:73
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::setFeatures
void setFeatures(const std::vector< cv::KeyPoint > &keypoints, const std::vector< cv::Point3f > &keypoints3D, const cv::Mat &descriptors)
Definition: SensorData.cpp:757
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:627
rtabmap::CameraInfo::timeUndistortDepth
float timeUndistortDepth
Definition: CameraInfo.h:66

rtabmap
Author(s): Mathieu Labbe
autogenerated on Mon Dec 14 2020 03:34:59