From 8810c9e694b8551b3290433b30e7eef17f480450 Mon Sep 17 00:00:00 2001 From: Walter Lucetti Date: Fri, 31 Jan 2020 20:17:10 +0100 Subject: [PATCH] Compilation fixes (#501) Great! Thx a lot! It should fix issue #499 as well. --- .../rtabmap/core/camera/CameraStereoZed.h | 2 +- corelib/src/camera/CameraStereoZed.cpp | 180 ++++++++++++++++-- 2 files changed, 169 insertions(+), 13 deletions(-) diff --git a/corelib/include/rtabmap/core/camera/CameraStereoZed.h b/corelib/include/rtabmap/core/camera/CameraStereoZed.h index e6187389..539960e6 100644 --- a/corelib/include/rtabmap/core/camera/CameraStereoZed.h +++ b/corelib/include/rtabmap/core/camera/CameraStereoZed.h @@ -89,7 +89,7 @@ private: Transform imuLocalTransform_; CameraVideo::Source src_; int usbDevice_; - std::string svoFilePath_; + std::string svoFilePath_; int resolution_; int quality_; bool selfCalibration_; diff --git a/corelib/src/camera/CameraStereoZed.cpp b/corelib/src/camera/CameraStereoZed.cpp index 431dfbb2..d4be871a 100644 --- a/corelib/src/camera/CameraStereoZed.cpp +++ b/corelib/src/camera/CameraStereoZed.cpp @@ -41,6 +41,7 @@ namespace rtabmap #ifdef RTABMAP_ZED static cv::Mat slMat2cvMat(sl::Mat& input) { +#if ZED_SDK_MAJOR_VERSION < 3 //convert MAT_TYPE to CV_TYPE int cv_type = -1; switch (input.getDataType()) { @@ -57,6 +58,24 @@ static cv::Mat slMat2cvMat(sl::Mat& input) { // 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)); +#else + //convert MAT_TYPE to CV_TYPE + int cv_type = -1; + switch (input.getDataType()) { + case sl::MAT_TYPE::F32_C1: cv_type = CV_32FC1; break; + case sl::MAT_TYPE::F32_C2: cv_type = CV_32FC2; break; + case sl::MAT_TYPE::F32_C3: cv_type = CV_32FC3; break; + case sl::MAT_TYPE::F32_C4: cv_type = CV_32FC4; break; + case sl::MAT_TYPE::U8_C1: cv_type = CV_8UC1; break; + case sl::MAT_TYPE::U8_C2: cv_type = CV_8UC2; break; + case sl::MAT_TYPE::U8_C3: cv_type = CV_8UC3; break; + case sl::MAT_TYPE::U8_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 } Transform zedPoseToTransform(const sl::Pose & pose) @@ -67,6 +86,7 @@ Transform zedPoseToTransform(const sl::Pose & pose) pose.pose_data.m[8], pose.pose_data.m[9], pose.pose_data.m[10], pose.pose_data.m[11]); } +#if ZED_SDK_MAJOR_VERSION < 3 IMU zedIMUtoIMU(const sl::IMUData & imuData, const Transform & imuLocalTransform) { sl::Orientation orientation = imuData.pose_data.getOrientation(); @@ -104,6 +124,45 @@ IMU zedIMUtoIMU(const sl::IMUData & imuData, const Transform & imuLocalTransform accCov, imuLocalTransform); } +#else +IMU zedIMUtoIMU(const sl::SensorsData & sensorData, const Transform & imuLocalTransform) +{ + sl::Orientation orientation = sensorData.imu.pose.getOrientation(); + + //Convert zed imu orientation from camera frame to world frame ENU! + Transform opticalTransform(0, 0, 1, 0, -1, 0, 0, 0, 0, -1, 0, 0); + Transform orientationT(0,0,0, orientation.ox, orientation.oy, orientation.oz, orientation.ow); + orientationT = opticalTransform * orientationT; + + static double deg2rad = 0.017453293; + Eigen::Vector4d accT = Eigen::Vector4d(sensorData.imu.linear_acceleration.v[0], sensorData.imu.linear_acceleration.v[1], sensorData.imu.linear_acceleration.v[2], 1); + Eigen::Vector4d gyrT = Eigen::Vector4d(sensorData.imu.angular_velocity.v[0]*deg2rad, sensorData.imu.angular_velocity.v[1]*deg2rad, sensorData.imu.angular_velocity.v[2]*deg2rad, 1); + + cv::Mat orientationCov = (cv::Mat_(3,3)<< + sensorData.imu.pose_covariance.r[0], sensorData.imu.pose_covariance.r[1], sensorData.imu.pose_covariance.r[2], + sensorData.imu.pose_covariance.r[3], sensorData.imu.pose_covariance.r[4], sensorData.imu.pose_covariance.r[5], + sensorData.imu.pose_covariance.r[6], sensorData.imu.pose_covariance.r[7], sensorData.imu.pose_covariance.r[8]); + cv::Mat angCov = (cv::Mat_(3,3)<< + sensorData.imu.angular_velocity_covariance.r[0], sensorData.imu.angular_velocity_covariance.r[1], sensorData.imu.angular_velocity_covariance.r[2], + sensorData.imu.angular_velocity_covariance.r[3], sensorData.imu.angular_velocity_covariance.r[4], sensorData.imu.angular_velocity_covariance.r[5], + sensorData.imu.angular_velocity_covariance.r[6], sensorData.imu.angular_velocity_covariance.r[7], sensorData.imu.angular_velocity_covariance.r[8]); + cv::Mat accCov = (cv::Mat_(3,3)<< + sensorData.imu.linear_acceleration_covariance.r[0], sensorData.imu.linear_acceleration_covariance.r[1], sensorData.imu.linear_acceleration_covariance.r[2], + sensorData.imu.linear_acceleration_covariance.r[3], sensorData.imu.linear_acceleration_covariance.r[4], sensorData.imu.linear_acceleration_covariance.r[5], + sensorData.imu.linear_acceleration_covariance.r[6], sensorData.imu.linear_acceleration_covariance.r[7], sensorData.imu.linear_acceleration_covariance.r[8]); + + Eigen::Quaternionf quat = orientationT.getQuaternionf(); + + return IMU( + cv::Vec4d(quat.x(), quat.y(), quat.z(), quat.w()), + orientationCov, + cv::Vec3d(gyrT[0], gyrT[1], gyrT[2]), + angCov, + cv::Vec3d(accT[0], accT[1], accT[2]), + accCov, + imuLocalTransform); +} +#endif class ZedIMUThread: public UThread { @@ -148,12 +207,21 @@ private: } frameRateTimer_.start(); +#if ZED_SDK_MAJOR_VERSION < 3 sl::IMUData imudata; bool res = zed_->getIMUData(imudata, sl::TIME_REFERENCE_IMAGE); if(res == sl::SUCCESS && imudata.valid) { UEventsManager::post(new IMUEvent(zedIMUtoIMU(imudata, imuLocalTransform_), UTimer::now())); } +#else + sl::SensorsData sensordata; + sl::ERROR_CODE res = zed_->getSensorsData(sensordata, sl::TIME_REFERENCE::IMAGE); + if(res == sl::ERROR_CODE::SUCCESS && sensordata.imu.is_available) + { + UEventsManager::post(new IMUEvent(zedIMUtoIMU(sensordata, imuLocalTransform_), UTimer::now())); + } +#endif } float rate_; sl::Camera * zed_; @@ -204,10 +272,21 @@ CameraStereoZed::CameraStereoZed( { UDEBUG(""); #ifdef RTABMAP_ZED +#if ZED_SDK_MAJOR_VERSION < 3 UASSERT(resolution_ >= sl::RESOLUTION_HD2K && resolution_ = sl::DEPTH_MODE_NONE && quality_ = sl::SENSING_MODE_STANDARD && sensingMode_ = 0 && confidenceThr_ <=100); +#else + sl::RESOLUTION res = static_cast(resolution_); + sl::DEPTH_MODE qual = static_cast(quality_); + sl::SENSING_MODE sens = static_cast(sensingMode_); + + UASSERT(res >= sl::RESOLUTION::HD2K && res < sl::RESOLUTION::LAST); + UASSERT(qual >= sl::DEPTH_MODE::NONE && qual < sl::DEPTH_MODE::LAST); + UASSERT(sens >= sl::SENSING_MODE::STANDARD && sens < sl::SENSING_MODE::LAST); + UASSERT(confidenceThr_ >= 0 && confidenceThr_ <=100); +#endif #endif } @@ -242,10 +321,21 @@ CameraStereoZed::CameraStereoZed( { UDEBUG(""); #ifdef RTABMAP_ZED +#if ZED_SDK_MAJOR_VERSION < 3 UASSERT(resolution_ >= sl::RESOLUTION_HD2K && resolution_ = sl::DEPTH_MODE_NONE && quality_ = sl::SENSING_MODE_STANDARD && sensingMode_ = 0 && confidenceThr_ <=100); +#else + sl::RESOLUTION res = static_cast(resolution_); + sl::DEPTH_MODE qual = static_cast(quality_); + sl::SENSING_MODE sens = static_cast(sensingMode_); + + UASSERT(res >= sl::RESOLUTION::HD2K && res < sl::RESOLUTION::LAST); + UASSERT(qual >= sl::DEPTH_MODE::NONE && qual < sl::DEPTH_MODE::LAST); + UASSERT(sens >= sl::SENSING_MODE::STANDARD && sens < sl::SENSING_MODE::LAST); + UASSERT(confidenceThr_ >= 0 && confidenceThr_ <=100); +#endif #endif } @@ -288,11 +378,16 @@ bool CameraStereoZed::init(const std::string & calibrationFolder, const std::str sl::InitParameters param; param.camera_resolution=static_cast(resolution_); - param.camera_fps=getImageRate(); - param.camera_linux_id=usbDevice_; + param.camera_fps=getImageRate(); param.depth_mode=(sl::DEPTH_MODE)quality_; +#if ZED_SDK_MAJOR_VERSION < 3 + param.camera_linux_id=usbDevice_; param.coordinate_units=sl::UNIT_METER; param.coordinate_system=(sl::COORDINATE_SYSTEM)sl::COORDINATE_SYSTEM_IMAGE ; +#else + param.coordinate_units=sl::UNIT::METER; + param.coordinate_system=sl::COORDINATE_SYSTEM::IMAGE ; +#endif param.sdk_verbose=true; param.sdk_gpu_id=-1; param.depth_minimum_distance=-1; @@ -303,11 +398,18 @@ bool CameraStereoZed::init(const std::string & calibrationFolder, const std::str { UINFO("svo file = %s", svoFilePath_.c_str()); zed_ = new sl::Camera(); // Use in SVO playback mode + #if ZED_SDK_MAJOR_VERSION < 3 param.svo_input_filename=svoFilePath_.c_str(); +#else + param.input.setFromSVOFile(svoFilePath_.c_str()); +#endif r = zed_->open(param); } else { +#if ZED_SDK_MAJOR_VERSION >= 3 + param.input.setFromCameraID(usbDevice_); +#endif UINFO("Resolution=%d imagerate=%f device=%d", resolution_, getImageRate(), usbDevice_); zed_ = new sl::Camera(); // Use in Live Mode r = zed_->open(param); @@ -321,22 +423,36 @@ bool CameraStereoZed::init(const std::string & calibrationFolder, const std::str return false; } - +#if ZED_SDK_MAJOR_VERSION < 3 UINFO("Init ZED: Mode=%d Unit=%d CoordinateSystem=%d Verbose=false device=-1 minDist=-1 self-calibration=%s vflip=false", quality_, sl::UNIT_METER, sl::COORDINATE_SYSTEM_IMAGE , selfCalibration_?"true":"false"); - UDEBUG(""); - if(quality_!=sl::DEPTH_MODE_NONE) - { - zed_->setConfidenceThreshold(confidenceThr_); - } + if(quality_!=sl::DEPTH_MODE_NONE) + { + zed_->setConfidenceThreshold(confidenceThr_); + } +#else + UINFO("Init ZED: Mode=%d Unit=%d CoordinateSystem=%d Verbose=false device=-1 minDist=-1 self-calibration=%s vflip=false", + quality_, sl::UNIT::METER, sl::COORDINATE_SYSTEM::IMAGE , selfCalibration_?"true":"false"); +#endif + + + + + UDEBUG(""); if (computeOdometry_) { +#if ZED_SDK_MAJOR_VERSION < 3 sl::TrackingParameters tparam; - tparam.enable_spatial_memory=false; - zed_->enableTracking(tparam); - if(r!=sl::ERROR_CODE::SUCCESS) + tparam.enable_spatial_memory=false; + r = zed_->enableTracking(tparam); +#else + sl::PositionalTrackingParameters tparam; + tparam.enable_area_memory=false; + r = zed_->enablePositionalTracking(tparam); +#endif + if(r!=sl::ERROR_CODE::SUCCESS) { UERROR("Camera tracking initialization failed: \"%s\"", toString(r).c_str()); } @@ -365,7 +481,11 @@ bool CameraStereoZed::init(const std::string & calibrationFolder, const std::str (int)res.height, this->getLocalTransform().prettyPrint().c_str()); +#if ZED_SDK_MAJOR_VERSION < 3 if(infos.camera_model == sl::MODEL_ZED_M) +#else + if(infos.camera_model != sl::MODEL::ZED) +#endif { imuLocalTransform_ = this->getLocalTransform() * zedPoseToTransform(infos.camera_imu_transform).inverse(); UINFO("IMU local transform: %s (imu2cam=%s))", @@ -419,10 +539,17 @@ SensorData CameraStereoZed::captureImage(CameraInfo * info) { SensorData data; #ifdef RTABMAP_ZED +#if ZED_SDK_MAJOR_VERSION < 3 sl::RuntimeParameters rparam((sl::SENSING_MODE)sensingMode_, quality_ > 0, quality_ > 0, sl::REFERENCE_FRAME_CAMERA); +#else + sl::RuntimeParameters rparam((sl::SENSING_MODE)sensingMode_, quality_ > 0, quality_ > 0, sl::REFERENCE_FRAME::CAMERA); + rparam.confidence_threshold = confidenceThr_; +#endif + if(zed_) { UTimer timer; +#if ZED_SDK_MAJOR_VERSION < 3 bool res = zed_->grab(rparam); while (src_ == CameraVideo::kUsbDevice && res!=sl::SUCCESS && timer.elapsed() < 2.0) { @@ -430,11 +557,21 @@ SensorData CameraStereoZed::captureImage(CameraInfo * info) uSleep(10); res = zed_->grab(rparam); } - if(res==sl::SUCCESS) + + if(res==sl::SUCCESS) +#else + sl::ERROR_CODE res = zed_->grab(rparam); + + if(res==sl::ERROR_CODE::SUCCESS) +#endif { // get left image sl::Mat tmp; +#if ZED_SDK_MAJOR_VERSION < 3 zed_->retrieveImage(tmp,sl::VIEW_LEFT); +#else + zed_->retrieveImage(tmp,sl::VIEW::LEFT); +#endif cv::Mat rgbaLeft = slMat2cvMat(tmp); cv::Mat left; @@ -445,7 +582,11 @@ SensorData CameraStereoZed::captureImage(CameraInfo * info) // get depth image cv::Mat depth; sl::Mat tmp; +#if ZED_SDK_MAJOR_VERSION < 3 zed_->retrieveMeasure(tmp,sl::MEASURE_DEPTH); +#else + zed_->retrieveMeasure(tmp,sl::MEASURE::DEPTH); +#endif slMat2cvMat(tmp).copyTo(depth); data = SensorData(left, depth, stereoModel_.left(), this->getNextSeqID(), UTimer::now()); @@ -453,7 +594,11 @@ SensorData CameraStereoZed::captureImage(CameraInfo * info) else { // get right image +#if ZED_SDK_MAJOR_VERSION < 3 sl::Mat tmp;zed_->retrieveImage(tmp,sl::VIEW_RIGHT ); +#else + sl::Mat tmp;zed_->retrieveImage(tmp,sl::VIEW::RIGHT ); +#endif cv::Mat rgbaRight = slMat2cvMat(tmp); cv::Mat right; cv::cvtColor(rgbaRight, right, cv::COLOR_BGRA2GRAY); @@ -463,9 +608,15 @@ SensorData CameraStereoZed::captureImage(CameraInfo * info) if(imuPublishingThread_ == 0) { +#if ZED_SDK_MAJOR_VERSION < 3 sl::IMUData imudata; res = zed_->getIMUData(imudata, sl::TIME_REFERENCE_IMAGE); if(res == sl::SUCCESS && imudata.valid) +#else + sl::SensorsData imudata; + res = zed_->getSensorsData(imudata, sl::TIME_REFERENCE::IMAGE); + if(res == sl::ERROR_CODE::SUCCESS && imudata.imu.is_available) +#endif { //ZED-Mini data.setIMU(zedIMUtoIMU(imudata, imuLocalTransform_)); @@ -475,8 +626,13 @@ SensorData CameraStereoZed::captureImage(CameraInfo * info) if (computeOdometry_ && info) { sl::Pose pose; +#if ZED_SDK_MAJOR_VERSION < 3 sl::TRACKING_STATE tracking_state = zed_->getPosition(pose); if (tracking_state == sl::TRACKING_STATE_OK) +#else + sl::POSITIONAL_TRACKING_STATE tracking_state = zed_->getPosition(pose); + if (tracking_state == sl::POSITIONAL_TRACKING_STATE::OK) +#endif { int trackingConfidence = pose.pose_confidence; // FIXME What does pose_confidence == -1 mean?