segment_viewer.cc

Go to the documentation of this file.

/*
 * segment_viewer.cc
 * Mac Mason <mac@cs.duke.edu>
 *
 * Listen for segment messages and show them to us, with colors.
 */

#include <opencv2/opencv.hpp>
#include "ros/ros.h"
#include "cv_bridge/cv_bridge.h"
#include "semanticmodel/Segment.h"

void process(const semanticmodel::SegmentConstPtr& segment);

int main(int argc, char* argv[])
{
  ros::init(argc, argv, "segment_viewer");
  ros::NodeHandle handle;
  ros::Subscriber sub = handle.subscribe("/flat_frames", 100, process);
  cv::namedWindow("rgb");
  bool keepgoing = true;
  while (keepgoing)
  {
    ros::spinOnce();
    char c = cv::waitKey(5);
    keepgoing = (c != 'q' && ros::ok());
  }
}

void process(const semanticmodel::SegmentConstPtr& segment)
{
  static int count = 0;
  cv_bridge::CvImagePtr rgb, depth;
  try 
  {
    rgb = cv_bridge::toCvCopy(segment->rgb_image);
    depth = cv_bridge::toCvCopy(segment->depth_image);
  }
  catch (cv_bridge::Exception e)
  {
    ROS_ERROR("cv_bridge problem: %s", e.what());
    return;
  }
  for (size_t i = 0 ; i < segment->coordinates.size() ; i += 2)
  {
    for (int r = -1 ; r < 2 ; ++r)
    {
      for (int c = -1 ; c < 2 ; ++c)
      {
        rgb->image.at<cv::Vec3b>(
            segment->coordinates[i] + r, 
            segment->coordinates[i+1] + c) = cv::Vec3b(0, 0, 255);
      }
    }
  }
  cv::imshow("rgb", rgb->image);
  cv::imwrite((boost::format("segment-%06d.png") % count).str(), rgb->image);
  count++;
}