diff --git a/src/sc.cpp b/src/sc.cpp index c7a0e04..1f508b3 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; } } @@ -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_);