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.