semanticmodel: semanticmodel: segment

00001 /*
00002  * segment_viewer.cc
00003  * Mac Mason <mac@cs.duke.edu>
00004  *
00005  * Listen for segment messages and show them to us, with colors.
00006  */
00007 
00008 #include <opencv2/opencv.hpp>
00009 #include "ros/ros.h"
00010 #include "cv_bridge/cv_bridge.h"
00011 #include "semanticmodel/Segment.h"
00012 
00013 void process(const semanticmodel::SegmentConstPtr& segment);
00014 
00015 int main(int argc, char* argv[])
00016 {
00017   ros::init(argc, argv, "segment_viewer");
00018   ros::NodeHandle handle;
00019   ros::Subscriber sub = handle.subscribe("/flat_frames", 100, process);
00020   cv::namedWindow("rgb");
00021   bool keepgoing = true;
00022   while (keepgoing)
00023   {
00024     ros::spinOnce();
00025     char c = cv::waitKey(5);
00026     keepgoing = (c != 'q' && ros::ok());
00027   }
00028 }
00029 
00030 void process(const semanticmodel::SegmentConstPtr& segment)
00031 {
00032   static int count = 0;
00033   cv_bridge::CvImagePtr rgb, depth;
00034   try 
00035   {
00036     rgb = cv_bridge::toCvCopy(segment->rgb_image);
00037     depth = cv_bridge::toCvCopy(segment->depth_image);
00038   }
00039   catch (cv_bridge::Exception e)
00040   {
00041     ROS_ERROR("cv_bridge problem: %s", e.what());
00042     return;
00043   }
00044   for (size_t i = 0 ; i < segment->coordinates.size() ; i += 2)
00045   {
00046     for (int r = -1 ; r < 2 ; ++r)
00047     {
00048       for (int c = -1 ; c < 2 ; ++c)
00049       {
00050         rgb->image.at<cv::Vec3b>(
00051             segment->coordinates[i] + r, 
00052             segment->coordinates[i+1] + c) = cv::Vec3b(0, 0, 255);
00053       }
00054     }
00055   }
00056   cv::imshow("rgb", rgb->image);
00057   cv::imwrite((boost::format("segment-%06d.png") % count).str(), rgb->image);
00058   count++;
00059 }