main.cpp
Go to the documentation of this file.
00001 /*
00002 Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
00003 All rights reserved.
00004 
00005 Redistribution and use in source and binary forms, with or without
00006 modification, are permitted provided that the following conditions are met:
00007     * Redistributions of source code must retain the above copyright
00008       notice, this list of conditions and the following disclaimer.
00009     * Redistributions in binary form must reproduce the above copyright
00010       notice, this list of conditions and the following disclaimer in the
00011       documentation and/or other materials provided with the distribution.
00012     * Neither the name of the Universite de Sherbrooke nor the
00013       names of its contributors may be used to endorse or promote products
00014       derived from this software without specific prior written permission.
00015 
00016 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
00017 ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
00018 WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
00019 DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
00020 DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
00021 (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00022 LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
00023 ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
00024 (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
00025 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
00026 */
00027 
00028 #include "rtabmap/core/Rtabmap.h"
00029 #include "rtabmap/core/Camera.h"
00030 #include <opencv2/core/core.hpp>
00031 #include "rtabmap/utilite/UFile.h"
00032 #include <stdio.h>
00033 
00034 void showUsage()
00035 {
00036         printf("\nUsage:\n"
00037                         "rtabmap-bow_mapping [options] \"path\"\n"
00038                         "  path       Path to a directory of images\n "
00039                         "  Options:"
00040                         "     -l   localization mode: use already built RTAB-Map database to localize\n ");
00041         exit(1);
00042 }
00043 
00044 int main(int argc, char * argv[])
00045 {
00046         //ULogger::setType(ULogger::kTypeConsole);
00047         //ULogger::setLevel(ULogger::kDebug);
00048 
00049         std::string path;
00050         bool localizationMode = false;
00051 
00052         if(argc < 2)
00053         {
00054                 showUsage();
00055         }
00056 
00057         for(int i=1; i<argc-1; ++i)
00058         {
00059                 if(strcmp(argv[i], "-l") == 0)
00060                 {
00061                         localizationMode = true;
00062                 }
00063                 else
00064                 {
00065                         printf("Unrecognized option \"%s\"\n", argv[i]);
00066                         showUsage();
00067                 }
00068         }
00069 
00070         path = argv[argc-1];
00071 
00072         // rtabmap::Camera is simply a convenience wrapper of OpenCV cv::VideoCapture and cv::imread
00073         rtabmap::CameraImages camera(path);
00074         if(!camera.init())
00075         {
00076                 printf("Camera init failed, using path \"%s\"\n", path.c_str());
00077                 exit(1);
00078         }
00079 
00080         // Create RTAB-Map
00081         rtabmap::Rtabmap rtabmap;
00082 
00083         // Set the time threshold
00084         rtabmap.setTimeThreshold(700.0f); // Time threshold : 700 ms, 0 ms means no limit
00085 
00086         // To set other parameters, the Parameters interface must be used (Parameters.h).
00087         // Example here to change the loop closure threshold (default 0.15).
00088         // Lower the threshold, more loop closures are detected but there is more chance of false positives.
00089         rtabmap::ParametersMap parameters;
00090         parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kRtabmapLoopThr(), "0.11"));
00091 
00092         // The time threshold set above is also a parameter, one could have set it the same way:
00093         //   parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kRtabmapTimeThr(), "700"));
00094         // Or SURF hessian treshold:
00095         //   parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kSURFHessianThreshold(), "150"));
00096 
00097         // Appearance-based only, disable RGB-D mode
00098         parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kRGBDEnabled(), "false"));
00099 
00100         std::string databasePath = rtabmap::Parameters::defaultRtabmapWorkingDirectory()+"/"+rtabmap::Parameters::getDefaultDatabaseName();
00101         if(localizationMode)
00102         {
00103                 parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kMemIncrementalMemory(), "false"));
00104                 parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kKpIncrementalDictionary(), "false"));
00105                 parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kMemSTMSize(), "1"));
00106         }
00107         else
00108         {
00109                 // delete previous database if there's one...
00110                 UFile::erase(databasePath);
00111         }
00112 
00113         // Initialize rtabmap: delete/create database...
00114         rtabmap.init(parameters, databasePath);
00115 
00116         // Process each image of the directory...
00117         printf("\nProcessing images... from directory \"%s\"\n", path.c_str());
00118 
00119         int countLoopDetected=0;
00120         int i=0;
00121         cv::Mat img = camera.takeImage();
00122         int nextIndex = rtabmap.getLastLocationId()+1;
00123         while(!img.empty())
00124         {
00125                 // Process image : Main loop of RTAB-Map
00126                 rtabmap.process(img, nextIndex);
00127 
00128                 // Check if a loop closure is detected and print some info
00129                 if(rtabmap.getLoopClosureId())
00130                 {
00131                         ++countLoopDetected;
00132                 }
00133                 ++i;
00134                 if(rtabmap.getLoopClosureId())
00135                 {
00136                         printf(" #%d ptime(%fs) STM(%d) WM(%d) hyp(%d) value(%.2f) *LOOP %d->%d*\n",
00137                                         i,
00138                                         rtabmap.getLastProcessTime(),
00139                                         (int)rtabmap.getSTM().size(), // short-term memory
00140                                         (int)rtabmap.getWM().size(), // working memory
00141                                         rtabmap.getLoopClosureId(),
00142                                         rtabmap.getLoopClosureValue(),
00143                                         nextIndex,
00144                                         rtabmap.getLoopClosureId());
00145                 }
00146                 else
00147                 {
00148                         printf(" #%d ptime(%fs) STM(%d) WM(%d) hyp(%d) value(%.2f)\n",
00149                                         i,
00150                                         rtabmap.getLastProcessTime(),
00151                                         (int)rtabmap.getSTM().size(), // short-term memory
00152                                         (int)rtabmap.getWM().size(), // working memory
00153                                         rtabmap.getHighestHypothesisId(), // highest loop closure hypothesis
00154                                         rtabmap.getLoopClosureValue());
00155                 }
00156 
00157                 ++nextIndex;
00158 
00159                 //Get next image
00160                 img = camera.takeImage();
00161         }
00162 
00163         printf("Processing images completed. Loop closures found = %d\n", countLoopDetected);
00164 
00165         // Generate a graph for visualization with Graphiz
00166         rtabmap.generateDOTGraph("Graph.dot");
00167         printf("Generated graph \"Graph.dot\", viewable with Graphiz using \"neato -Tpdf Graph.dot -o out.pdf\"\n");
00168 
00169         // Cleanup... save database and logs
00170         printf("Saving Long-Term Memory to \"rtabmap.db\"...\n");
00171         rtabmap.close();
00172 
00173         return 0;
00174 }


rtabmap
Author(s): Mathieu Labbe
autogenerated on Fri Aug 28 2015 12:51:31