Ask Your Question

Revision history [back]

click to hide/show revision 1
initial version

Hi, I know this is an old thread but since it cost me a few days to solve the problem and some others like Jluis619 are also looking for a quick solution here is my code and what I have figured out. Since I am also a beginner with ros please let me know if my solution is suboptimal in some regard.

This is the sensor plugin i have used in gazebo (openni kinect) to get the depth data

  <gazebo reference="camera_link">
<sensor type="depth" name="camera1">
<pose>0 0 0 0 0 0</pose>
  <update_rate>1.0</update_rate>
  <camera name="head">
    <horizontal_fov>1.3962634</horizontal_fov>
    <image>
      <width>640</width>
      <height>480</height>
      <format>R8G8B8</format>
    </image>
    <depth_camera>
      <width>640</width>
      <height>480</height>
      <format>L8</format>
       <!-- <output>points</output> -->
    </depth_camera>
    <clip>
      <near>0.02</near>
      <far>300</far>
    </clip>
    <noise>
      <type>gaussian</type>
      <!-- Noise is sampled independently per pixel on each frame.  
           That pixel's noise value is added to each of its color
           channels, which at that point lie in the range [0,1]. -->
      <mean>0.0</mean>
      <stddev>0.007</stddev>
    </noise>
  </camera>
    <plugin name="camera_controller" filename="libgazebo_ros_openni_kinect.so">
        <topicName>/kinect_sim/kinect</topicName>
        <baseline>0.2</baseline>
        <alwaysOn>true</alwaysOn>
        <updateRate>1.0</updateRate>
        <cameraName>kinect_sim/camera1</cameraName>
        <imageTopicName>rgb/image_raw</imageTopicName>
        <cameraInfoTopicName>rgb/camera_info</cameraInfoTopicName>
        <depthImageTopicName>depth/image_raw</depthImageTopicName>
        <depthImageInfoTopicName>depth/camera_info</depthImageInfoTopicName>
        <pointCloudTopicName>points</pointCloudTopicName>
        <frameName>camera_link</frameName>
        <pointCloudCutoff>0.5</pointCloudCutoff>
        <distortionK1>0.00000001</distortionK1>
        <distortionK2>0.00000001</distortionK2>
        <distortionK3>0.00000001</distortionK3>
        <distortionT1>0.00000001</distortionT1>
        <distortionT2>0.00000001</distortionT2>
        <CxPrime>0</CxPrime>
        <Cx>0</Cx>
        <Cy>0</Cy>
        <focalLength>0</focalLength>
        <hackBaseline>0</hackBaseline>
      </plugin>
</sensor>

</gazebo>

Afterwards I tried to read the depth data similar to Henschel.X but after "cv_image = self.bridge.imgmsg_to_cv2" a lot of values in the array are "nan" values while the real depth values are floats in the range of 0 - 5. These floats are rounded in a png image to the int values 0 - 5 hence the image is very dark and all the information which was encoded in the float values is discretized to the 6 values 0 - 5. Even if I tried to use cv2.normalize(depth_array, depth_array, 0, 1, cv2.NORM_MINMAX) cv2.imwrite('/home/andi/camera_depth.pgm', depth_array*255) The image got brighter but I had just the 6 discretized values and lost the most of the depth information. The solution for me was to replace the nan values with some numbers for example zeros or the maximal reach of the depth sensor. Here is the code which worked for me:

#!/usr/bin/env python
import roslib
roslib.load_manifest('kinect_pylistener')
import sys
import rospy
import cv2
from std_msgs.msg import String
from sensor_msgs.msg import Image
from cv_bridge import CvBridge, CvBridgeError
import time
import numpy as np
import message_filters

class image_converter:

  def __init__(self):
    self.bridge = CvBridge()
    image_sub = message_filters.Subscriber("/kinect_sim/camera1/rgb/image_raw", Image)
    depth_sub = message_filters.Subscriber("/kinect_sim/camera1/depth/image_raw", Image)
    self.ts = message_filters.ApproximateTimeSynchronizer([image_sub, depth_sub], 1, 1)
    self.ts.registerCallback(self.callback)
    print "test2"

  def callback(self,rgb_data, depth_data):
    try:
      image = self.bridge.imgmsg_to_cv2(rgb_data, "bgr8")
      depth_image = self.bridge.imgmsg_to_cv2(depth_data, "passthrough")
      depth_array = np.array(depth_image, dtype=np.float32)
      color_array = np.array(image, dtype=np.uint8)
      depth_array[np.isnan(depth_array)] = 0
      cv2.normalize(depth_array, depth_array, 0, 1, cv2.NORM_MINMAX)
      print "depth_array"
      print np.max(depth_array)
      print np.min(depth_array)
      print np.nanmax(depth_array)
      print np.nanmin(depth_array)
      cv2.imshow('Depth Image', depth_array)
      cv2.imshow('Depth Image', color_array)
      rospy.loginfo(image.shape)
      cv2.imwrite('/home/andi/camera_rgb.jpg', color_array)
      cv2.imwrite('/home/andi/camera_depth.pgm', depth_array*255)
      print "test3"
    except CvBridgeError as e:
      print(e)



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

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