00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029 #include <rtabmap/core/OdometryF2M.h>
00030 #include "rtabmap/core/Rtabmap.h"
00031 #include "rtabmap/core/CameraStereo.h"
00032 #include "rtabmap/core/CameraThread.h"
00033 #include "rtabmap/core/Graph.h"
00034 #include "rtabmap/core/OdometryInfo.h"
00035 #include "rtabmap/core/OdometryEvent.h"
00036 #include "rtabmap/core/Memory.h"
00037 #include "rtabmap/core/util3d_registration.h"
00038 #include "rtabmap/utilite/UConversion.h"
00039 #include "rtabmap/utilite/UDirectory.h"
00040 #include "rtabmap/utilite/UFile.h"
00041 #include "rtabmap/utilite/UMath.h"
00042 #include "rtabmap/utilite/UStl.h"
00043 #include "rtabmap/utilite/UProcessInfo.h"
00044 #include <pcl/common/common.h>
00045 #include <yaml-cpp/yaml.h>
00046 #include <stdio.h>
00047 #include <signal.h>
00048 #include <fstream>
00049
00050 using namespace rtabmap;
00051
00052 void showUsage()
00053 {
00054 printf("\nUsage:\n"
00055 "rtabmap-kitti_dataset [options] path\n"
00056 " path Folder of the sequence (e.g., \"~/EuRoC/V1_03_difficult\")\n"
00057 " containing least mav0/cam0/sensor.yaml, mav0/cam1/sensor.yaml and \n"
00058 " mav0/cam0/data and mav0/cam1/data folders.\n"
00059 " --output Output directory. By default, results are saved in \"path\".\n"
00060 " --output_name Output database name (default \"rtabmap\").\n"
00061 " --quiet Don't show log messages and iteration updates.\n"
00062 " --exposure_comp Do exposure compensation between left and right images.\n"
00063 " --disp Generate full disparity.\n"
00064 " --raw Use raw images (not rectified, this only works with okvis and msckf odometry).\n"
00065 "%s\n"
00066 "Example:\n\n"
00067 " $ rtabmap-euroc_dataset \\\n"
00068 " --Rtabmap/PublishRAMUsage true\\\n"
00069 " --Rtabmap/DetectionRate 2\\\n"
00070 " --RGBD/LinearUpdate 0\\\n"
00071 " --Mem/STMSize 30\\\n"
00072 " ~/EuRoC/V1_03_difficult\n\n", rtabmap::Parameters::showUsage());
00073 exit(1);
00074 }
00075
00076
00077 bool g_forever = true;
00078 void sighandler(int sig)
00079 {
00080 printf("\nSignal %d caught...\n", sig);
00081 g_forever = false;
00082 }
00083
00084 int main(int argc, char * argv[])
00085 {
00086 signal(SIGABRT, &sighandler);
00087 signal(SIGTERM, &sighandler);
00088 signal(SIGINT, &sighandler);
00089
00090 ULogger::setType(ULogger::kTypeConsole);
00091 ULogger::setLevel(ULogger::kWarning);
00092
00093 ParametersMap parameters;
00094 std::string path;
00095 std::string output;
00096 std::string outputName = "rtabmap";
00097 std::string seq;
00098 bool disp = false;
00099 bool raw = false;
00100 bool exposureCompensation = false;
00101 bool quiet = false;
00102 if(argc < 2)
00103 {
00104 showUsage();
00105 }
00106 else
00107 {
00108 for(int i=1; i<argc; ++i)
00109 {
00110 if(std::strcmp(argv[i], "--output") == 0)
00111 {
00112 output = argv[++i];
00113 }
00114 else if(std::strcmp(argv[i], "--output_name") == 0)
00115 {
00116 outputName = argv[++i];
00117 }
00118 else if(std::strcmp(argv[i], "--quiet") == 0)
00119 {
00120 quiet = true;
00121 }
00122 else if(std::strcmp(argv[i], "--disp") == 0)
00123 {
00124 disp = true;
00125 }
00126 else if(std::strcmp(argv[i], "--raw") == 0)
00127 {
00128 raw = true;
00129 }
00130 else if(std::strcmp(argv[i], "--exposure_comp") == 0)
00131 {
00132 exposureCompensation = true;
00133 }
00134 }
00135 parameters = Parameters::parseArguments(argc, argv);
00136 path = argv[argc-1];
00137 path = uReplaceChar(path, '~', UDirectory::homeDir());
00138 path = uReplaceChar(path, '\\', '/');
00139 if(output.empty())
00140 {
00141 output = path;
00142 }
00143 else
00144 {
00145 output = uReplaceChar(output, '~', UDirectory::homeDir());
00146 UDirectory::makeDir(output);
00147 }
00148 parameters.insert(ParametersPair(Parameters::kRtabmapWorkingDirectory(), output));
00149 parameters.insert(ParametersPair(Parameters::kRtabmapPublishRAMUsage(), "true"));
00150 if(raw)
00151 {
00152 parameters.insert(ParametersPair(Parameters::kRtabmapImagesAlreadyRectified(), "false"));
00153 }
00154 }
00155
00156 seq = uSplit(path, '/').back();
00157 std::string pathLeftImages = path+"/mav0/cam0/data";
00158 std::string pathRightImages = path+"/mav0/cam1/data";
00159 std::string pathCalibLeft = path+"/mav0/cam0/sensor.yaml";
00160 std::string pathCalibRight = path+"/mav0/cam1/sensor.yaml";
00161 std::string pathGt = path+"/mav0/state_groundtruth_estimate0/data.csv";
00162 std::string pathImu = path+"/mav0/imu0/data.csv";
00163 if(!UFile::exists(pathGt))
00164 {
00165 UWARN("Ground truth file path doesn't exist: \"%s\", benchmark values won't be computed.", pathGt.c_str());
00166 pathGt.clear();
00167 }
00168
00169 printf("Paths:\n"
00170 " Sequence number: %s\n"
00171 " Sequence path: %s\n"
00172 " Output: %s\n"
00173 " Output name: %s\n"
00174 " left images: %s\n"
00175 " right images: %s\n"
00176 " left calib: %s\n"
00177 " right calib: %s\n",
00178 seq.c_str(),
00179 path.c_str(),
00180 output.c_str(),
00181 outputName.c_str(),
00182 pathLeftImages.c_str(),
00183 pathRightImages.c_str(),
00184 pathCalibLeft.c_str(),
00185 pathCalibRight.c_str());
00186 if(!pathGt.empty())
00187 {
00188 printf(" Ground truth: %s\n", pathGt.c_str());
00189 }
00190 if(!pathImu.empty())
00191 {
00192 printf(" IMU: %s\n", pathImu.c_str());
00193 }
00194
00195 printf(" Exposure Compensation: %s\n", exposureCompensation?"true":"false");
00196 printf(" Disparity: %s\n", disp?"true":"false");
00197 printf(" Raw images: %s\n", raw?"true (Rtabmap/ImagesAlreadyRectified set to false)":"false");
00198
00199 if(!parameters.empty())
00200 {
00201 printf("Parameters:\n");
00202 for(ParametersMap::iterator iter=parameters.begin(); iter!=parameters.end(); ++iter)
00203 {
00204 printf(" %s=%s\n", iter->first.c_str(), iter->second.c_str());
00205 }
00206 }
00207 printf("RTAB-Map version: %s\n", RTABMAP_VERSION);
00208
00209 std::vector<CameraModel> models;
00210 int rateHz = 20;
00211 for(int k=0; k<2; ++k)
00212 {
00213
00214 std::string calibPath = k==0?pathCalibLeft:pathCalibRight;
00215 YAML::Node config = YAML::LoadFile(calibPath);
00216 if(config.IsNull())
00217 {
00218 UERROR("Cannot open calibration file \"%s\"", calibPath.c_str());
00219 return -1;
00220 }
00221
00222 YAML::Node T_BS = config["T_BS"];
00223 YAML::Node data = T_BS["data"];
00224 UASSERT(data.size() == 16);
00225 rateHz = config["rate_hz"].as<int>();
00226 YAML::Node resolution = config["resolution"];
00227 UASSERT(resolution.size() == 2);
00228 YAML::Node intrinsics = config["intrinsics"];
00229 UASSERT(intrinsics.size() == 4);
00230 YAML::Node distortion_coefficients = config["distortion_coefficients"];
00231 UASSERT(distortion_coefficients.size() == 4 || distortion_coefficients.size() == 5 || distortion_coefficients.size() == 8);
00232
00233 cv::Mat K = cv::Mat::eye(3, 3, CV_64FC1);
00234 K.at<double>(0,0) = intrinsics[0].as<double>();
00235 K.at<double>(1,1) = intrinsics[1].as<double>();
00236 K.at<double>(0,2) = intrinsics[2].as<double>();
00237 K.at<double>(1,2) = intrinsics[3].as<double>();
00238 cv::Mat R = cv::Mat::eye(3, 3, CV_64FC1);
00239 cv::Mat P = cv::Mat::zeros(3, 4, CV_64FC1);
00240 K.copyTo(cv::Mat(P, cv::Range(0,3), cv::Range(0,3)));
00241
00242 cv::Mat D = cv::Mat::zeros(1, distortion_coefficients.size(), CV_64FC1);
00243 for(unsigned int i=0; i<distortion_coefficients.size(); ++i)
00244 {
00245 D.at<double>(i) = distortion_coefficients[i].as<double>();
00246 }
00247
00248 Transform t(data[0].as<float>(), data[1].as<float>(), data[2].as<float>(), data[3].as<float>(),
00249 data[4].as<float>(), data[5].as<float>(), data[6].as<float>(), data[7].as<float>(),
00250 data[8].as<float>(), data[9].as<float>(), data[10].as<float>(), data[11].as<float>());
00251
00252 models.push_back(CameraModel(outputName+"_calib", cv::Size(resolution[0].as<int>(),resolution[1].as<int>()), K, D, R, P, t));
00253 UASSERT(models.back().isValidForRectification());
00254 }
00255
00256 StereoCameraModel model(outputName+"_calib", models[0], models[1], models[1].localTransform().inverse() * models[0].localTransform());
00257 if(!model.save(output, false))
00258 {
00259 UERROR("Could not save calibration!");
00260 return -1;
00261 }
00262 printf("Saved calibration \"%s\" to \"%s\"\n", (outputName+"_calib").c_str(), output.c_str());
00263
00264
00265 if(quiet)
00266 {
00267 ULogger::setLevel(ULogger::kError);
00268 }
00269
00270
00271 Transform baseToImu(0,0,1,0, 0,-1,0,0, 1,0,0,0);
00272 CameraThread cameraThread(new
00273 CameraStereoImages(
00274 pathLeftImages,
00275 pathRightImages,
00276 !raw,
00277 0.0f,
00278 baseToImu*models[0].localTransform()), parameters);
00279 printf("baseToImu=%s\n", baseToImu.prettyPrint().c_str());
00280 std::cout<<"baseToCam0:\n" << baseToImu*models[0].localTransform() << std::endl;
00281 printf("baseToCam0=%s\n", (baseToImu*models[0].localTransform()).prettyPrint().c_str());
00282 printf("imuToCam0=%s\n", models[0].localTransform().prettyPrint().c_str());
00283 printf("imuToCam1=%s\n", models[1].localTransform().prettyPrint().c_str());
00284 ((CameraStereoImages*)cameraThread.camera())->setTimestamps(true, "", false);
00285 if(exposureCompensation)
00286 {
00287 cameraThread.setStereoExposureCompensation(true);
00288 }
00289 if(disp)
00290 {
00291 cameraThread.setStereoToDepth(true);
00292 }
00293 if(!pathGt.empty())
00294 {
00295 ((CameraStereoImages*)cameraThread.camera())->setGroundTruthPath(pathGt, 9);
00296 }
00297
00298 float detectionRate = Parameters::defaultRtabmapDetectionRate();
00299 bool intermediateNodes = Parameters::defaultRtabmapCreateIntermediateNodes();
00300 int odomStrategy = Parameters::defaultOdomStrategy();
00301 Parameters::parse(parameters, Parameters::kOdomStrategy(), odomStrategy);
00302 Parameters::parse(parameters, Parameters::kRtabmapDetectionRate(), detectionRate);
00303 Parameters::parse(parameters, Parameters::kRtabmapCreateIntermediateNodes(), intermediateNodes);
00304
00305 int mapUpdate = rateHz / detectionRate;
00306 if(mapUpdate < 1)
00307 {
00308 mapUpdate = 1;
00309 }
00310
00311 std::ifstream imu_file;
00312 if(odomStrategy == Odometry::kTypeOkvis || odomStrategy == Odometry::kTypeMSCKF)
00313 {
00314
00315 std::string line;
00316 imu_file.open(pathImu.c_str());
00317 if (!imu_file.good()) {
00318 UERROR("no imu file found at %s",pathImu.c_str());
00319 return -1;
00320 }
00321 int number_of_lines = 0;
00322 while (std::getline(imu_file, line))
00323 ++number_of_lines;
00324 printf("No. IMU measurements: %d\n", number_of_lines-1);
00325 if (number_of_lines - 1 <= 0) {
00326 UERROR("no imu messages present in %s", pathImu.c_str());
00327 return -1;
00328 }
00329
00330 imu_file.clear();
00331 imu_file.seekg(0, std::ios::beg);
00332 std::getline(imu_file, line);
00333
00334 if(odomStrategy == Odometry::kTypeMSCKF)
00335 {
00336 if(seq.compare("MH_01_easy") == 0)
00337 {
00338 printf("MH_01_easy detected with MSCFK odometry, ignoring first moving 440 images...\n");
00339 ((CameraStereoImages*)cameraThread.camera())->setStartIndex(440);
00340 }
00341 else if(seq.compare("MH_02_easy") == 0)
00342 {
00343 printf("MH_02_easy detected with MSCFK odometry, ignoring first moving 525 images...\n");
00344 ((CameraStereoImages*)cameraThread.camera())->setStartIndex(525);
00345 }
00346 else if(seq.compare("MH_03_medium") == 0)
00347 {
00348 printf("MH_03_medium detected with MSCFK odometry, ignoring first moving 210 images...\n");
00349 ((CameraStereoImages*)cameraThread.camera())->setStartIndex(210);
00350 }
00351 else if(seq.compare("MH_04_difficult") == 0)
00352 {
00353 printf("MH_04_difficult detected with MSCFK odometry, ignoring first moving 250 images...\n");
00354 ((CameraStereoImages*)cameraThread.camera())->setStartIndex(250);
00355 }
00356 else if(seq.compare("MH_05_difficult") == 0)
00357 {
00358 printf("MH_05_difficult detected with MSCFK odometry, ignoring first moving 310 images...\n");
00359 ((CameraStereoImages*)cameraThread.camera())->setStartIndex(310);
00360 }
00361 }
00362 }
00363
00364
00365 std::string databasePath = output+"/"+outputName+".db";
00366 UFile::erase(databasePath);
00367 if(cameraThread.camera()->init(output, outputName+"_calib"))
00368 {
00369 int totalImages = (int)((CameraStereoImages*)cameraThread.camera())->filenames().size();
00370
00371 printf("Processing %d images...\n", totalImages);
00372
00373 ParametersMap odomParameters = parameters;
00374 odomParameters.erase(Parameters::kRtabmapPublishRAMUsage());
00375 Odometry * odom = Odometry::create(odomParameters);
00376 Rtabmap rtabmap;
00377 rtabmap.init(parameters, databasePath);
00378
00379 UTimer totalTime;
00380 UTimer timer;
00381 CameraInfo cameraInfo;
00382 UDEBUG("");
00383 SensorData data = cameraThread.camera()->takeImage(&cameraInfo);
00384 UDEBUG("");
00385 int iteration = 0;
00386 double start = data.stamp();
00387
00389
00391 cv::Mat covariance;
00392 int odomKeyFrames = 0;
00393 while(data.isValid() && g_forever)
00394 {
00395 UDEBUG("");
00396 if(odomStrategy == Odometry::kTypeOkvis || odomStrategy == Odometry::kTypeMSCKF)
00397 {
00398
00399 double t_imu = start;
00400 do {
00401 std::string line;
00402 if (!std::getline(imu_file, line)) {
00403 std::cout << std::endl << "Finished parsing IMU." << std::endl << std::flush;
00404 break;
00405 }
00406
00407 std::stringstream stream(line);
00408 std::string s;
00409 std::getline(stream, s, ',');
00410 std::string nanoseconds = s.substr(s.size() - 9, 9);
00411 std::string seconds = s.substr(0, s.size() - 9);
00412
00413 cv::Vec3d gyr;
00414 for (int j = 0; j < 3; ++j) {
00415 std::getline(stream, s, ',');
00416 gyr[j] = uStr2Double(s);
00417 }
00418
00419 cv::Vec3d acc;
00420 for (int j = 0; j < 3; ++j) {
00421 std::getline(stream, s, ',');
00422 acc[j] = uStr2Double(s);
00423 }
00424
00425 t_imu = double(uStr2Int(seconds)) + double(uStr2Int(nanoseconds))*1e-9;
00426
00427 if (t_imu - start + 1 > 0) {
00428 SensorData dataImu(IMU(gyr, cv::Mat(3,3,CV_64FC1), acc, cv::Mat(3,3,CV_64FC1), baseToImu), 0, t_imu);
00429 UDEBUG("");
00430 odom->process(dataImu);
00431 UDEBUG("");
00432 }
00433
00434 } while (t_imu <= data.stamp());
00435 }
00436
00437
00438 cameraThread.postUpdate(&data, &cameraInfo);
00439 cameraInfo.timeTotal = timer.ticks();
00440
00441 OdometryInfo odomInfo;
00442 UDEBUG("");
00443 Transform pose = odom->process(data, &odomInfo);
00444 UDEBUG("");
00445 if((odomStrategy == Odometry::kTypeOkvis || odomStrategy == Odometry::kTypeMSCKF) &&
00446 pose.isNull())
00447 {
00448 cameraInfo = CameraInfo();
00449 timer.restart();
00450 data = cameraThread.camera()->takeImage(&cameraInfo);
00451 continue;
00452 }
00453
00454 if(odomInfo.keyFrameAdded)
00455 {
00456 ++odomKeyFrames;
00457 }
00458
00459 if(odomStrategy == Odometry::kTypeFovis)
00460 {
00461
00462 if(!odomInfo.reg.covariance.empty() && odomInfo.reg.covariance.at<double>(0,0) >= 9999)
00463 {
00464 odomInfo.reg.covariance = cv::Mat::eye(6,6,CV_64FC1);
00465 }
00466 }
00467
00468 bool processData = true;
00469 if(iteration % mapUpdate != 0)
00470 {
00471
00472 data.setId(-1);
00473 data.setFeatures(std::vector<cv::KeyPoint>(), std::vector<cv::Point3f>(), cv::Mat());
00474 processData = intermediateNodes;
00475 }
00476 if(covariance.empty() || odomInfo.reg.covariance.at<double>(0,0) > covariance.at<double>(0,0))
00477 {
00478 covariance = odomInfo.reg.covariance;
00479 }
00480
00481 timer.restart();
00482 if(processData)
00483 {
00484 std::map<std::string, float> externalStats;
00485
00486 externalStats.insert(std::make_pair("Camera/BilateralFiltering/ms", cameraInfo.timeBilateralFiltering*1000.0f));
00487 externalStats.insert(std::make_pair("Camera/Capture/ms", cameraInfo.timeCapture*1000.0f));
00488 externalStats.insert(std::make_pair("Camera/Disparity/ms", cameraInfo.timeDisparity*1000.0f));
00489 externalStats.insert(std::make_pair("Camera/ImageDecimation/ms", cameraInfo.timeImageDecimation*1000.0f));
00490 externalStats.insert(std::make_pair("Camera/Mirroring/ms", cameraInfo.timeMirroring*1000.0f));
00491 externalStats.insert(std::make_pair("Camera/ExposureCompensation/ms", cameraInfo.timeStereoExposureCompensation*1000.0f));
00492 externalStats.insert(std::make_pair("Camera/ScanFromDepth/ms", cameraInfo.timeScanFromDepth*1000.0f));
00493 externalStats.insert(std::make_pair("Camera/TotalTime/ms", cameraInfo.timeTotal*1000.0f));
00494 externalStats.insert(std::make_pair("Camera/UndistortDepth/ms", cameraInfo.timeUndistortDepth*1000.0f));
00495
00496 externalStats.insert(std::make_pair("Odometry/LocalBundle/ms", odomInfo.localBundleTime*1000.0f));
00497 externalStats.insert(std::make_pair("Odometry/LocalBundleConstraints/", odomInfo.localBundleConstraints));
00498 externalStats.insert(std::make_pair("Odometry/LocalBundleOutliers/", odomInfo.localBundleOutliers));
00499 externalStats.insert(std::make_pair("Odometry/TotalTime/ms", odomInfo.timeEstimation*1000.0f));
00500 externalStats.insert(std::make_pair("Odometry/Registration/ms", odomInfo.reg.totalTime*1000.0f));
00501 externalStats.insert(std::make_pair("Odometry/Inliers/", odomInfo.reg.inliers));
00502 externalStats.insert(std::make_pair("Odometry/Features/", odomInfo.features));
00503 externalStats.insert(std::make_pair("Odometry/DistanceTravelled/m", odomInfo.distanceTravelled));
00504 externalStats.insert(std::make_pair("Odometry/KeyFrameAdded/", odomInfo.keyFrameAdded));
00505 externalStats.insert(std::make_pair("Odometry/LocalKeyFrames/", odomInfo.localKeyFrames));
00506 externalStats.insert(std::make_pair("Odometry/LocalMapSize/", odomInfo.localMapSize));
00507 externalStats.insert(std::make_pair("Odometry/LocalScanMapSize/", odomInfo.localScanMapSize));
00508
00509 OdometryEvent e(SensorData(), Transform(), odomInfo);
00510 rtabmap.process(data, pose, covariance, e.velocity(), externalStats);
00511 covariance = cv::Mat();
00512 }
00513
00514 ++iteration;
00515 if(!quiet || iteration == totalImages)
00516 {
00517 double slamTime = timer.ticks();
00518
00519 float rmse = -1;
00520 if(rtabmap.getStatistics().data().find(Statistics::kGtTranslational_rmse()) != rtabmap.getStatistics().data().end())
00521 {
00522 rmse = rtabmap.getStatistics().data().at(Statistics::kGtTranslational_rmse());
00523 }
00524
00525 if(data.keypoints().size() == 0 && data.laserScanRaw().size())
00526 {
00527 if(rmse >= 0.0f)
00528 {
00529 printf("Iteration %d/%d: camera=%dms, odom(quality=%f, kfs=%d)=%dms, slam=%dms, rmse=%fm",
00530 iteration, totalImages, int(cameraInfo.timeTotal*1000.0f), odomInfo.reg.icpInliersRatio, odomKeyFrames, int(odomInfo.timeEstimation*1000.0f), int(slamTime*1000.0f), rmse);
00531 }
00532 else
00533 {
00534 printf("Iteration %d/%d: camera=%dms, odom(quality=%f, kfs=%d)=%dms, slam=%dms",
00535 iteration, totalImages, int(cameraInfo.timeTotal*1000.0f), odomInfo.reg.icpInliersRatio, odomKeyFrames, int(odomInfo.timeEstimation*1000.0f), int(slamTime*1000.0f));
00536 }
00537 }
00538 else
00539 {
00540 if(rmse >= 0.0f)
00541 {
00542 printf("Iteration %d/%d: camera=%dms, odom(quality=%d/%d, kfs=%d)=%dms, slam=%dms, rmse=%fm",
00543 iteration, totalImages, int(cameraInfo.timeTotal*1000.0f), odomInfo.reg.inliers, odomInfo.features, odomKeyFrames, int(odomInfo.timeEstimation*1000.0f), int(slamTime*1000.0f), rmse);
00544 }
00545 else
00546 {
00547 printf("Iteration %d/%d: camera=%dms, odom(quality=%d/%d, kfs=%d)=%dms, slam=%dms",
00548 iteration, totalImages, int(cameraInfo.timeTotal*1000.0f), odomInfo.reg.inliers, odomInfo.features, odomKeyFrames, int(odomInfo.timeEstimation*1000.0f), int(slamTime*1000.0f));
00549 }
00550 }
00551 if(processData && rtabmap.getLoopClosureId()>0)
00552 {
00553 printf(" *");
00554 }
00555 printf("\n");
00556 }
00557 else if(iteration % (totalImages/10) == 0)
00558 {
00559 printf(".");
00560 fflush(stdout);
00561 }
00562
00563 cameraInfo = CameraInfo();
00564 timer.restart();
00565 data = cameraThread.camera()->takeImage(&cameraInfo);
00566 }
00567 delete odom;
00568 printf("Total time=%fs\n", totalTime.ticks());
00570
00572
00573
00574 printf("Saving trajectory ...\n");
00575 std::map<int, Transform> poses;
00576 std::map<int, Transform> vo_poses;
00577 std::multimap<int, Link> links;
00578 std::map<int, Signature> signatures;
00579 std::map<int, double> stamps;
00580 rtabmap.getGraph(vo_poses, links, false, true);
00581 links.clear();
00582 rtabmap.getGraph(poses, links, true, true, &signatures);
00583 for(std::map<int, Signature>::iterator iter=signatures.begin(); iter!=signatures.end(); ++iter)
00584 {
00585 stamps.insert(std::make_pair(iter->first, iter->second.getStamp()));
00586 }
00587 std::string pathTrajectory = output+"/"+outputName+"_poses.txt";
00588 if(poses.size() && graph::exportPoses(pathTrajectory, 2, poses, links))
00589 {
00590 printf("Saving %s... done!\n", pathTrajectory.c_str());
00591 }
00592 else
00593 {
00594 printf("Saving %s... failed!\n", pathTrajectory.c_str());
00595 }
00596
00597 if(!pathGt.empty())
00598 {
00599
00600 std::map<int, Transform> groundTruth;
00601
00602 for(std::map<int, Transform>::const_iterator iter=poses.begin(); iter!=poses.end(); ++iter)
00603 {
00604 Transform o, gtPose;
00605 int m,w;
00606 std::string l;
00607 double s;
00608 std::vector<float> v;
00609 GPS gps;
00610 rtabmap.getMemory()->getNodeInfo(iter->first, o, m, w, l, s, gtPose, v, gps, true);
00611 if(!gtPose.isNull())
00612 {
00613 groundTruth.insert(std::make_pair(iter->first, gtPose));
00614 }
00615 }
00616
00617
00618 float translational_rmse = 0.0f;
00619 float translational_mean = 0.0f;
00620 float translational_median = 0.0f;
00621 float translational_std = 0.0f;
00622 float translational_min = 0.0f;
00623 float translational_max = 0.0f;
00624 float rotational_rmse = 0.0f;
00625 float rotational_mean = 0.0f;
00626 float rotational_median = 0.0f;
00627 float rotational_std = 0.0f;
00628 float rotational_min = 0.0f;
00629 float rotational_max = 0.0f;
00630
00631 graph::calcRMSE(
00632 groundTruth,
00633 vo_poses,
00634 translational_rmse,
00635 translational_mean,
00636 translational_median,
00637 translational_std,
00638 translational_min,
00639 translational_max,
00640 rotational_rmse,
00641 rotational_mean,
00642 rotational_median,
00643 rotational_std,
00644 rotational_min,
00645 rotational_max);
00646 float translational_rmse_vo = translational_rmse;
00647 float rotational_rmse_vo = rotational_rmse;
00648
00649 graph::calcRMSE(
00650 groundTruth,
00651 poses,
00652 translational_rmse,
00653 translational_mean,
00654 translational_median,
00655 translational_std,
00656 translational_min,
00657 translational_max,
00658 rotational_rmse,
00659 rotational_mean,
00660 rotational_median,
00661 rotational_std,
00662 rotational_min,
00663 rotational_max);
00664
00665 printf(" translational_rmse= %f m (vo = %f m)\n", translational_rmse, translational_rmse_vo);
00666 printf(" rotational_rmse= %f deg (vo = %f def)\n", rotational_rmse, rotational_rmse_vo);
00667
00668 FILE * pFile = 0;
00669 std::string pathErrors = output+"/"+outputName+"_rmse.txt";
00670 pFile = fopen(pathErrors.c_str(),"w");
00671 if(!pFile)
00672 {
00673 UERROR("could not save RMSE results to \"%s\"", pathErrors.c_str());
00674 }
00675 fprintf(pFile, "Ground truth comparison:\n");
00676 fprintf(pFile, " translational_rmse= %f\n", translational_rmse);
00677 fprintf(pFile, " translational_mean= %f\n", translational_mean);
00678 fprintf(pFile, " translational_median= %f\n", translational_median);
00679 fprintf(pFile, " translational_std= %f\n", translational_std);
00680 fprintf(pFile, " translational_min= %f\n", translational_min);
00681 fprintf(pFile, " translational_max= %f\n", translational_max);
00682 fprintf(pFile, " rotational_rmse= %f\n", rotational_rmse);
00683 fprintf(pFile, " rotational_mean= %f\n", rotational_mean);
00684 fprintf(pFile, " rotational_median= %f\n", rotational_median);
00685 fprintf(pFile, " rotational_std= %f\n", rotational_std);
00686 fprintf(pFile, " rotational_min= %f\n", rotational_min);
00687 fprintf(pFile, " rotational_max= %f\n", rotational_max);
00688 fclose(pFile);
00689 }
00690 }
00691 else
00692 {
00693 UERROR("Camera init failed!");
00694 }
00695
00696 printf("Saving rtabmap database (with all statistics) to \"%s\"\n", (output+"/"+outputName+".db").c_str());
00697 printf("Do:\n"
00698 " $ rtabmap-databaseViewer %s\n\n", (output+"/"+outputName+".db").c_str());
00699
00700 return 0;
00701 }