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/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 "rtabmap/core/IMUFilter.h"
44 #include <pcl/common/common.h>
45 #include <rtabmap/core/SensorCaptureThread.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  SensorCaptureThread cameraThread(new
284  CameraStereoImages(
285  pathLeftImages,
286  pathRightImages,
287  !raw,
288  0.0f,
289  baseToImu*models[0].localTransform()*CameraModel::opticalRotation().inverse()), parameters);
290  printf("baseToImu=%s\n", baseToImu.prettyPrint().c_str());
291  std::cout<<"baseToCam0:\n" << baseToImu*models[0].localTransform()*CameraModel::opticalRotation().inverse() << std::endl;
292  printf("baseToCam0=%s\n", (baseToImu*models[0].localTransform()*CameraModel::opticalRotation().inverse()).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 
334  // open the IMU file
335  std::string line;
336  imu_file.open(pathImu.c_str());
337  if (!imu_file.good()) {
338  UERROR("no imu file found at %s",pathImu.c_str());
339  return -1;
340  }
341  int number_of_lines = 0;
342  while (std::getline(imu_file, line))
343  ++number_of_lines;
344  printf("No. IMU measurements: %d\n", number_of_lines-1);
345  if (number_of_lines - 1 <= 0) {
346  UERROR("no imu messages present in %s", pathImu.c_str());
347  return -1;
348  }
349  // set reading position to second line
350  imu_file.clear();
351  imu_file.seekg(0, std::ios::beg);
352  std::getline(imu_file, line);
353 
354  if(odomStrategy == Odometry::kTypeMSCKF)
355  {
356  if(seq.compare("MH_01_easy") == 0)
357  {
358  printf("MH_01_easy detected with MSCFK odometry, ignoring first moving 440 images...\n");
359  ((CameraStereoImages*)cameraThread.camera())->setStartIndex(440);
360  }
361  else if(seq.compare("MH_02_easy") == 0)
362  {
363  printf("MH_02_easy detected with MSCFK odometry, ignoring first moving 525 images...\n");
364  ((CameraStereoImages*)cameraThread.camera())->setStartIndex(525);
365  }
366  else if(seq.compare("MH_03_medium") == 0)
367  {
368  printf("MH_03_medium detected with MSCFK odometry, ignoring first moving 210 images...\n");
369  ((CameraStereoImages*)cameraThread.camera())->setStartIndex(210);
370  }
371  else if(seq.compare("MH_04_difficult") == 0)
372  {
373  printf("MH_04_difficult detected with MSCFK odometry, ignoring first moving 250 images...\n");
374  ((CameraStereoImages*)cameraThread.camera())->setStartIndex(250);
375  }
376  else if(seq.compare("MH_05_difficult") == 0)
377  {
378  printf("MH_05_difficult detected with MSCFK odometry, ignoring first moving 310 images...\n");
379  ((CameraStereoImages*)cameraThread.camera())->setStartIndex(310);
380  }
381  }
382 
383  cameraThread.enableIMUFiltering(imuFilter, parameters);
384 
385  Rtabmap rtabmap;
386  rtabmap.init(parameters, databasePath);
387 
388  UTimer totalTime;
389  UTimer timer;
390  SensorCaptureInfo cameraInfo;
391  UDEBUG("");
392  SensorData data = cameraThread.camera()->takeData(&cameraInfo);
393  UDEBUG("");
394  int iteration = 0;
395  double start = data.stamp();
396 
398  // Processing dataset begin
400  cv::Mat covariance;
401  int odomKeyFrames = 0;
402  while(data.isValid() && g_forever)
403  {
404  UDEBUG("");
405  // get all IMU measurements till then
406  double t_imu = start;
407  do {
408  std::string line;
409  if (!std::getline(imu_file, line)) {
410  std::cout << std::endl << "Finished parsing IMU." << std::endl << std::flush;
411  break;
412  }
413 
414  std::stringstream stream(line);
415  std::string s;
416  std::getline(stream, s, ',');
417  std::string nanoseconds = s.substr(s.size() - 9, 9);
418  std::string seconds = s.substr(0, s.size() - 9);
419 
420  cv::Vec3d gyr;
421  for (int j = 0; j < 3; ++j) {
422  std::getline(stream, s, ',');
423  gyr[j] = uStr2Double(s);
424  }
425 
426  cv::Vec3d acc;
427  for (int j = 0; j < 3; ++j) {
428  std::getline(stream, s, ',');
429  acc[j] = uStr2Double(s);
430  }
431 
432  t_imu = double(uStr2Int(seconds)) + double(uStr2Int(nanoseconds))*1e-9;
433 
434  if (t_imu - start + 1 > 0) {
435 
436  SensorData dataImu(IMU(gyr, cv::Mat(3,3,CV_64FC1), acc, cv::Mat(3,3,CV_64FC1), baseToImu), 0, t_imu);
437  cameraThread.postUpdate(&dataImu);
438  odom->process(dataImu);
439  }
440 
441  } while (t_imu <= data.stamp());
442 
443 
444  cameraThread.postUpdate(&data, &cameraInfo);
445  cameraInfo.timeTotal = timer.ticks();
446 
447  OdometryInfo odomInfo;
448  UDEBUG("");
449  Transform pose = odom->process(data, &odomInfo);
450  UDEBUG("");
451 
452  if(odomInfo.keyFrameAdded)
453  {
454  ++odomKeyFrames;
455  }
456 
457  if(odomStrategy == Odometry::kTypeFovis)
458  {
459  //special case for FOVIS, set covariance 1 if 9999 is detected
460  if(!odomInfo.reg.covariance.empty() && odomInfo.reg.covariance.at<double>(0,0) >= 9999)
461  {
462  odomInfo.reg.covariance = cv::Mat::eye(6,6,CV_64FC1);
463  }
464  }
465 
466  bool processData = true;
467  if(iteration % mapUpdate != 0)
468  {
469  // set negative id so rtabmap will detect it as an intermediate node
470  data.setId(-1);
471  data.setFeatures(std::vector<cv::KeyPoint>(), std::vector<cv::Point3f>(), cv::Mat());// remove features
472  processData = intermediateNodes;
473  }
474  if(covariance.empty() || odomInfo.reg.covariance.at<double>(0,0) > covariance.at<double>(0,0))
475  {
476  covariance = odomInfo.reg.covariance;
477  }
478 
479  timer.restart();
480  if(processData)
481  {
482  std::map<std::string, float> externalStats;
483  // save camera statistics to database
484  externalStats.insert(std::make_pair("Camera/BilateralFiltering/ms", cameraInfo.timeBilateralFiltering*1000.0f));
485  externalStats.insert(std::make_pair("Camera/Capture/ms", cameraInfo.timeCapture*1000.0f));
486  externalStats.insert(std::make_pair("Camera/Disparity/ms", cameraInfo.timeDisparity*1000.0f));
487  externalStats.insert(std::make_pair("Camera/ImageDecimation/ms", cameraInfo.timeImageDecimation*1000.0f));
488  externalStats.insert(std::make_pair("Camera/Mirroring/ms", cameraInfo.timeMirroring*1000.0f));
489  externalStats.insert(std::make_pair("Camera/HistogramEqualization/ms", cameraInfo.timeHistogramEqualization*1000.0f));
490  externalStats.insert(std::make_pair("Camera/ExposureCompensation/ms", cameraInfo.timeStereoExposureCompensation*1000.0f));
491  externalStats.insert(std::make_pair("Camera/ScanFromDepth/ms", cameraInfo.timeScanFromDepth*1000.0f));
492  externalStats.insert(std::make_pair("Camera/TotalTime/ms", cameraInfo.timeTotal*1000.0f));
493  externalStats.insert(std::make_pair("Camera/UndistortDepth/ms", cameraInfo.timeUndistortDepth*1000.0f));
494  // save odometry statistics to database
495  externalStats.insert(std::make_pair("Odometry/LocalBundle/ms", odomInfo.localBundleTime*1000.0f));
496  externalStats.insert(std::make_pair("Odometry/LocalBundleConstraints/", odomInfo.localBundleConstraints));
497  externalStats.insert(std::make_pair("Odometry/LocalBundleOutliers/", odomInfo.localBundleOutliers));
498  externalStats.insert(std::make_pair("Odometry/TotalTime/ms", odomInfo.timeEstimation*1000.0f));
499  externalStats.insert(std::make_pair("Odometry/Registration/ms", odomInfo.reg.totalTime*1000.0f));
500  externalStats.insert(std::make_pair("Odometry/Inliers/", odomInfo.reg.inliers));
501  externalStats.insert(std::make_pair("Odometry/Features/", odomInfo.features));
502  externalStats.insert(std::make_pair("Odometry/DistanceTravelled/m", odomInfo.distanceTravelled));
503  externalStats.insert(std::make_pair("Odometry/KeyFrameAdded/", odomInfo.keyFrameAdded));
504  externalStats.insert(std::make_pair("Odometry/LocalKeyFrames/", odomInfo.localKeyFrames));
505  externalStats.insert(std::make_pair("Odometry/LocalMapSize/", odomInfo.localMapSize));
506  externalStats.insert(std::make_pair("Odometry/LocalScanMapSize/", odomInfo.localScanMapSize));
507 
508  OdometryEvent e(SensorData(), Transform(), odomInfo);
509  rtabmap.process(data, pose, covariance, e.velocity(), externalStats);
510  covariance = cv::Mat();
511  }
512 
513  ++iteration;
514  if(!quiet || iteration == totalImages)
515  {
516  double slamTime = timer.ticks();
517 
518  float rmse = -1;
519  if(rtabmap.getStatistics().data().find(Statistics::kGtTranslational_rmse()) != rtabmap.getStatistics().data().end())
520  {
521  rmse = rtabmap.getStatistics().data().at(Statistics::kGtTranslational_rmse());
522  }
523 
524  if(data.keypoints().size() == 0 && data.laserScanRaw().size())
525  {
526  if(rmse >= 0.0f)
527  {
528  printf("Iteration %d/%d: camera=%dms, odom(quality=%f, kfs=%d)=%dms, slam=%dms, rmse=%fm",
529  iteration, totalImages, int(cameraInfo.timeTotal*1000.0f), odomInfo.reg.icpInliersRatio, odomKeyFrames, int(odomInfo.timeEstimation*1000.0f), int(slamTime*1000.0f), rmse);
530  }
531  else
532  {
533  printf("Iteration %d/%d: camera=%dms, odom(quality=%f, kfs=%d)=%dms, slam=%dms",
534  iteration, totalImages, int(cameraInfo.timeTotal*1000.0f), odomInfo.reg.icpInliersRatio, odomKeyFrames, int(odomInfo.timeEstimation*1000.0f), int(slamTime*1000.0f));
535  }
536  }
537  else
538  {
539  if(rmse >= 0.0f)
540  {
541  printf("Iteration %d/%d: camera=%dms, odom(quality=%d/%d, kfs=%d)=%dms, slam=%dms, rmse=%fm",
542  iteration, totalImages, int(cameraInfo.timeTotal*1000.0f), odomInfo.reg.inliers, odomInfo.features, odomKeyFrames, int(odomInfo.timeEstimation*1000.0f), int(slamTime*1000.0f), rmse);
543  }
544  else
545  {
546  printf("Iteration %d/%d: camera=%dms, odom(quality=%d/%d, kfs=%d)=%dms, slam=%dms",
547  iteration, totalImages, int(cameraInfo.timeTotal*1000.0f), odomInfo.reg.inliers, odomInfo.features, odomKeyFrames, int(odomInfo.timeEstimation*1000.0f), int(slamTime*1000.0f));
548  }
549  }
550  if(processData && rtabmap.getLoopClosureId()>0)
551  {
552  printf(" *");
553  }
554  printf("\n");
555  }
556  else if(iteration % (totalImages/10) == 0)
557  {
558  printf(".");
559  fflush(stdout);
560  }
561 
562  cameraInfo = SensorCaptureInfo();
563  timer.restart();
564  data = cameraThread.camera()->takeData(&cameraInfo);
565  }
566  delete odom;
567  printf("Total time=%fs\n", totalTime.ticks());
569  // Processing dataset end
571 
572  // Save trajectory
573  printf("Saving trajectory ...\n");
574  std::map<int, Transform> poses;
575  std::map<int, Transform> vo_poses;
576  std::multimap<int, Link> links;
577  std::map<int, Signature> signatures;
578  std::map<int, double> stamps;
579  rtabmap.getGraph(vo_poses, links, false, true);
580  links.clear();
581  rtabmap.getGraph(poses, links, true, true, &signatures);
582  for(std::map<int, Signature>::iterator iter=signatures.begin(); iter!=signatures.end(); ++iter)
583  {
584  stamps.insert(std::make_pair(iter->first, iter->second.getStamp()));
585  }
586  std::string pathTrajectory = output+"/"+outputName+"_poses.txt";
587  if(poses.size() && graph::exportPoses(pathTrajectory, 2, poses, links))
588  {
589  printf("Saving %s... done!\n", pathTrajectory.c_str());
590  }
591  else
592  {
593  printf("Saving %s... failed!\n", pathTrajectory.c_str());
594  }
595 
596  if(!pathGt.empty())
597  {
598  // Log ground truth statistics
599  std::map<int, Transform> groundTruth;
600 
601  for(std::map<int, Transform>::const_iterator iter=poses.begin(); iter!=poses.end(); ++iter)
602  {
603  Transform o, gtPose;
604  int m,w;
605  std::string l;
606  double s;
607  std::vector<float> v;
608  GPS gps;
609  EnvSensors sensors;
610  rtabmap.getMemory()->getNodeInfo(iter->first, o, m, w, l, s, gtPose, v, gps, sensors, 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 deg)\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 }
rtabmap::SensorData
Definition: SensorData.h:51
rtabmap::SensorCaptureThread
Definition: SensorCaptureThread.h:58
rtabmap::ParametersPair
std::pair< std::string, std::string > ParametersPair
Definition: Parameters.h:44
w
RowVector3d w
int
int
rtabmap::OdometryEvent
Definition: OdometryEvent.h:39
rtabmap::Odometry::create
static Odometry * create(const ParametersMap &parameters=ParametersMap())
Definition: Odometry.cpp:58
rtabmap::OdometryInfo::localMapSize
int localMapSize
Definition: OdometryInfo.h:101
rtabmap::StereoCameraModel
Definition: StereoCameraModel.h:35
UFile::erase
static int erase(const std::string &filePath)
Definition: UFile.cpp:58
SensorCaptureThread.h
D
MatrixXcd D
OdometryInfo.h
ULogger::kError
@ kError
Definition: ULogger.h:252
UDirectory::makeDir
static bool makeDir(const std::string &dirPath)
Definition: UDirectory.cpp:333
timer
s
RealScalar s
rtabmap::OdometryInfo::localBundleTime
float localBundleTime
Definition: OdometryInfo.h:106
stream
stream
rtabmap::Parameters::parseArguments
static ParametersMap parseArguments(int argc, char *argv[], bool onlyParameters=false)
Definition: Parameters.cpp:599
UProcessInfo.h
rtabmap::OdometryInfo::keyFrameAdded
bool keyFrameAdded
Definition: OdometryInfo.h:109
rtabmap::OdometryInfo::timeEstimation
float timeEstimation
Definition: OdometryInfo.h:111
UDirectory.h
ULogger::kTypeConsole
@ kTypeConsole
Definition: ULogger.h:244
rtabmap::CameraModel
Definition: CameraModel.h:38
rtabmap::OdometryInfo::features
int features
Definition: OdometryInfo.h:100
rtabmap::graph::exportPoses
bool RTABMAP_CORE_EXPORT exportPoses(const std::string &filePath, int format, const std::map< int, Transform > &poses, const std::multimap< int, Link > &constraints=std::multimap< int, Link >(), const std::map< int, double > &stamps=std::map< int, double >(), const ParametersMap &parameters=ParametersMap())
Definition: Graph.cpp:54
ULogger::setLevel
static void setLevel(ULogger::Level level)
Definition: ULogger.h:339
rtabmap::GPS
Definition: GPS.h:35
CameraStereo.h
timer::restart
void restart()
rtabmap::SensorCaptureInfo::timeHistogramEqualization
float timeHistogramEqualization
Definition: SensorCaptureInfo.h:69
rtabmap::Odometry::kTypeMSCKF
@ kTypeMSCKF
Definition: Odometry.h:55
rtabmap::SensorCaptureInfo::timeCapture
float timeCapture
Definition: SensorCaptureInfo.h:63
uStr2Double
double UTILITE_EXPORT uStr2Double(const std::string &str)
Definition: UConversion.cpp:147
rtabmap::OdometryInfo::localKeyFrames
int localKeyFrames
Definition: OdometryInfo.h:103
rtabmap::ParametersMap
std::map< std::string, std::string > ParametersMap
Definition: Parameters.h:43
stdout
stdout
rtabmap::graph::calcRMSE
Transform RTABMAP_CORE_EXPORT calcRMSE(const std::map< int, Transform > &groundTruth, const std::map< int, Transform > &poses, float &translational_rmse, float &translational_mean, float &translational_median, float &translational_std, float &translational_min, float &translational_max, float &rotational_rmse, float &rotational_mean, float &rotational_median, float &rotational_std, float &rotational_min, float &rotational_max)
Definition: Graph.cpp:759
rtabmap::Transform::isNull
bool isNull() const
Definition: Transform.cpp:107
showUsage
void showUsage()
Definition: tools/EurocDataset/main.cpp:53
UMath.h
Basic mathematics functions.
rtabmap::Parameters::parse
static bool parse(const ParametersMap &parameters, const std::string &key, bool &value)
Definition: Parameters.cpp:500
rtabmap::SensorCaptureInfo
Definition: SensorCaptureInfo.h:36
Odometry.h
main
int main(int argc, char *argv[])
Definition: tools/EurocDataset/main.cpp:86
UDirectory::homeDir
static std::string homeDir()
Definition: UDirectory.cpp:355
rtabmap::SensorCaptureThread::postUpdate
void postUpdate(SensorData *data, SensorCaptureInfo *info=0) const
Definition: SensorCaptureThread.cpp:573
rtabmap::SensorCapture::init
virtual bool init(const std::string &calibrationFolder=".", const std::string &cameraName="")=0
rtabmap::SensorCaptureThread::enableIMUFiltering
void enableIMUFiltering(int filteringStrategy=1, const ParametersMap &parameters=ParametersMap(), bool baseFrameConversion=false)
Definition: SensorCaptureThread.cpp:224
rtabmap::IMU
Definition: IMU.h:19
rtabmap::SensorCaptureInfo::timeScanFromDepth
float timeScanFromDepth
Definition: SensorCaptureInfo.h:70
seq
ArithmeticSequence< FirstTypeDerived, symbolic::AddExpr< symbolic::AddExpr< LastTypeDerived, symbolic::NegateExpr< FirstTypeDerived > >, symbolic::ValueExpr< internal::FixedInt< 1 > > > > seq(const symbolic::BaseExpr< FirstTypeDerived > &f, const symbolic::BaseExpr< LastTypeDerived > &l)
rtabmap::Transform::prettyPrint
std::string prettyPrint() const
Definition: Transform.cpp:326
data
int data[]
rtabmap::CameraStereoImages
Definition: CameraStereoImages.h:39
j
std::ptrdiff_t j
UConversion.h
Some conversion functions.
rtabmap::OdometryInfo::localBundleConstraints
int localBundleConstraints
Definition: OdometryInfo.h:105
rtabmap::Odometry::kTypeFovis
@ kTypeFovis
Definition: Odometry.h:49
rtabmap::OdometryInfo::localScanMapSize
int localScanMapSize
Definition: OdometryInfo.h:102
Rtabmap.h
rtabmap::Odometry::process
Transform process(SensorData &data, OdometryInfo *info=0)
Definition: Odometry.cpp:298
ULogger::kWarning
@ kWarning
Definition: ULogger.h:252
rtabmap::SensorCaptureThread::setStereoExposureCompensation
void setStereoExposureCompensation(bool enabled)
Definition: SensorCaptureThread.h:124
rtabmap::RegistrationInfo::covariance
cv::Mat covariance
Definition: RegistrationInfo.h:75
rtabmap::SensorCapture::takeData
SensorData takeData(SensorCaptureInfo *info=0)
Definition: SensorCapture.cpp:64
rtabmap::RegistrationInfo::icpInliersRatio
float icpInliersRatio
Definition: RegistrationInfo.h:90
rtabmap::RegistrationInfo::inliers
int inliers
Definition: RegistrationInfo.h:80
UASSERT
#define UASSERT(condition)
rtabmap_netvlad.argv
argv
Definition: rtabmap_netvlad.py:15
m
Matrix3f m
ULogger::setType
static void setType(Type type, const std::string &fileName=kDefaultLogFileName, bool append=true)
Definition: ULogger.cpp:176
rtabmap::CameraModel::opticalRotation
static Transform opticalRotation()
Definition: CameraModel.h:45
rtabmap::SensorCaptureInfo::timeBilateralFiltering
float timeBilateralFiltering
Definition: SensorCaptureInfo.h:72
OdometryEvent.h
rtabmap::SensorCaptureInfo::timeTotal
float timeTotal
Definition: SensorCaptureInfo.h:73
rtabmap::SensorCaptureThread::setStereoToDepth
void setStereoToDepth(bool enabled)
Definition: SensorCaptureThread.h:128
rtabmap::SensorCaptureInfo::timeImageDecimation
float timeImageDecimation
Definition: SensorCaptureInfo.h:68
rtabmap::Transform::inverse
Transform inverse() const
Definition: Transform.cpp:178
path
path
UWARN
#define UWARN(...)
rtabmap::Parameters::showUsage
static const char * showUsage()
Definition: Parameters.cpp:572
f
Point2(* f)(const Point3 &, OptionalJacobian< 2, 3 >)
K
K
uReplaceChar
std::string UTILITE_EXPORT uReplaceChar(const std::string &str, char before, char after)
Definition: UConversion.cpp:33
e
Array< double, 1, 3 > e(1./3., 0.5, 2.)
rtabmap::RegistrationInfo::totalTime
double totalTime
Definition: RegistrationInfo.h:77
rtabmap::Transform
Definition: Transform.h:41
Memory.h
UTimer::ticks
double ticks()
Definition: UTimer.cpp:117
uSplit
std::list< std::string > uSplit(const std::string &str, char separator=' ')
Definition: UStl.h:564
rtabmap::EnvSensors
std::map< EnvSensor::Type, EnvSensor > EnvSensors
Definition: EnvSensor.h:81
uStr2Int
int UTILITE_EXPORT uStr2Int(const std::string &str)
Definition: UConversion.cpp:125
rtabmap::OdometryInfo::distanceTravelled
float distanceTravelled
Definition: OdometryInfo.h:120
Graph.h
IMUFilter.h
PointMatcherSupport::Parametrizable
iter
iterator iter(handle obj)
rtabmap::SensorCaptureInfo::timeDisparity
float timeDisparity
Definition: SensorCaptureInfo.h:65
util3d_registration.h
c_str
const char * c_str(Args &&...args)
UStl.h
Wrappers of STL for convenient functions.
rtabmap::SensorCaptureInfo::timeStereoExposureCompensation
float timeStereoExposureCompensation
Definition: SensorCaptureInfo.h:67
rtabmap::SensorCaptureInfo::timeMirroring
float timeMirroring
Definition: SensorCaptureInfo.h:66
v
Array< int, Dynamic, 1 > v
UDEBUG
#define UDEBUG(...)
rtabmap::Odometry
Definition: Odometry.h:42
rtabmap::OdometryInfo::reg
RegistrationInfo reg
Definition: OdometryInfo.h:99
UTimer
Definition: UTimer.h:46
rtabmap::OdometryInfo
Definition: OdometryInfo.h:40
rtabmap::Rtabmap
Definition: Rtabmap.h:54
rtabmap::SensorCaptureThread::camera
Camera * camera()
Definition: SensorCaptureThread.h:169
rtabmap::SensorCaptureInfo::timeUndistortDepth
float timeUndistortDepth
Definition: SensorCaptureInfo.h:71
t
Point2 t(10, 10)
UFile.h
rtabmap
Definition: CameraARCore.cpp:35
UFile::exists
bool exists()
Definition: UFile.h:104
config
config
UERROR
#define UERROR(...)
trace.model
model
Definition: trace.py:4
glm::inverse
GLM_FUNC_DECL matType< T, P > inverse(matType< T, P > const &m)
sighandler
void sighandler(int sig)
Definition: tools/EurocDataset/main.cpp:80
i
int i
g_forever
bool g_forever
Definition: tools/EurocDataset/main.cpp:79
rtabmap::OdometryInfo::localBundleOutliers
int localBundleOutliers
Definition: OdometryInfo.h:104
R
R

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