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 #include <rtabmap/utilite/ULogger.h>
00029 #include <rtabmap/utilite/UEventsManager.h>
00030 #include <rtabmap/utilite/UFile.h>
00031 #include <rtabmap/utilite/UConversion.h>
00032 #include <rtabmap/core/Odometry.h>
00033 #include <rtabmap/core/OdometryThread.h>
00034 #include <rtabmap/gui/OdometryViewer.h>
00035 #include <rtabmap/core/CameraThread.h>
00036 #include <rtabmap/core/CameraRGBD.h>
00037 #include <rtabmap/core/DBReader.h>
00038 #include <rtabmap/core/VWDictionary.h>
00039 #include <QApplication>
00040 #include <QPushButton>
00041 #include <pcl/console/print.h>
00042
00043 void showUsage()
00044 {
00045 printf("\nUsage:\n"
00046 "odometryViewer [options]\n"
00047 "Options:\n"
00048 " -driver # Driver number to use: 0=OpenNI-PCL, 1=OpenNI2, 2=Freenect, 3=OpenNI-CV, 4=OpenNI-CV-ASUS, 5=Freenect2, 6=dc1394, 7=FlyCapture2\n"
00049 " -o # Odometry type (default 6): 0=SURF, 1=SIFT, 2=ORB, 3=FAST/FREAK, 4=FAST/BRIEF, 5=GFTT/FREAK, 6=GFTT/BRIEF, 7=BRISK\n"
00050 " -nn # Nearest neighbor strategy (default 3): kNNFlannNaive=0, kNNFlannKdTree=1, kNNFlannLSH=2, kNNBruteForce=3, kNNBruteForceGPU=4\n"
00051 " -nndr # Nearest neighbor distance ratio (default 0.7)\n"
00052 " -flow Use optical flow odometry.\n"
00053 " -icp Use ICP odometry\n"
00054 " -mono Use Mono odometry\n"
00055 "\n"
00056 " -hz #.# Camera rate (default 0, 0 means as fast as the camera can)\n"
00057 " -db \"input.db\" Use database instead of camera (recorded with rtabmap-dataRecorder)\n"
00058 " -clouds # Maximum clouds shown (default 10, zero means inf)\n"
00059 " -sec #.# Delay (seconds) before reading the database (if set)\n"
00060 "\n"
00061 " -in #.# Inliers maximum distance, features/ICP (default 0.01 m)\n"
00062 " -max # Max features used for matching (default 0=inf)\n"
00063 " -min # Minimum inliers to accept the transform (default 20)\n"
00064 " -depth #.# Maximum features depth (default 5.0 m)\n"
00065 " -i # RANSAC/ICP iterations (default 30)\n"
00066 " -reset # Reset countdown (default 0 = disabled)\n"
00067 " -gpu Use GPU\n"
00068 " -lh # Local history (default 1000)\n"
00069 "\n"
00070 " -brief_bytes # BRIEF bytes (default 32)\n"
00071 " -fast_thr # FAST threshold (default 30)\n"
00072 "\n"
00073 " -d # ICP decimation (default 4)\n"
00074 " -v # ICP voxel size (default 0.005)\n"
00075 " -s # ICP samples (default 0, not used if voxel is set.)\n"
00076 " -cr #.# ICP correspondence ratio (default 0.7)\n"
00077 " -p2p ICP point to point (default point to plane)"
00078 "\n"
00079 " -debug Log debug messages\n"
00080 "\n"
00081 "Examples:\n"
00082 " odometryViewer -odom 0 -lh 5000 SURF example\n"
00083 " odometryViewer -odom 1 -lh 10000 SIFT example\n"
00084 " odometryViewer -odom 4 -nn 2 -lh 1000 FAST/BRIEF example\n"
00085 " odometryViewer -odom 3 -nn 2 -lh 1000 FAST/FREAK example\n"
00086 " odometryViewer -icp -in 0.05 -i 30 ICP example\n"
00087 " odometryViewer -flow -in 0.02 Optical flow example\n");
00088 exit(1);
00089 }
00090
00091 int main (int argc, char * argv[])
00092 {
00093 ULogger::setType(ULogger::kTypeConsole);
00094 ULogger::setLevel(ULogger::kInfo);
00095 ULogger::setPrintTime(false);
00096 ULogger::setPrintWhere(false);
00097
00098
00099 float rate = 0.0;
00100 std::string inputDatabase;
00101 int driver = 0;
00102 int odomType = rtabmap::Parameters::defaultOdomFeatureType();
00103 bool icp = false;
00104 bool flow = false;
00105 bool mono = false;
00106 int nnType = rtabmap::Parameters::defaultOdomBowNNType();
00107 float nndr = rtabmap::Parameters::defaultOdomBowNNDR();
00108 float distance = rtabmap::Parameters::defaultOdomInlierDistance();
00109 int maxWords = rtabmap::Parameters::defaultOdomMaxFeatures();
00110 int minInliers = rtabmap::Parameters::defaultOdomMinInliers();
00111 float maxDepth = rtabmap::Parameters::defaultOdomMaxDepth();
00112 int iterations = rtabmap::Parameters::defaultOdomIterations();
00113 int resetCountdown = rtabmap::Parameters::defaultOdomResetCountdown();
00114 int decimation = 4;
00115 float voxel = 0.005;
00116 int samples = 10000;
00117 float ratio = 0.7f;
00118 int maxClouds = 10;
00119 int briefBytes = rtabmap::Parameters::defaultBRIEFBytes();
00120 int fastThr = rtabmap::Parameters::defaultFASTThreshold();
00121 float sec = 0.0f;
00122 bool gpu = false;
00123 int localHistory = rtabmap::Parameters::defaultOdomBowLocalHistorySize();
00124 bool p2p = rtabmap::Parameters::defaultOdomPnPEstimation();
00125
00126 for(int i=1; i<argc; ++i)
00127 {
00128 if(strcmp(argv[i], "-driver") == 0)
00129 {
00130 ++i;
00131 if(i < argc)
00132 {
00133 driver = std::atoi(argv[i]);
00134 if(driver < 0 || driver > 7)
00135 {
00136 showUsage();
00137 }
00138 }
00139 else
00140 {
00141 showUsage();
00142 }
00143 continue;
00144 }
00145 if(strcmp(argv[i], "-o") == 0)
00146 {
00147 ++i;
00148 if(i < argc)
00149 {
00150 odomType = std::atoi(argv[i]);
00151 if(odomType < 0 || odomType > 6)
00152 {
00153 showUsage();
00154 }
00155 }
00156 else
00157 {
00158 showUsage();
00159 }
00160 continue;
00161 }
00162 if(strcmp(argv[i], "-nn") == 0)
00163 {
00164 ++i;
00165 if(i < argc)
00166 {
00167 nnType = std::atoi(argv[i]);
00168 if(nnType < 0 || nnType > 4)
00169 {
00170 showUsage();
00171 }
00172 }
00173 else
00174 {
00175 showUsage();
00176 }
00177 continue;
00178 }
00179 if(strcmp(argv[i], "-nndr") == 0)
00180 {
00181 ++i;
00182 if(i < argc)
00183 {
00184 nndr = uStr2Float(argv[i]);
00185 if(nndr < 0.0f)
00186 {
00187 showUsage();
00188 }
00189 }
00190 else
00191 {
00192 showUsage();
00193 }
00194 continue;
00195 }
00196 if(strcmp(argv[i], "-hz") == 0)
00197 {
00198 ++i;
00199 if(i < argc)
00200 {
00201 rate = uStr2Float(argv[i]);
00202 if(rate < 0)
00203 {
00204 showUsage();
00205 }
00206 }
00207 else
00208 {
00209 showUsage();
00210 }
00211 continue;
00212 }
00213 if(strcmp(argv[i], "-db") == 0)
00214 {
00215 ++i;
00216 if(i < argc)
00217 {
00218 inputDatabase = argv[i];
00219 if(UFile::getExtension(inputDatabase).compare("db") != 0)
00220 {
00221 printf("Database path (%s) should end with \"db\" \n", inputDatabase.c_str());
00222 showUsage();
00223 }
00224 }
00225 else
00226 {
00227 showUsage();
00228 }
00229 continue;
00230 }
00231 if(strcmp(argv[i], "-clouds") == 0)
00232 {
00233 ++i;
00234 if(i < argc)
00235 {
00236 maxClouds = std::atoi(argv[i]);
00237 if(maxClouds < 0)
00238 {
00239 showUsage();
00240 }
00241 }
00242 else
00243 {
00244 showUsage();
00245 }
00246 continue;
00247 }
00248 if(strcmp(argv[i], "-sec") == 0)
00249 {
00250 ++i;
00251 if(i < argc)
00252 {
00253 sec = uStr2Float(argv[i]);
00254 if(sec < 0.0f)
00255 {
00256 showUsage();
00257 }
00258 }
00259 else
00260 {
00261 showUsage();
00262 }
00263 continue;
00264 }
00265 if(strcmp(argv[i], "-in") == 0)
00266 {
00267 ++i;
00268 if(i < argc)
00269 {
00270 distance = uStr2Float(argv[i]);
00271 if(distance <= 0)
00272 {
00273 showUsage();
00274 }
00275 }
00276 else
00277 {
00278 showUsage();
00279 }
00280 continue;
00281 }
00282 if(strcmp(argv[i], "-max") == 0)
00283 {
00284 ++i;
00285 if(i < argc)
00286 {
00287 maxWords = std::atoi(argv[i]);
00288 if(maxWords < 0)
00289 {
00290 showUsage();
00291 }
00292 }
00293 else
00294 {
00295 showUsage();
00296 }
00297 continue;
00298 }
00299 if(strcmp(argv[i], "-min") == 0)
00300 {
00301 ++i;
00302 if(i < argc)
00303 {
00304 minInliers = std::atoi(argv[i]);
00305 if(minInliers < 0)
00306 {
00307 showUsage();
00308 }
00309 }
00310 else
00311 {
00312 showUsage();
00313 }
00314 continue;
00315 }
00316 if(strcmp(argv[i], "-depth") == 0)
00317 {
00318 ++i;
00319 if(i < argc)
00320 {
00321 maxDepth = uStr2Float(argv[i]);
00322 if(maxDepth < 0)
00323 {
00324 showUsage();
00325 }
00326 }
00327 else
00328 {
00329 showUsage();
00330 }
00331 continue;
00332 }
00333 if(strcmp(argv[i], "-i") == 0)
00334 {
00335 ++i;
00336 if(i < argc)
00337 {
00338 iterations = std::atoi(argv[i]);
00339 if(iterations <= 0)
00340 {
00341 showUsage();
00342 }
00343 }
00344 else
00345 {
00346 showUsage();
00347 }
00348 continue;
00349 }
00350 if(strcmp(argv[i], "-reset") == 0)
00351 {
00352 ++i;
00353 if(i < argc)
00354 {
00355 resetCountdown = std::atoi(argv[i]);
00356 if(resetCountdown < 0)
00357 {
00358 showUsage();
00359 }
00360 }
00361 else
00362 {
00363 showUsage();
00364 }
00365 continue;
00366 }
00367 if(strcmp(argv[i], "-d") == 0)
00368 {
00369 ++i;
00370 if(i < argc)
00371 {
00372 decimation = std::atoi(argv[i]);
00373 if(decimation < 1)
00374 {
00375 showUsage();
00376 }
00377 }
00378 else
00379 {
00380 showUsage();
00381 }
00382 continue;
00383 }
00384 if(strcmp(argv[i], "-v") == 0)
00385 {
00386 ++i;
00387 if(i < argc)
00388 {
00389 voxel = uStr2Float(argv[i]);
00390 if(voxel < 0)
00391 {
00392 showUsage();
00393 }
00394 }
00395 else
00396 {
00397 showUsage();
00398 }
00399 continue;
00400 }
00401 if(strcmp(argv[i], "-s") == 0)
00402 {
00403 ++i;
00404 if(i < argc)
00405 {
00406 samples = std::atoi(argv[i]);
00407 if(samples < 0)
00408 {
00409 showUsage();
00410 }
00411 }
00412 else
00413 {
00414 showUsage();
00415 }
00416 continue;
00417 }
00418 if(strcmp(argv[i], "-cr") == 0)
00419 {
00420 ++i;
00421 if(i < argc)
00422 {
00423 ratio = uStr2Float(argv[i]);
00424 if(ratio < 0.0f)
00425 {
00426 showUsage();
00427 }
00428 }
00429 else
00430 {
00431 showUsage();
00432 }
00433 continue;
00434 }
00435 if(strcmp(argv[i], "-gpu") == 0)
00436 {
00437 gpu = true;
00438 continue;
00439 }
00440 if(strcmp(argv[i], "-lh") == 0)
00441 {
00442 ++i;
00443 if(i < argc)
00444 {
00445 localHistory = std::atoi(argv[i]);
00446 if(localHistory < 0)
00447 {
00448 showUsage();
00449 }
00450 }
00451 else
00452 {
00453 showUsage();
00454 }
00455 continue;
00456 }
00457 if(strcmp(argv[i], "-brief_bytes") == 0)
00458 {
00459 ++i;
00460 if(i < argc)
00461 {
00462 briefBytes = std::atoi(argv[i]);
00463 if(briefBytes < 1)
00464 {
00465 showUsage();
00466 }
00467 }
00468 else
00469 {
00470 showUsage();
00471 }
00472 continue;
00473 }
00474 if(strcmp(argv[i], "-fast_thr") == 0)
00475 {
00476 ++i;
00477 if(i < argc)
00478 {
00479 fastThr = std::atoi(argv[i]);
00480 if(fastThr < 1)
00481 {
00482 showUsage();
00483 }
00484 }
00485 else
00486 {
00487 showUsage();
00488 }
00489 continue;
00490 }
00491 if(strcmp(argv[i], "-icp") == 0)
00492 {
00493 icp = true;
00494 continue;
00495 }
00496 if(strcmp(argv[i], "-flow") == 0)
00497 {
00498 flow = true;
00499 continue;
00500 }
00501 if(strcmp(argv[i], "-mono") == 0)
00502 {
00503 mono = true;
00504 continue;
00505 }
00506 if(strcmp(argv[i], "-p2p") == 0)
00507 {
00508 p2p = true;
00509 continue;
00510 }
00511 if(strcmp(argv[i], "-debug") == 0)
00512 {
00513 ULogger::setLevel(ULogger::kDebug);
00514 ULogger::setPrintTime(true);
00515 ULogger::setPrintWhere(true);
00516 continue;
00517 }
00518
00519 printf("Unrecognized option : %s\n", argv[i]);
00520 showUsage();
00521 }
00522
00523 if(odomType > 1 && nnType == rtabmap::VWDictionary::kNNFlannKdTree)
00524 {
00525 UERROR("You set \"-o %d\" (binary descriptor), you must use \"-nn 2\" (any \"-nn\" other than kNNFlannKdTree)", odomType);
00526 showUsage();
00527 }
00528 else if(odomType <= 1 && nnType == rtabmap::VWDictionary::kNNFlannLSH)
00529 {
00530 UERROR("You set \"-o %d\" (float descriptor), you must use \"-nn 1\" (any \"-nn\" other than kNNFlannLSH)", odomType);
00531 showUsage();
00532 }
00533
00534 if(inputDatabase.size())
00535 {
00536 UINFO("Using database input \"%s\"", inputDatabase.c_str());
00537 }
00538 else
00539 {
00540 UINFO("Using OpenNI camera");
00541 }
00542
00543 std::string odomName;
00544 if(odomType == 0)
00545 {
00546 odomName = "SURF";
00547 }
00548 else if(odomType == 1)
00549 {
00550 odomName = "SIFT";
00551 }
00552 else if(odomType == 2)
00553 {
00554 odomName = "ORB";
00555 }
00556 else if(odomType == 3)
00557 {
00558 odomName = "FAST+FREAK";
00559 }
00560 else if(odomType == 4)
00561 {
00562 odomName = "FAST+BRIEF";
00563 }
00564 else if(odomType == 5)
00565 {
00566 odomName = "GFTT+FREAK";
00567 }
00568 else if(odomType == 6)
00569 {
00570 odomName = "GFTT+BRIEF";
00571 }
00572 else if(odomType == 7)
00573 {
00574 odomName = "BRISK";
00575 }
00576
00577 if(icp)
00578 {
00579 odomName= "ICP";
00580 }
00581
00582 if(flow)
00583 {
00584 odomName= "Optical Flow";
00585 }
00586
00587 std::string nnName;
00588 if(nnType == 0)
00589 {
00590 nnName = "kNNFlannLinear";
00591 }
00592 else if(nnType == 1)
00593 {
00594 nnName = "kNNFlannKdTree";
00595 }
00596 else if(nnType == 2)
00597 {
00598 nnName= "kNNFlannLSH";
00599 }
00600 else if(nnType == 3)
00601 {
00602 nnName= "kNNBruteForce";
00603 }
00604 else if(nnType == 4)
00605 {
00606 nnName= "kNNBruteForceGPU";
00607 }
00608
00609 UINFO("Odometry used = %s", odomName.c_str());
00610 UINFO("Camera rate = %f Hz", rate);
00611 UINFO("Maximum clouds shown = %d", maxClouds);
00612 UINFO("Delay = %f s", sec);
00613 UINFO("Max depth = %f", maxDepth);
00614 UINFO("Reset odometry coutdown = %d", resetCountdown);
00615
00616 QApplication app(argc, argv);
00617
00618 rtabmap::Odometry * odom = 0;
00619
00620 rtabmap::ParametersMap parameters;
00621
00622 parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kOdomMaxDepth(), uNumber2Str(maxDepth)));
00623 parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kOdomResetCountdown(), uNumber2Str(resetCountdown)));
00624
00625 if(!icp)
00626 {
00627 UINFO("Min inliers = %d", minInliers);
00628 UINFO("Inlier maximum correspondences distance = %f", distance);
00629 UINFO("RANSAC iterations = %d", iterations);
00630 UINFO("Max features = %d", maxWords);
00631 UINFO("GPU = %s", gpu?"true":"false");
00632 parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kOdomInlierDistance(), uNumber2Str(distance)));
00633 parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kOdomMinInliers(), uNumber2Str(minInliers)));
00634 parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kOdomIterations(), uNumber2Str(iterations)));
00635 parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kOdomMaxFeatures(), uNumber2Str(maxWords)));
00636 parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kOdomFeatureType(), uNumber2Str(odomType)));
00637 if(odomType == 0)
00638 {
00639 parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kSURFGpuVersion(), uBool2Str(gpu)));
00640 }
00641 if(odomType == 2)
00642 {
00643 parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kORBGpu(), uBool2Str(gpu)));
00644 }
00645 if(odomType == 3 || odomType == 4)
00646 {
00647 UINFO("FAST threshold = %d", fastThr);
00648 parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kFASTThreshold(), uNumber2Str(fastThr)));
00649 parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kFASTGpu(), uBool2Str(gpu)));
00650 }
00651 if(odomType == 4 || odomType == 6)
00652 {
00653 UINFO("BRIEF bytes = %d", briefBytes);
00654 parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kBRIEFBytes(), uNumber2Str(briefBytes)));
00655 }
00656
00657 if(flow)
00658 {
00659
00660 odom = new rtabmap::OdometryOpticalFlow(parameters);
00661 }
00662 else
00663 {
00664
00665 UINFO("Nearest neighbor = %s", nnName.c_str());
00666 UINFO("Nearest neighbor ratio = %f", nndr);
00667 UINFO("Local history = %d", localHistory);
00668 parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kOdomBowNNType(), uNumber2Str(nnType)));
00669 parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kOdomBowNNDR(), uNumber2Str(nndr)));
00670 parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kOdomBowLocalHistorySize(), uNumber2Str(localHistory)));
00671
00672 if(mono)
00673 {
00674 parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kOdomPnPFlags(), uNumber2Str(cv::ITERATIVE)));
00675 parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kOdomPnPReprojError(), "4.0"));
00676 parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kOdomIterations(), "100"));
00677 odom = new rtabmap::OdometryMono(parameters);
00678 }
00679 else
00680 {
00681 odom = new rtabmap::OdometryBOW(parameters);
00682 }
00683 }
00684 }
00685 else if(icp)
00686 {
00687 UINFO("ICP maximum correspondences distance = %f", distance);
00688 UINFO("ICP iterations = %d", iterations);
00689 UINFO("Cloud decimation = %d", decimation);
00690 UINFO("Cloud voxel size = %f", voxel);
00691 UINFO("Cloud samples = %d", samples);
00692 UINFO("Cloud correspondence ratio = %f", ratio);
00693 UINFO("Cloud point to plane = %s", p2p?"false":"true");
00694
00695 odom = new rtabmap::OdometryICP(decimation, voxel, samples, distance, iterations, ratio, !p2p);
00696 }
00697
00698 rtabmap::OdometryThread odomThread(odom);
00699 rtabmap::OdometryViewer odomViewer(maxClouds, 2, 0.0, 50);
00700 UEventsManager::addHandler(&odomThread);
00701 UEventsManager::addHandler(&odomViewer);
00702
00703 odomViewer.setWindowTitle("Odometry view");
00704 odomViewer.resize(1280, 480+QPushButton().minimumHeight());
00705
00706 if(inputDatabase.size())
00707 {
00708 rtabmap::DBReader camera(inputDatabase, rate, true);
00709 if(camera.init())
00710 {
00711 odomThread.start();
00712
00713 if(sec > 0)
00714 {
00715 uSleep(sec*1000);
00716 }
00717
00718 camera.start();
00719
00720 app.exec();
00721
00722 camera.join(true);
00723 odomThread.join(true);
00724 }
00725 }
00726 else
00727 {
00728 rtabmap::CameraRGBD * camera = 0;
00729 rtabmap::Transform t=rtabmap::Transform(0,0,1,0, -1,0,0,0, 0,-1,0,0);
00730 if(driver == 0)
00731 {
00732 camera = new rtabmap::CameraOpenni("", rate, t);
00733 }
00734 else if(driver == 1)
00735 {
00736 if(!rtabmap::CameraOpenNI2::available())
00737 {
00738 UERROR("Not built with OpenNI2 support...");
00739 exit(-1);
00740 }
00741 camera = new rtabmap::CameraOpenNI2("", rate, t);
00742 }
00743 else if(driver == 2)
00744 {
00745 if(!rtabmap::CameraFreenect::available())
00746 {
00747 UERROR("Not built with Freenect support...");
00748 exit(-1);
00749 }
00750 camera = new rtabmap::CameraFreenect(0, rate, t);
00751 }
00752 else if(driver == 3)
00753 {
00754 if(!rtabmap::CameraOpenNICV::available())
00755 {
00756 UERROR("Not built with OpenNI from OpenCV support...");
00757 exit(-1);
00758 }
00759 camera = new rtabmap::CameraOpenNICV(false, rate, t);
00760 }
00761 else if(driver == 4)
00762 {
00763 if(!rtabmap::CameraOpenNICV::available())
00764 {
00765 UERROR("Not built with OpenNI from OpenCV support...");
00766 exit(-1);
00767 }
00768 camera = new rtabmap::CameraOpenNICV(true, rate, t);
00769 }
00770 else if(driver == 5)
00771 {
00772 if(!rtabmap::CameraFreenect2::available())
00773 {
00774 UERROR("Not built with Freenect2 support...");
00775 exit(-1);
00776 }
00777 camera = new rtabmap::CameraFreenect2(0, rtabmap::CameraFreenect2::kTypeRGBDepthSD, rate, t);
00778 }
00779 else if(driver == 6)
00780 {
00781 if(!rtabmap::CameraStereoDC1394::available())
00782 {
00783 UERROR("Not built with dc1394 support...");
00784 exit(-1);
00785 }
00786 camera = new rtabmap::CameraStereoDC1394(rate, t);
00787 }
00788 else if(driver == 7)
00789 {
00790 if(!rtabmap::CameraStereoFlyCapture2::available())
00791 {
00792 UERROR("Not built with FlyCapture2/Triclops support...");
00793 exit(-1);
00794 }
00795 camera = new rtabmap::CameraStereoFlyCapture2(rate, t);
00796 }
00797 else
00798 {
00799 UFATAL("Camera driver (%d) not found!", driver);
00800 }
00801
00802
00803
00804 if(camera->init())
00805 {
00806 if(camera->isCalibrated())
00807 {
00808 rtabmap::CameraThread cameraThread(camera);
00809
00810 odomThread.start();
00811 cameraThread.start();
00812
00813 odomViewer.exec();
00814
00815 cameraThread.join(true);
00816 odomThread.join(true);
00817 }
00818 else
00819 {
00820 printf("The camera is not calibrated! You should calibrate the camera first.\n");
00821 delete camera;
00822 }
00823 }
00824 else
00825 {
00826 printf("Failed to initialize the camera! Please select another driver (see \"--help\").\n");
00827 delete camera;
00828 }
00829 }
00830
00831 return 0;
00832 }