diff --git a/corelib/include/rtabmap/core/CameraStereo.h b/corelib/include/rtabmap/core/CameraStereo.h index 796a09e6..f1db7875 100644 --- a/corelib/include/rtabmap/core/CameraStereo.h +++ b/corelib/include/rtabmap/core/CameraStereo.h @@ -42,11 +42,8 @@ class Camera; namespace sl { -namespace zed -{ class Camera; } -} namespace rtabmap { @@ -148,7 +145,7 @@ protected: private: #ifdef RTABMAP_ZED - sl::zed::Camera * zed_; + sl::Camera * zed_; StereoCameraModel stereoModel_; CameraVideo::Source src_; int usbDevice_; diff --git a/corelib/src/CameraStereo.cpp b/corelib/src/CameraStereo.cpp index 60388d37..967047ca 100644 --- a/corelib/src/CameraStereo.cpp +++ b/corelib/src/CameraStereo.cpp @@ -49,7 +49,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #endif #ifdef RTABMAP_ZED -#include +#include #endif namespace rtabmap @@ -785,9 +785,9 @@ CameraStereoZed::CameraStereoZed( { UDEBUG(""); #ifdef RTABMAP_ZED - UASSERT(resolution_ >= sl::zed::HD2K && resolution_ = sl::zed::NONE && quality_ = sl::zed::FILL && sensingMode_ = sl::RESOLUTION_HD2K && resolution_ = sl::DEPTH_MODE_NONE && quality_ = sl::SENSING_MODE_FILL && sensingMode_ = 0 && confidenceThr_ <=100); #endif } @@ -819,9 +819,9 @@ CameraStereoZed::CameraStereoZed( { UDEBUG(""); #ifdef RTABMAP_ZED - UASSERT(resolution_ >= sl::zed::HD2K && resolution_ = sl::zed::NONE && quality_ = sl::zed::FILL && sensingMode_ = sl::RESOLUTION_HD2K && resolution_ = sl::DEPTH_MODE_NONE && quality_ = sl::SENSING_MODE_FILL && sensingMode_ = 0 && confidenceThr_ <=100); #endif } @@ -850,56 +850,53 @@ bool CameraStereoZed::init(const std::string & calibrationFolder, const std::str if(src_ == CameraVideo::kVideoFile) { UINFO("svo file = %s", svoFilePath_.c_str()); - zed_ = new sl::zed::Camera(svoFilePath_); // Use in SVO playback mode + zed_ = new sl::Camera(); // Use in SVO playback mode + sl::InitParameters param; + param.svo_input_filename=svoFilePath_.c_str(); + zed_->open(param); } else { UINFO("Resolution=%d imagerate=%f device=%d", resolution_, getImageRate(), usbDevice_); - zed_ = new sl::zed::Camera((sl::zed::ZEDResolution_mode)resolution_, getImageRate(), usbDevice_); // Use in Live Mode + zed_ = new sl::Camera(); // Use in Live Mode + sl::InitParameters param; + param.camera_resolution=static_cast(resolution_); + param.camera_fps=getImageRate(); + param.camera_linux_id=usbDevice_; + param.depth_mode=(sl::DEPTH_MODE)quality_; + param.coordinate_units=sl::UNIT_METER; + param.coordinate_system=(sl::COORDINATE_SYSTEM)sl::COORDINATE_SYSTEM_IMAGE ; + param.sdk_verbose=false; + param.sdk_gpu_id=-1; + param.depth_minimum_distance=-1; + param.camera_disable_self_calib=!selfCalibration_; + zed_->open(param); } - sl::zed::InitParams parameters( - (sl::zed::MODE)quality_, //MODE - (sl::zed::UNIT)sl::zed::METER, //UNIT - (sl::zed::COORDINATE_SYSTEM)sl::zed::IMAGE, //COORDINATE_SYSTEM - false, // verbose - -1, //device (GPU) - -1., //minDist - !selfCalibration_, //disableSelfCalib: false = self calibrated - false); //vflip UINFO("Init ZED: Mode=%d Unit=%d CoordinateSystem=%d Verbose=false device=-1 minDist=-1 self-calibration=%s vflip=false", - quality_, sl::zed::METER, sl::zed::IMAGE, selfCalibration_?"true":"false"); - sl::zed::ERRCODE err = zed_->init(parameters); + quality_, sl::UNIT_METER, sl::COORDINATE_SYSTEM_IMAGE , selfCalibration_?"true":"false"); UDEBUG(""); - // Quit if an error occurred - if (err != sl::zed::SUCCESS) - { - UERROR("ZED camera initialization failed: %s", sl::zed::errcode2str(err).c_str()); - delete zed_; - zed_ = 0; - return false; - } - zed_->setConfidenceThreshold(confidenceThr_); if (computeOdometry_) { - Eigen::Matrix4f initPose; - initPose.setIdentity(4, 4); - zed_->enableTracking(initPose, false); + sl::TrackingParameters tparam; + tparam.enable_spatial_memory=false; + zed_->enableTracking(tparam); } - sl::zed::StereoParameters * stereoParams = zed_->getParameters(); - sl::zed::resolution res = zed_->getImageSize(); + sl::CameraInformation infos = zed_->getCameraInformation(); + sl::CalibrationParameters *stereoParams = &(infos.calibration_parameters ); + sl::Resolution res = stereoParams->left_cam.image_size; stereoModel_ = StereoCameraModel( - stereoParams->LeftCam.fx, - stereoParams->LeftCam.fy, - stereoParams->LeftCam.cx, - stereoParams->LeftCam.cy, - stereoParams->baseline, + stereoParams->left_cam.fx, + stereoParams->left_cam.fy, + stereoParams->left_cam.cx, + stereoParams->left_cam.cy, + stereoParams->T[0],//baseline this->getLocalTransform(), cv::Size(res.width, res.height)); @@ -924,7 +921,7 @@ std::string CameraStereoZed::getSerial() const #ifdef RTABMAP_ZED if(zed_) { - return uFormat("%x", zed_->getZEDSerial()); + return uFormat("%x", zed_->getCameraInformation ().serial_number); } #endif return ""; @@ -938,25 +935,51 @@ bool CameraStereoZed::odomProvided() const return false; #endif } +#ifdef RTABMAP_ZED +static cv::Mat slMat2cvMat(sl::Mat& input) { + //convert MAT_TYPE to CV_TYPE + int cv_type = -1; + switch (input.getDataType()) { + case sl::MAT_TYPE_32F_C1: cv_type = CV_32FC1; break; + case sl::MAT_TYPE_32F_C2: cv_type = CV_32FC2; break; + case sl::MAT_TYPE_32F_C3: cv_type = CV_32FC3; break; + case sl::MAT_TYPE_32F_C4: cv_type = CV_32FC4; break; + case sl::MAT_TYPE_8U_C1: cv_type = CV_8UC1; break; + case sl::MAT_TYPE_8U_C2: cv_type = CV_8UC2; break; + case sl::MAT_TYPE_8U_C3: cv_type = CV_8UC3; break; + case sl::MAT_TYPE_8U_C4: cv_type = CV_8UC4; break; + default: break; + } + // cv::Mat data requires a uchar* pointer. Therefore, we get the uchar1 pointer from sl::Mat (getPtr()) + //cv::Mat and sl::Mat will share the same memory pointer + return cv::Mat(input.getHeight(), input.getWidth(), cv_type, input.getPtr(sl::MEM_CPU)); +} +#endif SensorData CameraStereoZed::captureImage(CameraInfo * info) { SensorData data; #ifdef RTABMAP_ZED + sl::RuntimeParameters rparam; + rparam.sensing_mode=(sl::SENSING_MODE)sensingMode_; + rparam.enable_depth=quality_ > 0; + rparam.enable_point_cloud=quality_ > 0; + rparam.move_point_cloud_to_world_frame=false; if(zed_) { UTimer timer; - bool res = zed_->grab((sl::zed::SENSING_MODE)sensingMode_, quality_ > 0, quality_ > 0, false); + bool res = zed_->grab(rparam); while (src_ == CameraVideo::kUsbDevice && res && timer.elapsed() < 2.0) { // maybe there is a latency with the USB, try again in 10 ms (for the next 2 seconds) uSleep(10); - res = zed_->grab((sl::zed::SENSING_MODE)sensingMode_, quality_ > 0, quality_ > 0, false); + res = zed_->grab(rparam); } if(!res) { // get left image - cv::Mat rgbaLeft = sl::zed::slMat2cvMat(zed_->retrieveImage(static_cast (sl::zed::LEFT))); + sl::Mat tmp;zed_->retrieveImage(tmp,sl::VIEW_LEFT); + cv::Mat rgbaLeft = slMat2cvMat(tmp); cv::Mat left; cv::cvtColor(rgbaLeft, left, cv::COLOR_BGRA2BGR); @@ -965,14 +988,17 @@ SensorData CameraStereoZed::captureImage(CameraInfo * info) { // get depth image cv::Mat depth; - slMat2cvMat(zed_->retrieveMeasure(sl::zed::MEASURE::DEPTH)).copyTo(depth); + sl::Mat tmp; + zed_->retrieveMeasure(tmp,sl::MEASURE_DEPTH); + slMat2cvMat(tmp).copyTo(depth); data = SensorData(left, depth, stereoModel_.left(), this->getNextSeqID(), UTimer::now()); } else { // get right image - cv::Mat rgbaRight = sl::zed::slMat2cvMat(zed_->retrieveImage(static_cast (sl::zed::RIGHT))); + sl::Mat tmp;zed_->retrieveImage(tmp,sl::VIEW_RIGHT ); + cv::Mat rgbaRight = slMat2cvMat(tmp); cv::Mat right; cv::cvtColor(rgbaRight, right, cv::COLOR_BGRA2GRAY); @@ -981,12 +1007,14 @@ SensorData CameraStereoZed::captureImage(CameraInfo * info) if (computeOdometry_ && info) { - Eigen::Matrix4f path; - int trackingConfidence = zed_->getTrackingConfidence(); + sl::Pose pose; + zed_->getPosition(pose); + int trackingConfidence = pose.pose_confidence; if (trackingConfidence) { - zed_->getPosition(path); - info->odomPose = Transform::fromEigen4f(path); + Transform t; + for(int i=0;i<16;i++)t.data()[i]=pose.pose_data.m[i]; + info->odomPose = t; if (!info->odomPose.isNull()) { //transform x->forward, y->left, z->up