0
$\begingroup$

Rosanswers logo

I try to use the code below to save one frame of depth iamge from topic /camera/depth/image_raw, but the problem is I could only get a image looks black. When I read it in Matlab, the matrix elements are basically from number 1 to 7, I guess that's why it looks black. But when I rosrun the image_view (camera/depth/image_raw), it looks just fine. I guess probably it's the problem of encoding, can someone guide me what to do to save image just like the image_view showed to me. plz, thank you!

from __future__ import print_function
import sys
import rospy
import cv2
import cv
from std_msgs.msg import String
from sensor_msgs.msg import Image
from cv_bridge import CvBridge, CvBridgeError

class TakePhoto:
    def __init__(self):

        self.bridge = CvBridge()
        self.image_received = False

        # Connect image topic
        img_topic = "/camera/depth/image_raw"
        self.image_sub = rospy.Subscriber(img_topic, Image, self.callback)

        # Allow up to one second to connection
        rospy.sleep(1)

    def callback(self, data):

        # Convert image to OpenCV format
        try:
            cv_image = self.bridge.imgmsg_to_cv2(data,'16UC1')
        except CvBridgeError as e:
            print(e)

        self.image_received = True
        self.image = cv_image

    def take_picture(self, img_title):
        if self.image_received:
            # Save an image
            cv2.imwrite(img_title, self.image)
            return True
        else:
            return False

if __name__ == '__main__':

    # Initialize
    rospy.init_node('take_photo', anonymous=False)
    camera = TakePhoto()

    # Take a photo

    # Use '_image_title' parameter from command line
    # Default value is 'photo.jpg'
    img_title = rospy.get_param('~image_title', 'photo16U.jpg')

    if camera.take_picture(img_title):
        rospy.loginfo("Saved image " + img_title)
    else:
        rospy.loginfo("No images received")

    # Sleep to give the last log messages time to be sent
    rospy.sleep(1)

Originally posted by Henschel.X on ROS Answers with karma: 62 on 2016-03-23

Post score: 2


Original comments

Comment by Mehdi. on 2016-03-24:
when you divide the image by 7.0 and multiply by 255, can you see something in Matlab?

Comment by Henschel.X on 2016-03-24:
yes, but because those numbers from 1 to 7 are all int. so when I see it on Matlab it lacks detail.

Comment by Jluis619 on 2016-06-06:
is this the full code? If not could you give me please, I want to work with images in depth but I find somewhat complicated to start since 0. thank you :)

Comment by wy3 on 2017-06-26:
I run into the same problem. The difference is that I cannot even visualize the depth image using image_view. Have you run into this before?

$\endgroup$
1
  • $\begingroup$ The code above provided by AndyMunich is the best answer to solve this problem I have seen. Thank Andy !! $\endgroup$
    – Liu Jiayu
    Dec 21, 2023 at 2:56

2 Answers 2

0
$\begingroup$

Rosanswers logo

I faced the same problem and I was breaking my head over it. However, I recently came up with a solution that solves it. I looked at the source code of both nodes, image_view and image_saver. Looks like there is an explicit conversion that is implemented in image_view, but the same is not added in image_saver. So I cloned the source code from git (link) and made the following modification in the image_saver code. After this you can save exactly what you see in image_view

bool saveImage(const sensor_msgs::ImageConstPtr& image_msg, std::string &filename) {
cv::Mat image;
cv_bridge::CvImageConstPtr cv_ptr;
cv_bridge::CvtColorForDisplayOptions options;
options.bg_label=-1;
options.colormap=-1;
options.do_dynamic_scaling=0;
options.min_image_value=0;
options.max_image_value=10;
try
{
  image = cv_bridge::toCvShare(image_msg, encoding)->image;
} catch(cv_bridge::Exception)
{
   cv_ptr = cv_bridge::cvtColorForDisplay(cv_bridge::toCvShare(image_msg), "", options);
   image=cv_ptr->image;
  // ROS_ERROR("Unable to convert %s image to %s", image_msg->encoding.c_str(), encoding.c_str());
  // return false;
} catch(cv_bridge::Exception)
{
  
  ROS_ERROR("Unable to convert %s image to %s", image_msg->encoding.c_str(), encoding.c_str());
  return false;
}

if (!image.empty()) {
  try {
    filename = (g_format).str();
  } catch (...) { g_format.clear(); }
  try {
    filename = (g_format % count_).str();
  } catch (...) { g_format.clear(); }
  try { 
    filename = (g_format % count_ % "jpg").str();
  } catch (...) { g_format.clear(); }

  if ( save_all_image || save_image_service ) {
    cv::imwrite(filename, image);
    ROS_INFO("Saved image %s", filename.c_str());

    save_image_service = false;
  } else {
    return false;
  }
} else {
  ROS_WARN("Couldn't save image, no data!");
  return false;
}
return true;

}


Originally posted by Abhi-venkat with karma: 26 on 2017-08-02

This answer was ACCEPTED on the original site

Post score: 1


Original comments

Comment by fredli on 2017-12-22:
Hi, can you explain the parameters you set? How can I get the specific depth value(in meters) after the scaling? thx

$\endgroup$
0
$\begingroup$

Rosanswers logo

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>

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)

Originally posted by AndyMunich with karma: 11 on 2016-09-02

This answer was NOT ACCEPTED on the original site

Post score: 1


Original comments

Comment by wy3 on 2017-06-27:
Hi Andy thanks for your answer. I also run into the nan with cv_bridge. I then decrease the pointcloudcutoff from 0.5 to a lower value and the nan values are filled since there are stuff within 0.5m. Somehow that parameter for point cloud also affects the valid range of depth image if I'm correct

$\endgroup$

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.