CameraStereo.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/CameraStereo.h"
29 #include "rtabmap/core/util2d.h"
30 #include "rtabmap/core/CameraRGB.h"
31 #include "rtabmap/core/Version.h"
32 
33 #include <rtabmap/utilite/UEventsManager.h>
34 #include <rtabmap/utilite/UConversion.h>
35 #include <rtabmap/utilite/UStl.h>
36 #include <rtabmap/utilite/UConversion.h>
37 #include <rtabmap/utilite/UFile.h>
38 #include <rtabmap/utilite/UDirectory.h>
39 #include <rtabmap/utilite/UTimer.h>
40 #include <rtabmap/utilite/UMath.h>
41 
42 #include <opencv2/imgproc/types_c.h>
43 #if CV_MAJOR_VERSION >= 3
44 #include <opencv2/videoio/videoio_c.h>
45 #endif
46 
47 #ifdef RTABMAP_DC1394
48 #include <dc1394/dc1394.h>
49 #endif
50 
51 #ifdef RTABMAP_FLYCAPTURE2
52 #include <triclops.h>
53 #include <fc2triclops.h>
54 #endif
55 
56 #ifdef RTABMAP_ZED
57 #include <sl/Camera.hpp>
58 #endif
59 
60 namespace rtabmap
61 {
62 
63 //
64 // CameraStereoDC1394
65 // Inspired from ROS camera1394stereo package
66 //
67 
68 #ifdef RTABMAP_DC1394
69 class DC1394Device
70 {
71 public:
72  DC1394Device() :
73  camera_(0),
74  context_(0)
75  {
76 
77  }
78  ~DC1394Device()
79  {
80  if (camera_)
81  {
82  if (DC1394_SUCCESS != dc1394_video_set_transmission(camera_, DC1394_OFF) ||
83  DC1394_SUCCESS != dc1394_capture_stop(camera_))
84  {
85  UWARN("unable to stop camera");
86  }
87 
88  // Free resources
89  dc1394_capture_stop(camera_);
90  dc1394_camera_free(camera_);
91  camera_ = NULL;
92  }
93  if(context_)
94  {
95  dc1394_free(context_);
96  context_ = NULL;
97  }
98  }
99 
100  const std::string & guid() const {return guid_;}
101 
102  bool init()
103  {
104  if(camera_)
105  {
106  // Free resources
107  dc1394_capture_stop(camera_);
108  dc1394_camera_free(camera_);
109  camera_ = NULL;
110  }
111 
112  // look for a camera
113  int err;
114  if(context_ == NULL)
115  {
116  context_ = dc1394_new ();
117  if (context_ == NULL)
118  {
119  UERROR( "Could not initialize dc1394_context.\n"
120  "Make sure /dev/raw1394 exists, you have access permission,\n"
121  "and libraw1394 development package is installed.");
122  return false;
123  }
124  }
125 
126  dc1394camera_list_t *list;
127  err = dc1394_camera_enumerate(context_, &list);
128  if (err != DC1394_SUCCESS)
129  {
130  UERROR("Could not get camera list");
131  return false;
132  }
133 
134  if (list->num == 0)
135  {
136  UERROR("No cameras found");
137  dc1394_camera_free_list (list);
138  return false;
139  }
140  uint64_t guid = list->ids[0].guid;
141  dc1394_camera_free_list (list);
142 
143  // Create a camera
144  camera_ = dc1394_camera_new (context_, guid);
145  if (!camera_)
146  {
147  UERROR("Failed to initialize camera with GUID [%016lx]", guid);
148  return false;
149  }
150 
151  uint32_t value[3];
152  value[0]= camera_->guid & 0xffffffff;
153  value[1]= (camera_->guid >>32) & 0x000000ff;
154  value[2]= (camera_->guid >>40) & 0xfffff;
155  guid_ = uFormat("%06x%02x%08x", value[2], value[1], value[0]);
156 
157  UINFO("camera model: %s %s", camera_->vendor, camera_->model);
158 
159  // initialize camera
160  // Enable IEEE1394b mode if the camera and bus support it
161  bool bmode = camera_->bmode_capable;
162  if (bmode
163  && (DC1394_SUCCESS !=
164  dc1394_video_set_operation_mode(camera_,
165  DC1394_OPERATION_MODE_1394B)))
166  {
167  bmode = false;
168  UWARN("failed to set IEEE1394b mode");
169  }
170 
171  // start with highest speed supported
172  dc1394speed_t request = DC1394_ISO_SPEED_3200;
173  int rate = 3200;
174  if (!bmode)
175  {
176  // not IEEE1394b capable: so 400Mb/s is the limit
177  request = DC1394_ISO_SPEED_400;
178  rate = 400;
179  }
180 
181  // round requested speed down to next-lower defined value
182  while (rate > 400)
183  {
184  if (request <= DC1394_ISO_SPEED_MIN)
185  {
186  // get current ISO speed of the device
187  dc1394speed_t curSpeed;
188  if (DC1394_SUCCESS == dc1394_video_get_iso_speed(camera_, &curSpeed) && curSpeed <= DC1394_ISO_SPEED_MAX)
189  {
190  // Translate curSpeed back to an int for the parameter
191  // update, works as long as any new higher speeds keep
192  // doubling.
193  request = curSpeed;
194  rate = 100 << (curSpeed - DC1394_ISO_SPEED_MIN);
195  }
196  else
197  {
198  UWARN("Unable to get ISO speed; assuming 400Mb/s");
199  rate = 400;
200  request = DC1394_ISO_SPEED_400;
201  }
202  break;
203  }
204  // continue with next-lower possible value
205  request = (dc1394speed_t) ((int) request - 1);
206  rate = rate / 2;
207  }
208 
209  // set the requested speed
210  if (DC1394_SUCCESS != dc1394_video_set_iso_speed(camera_, request))
211  {
212  UERROR("Failed to set iso speed");
213  return false;
214  }
215 
216  // set video mode
217  dc1394video_modes_t vmodes;
218  err = dc1394_video_get_supported_modes(camera_, &vmodes);
219  if (err != DC1394_SUCCESS)
220  {
221  UERROR("unable to get supported video modes");
222  return (dc1394video_mode_t) 0;
223  }
224 
225  // see if requested mode is available
226  bool found = false;
227  dc1394video_mode_t videoMode = DC1394_VIDEO_MODE_FORMAT7_3; // bumblebee
228  for (uint32_t i = 0; i < vmodes.num; ++i)
229  {
230  if (vmodes.modes[i] == videoMode)
231  {
232  found = true;
233  }
234  }
235  if(!found)
236  {
237  UERROR("unable to get video mode %d", videoMode);
238  return false;
239  }
240 
241  if (DC1394_SUCCESS != dc1394_video_set_mode(camera_, videoMode))
242  {
243  UERROR("Failed to set video mode %d", videoMode);
244  return false;
245  }
246 
247  // special handling for Format7 modes
248  if (dc1394_is_video_mode_scalable(videoMode) == DC1394_TRUE)
249  {
250  if (DC1394_SUCCESS != dc1394_format7_set_color_coding(camera_, videoMode, DC1394_COLOR_CODING_RAW16))
251  {
252  UERROR("Could not set color coding");
253  return false;
254  }
255  uint32_t packetSize;
256  if (DC1394_SUCCESS != dc1394_format7_get_recommended_packet_size(camera_, videoMode, &packetSize))
257  {
258  UERROR("Could not get default packet size");
259  return false;
260  }
261 
262  if (DC1394_SUCCESS != dc1394_format7_set_packet_size(camera_, videoMode, packetSize))
263  {
264  UERROR("Could not set packet size");
265  return false;
266  }
267  }
268  else
269  {
270  UERROR("Video is not in mode scalable");
271  }
272 
273  // start the device streaming data
274  // Set camera to use DMA, improves performance.
275  if (DC1394_SUCCESS != dc1394_capture_setup(camera_, 4, DC1394_CAPTURE_FLAGS_DEFAULT))
276  {
277  UERROR("Failed to open device!");
278  return false;
279  }
280 
281  // Start transmitting camera data
282  if (DC1394_SUCCESS != dc1394_video_set_transmission(camera_, DC1394_ON))
283  {
284  UERROR("Failed to start device!");
285  return false;
286  }
287 
288  return true;
289  }
290 
291  bool getImages(cv::Mat & left, cv::Mat & right)
292  {
293  if(camera_)
294  {
295  dc1394video_frame_t * frame = NULL;
296  UDEBUG("[%016lx] waiting camera", camera_->guid);
297  dc1394_capture_dequeue (camera_, DC1394_CAPTURE_POLICY_WAIT, &frame);
298  if (!frame)
299  {
300  UERROR("Unable to capture frame");
301  return false;
302  }
303  dc1394video_frame_t frame1 = *frame;
304  // deinterlace frame into two imagesCount one on top the other
305  size_t frame1_size = frame->total_bytes;
306  frame1.image = (unsigned char *) malloc(frame1_size);
307  frame1.allocated_image_bytes = frame1_size;
308  frame1.color_coding = DC1394_COLOR_CODING_RAW8;
309  int err = dc1394_deinterlace_stereo_frames(frame, &frame1, DC1394_STEREO_METHOD_INTERLACED);
310  if (err != DC1394_SUCCESS)
311  {
312  free(frame1.image);
313  dc1394_capture_enqueue(camera_, frame);
314  UERROR("Could not extract stereo frames");
315  return false;
316  }
317 
318  uint8_t* capture_buffer = reinterpret_cast<uint8_t *>(frame1.image);
319  UASSERT(capture_buffer);
320 
321  cv::Mat image(frame->size[1], frame->size[0], CV_8UC3);
322  cv::Mat image2 = image.clone();
323 
324  //DC1394_COLOR_CODING_RAW16:
325  //DC1394_COLOR_FILTER_BGGR
326  cv::cvtColor(cv::Mat(frame->size[1], frame->size[0], CV_8UC1, capture_buffer), left, CV_BayerRG2BGR);
327  cv::cvtColor(cv::Mat(frame->size[1], frame->size[0], CV_8UC1, capture_buffer+image.total()), right, CV_BayerRG2GRAY);
328 
329  dc1394_capture_enqueue(camera_, frame);
330 
331  free(frame1.image);
332 
333  return true;
334  }
335  return false;
336  }
337 
338 private:
339  dc1394camera_t *camera_;
340  dc1394_t *context_;
341  std::string guid_;
342 };
343 #endif
344 
345 bool CameraStereoDC1394::available()
346 {
347 #ifdef RTABMAP_DC1394
348  return true;
349 #else
350  return false;
351 #endif
352 }
353 
354 CameraStereoDC1394::CameraStereoDC1394(float imageRate, const Transform & localTransform) :
355  Camera(imageRate, localTransform)
356 #ifdef RTABMAP_DC1394
357  ,
358  device_(0)
359 #endif
360 {
361 #ifdef RTABMAP_DC1394
362  device_ = new DC1394Device();
363 #endif
364 }
365 
366 CameraStereoDC1394::~CameraStereoDC1394()
367 {
368 #ifdef RTABMAP_DC1394
369  delete device_;
370 #endif
371 }
372 
373 bool CameraStereoDC1394::init(const std::string & calibrationFolder, const std::string & cameraName)
374 {
375 #ifdef RTABMAP_DC1394
376  if(device_)
377  {
378  bool ok = device_->init();
379  if(ok)
380  {
381  // look for calibration files
382  if(!calibrationFolder.empty())
383  {
384  if(!stereoModel_.load(calibrationFolder, cameraName.empty()?device_->guid():cameraName, false))
385  {
386  UWARN("Missing calibration files for camera \"%s\" in \"%s\" folder, you should calibrate the camera!",
387  cameraName.empty()?device_->guid().c_str():cameraName.c_str(), calibrationFolder.c_str());
388  }
389  else
390  {
391  UINFO("Stereo parameters: fx=%f cx=%f cy=%f baseline=%f",
392  stereoModel_.left().fx(),
393  stereoModel_.left().cx(),
394  stereoModel_.left().cy(),
395  stereoModel_.baseline());
396  }
397  }
398  }
399  return ok;
400  }
401 #else
402  UERROR("CameraDC1394: RTAB-Map is not built with dc1394 support!");
403 #endif
404  return false;
405 }
406 
407 bool CameraStereoDC1394::isCalibrated() const
408 {
409 #ifdef RTABMAP_DC1394
410  return stereoModel_.isValidForProjection();
411 #else
412  return false;
413 #endif
414 }
415 
416 std::string CameraStereoDC1394::getSerial() const
417 {
418 #ifdef RTABMAP_DC1394
419  if(device_)
420  {
421  return device_->guid();
422  }
423 #endif
424  return "";
425 }
426 
427 SensorData CameraStereoDC1394::captureImage(CameraInfo * info)
428 {
429  SensorData data;
430 #ifdef RTABMAP_DC1394
431  if(device_)
432  {
433  cv::Mat left, right;
434  device_->getImages(left, right);
435 
436  if(!left.empty() && !right.empty())
437  {
438  // Rectification
439  if(stereoModel_.left().isValidForRectification())
440  {
441  left = stereoModel_.left().rectifyImage(left);
442  }
443  if(stereoModel_.right().isValidForRectification())
444  {
445  right = stereoModel_.right().rectifyImage(right);
446  }
447  StereoCameraModel model;
448  if(stereoModel_.isValidForProjection())
449  {
450  model = StereoCameraModel(
451  stereoModel_.left().fx(), //fx
452  stereoModel_.left().fy(), //fy
453  stereoModel_.left().cx(), //cx
454  stereoModel_.left().cy(), //cy
455  stereoModel_.baseline(),
456  this->getLocalTransform(),
457  left.size());
458  }
459  data = SensorData(left, right, model, this->getNextSeqID(), UTimer::now());
460  }
461  }
462 #else
463  UERROR("CameraDC1394: RTAB-Map is not built with dc1394 support!");
464 #endif
465  return data;
466 }
467 
468 //
469 // CameraTriclops
470 //
471 CameraStereoFlyCapture2::CameraStereoFlyCapture2(float imageRate, const Transform & localTransform) :
472  Camera(imageRate, localTransform)
473 #ifdef RTABMAP_FLYCAPTURE2
474  ,
475  camera_(0),
476  triclopsCtx_(0)
477 #endif
478 {
479 #ifdef RTABMAP_FLYCAPTURE2
480  camera_ = new FlyCapture2::Camera();
481 #endif
482 }
483 
484 CameraStereoFlyCapture2::~CameraStereoFlyCapture2()
485 {
486 #ifdef RTABMAP_FLYCAPTURE2
487  // Close the camera
488  camera_->StopCapture();
489  camera_->Disconnect();
490 
491  // Destroy the Triclops context
492  triclopsDestroyContext( triclopsCtx_ ) ;
493 
494  delete camera_;
495 #endif
496 }
497 
498 bool CameraStereoFlyCapture2::available()
499 {
500 #ifdef RTABMAP_FLYCAPTURE2
501  return true;
502 #else
503  return false;
504 #endif
505 }
506 
507 bool CameraStereoFlyCapture2::init(const std::string & calibrationFolder, const std::string & cameraName)
508 {
509 #ifdef RTABMAP_FLYCAPTURE2
510  if(camera_)
511  {
512  // Close the camera
513  camera_->StopCapture();
514  camera_->Disconnect();
515  }
516  if(triclopsCtx_)
517  {
518  triclopsDestroyContext(triclopsCtx_);
519  triclopsCtx_ = 0;
520  }
521 
522  // connect camera
523  FlyCapture2::Error fc2Error = camera_->Connect();
524  if(fc2Error != FlyCapture2::PGRERROR_OK)
525  {
526  UERROR("Failed to connect the camera.");
527  return false;
528  }
529 
530  // configure camera
531  Fc2Triclops::StereoCameraMode mode = Fc2Triclops::TWO_CAMERA_NARROW;
532  if(Fc2Triclops::setStereoMode(*camera_, mode ))
533  {
534  UERROR("Failed to set stereo mode.");
535  return false;
536  }
537 
538  // generate the Triclops context
539  FlyCapture2::CameraInfo camInfo;
540  if(camera_->GetCameraInfo(&camInfo) != FlyCapture2::PGRERROR_OK)
541  {
542  UERROR("Failed to get camera info.");
543  return false;
544  }
545 
546  float dummy;
547  unsigned packetSz;
548  FlyCapture2::Format7ImageSettings imageSettings;
549  int maxWidth = 640;
550  int maxHeight = 480;
551  if(camera_->GetFormat7Configuration(&imageSettings, &packetSz, &dummy) == FlyCapture2::PGRERROR_OK)
552  {
553  maxHeight = imageSettings.height;
554  maxWidth = imageSettings.width;
555  }
556 
557  // Get calibration from th camera
558  if(Fc2Triclops::getContextFromCamera(camInfo.serialNumber, &triclopsCtx_))
559  {
560  UERROR("Failed to get calibration from the camera.");
561  return false;
562  }
563 
564  float fx, cx, cy, baseline;
565  triclopsGetFocalLength(triclopsCtx_, &fx);
566  triclopsGetImageCenter(triclopsCtx_, &cy, &cx);
567  triclopsGetBaseline(triclopsCtx_, &baseline);
568  UINFO("Stereo parameters: fx=%f cx=%f cy=%f baseline=%f", fx, cx, cy, baseline);
569 
570  triclopsSetCameraConfiguration(triclopsCtx_, TriCfg_2CAM_HORIZONTAL_NARROW );
571  UASSERT(triclopsSetResolutionAndPrepare(triclopsCtx_, maxHeight, maxWidth, maxHeight, maxWidth) == Fc2Triclops::ERRORTYPE_OK);
572 
573  if(camera_->StartCapture() != FlyCapture2::PGRERROR_OK)
574  {
575  UERROR("Failed to start capture.");
576  return false;
577  }
578 
579  return true;
580 #else
581  UERROR("CameraStereoFlyCapture2: RTAB-Map is not built with Triclops support!");
582 #endif
583  return false;
584 }
585 
586 bool CameraStereoFlyCapture2::isCalibrated() const
587 {
588 #ifdef RTABMAP_FLYCAPTURE2
589  if(triclopsCtx_)
590  {
591  float fx, cx, cy, baseline;
592  triclopsGetFocalLength(triclopsCtx_, &fx);
593  triclopsGetImageCenter(triclopsCtx_, &cy, &cx);
594  triclopsGetBaseline(triclopsCtx_, &baseline);
595  return fx > 0.0f && cx > 0.0f && cy > 0.0f && baseline > 0.0f;
596  }
597 #endif
598  return false;
599 }
600 
601 std::string CameraStereoFlyCapture2::getSerial() const
602 {
603 #ifdef RTABMAP_FLYCAPTURE2
604  if(camera_ && camera_->IsConnected())
605  {
606  FlyCapture2::CameraInfo camInfo;
607  if(camera_->GetCameraInfo(&camInfo) == FlyCapture2::PGRERROR_OK)
608  {
609  return uNumber2Str(camInfo.serialNumber);
610  }
611  }
612 #endif
613  return "";
614 }
615 
616 // struct containing image needed for processing
617 #ifdef RTABMAP_FLYCAPTURE2
618 struct ImageContainer
619 {
620  FlyCapture2::Image tmp[2];
621  FlyCapture2::Image unprocessed[2];
622 } ;
623 #endif
624 
625 SensorData CameraStereoFlyCapture2::captureImage(CameraInfo * info)
626 {
627  SensorData data;
628 #ifdef RTABMAP_FLYCAPTURE2
629  if(camera_ && triclopsCtx_ && camera_->IsConnected())
630  {
631  // grab image from camera.
632  // this image contains both right and left imagesCount
633  FlyCapture2::Image grabbedImage;
634  if(camera_->RetrieveBuffer(&grabbedImage) == FlyCapture2::PGRERROR_OK)
635  {
636  // right and left image extracted from grabbed image
637  ImageContainer imageCont;
638 
639  // generate triclops input from grabbed image
640  FlyCapture2::Image imageRawRight;
641  FlyCapture2::Image imageRawLeft;
642  FlyCapture2::Image * unprocessedImage = imageCont.unprocessed;
643 
644  // Convert the pixel interleaved raw data to de-interleaved and color processed data
645  if(Fc2Triclops::unpackUnprocessedRawOrMono16Image(
646  grabbedImage,
647  true /*assume little endian*/,
648  imageRawLeft /* right */,
649  imageRawRight /* left */) == Fc2Triclops::ERRORTYPE_OK)
650  {
651  // convert to color
652  FlyCapture2::Image srcImgRightRef(imageRawRight);
653  FlyCapture2::Image srcImgLeftRef(imageRawLeft);
654 
655  bool ok = true;;
656  if ( srcImgRightRef.SetColorProcessing(FlyCapture2::HQ_LINEAR) != FlyCapture2::PGRERROR_OK ||
657  srcImgLeftRef.SetColorProcessing(FlyCapture2::HQ_LINEAR) != FlyCapture2::PGRERROR_OK)
658  {
659  ok = false;
660  }
661 
662  if(ok)
663  {
664  FlyCapture2::Image imageColorRight;
665  FlyCapture2::Image imageColorLeft;
666  if ( srcImgRightRef.Convert(FlyCapture2::PIXEL_FORMAT_MONO8, &imageColorRight) != FlyCapture2::PGRERROR_OK ||
667  srcImgLeftRef.Convert(FlyCapture2::PIXEL_FORMAT_BGRU, &imageColorLeft) != FlyCapture2::PGRERROR_OK)
668  {
669  ok = false;
670  }
671 
672  if(ok)
673  {
674  //RECTIFY RIGHT
675  TriclopsInput triclopsColorInputs;
676  triclopsBuildRGBTriclopsInput(
677  grabbedImage.GetCols(),
678  grabbedImage.GetRows(),
679  imageColorRight.GetStride(),
680  (unsigned long)grabbedImage.GetTimeStamp().seconds,
681  (unsigned long)grabbedImage.GetTimeStamp().microSeconds,
682  imageColorRight.GetData(),
683  imageColorRight.GetData(),
684  imageColorRight.GetData(),
685  &triclopsColorInputs);
686 
687  triclopsRectify(triclopsCtx_, const_cast<TriclopsInput *>(&triclopsColorInputs) );
688  // Retrieve the rectified image from the triclops context
689  TriclopsImage rectifiedImage;
690  triclopsGetImage( triclopsCtx_,
691  TriImg_RECTIFIED,
692  TriCam_REFERENCE,
693  &rectifiedImage );
694 
695  cv::Mat left,right;
696  right = cv::Mat(rectifiedImage.nrows, rectifiedImage.ncols, CV_8UC1, rectifiedImage.data).clone();
697 
698  //RECTIFY LEFT COLOR
699  triclopsBuildPackedTriclopsInput(
700  grabbedImage.GetCols(),
701  grabbedImage.GetRows(),
702  imageColorLeft.GetStride(),
703  (unsigned long)grabbedImage.GetTimeStamp().seconds,
704  (unsigned long)grabbedImage.GetTimeStamp().microSeconds,
705  imageColorLeft.GetData(),
706  &triclopsColorInputs );
707 
708  cv::Mat pixelsLeftBuffer( grabbedImage.GetRows(), grabbedImage.GetCols(), CV_8UC4);
709  TriclopsPackedColorImage colorImage;
710  triclopsSetPackedColorImageBuffer(
711  triclopsCtx_,
712  TriCam_LEFT,
713  (TriclopsPackedColorPixel*)pixelsLeftBuffer.data );
714 
715  triclopsRectifyPackedColorImage(
716  triclopsCtx_,
717  TriCam_LEFT,
718  &triclopsColorInputs,
719  &colorImage );
720 
721  cv::cvtColor(pixelsLeftBuffer, left, CV_RGBA2RGB);
722 
723  // Set calibration stuff
724  float fx, cy, cx, baseline;
725  triclopsGetFocalLength(triclopsCtx_, &fx);
726  triclopsGetImageCenter(triclopsCtx_, &cy, &cx);
727  triclopsGetBaseline(triclopsCtx_, &baseline);
728 
729  StereoCameraModel model(
730  fx,
731  fx,
732  cx,
733  cy,
734  baseline,
735  this->getLocalTransform(),
736  left.size());
737  data = SensorData(left, right, model, this->getNextSeqID(), UTimer::now());
738  }
739  }
740  }
741  }
742  }
743 
744 #else
745  UERROR("CameraStereoFlyCapture2: RTAB-Map is not built with Triclops support!");
746 #endif
747  return data;
748 }
749 
750 //
751 // CameraStereoZED
752 //
753 bool CameraStereoZed::available()
754 {
755 #ifdef RTABMAP_ZED
756  return true;
757 #else
758  return false;
759 #endif
760 }
761 
762 CameraStereoZed::CameraStereoZed(
763  int deviceId,
764  int resolution,
765  int quality,
766  int sensingMode,
767  int confidenceThr,
768  bool computeOdometry,
769  float imageRate,
770  const Transform & localTransform,
771  bool selfCalibration) :
772  Camera(imageRate, localTransform)
773 #ifdef RTABMAP_ZED
774  ,
775  zed_(0),
776  src_(CameraVideo::kUsbDevice),
777  usbDevice_(deviceId),
778  svoFilePath_(""),
779  resolution_(resolution),
780  quality_(quality),
781  selfCalibration_(selfCalibration),
782  sensingMode_(sensingMode),
783  confidenceThr_(confidenceThr),
784  computeOdometry_(computeOdometry),
785  lost_(true)
786 #endif
787 {
788  UDEBUG("");
789 #ifdef RTABMAP_ZED
790  UASSERT(resolution_ >= sl::RESOLUTION_HD2K && resolution_ <sl::RESOLUTION_LAST);
791  UASSERT(quality_ >= sl::DEPTH_MODE_NONE && quality_ <sl::DEPTH_MODE_LAST);
792  UASSERT(sensingMode_ >= sl::SENSING_MODE_STANDARD && sensingMode_ <sl::SENSING_MODE_LAST);
793  UASSERT(confidenceThr_ >= 0 && confidenceThr_ <=100);
794 #endif
795 }
796 
797 CameraStereoZed::CameraStereoZed(
798  const std::string & filePath,
799  int quality,
800  int sensingMode,
801  int confidenceThr,
802  bool computeOdometry,
803  float imageRate,
804  const Transform & localTransform,
805  bool selfCalibration) :
806  Camera(imageRate, localTransform)
807 #ifdef RTABMAP_ZED
808  ,
809  zed_(0),
810  src_(CameraVideo::kVideoFile),
811  usbDevice_(0),
812  svoFilePath_(filePath),
813  resolution_(2),
814  quality_(quality),
815  selfCalibration_(selfCalibration),
816  sensingMode_(sensingMode),
817  confidenceThr_(confidenceThr),
818  computeOdometry_(computeOdometry),
819  lost_(true)
820 #endif
821 {
822  UDEBUG("");
823 #ifdef RTABMAP_ZED
824  UASSERT(resolution_ >= sl::RESOLUTION_HD2K && resolution_ <sl::RESOLUTION_LAST);
825  UASSERT(quality_ >= sl::DEPTH_MODE_NONE && quality_ <sl::DEPTH_MODE_LAST);
826  UASSERT(sensingMode_ >= sl::SENSING_MODE_STANDARD && sensingMode_ <sl::SENSING_MODE_LAST);
827  UASSERT(confidenceThr_ >= 0 && confidenceThr_ <=100);
828 #endif
829 }
830 
831 CameraStereoZed::~CameraStereoZed()
832 {
833 #ifdef RTABMAP_ZED
834  delete zed_;
835 #endif
836 }
837 
838 bool CameraStereoZed::init(const std::string & calibrationFolder, const std::string & cameraName)
839 {
840  UDEBUG("");
841 #ifdef RTABMAP_ZED
842  if(zed_)
843  {
844  delete zed_;
845  zed_ = 0;
846  }
847 
848  lost_ = true;
849 
850  sl::InitParameters param;
851  param.camera_resolution=static_cast<sl::RESOLUTION>(resolution_);
852  param.camera_fps=getImageRate();
853  param.camera_linux_id=usbDevice_;
854  param.depth_mode=(sl::DEPTH_MODE)quality_;
855  param.coordinate_units=sl::UNIT_METER;
856  param.coordinate_system=(sl::COORDINATE_SYSTEM)sl::COORDINATE_SYSTEM_IMAGE ;
857  param.sdk_verbose=true;
858  param.sdk_gpu_id=-1;
859  param.depth_minimum_distance=-1;
860  param.camera_disable_self_calib=!selfCalibration_;
861 
862  sl::ERROR_CODE r = sl::ERROR_CODE::SUCCESS;
863  if(src_ == CameraVideo::kVideoFile)
864  {
865  UINFO("svo file = %s", svoFilePath_.c_str());
866  zed_ = new sl::Camera(); // Use in SVO playback mode
867  param.svo_input_filename=svoFilePath_.c_str();
868  r = zed_->open(param);
869  }
870  else
871  {
872  UINFO("Resolution=%d imagerate=%f device=%d", resolution_, getImageRate(), usbDevice_);
873  zed_ = new sl::Camera(); // Use in Live Mode
874  r = zed_->open(param);
875  }
876 
877  if(r!=sl::ERROR_CODE::SUCCESS)
878  {
879  UERROR("Camera initialization failed: \"%s\"", toString(r).c_str());
880  delete zed_;
881  zed_ = 0;
882  return false;
883  }
884 
885 
886  UINFO("Init ZED: Mode=%d Unit=%d CoordinateSystem=%d Verbose=false device=-1 minDist=-1 self-calibration=%s vflip=false",
887  quality_, sl::UNIT_METER, sl::COORDINATE_SYSTEM_IMAGE , selfCalibration_?"true":"false");
888  UDEBUG("");
889 
890  if(quality_!=sl::DEPTH_MODE_NONE)
891  {
892  zed_->setConfidenceThreshold(confidenceThr_);
893  }
894 
895  if (computeOdometry_)
896  {
897  sl::TrackingParameters tparam;
898  tparam.enable_spatial_memory=false;
899  zed_->enableTracking(tparam);
900  if(r!=sl::ERROR_CODE::SUCCESS)
901  {
902  UERROR("Camera tracking initialization failed: \"%s\"", toString(r).c_str());
903  }
904  }
905 
906  sl::CameraInformation infos = zed_->getCameraInformation();
907  sl::CalibrationParameters *stereoParams = &(infos.calibration_parameters );
908  sl::Resolution res = stereoParams->left_cam.image_size;
909 
910  stereoModel_ = StereoCameraModel(
911  stereoParams->left_cam.fx,
912  stereoParams->left_cam.fy,
913  stereoParams->left_cam.cx,
914  stereoParams->left_cam.cy,
915  stereoParams->T[0],//baseline
916  this->getLocalTransform(),
917  cv::Size(res.width, res.height));
918 
919  UINFO("Calibration: fx=%f, fy=%f, cx=%f, cy=%f, baseline=%f, width=%d, height=%d, transform=%s",
920  stereoParams->left_cam.fx,
921  stereoParams->left_cam.fy,
922  stereoParams->left_cam.cx,
923  stereoParams->left_cam.cy,
924  stereoParams->T[0],//baseline
925  (int)res.width,
926  (int)res.height,
927  this->getLocalTransform().prettyPrint().c_str());
928 
929  return true;
930 #else
931  UERROR("CameraStereoZED: RTAB-Map is not built with ZED sdk support!");
932 #endif
933  return false;
934 }
935 
936 bool CameraStereoZed::isCalibrated() const
937 {
938 #ifdef RTABMAP_ZED
939  return stereoModel_.isValidForProjection();
940 #else
941  return false;
942 #endif
943 }
944 
945 std::string CameraStereoZed::getSerial() const
946 {
947 #ifdef RTABMAP_ZED
948  if(zed_)
949  {
950  return uFormat("%x", zed_->getCameraInformation ().serial_number);
951  }
952 #endif
953  return "";
954 }
955 
956 bool CameraStereoZed::odomProvided() const
957 {
958 #ifdef RTABMAP_ZED
959  return computeOdometry_;
960 #else
961  return false;
962 #endif
963 }
964 #ifdef RTABMAP_ZED
965 static cv::Mat slMat2cvMat(sl::Mat& input) {
966  //convert MAT_TYPE to CV_TYPE
967  int cv_type = -1;
968  switch (input.getDataType()) {
969  case sl::MAT_TYPE_32F_C1: cv_type = CV_32FC1; break;
970  case sl::MAT_TYPE_32F_C2: cv_type = CV_32FC2; break;
971  case sl::MAT_TYPE_32F_C3: cv_type = CV_32FC3; break;
972  case sl::MAT_TYPE_32F_C4: cv_type = CV_32FC4; break;
973  case sl::MAT_TYPE_8U_C1: cv_type = CV_8UC1; break;
974  case sl::MAT_TYPE_8U_C2: cv_type = CV_8UC2; break;
975  case sl::MAT_TYPE_8U_C3: cv_type = CV_8UC3; break;
976  case sl::MAT_TYPE_8U_C4: cv_type = CV_8UC4; break;
977  default: break;
978  }
979  // cv::Mat data requires a uchar* pointer. Therefore, we get the uchar1 pointer from sl::Mat (getPtr<T>())
980  //cv::Mat and sl::Mat will share the same memory pointer
981  return cv::Mat(input.getHeight(), input.getWidth(), cv_type, input.getPtr<sl::uchar1>(sl::MEM_CPU));
982 }
983 
984 Transform zedPoseToTransform(const sl::Pose & pose)
985 {
986  return Transform(
987  pose.pose_data.m[0], pose.pose_data.m[1], pose.pose_data.m[2], pose.pose_data.m[3],
988  pose.pose_data.m[4], pose.pose_data.m[5], pose.pose_data.m[6], pose.pose_data.m[7],
989  pose.pose_data.m[8], pose.pose_data.m[9], pose.pose_data.m[10], pose.pose_data.m[11]);
990 }
991 #endif
992 
993 SensorData CameraStereoZed::captureImage(CameraInfo * info)
994 {
995  SensorData data;
996 #ifdef RTABMAP_ZED
997  sl::RuntimeParameters rparam((sl::SENSING_MODE)sensingMode_, quality_ > 0, quality_ > 0, sl::REFERENCE_FRAME_CAMERA);
998  if(zed_)
999  {
1000  UTimer timer;
1001  bool res = zed_->grab(rparam);
1002  while (src_ == CameraVideo::kUsbDevice && res!=sl::SUCCESS && timer.elapsed() < 2.0)
1003  {
1004  // maybe there is a latency with the USB, try again in 10 ms (for the next 2 seconds)
1005  uSleep(10);
1006  res = zed_->grab(rparam);
1007  }
1008  if(res==sl::SUCCESS)
1009  {
1010  // get left image
1011  sl::Mat tmp;
1012  zed_->retrieveImage(tmp,sl::VIEW_LEFT);
1013  cv::Mat rgbaLeft = slMat2cvMat(tmp);
1014 
1015  cv::Mat left;
1016  cv::cvtColor(rgbaLeft, left, cv::COLOR_BGRA2BGR);
1017 
1018  if(quality_ > 0)
1019  {
1020  // get depth image
1021  cv::Mat depth;
1022  sl::Mat tmp;
1023  zed_->retrieveMeasure(tmp,sl::MEASURE_DEPTH);
1024  slMat2cvMat(tmp).copyTo(depth);
1025 
1026  data = SensorData(left, depth, stereoModel_.left(), this->getNextSeqID(), UTimer::now());
1027  }
1028  else
1029  {
1030  // get right image
1031  sl::Mat tmp;zed_->retrieveImage(tmp,sl::VIEW_RIGHT );
1032  cv::Mat rgbaRight = slMat2cvMat(tmp);
1033  cv::Mat right;
1034  cv::cvtColor(rgbaRight, right, cv::COLOR_BGRA2GRAY);
1035 
1036  data = SensorData(left, right, stereoModel_, this->getNextSeqID(), UTimer::now());
1037  }
1038 
1039  if (computeOdometry_ && info)
1040  {
1041  sl::Pose pose;
1042  sl::TRACKING_STATE tracking_state = zed_->getPosition(pose);
1043  if (tracking_state == sl::TRACKING_STATE_OK)
1044  {
1045  int trackingConfidence = pose.pose_confidence;
1046  // FIXME What does pose_confidence == -1 mean?
1047  if (trackingConfidence>0)
1048  {
1049  info->odomPose = zedPoseToTransform(pose);
1050  if (!info->odomPose.isNull())
1051  {
1052  //transform x->forward, y->left, z->up
1053  Transform opticalTransform(0, 0, 1, 0, -1, 0, 0, 0, 0, -1, 0, 0);
1054  info->odomPose = opticalTransform * info->odomPose * opticalTransform.inverse();
1055 
1056  if (lost_)
1057  {
1058  info->odomCovariance = cv::Mat::eye(6, 6, CV_64FC1) * 9999.0f; // don't know transform with previous pose
1059  lost_ = false;
1060  UDEBUG("Init %s (var=%f)", info->odomPose.prettyPrint().c_str(), 9999.0f);
1061  }
1062  else
1063  {
1064  info->odomCovariance = cv::Mat::eye(6, 6, CV_64FC1) * 1.0f / float(trackingConfidence);
1065  UDEBUG("Run %s (var=%f)", info->odomPose.prettyPrint().c_str(), 1.0f / float(trackingConfidence));
1066  }
1067  }
1068  else
1069  {
1070  info->odomCovariance = cv::Mat::eye(6, 6, CV_64FC1) * 9999.0f; // lost
1071  lost_ = true;
1072  UWARN("ZED lost! (trackingConfidence=%d)", trackingConfidence);
1073  }
1074  }
1075  else
1076  {
1077  info->odomCovariance = cv::Mat::eye(6, 6, CV_64FC1) * 9999.0f; // lost
1078  lost_ = true;
1079  UWARN("ZED lost! (trackingConfidence=%d)", trackingConfidence);
1080  }
1081  }
1082  else
1083  {
1084  UWARN("Tracking not ok: state=\"%s\"", toString(tracking_state).c_str());
1085  }
1086  }
1087  }
1088  else if(src_ == CameraVideo::kUsbDevice)
1089  {
1090  UERROR("CameraStereoZed: Failed to grab images after 2 seconds!");
1091  }
1092  else
1093  {
1094  UWARN("CameraStereoZed: end of stream is reached!");
1095  }
1096  }
1097 #else
1098  UERROR("CameraStereoZED: RTAB-Map is not built with ZED sdk support!");
1099 #endif
1100  return data;
1101 }
1102 
1103 
1104 //
1105 // CameraStereoImages
1106 //
1107 bool CameraStereoImages::available()
1108 {
1109  return true;
1110 }
1111 
1112 CameraStereoImages::CameraStereoImages(
1113  const std::string & pathLeftImages,
1114  const std::string & pathRightImages,
1115  bool rectifyImages,
1116  float imageRate,
1117  const Transform & localTransform) :
1118  CameraImages(pathLeftImages, imageRate, localTransform),
1119  camera2_(new CameraImages(pathRightImages))
1120 {
1121  this->setImagesRectified(rectifyImages);
1122 }
1123 
1124 CameraStereoImages::CameraStereoImages(
1125  const std::string & pathLeftRightImages,
1126  bool rectifyImages,
1127  float imageRate,
1128  const Transform & localTransform) :
1129  CameraImages("", imageRate, localTransform),
1130  camera2_(0)
1131 {
1132  std::vector<std::string> paths = uListToVector(uSplit(pathLeftRightImages, uStrContains(pathLeftRightImages, ":")?':':';'));
1133  if(paths.size() >= 1)
1134  {
1135  this->setPath(paths[0]);
1136  this->setImagesRectified(rectifyImages);
1137 
1138  if(paths.size() >= 2)
1139  {
1140  camera2_ = new CameraImages(paths[1]);
1141  }
1142  }
1143  else
1144  {
1145  UERROR("The path is empty!");
1146  }
1147 }
1148 
1149 CameraStereoImages::~CameraStereoImages()
1150 {
1151  UDEBUG("");
1152  delete camera2_;
1153  UDEBUG("");
1154 }
1155 
1156 bool CameraStereoImages::init(const std::string & calibrationFolder, const std::string & cameraName)
1157 {
1158  // look for calibration files
1159  if(!calibrationFolder.empty() && !cameraName.empty())
1160  {
1161  if(!stereoModel_.load(calibrationFolder, cameraName, false) && !stereoModel_.isValidForProjection())
1162  {
1163  UWARN("Missing calibration files for camera \"%s\" in \"%s\" folder, you should calibrate the camera!",
1164  cameraName.c_str(), calibrationFolder.c_str());
1165  }
1166  else
1167  {
1168  UINFO("Stereo parameters: fx=%f cx=%f cy=%f baseline=%f",
1169  stereoModel_.left().fx(),
1170  stereoModel_.left().cx(),
1171  stereoModel_.left().cy(),
1172  stereoModel_.baseline());
1173  }
1174  }
1175 
1176  stereoModel_.setLocalTransform(this->getLocalTransform());
1177  stereoModel_.setName(cameraName);
1178  if(this->isImagesRectified() && !stereoModel_.isValidForRectification())
1179  {
1180  UERROR("Parameter \"rectifyImages\" is set, but no stereo model is loaded or valid.");
1181  return false;
1182  }
1183 
1184  //desactivate before init as we will do it in this class instead for convenience
1185  bool rectify = this->isImagesRectified();
1186  this->setImagesRectified(false);
1187 
1188  bool success = false;
1189  if(CameraImages::init())
1190  {
1191  if(camera2_)
1192  {
1193  camera2_->setBayerMode(this->getBayerMode());
1194  if(camera2_->init())
1195  {
1196  if(this->imagesCount() == camera2_->imagesCount())
1197  {
1198  success = true;
1199  }
1200  else
1201  {
1202  UERROR("Cameras don't have the same number of images (%d vs %d)",
1203  this->imagesCount(), camera2_->imagesCount());
1204  }
1205  }
1206  else
1207  {
1208  UERROR("Cannot initialize the second camera.");
1209  }
1210  }
1211  else
1212  {
1213  success = true;
1214  }
1215  }
1216  this->setImagesRectified(rectify); // reset the flag
1217  return success;
1218 }
1219 
1220 bool CameraStereoImages::isCalibrated() const
1221 {
1222  return stereoModel_.isValidForProjection();
1223 }
1224 
1225 std::string CameraStereoImages::getSerial() const
1226 {
1227  return stereoModel_.name();
1228 }
1229 
1230 SensorData CameraStereoImages::captureImage(CameraInfo * info)
1231 {
1232  SensorData data;
1233 
1234  SensorData left, right;
1235  left = CameraImages::captureImage(info);
1236  if(!left.imageRaw().empty())
1237  {
1238  if(camera2_)
1239  {
1240  right = camera2_->takeImage(info);
1241  }
1242  else
1243  {
1244  right = this->takeImage(info);
1245  }
1246 
1247  if(!right.imageRaw().empty())
1248  {
1249  // Rectification
1250  cv::Mat leftImage = left.imageRaw();
1251  cv::Mat rightImage = right.imageRaw();
1252  if(rightImage.type() != CV_8UC1)
1253  {
1254  cv::Mat tmp;
1255  cv::cvtColor(rightImage, tmp, CV_BGR2GRAY);
1256  rightImage = tmp;
1257  }
1258  if(this->isImagesRectified() && stereoModel_.isValidForRectification())
1259  {
1260  leftImage = stereoModel_.left().rectifyImage(leftImage);
1261  rightImage = stereoModel_.right().rectifyImage(rightImage);
1262  }
1263 
1264  if(stereoModel_.left().imageHeight() == 0 || stereoModel_.left().imageWidth() == 0)
1265  {
1266  stereoModel_.setImageSize(leftImage.size());
1267  }
1268 
1269  data = SensorData(left.laserScanRaw(), leftImage, rightImage, stereoModel_, left.id()/(camera2_?1:2), left.stamp());
1270  data.setGroundTruth(left.groundTruth());
1271  }
1272  }
1273  return data;
1274 }
1275 
1276 //
1277 // CameraStereoVideo
1278 //
1279 bool CameraStereoVideo::available()
1280 {
1281  return true;
1282 }
1283 
1284 CameraStereoVideo::CameraStereoVideo(
1285  const std::string & path,
1286  bool rectifyImages,
1287  float imageRate,
1288  const Transform & localTransform) :
1289  Camera(imageRate, localTransform),
1290  path_(path),
1291  rectifyImages_(rectifyImages),
1292  src_(CameraVideo::kVideoFile),
1293  usbDevice_(0),
1294  usbDevice2_(-1)
1295 {
1296 }
1297 
1298 CameraStereoVideo::CameraStereoVideo(
1299  const std::string & pathLeft,
1300  const std::string & pathRight,
1301  bool rectifyImages,
1302  float imageRate,
1303  const Transform & localTransform) :
1304  Camera(imageRate, localTransform),
1305  path_(pathLeft),
1306  path2_(pathRight),
1307  rectifyImages_(rectifyImages),
1308  src_(CameraVideo::kVideoFile),
1309  usbDevice_(0),
1310  usbDevice2_(-1)
1311 {
1312 }
1313 
1314 CameraStereoVideo::CameraStereoVideo(
1315  int device,
1316  bool rectifyImages,
1317  float imageRate,
1318  const Transform & localTransform) :
1319  Camera(imageRate, localTransform),
1320  rectifyImages_(rectifyImages),
1321  src_(CameraVideo::kUsbDevice),
1322  usbDevice_(device),
1323  usbDevice2_(-1)
1324 {
1325 }
1326 
1327 CameraStereoVideo::CameraStereoVideo(
1328  int deviceLeft,
1329  int deviceRight,
1330  bool rectifyImages,
1331  float imageRate,
1332  const Transform & localTransform) :
1333  Camera(imageRate, localTransform),
1334  rectifyImages_(rectifyImages),
1335  src_(CameraVideo::kUsbDevice),
1336  usbDevice_(deviceLeft),
1337  usbDevice2_(deviceRight)
1338 {
1339 }
1340 
1341 CameraStereoVideo::~CameraStereoVideo()
1342 {
1343  capture_.release();
1344  capture2_.release();
1345 }
1346 
1347 bool CameraStereoVideo::init(const std::string & calibrationFolder, const std::string & cameraName)
1348 {
1349  cameraName_ = cameraName;
1350  if(capture_.isOpened())
1351  {
1352  capture_.release();
1353  }
1354  if(capture2_.isOpened())
1355  {
1356  capture2_.release();
1357  }
1358 
1359  if (src_ == CameraVideo::kUsbDevice)
1360  {
1361  capture_.open(usbDevice_);
1362  if(usbDevice2_ < 0)
1363  {
1364  ULOGGER_DEBUG("CameraStereoVideo: Usb device initialization on device %d", usbDevice_);
1365  }
1366  else
1367  {
1368  ULOGGER_DEBUG("CameraStereoVideo: Usb device initialization on devices %d and %d", usbDevice_, usbDevice2_);
1369  capture2_.open(usbDevice2_);
1370  }
1371  }
1372  else if (src_ == CameraVideo::kVideoFile)
1373  {
1374  capture_.open(path_.c_str());
1375  if(path2_.empty())
1376  {
1377  ULOGGER_DEBUG("CameraStereoVideo: filename=\"%s\"", path_.c_str());
1378  }
1379  else
1380  {
1381  ULOGGER_DEBUG("CameraStereoVideo: filenames=\"%s\" and \"%s\"", path_.c_str(), path2_.c_str());
1382  capture2_.open(path2_.c_str());
1383  }
1384  }
1385  else
1386  {
1387  ULOGGER_ERROR("CameraStereoVideo: Unknown source...");
1388  }
1389 
1390  if(!capture_.isOpened() || ((!path2_.empty() || usbDevice2_>=0) && !capture2_.isOpened()))
1391  {
1392  ULOGGER_ERROR("CameraStereoVideo: Failed to create a capture object!");
1393  capture_.release();
1394  capture2_.release();
1395  return false;
1396  }
1397 
1398  if (cameraName_.empty())
1399  {
1400  unsigned int guid = (unsigned int)capture_.get(CV_CAP_PROP_GUID);
1401  if (guid != 0 && guid != 0xffffffff)
1402  {
1403  cameraName_ = uFormat("%08x", guid);
1404  }
1405  }
1406 
1407  // look for calibration files
1408  if(!calibrationFolder.empty() && !cameraName_.empty())
1409  {
1410  if(!stereoModel_.load(calibrationFolder, cameraName_, false))
1411  {
1412  UWARN("Missing calibration files for camera \"%s\" in \"%s\" folder, you should calibrate the camera!",
1413  cameraName_.c_str(), calibrationFolder.c_str());
1414  }
1415  else
1416  {
1417  UINFO("Stereo parameters: fx=%f cx=%f cy=%f baseline=%f",
1418  stereoModel_.left().fx(),
1419  stereoModel_.left().cx(),
1420  stereoModel_.left().cy(),
1421  stereoModel_.baseline());
1422  }
1423  }
1424 
1425  stereoModel_.setLocalTransform(this->getLocalTransform());
1426  if(rectifyImages_ && !stereoModel_.isValidForRectification())
1427  {
1428  UERROR("Parameter \"rectifyImages\" is set, but no stereo model is loaded or valid.");
1429  return false;
1430  }
1431  return true;
1432 }
1433 
1434 bool CameraStereoVideo::isCalibrated() const
1435 {
1436  return stereoModel_.isValidForProjection();
1437 }
1438 
1439 std::string CameraStereoVideo::getSerial() const
1440 {
1441  return cameraName_;
1442 }
1443 
1444 SensorData CameraStereoVideo::captureImage(CameraInfo * info)
1445 {
1446  SensorData data;
1447 
1448  cv::Mat img;
1449  if(capture_.isOpened() && ((path2_.empty() && usbDevice2_ < 0) || capture2_.isOpened()))
1450  {
1451  cv::Mat leftImage;
1452  cv::Mat rightImage;
1453  if(path2_.empty() && usbDevice2_ < 0)
1454  {
1455  if(!capture_.read(img))
1456  {
1457  return data;
1458  }
1459  // Side by side stream
1460  leftImage = cv::Mat(img, cv::Rect( 0, 0, img.size().width/2, img.size().height ));
1461  rightImage = cv::Mat(img, cv::Rect( img.size().width/2, 0, img.size().width/2, img.size().height ));
1462  }
1463  else if(!capture_.read(leftImage) || !capture2_.read(rightImage))
1464  {
1465  return data;
1466  }
1467  else if(leftImage.cols != rightImage.cols || leftImage.rows != rightImage.rows)
1468  {
1469  UERROR("Left and right streams don't have image of the same size: left=%dx%d right=%dx%d",
1470  leftImage.cols, leftImage.rows, rightImage.cols, rightImage.rows);
1471  return data;
1472  }
1473 
1474  // Rectification
1475  bool rightCvt = false;
1476  if(rightImage.type() != CV_8UC1)
1477  {
1478  cv::Mat tmp;
1479  cv::cvtColor(rightImage, tmp, CV_BGR2GRAY);
1480  rightImage = tmp;
1481  rightCvt = true;
1482  }
1483 
1484  if(rectifyImages_ && stereoModel_.left().isValidForRectification() && stereoModel_.right().isValidForRectification())
1485  {
1486  leftImage = stereoModel_.left().rectifyImage(leftImage);
1487  rightImage = stereoModel_.right().rectifyImage(rightImage);
1488  }
1489  else
1490  {
1491  leftImage = leftImage.clone();
1492  if(!rightCvt)
1493  {
1494  rightImage = rightImage.clone();
1495  }
1496  }
1497 
1498  if(stereoModel_.left().imageHeight() == 0 || stereoModel_.left().imageWidth() == 0)
1499  {
1500  stereoModel_.setImageSize(leftImage.size());
1501  }
1502 
1503  data = SensorData(leftImage, rightImage, stereoModel_, this->getNextSeqID(), UTimer::now());
1504  }
1505  else
1506  {
1507  ULOGGER_WARN("The camera must be initialized before requesting an image.");
1508  }
1509 
1510  return data;
1511 }
1512 
1513 
1514 } // namespace rtabmap
rtabmap::CameraImages::getBayerMode
int getBayerMode() const
Definition: CameraRGB.h:67
NULL
#define NULL
rtabmap::CameraModel::imageWidth
int imageWidth() const
Definition: CameraModel.h:113
rtabmap::CameraImages::setPath
void setPath(const std::string &dir)
Definition: CameraRGB.h:70
rtabmap::CameraStereoImages::stereoModel_
StereoCameraModel stereoModel_
Definition: CameraStereo.h:198
rtabmap::CameraInfo::odomCovariance
cv::Mat odomCovariance
Definition: CameraInfo.h:69
UTimer
Definition: UTimer.h:46
rtabmap::CameraStereoVideo::capture_
cv::VideoCapture capture_
Definition: CameraStereo.h:245
rtabmap::Transform::prettyPrint
std::string prettyPrint() const
Definition: Transform.cpp:274
UTimer::elapsed
double elapsed()
Definition: UTimer.h:75
rtabmap::CameraImages::setImagesRectified
void setImagesRectified(bool enabled)
Definition: CameraRGB.h:73
rtabmap::Camera::getLocalTransform
const Transform & getLocalTransform() const
Definition: Camera.h:63
rtabmap::CameraStereoFlyCapture2::captureImage
virtual SensorData captureImage(CameraInfo *info=0)
Definition: CameraStereo.cpp:625
rtabmap::CameraImages::captureImage
virtual SensorData captureImage(CameraInfo *info=0)
Definition: CameraRGB.cpp:496
rtabmap::CameraStereoVideo::captureImage
virtual SensorData captureImage(CameraInfo *info=0)
Definition: CameraStereo.cpp:1444
rtabmap::CameraStereoImages::init
virtual bool init(const std::string &calibrationFolder=".", const std::string &cameraName="")
Definition: CameraStereo.cpp:1156
rtabmap::CameraStereoVideo::CameraStereoVideo
CameraStereoVideo(const std::string &pathSideBySide, bool rectifyImages=false, float imageRate=0.0f, const Transform &localTransform=Transform::getIdentity())
Definition: CameraStereo.cpp:1284
rtabmap::CameraStereoImages::captureImage
virtual SensorData captureImage(CameraInfo *info=0)
Definition: CameraStereo.cpp:1230
UMath.h
Basic mathematics functions.
rtabmap::CameraImages::imagesCount
unsigned int imagesCount() const
Definition: CameraRGB.cpp:478
UConversion.h
Some conversion functions.
rtabmap::SensorData::laserScanRaw
const LaserScan & laserScanRaw() const
Definition: SensorData.h:163
rtabmap::SensorData::groundTruth
const Transform & groundTruth() const
Definition: SensorData.h:232
rtabmap::SensorData::imageRaw
const cv::Mat & imageRaw() const
Definition: SensorData.h:161
uStrContains
bool uStrContains(const std::string &string, const std::string &substring)
Definition: UStl.h:787
rtabmap::CameraStereoVideo::init
virtual bool init(const std::string &calibrationFolder=".", const std::string &cameraName="")
Definition: CameraStereo.cpp:1347
rtabmap::CameraStereoFlyCapture2::isCalibrated
virtual bool isCalibrated() const
Definition: CameraStereo.cpp:586
uSplit
std::list< std::string > uSplit(const std::string &str, char separator= ' ')
Definition: UStl.h:566
glm::uint8_t
detail::uint8 uint8_t
Definition: fwd.hpp:908
UASSERT
#define UASSERT(condition)
rtabmap::Camera::takeImage
SensorData takeImage(CameraInfo *info=0)
Definition: Camera.cpp:67
UStl.h
Wrappers of STL for convenient functions.
rtabmap::CameraStereoVideo::getSerial
virtual std::string getSerial() const
Definition: CameraStereo.cpp:1439
rtabmap::StereoCameraModel::left
const CameraModel & left() const
Definition: StereoCameraModel.h:119
rtabmap::CameraModel::cx
double cx() const
Definition: CameraModel.h:97
true
#define true
Definition: ConvertUTF.c:57
rtabmap::CameraImages::setBayerMode
void setBayerMode(int mode)
Definition: CameraRGB.h:74
rtabmap::CameraStereoVideo::src_
CameraVideo::Source src_
Definition: CameraStereo.h:252
rtabmap::Transform::isNull
bool isNull() const
Definition: Transform.cpp:107
rtabmap::CameraStereoDC1394::captureImage
virtual SensorData captureImage(CameraInfo *info=0)
Definition: CameraStereo.cpp:427
ULOGGER_DEBUG
#define ULOGGER_DEBUG(...)
Definition: ULogger.h:53
rtabmap::CameraStereoImages::CameraStereoImages
CameraStereoImages(const std::string &pathLeftImages, const std::string &pathRightImages, bool rectifyImages=false, float imageRate=0.0f, const Transform &localTransform=Transform::getIdentity())
Definition: CameraStereo.cpp:1112
glm::uint64_t
detail::uint64 uint64_t
Definition: fwd.hpp:920
rtabmap::CameraStereoVideo::isCalibrated
virtual bool isCalibrated() const
Definition: CameraStereo.cpp:1434
rtabmap::CameraModel::isValidForRectification
bool isValidForRectification() const
Definition: CameraModel.h:82
rtabmap::CameraStereoVideo::stereoModel_
StereoCameraModel stereoModel_
Definition: CameraStereo.h:250
rtabmap::SensorData::id
int id() const
Definition: SensorData.h:152
rtabmap::CameraStereoZed::isCalibrated
virtual bool isCalibrated() const
Definition: CameraStereo.cpp:936
rtabmap::SensorData::stamp
double stamp() const
Definition: SensorData.h:154
rtabmap::CameraModel::cy
double cy() const
Definition: CameraModel.h:98
rtabmap::CameraModel::rectifyImage
cv::Mat rectifyImage(const cv::Mat &raw, int interpolation=cv::INTER_LINEAR) const
Definition: CameraModel.cpp:537
rtabmap::CameraStereoFlyCapture2::getSerial
virtual std::string getSerial() const
Definition: CameraStereo.cpp:601
glm::uint32_t
detail::uint32 uint32_t
Definition: fwd.hpp:916
uSleep
void uSleep(unsigned int ms)
Definition: Posix/UThreadC.h:23
uListToVector
std::vector< V > uListToVector(const std::list< V > &list)
Definition: UStl.h:475
rtabmap::Camera::getImageRate
float getImageRate() const
Definition: Camera.h:62
rtabmap::StereoCameraModel::load
bool load(const std::string &directory, const std::string &cameraName, bool ignoreStereoTransform=true)
Definition: StereoCameraModel.cpp:175
rtabmap::Camera::getNextSeqID
int getNextSeqID()
Definition: Camera.h:83
UDEBUG
#define UDEBUG(...)
rtabmap::CameraModel::fx
double fx() const
Definition: CameraModel.h:95
rtabmap::StereoCameraModel::isValidForRectification
bool isValidForRectification() const
Definition: StereoCameraModel.h:86
rtabmap::StereoCameraModel::right
const CameraModel & right() const
Definition: StereoCameraModel.h:120
rtabmap::CameraStereoDC1394::init
virtual bool init(const std::string &calibrationFolder=".", const std::string &cameraName="")
Definition: CameraStereo.cpp:373
UERROR
#define UERROR(...)
rtabmap::CameraStereoImages::getSerial
virtual std::string getSerial() const
Definition: CameraStereo.cpp:1225
ULOGGER_WARN
#define ULOGGER_WARN(...)
Definition: ULogger.h:55
rtabmap::CameraImages::init
virtual bool init(const std::string &calibrationFolder=".", const std::string &cameraName="")
Definition: CameraRGB.cpp:131
UWARN
#define UWARN(...)
rtabmap::StereoCameraModel::setName
void setName(const std::string &name, const std::string &leftSuffix="left", const std::string &rightSuffix="right")
Definition: StereoCameraModel.cpp:166
UTimer::now
static double now()
Definition: UTimer.cpp:73
rtabmap::CameraStereoDC1394::getSerial
virtual std::string getSerial() const
Definition: CameraStereo.cpp:416
rtabmap::CameraStereoImages::isCalibrated
virtual bool isCalibrated() const
Definition: CameraStereo.cpp:1220
rtabmap::CameraStereoDC1394::CameraStereoDC1394
CameraStereoDC1394(float imageRate=0.0f, const Transform &localTransform=Transform::getIdentity())
Definition: CameraStereo.cpp:354
rtabmap::SensorData::setGroundTruth
void setGroundTruth(const Transform &pose)
Definition: SensorData.h:231
rtabmap::Transform::inverse
Transform inverse() const
Definition: Transform.cpp:169
rtabmap::StereoCameraModel::setLocalTransform
void setLocalTransform(const Transform &transform)
Definition: StereoCameraModel.h:115
rtabmap::CameraStereoFlyCapture2::CameraStereoFlyCapture2
CameraStereoFlyCapture2(float imageRate=0.0f, const Transform &localTransform=Transform::getIdentity())
Definition: CameraStereo.cpp:471
rtabmap::StereoCameraModel::setImageSize
void setImageSize(const cv::Size &size)
Definition: StereoCameraModel.h:95
rtabmap::CameraImages::isImagesRectified
bool isImagesRectified() const
Definition: CameraRGB.h:66
rtabmap::CameraStereoDC1394::isCalibrated
virtual bool isCalibrated() const
Definition: CameraStereo.cpp:407
rtabmap::CameraStereoFlyCapture2::init
virtual bool init(const std::string &calibrationFolder=".", const std::string &cameraName="")
Definition: CameraStereo.cpp:507
ULOGGER_ERROR
#define ULOGGER_ERROR(...)
Definition: ULogger.h:56
rtabmap::CameraInfo::odomPose
Transform odomPose
Definition: CameraInfo.h:68
rtabmap::Transform::size
int size() const
Definition: Transform.h:89
rtabmap::CameraStereoZed::init
virtual bool init(const std::string &calibrationFolder=".", const std::string &cameraName="")
Definition: CameraStereo.cpp:838
uFormat
std::string UTILITE_EXP uFormat(const char *fmt,...)
Definition: UConversion.cpp:350
rtabmap::CameraStereoZed::odomProvided
virtual bool odomProvided() const
Definition: CameraStereo.cpp:956
uNumber2Str
std::string UTILITE_EXP uNumber2Str(unsigned int number)
Definition: UConversion.cpp:90
rtabmap::CameraStereoZed::CameraStereoZed
CameraStereoZed(int deviceId, int resolution=2, int quality=1, int sensingMode=0, int confidenceThr=100, bool computeOdometry=false, float imageRate=0.0f, const Transform &localTransform=Transform::getIdentity(), bool selfCalibration=true)
Definition: CameraStereo.cpp:762
rtabmap::CameraStereoZed::getSerial
virtual std::string getSerial() const
Definition: CameraStereo.cpp:945
rtabmap::CameraModel::imageHeight
int imageHeight() const
Definition: CameraModel.h:114
rtabmap::CameraStereoVideo::capture2_
cv::VideoCapture capture2_
Definition: CameraStereo.h:246
rtabmap::CameraStereoZed::captureImage
virtual SensorData captureImage(CameraInfo *info=0)
Definition: CameraStereo.cpp:993
rtabmap::StereoCameraModel::name
const std::string & name() const
Definition: StereoCameraModel.h:92
UINFO
#define UINFO(...)

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