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 <opencv/highgui.h>
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
00050 #include <image_view2/ImageMarker2.h>
00051 #include <geometry_msgs/PointStamped.h>
00052 #include <geometry_msgs/PolygonStamped.h>
00053 #include <std_msgs/Empty.h>
00054
00055 #include <boost/thread.hpp>
00056 #include <boost/format.hpp>
00057 #include <boost/foreach.hpp>
00058 #include <boost/circular_buffer.hpp>
00059 #include <boost/lambda/lambda.hpp>
00060 #include <pcl/point_types.h>
00061 #include <pcl_ros/publisher.h>
00062
00063 #define DEFAULT_COLOR CV_RGB(255,0,0)
00064 #define USER_ROI_COLOR CV_RGB(255,0,0)
00065 #define DEFAULT_CIRCLE_SCALE 20
00066 #define DEFAULT_LINE_WIDTH 3
00067
00068 namespace image_view2
00069 {
00070 typedef std::vector<image_view2::ImageMarker2::ConstPtr> V_ImageMarkerMessage;
00071 inline CvScalar MsgToRGB(const std_msgs::ColorRGBA &color){
00072 if(color.a == 0.0 && color.r == 0.0 && color.g == 0.0 && color.b == 0.0)
00073 return DEFAULT_COLOR;
00074 else
00075 return CV_RGB(color.r*255, color.g*255, color.b*255);
00076 }
00077
00078 class ImageView2
00079 {
00080 public:
00081 enum KEY_MODE {
00082 MODE_RECTANGLE,
00083 MODE_SERIES,
00084 MODE_SELECT_FORE_AND_BACK,
00085 MODE_SELECT_FORE_AND_BACK_RECT
00086 };
00087
00088 ImageView2();
00089 ImageView2(ros::NodeHandle& nh);
00090 ~ImageView2();
00091 void pressKey(int key);
00092 void markerCb(const image_view2::ImageMarker2ConstPtr& marker);
00093 void infoCb(const sensor_msgs::CameraInfoConstPtr& msg);
00094 void redraw();
00095 void imageCb(const sensor_msgs::ImageConstPtr& msg);
00096 void drawImage();
00097 void addPoint(int x, int y);
00098 void addRegionPoint(int x, int y);
00099 void updateRegionWindowSize(int x, int y);
00100 void setRegionWindowPoint(int x, int y);
00101 void clearPointArray();
00102 void publishPointArray();
00103 void setMode(KEY_MODE mode);
00104 KEY_MODE getMode();
00105 void showImage();
00106 static void mouseCb(int event, int x, int y, int flags, void* param);
00107 bool isValidMovement(const cv::Point2f& start_point,
00108 const cv::Point2f& end_point);
00109 bool toggleSelection();
00110 void publishForegroundBackgroundMask();
00111 bool use_window;
00112 protected:
00113 private:
00114 void pointArrayToMask(std::vector<cv::Point2d>& points,
00115 cv::Mat& mask);
00116 void publishMonoImage(ros::Publisher& pub,
00117 cv::Mat& image,
00118 const std_msgs::Header& header);
00120
00122 void drawLineStrip(const image_view2::ImageMarker2::ConstPtr& marker,
00123 std::vector<CvScalar>& colors,
00124 std::vector<CvScalar>::iterator& col_it);
00125 void drawLineList(const image_view2::ImageMarker2::ConstPtr& marker,
00126 std::vector<CvScalar>& colors,
00127 std::vector<CvScalar>::iterator& col_it);
00128 void drawPolygon(const image_view2::ImageMarker2::ConstPtr& marker,
00129 std::vector<CvScalar>& colors,
00130 std::vector<CvScalar>::iterator& col_it);
00131 void drawPoints(const image_view2::ImageMarker2::ConstPtr& marker,
00132 std::vector<CvScalar>& colors,
00133 std::vector<CvScalar>::iterator& col_it);
00134 void drawFrames(const image_view2::ImageMarker2::ConstPtr& marker,
00135 std::vector<CvScalar>& colors,
00136 std::vector<CvScalar>::iterator& col_it);
00137 void drawText(const image_view2::ImageMarker2::ConstPtr& marker,
00138 std::vector<CvScalar>& colors,
00139 std::vector<CvScalar>::iterator& col_it);
00140 void drawLineStrip3D(const image_view2::ImageMarker2::ConstPtr& marker,
00141 std::vector<CvScalar>& colors,
00142 std::vector<CvScalar>::iterator& col_it);
00143 void drawLineList3D(const image_view2::ImageMarker2::ConstPtr& marker,
00144 std::vector<CvScalar>& colors,
00145 std::vector<CvScalar>::iterator& col_it);
00146 void drawPolygon3D(const image_view2::ImageMarker2::ConstPtr& marker,
00147 std::vector<CvScalar>& colors,
00148 std::vector<CvScalar>::iterator& col_it);
00149 void drawPoints3D(const image_view2::ImageMarker2::ConstPtr& marker,
00150 std::vector<CvScalar>& colors,
00151 std::vector<CvScalar>::iterator& col_it);
00152 void drawText3D(const image_view2::ImageMarker2::ConstPtr& marker,
00153 std::vector<CvScalar>& colors,
00154 std::vector<CvScalar>::iterator& col_it);
00155 void drawCircle3D(const image_view2::ImageMarker2::ConstPtr& marker,
00156 std::vector<CvScalar>& colors,
00157 std::vector<CvScalar>::iterator& col_it);
00158 void drawCircle(const image_view2::ImageMarker2::ConstPtr& marker);
00159 void drawMarkers();
00160 void drawInteraction();
00161 void drawInfo(ros::Time& before_rendering);
00162 void resolveLocalMarkerQueue();
00163 bool lookupTransformation(
00164 std::string frame_id, ros::Time& acquisition_time,
00165 std::map<std::string, int>& tf_fail,
00166 tf::StampedTransform &transform);
00167 void processMouseEvent(int event, int x, int y, int flags, void* param);
00168 void processLeftButtonDown(int x, int y);
00169 void processMove(int x, int y);
00170 void processLeftButtonUp(int x, int y);
00171 void publishMouseInteractionResult();
00172 void checkMousePos(int& x, int& y);
00173 V_ImageMarkerMessage local_queue_;
00174 image_transport::Subscriber image_sub_;
00175 ros::Subscriber info_sub_;
00176 ros::Subscriber marker_sub_;
00177 std::string marker_topic_;
00178 boost::circular_buffer<double> times_;
00179 image_transport::Publisher image_pub_;
00180
00181 V_ImageMarkerMessage marker_queue_;
00182 boost::mutex queue_mutex_;
00183 boost::mutex point_array_mutex_;
00184 sensor_msgs::ImageConstPtr last_msg_;
00185 sensor_msgs::CameraInfoConstPtr info_msg_;
00186 cv_bridge::CvImage img_bridge_;
00187 boost::mutex image_mutex_;
00188 int skip_draw_rate_;
00189 cv::Mat original_image_, image_, draw_;
00190
00191 tf::TransformListener tf_listener_;
00192 image_geometry::PinholeCameraModel cam_model_;
00193 std::vector<std::string> frame_ids_;
00194 std::vector<cv::Point2d> point_array_;
00195 boost::mutex info_mutex_;
00196
00197
00198 bool selecting_fg_;
00199 std::vector<cv::Point2d> point_bg_array_;
00200 std::vector<cv::Point2d> point_fg_array_;
00201 cv::Rect rect_bg_;
00202 cv::Rect rect_fg_;
00203 std::string window_name_;
00204 boost::format filename_format_;
00205 int font_;
00206
00207 double resize_x_, resize_y_;
00208 CvRect window_selection_;
00209 cv::Point2f button_up_pos_;
00210 int count_;
00211 bool blurry_mode_;
00212 bool show_info_;
00213 double tf_timeout_;
00214 bool region_continuous_publish_;
00215 bool continuous_ready_;
00216 bool left_button_clicked_;
00217 ros::Publisher point_pub_;
00218 ros::Publisher point_array_pub_;
00219 ros::Publisher rectangle_pub_;
00220 ros::Publisher move_point_pub_;
00221 ros::Publisher foreground_mask_pub_;
00222 ros::Publisher background_mask_pub_;
00223 KEY_MODE mode_;
00224 bool autosize_;
00225 bool window_initialized_;
00226 };
00227 }
00228
00229 #endif