examples/BOWMapping/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  names of its contributors may be used to endorse or promote products
14  derived from this software without specific prior written permission.
15 
16 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
17 ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
18 WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
19 DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
20 DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
21 (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
22 LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
23 ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
24 (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
25 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
26 */
27 
28 #include "rtabmap/core/Rtabmap.h"
29 #include "rtabmap/core/CameraRGB.h"
30 #include <opencv2/core/core.hpp>
31 #include "rtabmap/utilite/UFile.h"
32 #include <stdio.h>
33 
34 void showUsage()
35 {
36  printf("\nUsage:\n"
37  "rtabmap-bow_mapping [options] \"path\"\n"
38  " path Path to a directory of images\n "
39  " Options:"
40  " -l localization mode: use already built RTAB-Map database to localize\n ");
41  exit(1);
42 }
43 
44 int main(int argc, char * argv[])
45 {
46  //ULogger::setType(ULogger::kTypeConsole);
47  //ULogger::setLevel(ULogger::kDebug);
48 
49  std::string path;
50  bool localizationMode = false;
51 
52  if(argc < 2)
53  {
54  showUsage();
55  }
56 
57  for(int i=1; i<argc-1; ++i)
58  {
59  if(strcmp(argv[i], "-l") == 0)
60  {
61  localizationMode = true;
62  }
63  else
64  {
65  printf("Unrecognized option \"%s\"\n", argv[i]);
66  showUsage();
67  }
68  }
69 
70  path = argv[argc-1];
71 
72  // rtabmap::Camera is simply a convenience wrapper of OpenCV cv::VideoCapture and cv::imread
73  rtabmap::CameraImages camera(path);
74  if(!camera.init())
75  {
76  printf("Camera init failed, using path \"%s\"\n", path.c_str());
77  exit(1);
78  }
79 
80  // Create RTAB-Map
82 
83  // Set the time threshold
84  rtabmap.setTimeThreshold(700.0f); // Time threshold : 700 ms, 0 ms means no limit
85 
86  // To set other parameters, the Parameters interface must be used (Parameters.h).
87  // Example here to change the loop closure threshold (default 0.15).
88  // Lower the threshold, more loop closures are detected but there is more chance of false positives.
89  rtabmap::ParametersMap parameters;
90  parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kRtabmapLoopThr(), "0.11"));
91 
92  // The time threshold set above is also a parameter, one could have set it the same way:
93  // parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kRtabmapTimeThr(), "700"));
94  // Or SURF hessian treshold:
95  // parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kSURFHessianThreshold(), "150"));
96 
97  // Appearance-based only, disable RGB-D mode
98  parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kRGBDEnabled(), "false"));
99 
101  if(localizationMode)
102  {
103  parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kMemIncrementalMemory(), "false"));
104  parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kKpIncrementalDictionary(), "false"));
105  parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kMemSTMSize(), "1"));
106  }
107  else
108  {
109  // delete previous database if there's one...
110  UFile::erase(databasePath);
111  }
112 
113  // Initialize rtabmap: delete/create database...
114  rtabmap.init(parameters, databasePath);
115 
116  // Process each image of the directory...
117  printf("\nProcessing images... from directory \"%s\"\n", path.c_str());
118 
119  int countLoopDetected=0;
120  int i=0;
121  rtabmap::SensorData data = camera.takeImage();
122  int nextIndex = rtabmap.getLastLocationId()+1;
123  while(!data.imageRaw().empty())
124  {
125  // Process image : Main loop of RTAB-Map
126  rtabmap.process(data.imageRaw(), nextIndex);
127 
128  // Check if a loop closure is detected and print some info
129  if(rtabmap.getLoopClosureId())
130  {
131  ++countLoopDetected;
132  }
133  ++i;
134  if(rtabmap.getLoopClosureId())
135  {
136  printf(" #%d ptime(%fs) STM(%d) WM(%d) hyp(%d) value(%.2f) *LOOP %d->%d*\n",
137  i,
138  rtabmap.getLastProcessTime(),
139  (int)rtabmap.getSTM().size(), // short-term memory
140  (int)rtabmap.getWM().size(), // working memory
141  rtabmap.getLoopClosureId(),
142  rtabmap.getLoopClosureValue(),
143  nextIndex,
144  rtabmap.getLoopClosureId());
145  }
146  else
147  {
148  printf(" #%d ptime(%fs) STM(%d) WM(%d) hyp(%d) value(%.2f)\n",
149  i,
150  rtabmap.getLastProcessTime(),
151  (int)rtabmap.getSTM().size(), // short-term memory
152  (int)rtabmap.getWM().size(), // working memory
153  rtabmap.getHighestHypothesisId(), // highest loop closure hypothesis
154  rtabmap.getLoopClosureValue());
155  }
156 
157  ++nextIndex;
158 
159  //Get next image
160  data = camera.takeImage();
161  }
162 
163  printf("Processing images completed. Loop closures found = %d\n", countLoopDetected);
164 
165  // Generate a graph for visualization with Graphiz
166  rtabmap.generateDOTGraph("Graph.dot");
167  printf("Generated graph \"Graph.dot\", viewable with Graphiz using \"neato -Tpdf Graph.dot -o out.pdf\"\n");
168 
169  // Cleanup... save database and logs
170  printf("Saving Long-Term Memory to \"rtabmap.db\"...\n");
171  rtabmap.close();
172 
173  return 0;
174 }
double getLastProcessTime() const
Definition: Rtabmap.h:138
int main(int argc, char *argv[])
std::set< int > getSTM() const
Definition: Rtabmap.cpp:650
std::list< int > getWM() const
Definition: Rtabmap.cpp:619
f
std::pair< std::string, std::string > ParametersPair
Definition: Parameters.h:44
void generateDOTGraph(const std::string &path, int id=0, int margin=5)
Definition: Rtabmap.cpp:826
static int erase(const std::string &filePath)
Definition: UFile.cpp:58
std::map< std::string, std::string > ParametersMap
Definition: Parameters.h:43
void init(const ParametersMap &parameters, const std::string &databasePath="", bool loadDatabaseParameters=false)
Definition: Rtabmap.cpp:292
const cv::Mat & imageRaw() const
Definition: SensorData.h:164
float getLoopClosureValue() const
Definition: Rtabmap.h:128
int getHighestHypothesisId() const
Definition: Rtabmap.h:129
void showUsage()
SensorData takeImage(CameraInfo *info=0)
Definition: Camera.cpp:72
void close(bool databaseSaved=true, const std::string &ouputDatabasePath="")
Definition: Rtabmap.cpp:392
int getLastLocationId() const
Definition: Rtabmap.cpp:609
int getLoopClosureId() const
Definition: Rtabmap.h:127
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:992
virtual bool init(const std::string &calibrationFolder=".", const std::string &cameraName="")
static std::string getDefaultDatabaseName()
Definition: Parameters.cpp:88
void setTimeThreshold(float maxTimeAllowed)
Definition: Rtabmap.cpp:3828
static std::string createDefaultWorkingDirectory()
Definition: Parameters.cpp:66


rtabmap
Author(s): Mathieu Labbe
autogenerated on Mon Dec 14 2020 03:34:59