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

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