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/Odometry.h>
34 #include <rtabmap/core/odometry/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 
46 void showUsage()
47 {
48  printf("\nUsage:\n"
49  "odometryViewer [options]\n"
50  "Options:\n"
51  " -driver # Driver number to use: \n"
52  " 0=OpenNI-PCL (Kinect)\n"
53  " 1=OpenNI2 (Kinect and Xtion PRO Live)\n"
54  " 2=Freenect (Kinect)\n"
55  " 3=OpenNI-CV (Kinect)\n"
56  " 4=OpenNI-CV-ASUS (Xtion PRO Live)\n"
57  " 5=Freenect2 (Kinect v2)\n"
58  " 6=DC1394 (Bumblebee2)\n"
59  " 7=FlyCapture2 (Bumblebee2)\n"
60  " 8=ZED stereo\n"
61  " 9=RealSense\n"
62  " 10=Kinect for Windows 2 SDK\n"
63  " 11=RealSense2\n"
64  " 12=Kinect for Azure SDK\n"
65  " 13=MYNT EYE S\n"
66  " -hz #.# Camera rate (default 0, 0 means as fast as the camera can)\n"
67  " -db \"input.db\" Use database instead of camera (recorded with rtabmap-dataRecorder)\n"
68  " -clouds # Maximum clouds shown (default 10, zero means inf)\n"
69  " -sec #.# Delay (seconds) before reading the database (if set)\n"
70  "%s\n",
71  rtabmap::Parameters::showUsage());
72  exit(1);
73 }
74 
75 int main (int argc, char * argv[])
76 {
77  ULogger::setType(ULogger::kTypeConsole);
78  ULogger::setLevel(ULogger::kInfo);
79 
80  // parse arguments
81  float rate = 0.0;
82  std::string inputDatabase;
83  int driver = 0;
84  int maxClouds = 10;
85  float sec = 0.0f;
86 
87  for(int i=1; i<argc; ++i)
88  {
89  if(strcmp(argv[i], "-driver") == 0)
90  {
91  ++i;
92  if(i < argc)
93  {
94  driver = std::atoi(argv[i]);
95  if(driver < 0 || driver > 13)
96  {
97  showUsage();
98  }
99  }
100  else
101  {
102  showUsage();
103  }
104  continue;
105  }
106  if(strcmp(argv[i], "-hz") == 0)
107  {
108  ++i;
109  if(i < argc)
110  {
111  rate = uStr2Float(argv[i]);
112  if(rate < 0)
113  {
114  showUsage();
115  }
116  }
117  else
118  {
119  showUsage();
120  }
121  continue;
122  }
123  if(strcmp(argv[i], "-db") == 0)
124  {
125  ++i;
126  if(i < argc)
127  {
128  inputDatabase = argv[i];
129  if(UFile::getExtension(inputDatabase).compare("db") != 0)
130  {
131  printf("Database path (%s) should end with \"db\" \n", inputDatabase.c_str());
132  showUsage();
133  }
134  }
135  else
136  {
137  showUsage();
138  }
139  continue;
140  }
141  if(strcmp(argv[i], "-clouds") == 0)
142  {
143  ++i;
144  if(i < argc)
145  {
146  maxClouds = std::atoi(argv[i]);
147  if(maxClouds < 0)
148  {
149  showUsage();
150  }
151  }
152  else
153  {
154  showUsage();
155  }
156  continue;
157  }
158  if(strcmp(argv[i], "-sec") == 0)
159  {
160  ++i;
161  if(i < argc)
162  {
163  sec = uStr2Float(argv[i]);
164  if(sec < 0.0f)
165  {
166  showUsage();
167  }
168  }
169  else
170  {
171  showUsage();
172  }
173  continue;
174  }
175  if(strcmp(argv[i], "-help") == 0 || strcmp(argv[i], "--help") == 0)
176  {
177  showUsage();
178  }
179  }
180 
181  if(inputDatabase.size())
182  {
183  UINFO("Using database input \"%s\"", inputDatabase.c_str());
184  }
185  else
186  {
187  UINFO("Using OpenNI camera");
188  }
189 
190  UINFO("Camera rate = %f Hz", rate);
191  UINFO("Maximum clouds shown = %d", maxClouds);
192  UINFO("Delay = %f s", sec);
193 
194  rtabmap::ParametersMap parameters = rtabmap::Parameters::parseArguments(argc, argv);
195  for(rtabmap::ParametersMap::iterator iter=parameters.begin(); iter!=parameters.end(); ++iter)
196  {
197  UINFO(" Param \"%s\"=\"%s\"", iter->first.c_str(), iter->second.c_str());
198  }
199 
200  bool icp = false;
201  int regStrategy = rtabmap::Parameters::defaultRegStrategy();
202  int odomStrategy = rtabmap::Parameters::defaultOdomStrategy();
203  rtabmap::Parameters::parse(parameters, rtabmap::Parameters::kRegStrategy(), regStrategy);
204  rtabmap::Parameters::parse(parameters, rtabmap::Parameters::kOdomStrategy(), odomStrategy);
205  int decimation = 8;
206  float maxDepth = 4.0f;
207  float voxelSize = rtabmap::Parameters::defaultIcpVoxelSize();
208  int normalsK = 0;
209  float normalsRadius = 0.0f;
210  if(regStrategy == 1 || regStrategy == 2)
211  {
212  // icp requires scans
213  icp = true;
214 
215  rtabmap::Parameters::parse(parameters, rtabmap::Parameters::kIcpDownsamplingStep(), decimation);
216  rtabmap::Parameters::parse(parameters, rtabmap::Parameters::kIcpVoxelSize(), voxelSize);
217 
218  bool pointToPlane = rtabmap::Parameters::defaultIcpPointToPlane();
219  rtabmap::Parameters::parse(parameters, rtabmap::Parameters::kIcpPointToPlane(), pointToPlane);
220  if(pointToPlane)
221  {
222  normalsK = rtabmap::Parameters::defaultIcpPointToPlaneK();
223  rtabmap::Parameters::parse(parameters, rtabmap::Parameters::kIcpPointToPlaneK(), normalsK);
224  normalsRadius = rtabmap::Parameters::defaultIcpPointToPlaneRadius();
225  rtabmap::Parameters::parse(parameters, rtabmap::Parameters::kIcpPointToPlaneRadius(), normalsRadius);
226  }
227 
228  uInsert(parameters, rtabmap::ParametersPair(rtabmap::Parameters::kIcpDownsamplingStep(), "1"));
229  uInsert(parameters, rtabmap::ParametersPair(rtabmap::Parameters::kIcpVoxelSize(), "0"));
230  }
231 
232  QApplication app(argc, argv);
233 
234  rtabmap::Odometry * odom;
235  if(odomStrategy == -1)
236  {
237  // experimental mono
238  odom = new rtabmap::OdometryMono(parameters);
239  }
240  else
241  {
242  odom = rtabmap::Odometry::create(parameters);
243  }
244 
245 
246  rtabmap::OdometryThread odomThread(odom);
247  rtabmap::OdometryViewer odomViewer(maxClouds, 2, 0.0, 50);
248  UEventsManager::addHandler(&odomThread);
249  UEventsManager::addHandler(&odomViewer);
250 
251  odomViewer.setWindowTitle("Odometry view");
252  odomViewer.resize(1280, 480+QPushButton().minimumHeight());
253 
254  rtabmap::Camera * camera = 0;
255  if(inputDatabase.size())
256  {
257  camera = new rtabmap::DBReader(inputDatabase, rate, true);
258  }
259  else if(driver == 0)
260  {
261  camera = new rtabmap::CameraOpenni("", rate);
262  }
263  else if(driver == 1)
264  {
265  if(!rtabmap::CameraOpenNI2::available())
266  {
267  UERROR("Not built with OpenNI2 support...");
268  exit(-1);
269  }
270  camera = new rtabmap::CameraOpenNI2("", rtabmap::CameraOpenNI2::kTypeColorDepth, rate);
271  }
272  else if(driver == 2)
273  {
274  if(!rtabmap::CameraFreenect::available())
275  {
276  UERROR("Not built with Freenect support...");
277  exit(-1);
278  }
279  camera = new rtabmap::CameraFreenect(0, rtabmap::CameraFreenect::kTypeColorDepth, rate);
280  }
281  else if(driver == 3)
282  {
283  if(!rtabmap::CameraOpenNICV::available())
284  {
285  UERROR("Not built with OpenNI from OpenCV support...");
286  exit(-1);
287  }
288  camera = new rtabmap::CameraOpenNICV(false, rate);
289  }
290  else if(driver == 4)
291  {
292  if(!rtabmap::CameraOpenNICV::available())
293  {
294  UERROR("Not built with OpenNI from OpenCV support...");
295  exit(-1);
296  }
297  camera = new rtabmap::CameraOpenNICV(true, rate);
298  }
299  else if(driver == 5)
300  {
301  if(!rtabmap::CameraFreenect2::available())
302  {
303  UERROR("Not built with Freenect2 support...");
304  exit(-1);
305  }
306  camera = new rtabmap::CameraFreenect2(0, rtabmap::CameraFreenect2::kTypeColor2DepthSD, rate);
307  }
308  else if(driver == 6)
309  {
310  if(!rtabmap::CameraStereoDC1394::available())
311  {
312  UERROR("Not built with dc1394 support...");
313  exit(-1);
314  }
315  camera = new rtabmap::CameraStereoDC1394(rate);
316  }
317  else if(driver == 7)
318  {
319  if(!rtabmap::CameraStereoFlyCapture2::available())
320  {
321  UERROR("Not built with FlyCapture2/Triclops support...");
322  exit(-1);
323  }
324  camera = new rtabmap::CameraStereoFlyCapture2(rate);
325  }
326  else if(driver == 8)
327  {
328  if(!rtabmap::CameraStereoZed::available())
329  {
330  UERROR("Not built with ZED sdk support...");
331  exit(-1);
332  }
333  camera = new rtabmap::CameraStereoZed(0,3,1,0,100,false,rate);
334  }
335  else if (driver == 9)
336  {
337  if (!rtabmap::CameraRealSense::available())
338  {
339  UERROR("Not built with RealSense support...");
340  exit(-1);
341  }
342  camera = new rtabmap::CameraRealSense(0, 0, 0, false, rate);
343  }
344  else if (driver == 10)
345  {
346  if (!rtabmap::CameraK4W2::available())
347  {
348  UERROR("Not built with Kinect for Windows 2 SDK support...");
349  exit(-1);
350  }
351  camera = new rtabmap::CameraK4W2(0, rtabmap::CameraK4W2::kTypeDepth2ColorSD, rate);
352  }
353  else if (driver == 11)
354  {
355  if (!rtabmap::CameraRealSense2::available())
356  {
357  UERROR("Not built with RealSense2 SDK support...");
358  exit(-1);
359  }
360  camera = new rtabmap::CameraRealSense2("", rate);
361  }
362  else if (driver == 12)
363  {
364  if (!rtabmap::CameraK4A::available())
365  {
366  UERROR("Not built with Kinect for Azure SDK support...");
367  exit(-1);
368  }
369  camera = new rtabmap::CameraK4A();
370  }
371  else if (driver == 13)
372  {
373  if (!rtabmap::CameraMyntEye::available())
374  {
375  UERROR("Not built with Mynt Eye S support...");
376  exit(-1);
377  }
378  camera = new rtabmap::CameraMyntEye("", false, false, rate);
379  }
380  else
381  {
382  UFATAL("Camera driver (%d) not found!", driver);
383  }
384 
385  //pcl::console::setVerbosityLevel(pcl::console::L_DEBUG);
386 
387  if(camera->init())
388  {
389  if(camera->isCalibrated())
390  {
391  rtabmap::CameraThread cameraThread(camera, parameters);
392 
393  cameraThread.setScanParameters(icp, decimation<1?1:decimation, 0, maxDepth, voxelSize, normalsK, normalsRadius);
394 
395  odomThread.start();
396  cameraThread.start();
397 
398  odomViewer.exec();
399 
400  cameraThread.join(true);
401  odomThread.join(true);
402  }
403  else
404  {
405  printf("The camera is not calibrated! You should calibrate the camera first.\n");
406  delete camera;
407  }
408  }
409  else
410  {
411  printf("Failed to initialize the camera! Please select another driver (see \"--help\").\n");
412  delete camera;
413  }
414 
415  return 0;
416 }
rtabmap::Parameters::parse
static bool parse(const ParametersMap &parameters, const std::string &key, bool &value)
Definition: Parameters.cpp:476
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:548
rtabmap::ParametersPair
std::pair< std::string, std::string > ParametersPair
Definition: Parameters.h:44
rtabmap::Parameters::parseArguments
static ParametersMap parseArguments(int argc, char *argv[], bool onlyParameters=false)
Definition: Parameters.cpp:575
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:43
showUsage
void showUsage()
Definition: tools/OdometryViewer/main.cpp:46
UConversion.h
Some conversion functions.
UFile::getExtension
std::string getExtension()
Definition: UFile.h:140
ULogger::setLevel
static void setLevel(ULogger::Level level)
Definition: ULogger.h:339
UFATAL
#define UFATAL(...)
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::Odometry::create
static Odometry * create(const ParametersMap &parameters=ParametersMap())
Definition: Odometry.cpp:54
rtabmap::CameraThread::setScanParameters
void setScanParameters(bool fromDepth, int downsampleStep=1, float rangeMin=0.0f, float rangeMax=0.0f, float voxelSize=0.0f, int normalsK=0, int normalsRadius=0.0f, float groundNormalsUp=0.0f)
Definition: CameraThread.cpp:132
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:418
ULogger::setType
static void setType(Type type, const std::string &fileName=kDefaultLogFileName, bool append=true)
Definition: ULogger.cpp:176
app
QApplication * app
Definition: tools/DataRecorder/main.cpp:70
UEventsManager::addHandler
static void addHandler(UEventsHandler *handler)
Definition: UEventsManager.cpp:28
UERROR
#define UERROR(...)
rtabmap_oanet.argv
argv
Definition: rtabmap_oanet.py:12
ULogger.h
ULogger class and convenient macros.
rtabmap::CameraK4A::available
static bool available()
Definition: CameraK4A.cpp:42
UThread::join
void join(bool killFirst=false)
Definition: UThread.cpp:85
main
int main(int argc, char *argv[])
Definition: tools/OdometryViewer/main.cpp:75
rtabmap::CameraK4W2::available
static bool available()
Definition: CameraK4W2.cpp:53
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 Mon Dec 14 2020 03:34:59