Ask Your Question
5

Processing an image outside the Callback function

asked 2013-01-23 18:51:43 -0500

updated 2013-01-26 16:34:49 -0500

Hi guys,

The answer to this question is probably trivial, but I have looked around and cannot find the answer I am looking for. I have written a simple Harris corner detector algorithm in which I receive an image from the camera/image_raw topic and then inside the Callback function the image is converted to a open cv type image (I call it cv_ptr) using the cv::bridge, and the processing of detecting the corner is all done inside the callback function. It works but is very slow (lag and a black screen for a couple of seconds) when the camera movement is fast. I am assuming this is because I have my processing in the callback function?

I have read in other posts that doing processing inside the callback function is not the correct way and that processing should be in the main loop. Which leeds to my question, how can I access cv_ptr (or better the sensor_msgs::ImageConstPtr& msg passed to the callback function) in the main process since callbacks are void functions and do not return anything? Note: I have tried making cv_ptr a global variable and then access it outside the callback function, although there is no error in compiling it, the process dies immediately) I am sure there is a better way to access messages passed to the callback function.

What am I missing? I have posted my code for clarification. Thanks alot for your help.

#include <ros/ros.h>
#include <image_transport/image_transport.h>
#include <cv_bridge/cv_bridge.h>
#include <sensor_msgs/image_encodings.h>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <iostream>
#include <stdio.h>
#include <stdlib.h>

using namespace cv;
using namespace std;
namespace enc = sensor_msgs::image_encodings;

static const char WINDOW[] = "Image window";
static const char WINDOWDST[] = "DST window";

/// Global variables
Mat src, src_gray, src_gray_corner;
int thresh = 200;
int max_thresh = 255;


char* source_window = "Source image";
char* corners_window = "Corners detected";

/// Function header
void cornerHarris_demo( int, void* );

/** @function cornerHarris_demo */


class ImageConverter
{
 ros::NodeHandle nh_;
 image_transport::ImageTransport it_;
 image_transport::Subscriber image_sub_;
 image_transport::Publisher image_pub_;

public:
  ImageConverter()
   : it_(nh_)
 {
  image_pub_ = it_.advertise("out", 1);
  image_sub_ = it_.subscribe("camera/image_raw", 1, &ImageConverter::imageCb,this);

  cv::namedWindow(WINDOW);
  }

 ~ImageConverter()
 {
  cv::destroyWindow(WINDOW);
 }

