From 3047b7da6b734628bdfe67eef1550d61a928a0d7 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Sat, 17 Oct 2020 19:57:27 -0400 Subject: [PATCH] CameraRealSense2: update for L515 support --- .../rtabmap/core/camera/CameraRealSense2.h | 1 + corelib/src/camera/CameraRealSense2.cpp | 94 ++++++++++++++++--- 2 files changed, 80 insertions(+), 15 deletions(-) diff --git a/corelib/include/rtabmap/core/camera/CameraRealSense2.h b/corelib/include/rtabmap/core/camera/CameraRealSense2.h index 6ff1145b..3bec4e2e 100644 --- a/corelib/include/rtabmap/core/camera/CameraRealSense2.h +++ b/corelib/include/rtabmap/core/camera/CameraRealSense2.h @@ -135,6 +135,7 @@ private: Transform dualExtrinsics_; std::string jsonConfig_; bool closing_; + bool isL500_; static Transform realsense2PoseRotation_; static Transform realsense2PoseRotationInv_; diff --git a/corelib/src/camera/CameraRealSense2.cpp b/corelib/src/camera/CameraRealSense2.cpp index 135767f5..2d42ffc7 100644 --- a/corelib/src/camera/CameraRealSense2.cpp +++ b/corelib/src/camera/CameraRealSense2.cpp @@ -78,7 +78,8 @@ CameraRealSense2::CameraRealSense2( cameraFps_(30), publishInterIMU_(false), dualMode_(false), - closing_(false) + closing_(false), + isL500_(false) #endif { UDEBUG(""); @@ -560,6 +561,7 @@ bool CameraRealSense2::init(const std::string & calibrationFolder, const std::st UINFO("Device Sensors: "); std::vector sensors(2); //0=rgb 1=depth 2=(pose in dualMode_) bool stereo = false; + isL500_ = false; for(auto&& elem : dev_sensors) { std::string module_name = elem.get_info(RS2_CAMERA_INFO_NAME); @@ -604,6 +606,11 @@ bool CameraRealSense2::init(const std::string & calibrationFolder, const std::st sensors.back().set_option(rs2_option::RS2_OPTION_ENABLE_POSE_JUMPING, 0); sensors.back().set_option(rs2_option::RS2_OPTION_ENABLE_RELOCALIZATION, 0); } + else if ("L500 Depth Sensor" == module_name) + { + sensors[1] = elem; + isL500_ = true; + } else { UERROR("Module Name \"%s\" isn't supported!", module_name.c_str()); @@ -649,8 +656,35 @@ bool CameraRealSense2::init(const std::string & calibrationFolder, const std::st auto video_profile = profile.as(); if(!stereo) { + if(isL500_ + && (video_profile.width() == 640 && + video_profile.height() == 480 && + video_profile.fps() == 30)) + { + if( i==0 // rgb + && video_profile.format() == RS2_FORMAT_RGB8 && video_profile.stream_type() == RS2_STREAM_COLOR) + { + auto intrinsic = video_profile.get_intrinsics(); + profilesPerSensor[i].push_back(profile); + rgbBuffer_ = cv::Mat(cv::Size(video_profile.width(), video_profile.height()), CV_8UC3, cv::Scalar(0, 0, 0)); + model_ = CameraModel(camera_name, intrinsic.fx, intrinsic.fy, intrinsic.ppx, intrinsic.ppy, this->getLocalTransform(), 0, cv::Size(intrinsic.width, intrinsic.height)); + rgbStreamProfile = profile; + *rgbIntrinsics_ = intrinsic; + added = true; + } + else if( i==1 // depth + && video_profile.format() == RS2_FORMAT_Z16 && video_profile.stream_type() == RS2_STREAM_DEPTH) + { + auto intrinsic = video_profile.get_intrinsics(); + profilesPerSensor[i].push_back(profile); + depthBuffer_ = cv::Mat(cv::Size(video_profile.width(), video_profile.height()), CV_16UC1, cv::Scalar(0)); + depthStreamProfile = profile; + *depthIntrinsics_ = intrinsic; + added = true; + } + } //D400 series: - if (video_profile.width() == cameraWidth_ && + else if (video_profile.width() == cameraWidth_ && video_profile.height() == cameraHeight_ && video_profile.fps() == cameraFps_) { @@ -964,26 +998,33 @@ bool CameraRealSense2::init(const std::string & calibrationFolder, const std::st { auto video_profile = profilesPerSensor[i][j].as(); UINFO("Opening: %s %d %d %d %d %s type=%d", rs2_format_to_string( - video_profile.format()), - video_profile.width(), - video_profile.height(), - video_profile.fps(), - video_profile.stream_index(), - video_profile.stream_name().c_str(), - video_profile.stream_type()); + video_profile.format()), + video_profile.width(), + video_profile.height(), + video_profile.fps(), + video_profile.stream_index(), + video_profile.stream_name().c_str(), + video_profile.stream_type()); + } + if(sensors[i].supports(rs2_option::RS2_OPTION_GLOBAL_TIME_ENABLED)) + { + float value = sensors[i].get_option(rs2_option::RS2_OPTION_GLOBAL_TIME_ENABLED); + UINFO("Set RS2_OPTION_GLOBAL_TIME_ENABLED=1 (was %f) for sensor %d", value, (int)i); + sensors[i].set_option(rs2_option::RS2_OPTION_GLOBAL_TIME_ENABLED, 1); } sensors[i].open(profilesPerSensor[i]); if(sensors[i].is()) { auto depth_sensor = sensors[i].as(); depth_scale_meters_ = depth_sensor.get_depth_scale(); + UINFO("Depth scale %f for sensor %d", depth_scale_meters_, (int)i); } sensors[i].start(multiple_message_callback_function); } } - uSleep(1000); // ignore the first frames - UINFO("Enabling streams...done!"); + uSleep(1000); // ignore the first frames + UINFO("Enabling streams...done!"); return true; @@ -1100,12 +1141,15 @@ SensorData CameraRealSense2::captureImage(CameraInfo * info) try{ auto frameset = syncer_->wait_for_frames(5000); UTimer timer; - while (frameset.size() != 2 && timer.elapsed() < 2.0) + int desiredFramesetSize = 2; + if(isL500_) + desiredFramesetSize = 3; + while ((int)frameset.size() != desiredFramesetSize && timer.elapsed() < 2.0) { // maybe there is a latency with the USB, try again in 100 ms (for the next 2 seconds) frameset = syncer_->wait_for_frames(100); } - if (frameset.size() == 2) + if ((int)frameset.size() == desiredFramesetSize) { double now = UTimer::now(); bool is_rgb_arrived = false; @@ -1125,7 +1169,15 @@ SensorData CameraRealSense2::captureImage(CameraInfo * info) auto stream_type = f.get_profile().stream_type(); if (stream_type == RS2_STREAM_COLOR || stream_type == RS2_STREAM_INFRARED) { - if(ir_ && !irDepth_) + if(isL500_) + { + if(stream_type == RS2_STREAM_COLOR) + { + rgb_frame = f; + is_rgb_arrived = true; + } + } + else if(ir_ && !irDepth_) { //stereo D435 if(!is_depth_arrived) @@ -1183,7 +1235,6 @@ SensorData CameraRealSense2::captureImage(CameraInfo * info) if(is_rgb_arrived && is_depth_arrived) { - auto from_image_frame = depth_frame.as(); cv::Mat depth; if(ir_) { @@ -1195,6 +1246,19 @@ SensorData CameraRealSense2::captureImage(CameraInfo * info) rs2::frameset processed = frameset.apply_filter(align); rs2::depth_frame aligned_depth_frame = processed.get_depth_frame(); depth = cv::Mat(depthBuffer_.size(), depthBuffer_.type(), (void*)aligned_depth_frame.get_data()).clone(); + if(depth_scale_meters_ != 0.001f) + { // convert to mm + if(depth.type() == CV_16UC1) + { + float scale = depth_scale_meters_ / 0.001f; + uint16_t *p = depth.ptr(); + int buffSize = depth.rows * depth.cols; + #pragma omp parallel for + for(int i = 0; i < buffSize; ++i) { + p[i] *= scale; + } + } + } } cv::Mat rgb = cv::Mat(rgbBuffer_.size(), rgbBuffer_.type(), (void*)rgb_frame.get_data());