Convert RAW depth data to depth image using cv_bridge and publish it using image transport
I am using a custom package which gives me raw depth data of type const uint16_t*
with values like 2027, 1980, 0, 0, 1678...
I want to be able to convert this raw depth data into an image and publish it over some topic.
So far, my code looks like this
m_DepthFrame = *m_DepthProvider->getDepthFrame();
if(m_DepthFrame.getDepthData() != nullptr)
{
cv::Mat l_depthImage(m_DepthFrame.getheight(), m_DepthFrame.getwidth(), CV_16UC1, (void*)m_DepthFrame.getDepthData());
sensor_msgs::ImagePtr l_DepthImageMsg = cv_bridge::CvImage(std_msgs::Header(), "mono16", l_depthImage).toImageMsg();
m_depthImagePublisher.publish(l_DepthImageMsg);
}
And my subscriber looks something like,
cv_bridge::CvImageConstPtr cv_ptr;
try
{
cv_ptr = cv_bridge::toCvShare(msg, sensor_msgs::image_encodings::TYPE_16UC1);
}
catch (cv_bridge::Exception& e)
{
ROS_ERROR("cv_bridge exception: %s", e.what());
return;
}
cv::imshow(OPENCV_WINDOW, cv_ptr->image);
But my issue is that all I can see is a dark screen, like totally black with some white spots on the top. How can I use cv_bridge to create a depth image from raw depth data? Thanks.
P.S. I used this another approach but somehow the frame rate seems slow and the depth image is too dull (darkish) to clearly see what is going on.
cv::Mat l_depthFrame(m_DepthFrame.getheight(), m_DepthFrame.getwidth(), CV_16UC1);
memcpy(l_depthFrame.data, m_DepthFrame.getDepthData(), m_DepthFrame.getheight() * m_DepthFrame.getwidth() * sizeof(uint16_t));
std_msgs::Header l_header;
l_header.stamp = ros::Time::now();
cv_bridge::CvImage l_depthImage;
l_depthImage = cv_bridge::CvImage(l_header, sensor_msgs::image_encodings::TYPE_16UC1, l_depthFrame);
sensor_msgs::Image l_depthImageMsg;
l_depthImage.toImageMsg(l_depthImageMsg);
m_depthFramePublisher.publish(l_depthImageMsg);
Also, would be nice to know to do the same for a RGB Image.