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

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