Merge pull request #1218 from wkentaro/roi-cropped-image

[image_view2] Publish cropped image with roi

jsk_ros_patch/image_view2/README.md

@@ -14,55 +14,74 @@ image_view2 support several mode to interact with user.
 You can set interaction mode by `~interaction_mode` parameter or
 change mode by service interfaces.
 
-* Rectangle mode
+* Rectangle mode (`~interaction_mode:=rectangle`)
 
   In rectangle mode, user can select region-of-interest by dragging mouse pointer.
 
   ![](images/image_view2_rect_interaction.gif)
 
-  Selected regions are published to `image/screenrectangle` topic.
+  Selected regions are published to
+  `image/screenrectangle` (`geometry_msgs/PolygonStamped`) topic.
 
   If user clicks on the view instead of dragging pointer, clicked position
-  is published to `image/screenpoint` topic.
-* Line mode
+  is published to `image/screenpoint` (`geometry_msgs/PointStamped`) topic.
+
+* Line mode (`~interaction_mode:=line`)
 
   In line mode, user can select a line by dragging mouse pointer.
 
   ![](images/image_view2_line_interaction.gif)
 
-  Selected line are published to `image/line` topic.
-* Polygon mode
+  Selected line are published to `image/line` (`geometry_msgs/PolygonStamped`) topic.
+
+* Polygon mode (`~interaction_mode:=poly`)
 
   In polygon mode, user can select a series of closed lines.
   User can add line by left-click and close the lines by right-click.
 
   ![](images/image_view2_poly_interaction.gif)
 
-  Selected polygon are published to `image/poly` topic.
-* Grabcut mode
+  Selected polygon are published to `image/poly` (`geometry_msgs/PolygonStamped`) topic.
+
+* Grabcut mode (`~interaction_mode:=grabcut`)
 
   In grabcut mode, user can select two curves. By typing `Esc` key, you can clear
   selected curves.
 
   ![](images/image_view2_grabcut_interaction.gif)
 
-  The selected first red curve  is published to `image/foreground` and
-  the second green one is publiehd to `image/background`.
+  The selected first red curve  is published to
+  `image/foreground` (`sensor_msgs/Image`) and
+  the second green one is publiehd to `image/background` (`sensor_msgs/Image`).
   This mode is originally implemented to select forground and background pixels for grabcut.
-* Grabcut Rectangle mode
+* Grabcut Rectangle mode (`~interaction_mode:=grabcut_rect`)
 
   In grabcut rectangle mode, user can select two rectangles. By typing `Esc` key, you can clear
   selected rectangles.
 
   ![](images/image_view2_grabcut_rect_interaction.gif)
 
-  The selected first red rectangle  is published to `image/foreground_rect` and
-  the second green one is publiehd to `image/background_rect`.
+  The selected first red rectangle  is published to
+  `image/foreground_rect` (`geometry_msgs/PolygonStamped`) and
+  the second green one is publiehd to
+  `image/background_rect` (`geometry_msgs/PolygonStamped`).
   This mode is originally implemented to select forground and background regions for grabcut.
 
 ROS API
 -------
 
+### Publising Topics
+
+* `image/marked` (`sensor_msgs/Image`)
+
+  Image with marks drawed.
+
+* `image/screenrectangle_image` (`sensor_msgs/Image`)
+
+  Cropped image with user selection at **Rectangle mode**.
+
+  <img src="images/image_view2_screenrectangle_image.png" width="25%" />
+
 ### Subscribing Topics
 * `image` (`sensor_msgs/Image`)
 

jsk_ros_patch/image_view2/image_view2.cpp

@@ -56,6 +56,7 @@ namespace image_view2{
     point_pub_ = nh.advertise<geometry_msgs::PointStamped>(camera + "/screenpoint",100);
     point_array_pub_ = nh.advertise<sensor_msgs::PointCloud2>(camera + "/screenpoint_array",100);
     rectangle_pub_ = nh.advertise<geometry_msgs::PolygonStamped>(camera + "/screenrectangle",100);
+    rectangle_img_pub_ = nh.advertise<sensor_msgs::Image>(camera + "/screenrectangle_image", 100);
     move_point_pub_ = nh.advertise<geometry_msgs::PointStamped>(camera + "/movepoint", 100);
     foreground_mask_pub_ = nh.advertise<sensor_msgs::Image>(camera + "/foreground", 100);
     background_mask_pub_ = nh.advertise<sensor_msgs::Image>(camera + "/background", 100);
@@ -1031,7 +1032,21 @@ namespace image_view2{
       cv::putText(image_, info_str_2.c_str(), cv::Point(10,image_.rows-10), cv::FONT_HERSHEY_SIMPLEX, 0.8, cv::Scalar::all(255), 2);
     }
   }
-
+
+  void ImageView2::cropROI()
+  {
+    if (mode_ == MODE_RECTANGLE) {
+      // publish rectangle cropped image
+      cv::Rect screen_rect(window_selection_.x, window_selection_.y, window_selection_.width, window_selection_.height);
+      cv::Mat cropped_img = original_image_(screen_rect);
+      rectangle_img_pub_.publish(
+        cv_bridge::CvImage(
+          last_msg_->header,
+          last_msg_->encoding,
+          cropped_img).toImageMsg());
+    }
+  }
+
   void ImageView2::redraw()
   {
     if (original_image_.empty()) {
@@ -1048,6 +1063,7 @@ namespace image_view2{
     }
     drawMarkers();
     drawInteraction();
+    cropROI();
 
     if ( draw_grid_ ) drawGrid();
     if ( blurry_mode_ ) cv::addWeighted(image_, 0.9, draw_, 1.0, 0.0, image_);

jsk_ros_patch/image_view2/image_view2.h

@@ -174,6 +174,7 @@ namespace image_view2
     void drawMarkers();
     void drawInteraction();
     void drawGrid();
+    void cropROI();
     void drawInfo(ros::Time& before_rendering);
     void resolveLocalMarkerQueue();
     bool lookupTransformation(
@@ -247,6 +248,7 @@ namespace image_view2
     ros::Publisher point_pub_;
     ros::Publisher point_array_pub_;
     ros::Publisher rectangle_pub_;
+    ros::Publisher rectangle_img_pub_;
     ros::Publisher move_point_pub_;
     ros::Publisher foreground_mask_pub_;
     ros::Publisher background_mask_pub_;