 void imageCb(const sensor_msgs::ImageConstPtr& msg)
{



cv_bridge::CvImagePtr cv_ptr;
cv_bridge::CvImagePtr cv_ptr2;


/// Detector parameters
int blockSize = 2;
int apertureSize = 3;
double k = .04;
try
{
  cv_ptr = cv_bridge::toCvCopy(msg, enc::BGR8);
  cv_ptr2 = cv_bridge::toCvCopy(msg, enc::BGR8);
}
catch (cv_bridge::Exception& e)
{
  ROS_ERROR("cv_bridge exception: %s", e.what());
  return;
}


Mat dst, dst_norm, dst_norm_scaled;


cv::imshow(WINDOW, cv_ptr->image);

cvtColor( cv_ptr->image, src_gray, CV_BGR2GRAY );

dst = Mat::zeros( cv_ptr->image.size(), CV_32FC1);

 /// Detecting corners
 cornerHarris( src_gray, dst, blockSize, apertureSize, k, BORDER_DEFAULT );
 cv::imshow(WINDOWDST, dst);


 // Normalizing
 normalize( dst,dst_norm, 0, 255, NORM_MINMAX, CV_32FC1, Mat() );
 convertScaleAbs( dst_norm, dst_norm_scaled );
 cv::imshow(WINDOWDST, dst_norm_scaled );
 cv::waitKey(1);

 /// Drawing a circle around corners
 for( int j = 0; j < dst_norm.rows ; j++ )
  { for( int i = 0; i < dst_norm.cols; i++ )
      {
        if( (int) dst_norm.at<float>(j,i) > thresh )
          {
           circle( dst_norm_scaled, Point( i, j ), 5,  Scalar ...
(more)
edit retag flag offensive close merge delete

Comments

waitKey is the function that draws to screen, so yes if there is too much time between calls to that then the OpenCV output will not look very good. How did you attempt to process cv_ptr outside the callback? Were you able to debug the dead process? It couldn't've been a null pointer issue?

DamienJadeDuff gravatar imageDamienJadeDuff ( 2013-01-23 19:39:43 -0500 )edit

You will probably need to do the image processing outside the main thread so that you can call waitKey to draw to screen from the main thread. See e.g. http://www.ros.org/wiki/roscpp/Overview/Callbacks%20and%20Spinning#Multi-threaded_Spinning - also to avoid weird errors, use a mutex.

DamienJadeDuff gravatar imageDamienJadeDuff ( 2013-01-23 19:42:47 -0500 )edit

2 Answers

Sort by ยป oldest newest most voted
10

answered 2013-01-23 20:26:26 -0500

Thomas gravatar image

updated 2013-01-26 00:49:51 -0500

Here is how I would refactor this code (in pseudo-C++)

class Node
{
    Node (ros::NodeHandle& nh, ros::NodeHandle& nhPrivate)
     // initialization list copy the arguments to the class attributes.
    {
      // create subscriber, publisher using nh_.
    }

    // always use shared pointer version of callbacks.
    subscriberCb (const sensor_msgs::ImageConstPtr& image) 
    {
       imageIn_ = image;
    }

    void process ()
    {
       // Use imageIn as your input image.
       sensor_msgs::ImageConstPtr imageIn = imageIn_; // See note (1)

       // No new images since last time, do nothing.
       if (imageOut_.header.seq == imageIn.header.seq)
         return;

       // use imageOut_.image and images which are *class attributes*
       // to realize the image processing, so it will allocate memory only once.
       // You should have *no* instance of big objects as local variables here.
       // Use directly imageOut_.image as the cv::Mat destination object  of your last
       // OpenCV function call.

       // publish imageOut_.toMsg ();
    }

    virtual void spin ()
    {
      ros::Rate rate (30);
      while (ros::ok ())
      {
        spinOnce ();
        rate.sleep ();
       }
    }

    virtual void spinOnce ()
    {
       process ();
       spinOnce ();
    }


protected:
  image_transport::ImageTransport it_;
  image_transport::Subscriber sub_;
  image_transport::Publisher pub_;
  ros::NodeHandle nh_;
  ros::NodeHandle nhPrivate_;

  sensor_msgs::ImageConstPtr imageIn_;
  cv_bridge::CvImage imageOut_;
};

class NodeWithGui : public Node
{
    virtual void spinOnce ()
    {
       Node::spinOnce ();
       // and refresh GUI using imshow.
    }
};

int main ()
{
  ros::init (...);
  ros::NodeHandle nh;
  ros::NodeHandle nhPrivate ("~");

  Node* node = 0;
  if (ros::param::get<bool>("~use_gui"))
   node = new NodeWithGui (nh, nhPrivate);
  else
    node = new Node (nh, nhPrivate);
  node->spin ();
}

So the reason is not that you are doing your computation in the callback, the reason is that you reallocate at each callback all your objects!

To have an efficient node, allocation should be done once. At the beginning (here when the first image is received). Then, old structures should be reused.

Additional introduced features are:

  • Pass the node handle and private node handle explicitly. It will help you convert your node to a nodelet one day (see nodelet conversion tutorial).
  • ALWAYS separate algorithms from display (GUI). Especially in robotics, robots should not embed X11 server.
  • No global variables.

Also, you can see that this code is much easier to benchmark as different piece of codes are separated. You can, for instance, bench how fast process is very easily. Never use waitKey to get a "sensation" of how fast it is going. This is bad practice even if it is used in the cv_bridge tutorial. Do not use waitkey in your code. Use Timers to publish statistics through the ros logging api (rosconsole) about processing speed if you want to.

Note (1): you may notice that before processing the data, I copy the shared pointer. This is to ensure that, if you use an ASyncSpinner, your image won't change in the middle of the processing. Shared pointers copies are atomic operations which means that this callback is thread-safe. This is required to convert this node to a nodelet for instance and also to deal with more complex GUI that may require you to use the ASyncSpinner. Note that no mutex are needed here, even in this case.

Bonus points for advanced C++ developpers: template the Node class to realize different kind of spinning ... (more)

edit flag offensive delete link more

Comments

1

Thank you Thomas for your detailed answer. I do not have the equipment with me at the moment but I will try to implement this and follow your description as soon as possible and will post back with the result.

K_Yousif gravatar imageK_Yousif ( 2013-01-24 02:12:04 -0500 )edit

Hello Thomas, I have tried implementing your pseudo code but my process is dying immediately after launch, I have edited the question and posted the new code (see Edit 1). I am assuming that by CvImageConstPtr imageIn_ you mean cv_bridge::CvImageConstPtr imageIn_ ?

K_Yousif gravatar imageK_Yousif ( 2013-01-25 18:32:31 -0500 )edit

Also the type sensor_msgs::ImageShPtr& image does not seem to exist.. How do you properly define the shared pointer? It is only accepting sensor_msgs::ImageConstPtr or otherwise it will give me an error.

K_Yousif gravatar imageK_Yousif ( 2013-01-25 18:36:16 -0500 )edit

Well yes it does not compile, that is why this is pseudo-code :) The input image (imageIn_) is a sensor_msgs::ImageConstPtr to avoid having to do the conversion in the callback. What do you mean by die immediately? (I edited my answer to fix the type)

Thomas gravatar imageThomas ( 2013-01-26 00:51:55 -0500 )edit

I set the imageIn_ type to sensor_msgs::ImageConstPtr and I also had to change the cv_bridge::CvImage imageOut_ to the type cv_bridge::CvImagePtr (I have updated the code above). The code compiles with no errors but when I rosrun the node I get a segmentation fault (core dumped) ? Any idea why?

K_Yousif gravatar imageK_Yousif ( 2013-01-26 01:51:14 -0500 )edit

You also have to check at the beginning of process that the shared pointer is not null. If you still have an issue, run the code in gdb to see where the problem is :)

Thomas gravatar imageThomas ( 2013-01-26 03:29:41 -0500 )edit

Hi Thomas, at the beginning of the process() function I added this: if ( &imageIn_ == NULL) return;

is that the correct way of checking if it is a NULL pointer? after adding this condition and running, I still get the segmentation fault.

K_Yousif gravatar imageK_Yousif ( 2013-01-26 16:17:37 -0500 )edit

I debugged it with gdp, it crashes as soon as it enters the Node constructor: Breakpoint 7, Node::Node (this=0x623a40, nh=..., nhPrivate=...) at ~/fuerte_workspace/firefly_mv/src/harris3.cpp:20 20 Node (ros::NodeHandle& nh, ros::NodeHandle& nhPrivate) : it_(nh_) (gdb) continue Continuing.

K_Yousif gravatar imageK_Yousif ( 2013-01-26 18:27:12 -0500 )edit
-2

answered 2014-03-20 17:39:16 -0500

thank you for sharing the above codes, i wish i could be more helpful by providing the following two image processing methods in different platforms. process image in asp.net process image in winforms

edit flag offensive delete link more

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

2 followers

Stats

Asked: 2013-01-23 18:51:43 -0500

Seen: 4,579 times

Last updated: Mar 20 '14