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

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