00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028 #include "rtabmap/core/CameraStereo.h"
00029 #include "rtabmap/core/util2d.h"
00030 #include "rtabmap/core/CameraRGB.h"
00031 #include "rtabmap/core/Version.h"
00032
00033 #include <rtabmap/utilite/UEventsManager.h>
00034 #include <rtabmap/utilite/UConversion.h>
00035 #include <rtabmap/utilite/UStl.h>
00036 #include <rtabmap/utilite/UConversion.h>
00037 #include <rtabmap/utilite/UFile.h>
00038 #include <rtabmap/utilite/UDirectory.h>
00039 #include <rtabmap/utilite/UTimer.h>
00040 #include <rtabmap/utilite/UMath.h>
00041
00042 #include <opencv2/imgproc/types_c.h>
00043 #if CV_MAJOR_VERSION >= 3
00044 #include <opencv2/videoio/videoio_c.h>
00045 #endif
00046
00047 #ifdef RTABMAP_DC1394
00048 #include <dc1394/dc1394.h>
00049 #endif
00050
00051 #ifdef RTABMAP_FLYCAPTURE2
00052 #include <triclops.h>
00053 #include <fc2triclops.h>
00054 #endif
00055
00056 #ifdef RTABMAP_ZED
00057 #include <sl/Camera.hpp>
00058 #endif
00059
00060 namespace rtabmap
00061 {
00062
00063
00064
00065
00066
00067
00068 #ifdef RTABMAP_DC1394
00069 class DC1394Device
00070 {
00071 public:
00072 DC1394Device() :
00073 camera_(0),
00074 context_(0)
00075 {
00076
00077 }
00078 ~DC1394Device()
00079 {
00080 if (camera_)
00081 {
00082 if (DC1394_SUCCESS != dc1394_video_set_transmission(camera_, DC1394_OFF) ||
00083 DC1394_SUCCESS != dc1394_capture_stop(camera_))
00084 {
00085 UWARN("unable to stop camera");
00086 }
00087
00088
00089 dc1394_capture_stop(camera_);
00090 dc1394_camera_free(camera_);
00091 camera_ = NULL;
00092 }
00093 if(context_)
00094 {
00095 dc1394_free(context_);
00096 context_ = NULL;
00097 }
00098 }
00099
00100 const std::string & guid() const {return guid_;}
00101
00102 bool init()
00103 {
00104 if(camera_)
00105 {
00106
00107 dc1394_capture_stop(camera_);
00108 dc1394_camera_free(camera_);
00109 camera_ = NULL;
00110 }
00111
00112
00113 int err;
00114 if(context_ == NULL)
00115 {
00116 context_ = dc1394_new ();
00117 if (context_ == NULL)
00118 {
00119 UERROR( "Could not initialize dc1394_context.\n"
00120 "Make sure /dev/raw1394 exists, you have access permission,\n"
00121 "and libraw1394 development package is installed.");
00122 return false;
00123 }
00124 }
00125
00126 dc1394camera_list_t *list;
00127 err = dc1394_camera_enumerate(context_, &list);
00128 if (err != DC1394_SUCCESS)
00129 {
00130 UERROR("Could not get camera list");
00131 return false;
00132 }
00133
00134 if (list->num == 0)
00135 {
00136 UERROR("No cameras found");
00137 dc1394_camera_free_list (list);
00138 return false;
00139 }
00140 uint64_t guid = list->ids[0].guid;
00141 dc1394_camera_free_list (list);
00142
00143
00144 camera_ = dc1394_camera_new (context_, guid);
00145 if (!camera_)
00146 {
00147 UERROR("Failed to initialize camera with GUID [%016lx]", guid);
00148 return false;
00149 }
00150
00151 uint32_t value[3];
00152 value[0]= camera_->guid & 0xffffffff;
00153 value[1]= (camera_->guid >>32) & 0x000000ff;
00154 value[2]= (camera_->guid >>40) & 0xfffff;
00155 guid_ = uFormat("%06x%02x%08x", value[2], value[1], value[0]);
00156
00157 UINFO("camera model: %s %s", camera_->vendor, camera_->model);
00158
00159
00160
00161 bool bmode = camera_->bmode_capable;
00162 if (bmode
00163 && (DC1394_SUCCESS !=
00164 dc1394_video_set_operation_mode(camera_,
00165 DC1394_OPERATION_MODE_1394B)))
00166 {
00167 bmode = false;
00168 UWARN("failed to set IEEE1394b mode");
00169 }
00170
00171
00172 dc1394speed_t request = DC1394_ISO_SPEED_3200;
00173 int rate = 3200;
00174 if (!bmode)
00175 {
00176
00177 request = DC1394_ISO_SPEED_400;
00178 rate = 400;
00179 }
00180
00181
00182 while (rate > 400)
00183 {
00184 if (request <= DC1394_ISO_SPEED_MIN)
00185 {
00186
00187 dc1394speed_t curSpeed;
00188 if (DC1394_SUCCESS == dc1394_video_get_iso_speed(camera_, &curSpeed) && curSpeed <= DC1394_ISO_SPEED_MAX)
00189 {
00190
00191
00192
00193 request = curSpeed;
00194 rate = 100 << (curSpeed - DC1394_ISO_SPEED_MIN);
00195 }
00196 else
00197 {
00198 UWARN("Unable to get ISO speed; assuming 400Mb/s");
00199 rate = 400;
00200 request = DC1394_ISO_SPEED_400;
00201 }
00202 break;
00203 }
00204
00205 request = (dc1394speed_t) ((int) request - 1);
00206 rate = rate / 2;
00207 }
00208
00209
00210 if (DC1394_SUCCESS != dc1394_video_set_iso_speed(camera_, request))
00211 {
00212 UERROR("Failed to set iso speed");
00213 return false;
00214 }
00215
00216
00217 dc1394video_modes_t vmodes;
00218 err = dc1394_video_get_supported_modes(camera_, &vmodes);
00219 if (err != DC1394_SUCCESS)
00220 {
00221 UERROR("unable to get supported video modes");
00222 return (dc1394video_mode_t) 0;
00223 }
00224
00225
00226 bool found = false;
00227 dc1394video_mode_t videoMode = DC1394_VIDEO_MODE_FORMAT7_3;
00228 for (uint32_t i = 0; i < vmodes.num; ++i)
00229 {
00230 if (vmodes.modes[i] == videoMode)
00231 {
00232 found = true;
00233 }
00234 }
00235 if(!found)
00236 {
00237 UERROR("unable to get video mode %d", videoMode);
00238 return false;
00239 }
00240
00241 if (DC1394_SUCCESS != dc1394_video_set_mode(camera_, videoMode))
00242 {
00243 UERROR("Failed to set video mode %d", videoMode);
00244 return false;
00245 }
00246
00247
00248 if (dc1394_is_video_mode_scalable(videoMode) == DC1394_TRUE)
00249 {
00250 if (DC1394_SUCCESS != dc1394_format7_set_color_coding(camera_, videoMode, DC1394_COLOR_CODING_RAW16))
00251 {
00252 UERROR("Could not set color coding");
00253 return false;
00254 }
00255 uint32_t packetSize;
00256 if (DC1394_SUCCESS != dc1394_format7_get_recommended_packet_size(camera_, videoMode, &packetSize))
00257 {
00258 UERROR("Could not get default packet size");
00259 return false;
00260 }
00261
00262 if (DC1394_SUCCESS != dc1394_format7_set_packet_size(camera_, videoMode, packetSize))
00263 {
00264 UERROR("Could not set packet size");
00265 return false;
00266 }
00267 }
00268 else
00269 {
00270 UERROR("Video is not in mode scalable");
00271 }
00272
00273
00274
00275 if (DC1394_SUCCESS != dc1394_capture_setup(camera_, 4, DC1394_CAPTURE_FLAGS_DEFAULT))
00276 {
00277 UERROR("Failed to open device!");
00278 return false;
00279 }
00280
00281
00282 if (DC1394_SUCCESS != dc1394_video_set_transmission(camera_, DC1394_ON))
00283 {
00284 UERROR("Failed to start device!");
00285 return false;
00286 }
00287
00288 return true;
00289 }
00290
00291 bool getImages(cv::Mat & left, cv::Mat & right)
00292 {
00293 if(camera_)
00294 {
00295 dc1394video_frame_t * frame = NULL;
00296 UDEBUG("[%016lx] waiting camera", camera_->guid);
00297 dc1394_capture_dequeue (camera_, DC1394_CAPTURE_POLICY_WAIT, &frame);
00298 if (!frame)
00299 {
00300 UERROR("Unable to capture frame");
00301 return false;
00302 }
00303 dc1394video_frame_t frame1 = *frame;
00304
00305 size_t frame1_size = frame->total_bytes;
00306 frame1.image = (unsigned char *) malloc(frame1_size);
00307 frame1.allocated_image_bytes = frame1_size;
00308 frame1.color_coding = DC1394_COLOR_CODING_RAW8;
00309 int err = dc1394_deinterlace_stereo_frames(frame, &frame1, DC1394_STEREO_METHOD_INTERLACED);
00310 if (err != DC1394_SUCCESS)
00311 {
00312 free(frame1.image);
00313 dc1394_capture_enqueue(camera_, frame);
00314 UERROR("Could not extract stereo frames");
00315 return false;
00316 }
00317
00318 uint8_t* capture_buffer = reinterpret_cast<uint8_t *>(frame1.image);
00319 UASSERT(capture_buffer);
00320
00321 cv::Mat image(frame->size[1], frame->size[0], CV_8UC3);
00322 cv::Mat image2 = image.clone();
00323
00324
00325
00326 cv::cvtColor(cv::Mat(frame->size[1], frame->size[0], CV_8UC1, capture_buffer), left, CV_BayerRG2BGR);
00327 cv::cvtColor(cv::Mat(frame->size[1], frame->size[0], CV_8UC1, capture_buffer+image.total()), right, CV_BayerRG2GRAY);
00328
00329 dc1394_capture_enqueue(camera_, frame);
00330
00331 free(frame1.image);
00332
00333 return true;
00334 }
00335 return false;
00336 }
00337
00338 private:
00339 dc1394camera_t *camera_;
00340 dc1394_t *context_;
00341 std::string guid_;
00342 };
00343 #endif
00344
00345 bool CameraStereoDC1394::available()
00346 {
00347 #ifdef RTABMAP_DC1394
00348 return true;
00349 #else
00350 return false;
00351 #endif
00352 }
00353
00354 CameraStereoDC1394::CameraStereoDC1394(float imageRate, const Transform & localTransform) :
00355 Camera(imageRate, localTransform)
00356 #ifdef RTABMAP_DC1394
00357 ,
00358 device_(0)
00359 #endif
00360 {
00361 #ifdef RTABMAP_DC1394
00362 device_ = new DC1394Device();
00363 #endif
00364 }
00365
00366 CameraStereoDC1394::~CameraStereoDC1394()
00367 {
00368 #ifdef RTABMAP_DC1394
00369 delete device_;
00370 #endif
00371 }
00372
00373 bool CameraStereoDC1394::init(const std::string & calibrationFolder, const std::string & cameraName)
00374 {
00375 #ifdef RTABMAP_DC1394
00376 if(device_)
00377 {
00378 bool ok = device_->init();
00379 if(ok)
00380 {
00381
00382 if(!calibrationFolder.empty())
00383 {
00384 if(!stereoModel_.load(calibrationFolder, cameraName.empty()?device_->guid():cameraName, false))
00385 {
00386 UWARN("Missing calibration files for camera \"%s\" in \"%s\" folder, you should calibrate the camera!",
00387 cameraName.empty()?device_->guid().c_str():cameraName.c_str(), calibrationFolder.c_str());
00388 }
00389 else
00390 {
00391 UINFO("Stereo parameters: fx=%f cx=%f cy=%f baseline=%f",
00392 stereoModel_.left().fx(),
00393 stereoModel_.left().cx(),
00394 stereoModel_.left().cy(),
00395 stereoModel_.baseline());
00396 }
00397 }
00398 }
00399 return ok;
00400 }
00401 #else
00402 UERROR("CameraDC1394: RTAB-Map is not built with dc1394 support!");
00403 #endif
00404 return false;
00405 }
00406
00407 bool CameraStereoDC1394::isCalibrated() const
00408 {
00409 #ifdef RTABMAP_DC1394
00410 return stereoModel_.isValidForProjection();
00411 #else
00412 return false;
00413 #endif
00414 }
00415
00416 std::string CameraStereoDC1394::getSerial() const
00417 {
00418 #ifdef RTABMAP_DC1394
00419 if(device_)
00420 {
00421 return device_->guid();
00422 }
00423 #endif
00424 return "";
00425 }
00426
00427 SensorData CameraStereoDC1394::captureImage(CameraInfo * info)
00428 {
00429 SensorData data;
00430 #ifdef RTABMAP_DC1394
00431 if(device_)
00432 {
00433 cv::Mat left, right;
00434 device_->getImages(left, right);
00435
00436 if(!left.empty() && !right.empty())
00437 {
00438
00439 if(stereoModel_.left().isValidForRectification())
00440 {
00441 left = stereoModel_.left().rectifyImage(left);
00442 }
00443 if(stereoModel_.right().isValidForRectification())
00444 {
00445 right = stereoModel_.right().rectifyImage(right);
00446 }
00447 StereoCameraModel model;
00448 if(stereoModel_.isValidForProjection())
00449 {
00450 model = StereoCameraModel(
00451 stereoModel_.left().fx(),
00452 stereoModel_.left().fy(),
00453 stereoModel_.left().cx(),
00454 stereoModel_.left().cy(),
00455 stereoModel_.baseline(),
00456 this->getLocalTransform(),
00457 left.size());
00458 }
00459 data = SensorData(left, right, model, this->getNextSeqID(), UTimer::now());
00460 }
00461 }
00462 #else
00463 UERROR("CameraDC1394: RTAB-Map is not built with dc1394 support!");
00464 #endif
00465 return data;
00466 }
00467
00468
00469
00470
00471 CameraStereoFlyCapture2::CameraStereoFlyCapture2(float imageRate, const Transform & localTransform) :
00472 Camera(imageRate, localTransform)
00473 #ifdef RTABMAP_FLYCAPTURE2
00474 ,
00475 camera_(0),
00476 triclopsCtx_(0)
00477 #endif
00478 {
00479 #ifdef RTABMAP_FLYCAPTURE2
00480 camera_ = new FlyCapture2::Camera();
00481 #endif
00482 }
00483
00484 CameraStereoFlyCapture2::~CameraStereoFlyCapture2()
00485 {
00486 #ifdef RTABMAP_FLYCAPTURE2
00487
00488 camera_->StopCapture();
00489 camera_->Disconnect();
00490
00491
00492 triclopsDestroyContext( triclopsCtx_ ) ;
00493
00494 delete camera_;
00495 #endif
00496 }
00497
00498 bool CameraStereoFlyCapture2::available()
00499 {
00500 #ifdef RTABMAP_FLYCAPTURE2
00501 return true;
00502 #else
00503 return false;
00504 #endif
00505 }
00506
00507 bool CameraStereoFlyCapture2::init(const std::string & calibrationFolder, const std::string & cameraName)
00508 {
00509 #ifdef RTABMAP_FLYCAPTURE2
00510 if(camera_)
00511 {
00512
00513 camera_->StopCapture();
00514 camera_->Disconnect();
00515 }
00516 if(triclopsCtx_)
00517 {
00518 triclopsDestroyContext(triclopsCtx_);
00519 triclopsCtx_ = 0;
00520 }
00521
00522
00523 FlyCapture2::Error fc2Error = camera_->Connect();
00524 if(fc2Error != FlyCapture2::PGRERROR_OK)
00525 {
00526 UERROR("Failed to connect the camera.");
00527 return false;
00528 }
00529
00530
00531 Fc2Triclops::StereoCameraMode mode = Fc2Triclops::TWO_CAMERA_NARROW;
00532 if(Fc2Triclops::setStereoMode(*camera_, mode ))
00533 {
00534 UERROR("Failed to set stereo mode.");
00535 return false;
00536 }
00537
00538
00539 FlyCapture2::CameraInfo camInfo;
00540 if(camera_->GetCameraInfo(&camInfo) != FlyCapture2::PGRERROR_OK)
00541 {
00542 UERROR("Failed to get camera info.");
00543 return false;
00544 }
00545
00546 float dummy;
00547 unsigned packetSz;
00548 FlyCapture2::Format7ImageSettings imageSettings;
00549 int maxWidth = 640;
00550 int maxHeight = 480;
00551 if(camera_->GetFormat7Configuration(&imageSettings, &packetSz, &dummy) == FlyCapture2::PGRERROR_OK)
00552 {
00553 maxHeight = imageSettings.height;
00554 maxWidth = imageSettings.width;
00555 }
00556
00557
00558 if(Fc2Triclops::getContextFromCamera(camInfo.serialNumber, &triclopsCtx_))
00559 {
00560 UERROR("Failed to get calibration from the camera.");
00561 return false;
00562 }
00563
00564 float fx, cx, cy, baseline;
00565 triclopsGetFocalLength(triclopsCtx_, &fx);
00566 triclopsGetImageCenter(triclopsCtx_, &cy, &cx);
00567 triclopsGetBaseline(triclopsCtx_, &baseline);
00568 UINFO("Stereo parameters: fx=%f cx=%f cy=%f baseline=%f", fx, cx, cy, baseline);
00569
00570 triclopsSetCameraConfiguration(triclopsCtx_, TriCfg_2CAM_HORIZONTAL_NARROW );
00571 UASSERT(triclopsSetResolutionAndPrepare(triclopsCtx_, maxHeight, maxWidth, maxHeight, maxWidth) == Fc2Triclops::ERRORTYPE_OK);
00572
00573 if(camera_->StartCapture() != FlyCapture2::PGRERROR_OK)
00574 {
00575 UERROR("Failed to start capture.");
00576 return false;
00577 }
00578
00579 return true;
00580 #else
00581 UERROR("CameraStereoFlyCapture2: RTAB-Map is not built with Triclops support!");
00582 #endif
00583 return false;
00584 }
00585
00586 bool CameraStereoFlyCapture2::isCalibrated() const
00587 {
00588 #ifdef RTABMAP_FLYCAPTURE2
00589 if(triclopsCtx_)
00590 {
00591 float fx, cx, cy, baseline;
00592 triclopsGetFocalLength(triclopsCtx_, &fx);
00593 triclopsGetImageCenter(triclopsCtx_, &cy, &cx);
00594 triclopsGetBaseline(triclopsCtx_, &baseline);
00595 return fx > 0.0f && cx > 0.0f && cy > 0.0f && baseline > 0.0f;
00596 }
00597 #endif
00598 return false;
00599 }
00600
00601 std::string CameraStereoFlyCapture2::getSerial() const
00602 {
00603 #ifdef RTABMAP_FLYCAPTURE2
00604 if(camera_ && camera_->IsConnected())
00605 {
00606 FlyCapture2::CameraInfo camInfo;
00607 if(camera_->GetCameraInfo(&camInfo) == FlyCapture2::PGRERROR_OK)
00608 {
00609 return uNumber2Str(camInfo.serialNumber);
00610 }
00611 }
00612 #endif
00613 return "";
00614 }
00615
00616
00617 #ifdef RTABMAP_FLYCAPTURE2
00618 struct ImageContainer
00619 {
00620 FlyCapture2::Image tmp[2];
00621 FlyCapture2::Image unprocessed[2];
00622 } ;
00623 #endif
00624
00625 SensorData CameraStereoFlyCapture2::captureImage(CameraInfo * info)
00626 {
00627 SensorData data;
00628 #ifdef RTABMAP_FLYCAPTURE2
00629 if(camera_ && triclopsCtx_ && camera_->IsConnected())
00630 {
00631
00632
00633 FlyCapture2::Image grabbedImage;
00634 if(camera_->RetrieveBuffer(&grabbedImage) == FlyCapture2::PGRERROR_OK)
00635 {
00636
00637 ImageContainer imageCont;
00638
00639
00640 FlyCapture2::Image imageRawRight;
00641 FlyCapture2::Image imageRawLeft;
00642 FlyCapture2::Image * unprocessedImage = imageCont.unprocessed;
00643
00644
00645 if(Fc2Triclops::unpackUnprocessedRawOrMono16Image(
00646 grabbedImage,
00647 true ,
00648 imageRawLeft ,
00649 imageRawRight ) == Fc2Triclops::ERRORTYPE_OK)
00650 {
00651
00652 FlyCapture2::Image srcImgRightRef(imageRawRight);
00653 FlyCapture2::Image srcImgLeftRef(imageRawLeft);
00654
00655 bool ok = true;;
00656 if ( srcImgRightRef.SetColorProcessing(FlyCapture2::HQ_LINEAR) != FlyCapture2::PGRERROR_OK ||
00657 srcImgLeftRef.SetColorProcessing(FlyCapture2::HQ_LINEAR) != FlyCapture2::PGRERROR_OK)
00658 {
00659 ok = false;
00660 }
00661
00662 if(ok)
00663 {
00664 FlyCapture2::Image imageColorRight;
00665 FlyCapture2::Image imageColorLeft;
00666 if ( srcImgRightRef.Convert(FlyCapture2::PIXEL_FORMAT_MONO8, &imageColorRight) != FlyCapture2::PGRERROR_OK ||
00667 srcImgLeftRef.Convert(FlyCapture2::PIXEL_FORMAT_BGRU, &imageColorLeft) != FlyCapture2::PGRERROR_OK)
00668 {
00669 ok = false;
00670 }
00671
00672 if(ok)
00673 {
00674
00675 TriclopsInput triclopsColorInputs;
00676 triclopsBuildRGBTriclopsInput(
00677 grabbedImage.GetCols(),
00678 grabbedImage.GetRows(),
00679 imageColorRight.GetStride(),
00680 (unsigned long)grabbedImage.GetTimeStamp().seconds,
00681 (unsigned long)grabbedImage.GetTimeStamp().microSeconds,
00682 imageColorRight.GetData(),
00683 imageColorRight.GetData(),
00684 imageColorRight.GetData(),
00685 &triclopsColorInputs);
00686
00687 triclopsRectify(triclopsCtx_, const_cast<TriclopsInput *>(&triclopsColorInputs) );
00688
00689 TriclopsImage rectifiedImage;
00690 triclopsGetImage( triclopsCtx_,
00691 TriImg_RECTIFIED,
00692 TriCam_REFERENCE,
00693 &rectifiedImage );
00694
00695 cv::Mat left,right;
00696 right = cv::Mat(rectifiedImage.nrows, rectifiedImage.ncols, CV_8UC1, rectifiedImage.data).clone();
00697
00698
00699 triclopsBuildPackedTriclopsInput(
00700 grabbedImage.GetCols(),
00701 grabbedImage.GetRows(),
00702 imageColorLeft.GetStride(),
00703 (unsigned long)grabbedImage.GetTimeStamp().seconds,
00704 (unsigned long)grabbedImage.GetTimeStamp().microSeconds,
00705 imageColorLeft.GetData(),
00706 &triclopsColorInputs );
00707
00708 cv::Mat pixelsLeftBuffer( grabbedImage.GetRows(), grabbedImage.GetCols(), CV_8UC4);
00709 TriclopsPackedColorImage colorImage;
00710 triclopsSetPackedColorImageBuffer(
00711 triclopsCtx_,
00712 TriCam_LEFT,
00713 (TriclopsPackedColorPixel*)pixelsLeftBuffer.data );
00714
00715 triclopsRectifyPackedColorImage(
00716 triclopsCtx_,
00717 TriCam_LEFT,
00718 &triclopsColorInputs,
00719 &colorImage );
00720
00721 cv::cvtColor(pixelsLeftBuffer, left, CV_RGBA2RGB);
00722
00723
00724 float fx, cy, cx, baseline;
00725 triclopsGetFocalLength(triclopsCtx_, &fx);
00726 triclopsGetImageCenter(triclopsCtx_, &cy, &cx);
00727 triclopsGetBaseline(triclopsCtx_, &baseline);
00728
00729 StereoCameraModel model(
00730 fx,
00731 fx,
00732 cx,
00733 cy,
00734 baseline,
00735 this->getLocalTransform(),
00736 left.size());
00737 data = SensorData(left, right, model, this->getNextSeqID(), UTimer::now());
00738 }
00739 }
00740 }
00741 }
00742 }
00743
00744 #else
00745 UERROR("CameraStereoFlyCapture2: RTAB-Map is not built with Triclops support!");
00746 #endif
00747 return data;
00748 }
00749
00750
00751
00752
00753 bool CameraStereoZed::available()
00754 {
00755 #ifdef RTABMAP_ZED
00756 return true;
00757 #else
00758 return false;
00759 #endif
00760 }
00761
00762 CameraStereoZed::CameraStereoZed(
00763 int deviceId,
00764 int resolution,
00765 int quality,
00766 int sensingMode,
00767 int confidenceThr,
00768 bool computeOdometry,
00769 float imageRate,
00770 const Transform & localTransform,
00771 bool selfCalibration) :
00772 Camera(imageRate, localTransform)
00773 #ifdef RTABMAP_ZED
00774 ,
00775 zed_(0),
00776 src_(CameraVideo::kUsbDevice),
00777 usbDevice_(deviceId),
00778 svoFilePath_(""),
00779 resolution_(resolution),
00780 quality_(quality),
00781 selfCalibration_(selfCalibration),
00782 sensingMode_(sensingMode),
00783 confidenceThr_(confidenceThr),
00784 computeOdometry_(computeOdometry),
00785 lost_(true)
00786 #endif
00787 {
00788 UDEBUG("");
00789 #ifdef RTABMAP_ZED
00790 UASSERT(resolution_ >= sl::RESOLUTION_HD2K && resolution_ <sl::RESOLUTION_LAST);
00791 UASSERT(quality_ >= sl::DEPTH_MODE_NONE && quality_ <sl::DEPTH_MODE_LAST);
00792 UASSERT(sensingMode_ >= sl::SENSING_MODE_STANDARD && sensingMode_ <sl::SENSING_MODE_LAST);
00793 UASSERT(confidenceThr_ >= 0 && confidenceThr_ <=100);
00794 #endif
00795 }
00796
00797 CameraStereoZed::CameraStereoZed(
00798 const std::string & filePath,
00799 int quality,
00800 int sensingMode,
00801 int confidenceThr,
00802 bool computeOdometry,
00803 float imageRate,
00804 const Transform & localTransform,
00805 bool selfCalibration) :
00806 Camera(imageRate, localTransform)
00807 #ifdef RTABMAP_ZED
00808 ,
00809 zed_(0),
00810 src_(CameraVideo::kVideoFile),
00811 usbDevice_(0),
00812 svoFilePath_(filePath),
00813 resolution_(2),
00814 quality_(quality),
00815 selfCalibration_(selfCalibration),
00816 sensingMode_(sensingMode),
00817 confidenceThr_(confidenceThr),
00818 computeOdometry_(computeOdometry),
00819 lost_(true)
00820 #endif
00821 {
00822 UDEBUG("");
00823 #ifdef RTABMAP_ZED
00824 UASSERT(resolution_ >= sl::RESOLUTION_HD2K && resolution_ <sl::RESOLUTION_LAST);
00825 UASSERT(quality_ >= sl::DEPTH_MODE_NONE && quality_ <sl::DEPTH_MODE_LAST);
00826 UASSERT(sensingMode_ >= sl::SENSING_MODE_STANDARD && sensingMode_ <sl::SENSING_MODE_LAST);
00827 UASSERT(confidenceThr_ >= 0 && confidenceThr_ <=100);
00828 #endif
00829 }
00830
00831 CameraStereoZed::~CameraStereoZed()
00832 {
00833 #ifdef RTABMAP_ZED
00834 delete zed_;
00835 #endif
00836 }
00837
00838 bool CameraStereoZed::init(const std::string & calibrationFolder, const std::string & cameraName)
00839 {
00840 UDEBUG("");
00841 #ifdef RTABMAP_ZED
00842 if(zed_)
00843 {
00844 delete zed_;
00845 zed_ = 0;
00846 }
00847
00848 lost_ = true;
00849
00850 sl::InitParameters param;
00851 param.camera_resolution=static_cast<sl::RESOLUTION>(resolution_);
00852 param.camera_fps=getImageRate();
00853 param.camera_linux_id=usbDevice_;
00854 param.depth_mode=(sl::DEPTH_MODE)quality_;
00855 param.coordinate_units=sl::UNIT_METER;
00856 param.coordinate_system=(sl::COORDINATE_SYSTEM)sl::COORDINATE_SYSTEM_IMAGE ;
00857 param.sdk_verbose=true;
00858 param.sdk_gpu_id=-1;
00859 param.depth_minimum_distance=-1;
00860 param.camera_disable_self_calib=!selfCalibration_;
00861
00862 sl::ERROR_CODE r = sl::ERROR_CODE::SUCCESS;
00863 if(src_ == CameraVideo::kVideoFile)
00864 {
00865 UINFO("svo file = %s", svoFilePath_.c_str());
00866 zed_ = new sl::Camera();
00867 param.svo_input_filename=svoFilePath_.c_str();
00868 r = zed_->open(param);
00869 }
00870 else
00871 {
00872 UINFO("Resolution=%d imagerate=%f device=%d", resolution_, getImageRate(), usbDevice_);
00873 zed_ = new sl::Camera();
00874 r = zed_->open(param);
00875 }
00876
00877 if(r!=sl::ERROR_CODE::SUCCESS)
00878 {
00879 UERROR("Camera initialization failed: \"%s\"", toString(r).c_str());
00880 delete zed_;
00881 zed_ = 0;
00882 return false;
00883 }
00884
00885
00886 UINFO("Init ZED: Mode=%d Unit=%d CoordinateSystem=%d Verbose=false device=-1 minDist=-1 self-calibration=%s vflip=false",
00887 quality_, sl::UNIT_METER, sl::COORDINATE_SYSTEM_IMAGE , selfCalibration_?"true":"false");
00888 UDEBUG("");
00889
00890 if(quality_!=sl::DEPTH_MODE_NONE)
00891 {
00892 zed_->setConfidenceThreshold(confidenceThr_);
00893 }
00894
00895 if (computeOdometry_)
00896 {
00897 sl::TrackingParameters tparam;
00898 tparam.enable_spatial_memory=false;
00899 zed_->enableTracking(tparam);
00900 if(r!=sl::ERROR_CODE::SUCCESS)
00901 {
00902 UERROR("Camera tracking initialization failed: \"%s\"", toString(r).c_str());
00903 }
00904 }
00905
00906 sl::CameraInformation infos = zed_->getCameraInformation();
00907 sl::CalibrationParameters *stereoParams = &(infos.calibration_parameters );
00908 sl::Resolution res = stereoParams->left_cam.image_size;
00909
00910 stereoModel_ = StereoCameraModel(
00911 stereoParams->left_cam.fx,
00912 stereoParams->left_cam.fy,
00913 stereoParams->left_cam.cx,
00914 stereoParams->left_cam.cy,
00915 stereoParams->T[0],
00916 this->getLocalTransform(),
00917 cv::Size(res.width, res.height));
00918
00919 UINFO("Calibration: fx=%f, fy=%f, cx=%f, cy=%f, baseline=%f, width=%d, height=%d, transform=%s",
00920 stereoParams->left_cam.fx,
00921 stereoParams->left_cam.fy,
00922 stereoParams->left_cam.cx,
00923 stereoParams->left_cam.cy,
00924 stereoParams->T[0],
00925 (int)res.width,
00926 (int)res.height,
00927 this->getLocalTransform().prettyPrint().c_str());
00928
00929 return true;
00930 #else
00931 UERROR("CameraStereoZED: RTAB-Map is not built with ZED sdk support!");
00932 #endif
00933 return false;
00934 }
00935
00936 bool CameraStereoZed::isCalibrated() const
00937 {
00938 #ifdef RTABMAP_ZED
00939 return stereoModel_.isValidForProjection();
00940 #else
00941 return false;
00942 #endif
00943 }
00944
00945 std::string CameraStereoZed::getSerial() const
00946 {
00947 #ifdef RTABMAP_ZED
00948 if(zed_)
00949 {
00950 return uFormat("%x", zed_->getCameraInformation ().serial_number);
00951 }
00952 #endif
00953 return "";
00954 }
00955
00956 bool CameraStereoZed::odomProvided() const
00957 {
00958 #ifdef RTABMAP_ZED
00959 return computeOdometry_;
00960 #else
00961 return false;
00962 #endif
00963 }
00964 #ifdef RTABMAP_ZED
00965 static cv::Mat slMat2cvMat(sl::Mat& input) {
00966
00967 int cv_type = -1;
00968 switch (input.getDataType()) {
00969 case sl::MAT_TYPE_32F_C1: cv_type = CV_32FC1; break;
00970 case sl::MAT_TYPE_32F_C2: cv_type = CV_32FC2; break;
00971 case sl::MAT_TYPE_32F_C3: cv_type = CV_32FC3; break;
00972 case sl::MAT_TYPE_32F_C4: cv_type = CV_32FC4; break;
00973 case sl::MAT_TYPE_8U_C1: cv_type = CV_8UC1; break;
00974 case sl::MAT_TYPE_8U_C2: cv_type = CV_8UC2; break;
00975 case sl::MAT_TYPE_8U_C3: cv_type = CV_8UC3; break;
00976 case sl::MAT_TYPE_8U_C4: cv_type = CV_8UC4; break;
00977 default: break;
00978 }
00979
00980
00981 return cv::Mat(input.getHeight(), input.getWidth(), cv_type, input.getPtr<sl::uchar1>(sl::MEM_CPU));
00982 }
00983
00984 Transform zedPoseToTransform(const sl::Pose & pose)
00985 {
00986 return Transform(
00987 pose.pose_data.m[0], pose.pose_data.m[1], pose.pose_data.m[2], pose.pose_data.m[3],
00988 pose.pose_data.m[4], pose.pose_data.m[5], pose.pose_data.m[6], pose.pose_data.m[7],
00989 pose.pose_data.m[8], pose.pose_data.m[9], pose.pose_data.m[10], pose.pose_data.m[11]);
00990 }
00991 #endif
00992
00993 SensorData CameraStereoZed::captureImage(CameraInfo * info)
00994 {
00995 SensorData data;
00996 #ifdef RTABMAP_ZED
00997 sl::RuntimeParameters rparam((sl::SENSING_MODE)sensingMode_, quality_ > 0, quality_ > 0, sl::REFERENCE_FRAME_CAMERA);
00998 if(zed_)
00999 {
01000 UTimer timer;
01001 bool res = zed_->grab(rparam);
01002 while (src_ == CameraVideo::kUsbDevice && res!=sl::SUCCESS && timer.elapsed() < 2.0)
01003 {
01004
01005 uSleep(10);
01006 res = zed_->grab(rparam);
01007 }
01008 if(res==sl::SUCCESS)
01009 {
01010
01011 sl::Mat tmp;
01012 zed_->retrieveImage(tmp,sl::VIEW_LEFT);
01013 cv::Mat rgbaLeft = slMat2cvMat(tmp);
01014
01015 cv::Mat left;
01016 cv::cvtColor(rgbaLeft, left, cv::COLOR_BGRA2BGR);
01017
01018 if(quality_ > 0)
01019 {
01020
01021 cv::Mat depth;
01022 sl::Mat tmp;
01023 zed_->retrieveMeasure(tmp,sl::MEASURE_DEPTH);
01024 slMat2cvMat(tmp).copyTo(depth);
01025
01026 data = SensorData(left, depth, stereoModel_.left(), this->getNextSeqID(), UTimer::now());
01027 }
01028 else
01029 {
01030
01031 sl::Mat tmp;zed_->retrieveImage(tmp,sl::VIEW_RIGHT );
01032 cv::Mat rgbaRight = slMat2cvMat(tmp);
01033 cv::Mat right;
01034 cv::cvtColor(rgbaRight, right, cv::COLOR_BGRA2GRAY);
01035
01036 data = SensorData(left, right, stereoModel_, this->getNextSeqID(), UTimer::now());
01037 }
01038
01039 if (computeOdometry_ && info)
01040 {
01041 sl::Pose pose;
01042 sl::TRACKING_STATE tracking_state = zed_->getPosition(pose);
01043 if (tracking_state == sl::TRACKING_STATE_OK)
01044 {
01045 int trackingConfidence = pose.pose_confidence;
01046
01047 if (trackingConfidence>0)
01048 {
01049 info->odomPose = zedPoseToTransform(pose);
01050 if (!info->odomPose.isNull())
01051 {
01052
01053 Transform opticalTransform(0, 0, 1, 0, -1, 0, 0, 0, 0, -1, 0, 0);
01054 info->odomPose = opticalTransform * info->odomPose * opticalTransform.inverse();
01055
01056 if (lost_)
01057 {
01058 info->odomCovariance = cv::Mat::eye(6, 6, CV_64FC1) * 9999.0f;
01059 lost_ = false;
01060 UDEBUG("Init %s (var=%f)", info->odomPose.prettyPrint().c_str(), 9999.0f);
01061 }
01062 else
01063 {
01064 info->odomCovariance = cv::Mat::eye(6, 6, CV_64FC1) * 1.0f / float(trackingConfidence);
01065 UDEBUG("Run %s (var=%f)", info->odomPose.prettyPrint().c_str(), 1.0f / float(trackingConfidence));
01066 }
01067 }
01068 else
01069 {
01070 info->odomCovariance = cv::Mat::eye(6, 6, CV_64FC1) * 9999.0f;
01071 lost_ = true;
01072 UWARN("ZED lost! (trackingConfidence=%d)", trackingConfidence);
01073 }
01074 }
01075 else
01076 {
01077 info->odomCovariance = cv::Mat::eye(6, 6, CV_64FC1) * 9999.0f;
01078 lost_ = true;
01079 UWARN("ZED lost! (trackingConfidence=%d)", trackingConfidence);
01080 }
01081 }
01082 else
01083 {
01084 UWARN("Tracking not ok: state=\"%s\"", toString(tracking_state).c_str());
01085 }
01086 }
01087 }
01088 else if(src_ == CameraVideo::kUsbDevice)
01089 {
01090 UERROR("CameraStereoZed: Failed to grab images after 2 seconds!");
01091 }
01092 else
01093 {
01094 UWARN("CameraStereoZed: end of stream is reached!");
01095 }
01096 }
01097 #else
01098 UERROR("CameraStereoZED: RTAB-Map is not built with ZED sdk support!");
01099 #endif
01100 return data;
01101 }
01102
01103
01104
01105
01106
01107 bool CameraStereoImages::available()
01108 {
01109 return true;
01110 }
01111
01112 CameraStereoImages::CameraStereoImages(
01113 const std::string & pathLeftImages,
01114 const std::string & pathRightImages,
01115 bool rectifyImages,
01116 float imageRate,
01117 const Transform & localTransform) :
01118 CameraImages(pathLeftImages, imageRate, localTransform),
01119 camera2_(new CameraImages(pathRightImages))
01120 {
01121 this->setImagesRectified(rectifyImages);
01122 }
01123
01124 CameraStereoImages::CameraStereoImages(
01125 const std::string & pathLeftRightImages,
01126 bool rectifyImages,
01127 float imageRate,
01128 const Transform & localTransform) :
01129 CameraImages("", imageRate, localTransform),
01130 camera2_(0)
01131 {
01132 std::vector<std::string> paths = uListToVector(uSplit(pathLeftRightImages, uStrContains(pathLeftRightImages, ":")?':':';'));
01133 if(paths.size() >= 1)
01134 {
01135 this->setPath(paths[0]);
01136 this->setImagesRectified(rectifyImages);
01137
01138 if(paths.size() >= 2)
01139 {
01140 camera2_ = new CameraImages(paths[1]);
01141 }
01142 }
01143 else
01144 {
01145 UERROR("The path is empty!");
01146 }
01147 }
01148
01149 CameraStereoImages::~CameraStereoImages()
01150 {
01151 UDEBUG("");
01152 delete camera2_;
01153 UDEBUG("");
01154 }
01155
01156 bool CameraStereoImages::init(const std::string & calibrationFolder, const std::string & cameraName)
01157 {
01158
01159 if(!calibrationFolder.empty() && !cameraName.empty())
01160 {
01161 if(!stereoModel_.load(calibrationFolder, cameraName, false) && !stereoModel_.isValidForProjection())
01162 {
01163 UWARN("Missing calibration files for camera \"%s\" in \"%s\" folder, you should calibrate the camera!",
01164 cameraName.c_str(), calibrationFolder.c_str());
01165 }
01166 else
01167 {
01168 UINFO("Stereo parameters: fx=%f cx=%f cy=%f baseline=%f",
01169 stereoModel_.left().fx(),
01170 stereoModel_.left().cx(),
01171 stereoModel_.left().cy(),
01172 stereoModel_.baseline());
01173 }
01174 }
01175
01176 stereoModel_.setLocalTransform(this->getLocalTransform());
01177 stereoModel_.setName(cameraName);
01178 if(this->isImagesRectified() && !stereoModel_.isValidForRectification())
01179 {
01180 UERROR("Parameter \"rectifyImages\" is set, but no stereo model is loaded or valid.");
01181 return false;
01182 }
01183
01184
01185 bool rectify = this->isImagesRectified();
01186 this->setImagesRectified(false);
01187
01188 bool success = false;
01189 if(CameraImages::init())
01190 {
01191 if(camera2_)
01192 {
01193 camera2_->setBayerMode(this->getBayerMode());
01194 if(camera2_->init())
01195 {
01196 if(this->imagesCount() == camera2_->imagesCount())
01197 {
01198 success = true;
01199 }
01200 else
01201 {
01202 UERROR("Cameras don't have the same number of images (%d vs %d)",
01203 this->imagesCount(), camera2_->imagesCount());
01204 }
01205 }
01206 else
01207 {
01208 UERROR("Cannot initialize the second camera.");
01209 }
01210 }
01211 else
01212 {
01213 success = true;
01214 }
01215 }
01216 this->setImagesRectified(rectify);
01217 return success;
01218 }
01219
01220 bool CameraStereoImages::isCalibrated() const
01221 {
01222 return stereoModel_.isValidForProjection();
01223 }
01224
01225 std::string CameraStereoImages::getSerial() const
01226 {
01227 return stereoModel_.name();
01228 }
01229
01230 SensorData CameraStereoImages::captureImage(CameraInfo * info)
01231 {
01232 SensorData data;
01233
01234 SensorData left, right;
01235 left = CameraImages::captureImage(info);
01236 if(!left.imageRaw().empty())
01237 {
01238 if(camera2_)
01239 {
01240 right = camera2_->takeImage(info);
01241 }
01242 else
01243 {
01244 right = this->takeImage(info);
01245 }
01246
01247 if(!right.imageRaw().empty())
01248 {
01249
01250 cv::Mat leftImage = left.imageRaw();
01251 cv::Mat rightImage = right.imageRaw();
01252 if(rightImage.type() != CV_8UC1)
01253 {
01254 cv::Mat tmp;
01255 cv::cvtColor(rightImage, tmp, CV_BGR2GRAY);
01256 rightImage = tmp;
01257 }
01258 if(this->isImagesRectified() && stereoModel_.isValidForRectification())
01259 {
01260 leftImage = stereoModel_.left().rectifyImage(leftImage);
01261 rightImage = stereoModel_.right().rectifyImage(rightImage);
01262 }
01263
01264 if(stereoModel_.left().imageHeight() == 0 || stereoModel_.left().imageWidth() == 0)
01265 {
01266 stereoModel_.setImageSize(leftImage.size());
01267 }
01268
01269 data = SensorData(left.laserScanRaw(), leftImage, rightImage, stereoModel_, left.id()/(camera2_?1:2), left.stamp());
01270 data.setGroundTruth(left.groundTruth());
01271 }
01272 }
01273 return data;
01274 }
01275
01276
01277
01278
01279 bool CameraStereoVideo::available()
01280 {
01281 return true;
01282 }
01283
01284 CameraStereoVideo::CameraStereoVideo(
01285 const std::string & path,
01286 bool rectifyImages,
01287 float imageRate,
01288 const Transform & localTransform) :
01289 Camera(imageRate, localTransform),
01290 path_(path),
01291 rectifyImages_(rectifyImages),
01292 src_(CameraVideo::kVideoFile),
01293 usbDevice_(0),
01294 usbDevice2_(-1)
01295 {
01296 }
01297
01298 CameraStereoVideo::CameraStereoVideo(
01299 const std::string & pathLeft,
01300 const std::string & pathRight,
01301 bool rectifyImages,
01302 float imageRate,
01303 const Transform & localTransform) :
01304 Camera(imageRate, localTransform),
01305 path_(pathLeft),
01306 path2_(pathRight),
01307 rectifyImages_(rectifyImages),
01308 src_(CameraVideo::kVideoFile),
01309 usbDevice_(0),
01310 usbDevice2_(-1)
01311 {
01312 }
01313
01314 CameraStereoVideo::CameraStereoVideo(
01315 int device,
01316 bool rectifyImages,
01317 float imageRate,
01318 const Transform & localTransform) :
01319 Camera(imageRate, localTransform),
01320 rectifyImages_(rectifyImages),
01321 src_(CameraVideo::kUsbDevice),
01322 usbDevice_(device),
01323 usbDevice2_(-1)
01324 {
01325 }
01326
01327 CameraStereoVideo::CameraStereoVideo(
01328 int deviceLeft,
01329 int deviceRight,
01330 bool rectifyImages,
01331 float imageRate,
01332 const Transform & localTransform) :
01333 Camera(imageRate, localTransform),
01334 rectifyImages_(rectifyImages),
01335 src_(CameraVideo::kUsbDevice),
01336 usbDevice_(deviceLeft),
01337 usbDevice2_(deviceRight)
01338 {
01339 }
01340
01341 CameraStereoVideo::~CameraStereoVideo()
01342 {
01343 capture_.release();
01344 capture2_.release();
01345 }
01346
01347 bool CameraStereoVideo::init(const std::string & calibrationFolder, const std::string & cameraName)
01348 {
01349 cameraName_ = cameraName;
01350 if(capture_.isOpened())
01351 {
01352 capture_.release();
01353 }
01354 if(capture2_.isOpened())
01355 {
01356 capture2_.release();
01357 }
01358
01359 if (src_ == CameraVideo::kUsbDevice)
01360 {
01361 capture_.open(usbDevice_);
01362 if(usbDevice2_ < 0)
01363 {
01364 ULOGGER_DEBUG("CameraStereoVideo: Usb device initialization on device %d", usbDevice_);
01365 }
01366 else
01367 {
01368 ULOGGER_DEBUG("CameraStereoVideo: Usb device initialization on devices %d and %d", usbDevice_, usbDevice2_);
01369 capture2_.open(usbDevice2_);
01370 }
01371 }
01372 else if (src_ == CameraVideo::kVideoFile)
01373 {
01374 capture_.open(path_.c_str());
01375 if(path2_.empty())
01376 {
01377 ULOGGER_DEBUG("CameraStereoVideo: filename=\"%s\"", path_.c_str());
01378 }
01379 else
01380 {
01381 ULOGGER_DEBUG("CameraStereoVideo: filenames=\"%s\" and \"%s\"", path_.c_str(), path2_.c_str());
01382 capture2_.open(path2_.c_str());
01383 }
01384 }
01385 else
01386 {
01387 ULOGGER_ERROR("CameraStereoVideo: Unknown source...");
01388 }
01389
01390 if(!capture_.isOpened() || ((!path2_.empty() || usbDevice2_>=0) && !capture2_.isOpened()))
01391 {
01392 ULOGGER_ERROR("CameraStereoVideo: Failed to create a capture object!");
01393 capture_.release();
01394 capture2_.release();
01395 return false;
01396 }
01397
01398 if (cameraName_.empty())
01399 {
01400 unsigned int guid = (unsigned int)capture_.get(CV_CAP_PROP_GUID);
01401 if (guid != 0 && guid != 0xffffffff)
01402 {
01403 cameraName_ = uFormat("%08x", guid);
01404 }
01405 }
01406
01407
01408 if(!calibrationFolder.empty() && !cameraName_.empty())
01409 {
01410 if(!stereoModel_.load(calibrationFolder, cameraName_, false))
01411 {
01412 UWARN("Missing calibration files for camera \"%s\" in \"%s\" folder, you should calibrate the camera!",
01413 cameraName_.c_str(), calibrationFolder.c_str());
01414 }
01415 else
01416 {
01417 UINFO("Stereo parameters: fx=%f cx=%f cy=%f baseline=%f",
01418 stereoModel_.left().fx(),
01419 stereoModel_.left().cx(),
01420 stereoModel_.left().cy(),
01421 stereoModel_.baseline());
01422 }
01423 }
01424
01425 stereoModel_.setLocalTransform(this->getLocalTransform());
01426 if(rectifyImages_ && !stereoModel_.isValidForRectification())
01427 {
01428 UERROR("Parameter \"rectifyImages\" is set, but no stereo model is loaded or valid.");
01429 return false;
01430 }
01431 return true;
01432 }
01433
01434 bool CameraStereoVideo::isCalibrated() const
01435 {
01436 return stereoModel_.isValidForProjection();
01437 }
01438
01439 std::string CameraStereoVideo::getSerial() const
01440 {
01441 return cameraName_;
01442 }
01443
01444 SensorData CameraStereoVideo::captureImage(CameraInfo * info)
01445 {
01446 SensorData data;
01447
01448 cv::Mat img;
01449 if(capture_.isOpened() && ((path2_.empty() && usbDevice2_ < 0) || capture2_.isOpened()))
01450 {
01451 cv::Mat leftImage;
01452 cv::Mat rightImage;
01453 if(path2_.empty() && usbDevice2_ < 0)
01454 {
01455 if(!capture_.read(img))
01456 {
01457 return data;
01458 }
01459
01460 leftImage = cv::Mat(img, cv::Rect( 0, 0, img.size().width/2, img.size().height ));
01461 rightImage = cv::Mat(img, cv::Rect( img.size().width/2, 0, img.size().width/2, img.size().height ));
01462 }
01463 else if(!capture_.read(leftImage) || !capture2_.read(rightImage))
01464 {
01465 return data;
01466 }
01467 else if(leftImage.cols != rightImage.cols || leftImage.rows != rightImage.rows)
01468 {
01469 UERROR("Left and right streams don't have image of the same size: left=%dx%d right=%dx%d",
01470 leftImage.cols, leftImage.rows, rightImage.cols, rightImage.rows);
01471 return data;
01472 }
01473
01474
01475 bool rightCvt = false;
01476 if(rightImage.type() != CV_8UC1)
01477 {
01478 cv::Mat tmp;
01479 cv::cvtColor(rightImage, tmp, CV_BGR2GRAY);
01480 rightImage = tmp;
01481 rightCvt = true;
01482 }
01483
01484 if(rectifyImages_ && stereoModel_.left().isValidForRectification() && stereoModel_.right().isValidForRectification())
01485 {
01486 leftImage = stereoModel_.left().rectifyImage(leftImage);
01487 rightImage = stereoModel_.right().rectifyImage(rightImage);
01488 }
01489 else
01490 {
01491 leftImage = leftImage.clone();
01492 if(!rightCvt)
01493 {
01494 rightImage = rightImage.clone();
01495 }
01496 }
01497
01498 if(stereoModel_.left().imageHeight() == 0 || stereoModel_.left().imageWidth() == 0)
01499 {
01500 stereoModel_.setImageSize(leftImage.size());
01501 }
01502
01503 data = SensorData(leftImage, rightImage, stereoModel_, this->getNextSeqID(), UTimer::now());
01504 }
01505 else
01506 {
01507 ULOGGER_WARN("The camera must be initialized before requesting an image.");
01508 }
01509
01510 return data;
01511 }
01512
01513
01514 }