00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036 #ifndef IMAGE_VIEW2_H_
00037 #define IMAGE_VIEW2_H_
00038
00039 #include <opencv/cv.h>
00040 #include <opencv2/highgui/highgui.hpp>
00041
00042 #include <ros/ros.h>
00043 #include <sensor_msgs/Image.h>
00044 #include <cv_bridge/cv_bridge.h>
00045 #include <sensor_msgs/image_encodings.h>
00046 #include <image_transport/image_transport.h>
00047 #include <image_geometry/pinhole_camera_model.h>
00048 #include <tf/transform_listener.h>
00049 #include <dynamic_reconfigure/server.h>
00050
00051 #include <image_view2/ImageMarker2.h>
00052 #include <geometry_msgs/PointStamped.h>
00053 #include <geometry_msgs/PolygonStamped.h>
00054 #include <std_msgs/Empty.h>
00055 #include <std_srvs/Empty.h>
00056 #include <image_view2/ChangeMode.h>
00057 #include <boost/thread.hpp>
00058 #include <boost/format.hpp>
00059 #include <boost/foreach.hpp>
00060 #include <boost/circular_buffer.hpp>
00061 #include <boost/lambda/lambda.hpp>
00062 #include <pcl/point_types.h>
00063 #include <pcl_ros/publisher.h>
00064 #include <image_view2/ImageView2Config.h>
00065
00066 #include <image_view2/MouseEvent.h>
00067
00068 #define DEFAULT_COLOR CV_RGB(255,0,0)
00069 #define USER_ROI_COLOR CV_RGB(255,0,0)
00070 #define DEFAULT_CIRCLE_SCALE 20
00071 #define DEFAULT_LINE_WIDTH 3
00072
00073 namespace image_view2
00074 {
00075 typedef std::vector<image_view2::ImageMarker2::ConstPtr> V_ImageMarkerMessage;
00076 inline CvScalar MsgToRGB(const std_msgs::ColorRGBA &color){
00077 if(color.a == 0.0 && color.r == 0.0 && color.g == 0.0 && color.b == 0.0)
00078 return DEFAULT_COLOR;
00079 else
00080 return CV_RGB(color.r*255, color.g*255, color.b*255);
00081 }
00082
00083 class ImageView2
00084 {
00085 public:
00086 typedef ImageView2Config Config;
00087 enum KEY_MODE {
00088 MODE_RECTANGLE,
00089 MODE_SERIES,
00090 MODE_SELECT_FORE_AND_BACK,
00091 MODE_SELECT_FORE_AND_BACK_RECT,
00092 MODE_LINE,
00093 MODE_POLY,
00094 MODE_NONE
00095 };
00096
00097 ImageView2();
00098 ImageView2(ros::NodeHandle& nh);
00099 ~ImageView2();
00100 void pressKey(int key);
00101 void markerCb(const image_view2::ImageMarker2ConstPtr& marker);
00102 void infoCb(const sensor_msgs::CameraInfoConstPtr& msg);
00103 void redraw();
00104 void imageCb(const sensor_msgs::ImageConstPtr& msg);
00105 void drawImage();
00106 void addPoint(int x, int y);
00107 void addRegionPoint(int x, int y);
00108 void updateRegionWindowSize(int x, int y);
00109 void setRegionWindowPoint(int x, int y);
00110 void clearPointArray();
00111 void publishPointArray();
00112 void setMode(KEY_MODE mode);
00113 KEY_MODE getMode();
00114 void showImage();
00115 static void mouseCb(int event, int x, int y, int flags, void* param);
00116 bool isValidMovement(const cv::Point2f& start_point,
00117 const cv::Point2f& end_point);
00118 bool toggleSelection();
00119 void publishForegroundBackgroundMask();
00120 bool use_window;
00121 protected:
00122 private:
00123 void config_callback(Config &config, uint32_t level);
00124 void eventCb(
00125 const image_view2::MouseEvent::ConstPtr& event_msg);
00126 void pointArrayToMask(std::vector<cv::Point2d>& points,
00127 cv::Mat& mask);
00128 void publishMonoImage(ros::Publisher& pub,
00129 cv::Mat& image,
00130 const std_msgs::Header& header);
00131 void publishRectFromMaskImage(ros::Publisher& pub,
00132 cv::Mat& image,
00133 const std_msgs::Header& header);
00135
00137 void drawLineStrip(const image_view2::ImageMarker2::ConstPtr& marker,
00138 std::vector<CvScalar>& colors,
00139 std::vector<CvScalar>::iterator& col_it);
00140 void drawLineList(const image_view2::ImageMarker2::ConstPtr& marker,
00141 std::vector<CvScalar>& colors,
00142 std::vector<CvScalar>::iterator& col_it);
00143 void drawPolygon(const image_view2::ImageMarker2::ConstPtr& marker,
00144 std::vector<CvScalar>& colors,
00145 std::vector<CvScalar>::iterator& col_it);
00146 void drawPoints(const image_view2::ImageMarker2::ConstPtr& marker,
00147 std::vector<CvScalar>& colors,
00148 std::vector<CvScalar>::iterator& col_it);
00149 void drawFrames(const image_view2::ImageMarker2::ConstPtr& marker,
00150 std::vector<CvScalar>& colors,
00151 std::vector<CvScalar>::iterator& col_it);
00152 void drawText(const image_view2::ImageMarker2::ConstPtr& marker,
00153 std::vector<CvScalar>& colors,
00154 std::vector<CvScalar>::iterator& col_it);
00155 void drawLineStrip3D(const image_view2::ImageMarker2::ConstPtr& marker,
00156 std::vector<CvScalar>& colors,
00157 std::vector<CvScalar>::iterator& col_it);
00158 void drawLineList3D(const image_view2::ImageMarker2::ConstPtr& marker,
00159 std::vector<CvScalar>& colors,
00160 std::vector<CvScalar>::iterator& col_it);
00161 void drawPolygon3D(const image_view2::ImageMarker2::ConstPtr& marker,
00162 std::vector<CvScalar>& colors,
00163 std::vector<CvScalar>::iterator& col_it);
00164 void drawPoints3D(const image_view2::ImageMarker2::ConstPtr& marker,
00165 std::vector<CvScalar>& colors,
00166 std::vector<CvScalar>::iterator& col_it);
00167 void drawText3D(const image_view2::ImageMarker2::ConstPtr& marker,
00168 std::vector<CvScalar>& colors,
00169 std::vector<CvScalar>::iterator& col_it);
00170 void drawCircle3D(const image_view2::ImageMarker2::ConstPtr& marker,
00171 std::vector<CvScalar>& colors,
00172 std::vector<CvScalar>::iterator& col_it);
00173 void drawCircle(const image_view2::ImageMarker2::ConstPtr& marker);
00174 void drawMarkers();
00175 void drawInteraction();
00176 void drawGrid();
00177 void cropROI();
00178 void drawInfo(ros::Time& before_rendering);
00179 void resolveLocalMarkerQueue();
00180 bool lookupTransformation(
00181 std::string frame_id, ros::Time& acquisition_time,
00182 std::map<std::string, int>& tf_fail,
00183 tf::StampedTransform &transform);
00184 void processMouseEvent(int event, int x, int y, int flags, void* param);
00185 void processLeftButtonDown(int x, int y);
00186 void processMove(int x, int y);
00187 void processLeftButtonUp(int x, int y);
00188 void publishMouseInteractionResult();
00189 void checkMousePos(int& x, int& y);
00190 void createDistortGridImage();
00191 V_ImageMarkerMessage local_queue_;
00192 image_transport::Subscriber image_sub_;
00193 ros::Subscriber event_sub_;
00194 ros::Subscriber info_sub_;
00195 ros::Subscriber marker_sub_;
00196 std::string marker_topic_;
00197 boost::circular_buffer<double> times_;
00198 image_transport::Publisher image_pub_;
00199 image_transport::Publisher local_image_pub_;
00200 boost::shared_ptr <dynamic_reconfigure::Server<Config> > srv_;
00201
00202 V_ImageMarkerMessage marker_queue_;
00203 boost::mutex queue_mutex_;
00204 boost::mutex point_array_mutex_;
00205 sensor_msgs::ImageConstPtr last_msg_;
00206 sensor_msgs::CameraInfoConstPtr info_msg_;
00207 cv_bridge::CvImage img_bridge_;
00208 boost::mutex image_mutex_;
00209 int skip_draw_rate_;
00210 cv::Mat original_image_, image_, draw_;
00211 cv::Mat distort_grid_mask_;
00212 int div_u_, div_v_;
00213 int space_,prev_space_;
00214 int grid_red_, grid_green_, grid_blue_, prev_red_, prev_green_, prev_blue_;
00215 int grid_thickness_, prev_thickness_;
00216 bool fisheye_mode_;
00217
00218 tf::TransformListener tf_listener_;
00219 image_geometry::PinholeCameraModel cam_model_;
00220 std::vector<std::string> frame_ids_;
00221 std::vector<cv::Point2d> point_array_;
00222 boost::mutex info_mutex_;
00223
00224
00225 bool selecting_fg_;
00226 std::vector<cv::Point2d> point_bg_array_;
00227 std::vector<cv::Point2d> point_fg_array_;
00228 std::vector<cv::Point2d> poly_points_;
00229 cv::Point poly_selecting_point_;
00230 bool poly_selecting_done_;
00231 cv::Rect rect_bg_;
00232 cv::Rect rect_fg_;
00233 std::string window_name_;
00234 boost::format filename_format_;
00235 int font_;
00236
00237 double resize_x_, resize_y_;
00238 CvRect window_selection_;
00239 cv::Point2f button_up_pos_;
00240 int count_;
00241 bool draw_grid_;
00242 bool blurry_mode_;
00243 bool show_info_;
00244 double tf_timeout_;
00245 bool region_continuous_publish_;
00246 bool continuous_ready_;
00247 bool left_button_clicked_;
00248 ros::Publisher point_pub_;
00249 ros::Publisher point_array_pub_;
00250 ros::Publisher rectangle_pub_;
00251 ros::Publisher rectangle_img_pub_;
00252 ros::Publisher move_point_pub_;
00253 ros::Publisher foreground_mask_pub_;
00254 ros::Publisher background_mask_pub_;
00255 ros::Publisher foreground_rect_pub_;
00256 ros::Publisher background_rect_pub_;
00257 ros::Publisher line_pub_;
00258 ros::Publisher poly_pub_;
00259 KEY_MODE mode_;
00260 bool autosize_;
00261 bool window_initialized_;
00262
00263
00264 boost::mutex poly_point_mutex_;
00265 boost::mutex line_point_mutex_;
00266 bool line_select_start_point_;
00267 bool line_selected_;
00268 cv::Point line_start_point_;
00269 cv::Point line_end_point_;
00270
00271 void updateLineStartPoint(cv::Point p);
00272 void updateLineEndPoint(cv::Point p);
00273 cv::Point getLineStartPoint();
00274 cv::Point getLineEndPoint();
00275 void publishLinePoints();
00276 void updateLinePoint(cv::Point p);
00277 void updatePolyPoint(cv::Point p);
00278 void updatePolySelectingPoint(cv::Point p);
00279 void clearPolyPoints();
00280 void publishPolyPoints();
00281 void finishSelectingPoly();
00282 bool isPolySelectingFirstTime();
00283 bool isSelectingLineStartPoint();
00284 void resetInteraction();
00285 ros::ServiceServer rectangle_mode_srv_;
00286 ros::ServiceServer series_mode_srv_;
00287 ros::ServiceServer grabcut_mode_srv_;
00288 ros::ServiceServer grabcut_rect_mode_srv_;
00289 ros::ServiceServer line_mode_srv_;
00290 ros::ServiceServer none_mode_srv_;
00291 ros::ServiceServer poly_mode_srv_;
00292 ros::ServiceServer change_mode_srv_;
00293 bool changeModeServiceCallback(
00294 image_view2::ChangeModeRequest& req,
00295 image_view2::ChangeModeResponse& res);
00296 bool rectangleModeServiceCallback(
00297 std_srvs::EmptyRequest& req,
00298 std_srvs::EmptyResponse& res);
00299 bool seriesModeServiceCallback(
00300 std_srvs::EmptyRequest& req,
00301 std_srvs::EmptyResponse& res);
00302 bool grabcutModeServiceCallback(
00303 std_srvs::EmptyRequest& req,
00304 std_srvs::EmptyResponse& res);
00305 bool grabcutRectModeServiceCallback(
00306 std_srvs::EmptyRequest& req,
00307 std_srvs::EmptyResponse& res);
00308 bool lineModeServiceCallback(
00309 std_srvs::EmptyRequest& req,
00310 std_srvs::EmptyResponse& res);
00311 bool polyModeServiceCallback(
00312 std_srvs::EmptyRequest& req,
00313 std_srvs::EmptyResponse& res);
00314 bool noneModeServiceCallback(
00315 std_srvs::EmptyRequest& req,
00316 std_srvs::EmptyResponse& res);
00317 cv::Point ratioPoint(double x, double y);
00318 KEY_MODE stringToMode(const std::string& str);
00319 };
00320 }
00321
00322 #endif