Using image callback and videocallback OpenCV Kinetic

edit

Hi all! First question in the site!

Hope you guys can help me out, I've got a problem by trying to use an image inside a function. In this function I got an example of edge detector from the camera. I want to use SIFT Feature Detector but I can't manage to use the information of the image. I think it is because in some way I haven't made the callback properly.

I have a node that publishes image and in an other node I want to subscribe to that image and compare it with the video frame. The only way I got to read that image is putting the subscriber in the main function.

#include <ros/ros.h>
#include <iostream>
#include <sstream>
#include <math.h>
#include <string>
#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 <opencv2/core/types_c.h>
#include <opencv2/core/core_c.h>
#include <opencv2/xfeatures2d/nonfree.hpp>
#include <opencv2/features2d/features2d.hpp>

using namespace cv;
using namespace std;
using namespace xfeatures2d;

Mat train, train_g;
Mat train_desc;
vector<KeyPoint> train_kp;
static const std::string OPENCV_WINDOW = "Raw Image window";
static const std::string OPENCV_WINDOW_1 = "Edge Detection";
static const std::string OPENCV_WINDOW_2 = "TEST_G";

void imageCallback(const sensor_msgs::ImageConstPtr& msg) {
   try {

        train=cv_bridge::toCvShare(msg, "bgr8")->image;
    cvtColor(train,train_g,CV_BGR2GRAY);    
    cv::imshow("Original", train);
    cv::imshow("Gray", train_g);
    cv::waitKey(30);
 }
  catch (cv_bridge::Exception& e) {
    ROS_ERROR("Could not convert from '%s' to 'bgr8'.", msg->encoding.c_str());
  }
}

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

public:
 Edge_Detector():it_(nh_)
 {
   // Subscribe to input video feed and publish output video feed
   image_sub_ = it_.subscribe("/cv_camera/image_raw", 1, &Edge_Detector::imageCb, this); //usb_cam
   image_pub_ = it_.advertise("/edge_detector/raw_image", 1); ///edge_detector/raw_image
   //createTrackbars();
 }

 ~Edge_Detector()
 {
  cv::destroyWindow(OPENCV_WINDOW);
 }

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

   cv_bridge::CvImagePtr cv_ptr;
   namespace enc = sensor_msgs::image_encodings;

   try
       {
       cv_ptr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::BGR8);
       }
       catch (cv_bridge::Exception& e)
       {
       ROS_ERROR("cv_bridge exception: %s", e.what());
       return;
       }

// Draw an example circle on the video stream
//if (cv_ptr->image.rows > 400 && cv_ptr->image.cols > 600){

detect_edges(cv_ptr->image); // sth tells me here is the problem, I'm not caling the image but I have tried many things and still doesn't work.
    image_pub_.publish(cv_ptr->toImageMsg());

//}
    }


 void detect_edges(cv::Mat img) {

     cv::Mat src, src_gray;
 cv::Mat dst, detected_edges;

     int edgeThresh = 1;
 int lowThreshold = 200;
 int highThreshold =300;
 int kernel_size = 5;

 img.copyTo(src);

 cv::cvtColor( img, src_gray, CV_BGR2GRAY );
     cv::blur( src_gray, detected_edges, cv::Size(5,5) );
 cv::Canny( detected_edges, detected_edges, lowThreshold, highThreshold, kernel_size );

 dst = cv::Scalar::all(0);
 img.copyTo( dst, detected_edges);
 dst.copyTo(img);

     Mat test, test_g;

    cv::cvtColor(img, test_g, CV_BGR2GRAY);


 //detect SIFT keypoints and extract descriptors in the test image
vector<KeyPoint> test_kp;
Mat test_desc;
Ptr<SiftFeatureDetector> featureDetector=SiftFeatureDetector::create();//SiftFeatureDetector featureDetector;
featureDetector->detect(test_g, test_kp);//featureDetector.detect(test_g, test_kp);
Ptr<SiftDescriptorExtractor> featureExtractor=SiftDescriptorExtractor::create(); //SiftDescriptorExtractor featureExtractor;
featureExtractor->compute(test_g, test_kp, test_desc);//featureExtractor.compute(test_g, test_kp, test_desc ...