tools/EurocDataset/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 
14  names of its contributors may be used to endorse or promote products
15  derived from this software without specific prior written permission.
16 
17 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
18 ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
19 WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
20 DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
21 DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
22 (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
23 LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
24 ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
25 (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
26 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
27 */
28 
29 #include <rtabmap/core/OdometryF2M.h>
30 #include "rtabmap/core/Rtabmap.h"
31 #include "rtabmap/core/CameraStereo.h"
32 #include "rtabmap/core/CameraThread.h"
33 #include "rtabmap/core/Graph.h"
34 #include "rtabmap/core/OdometryInfo.h"
35 #include "rtabmap/core/OdometryEvent.h"
36 #include "rtabmap/core/Memory.h"
37 #include "rtabmap/core/util3d_registration.h"
38 #include "rtabmap/utilite/UConversion.h"
39 #include "rtabmap/utilite/UDirectory.h"
40 #include "rtabmap/utilite/UFile.h"
41 #include "rtabmap/utilite/UMath.h"
42 #include "rtabmap/utilite/UStl.h"
43 #include "rtabmap/utilite/UProcessInfo.h"
44 #include <pcl/common/common.h>
45 #include <yaml-cpp/yaml.h>
46 #include <stdio.h>
47 #include <signal.h>
48 #include <fstream>
49 
50 using namespace rtabmap;
51 
52 void showUsage()
53 {
54  printf("\nUsage:\n"
55  "rtabmap-kitti_dataset [options] path\n"
56  " path Folder of the sequence (e.g., \"~/EuRoC/V1_03_difficult\")\n"
57  " containing least mav0/cam0/sensor.yaml, mav0/cam1/sensor.yaml and \n"
58  " mav0/cam0/data and mav0/cam1/data folders.\n"
59  " --output Output directory. By default, results are saved in \"path\".\n"
60  " --output_name Output database name (default \"rtabmap\").\n"
61  " --quiet Don't show log messages and iteration updates.\n"
62  " --exposure_comp Do exposure compensation between left and right images.\n"
63  " --disp Generate full disparity.\n"
64  " --raw Use raw images (not rectified, this only works with okvis and msckf odometry).\n"
65  "%s\n"
66  "Example:\n\n"
67  " $ rtabmap-euroc_dataset \\\n"
68  " --Rtabmap/PublishRAMUsage true\\\n"
69  " --Rtabmap/DetectionRate 2\\\n"
70  " --RGBD/LinearUpdate 0\\\n"
71  " --Mem/STMSize 30\\\n"
72  " ~/EuRoC/V1_03_difficult\n\n", rtabmap::Parameters::showUsage());
73  exit(1);
74 }
75 
76 // catch ctrl-c
77 bool g_forever = true;
78 void sighandler(int sig)
79 {
80  printf("\nSignal %d caught...\n", sig);
81  g_forever = false;
82 }
83 
84 int main(int argc, char * argv[])
85 {
86  signal(SIGABRT, &sighandler);
87  signal(SIGTERM, &sighandler);
88  signal(SIGINT, &sighandler);
89 
90  ULogger::setType(ULogger::kTypeConsole);
91  ULogger::setLevel(ULogger::kWarning);
92 
93  ParametersMap parameters;
94  std::string path;
95  std::string output;
96  std::string outputName = "rtabmap";
97  std::string seq;
98  bool disp = false;
99  bool raw = false;
100  bool exposureCompensation = false;
101  bool quiet = false;
102  if(argc < 2)
103  {
104  showUsage();
105  }
106  else
107  {
108  for(int i=1; i<argc; ++i)
109  {
110  if(std::strcmp(argv[i], "--output") == 0)
111  {
112  output = argv[++i];
113  }
114  else if(std::strcmp(argv[i], "--output_name") == 0)
115  {
116  outputName = argv[++i];
117  }
118  else if(std::strcmp(argv[i], "--quiet") == 0)
119  {
120  quiet = true;
121  }
122  else if(std::strcmp(argv[i], "--disp") == 0)
123  {
124  disp = true;
125  }
126  else if(std::strcmp(argv[i], "--raw") == 0)
127  {
128  raw = true;
129  }
130  else if(std::strcmp(argv[i], "--exposure_comp") == 0)
131  {
132  exposureCompensation = true;
133  }
134  }
135  parameters = Parameters::parseArguments(argc, argv);
136  path = argv[argc-1];
137  path = uReplaceChar(path, '~', UDirectory::homeDir());
138  path = uReplaceChar(path, '\\', '/');
139  if(output.empty())
140  {
141  output = path;
142  }
143  else
144  {
145  output = uReplaceChar(output, '~', UDirectory::homeDir());
146  UDirectory::makeDir(output);
147  }
148  parameters.insert(ParametersPair(Parameters::kRtabmapWorkingDirectory(), output));
149  parameters.insert(ParametersPair(Parameters::kRtabmapPublishRAMUsage(), "true"));
150  if(raw)
151  {
152  parameters.insert(ParametersPair(Parameters::kRtabmapImagesAlreadyRectified(), "false"));
153  }
154  }
155 
156  seq = uSplit(path, '/').back();
157  std::string pathLeftImages = path+"/mav0/cam0/data";
158  std::string pathRightImages = path+"/mav0/cam1/data";
159  std::string pathCalibLeft = path+"/mav0/cam0/sensor.yaml";
160  std::string pathCalibRight = path+"/mav0/cam1/sensor.yaml";
161  std::string pathGt = path+"/mav0/state_groundtruth_estimate0/data.csv";
162  std::string pathImu = path+"/mav0/imu0/data.csv";
163  if(!UFile::exists(pathGt))
164  {
165  UWARN("Ground truth file path doesn't exist: \"%s\", benchmark values won't be computed.", pathGt.c_str());
166  pathGt.clear();
167  }
168 
169  printf("Paths:\n"
170  " Sequence number: %s\n"
171  " Sequence path: %s\n"
172  " Output: %s\n"
173  " Output name: %s\n"
174  " left images: %s\n"
175  " right images: %s\n"
176  " left calib: %s\n"
177  " right calib: %s\n",
178  seq.c_str(),
179  path.c_str(),
180  output.c_str(),
181  outputName.c_str(),
182  pathLeftImages.c_str(),
183  pathRightImages.c_str(),
184  pathCalibLeft.c_str(),
185  pathCalibRight.c_str());
186  if(!pathGt.empty())
187  {
188  printf(" Ground truth: %s\n", pathGt.c_str());
189  }
190  if(!pathImu.empty())
191  {
192  printf(" IMU: %s\n", pathImu.c_str());
193  }
194 
195  printf(" Exposure Compensation: %s\n", exposureCompensation?"true":"false");
196  printf(" Disparity: %s\n", disp?"true":"false");
197  printf(" Raw images: %s\n", raw?"true (Rtabmap/ImagesAlreadyRectified set to false)":"false");
198 
199  if(!parameters.empty())
200  {
201  printf("Parameters:\n");
202  for(ParametersMap::iterator iter=parameters.begin(); iter!=parameters.end(); ++iter)
203  {
204  printf(" %s=%s\n", iter->first.c_str(), iter->second.c_str());
205  }
206  }
207  printf("RTAB-Map version: %s\n", RTABMAP_VERSION);
208 
209  std::vector<CameraModel> models;
210  int rateHz = 20;
211  for(int k=0; k<2; ++k)
212  {
213  // Left calibration
214  std::string calibPath = k==0?pathCalibLeft:pathCalibRight;
215  YAML::Node config = YAML::LoadFile(calibPath);
216  if(config.IsNull())
217  {
218  UERROR("Cannot open calibration file \"%s\"", calibPath.c_str());
219  return -1;
220  }
221 
222  YAML::Node T_BS = config["T_BS"];
223  YAML::Node data = T_BS["data"];
224  UASSERT(data.size() == 16);
225  rateHz = config["rate_hz"].as<int>();
226  YAML::Node resolution = config["resolution"];
227  UASSERT(resolution.size() == 2);
228  YAML::Node intrinsics = config["intrinsics"];
229  UASSERT(intrinsics.size() == 4);
230  YAML::Node distortion_coefficients = config["distortion_coefficients"];
231  UASSERT(distortion_coefficients.size() == 4 || distortion_coefficients.size() == 5 || distortion_coefficients.size() == 8);
232 
233  cv::Mat K = cv::Mat::eye(3, 3, CV_64FC1);
234  K.at<double>(0,0) = intrinsics[0].as<double>();
235  K.at<double>(1,1) = intrinsics[1].as<double>();
236  K.at<double>(0,2) = intrinsics[2].as<double>();
237  K.at<double>(1,2) = intrinsics[3].as<double>();
238  cv::Mat R = cv::Mat::eye(3, 3, CV_64FC1);
239  cv::Mat P = cv::Mat::zeros(3, 4, CV_64FC1);
240  K.copyTo(cv::Mat(P, cv::Range(0,3), cv::Range(0,3)));
241 
242  cv::Mat D = cv::Mat::zeros(1, distortion_coefficients.size(), CV_64FC1);
243  for(unsigned int i=0; i<distortion_coefficients.size(); ++i)
244  {
245  D.at<double>(i) = distortion_coefficients[i].as<double>();
246  }
247 
248  Transform t(data[0].as<float>(), data[1].as<float>(), data[2].as<float>(), data[3].as<float>(),
249  data[4].as<float>(), data[5].as<float>(), data[6].as<float>(), data[7].as<float>(),
250  data[8].as<float>(), data[9].as<float>(), data[10].as<float>(), data[11].as<float>());
251 
252  models.push_back(CameraModel(outputName+"_calib", cv::Size(resolution[0].as<int>(),resolution[1].as<int>()), K, D, R, P, t));
253  UASSERT(models.back().isValidForRectification());
254  }
255 
256  StereoCameraModel model(outputName+"_calib", models[0], models[1], models[1].localTransform().inverse() * models[0].localTransform());
257  if(!model.save(output, false))
258  {
259  UERROR("Could not save calibration!");
260  return -1;
261  }
262  printf("Saved calibration \"%s\" to \"%s\"\n", (outputName+"_calib").c_str(), output.c_str());
263 
264 
265  if(quiet)
266  {
267  ULogger::setLevel(ULogger::kError);
268  }
269 
270  // We use CameraThread only to use postUpdate() method
271  Transform baseToImu(0,0,1,0, 0,-1,0,0, 1,0,0,0);
272  CameraThread cameraThread(new
273  CameraStereoImages(
274  pathLeftImages,
275  pathRightImages,
276  !raw,
277  0.0f,
278  baseToImu*models[0].localTransform()), parameters);
279  printf("baseToImu=%s\n", baseToImu.prettyPrint().c_str());
280  std::cout<<"baseToCam0:\n" << baseToImu*models[0].localTransform() << std::endl;
281  printf("baseToCam0=%s\n", (baseToImu*models[0].localTransform()).prettyPrint().c_str());
282  printf("imuToCam0=%s\n", models[0].localTransform().prettyPrint().c_str());
283  printf("imuToCam1=%s\n", models[1].localTransform().prettyPrint().c_str());
284  ((CameraStereoImages*)cameraThread.camera())->setTimestamps(true, "", false);
285  if(exposureCompensation)
286  {
287  cameraThread.setStereoExposureCompensation(true);
288  }
289  if(disp)
290  {
291  cameraThread.setStereoToDepth(true);
292  }
293  if(!pathGt.empty())
294  {
295  ((CameraStereoImages*)cameraThread.camera())->setGroundTruthPath(pathGt, 9);
296  }
297 
298  float detectionRate = Parameters::defaultRtabmapDetectionRate();
299  bool intermediateNodes = Parameters::defaultRtabmapCreateIntermediateNodes();
300  int odomStrategy = Parameters::defaultOdomStrategy();
301  Parameters::parse(parameters, Parameters::kOdomStrategy(), odomStrategy);
302  Parameters::parse(parameters, Parameters::kRtabmapDetectionRate(), detectionRate);
303  Parameters::parse(parameters, Parameters::kRtabmapCreateIntermediateNodes(), intermediateNodes);
304 
305  int mapUpdate = rateHz / detectionRate;
306  if(mapUpdate < 1)
307  {
308  mapUpdate = 1;
309  }
310 
311  std::ifstream imu_file;
312  if(odomStrategy == Odometry::kTypeOkvis || odomStrategy == Odometry::kTypeMSCKF)
313  {
314  // open the IMU file
315  std::string line;
316  imu_file.open(pathImu.c_str());
317  if (!imu_file.good()) {
318  UERROR("no imu file found at %s",pathImu.c_str());
319  return -1;
320  }
321  int number_of_lines = 0;
322  while (std::getline(imu_file, line))
323  ++number_of_lines;
324  printf("No. IMU measurements: %d\n", number_of_lines-1);
325  if (number_of_lines - 1 <= 0) {
326  UERROR("no imu messages present in %s", pathImu.c_str());
327  return -1;
328  }
329  // set reading position to second line
330  imu_file.clear();
331  imu_file.seekg(0, std::ios::beg);
332  std::getline(imu_file, line);
333 
334  if(odomStrategy == Odometry::kTypeMSCKF)
335  {
336  if(seq.compare("MH_01_easy") == 0)
337  {
338  printf("MH_01_easy detected with MSCFK odometry, ignoring first moving 440 images...\n");
339  ((CameraStereoImages*)cameraThread.camera())->setStartIndex(440);
340  }
341  else if(seq.compare("MH_02_easy") == 0)
342  {
343  printf("MH_02_easy detected with MSCFK odometry, ignoring first moving 525 images...\n");
344  ((CameraStereoImages*)cameraThread.camera())->setStartIndex(525);
345  }
346  else if(seq.compare("MH_03_medium") == 0)
347  {
348  printf("MH_03_medium detected with MSCFK odometry, ignoring first moving 210 images...\n");
349  ((CameraStereoImages*)cameraThread.camera())->setStartIndex(210);
350  }
351  else if(seq.compare("MH_04_difficult") == 0)
352  {
353  printf("MH_04_difficult detected with MSCFK odometry, ignoring first moving 250 images...\n");
354  ((CameraStereoImages*)cameraThread.camera())->setStartIndex(250);
355  }
356  else if(seq.compare("MH_05_difficult") == 0)
357  {
358  printf("MH_05_difficult detected with MSCFK odometry, ignoring first moving 310 images...\n");
359  ((CameraStereoImages*)cameraThread.camera())->setStartIndex(310);
360  }
361  }
362  }
363 
364 
365  std::string databasePath = output+"/"+outputName+".db";
366  UFile::erase(databasePath);
367  if(cameraThread.camera()->init(output, outputName+"_calib"))
368  {
369  int totalImages = (int)((CameraStereoImages*)cameraThread.camera())->filenames().size();
370 
371  printf("Processing %d images...\n", totalImages);
372 
373  ParametersMap odomParameters = parameters;
374  odomParameters.erase(Parameters::kRtabmapPublishRAMUsage()); // as odometry is in the same process than rtabmap, don't get RAM usage in odometry.
375  Odometry * odom = Odometry::create(odomParameters);
376  Rtabmap rtabmap;
377  rtabmap.init(parameters, databasePath);
378 
379  UTimer totalTime;
380  UTimer timer;
381  CameraInfo cameraInfo;
382  UDEBUG("");
383  SensorData data = cameraThread.camera()->takeImage(&cameraInfo);
384  UDEBUG("");
385  int iteration = 0;
386  double start = data.stamp();
387 
389  // Processing dataset begin
391  cv::Mat covariance;
392  int odomKeyFrames = 0;
393  while(data.isValid() && g_forever)
394  {
395  UDEBUG("");
396  if(odomStrategy == Odometry::kTypeOkvis || odomStrategy == Odometry::kTypeMSCKF)
397  {
398  // get all IMU measurements till then
399  double t_imu = start;
400  do {
401  std::string line;
402  if (!std::getline(imu_file, line)) {
403  std::cout << std::endl << "Finished parsing IMU." << std::endl << std::flush;
404  break;
405  }
406 
407  std::stringstream stream(line);
408  std::string s;
409  std::getline(stream, s, ',');
410  std::string nanoseconds = s.substr(s.size() - 9, 9);
411  std::string seconds = s.substr(0, s.size() - 9);
412 
413  cv::Vec3d gyr;
414  for (int j = 0; j < 3; ++j) {
415  std::getline(stream, s, ',');
416  gyr[j] = uStr2Double(s);
417  }
418 
419  cv::Vec3d acc;
420  for (int j = 0; j < 3; ++j) {
421  std::getline(stream, s, ',');
422  acc[j] = uStr2Double(s);
423  }
424 
425  t_imu = double(uStr2Int(seconds)) + double(uStr2Int(nanoseconds))*1e-9;
426 
427  if (t_imu - start + 1 > 0) {
428  SensorData dataImu(IMU(gyr, cv::Mat(3,3,CV_64FC1), acc, cv::Mat(3,3,CV_64FC1), baseToImu), 0, t_imu);
429  UDEBUG("");
430  odom->process(dataImu);
431  UDEBUG("");
432  }
433 
434  } while (t_imu <= data.stamp());
435  }
436 
437 
438  cameraThread.postUpdate(&data, &cameraInfo);
439  cameraInfo.timeTotal = timer.ticks();
440 
441  OdometryInfo odomInfo;
442  UDEBUG("");
443  Transform pose = odom->process(data, &odomInfo);
444  UDEBUG("");
445  if((odomStrategy == Odometry::kTypeOkvis || odomStrategy == Odometry::kTypeMSCKF) &&
446  pose.isNull())
447  {
448  cameraInfo = CameraInfo();
449  timer.restart();
450  data = cameraThread.camera()->takeImage(&cameraInfo);
451  continue;
452  }
453 
454  if(odomInfo.keyFrameAdded)
455  {
456  ++odomKeyFrames;
457  }
458 
459  if(odomStrategy == Odometry::kTypeFovis)
460  {
461  //special case for FOVIS, set covariance 1 if 9999 is detected
462  if(!odomInfo.reg.covariance.empty() && odomInfo.reg.covariance.at<double>(0,0) >= 9999)
463  {
464  odomInfo.reg.covariance = cv::Mat::eye(6,6,CV_64FC1);
465  }
466  }
467 
468  bool processData = true;
469  if(iteration % mapUpdate != 0)
470  {
471  // set negative id so rtabmap will detect it as an intermediate node
472  data.setId(-1);
473  data.setFeatures(std::vector<cv::KeyPoint>(), std::vector<cv::Point3f>(), cv::Mat());// remove features
474  processData = intermediateNodes;
475  }
476  if(covariance.empty() || odomInfo.reg.covariance.at<double>(0,0) > covariance.at<double>(0,0))
477  {
478  covariance = odomInfo.reg.covariance;
479  }
480 
481  timer.restart();
482  if(processData)
483  {
484  std::map<std::string, float> externalStats;
485  // save camera statistics to database
486  externalStats.insert(std::make_pair("Camera/BilateralFiltering/ms", cameraInfo.timeBilateralFiltering*1000.0f));
487  externalStats.insert(std::make_pair("Camera/Capture/ms", cameraInfo.timeCapture*1000.0f));
488  externalStats.insert(std::make_pair("Camera/Disparity/ms", cameraInfo.timeDisparity*1000.0f));
489  externalStats.insert(std::make_pair("Camera/ImageDecimation/ms", cameraInfo.timeImageDecimation*1000.0f));
490  externalStats.insert(std::make_pair("Camera/Mirroring/ms", cameraInfo.timeMirroring*1000.0f));
491  externalStats.insert(std::make_pair("Camera/ExposureCompensation/ms", cameraInfo.timeStereoExposureCompensation*1000.0f));
492  externalStats.insert(std::make_pair("Camera/ScanFromDepth/ms", cameraInfo.timeScanFromDepth*1000.0f));
493  externalStats.insert(std::make_pair("Camera/TotalTime/ms", cameraInfo.timeTotal*1000.0f));
494  externalStats.insert(std::make_pair("Camera/UndistortDepth/ms", cameraInfo.timeUndistortDepth*1000.0f));
495  // save odometry statistics to database
496  externalStats.insert(std::make_pair("Odometry/LocalBundle/ms", odomInfo.localBundleTime*1000.0f));
497  externalStats.insert(std::make_pair("Odometry/LocalBundleConstraints/", odomInfo.localBundleConstraints));
498  externalStats.insert(std::make_pair("Odometry/LocalBundleOutliers/", odomInfo.localBundleOutliers));
499  externalStats.insert(std::make_pair("Odometry/TotalTime/ms", odomInfo.timeEstimation*1000.0f));
500  externalStats.insert(std::make_pair("Odometry/Registration/ms", odomInfo.reg.totalTime*1000.0f));
501  externalStats.insert(std::make_pair("Odometry/Inliers/", odomInfo.reg.inliers));
502  externalStats.insert(std::make_pair("Odometry/Features/", odomInfo.features));
503  externalStats.insert(std::make_pair("Odometry/DistanceTravelled/m", odomInfo.distanceTravelled));
504  externalStats.insert(std::make_pair("Odometry/KeyFrameAdded/", odomInfo.keyFrameAdded));
505  externalStats.insert(std::make_pair("Odometry/LocalKeyFrames/", odomInfo.localKeyFrames));
506  externalStats.insert(std::make_pair("Odometry/LocalMapSize/", odomInfo.localMapSize));
507  externalStats.insert(std::make_pair("Odometry/LocalScanMapSize/", odomInfo.localScanMapSize));
508 
509  OdometryEvent e(SensorData(), Transform(), odomInfo);
510  rtabmap.process(data, pose, covariance, e.velocity(), externalStats);
511  covariance = cv::Mat();
512  }
513 
514  ++iteration;
515  if(!quiet || iteration == totalImages)
516  {
517  double slamTime = timer.ticks();
518 
519  float rmse = -1;
520  if(rtabmap.getStatistics().data().find(Statistics::kGtTranslational_rmse()) != rtabmap.getStatistics().data().end())
521  {
522  rmse = rtabmap.getStatistics().data().at(Statistics::kGtTranslational_rmse());
523  }
524 
525  if(data.keypoints().size() == 0 && data.laserScanRaw().size())
526  {
527  if(rmse >= 0.0f)
528  {
529  printf("Iteration %d/%d: camera=%dms, odom(quality=%f, kfs=%d)=%dms, slam=%dms, rmse=%fm",
530  iteration, totalImages, int(cameraInfo.timeTotal*1000.0f), odomInfo.reg.icpInliersRatio, odomKeyFrames, int(odomInfo.timeEstimation*1000.0f), int(slamTime*1000.0f), rmse);
531  }
532  else
533  {
534  printf("Iteration %d/%d: camera=%dms, odom(quality=%f, kfs=%d)=%dms, slam=%dms",
535  iteration, totalImages, int(cameraInfo.timeTotal*1000.0f), odomInfo.reg.icpInliersRatio, odomKeyFrames, int(odomInfo.timeEstimation*1000.0f), int(slamTime*1000.0f));
536  }
537  }
538  else
539  {
540  if(rmse >= 0.0f)
541  {
542  printf("Iteration %d/%d: camera=%dms, odom(quality=%d/%d, kfs=%d)=%dms, slam=%dms, rmse=%fm",
543  iteration, totalImages, int(cameraInfo.timeTotal*1000.0f), odomInfo.reg.inliers, odomInfo.features, odomKeyFrames, int(odomInfo.timeEstimation*1000.0f), int(slamTime*1000.0f), rmse);
544  }
545  else
546  {
547  printf("Iteration %d/%d: camera=%dms, odom(quality=%d/%d, kfs=%d)=%dms, slam=%dms",
548  iteration, totalImages, int(cameraInfo.timeTotal*1000.0f), odomInfo.reg.inliers, odomInfo.features, odomKeyFrames, int(odomInfo.timeEstimation*1000.0f), int(slamTime*1000.0f));
549  }
550  }
551  if(processData && rtabmap.getLoopClosureId()>0)
552  {
553  printf(" *");
554  }
555  printf("\n");
556  }
557  else if(iteration % (totalImages/10) == 0)
558  {
559  printf(".");
560  fflush(stdout);
561  }
562 
563  cameraInfo = CameraInfo();
564  timer.restart();
565  data = cameraThread.camera()->takeImage(&cameraInfo);
566  }
567  delete odom;
568  printf("Total time=%fs\n", totalTime.ticks());
570  // Processing dataset end
572 
573  // Save trajectory
574  printf("Saving trajectory ...\n");
575  std::map<int, Transform> poses;
576  std::map<int, Transform> vo_poses;
577  std::multimap<int, Link> links;
578  std::map<int, Signature> signatures;
579  std::map<int, double> stamps;
580  rtabmap.getGraph(vo_poses, links, false, true);
581  links.clear();
582  rtabmap.getGraph(poses, links, true, true, &signatures);
583  for(std::map<int, Signature>::iterator iter=signatures.begin(); iter!=signatures.end(); ++iter)
584  {
585  stamps.insert(std::make_pair(iter->first, iter->second.getStamp()));
586  }
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(!pathGt.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  rtabmap.getMemory()->getNodeInfo(iter->first, o, m, w, l, s, gtPose, v, gps, true);
611  if(!gtPose.isNull())
612  {
613  groundTruth.insert(std::make_pair(iter->first, gtPose));
614  }
615  }
616 
617  // compute RMSE statistics
618  float translational_rmse = 0.0f;
619  float translational_mean = 0.0f;
620  float translational_median = 0.0f;
621  float translational_std = 0.0f;
622  float translational_min = 0.0f;
623  float translational_max = 0.0f;
624  float rotational_rmse = 0.0f;
625  float rotational_mean = 0.0f;
626  float rotational_median = 0.0f;
627  float rotational_std = 0.0f;
628  float rotational_min = 0.0f;
629  float rotational_max = 0.0f;
630  // vo performance
631  graph::calcRMSE(
632  groundTruth,
633  vo_poses,
634  translational_rmse,
635  translational_mean,
636  translational_median,
637  translational_std,
638  translational_min,
639  translational_max,
640  rotational_rmse,
641  rotational_mean,
642  rotational_median,
643  rotational_std,
644  rotational_min,
645  rotational_max);
646  float translational_rmse_vo = translational_rmse;
647  float rotational_rmse_vo = rotational_rmse;
648  // SLAM performance
649  graph::calcRMSE(
650  groundTruth,
651  poses,
652  translational_rmse,
653  translational_mean,
654  translational_median,
655  translational_std,
656  translational_min,
657  translational_max,
658  rotational_rmse,
659  rotational_mean,
660  rotational_median,
661  rotational_std,
662  rotational_min,
663  rotational_max);
664 
665  printf(" translational_rmse= %f m (vo = %f m)\n", translational_rmse, translational_rmse_vo);
666  printf(" rotational_rmse= %f deg (vo = %f def)\n", rotational_rmse, rotational_rmse_vo);
667 
668  FILE * pFile = 0;
669  std::string pathErrors = output+"/"+outputName+"_rmse.txt";
670  pFile = fopen(pathErrors.c_str(),"w");
671  if(!pFile)
672  {
673  UERROR("could not save RMSE results to \"%s\"", pathErrors.c_str());
674  }
675  fprintf(pFile, "Ground truth comparison:\n");
676  fprintf(pFile, " translational_rmse= %f\n", translational_rmse);
677  fprintf(pFile, " translational_mean= %f\n", translational_mean);
678  fprintf(pFile, " translational_median= %f\n", translational_median);
679  fprintf(pFile, " translational_std= %f\n", translational_std);
680  fprintf(pFile, " translational_min= %f\n", translational_min);
681  fprintf(pFile, " translational_max= %f\n", translational_max);
682  fprintf(pFile, " rotational_rmse= %f\n", rotational_rmse);
683  fprintf(pFile, " rotational_mean= %f\n", rotational_mean);
684  fprintf(pFile, " rotational_median= %f\n", rotational_median);
685  fprintf(pFile, " rotational_std= %f\n", rotational_std);
686  fprintf(pFile, " rotational_min= %f\n", rotational_min);
687  fprintf(pFile, " rotational_max= %f\n", rotational_max);
688  fclose(pFile);
689  }
690  }
691  else
692  {
693  UERROR("Camera init failed!");
694  }
695 
696  printf("Saving rtabmap database (with all statistics) to \"%s\"\n", (output+"/"+outputName+".db").c_str());
697  printf("Do:\n"
698  " $ rtabmap-databaseViewer %s\n\n", (output+"/"+outputName+".db").c_str());
699 
700  return 0;
701 }
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
UDirectory::makeDir
static bool makeDir(const std::string &dirPath)
Definition: UDirectory.cpp:333
rtabmap::CameraThread::setStereoExposureCompensation
void setStereoExposureCompensation(bool enabled)
Definition: CameraThread.h:63
rtabmap::Transform::prettyPrint
std::string prettyPrint() const
Definition: Transform.cpp:274
uStr2Double
double UTILITE_EXP uStr2Double(const std::string &str)
Definition: UConversion.cpp:132
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()
config
config
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::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
rtabmap::Camera::init
virtual bool init(const std::string &calibrationFolder=".", const std::string &cameraName="")=0
UASSERT
#define UASSERT(condition)
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
sighandler
void sighandler(int sig)
Definition: tools/EurocDataset/main.cpp:78
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
rtabmap::SensorData::stamp
double stamp() const
Definition: SensorData.h:154
rtabmap::CameraThread::setStereoToDepth
void setStereoToDepth(bool enabled)
Definition: CameraThread.h:66
UDEBUG
#define UDEBUG(...)
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
showUsage
void showUsage()
Definition: tools/EurocDataset/main.cpp:52
main
int main(int argc, char *argv[])
Definition: tools/EurocDataset/main.cpp:84
rtabmap::OdometryInfo::reg
RegistrationInfo reg
Definition: OdometryInfo.h:91
rtabmap::OdometryEvent::velocity
std::vector< float > velocity() const
Definition: OdometryEvent.h:73
rtabmap::CameraInfo::timeBilateralFiltering
float timeBilateralFiltering
Definition: CameraInfo.h:66
g_forever
bool g_forever
Definition: tools/EurocDataset/main.cpp:77
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::CameraInfo::timeUndistortDepth
float timeUndistortDepth
Definition: CameraInfo.h:65
glm::inverse
GLM_FUNC_DECL matType< T, P > inverse(matType< T, P > const &m)

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