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