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()*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  CameraInfo cameraInfo;
391  UDEBUG("");
392  SensorData data = cameraThread.camera()->takeImage(&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/ExposureCompensation/ms", cameraInfo.timeStereoExposureCompensation*1000.0f));
490  externalStats.insert(std::make_pair("Camera/ScanFromDepth/ms", cameraInfo.timeScanFromDepth*1000.0f));
491  externalStats.insert(std::make_pair("Camera/TotalTime/ms", cameraInfo.timeTotal*1000.0f));
492  externalStats.insert(std::make_pair("Camera/UndistortDepth/ms", cameraInfo.timeUndistortDepth*1000.0f));
493  // save odometry statistics to database
494  externalStats.insert(std::make_pair("Odometry/LocalBundle/ms", odomInfo.localBundleTime*1000.0f));
495  externalStats.insert(std::make_pair("Odometry/LocalBundleConstraints/", odomInfo.localBundleConstraints));
496  externalStats.insert(std::make_pair("Odometry/LocalBundleOutliers/", odomInfo.localBundleOutliers));
497  externalStats.insert(std::make_pair("Odometry/TotalTime/ms", odomInfo.timeEstimation*1000.0f));
498  externalStats.insert(std::make_pair("Odometry/Registration/ms", odomInfo.reg.totalTime*1000.0f));
499  externalStats.insert(std::make_pair("Odometry/Inliers/", odomInfo.reg.inliers));
500  externalStats.insert(std::make_pair("Odometry/Features/", odomInfo.features));
501  externalStats.insert(std::make_pair("Odometry/DistanceTravelled/m", odomInfo.distanceTravelled));
502  externalStats.insert(std::make_pair("Odometry/KeyFrameAdded/", odomInfo.keyFrameAdded));
503  externalStats.insert(std::make_pair("Odometry/LocalKeyFrames/", odomInfo.localKeyFrames));
504  externalStats.insert(std::make_pair("Odometry/LocalMapSize/", odomInfo.localMapSize));
505  externalStats.insert(std::make_pair("Odometry/LocalScanMapSize/", odomInfo.localScanMapSize));
506 
507  OdometryEvent e(SensorData(), Transform(), odomInfo);
508  rtabmap.process(data, pose, covariance, e.velocity(), externalStats);
509  covariance = cv::Mat();
510  }
511 
512  ++iteration;
513  if(!quiet || iteration == totalImages)
514  {
515  double slamTime = timer.ticks();
516 
517  float rmse = -1;
518  if(rtabmap.getStatistics().data().find(Statistics::kGtTranslational_rmse()) != rtabmap.getStatistics().data().end())
519  {
520  rmse = rtabmap.getStatistics().data().at(Statistics::kGtTranslational_rmse());
521  }
522 
523  if(data.keypoints().size() == 0 && data.laserScanRaw().size())
524  {
525  if(rmse >= 0.0f)
526  {
527  printf("Iteration %d/%d: camera=%dms, odom(quality=%f, kfs=%d)=%dms, slam=%dms, rmse=%fm",
528  iteration, totalImages, int(cameraInfo.timeTotal*1000.0f), odomInfo.reg.icpInliersRatio, odomKeyFrames, int(odomInfo.timeEstimation*1000.0f), int(slamTime*1000.0f), rmse);
529  }
530  else
531  {
532  printf("Iteration %d/%d: camera=%dms, odom(quality=%f, kfs=%d)=%dms, slam=%dms",
533  iteration, totalImages, int(cameraInfo.timeTotal*1000.0f), odomInfo.reg.icpInliersRatio, odomKeyFrames, int(odomInfo.timeEstimation*1000.0f), int(slamTime*1000.0f));
534  }
535  }
536  else
537  {
538  if(rmse >= 0.0f)
539  {
540  printf("Iteration %d/%d: camera=%dms, odom(quality=%d/%d, kfs=%d)=%dms, slam=%dms, rmse=%fm",
541  iteration, totalImages, int(cameraInfo.timeTotal*1000.0f), odomInfo.reg.inliers, odomInfo.features, odomKeyFrames, int(odomInfo.timeEstimation*1000.0f), int(slamTime*1000.0f), rmse);
542  }
543  else
544  {
545  printf("Iteration %d/%d: camera=%dms, odom(quality=%d/%d, kfs=%d)=%dms, slam=%dms",
546  iteration, totalImages, int(cameraInfo.timeTotal*1000.0f), odomInfo.reg.inliers, odomInfo.features, odomKeyFrames, int(odomInfo.timeEstimation*1000.0f), int(slamTime*1000.0f));
547  }
548  }
549  if(processData && rtabmap.getLoopClosureId()>0)
550  {
551  printf(" *");
552  }
553  printf("\n");
554  }
555  else if(iteration % (totalImages/10) == 0)
556  {
557  printf(".");
558  fflush(stdout);
559  }
560 
561  cameraInfo = CameraInfo();
562  timer.restart();
563  data = cameraThread.camera()->takeImage(&cameraInfo);
564  }
565  delete odom;
566  printf("Total time=%fs\n", totalTime.ticks());
568  // Processing dataset end
570 
571  // Save trajectory
572  printf("Saving trajectory ...\n");
573  std::map<int, Transform> poses;
574  std::map<int, Transform> vo_poses;
575  std::multimap<int, Link> links;
576  std::map<int, Signature> signatures;
577  std::map<int, double> stamps;
578  rtabmap.getGraph(vo_poses, links, false, true);
579  links.clear();
580  rtabmap.getGraph(poses, links, true, true, &signatures);
581  for(std::map<int, Signature>::iterator iter=signatures.begin(); iter!=signatures.end(); ++iter)
582  {
583  stamps.insert(std::make_pair(iter->first, iter->second.getStamp()));
584  }
585  std::string pathTrajectory = output+"/"+outputName+"_poses.txt";
586  if(poses.size() && graph::exportPoses(pathTrajectory, 2, poses, links))
587  {
588  printf("Saving %s... done!\n", pathTrajectory.c_str());
589  }
590  else
591  {
592  printf("Saving %s... failed!\n", pathTrajectory.c_str());
593  }
594 
595  if(!pathGt.empty())
596  {
597  // Log ground truth statistics
598  std::map<int, Transform> groundTruth;
599 
600  for(std::map<int, Transform>::const_iterator iter=poses.begin(); iter!=poses.end(); ++iter)
601  {
602  Transform o, gtPose;
603  int m,w;
604  std::string l;
605  double s;
606  std::vector<float> v;
607  GPS gps;
608  EnvSensors sensors;
609  rtabmap.getMemory()->getNodeInfo(iter->first, o, m, w, l, s, gtPose, v, gps, sensors, true);
610  if(!gtPose.isNull())
611  {
612  groundTruth.insert(std::make_pair(iter->first, gtPose));
613  }
614  }
615 
616  // compute RMSE statistics
617  float translational_rmse = 0.0f;
618  float translational_mean = 0.0f;
619  float translational_median = 0.0f;
620  float translational_std = 0.0f;
621  float translational_min = 0.0f;
622  float translational_max = 0.0f;
623  float rotational_rmse = 0.0f;
624  float rotational_mean = 0.0f;
625  float rotational_median = 0.0f;
626  float rotational_std = 0.0f;
627  float rotational_min = 0.0f;
628  float rotational_max = 0.0f;
629  // vo performance
630  graph::calcRMSE(
631  groundTruth,
632  vo_poses,
633  translational_rmse,
634  translational_mean,
635  translational_median,
636  translational_std,
637  translational_min,
638  translational_max,
639  rotational_rmse,
640  rotational_mean,
641  rotational_median,
642  rotational_std,
643  rotational_min,
644  rotational_max);
645  float translational_rmse_vo = translational_rmse;
646  float rotational_rmse_vo = rotational_rmse;
647  // SLAM performance
648  graph::calcRMSE(
649  groundTruth,
650  poses,
651  translational_rmse,
652  translational_mean,
653  translational_median,
654  translational_std,
655  translational_min,
656  translational_max,
657  rotational_rmse,
658  rotational_mean,
659  rotational_median,
660  rotational_std,
661  rotational_min,
662  rotational_max);
663 
664  printf(" translational_rmse= %f m (vo = %f m)\n", translational_rmse, translational_rmse_vo);
665  printf(" rotational_rmse= %f deg (vo = %f deg)\n", rotational_rmse, rotational_rmse_vo);
666 
667  FILE * pFile = 0;
668  std::string pathErrors = output+"/"+outputName+"_rmse.txt";
669  pFile = fopen(pathErrors.c_str(),"w");
670  if(!pFile)
671  {
672  UERROR("could not save RMSE results to \"%s\"", pathErrors.c_str());
673  }
674  fprintf(pFile, "Ground truth comparison:\n");
675  fprintf(pFile, " translational_rmse= %f\n", translational_rmse);
676  fprintf(pFile, " translational_mean= %f\n", translational_mean);
677  fprintf(pFile, " translational_median= %f\n", translational_median);
678  fprintf(pFile, " translational_std= %f\n", translational_std);
679  fprintf(pFile, " translational_min= %f\n", translational_min);
680  fprintf(pFile, " translational_max= %f\n", translational_max);
681  fprintf(pFile, " rotational_rmse= %f\n", rotational_rmse);
682  fprintf(pFile, " rotational_mean= %f\n", rotational_mean);
683  fprintf(pFile, " rotational_median= %f\n", rotational_median);
684  fprintf(pFile, " rotational_std= %f\n", rotational_std);
685  fprintf(pFile, " rotational_min= %f\n", rotational_min);
686  fprintf(pFile, " rotational_max= %f\n", rotational_max);
687  fclose(pFile);
688  }
689  }
690  else
691  {
692  UERROR("Camera init failed!");
693  }
694 
695  printf("Saving rtabmap database (with all statistics) to \"%s\"\n", (output+"/"+outputName+".db").c_str());
696  printf("Do:\n"
697  " $ rtabmap-databaseViewer %s\n\n", (output+"/"+outputName+".db").c_str());
698 
699  return 0;
700 }
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:4021
uStr2Int
int UTILITE_EXP uStr2Int(const std::string &str)
Definition: UConversion.cpp:125
UDirectory::homeDir
static std::string homeDir()
Definition: UDirectory.cpp:355
rtabmap::StereoCameraModel::save
bool save(const std::string &directory, bool ignoreStereoTransform=true) const
Definition: StereoCameraModel.cpp:334
rtabmap::Parameters::parse
static bool parse(const ParametersMap &parameters, const std::string &key, bool &value)
Definition: Parameters.cpp:497
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:80
uStr2Double
double UTILITE_EXP uStr2Double(const std::string &str)
Definition: UConversion.cpp:147
f
f
rtabmap::Parameters::showUsage
static const char * showUsage()
Definition: Parameters.cpp:569
rtabmap::Rtabmap::getStatistics
const Statistics & getStatistics() const
Definition: Rtabmap.cpp:830
rtabmap::ParametersPair
std::pair< std::string, std::string > ParametersPair
Definition: Parameters.h:44
data
data
rtabmap::Parameters::parseArguments
static ParametersMap parseArguments(int argc, char *argv[], bool onlyParameters=false)
Definition: Parameters.cpp:596
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::SensorData::keypoints
const std::vector< cv::KeyPoint > & keypoints() const
Definition: SensorData.h:270
rtabmap::Rtabmap::init
void init(const ParametersMap &parameters, const std::string &databasePath="", bool loadDatabaseParameters=false)
Definition: Rtabmap.cpp:310
UMath.h
Basic mathematics functions.
rtabmap::SensorData::setId
void setId(int id)
Definition: SensorData.h:175
UConversion.h
Some conversion functions.
ULogger::setLevel
static void setLevel(ULogger::Level level)
Definition: ULogger.h:339
rtabmap::CameraModel::opticalRotation
static Transform opticalRotation()
Definition: CameraModel.h:45
rtabmap::Odometry::process
Transform process(SensorData &data, OdometryInfo *info=0)
Definition: Odometry.cpp:291
util3d_registration.h
rtabmap::Statistics::data
const std::map< std::string, float > & data() const
Definition: Statistics.h:295
rtabmap::Camera::init
virtual bool init(const std::string &calibrationFolder=".", const std::string &cameraName="")=0
UASSERT
#define UASSERT(condition)
rtabmap::Camera::takeImage
SensorData takeImage(CameraInfo *info=0)
Definition: Camera.cpp:72
UStl.h
Wrappers of STL for convenient functions.
uSplit
std::list< std::string > uSplit(const std::string &str, char separator=' ')
Definition: UStl.h:566
rtabmap::CameraInfo::timeStereoExposureCompensation
float timeStereoExposureCompensation
Definition: CameraInfo.h:63
sighandler
void sighandler(int sig)
Definition: tools/EurocDataset/main.cpp:80
rtabmap::Odometry::create
static Odometry * create(const ParametersMap &parameters=ParametersMap())
Definition: Odometry.cpp:57
rtabmap::Rtabmap::getMemory
const Memory * getMemory() const
Definition: Rtabmap.h:147
rtabmap::CameraThread::enableIMUFiltering
void enableIMUFiltering(int filteringStrategy=1, const ParametersMap &parameters=ParametersMap(), bool baseFrameConversion=false)
Definition: CameraThread.cpp:204
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:759
rtabmap::Transform::prettyPrint
std::string prettyPrint() const
Definition: Transform.cpp:316
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:1151
uReplaceChar
std::string UTILITE_EXP uReplaceChar(const std::string &str, char before, char after)
Definition: UConversion.cpp:33
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::OdometryEvent::velocity
std::vector< float > velocity() const
Definition: OdometryEvent.h:73
rtabmap::Transform::isNull
bool isNull() const
Definition: Transform.cpp:107
rtabmap::CameraThread::postUpdate
void postUpdate(SensorData *data, CameraInfo *info=0) const
Definition: CameraThread.cpp:350
rtabmap::CameraThread::setStereoToDepth
void setStereoToDepth(bool enabled)
Definition: CameraThread.h:83
rtabmap::SensorData::laserScanRaw
const LaserScan & laserScanRaw() const
Definition: SensorData.h:185
UDEBUG
#define UDEBUG(...)
UFile::exists
bool exists()
Definition: UFile.h:104
UERROR
#define UERROR(...)
rtabmap_oanet.argv
argv
Definition: rtabmap_oanet.py:12
UWARN
#define UWARN(...)
rtabmap::SensorData::stamp
double stamp() const
Definition: SensorData.h:176
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:5189
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
trace.model
model
Definition: trace.py:4
main
int main(int argc, char *argv[])
Definition: tools/EurocDataset/main.cpp:86
rtabmap::OdometryInfo::reg
RegistrationInfo reg
Definition: OdometryInfo.h:97
rtabmap::EnvSensors
std::map< EnvSensor::Type, EnvSensor > EnvSensors
Definition: EnvSensor.h:81
rtabmap::CameraInfo::timeBilateralFiltering
float timeBilateralFiltering
Definition: CameraInfo.h:67
g_forever
bool g_forever
Definition: tools/EurocDataset/main.cpp:79
rtabmap::SensorData::isValid
bool isValid() const
Definition: SensorData.h:156
rtabmap::LaserScan::size
int size() const
Definition: LaserScan.h:126
rtabmap::SensorData::setFeatures
void setFeatures(const std::vector< cv::KeyPoint > &keypoints, const std::vector< cv::Point3f > &keypoints3D, const cv::Mat &descriptors)
Definition: SensorData.cpp:801
rtabmap::Rtabmap::getLoopClosureId
int getLoopClosureId() const
Definition: Rtabmap.h:128
rtabmap::Transform::inverse
Transform inverse() const
Definition: Transform.cpp:178
rtabmap::CameraInfo::timeUndistortDepth
float timeUndistortDepth
Definition: CameraInfo.h:66
glm::inverse
GLM_FUNC_DECL matType< T, P > inverse(matType< T, P > const &m)

rtabmap
Author(s): Mathieu Labbe
autogenerated on Mon Jan 23 2023 03:37:29