tools/CameraRGBD/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/CameraRGBD.h"
29 #include "rtabmap/core/CameraStereo.h"
30 #include "rtabmap/core/util2d.h"
31 #include "rtabmap/core/util3d.h"
32 #include "rtabmap/core/util3d_transforms.h"
33 #include "rtabmap/utilite/ULogger.h"
34 #include "rtabmap/utilite/UMath.h"
35 #include "rtabmap/utilite/UFile.h"
36 #include "rtabmap/utilite/UDirectory.h"
37 #include "rtabmap/utilite/UConversion.h"
38 #include <opencv2/highgui/highgui.hpp>
39 #include <opencv2/imgproc/imgproc.hpp>
40 #include <opencv2/imgproc/types_c.h>
41 #if CV_MAJOR_VERSION >= 3
42 #include <opencv2/videoio/videoio_c.h>
43 #endif
44 #include <pcl/visualization/cloud_viewer.h>
45 #include <stdio.h>
46 #include <signal.h>
47 
48 void showUsage()
49 {
50  printf("\nUsage:\n"
51  "rtabmap-rgbd_camera [options] driver\n"
52  " driver Driver number to use: 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  " 14=ZED Open Capture\n"
67  " 15=depthai-core\n"
68  " Options:\n"
69  " -rate #.# Input rate Hz (default 0=inf)\n"
70  " -device # Device ID (number or string)\n"
71  " -save_stereo \"path\" Save stereo images in a folder or a video file (side by side *.avi).\n"
72  " -fourcc \"XXXX\" Four characters FourCC code (default is \"MJPG\") used\n"
73  " when saving stereo images to a video file.\n"
74  " See http://www.fourcc.org/codecs.php for more codes.\n");
75  exit(1);
76 }
77 
78 // catch ctrl-c
79 bool running = true;
80 void sighandler(int sig)
81 {
82  printf("\nSignal %d caught...\n", sig);
83  running = false;
84 }
85 
86 int main(int argc, char * argv[])
87 {
88  ULogger::setType(ULogger::kTypeConsole);
89  ULogger::setLevel(ULogger::kInfo);
90  //ULogger::setPrintTime(false);
91  //ULogger::setPrintWhere(false);
92 
93  int driver = 0;
94  std::string stereoSavePath;
95  float rate = 0.0f;
96  std::string fourcc = "MJPG";
97  std::string deviceId;
98  if(argc < 2)
99  {
100  showUsage();
101  }
102  else
103  {
104  for(int i=1; i<argc; ++i)
105  {
106  if(strcmp(argv[i], "-rate") == 0)
107  {
108  ++i;
109  if(i < argc)
110  {
111  rate = uStr2Float(argv[i]);
112  if(rate < 0.0f)
113  {
114  showUsage();
115  }
116  }
117  else
118  {
119  showUsage();
120  }
121  continue;
122  }
123  if (strcmp(argv[i], "-device") == 0)
124  {
125  ++i;
126  if (i < argc)
127  {
128  deviceId = argv[i];
129  }
130  else
131  {
132  showUsage();
133  }
134  continue;
135  }
136  if(strcmp(argv[i], "-save_stereo") == 0)
137  {
138  ++i;
139  if(i < argc)
140  {
141  stereoSavePath = argv[i];
142  }
143  else
144  {
145  showUsage();
146  }
147  continue;
148  }
149  if(strcmp(argv[i], "-fourcc") == 0)
150  {
151  ++i;
152  if(i < argc)
153  {
154  fourcc = argv[i];
155  if(fourcc.size() != 4)
156  {
157  UERROR("fourcc should be 4 characters.");
158  showUsage();
159  }
160  }
161  else
162  {
163  showUsage();
164  }
165  continue;
166  }
167  if(strcmp(argv[i], "--help") == 0 || strcmp(argv[i], "-help") == 0)
168  {
169  showUsage();
170  }
171  else if(i< argc-1)
172  {
173  printf("Unrecognized option \"%s\"", argv[i]);
174  showUsage();
175  }
176 
177  // last
178  driver = atoi(argv[i]);
179  if(driver < 0 || driver > 15)
180  {
181  UERROR("driver should be between 0 and 15.");
182  showUsage();
183  }
184  }
185  }
186  UINFO("Using driver %d (device=%s)", driver, deviceId.empty()?"0": deviceId.c_str());
187 
188  rtabmap::Camera * camera = 0;
189  if(driver < 6)
190  {
191  if(!stereoSavePath.empty())
192  {
193  UWARN("-save_stereo option cannot be used with RGB-D drivers.");
194  stereoSavePath.clear();
195  }
196 
197  if(driver == 0)
198  {
199  camera = new rtabmap::CameraOpenni(deviceId);
200  }
201  else if(driver == 1)
202  {
203  if(!rtabmap::CameraOpenNI2::available())
204  {
205  UERROR("Not built with OpenNI2 support...");
206  exit(-1);
207  }
208  camera = new rtabmap::CameraOpenNI2(deviceId);
209  }
210  else if(driver == 2)
211  {
212  if(!rtabmap::CameraFreenect::available())
213  {
214  UERROR("Not built with Freenect support...");
215  exit(-1);
216  }
217  camera = new rtabmap::CameraFreenect(deviceId.empty()?0:uStr2Int(deviceId));
218  }
219  else if(driver == 3)
220  {
221  if(!rtabmap::CameraOpenNICV::available())
222  {
223  UERROR("Not built with OpenNI from OpenCV support...");
224  exit(-1);
225  }
226  camera = new rtabmap::CameraOpenNICV(false);
227  }
228  else if(driver == 4)
229  {
230  if(!rtabmap::CameraOpenNICV::available())
231  {
232  UERROR("Not built with OpenNI from OpenCV support...");
233  exit(-1);
234  }
235  camera = new rtabmap::CameraOpenNICV(true);
236  }
237  else if(driver == 5)
238  {
239  if(!rtabmap::CameraFreenect2::available())
240  {
241  UERROR("Not built with Freenect2 support...");
242  exit(-1);
243  }
244  camera = new rtabmap::CameraFreenect2(deviceId.empty()?0:uStr2Int(deviceId), rtabmap::CameraFreenect2::kTypeColor2DepthSD);
245  }
246  }
247  else if(driver == 6)
248  {
249  if(!rtabmap::CameraStereoDC1394::available())
250  {
251  UERROR("Not built with DC1394 support...");
252  exit(-1);
253  }
254  camera = new rtabmap::CameraStereoDC1394();
255  }
256  else if(driver == 7)
257  {
258  if(!rtabmap::CameraStereoFlyCapture2::available())
259  {
260  UERROR("Not built with FlyCapture2/Triclops support...");
261  exit(-1);
262  }
263  camera = new rtabmap::CameraStereoFlyCapture2();
264  }
265  else if(driver == 8)
266  {
267  if(!rtabmap::CameraStereoZed::available())
268  {
269  UERROR("Not built with ZED sdk support...");
270  exit(-1);
271  }
272  camera = new rtabmap::CameraStereoZed(deviceId.empty()?0:uStr2Int(deviceId));
273  }
274  else if (driver == 9)
275  {
276  if (!rtabmap::CameraRealSense::available())
277  {
278  UERROR("Not built with RealSense support...");
279  exit(-1);
280  }
281  camera = new rtabmap::CameraRealSense(deviceId.empty()?0:uStr2Int(deviceId));
282  }
283  else if (driver == 10)
284  {
285  if (!rtabmap::CameraK4W2::available())
286  {
287  UERROR("Not built with Kinect for Windows 2 SDK support...");
288  exit(-1);
289  }
290  camera = new rtabmap::CameraK4W2(deviceId.empty()?0:uStr2Int(deviceId));
291  }
292  else if (driver == 11)
293  {
294  if (!rtabmap::CameraRealSense2::available())
295  {
296  UERROR("Not built with RealSense2 SDK support...");
297  exit(-1);
298  }
299  camera = new rtabmap::CameraRealSense2(deviceId);
300  }
301  else if (driver == 12)
302  {
303  if (!rtabmap::CameraK4A::available())
304  {
305  UERROR("Not built with Kinect for Azure SDK support...");
306  exit(-1);
307  }
308  camera = new rtabmap::CameraK4A(1);
309  }
310  else if (driver == 13)
311  {
312  if (!rtabmap::CameraMyntEye::available())
313  {
314  UERROR("Not built with Mynt Eye S support...");
315  exit(-1);
316  }
317  camera = new rtabmap::CameraMyntEye(deviceId);
318  }
319  else if (driver == 14)
320  {
321  if (!rtabmap::CameraStereoZedOC::available())
322  {
323  UERROR("Not built with Zed Open Capture support...");
324  exit(-1);
325  }
326  camera = new rtabmap::CameraStereoZedOC(deviceId.empty()?-1:uStr2Int(deviceId));
327  }
328  else if (driver == 15)
329  {
330  if (!rtabmap::CameraDepthAI::available())
331  {
332  UERROR("Not built with depthai-core support...");
333  exit(-1);
334  }
335  camera = new rtabmap::CameraDepthAI(deviceId);
336  }
337  else
338  {
339  UFATAL("");
340  }
341 
342  if(!camera->init())
343  {
344  printf("Camera init failed! Please select another driver (see \"--help\").\n");
345  delete camera;
346  exit(1);
347  }
348 
349  rtabmap::SensorData data = camera->takeImage();
350  if (data.imageRaw().empty())
351  {
352  printf("Cloud not get frame from the camera!\n");
353  delete camera;
354  exit(1);
355  }
356  if(data.imageRaw().cols % data.depthOrRightRaw().cols != 0 || data.imageRaw().rows % data.depthOrRightRaw().rows != 0)
357  {
358  UWARN("RGB (%d/%d) and depth (%d/%d) frames are not the same size! The registered cloud cannot be shown.",
359  data.imageRaw().cols, data.imageRaw().rows, data.depthOrRightRaw().cols, data.depthOrRightRaw().rows);
360  }
361  pcl::visualization::CloudViewer * viewer = 0;
362  if((data.stereoCameraModels().empty() || data.stereoCameraModels()[0].isValidForProjection()) &&
363  (data.cameraModels().empty() || !data.cameraModels()[0].isValidForProjection()))
364  {
365  UWARN("Camera not calibrated! The registered cloud cannot be shown.");
366  }
367  else
368  {
369  viewer = new pcl::visualization::CloudViewer("cloud");
370  }
371 
372  cv::VideoWriter videoWriter;
373  UDirectory dir;
374  if(!stereoSavePath.empty() &&
375  !data.imageRaw().empty() &&
376  !data.rightRaw().empty())
377  {
378  if(UFile::getExtension(stereoSavePath).compare("avi") == 0)
379  {
380  if(data.imageRaw().size() == data.rightRaw().size())
381  {
382  if(rate <= 0)
383  {
384  UERROR("You should set the input rate when saving stereo images to a video file.");
385  showUsage();
386  }
387  cv::Size targetSize = data.imageRaw().size();
388  targetSize.width *= 2;
389  UASSERT(fourcc.size() == 4);
390  videoWriter.open(
391  stereoSavePath,
392  CV_FOURCC(fourcc.at(0), fourcc.at(1), fourcc.at(2), fourcc.at(3)),
393  rate,
394  targetSize,
395  data.imageRaw().channels() == 3);
396  }
397  else
398  {
399  UERROR("Images not the same size, cannot save stereo images to the video file.");
400  }
401  }
402  else if(UDirectory::exists(stereoSavePath))
403  {
404  UDirectory::makeDir(stereoSavePath+"/"+"left");
405  UDirectory::makeDir(stereoSavePath+"/"+"right");
406  }
407  else
408  {
409  UERROR("Directory \"%s\" doesn't exist.", stereoSavePath.c_str());
410  stereoSavePath.clear();
411  }
412  }
413 
414  // to catch the ctrl-c
415  signal(SIGABRT, &sighandler);
416  signal(SIGTERM, &sighandler);
417  signal(SIGINT, &sighandler);
418 
419  int id=1;
420  while(!data.imageRaw().empty() && (viewer==0 || !viewer->wasStopped()) && running)
421  {
422  cv::Mat rgb = data.imageRaw();
423  if(!data.depthRaw().empty() && (data.depthRaw().type() == CV_16UC1 || data.depthRaw().type() == CV_32FC1))
424  {
425  // depth
426  cv::Mat depth = data.depthRaw();
427  if(depth.type() == CV_32FC1)
428  {
429  depth = rtabmap::util2d::cvtDepthFromFloat(depth);
430  }
431 
432  if(!rgb.empty() && rgb.cols % depth.cols == 0 && rgb.rows % depth.rows == 0 &&
433  data.cameraModels().size() &&
434  data.cameraModels()[0].isValidForProjection())
435  {
436  pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = rtabmap::util3d::cloudFromDepthRGB(
437  rgb, depth,
438  data.cameraModels()[0]);
439  cloud = rtabmap::util3d::transformPointCloud(cloud, rtabmap::Transform::opengl_T_rtabmap()*data.cameraModels()[0].localTransform());
440  if(viewer)
441  viewer->showCloud(cloud, "cloud");
442  }
443  else if(!depth.empty() &&
444  data.cameraModels().size() &&
445  data.cameraModels()[0].isValidForProjection())
446  {
447  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud = rtabmap::util3d::cloudFromDepth(
448  depth,
449  data.cameraModels()[0]);
450  cloud = rtabmap::util3d::transformPointCloud(cloud, rtabmap::Transform::opengl_T_rtabmap()*data.cameraModels()[0].localTransform());
451  viewer->showCloud(cloud, "cloud");
452  }
453 
454  cv::Mat tmp;
455  unsigned short min=0, max = 2048;
456  uMinMax((unsigned short*)depth.data, depth.rows*depth.cols, min, max);
457  depth.convertTo(tmp, CV_8UC1, 255.0/max);
458 
459  cv::imshow("Video", rgb); // show frame
460  cv::imshow("Depth", tmp);
461  }
462  else if(!data.rightRaw().empty())
463  {
464  // stereo
465  cv::Mat right = data.rightRaw();
466  cv::imshow("Left", rgb); // show frame
467  cv::imshow("Right", right);
468 
469  if(rgb.cols == right.cols && rgb.rows == right.rows && data.stereoCameraModels().size()==1 && data.stereoCameraModels()[0].isValidForProjection())
470  {
471  if(right.channels() == 3)
472  {
473  cv::cvtColor(right, right, CV_BGR2GRAY);
474  }
475  pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = rtabmap::util3d::cloudFromStereoImages(
476  rgb, right,
477  data.stereoCameraModels()[0]);
478  cloud = rtabmap::util3d::transformPointCloud(cloud, rtabmap::Transform::opengl_T_rtabmap()*data.stereoCameraModels()[0].localTransform());
479  if(viewer)
480  viewer->showCloud(cloud, "cloud");
481  }
482  }
483 
484  int c = cv::waitKey(10); // wait 10 ms or for key stroke
485  if(c == 27)
486  break; // if ESC, break and quit
487 
488  if(videoWriter.isOpened())
489  {
490  cv::Mat left = data.imageRaw();
491  cv::Mat right = data.rightRaw();
492  if(left.size() == right.size())
493  {
494  cv::Size targetSize = left.size();
495  targetSize.width *= 2;
496  cv::Mat targetImage(targetSize, left.type());
497  if(right.type() != left.type())
498  {
499  cv::Mat tmp;
500  cv::cvtColor(right, tmp, left.channels()==3?CV_GRAY2BGR:CV_BGR2GRAY);
501  right = tmp;
502  }
503  UASSERT(left.type() == right.type());
504 
505  cv::Mat roiA(targetImage, cv::Rect( 0, 0, left.size().width, left.size().height ));
506  left.copyTo(roiA);
507  cv::Mat roiB( targetImage, cvRect( left.size().width, 0, left.size().width, left.size().height ) );
508  right.copyTo(roiB);
509 
510  videoWriter.write(targetImage);
511  printf("Saved frame %d to \"%s\"\n", id, stereoSavePath.c_str());
512  }
513  else
514  {
515  UERROR("Left and right images are not the same size!?");
516  }
517  }
518  else if(!stereoSavePath.empty())
519  {
520  cv::imwrite(stereoSavePath+"/"+"left/"+uNumber2Str(id) + ".jpg", data.imageRaw());
521  cv::imwrite(stereoSavePath+"/"+"right/"+uNumber2Str(id) + ".jpg", data.rightRaw());
522  printf("Saved frames %d to \"%s/left\" and \"%s/right\" directories\n", id, stereoSavePath.c_str(), stereoSavePath.c_str());
523  }
524  ++id;
525  data = camera->takeImage();
526  }
527  printf("Closing...\n");
528  if(viewer)
529  {
530  delete viewer;
531  }
532  cv::destroyWindow("Video");
533  cv::destroyWindow("Depth");
534  delete camera;
535  return 0;
536 }
uStr2Int
int UTILITE_EXP uStr2Int(const std::string &str)
Definition: UConversion.cpp:125
running
bool running
Definition: tools/CameraRGBD/main.cpp:79
sighandler
void sighandler(int sig)
Definition: tools/CameraRGBD/main.cpp:80
UDirectory::makeDir
static bool makeDir(const std::string &dirPath)
Definition: UDirectory.cpp:333
glm::min
GLM_FUNC_DECL genType min(genType const &x, genType const &y)
rtabmap::util3d::transformPointCloud
pcl::PointCloud< pcl::PointXYZ >::Ptr RTABMAP_EXP transformPointCloud(const pcl::PointCloud< pcl::PointXYZ >::Ptr &cloud, const Transform &transform)
Definition: util3d_transforms.cpp:107
uMinMax
void uMinMax(const T *v, unsigned int size, T &min, T &max, unsigned int &indexMin, unsigned int &indexMax)
Definition: UMath.h:226
f
f
rtabmap::SensorData::depthOrRightRaw
const cv::Mat & depthOrRightRaw() const
Definition: SensorData.h:184
data
data
uStr2Float
float UTILITE_EXP uStr2Float(const std::string &str)
Definition: UConversion.cpp:138
rtabmap::SensorData::stereoCameraModels
const std::vector< StereoCameraModel > & stereoCameraModels() const
Definition: SensorData.h:237
UMath.h
Basic mathematics functions.
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
rtabmap::Transform::opengl_T_rtabmap
static Transform opengl_T_rtabmap()
Definition: Transform.h:149
UFATAL
#define UFATAL(...)
rtabmap::util3d::cloudFromDepth
pcl::PointCloud< pcl::PointXYZ >::Ptr RTABMAP_EXP cloudFromDepth(const cv::Mat &imageDepth, const CameraModel &model, int decimation=1, float maxDepth=0.0f, float minDepth=0.0f, std::vector< int > *validIndices=0)
Definition: util3d.cpp:278
rtabmap::SensorData::rightRaw
cv::Mat rightRaw() const
Definition: SensorData.h:211
rtabmap::util3d::cloudFromDepthRGB
pcl::PointCloud< pcl::PointXYZRGB >::Ptr RTABMAP_EXP cloudFromDepthRGB(const cv::Mat &imageRgb, const cv::Mat &imageDepth, const CameraModel &model, int decimation=1, float maxDepth=0.0f, float minDepth=0.0f, std::vector< int > *validIndices=0)
Definition: util3d.cpp:429
UASSERT
#define UASSERT(condition)
rtabmap::SensorData::cameraModels
const std::vector< CameraModel > & cameraModels() const
Definition: SensorData.h:236
rtabmap::SensorData::depthRaw
cv::Mat depthRaw() const
Definition: SensorData.h:210
rtabmap::SensorData::imageRaw
const cv::Mat & imageRaw() const
Definition: SensorData.h:183
main
int main(int argc, char *argv[])
Definition: tools/CameraRGBD/main.cpp:86
ULogger::setType
static void setType(Type type, const std::string &fileName=kDefaultLogFileName, bool append=true)
Definition: ULogger.cpp:176
util3d_transforms.h
glm::max
GLM_FUNC_DECL genType max(genType const &x, genType const &y)
showUsage
void showUsage()
Definition: tools/CameraRGBD/main.cpp:48
UERROR
#define UERROR(...)
rtabmap_oanet.argv
argv
Definition: rtabmap_oanet.py:12
ULogger.h
ULogger class and convenient macros.
UWARN
#define UWARN(...)
rtabmap::CameraK4A::available
static bool available()
Definition: CameraK4A.cpp:42
UDirectory::exists
static bool exists(const std::string &dirPath)
Definition: UDirectory.cpp:249
rtabmap::util2d::cvtDepthFromFloat
cv::Mat RTABMAP_EXP cvtDepthFromFloat(const cv::Mat &depth32F)
Definition: util2d.cpp:892
rtabmap::util3d::cloudFromStereoImages
pcl::PointCloud< pcl::PointXYZRGB >::Ptr RTABMAP_EXP cloudFromStereoImages(const cv::Mat &imageLeft, const cv::Mat &imageRight, const StereoCameraModel &model, int decimation=1, float maxDepth=0.0f, float minDepth=0.0f, std::vector< int > *validIndices=0, const ParametersMap &parameters=ParametersMap())
Definition: util3d.cpp:813
rtabmap::CameraK4W2::available
static bool available()
Definition: CameraK4W2.cpp:53
uNumber2Str
std::string UTILITE_EXP uNumber2Str(unsigned int number)
Definition: UConversion.cpp:91
UINFO
#define UINFO(...)

rtabmap
Author(s): Mathieu Labbe
autogenerated on Mon Jan 23 2023 03:37:29