Skip to content

Color frame is rgb #9

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
wants to merge 3 commits into
base: master
Choose a base branch
from
Open

Color frame is rgb #9

Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
faad764
Convert depth to meters
efernandez Mar 5, 2019
2cec3fd
Add depth_correction_enable param
efernandez Mar 5, 2019
c1e0d63
Fix visible/color frame conversion
efernandez Mar 5, 2019
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
33 changes: 20 additions & 13 deletions src/sc.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down Expand Up @@ -198,25 +208,16 @@ struct SessionDelegate : ST::CaptureSessionDelegate {
return;
}

cv::Mat img(visFrame.height(), visFrame.width(), CV_8UC1);

const uint8_t* buf = visFrame.rgbData();
const std::string encoding = getEncoding(visFrame);

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<uchar>(y, x) = buf[pixelOffset];
}
}
cv::Mat img(visFrame.height(), visFrame.width(), cv_bridge::getCvType(encoding), static_cast<void*>(const_cast<uint8_t*>(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;
Expand All @@ -238,7 +239,7 @@ struct SessionDelegate : ST::CaptureSessionDelegate {
for(int x = 0; x < depthFrame.width(); x++)
{
std::size_t pixelOffset = (y * depthFrame.width()) + x;
img.at<float>(y, x) = buf[pixelOffset];
img.at<float>(y, x) = buf[pixelOffset] * 0.001f;
}
}

Expand Down Expand Up @@ -351,6 +352,8 @@ class SCNode

bool imu_enable_;

bool depth_correction_enable_;

SessionDelegate *delegate_ = nullptr;
ST::CaptureSessionSettings sessionConfig_;
ST::CaptureSession captureSession_;
Expand All @@ -374,6 +377,9 @@ class SCNode
ros::param::param<bool>("~imu_enable", imu_enable_, false);
ROS_INFO_STREAM(NODE_NAME << ": imu_enable = " << imu_enable_);

ros::param::param<bool>("~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<std::string>("~frame_id", frame_id, DEFAULT_FRAME_ID);
ROS_INFO_STREAM(NODE_NAME << ": frame_id = " << frame_id);
Expand All @@ -393,6 +399,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_);
Expand Down