From faad764f24fc6b0e52270c5a4517bba19a67e59f Mon Sep 17 00:00:00 2001 From: Enrique Fernandez Date: Tue, 5 Mar 2019 18:35:12 +0100 Subject: [PATCH 1/3] Convert depth to meters --- src/sc.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/sc.cpp b/src/sc.cpp index c7a0e04..a1c4e42 100644 --- a/src/sc.cpp +++ b/src/sc.cpp @@ -238,7 +238,7 @@ struct SessionDelegate : ST::CaptureSessionDelegate { for(int x = 0; x < depthFrame.width(); x++) { std::size_t pixelOffset = (y * depthFrame.width()) + x; - img.at(y, x) = buf[pixelOffset]; + img.at(y, x) = buf[pixelOffset] * 0.001f; } } From 2cec3fdef53bd5a1fd5cca51f485cb84c3d8c98a Mon Sep 17 00:00:00 2001 From: Enrique Fernandez Date: Tue, 5 Mar 2019 18:35:34 +0100 Subject: [PATCH 2/3] Add depth_correction_enable param Defaults to false. --- src/sc.cpp | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/src/sc.cpp b/src/sc.cpp index a1c4e42..1f508b3 100644 --- a/src/sc.cpp +++ b/src/sc.cpp @@ -351,6 +351,8 @@ class SCNode bool imu_enable_; + bool depth_correction_enable_; + SessionDelegate *delegate_ = nullptr; ST::CaptureSessionSettings sessionConfig_; ST::CaptureSession captureSession_; @@ -374,6 +376,9 @@ class SCNode ros::param::param("~imu_enable", imu_enable_, false); ROS_INFO_STREAM(NODE_NAME << ": imu_enable = " << imu_enable_); + ros::param::param("~depth_correction_enable", depth_correction_enable_, false); + ROS_INFO_STREAM(NODE_NAME << ": depth_correction_enable = " << depth_correction_enable_); + std::string frame_id; ros::param::param("~frame_id", frame_id, DEFAULT_FRAME_ID); ROS_INFO_STREAM(NODE_NAME << ": frame_id = " << frame_id); @@ -393,6 +398,7 @@ class SCNode sessionConfig_.source = ST::CaptureSessionSourceId::StructureCore; sessionConfig_.structureCore = scConfig; + sessionConfig_.applyExpensiveCorrection = depth_correction_enable_; delegate_ = new SessionDelegate(nh_, frame_id, sessionConfig_); captureSession_.setDelegate(delegate_); From c1e0d63ab2fc355876e15fb25b044e525f1a404f Mon Sep 17 00:00:00 2001 From: Enrique Fernandez Date: Tue, 5 Mar 2019 20:41:16 +0100 Subject: [PATCH 3/3] Fix visible/color frame conversion It's RGB8, not MONO8. It's also possible to do a shallow copy instead of a deep copy, so the conversion is faster. --- src/sc.cpp | 25 +++++++++++++------------ 1 file changed, 13 insertions(+), 12 deletions(-) diff --git a/src/sc.cpp b/src/sc.cpp index 1f508b3..fda086a 100644 --- a/src/sc.cpp +++ b/src/sc.cpp @@ -21,6 +21,16 @@ #define LOG_PERIOD_S 10 #define NODE_NAME "sc" +bool isMono(const ST::ColorFrame &visFrame) +{ + return visFrame.width() * visFrame.height() == visFrame.rgbSize(); +} + +std::string getEncoding(const ST::ColorFrame &visFrame) +{ + return isMono(visFrame) ? sensor_msgs::image_encodings::MONO8 : sensor_msgs::image_encodings::RGB8; +} + struct SessionDelegate : ST::CaptureSessionDelegate { private: std::mutex lock; @@ -198,25 +208,16 @@ struct SessionDelegate : ST::CaptureSessionDelegate { return; } - cv::Mat img(visFrame.height(), visFrame.width(), CV_8UC1); + const std::string encoding = getEncoding(visFrame); - const uint8_t* buf = visFrame.rgbData(); - - for(int y = 0; y < visFrame.height(); y++) - { - for(int x = 0; x < visFrame.width(); x++) - { - std::size_t pixelOffset = (y * visFrame.width()) + x; - img.at(y, x) = buf[pixelOffset]; - } - } + cv::Mat img(visFrame.height(), visFrame.width(), cv_bridge::getCvType(encoding), static_cast(const_cast(visFrame.rgbData()))); std_msgs::Header header; header.frame_id = frame_id_; // TODO: convert timstamp to ROS time // header.stamp = infraredFrame.timestamp(); - sensor_msgs::ImagePtr msg = cv_bridge::CvImage(header, "mono8", img).toImageMsg(); + sensor_msgs::ImagePtr msg = cv_bridge::CvImage(header, encoding, img).toImageMsg(); vis_pub_.publish(msg); sensor_msgs::CameraInfo cam_info;