Gazebo | Ignition | Community
Ask Your Question

Revision history [back]

How to capture a point cloud from deph sensor

Hello everyone.

I am trying to simulate a rgbd camera. I can manage to capture pictures from the camera, but not from the depth sensor.

Basically, this is the important part of the sdf file:

<?xml version="1.0"?>
<sdf version="1.6">
  <model name="realsense">
    <link name='cameraLink'>
        <sensor name='depth' type='depth'>
        <topic>realsense</topic>
        <always_on>true</always_on>
        <camera name='d435i_depth'>
          <horizontal_fov>1.01229</horizontal_fov>
          <image>
            <width>1920</width>
            <height>1080</height>
          </image>
          <depth_camera>
            <clip>
              <near>0.3</near>
              <far>3.0</far>
            </clip>
          </depth_camera>
        </camera>
      </sensor>
      <sensor name='color' type='camera'>
        <update_rate>10</update_rate>
        <topic>realsense</topic>
        <always_on>true</always_on>
        <camera name='d435i'>
          <horizontal_fov>1.01229</horizontal_fov>
          <image>
            <width>1920</width>
            <height>1080</height>
          </image>
        </camera>
      </sensor>
    </link>
  </model>
</sdf>

It publishes information to two gazebo topics, one from the camera and another one from the depth sensor. Both topics are of type gazebo.msgs.ImageStamped. I am subscribed to both topics in the same way:

    int width = static_cast<int>(msg->image().width());
    int height = static_cast<int>(msg->image().height());
    char *data;
    data = new char[msg->image().data().length() + 1];

    memcpy(data, msg->image().data().c_str(), msg->image().data().length());
    cv::Mat image(height, width, CV_8UC3, data);

    cv::resizeWindow("image", height, width);
    cv::imshow("image", image);
    cv::waitKey(10);
    lastPicture_ = image;
    delete data;

However, when I show the pictures, something is wrong with the depth image. Here it is the camera picture:

image description

And here, the depth image:

image description

It has to be a problem of interpretation, because with the Gazebo GUI ("Topic visualization"), it is possible to see the depth image correctly:

image description

Someone knows how to solve it? Many thanks in advance.