tools/OdometryViewer/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/utilite/ULogger.h>
29 #include <rtabmap/utilite/UEventsManager.h>
30 #include <rtabmap/utilite/UFile.h>
31 #include <rtabmap/utilite/UConversion.h>
32 #include <rtabmap/utilite/UStl.h>
33 #include <rtabmap/core/OdometryF2F.h>
34 #include <rtabmap/core/OdometryMono.h>
35 #include <rtabmap/core/OdometryThread.h>
36 #include <rtabmap/gui/OdometryViewer.h>
37 #include <rtabmap/core/CameraThread.h>
38 #include <rtabmap/core/CameraRGBD.h>
39 #include <rtabmap/core/CameraStereo.h>
40 #include <rtabmap/core/DBReader.h>
41 #include <rtabmap/core/VWDictionary.h>
42 #include <QApplication>
43 #include <QPushButton>
44 #include <pcl/console/print.h>
45 #include <rtabmap/core/OdometryF2M.h>
46 
47 void showUsage()
48 {
49  printf("\nUsage:\n"
50  "odometryViewer [options]\n"
51  "Options:\n"
52  " -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"
53  " -hz #.# Camera rate (default 0, 0 means as fast as the camera can)\n"
54  " -db \"input.db\" Use database instead of camera (recorded with rtabmap-dataRecorder)\n"
55  " -clouds # Maximum clouds shown (default 10, zero means inf)\n"
56  " -sec #.# Delay (seconds) before reading the database (if set)\n"
57  "%s\n",
58  rtabmap::Parameters::showUsage());
59  exit(1);
60 }
61 
62 int main (int argc, char * argv[])
63 {
64  ULogger::setType(ULogger::kTypeConsole);
65  ULogger::setLevel(ULogger::kInfo);
66 
67  // parse arguments
68  float rate = 0.0;
69  std::string inputDatabase;
70  int driver = 0;
71  int maxClouds = 10;
72  float sec = 0.0f;
73 
74  for(int i=1; i<argc; ++i)
75  {
76  if(strcmp(argv[i], "-driver") == 0)
77  {
78  ++i;
79  if(i < argc)
80  {
81  driver = std::atoi(argv[i]);
82  if(driver < 0 || driver > 7)
83  {
84  showUsage();
85  }
86  }
87  else
88  {
89  showUsage();
90  }
91  continue;
92  }
93  if(strcmp(argv[i], "-hz") == 0)
94  {
95  ++i;
96  if(i < argc)
97  {
98  rate = uStr2Float(argv[i]);
99  if(rate < 0)
100  {
101  showUsage();
102  }
103  }
104  else
105  {
106  showUsage();
107  }
108  continue;
109  }
110  if(strcmp(argv[i], "-db") == 0)
111  {
112  ++i;
113  if(i < argc)
114  {
115  inputDatabase = argv[i];
116  if(UFile::getExtension(inputDatabase).compare("db") != 0)
117  {
118  printf("Database path (%s) should end with \"db\" \n", inputDatabase.c_str());
119  showUsage();
120  }
121  }
122  else
123  {
124  showUsage();
125  }
126  continue;
127  }
128  if(strcmp(argv[i], "-clouds") == 0)
129  {
130  ++i;
131  if(i < argc)
132  {
133  maxClouds = std::atoi(argv[i]);
134  if(maxClouds < 0)
135  {
136  showUsage();
137  }
138  }
139  else
140  {
141  showUsage();
142  }
143  continue;
144  }
145  if(strcmp(argv[i], "-sec") == 0)
146  {
147  ++i;
148  if(i < argc)
149  {
150  sec = uStr2Float(argv[i]);
151  if(sec < 0.0f)
152  {
153  showUsage();
154  }
155  }
156  else
157  {
158  showUsage();
159  }
160  continue;
161  }
162  if(strcmp(argv[i], "-help") == 0 || strcmp(argv[i], "--help") == 0)
163  {
164  showUsage();
165  }
166  }
167 
168  if(inputDatabase.size())
169  {
170  UINFO("Using database input \"%s\"", inputDatabase.c_str());
171  }
172  else
173  {
174  UINFO("Using OpenNI camera");
175  }
176 
177  UINFO("Camera rate = %f Hz", rate);
178  UINFO("Maximum clouds shown = %d", maxClouds);
179  UINFO("Delay = %f s", sec);
180 
181  rtabmap::ParametersMap parameters = rtabmap::Parameters::parseArguments(argc, argv);
182  for(rtabmap::ParametersMap::iterator iter=parameters.begin(); iter!=parameters.end(); ++iter)
183  {
184  UINFO(" Param \"%s\"=\"%s\"", iter->first.c_str(), iter->second.c_str());
185  }
186 
187  bool icp = false;
188  int regStrategy = rtabmap::Parameters::defaultRegStrategy();
189  rtabmap::Parameters::parse(parameters, rtabmap::Parameters::kRegStrategy(), regStrategy);
190  int decimation = 8;
191  float maxDepth = 4.0f;
192  float voxelSize = rtabmap::Parameters::defaultIcpVoxelSize();
193  int normalsK = 0;
194  float normalsRadius = 0.0f;
195  if(regStrategy == 1 || regStrategy == 2)
196  {
197  // icp requires scans
198  icp = true;
199 
200  rtabmap::Parameters::parse(parameters, rtabmap::Parameters::kIcpDownsamplingStep(), decimation);
201  rtabmap::Parameters::parse(parameters, rtabmap::Parameters::kIcpVoxelSize(), voxelSize);
202 
203  bool pointToPlane = rtabmap::Parameters::defaultIcpPointToPlane();
204  rtabmap::Parameters::parse(parameters, rtabmap::Parameters::kIcpPointToPlane(), pointToPlane);
205  if(pointToPlane)
206  {
207  normalsK = rtabmap::Parameters::defaultIcpPointToPlaneK();
208  rtabmap::Parameters::parse(parameters, rtabmap::Parameters::kIcpPointToPlaneK(), normalsK);
209  normalsRadius = rtabmap::Parameters::defaultIcpPointToPlaneRadius();
210  rtabmap::Parameters::parse(parameters, rtabmap::Parameters::kIcpPointToPlaneRadius(), normalsRadius);
211  }
212 
213  uInsert(parameters, rtabmap::ParametersPair(rtabmap::Parameters::kIcpDownsamplingStep(), "1"));
214  uInsert(parameters, rtabmap::ParametersPair(rtabmap::Parameters::kIcpVoxelSize(), "0"));
215  }
216 
217  QApplication app(argc, argv);
218 
219  rtabmap::Odometry * odom = rtabmap::Odometry::create(parameters);
220 
221  rtabmap::OdometryThread odomThread(odom);
222  rtabmap::OdometryViewer odomViewer(maxClouds, 2, 0.0, 50);
223  UEventsManager::addHandler(&odomThread);
224  UEventsManager::addHandler(&odomViewer);
225 
226  odomViewer.setWindowTitle("Odometry view");
227  odomViewer.resize(1280, 480+QPushButton().minimumHeight());
228 
229  rtabmap::Camera * camera = 0;
230  rtabmap::Transform t=rtabmap::Transform(0,0,1,0, -1,0,0,0, 0,-1,0,0);
231 
232  if(inputDatabase.size())
233  {
234  camera = new rtabmap::DBReader(inputDatabase, rate, true);
235  }
236  else if(driver == 0)
237  {
238  camera = new rtabmap::CameraOpenni("", rate, t);
239  }
240  else if(driver == 1)
241  {
242  if(!rtabmap::CameraOpenNI2::available())
243  {
244  UERROR("Not built with OpenNI2 support...");
245  exit(-1);
246  }
247  camera = new rtabmap::CameraOpenNI2("", rtabmap::CameraOpenNI2::kTypeColorDepth, rate, t);
248  }
249  else if(driver == 2)
250  {
251  if(!rtabmap::CameraFreenect::available())
252  {
253  UERROR("Not built with Freenect support...");
254  exit(-1);
255  }
256  camera = new rtabmap::CameraFreenect(0, rtabmap::CameraFreenect::kTypeColorDepth, rate, t);
257  }
258  else if(driver == 3)
259  {
260  if(!rtabmap::CameraOpenNICV::available())
261  {
262  UERROR("Not built with OpenNI from OpenCV support...");
263  exit(-1);
264  }
265  camera = new rtabmap::CameraOpenNICV(false, rate, t);
266  }
267  else if(driver == 4)
268  {
269  if(!rtabmap::CameraOpenNICV::available())
270  {
271  UERROR("Not built with OpenNI from OpenCV support...");
272  exit(-1);
273  }
274  camera = new rtabmap::CameraOpenNICV(true, rate, t);
275  }
276  else if(driver == 5)
277  {
278  if(!rtabmap::CameraFreenect2::available())
279  {
280  UERROR("Not built with Freenect2 support...");
281  exit(-1);
282  }
283  camera = new rtabmap::CameraFreenect2(0, rtabmap::CameraFreenect2::kTypeColor2DepthSD, rate, t);
284  }
285  else if(driver == 6)
286  {
287  if(!rtabmap::CameraStereoDC1394::available())
288  {
289  UERROR("Not built with dc1394 support...");
290  exit(-1);
291  }
292  camera = new rtabmap::CameraStereoDC1394(rate, t);
293  }
294  else if(driver == 7)
295  {
296  if(!rtabmap::CameraStereoFlyCapture2::available())
297  {
298  UERROR("Not built with FlyCapture2/Triclops support...");
299  exit(-1);
300  }
301  camera = new rtabmap::CameraStereoFlyCapture2(rate, t);
302  }
303  else
304  {
305  UFATAL("Camera driver (%d) not found!", driver);
306  }
307 
308  //pcl::console::setVerbosityLevel(pcl::console::L_DEBUG);
309 
310  if(camera->init())
311  {
312  if(camera->isCalibrated())
313  {
314  rtabmap::CameraThread cameraThread(camera, parameters);
315 
316  cameraThread.setScanFromDepth(icp, decimation<1?1:decimation, maxDepth, voxelSize, normalsK, normalsRadius);
317 
318  odomThread.start();
319  cameraThread.start();
320 
321  odomViewer.exec();
322 
323  cameraThread.join(true);
324  odomThread.join(true);
325  }
326  else
327  {
328  printf("The camera is not calibrated! You should calibrate the camera first.\n");
329  delete camera;
330  }
331  }
332  else
333  {
334  printf("Failed to initialize the camera! Please select another driver (see \"--help\").\n");
335  delete camera;
336  }
337 
338  return 0;
339 }
rtabmap::Parameters::parse
static bool parse(const ParametersMap &parameters, const std::string &key, bool &value)
Definition: Parameters.cpp:446
UThread::start
void start()
Definition: UThread.cpp:122
glm::sec
GLM_FUNC_DECL genType sec(genType const &angle)
f
f
rtabmap::Parameters::showUsage
static const char * showUsage()
Definition: Parameters.cpp:518
rtabmap::ParametersPair
std::pair< std::string, std::string > ParametersPair
Definition: Parameters.h:42
rtabmap::Parameters::parseArguments
static ParametersMap parseArguments(int argc, char *argv[], bool onlyParameters=false)
Definition: Parameters.cpp:543
uStr2Float
float UTILITE_EXP uStr2Float(const std::string &str)
Definition: UConversion.cpp:123
rtabmap::ParametersMap
std::map< std::string, std::string > ParametersMap
Definition: Parameters.h:41
showUsage
void showUsage()
Definition: tools/OdometryViewer/main.cpp:47
UConversion.h
Some conversion functions.
UFile::getExtension
std::string getExtension()
Definition: UFile.h:140
rtabmap::CameraOpenNI2::available
static bool available()
Definition: CameraRGBD.cpp:382
ULogger::setLevel
static void setLevel(ULogger::Level level)
Definition: ULogger.h:339
UFATAL
#define UFATAL(...)
rtabmap::CameraOpenNICV::available
static bool available()
Definition: CameraRGBD.cpp:272
rtabmap::Camera::init
virtual bool init(const std::string &calibrationFolder=".", const std::string &cameraName="")=0
UStl.h
Wrappers of STL for convenient functions.
rtabmap::Camera::isCalibrated
virtual bool isCalibrated() const =0
rtabmap::util3d::icp
Transform RTABMAP_EXP icp(const pcl::PointCloud< pcl::PointXYZ >::ConstPtr &cloud_source, const pcl::PointCloud< pcl::PointXYZ >::ConstPtr &cloud_target, double maxCorrespondenceDistance, int maximumIterations, bool &hasConverged, pcl::PointCloud< pcl::PointXYZ > &cloud_source_registered, float epsilon=0.0f, bool icp2D=false)
Definition: util3d_registration.cpp:335
ULogger::setType
static void setType(Type type, const std::string &fileName=kDefaultLogFileName, bool append=true)
Definition: ULogger.cpp:176
rtabmap::Odometry::create
static Odometry * create(const ParametersMap &parameters)
Definition: Odometry.cpp:53
app
static RTABMapApp app
Definition: jni_interface.cpp:34
rtabmap::CameraThread::setScanFromDepth
void setScanFromDepth(bool enabled, int decimation=4, float maxDepth=4.0f, float voxelSize=0.0f, int normalsK=0, int normalsRadius=0.0f)
Definition: CameraThread.h:72
UEventsManager::addHandler
static void addHandler(UEventsHandler *handler)
Definition: UEventsManager.cpp:28
UERROR
#define UERROR(...)
ULogger.h
ULogger class and convenient macros.
UThread::join
void join(bool killFirst=false)
Definition: UThread.cpp:85
main
int main(int argc, char *argv[])
Definition: tools/OdometryViewer/main.cpp:62
uInsert
void uInsert(std::map< K, V > &map, const std::pair< K, V > &pair)
Definition: UStl.h:443
UINFO
#define UINFO(...)

rtabmap
Author(s): Mathieu Labbe
autogenerated on Wed Jun 5 2019 22:41:31