ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange
Ask Your Question
0

How to store the depth data from kinect(/camera/depth_registrered/image_raw) as gray scale image?

asked 2013-03-21 20:07:38 -0500

updated 2016-10-24 08:59:22 -0500

ngrennan gravatar image

I want to get the depth data from kinect by receiving sensor_msgs/Image from topic /camera/depth_registrered/image_raw(reference), I use cv_bridge to store the data to a gray scale image.

I use this snippet of code to receive sensor_msgs/Image from topic /camera/depth_registrered/image_raw, change it to gray scale image by cvBridge, and then store the image to a jpeg file.

#!/usr/bin/env python
import roslib
roslib.load_manifest('store_stereo_image')
import sys 
import rospy
import cv
from std_msgs.msg import String
from sensor_msgs.msg import Image
from cv_bridge import CvBridge, CvBridgeError

class image_converter:

  def __init__(self):
    self.bridge = CvBridge()
    self.image_sub = rospy.Subscriber("/camera/depth_registered/image_raw",Image,self.callback)

  def callback(self,data):
    try:
        cv_image = self.bridge.imgmsg_to_cv(data, "mono8")
        cv.SaveImage("depth_camera_msg.jpg", cv_image)
        print "image saved!"
    except CvBridgeError, e:
      print e

def main(args):
  ic = image_converter()
  rospy.init_node('image_converter', anonymous=True)
  try:
    rospy.spin()
  except KeyboardInterrupt:
    print "Shutting down"

if __name__ == '__main__':
    main(sys.argv)

This is the jpeg file I stored, I suppose the value in this picture should vary from 0 to 255. However, I found the values in this picture are either 0 or 255(only black and white).

image description

I use rostopic echo /camera/depth_registered/image_raw to check, I found that the data it published vary from 0~255, but I don't know why the image is only black and white~

Did I miss something? Thanks in advance.

---EDIT---

Thanks to DamienJadeDuff's comment, I learned that the raw data from msg sensor_msgs/Image is uint8, and the data contained in /camera/depth_registered/image_raw is uint16 depths in mm. Does this means depth data is retrieved from combining two uint8 data?

edit retag flag offensive close merge delete

Comments

From memory, the depth image is a 32 bit float. Perhaps the rostopic echo is only showing the byte level data?

DamienJadeDuff gravatar image DamienJadeDuff  ( 2013-03-22 03:29:11 -0500 )edit

To respond to your edit, Ricky; sorry I got the encoding wrong for your message (depth_registered/image_raw), but why don't you use depth_registered/image instead? See http://www.ros.org/wiki/depth_image_proc --- then you can use jelfring's code for image type 32FC1.

DamienJadeDuff gravatar image DamienJadeDuff  ( 2013-03-31 22:28:47 -0500 )edit

Also, yes, check the encoding field in the message; that gives a cue as to how the data should be interpreted. But you should be able to use the cv_bridge library to interpret the message. See http://www.ros.org/wiki/cv_bridge

DamienJadeDuff gravatar image DamienJadeDuff  ( 2013-03-31 22:31:13 -0500 )edit

4 Answers

Sort by ยป oldest newest most voted
2

answered 2013-03-21 23:16:39 -0500

jelfring gravatar image

Hi,

We have made a tool to store camera data from a bag file, e.g., depth images, as a video sequence. Part of the procedure can help you I guess. The relevant file can be found here:

ros.org/doc/electric/api/rosbag_video/html/bag__to__video_8cpp_source.html (lines 95-125).

Jos

edit flag offensive delete link more

Comments

@jelfring: I've met the same problem with Ricky. Thanks for the code you provided, it solves the problem. But why is that we need to do the data conversion by ourselves? The function toCvCopy() should be able to convert the encoding internally. What's wrong when using toCvCopy(msg, mono8)

Albert K gravatar image Albert K  ( 2013-04-13 18:37:14 -0500 )edit

By the way, I use toCvCopy(msg, mono8) and try to convert float32 to int8 and fialed. That's a similar condition to Ricky's.

Albert K gravatar image Albert K  ( 2013-04-13 18:38:59 -0500 )edit
5

answered 2014-05-13 03:50:38 -0500

E1000ii gravatar image

updated 2014-05-13 03:51:51 -0500

I now is an old thread. But just for the others that need to store a depth image (from camera or from rosbag) to a png file. This is what you should do on the depth image subscription callback or in any place you want to perform this operation with python:

def callback(self, data):
    try:
        # The depth image is a single-channel float32 image
        # the values is the distance in mm in z axis
        depth_image = self.bridge.imgmsg_to_cv(data, '32FC1')
        # Convert the depth image to a Numpy array since most cv2 functions
        # require Numpy arrays.
        depth_array = np.array(depth_image, dtype=np.float32)
        # Normalize the depth image to fall between 0 (black) and 1 (white)
        cv2.normalize(depth_array, depth_array, 0, 1, cv2.NORM_MINMAX)
        # At this point you can display the result properly:
        # cv2.imshow('Depth Image', depth_display_image)
        # If you write it as it si, the result will be a image with only 0 to 1 values.
        # To actually store in a this a image like the one we are showing its needed
        # to reescale the otuput to 255 gray scale.
        cv2.imwrite('capture_depth.png',frame*255)
    except CvBridgeError, e:
        print e

I'm using at the moment ubuntu 12.04 and Ros Hydro

edit flag offensive delete link more

Comments

What would be the case if I had a compressedDepth image instead?

Pototo gravatar image Pototo  ( 2016-12-08 19:59:08 -0500 )edit
0

answered 2016-12-26 19:07:12 -0500

taka084 gravatar image
depth_image = self.bridge.imgmsg_to_cv(data, '32FC1')

you should change the code to

depth_image = self.bridge.imgmsg_to_cv(data, "passthrough")

then, you'll get depth_image as float32.

(no longer transform using np.array is not needed.)

edit flag offensive delete link more
0

answered 2021-11-16 03:09:18 -0500

Here you can find a short code to store depth data into CV image:

unsigned int depthSamples = msg->image().width() * msg->image().height();
float f;
// cppchecker recommends using sizeof(varname)
unsigned int depthBufferSize = depthSamples * sizeof(f);

float *depthBuffer = new float[depthSamples];
uint16_t *imageBufferHalf = new uint16_t[depthSamples];

memcpy(depthBuffer, msg->image().data().c_str(), depthBufferSize);

float maxDepth = 0;
for (unsigned int i = 0; i < msg->image().height() * msg->image().width(); ++i) {
    if (depthBuffer[i] > maxDepth && !std::isinf(depthBuffer[i])) {
        maxDepth = depthBuffer[i];
    }
}

cv::Mat image(msg->image().height(), msg->image().width(), CV_8UC3);
unsigned int idx = 0;
double factor = 255 / maxDepth;
for (unsigned int j = 0; j < msg->image().height(); ++j) {
  for (unsigned int i = 0; i < msg->image().width(); ++i) {
    float d = depthBuffer[idx++];
    if (d > maxDepth)
        d = maxDepth;
    d = 255 - (d * factor);
    cv::Vec3b color;
    color[0] = d;
    color[1] = d;
    color[2] = d;
    image.at<cv::Vec3b>(cv::Point(i, j)) = color;
  }
}
edit flag offensive delete link more

Question Tools

1 follower

Stats

Asked: 2013-03-21 20:07:38 -0500

Seen: 10,939 times

Last updated: May 13 '14