void depthCallback(const sensor_msgs::ImageConstPtr& msg)
  {
    if (!LineMOD_Detector::got_color)
    {
      return;
    }
    cv_bridge::CvImagePtr depth_ptr;
    bool show_match_result = true;
    try
    {
      depth_ptr = cv_bridge::toCvCopy(msg, "");
    }
    catch (cv_bridge::Exception& e)
    {
      ROS_ERROR("depth cv_bridge exception: %s", e.what());
      return;
    }
    LineMOD_Detector::sources.push_back(LineMOD_Detector::color_img);
    LineMOD_Detector::sources.push_back(depth_ptr->image);


      // Perform matching
      std::vector<cv::linemod::Match> matches;
      std::vector<cv::String> class_ids;
      std::vector<cv::Mat> quantized_images;

      LineMOD_Detector::detector->match(sources, (float)LineMOD_Detector::matching_threshold, matches, class_ids, quantized_images);

      LineMOD_Detector::num_classes = detector->numClasses();
      int classes_visited = 0;
      std::set<std::string> visited;

      for (int i = 0; (i < (int)matches.size()) && (classes_visited < LineMOD_Detector::num_classes); ++i)
      {
        cv::linemod::Match m = matches[i];

        if (visited.insert(m.class_id).second)
        {
          ++classes_visited;

          if (show_match_result)
          {
            printf("Similarity: %5.1f%%; x: %3d; y: %3d; class: %s; template: %3d\n",
                   m.similarity, m.x, m.y, m.class_id.c_str(), m.template_id);
          }

          // Draw matching template
          const std::vector<cv::linemod::Template>& templates = LineMOD_Detector::detector->getTemplates(m.class_id, m.template_id);
          drawResponse(templates, LineMOD_Detector::num_modalities, LineMOD_Detector::display, cv::Point(m.x, m.y), LineMOD_Detector::detector->getT(0));
          //std::cout << "pub_threshold " << LineMOD_Detector::pub_threshold << std::endl;
          if (m.similarity > LineMOD_Detector::pub_threshold)
          {
            LineMOD_Detector::publishPoint(templates, m, depth_ptr->image, msg->header);
          }

        }
      }

      LineMOD_Detector::sources.clear();
  }
  void disparityCallback(const stereo_msgs::DisparityImageConstPtr& msg)
  {
    ROS_DEBUG("Pub Threshold:%f ", LineMOD_Detector::pub_threshold);
    if (!LineMOD_Detector::got_color)
    {
      return;
    }
    bool show_match_result = true;
    cv_bridge::CvImagePtr disp_ptr;
    try
    {
      disp_ptr = cv_bridge::toCvCopy(msg->image, "");
    }
    catch (cv_bridge::Exception& e)
    {
      ROS_ERROR("disp cv_bridge exception: %s", e.what());
      return;
    }

    float f = msg->f;
    float T = msg->T;
    cv::Mat depth_img = (f*T*1000)/(disp_ptr->image).clone();
    depth_img.convertTo(depth_img, CV_16U);
    LineMOD_Detector::sources.push_back(LineMOD_Detector::color_img);
    LineMOD_Detector::sources.push_back(depth_img);

      // Perform matching
      std::vector<cv::linemod::Match> matches;
      std::vector<cv::String> class_ids;
      std::vector<cv::Mat> quantized_images;

      LineMOD_Detector::detector->match(sources, (float)LineMOD_Detector::matching_threshold, matches, class_ids, quantized_images);

      LineMOD_Detector::num_classes = detector->numClasses();
      ROS_DEBUG("Num Classes: %u", LineMOD_Detector::num_classes);
      int classes_visited = 0;
      std::set<std::string> visited;

      ROS_DEBUG("Matches size: %u", (int)matches.size());
      for (int i = 0; (i < (int)matches.size()) && (classes_visited < LineMOD_Detector::num_classes); ++i)
      {
        cv::linemod::Match m = matches[i];
        ROS_DEBUG("Matching count: %u", i);

        if (visited.insert(m.class_id).second)
        {
          ++classes_visited;

          if (show_match_result)
          {
            ROS_DEBUG("Similarity: %5.1f%%; x: %3d; y: %3d; class: %s; template: %3d\n",
                   m.similarity, m.x, m.y, m.class_id.c_str(), m.template_id);
            printf("Similarity: %5.1f%%; x: %3d; y: %3d; class: %s; template: %3d\n",
                   m.similarity, m.x, m.y, m.class_id.c_str(), m.template_id);
          }

          // Draw matching template
          const std::vector<cv::linemod::Template>& templates = LineMOD_Detector::detector->getTemplates(m.class_id, m.template_id);
          drawResponse(templates, LineMOD_Detector::num_modalities, LineMOD_Detector::display, cv::Point(m.x, m.y), LineMOD_Detector::detector->getT(0));
          if (m.similarity > LineMOD_Detector::pub_threshold)
          {
            LineMOD_Detector::publishPoint(templates, m, depth_img, msg->header);
          }

        }
      }

      LineMOD_Detector::sources.clear();
  }