Using depth_image_proc with different data. [closed]

edit

Hi,

I am attempting to use my own rgb and depth images in depth_image_proc instead of those provided by openni.launch. What I want to do is to perform some color segmentation before building a point cloud. By following some examples here's what I've come up with:

#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>


namespace enc = sensor_msgs::image_encodings;

using namespace cv;
using namespace std;

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

class ImageConverter
{
  ros::NodeHandle nh_;
  image_transport::ImageTransport it_;
  image_transport::Subscriber image_sub_, depth_sub_;
  image_transport::Publisher image_pub_, depth_pub_;
  Mat dst;//=Mat::zeros(cv_ptr->image.size(), cv_ptr->image.type());

public:
  ImageConverter()
    : it_(nh_)
  {
    image_pub_ = it_.advertise("out", 1);
    depth_pub_=it_.advertise("depth_filtered",1);
    image_sub_ = it_.subscribe("/camera/rgb/image_color", 1, &ImageConverter::imageCb, this);
    depth_sub_ = it_.subscribe("/camera/depth/image_raw", 1, &ImageConverter::depthCb, this),
    cv::namedWindow(WINDOW);
    dst=Mat::zeros(480, 640, 16);
  }

  ~ImageConverter()
  {
    cv::destroyWindow(WINDOW);
  }
  string getImgType(int imgTypeInt)
{
    int numImgTypes = 35; // 7 base types, with five channel options each (none or C1, ..., C4)

    int enum_ints[] =       {CV_8U,  CV_8UC1,  CV_8UC2,  CV_8UC3,  CV_8UC4,
                             CV_8S,  CV_8SC1,  CV_8SC2,  CV_8SC3,  CV_8SC4,
                             CV_16U, CV_8UC1,  CV_8UC2,  CV_8UC3,  CV_8UC4,
                             CV_16S, CV_16SC1, CV_16SC2, CV_16SC3, CV_16SC4,
                             CV_32S, CV_32SC1, CV_32SC2, CV_32SC3, CV_32SC4,
                             CV_32F, CV_32FC1, CV_32FC2, CV_32FC3, CV_32FC4,
                             CV_64F, CV_64FC1, CV_64FC2, CV_64FC3, CV_64FC4};

    string enum_strings[] = {"CV_8U",  "CV_8UC1",  "CV_8UC2",  "CV_8UC3",  "CV_8UC4",
                             "CV_8S",  "CV_8SC1",  "CV_8SC2",  "CV_8SC3",  "CV_8SC4",
                             "CV_16U", "CV_8UC1",  "CV_8UC2",  "CV_8UC3",  "CV_8UC4",
                             "CV_16S", "CV_16SC1", "CV_16SC2", "CV_16SC3", "CV_16SC4",
                             "CV_32S", "CV_32SC1", "CV_32SC2", "CV_32SC3", "CV_32SC4",
                             "CV_32F"  "CV_32FC1", "CV_32FC2", "CV_32FC3", "CV_32FC4",
                             "CV_64F", "CV_64FC1", "CV_64FC2", "CV_64FC3", "CV_64FC4"};

    for(int i=0; i<numImgTypes; i++)
    {
        if(imgTypeInt == enum_ints[i]) return enum_strings[i];
    }
    return "unknown image type";
}

  void imageCb(const sensor_msgs::ImageConstPtr& msg)
  {
    cv_bridge::CvImagePtr cv_ptr;
    try
    {
      cv_ptr = cv_bridge::toCvCopy(msg, enc::BGR8);
    }
    catch (cv_bridge::Exception& e)
    {
      ROS_ERROR("cv_bridge exception: %s", e.what());
      return;
    }

   Mat hsv;
   Mat src=cv_ptr->image;
    cv::cvtColor(cv_ptr->image, hsv, CV_BGR2HSV);

    Mat bw;
    ROS_INFO("Creating mask with color segmentation");
        cv::inRange(hsv, Scalar(60,100, 50), Scalar(86,170, 255), bw);


    vector<vector<Point> > contours;
    Mat smthg=bw.clone();
    ROS_INFO("Finding countours to be filled");
    cv::findContours(smthg, contours, CV_RETR_EXTERNAL, CV_CHAIN_APPROX_SIMPLE);

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

    cv::drawContours(dst, contours, -1, Scalar::all(255), CV_FILLED);

    Mat result=dst & src;

    cv::imshow("mask", bw);


    cv::imshow("rgb_filtered", result);



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

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

void depthCb(const sensor_msgs::ImageConstPtr& msg)
  {
    cv_bridge::CvImageConstPtr cv_ptr;
    try
    {
      cv_ptr = cv_bridge::toCvShare(msg, enc::TYPE_16UC1);
    }
    catch (cv_bridge::Exception& e)
    {
      ROS_ERROR("cv_bridge exception: %s", e.what());
      return;
    }

     ROS_INFO("Applying mask to depth image");
   Mat m=Mat(cv_ptr->image.size(), cv_ptr->image.type());


   cvtColor(dst,m,CV_RGB2GRAY);


   Mat result=Mat(cv_ptr->image.size(), cv_ptr->image.type());
   cv_ptr->image.copyTo(result, m); //TESTE

    cv::imshow("depth_filtered", result);

    cv::imshow("depth_image", cv_ptr->image);
    cv::waitKey(1);

    depth_pub_.publish(cv_ptr->toImageMsg());
  }

};

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

I know this setup is not the best but I ... (more)