Home | Tutorials | Wiki | Issues
Ask Your Question
0

How to convert Gazebo's ConstImageStamped to cv::Mat?

asked 2016-10-18 16:48:19 -0500

chutsu gravatar image

updated 2016-10-19 10:19:58 -0500

I'm having difficulty finding information on how to convert the Gazebo Image class to OpenCV's own cv::Mat. Was wondering if someone has a code snippet to help me out a little bit here.

I should mention that I have no intension of using ROS.

More Information: I have a robot with a camera attached, using the standard camera model provided in Gazebo, it publishes a camera topic, you can then write a Gazebo subscriber to subscribe to the topic and obtain the image data. However! most developers who work in OpenCV need that image data to be in cv::Mat. This question is how to convert Gazebo Image data to OpenCV's cv::Mat.

edit retag flag offensive close merge delete

1 Answer

Sort by ยป oldest newest most voted
1

answered 2016-10-19 10:22:54 -0500

chutsu gravatar image

updated 2016-11-09 09:57:24 -0500

I finally found the solution, the following assumes your sensor in Gazebo outputs RGB data, the code to subscribe to the image topic and convert the Gazebo Image data to OpenCV cv::Mat is as follows:

#include <string>
#include <vector>

#include <gazebo/gazebo_config.h>
#include <gazebo/gazebo_client.hh>
#include <gazebo/msgs/msgs.hh>
#include <gazebo/transport/transport.hh>

#include <opencv2/opencv.hpp>
#include <opencv2/core/core.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/features2d/features2d.hpp>

#define IMAGE_TOPIC "~/quadrotor/camera/camera/link/camera/image"


void cb(ConstImageStampedPtr &msg)
{
    // std::cout << msg->image().width() << std::endl;
    // std::cout << msg->image().height() << std::endl;
    // std::cout << msg->image().pixel_format() << std::endl;
    // std::cout << std::endl;

    int width;
    int height;
    char *data;

    width = (int) msg->image().width();
    height = (int) msg->image().height();
    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::imshow("camera", image);
    cv::waitKey(1);
    delete data;  // DO NOT FORGET TO DELETE THIS, 
                  // ELSE GAZEBO WILL TAKE ALL YOUR MEMORY
}

int main(int argc, char **argv)
{
    gazebo::client::setup(argc, argv);
    gazebo::transport::NodePtr node(new gazebo::transport::Node());
    gazebo::transport::SubscriberPtr sub;

    // setup
    node->Init();
    sub = node->Subscribe(IMAGE_TOPIC, cb);

    // loop
    while (true) {
        gazebo::common::Time::MSleep(10);
    }

    // clean up
    gazebo::client::shutdown();

    return 0;
}

Of particular interest is the four lines here:

    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);

To convert the data, we first allocate memory for the data variable (+1 for null character at the end). We then copy the image message data from msg->image().data().c_str() to data. Then we use the information about the height and width of the image to form the cv::Mat. Don't forget to delete data with delete, else you're introducing memory leaks.

edit flag offensive delete link more
Login/Signup to Answer

Question Tools

1 follower

Stats

Asked: 2016-10-18 16:48:19 -0500

Seen: 836 times

Last updated: Nov 09 '16