Processing an image outside the Callback function

Question

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(0), 2, 8, 0 );
          }
      }
   }
  /// Showing the result
  namedWindow( corners_window, CV_WINDOW_AUTOSIZE );
  imshow( corners_window, dst_norm_scaled );
   
  cv_ptr2->image =dst_norm_scaled;
  cv::imshow("cv_ptr2", cv_ptr2->image );
  cv::waitKey(3);

image_pub_.publish(cv_ptr2->toImageMsg());
 }
 };

 void test()
{
  cv::imshow("cv_ptr2", cv_ptr2->image );
  
}

int main(int argc, char** argv)
{
 ros::init(argc, argv, "image_converter");
 ImageConverter ic;

 ros::spin();
 
 return 0;
}

EDIT 1: After following Thomas's instructions (pseudo code), the process dies immediately after launching:

[hariss3-6] process has died [pid 28431, exit code -11, cmd /home/samme/fuerte_workspace/firefly_mv/bin/harris3 __name:=hariss3 __log:=/home/samme/.ros/log/0ad3107e-6752-11e2-b522-60672021cf54/hariss3-6.log].log file: /home/samme/.ros/log/0ad3107e-6752-11e2-b522-60672021cf54/hariss3-6*.log

Here is the new code:

class Node
{
public:
Node (ros::NodeHandle& nh, ros::NodeHandle& nhPrivate) : it_(nh_)
 // initialization list copy the arguments to the class attributes.
{
  nh_ = nh;
  nhPrivate_=nhPrivate;
 
   pub_ = it_.advertise("out", 1);
   sub_ = it_.subscribe("camera/image_raw", 1, &Node::subscriberCb,this);
  
  // create subscriber, publisher using nh_.
  }

 // always use shared pointer version of callbacks. sensor_msgs::ImageShPtr&
 //void subscriberCb (const boost::shared_ptr< sensor_msgs::ImagConstPtr >& image)
 void subscriberCb (const sensor_msgs::ImageConstPtr& image) 

 {
   imageIn_ = image;
  }

void process ()
{
   if(&imageIn_ == NULL)
    return;
   // Use imageIn as your input image.
   sensor_msgs::ImageConstPtr imageIn = imageIn_; // See note (1)
     try
{
  imageOut_ = cv_bridge::toCvCopy(imageIn, enc::BGR8);
}
catch (cv_bridge::Exception& e)
{
  ROS_ERROR("cv_bridge exception: %s", e.what());
  return;
}

   

   // 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 ();
    pub_.publish(imageOut_->toImageMsg());
}

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

 //CvImageConstPtr imageIn_;
 sensor_msgs::ImageConstPtr imageIn_ ;
 cv_bridge::CvImagePtr imageOut_;

 };

 class NodeWithGui : public Node
 {
public:
NodeWithGui(ros::NodeHandle& nh, ros::NodeHandle& nhPrivate) : Node (nh,nhPrivate){}
virtual void spinOnce ()
{
   Node::spinOnce ();
   // and refresh GUI using imshow.
    cv::imshow("Corners",imageOut_.image);
   }
 };

int main(int argc, char **argv)
{

 ros::init(argc, argv, "harris3");
 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 ();
  return 0;
   }

---

Originally posted by K_Yousif on ROS Answers with karma: 735 on 2013-01-23

Post score: 6

---

Original comments

Comment by DamienJadeDuff on 2013-01-23:
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?

Comment by DamienJadeDuff on 2013-01-23:
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.

Answer 1

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/scheduling using policy-based design for instance :)

---

Originally posted by Thomas with karma: 4478 on 2013-01-23

This answer was ACCEPTED on the original site

Post score: 12

---

Original comments

Comment by K_Yousif on 2013-01-24:
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.

Comment by K_Yousif on 2013-01-25:
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_ ?

Comment by K_Yousif on 2013-01-25:
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.

Comment by Thomas on 2013-01-26:
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)

Comment by K_Yousif on 2013-01-26:
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?

Comment by Thomas on 2013-01-26:
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 :)

Comment by K_Yousif on 2013-01-26:
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.

Comment by K_Yousif on 2013-01-26:
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.

Comment by K_Yousif on 2013-01-26:
Program received signal SIGSEGV, Segmentation fault. 0x00007ffff68e0f28 in std::basic_string<char, std::char_traits, std::allocator >::basic_string(std::string const&) () from /usr/lib/x86_64-linux-gnu/libstdc++.so.6

Comment by Thomas on 2013-01-27:
After looking at your code, you initialize the image transport object with the non-initialized nh_ attribute. I recommend also initializing nh_ and nhPrivate_ using the initialization list instead of doing it in the constructor body.

Comment by K_Yousif on 2013-01-27:
Hi Thomas, thanks a lot for taking the time to look at my code. I have managed to get it to work, I have the same problem though, when I move the camera fast it lags a lot and the screen becomes black for a second. However if i keep it steady or move it slow its fine.

Comment by K_Yousif on 2013-01-27:
It seems like an image encoding issue, when I change the encodings I get different results. For example if I change the dst_norm_scaled to CV_8SC1 instead of CV_32FC1 , It becomes very fast but the image is replicated 4 times.

Comment by Thomas on 2013-01-28:
Might it be an acquisition problem? if you do "rostopic hz /my_image_topic" you can see how fast you are acquiring data, then do the same thing on your output topic from your node and you'll know if you are real-time or not...

Comment by K_Yousif on 2013-01-28:
the image_raw is acquired at around 14.2 hz and the output is at around 12.5 hz. also I noted when moving the camera fast, the output says no new messages, although the camera_raw is continuously publishing at 14.2 hz.

Comment by K_Yousif on 2013-01-28:
I tried implementing sift and surf instead and they are very fast, It seems the problem is coming from the harris corner detector. I really appreciate your help and will mark this question as solved.

Comment by Pashek on 2019-07-01:
Though it's an old topic, but it's still relevant. The problem with this approach is that processing in a different thread with different loop rate adds latency comparing to processing in callback. Also, copying shared pointer (imageIn = imageIn_) and writing to it (imageIn_ = image) isn't thread-safe (link1, link2).

Comment by Thomas on 2019-07-11:
@Pashek the usual approach is through a lock-free ring buffer and making sure that when you push the ring-buffer, the other thread is already waiting for process something (using e.g. a condition variable). If you add latency in this world, it means you are receiving messages faster than you can process them and you should 1/ reduce the frequency at which messages are sent on the publisher side or 2/ if you can't reduce the frequency, drop some messages as needed (sub-sampling?). More than one worker thread would also be a solution, but that would significantly increase the complexity of the architecture...

Comment by iad-AR on 2021-03-23:
Am I missing something ? why virtual void spinOnce () calls himself ? This is a recursive call that never ends. (note : I am a beginner both in ros and C++)