$search
00001 /* 00002 * Copyright (c) 2009, Willow Garage, Inc. 00003 * All rights reserved. 00004 * 00005 * Redistribution and use in source and binary forms, with or without 00006 * modification, are permitted provided that the following conditions are met: 00007 * 00008 * * Redistributions of source code must retain the above copyright 00009 * notice, this list of conditions and the following disclaimer. 00010 * * Redistributions in binary form must reproduce the above copyright 00011 * notice, this list of conditions and the following disclaimer in the 00012 * documentation and/or other materials provided with the distribution. 00013 * * Neither the name of the Willow Garage, Inc. nor the names of its 00014 * contributors may be used to endorse or promote products derived from 00015 * this software without specific prior written permission. 00016 * 00017 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 00018 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 00019 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 00020 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 00021 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 00022 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 00023 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 00024 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 00025 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 00026 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00027 * POSSIBILITY OF SUCH DAMAGE. 00028 */ 00029 00030 #ifndef SEGMENTATION_INTERFACE 00031 #define SEGMENTATION_INTERFACE 00032 00033 #include "object_segmentation_gui/ObjectSegmentationGuiAction.h" 00034 #include "object_segmentation_gui/utils.h" 00035 00036 #include "active_realtime_segmentation/fgbgsegment.h" 00037 #include "active_realtime_segmentation/pyra/tpimage.h" 00038 00039 #include <sensor_msgs/Image.h> 00040 00041 #include <boost/thread/mutex.hpp> 00042 #include <boost/thread/thread.hpp> 00043 #include <boost/thread/condition_variable.hpp> 00044 #include <deque> 00045 00046 namespace object_segmentation_gui { 00047 00048 typedef geometry_msgs::Point32 Point; 00049 00050 class ObjectSegmenter 00051 { 00052 public: 00053 00054 enum ActionType{PAUSE, RESET, CLICK, REGION, STOP, GRADCHANGE}; 00055 00056 struct Box2D 00057 { 00058 Point p_tl_; 00059 Point p_br_; 00060 }; 00061 00062 struct Action 00063 { 00064 ActionType type_; 00065 Box2D box_; 00066 00067 // cues and model flags for segmentation 00068 bool with_colors_; 00069 bool with_color_holes_; 00070 bool uniform_; 00071 bool with_disparities_; 00072 bool with_surface_; 00073 00074 float grad_weight_; 00075 00076 }; 00077 00078 ObjectSegmenter( float grad_weight, int n_iter, 00079 bool use_gpu, float w_size = 0.20f, 00080 float b_size = 0.20f); 00081 ~ObjectSegmenter(); 00082 00083 void setNewData( const sensor_msgs::Image &image_, 00084 const stereo_msgs::DisparityImage &disparity_); 00085 00086 void queueAction( const Action &action); 00087 00088 bool getCurrentResult( sensor_msgs::Image &image); 00089 void getCurrentSurface(float &alpha, float &beta, float &gamma); 00090 00091 void getCurrentSegmFlags( bool &with_colors, 00092 bool &with_color_holes_, 00093 bool &uniform_, 00094 bool &with_disparities_, 00095 bool &with_surface_ ); 00096 00097 void setCurrentSegmFlags( bool with_colors, 00098 bool with_color_holes, 00099 bool uniform, 00100 bool with_disparities, 00101 bool with_surface ); 00102 00103 protected: 00104 00105 private: 00106 00107 // execute by segmentation thread 00108 void doSegment(); 00109 00110 void stopThread(); 00111 00112 void setCurrentResult(); 00113 bool dequeueAction( Action &action); 00114 void clearQueue(); 00115 00116 void waitForAction(); 00117 00118 void pause(); 00119 00120 // start thread 00121 template <typename Callable> 00122 void startThread( Callable f ); 00123 00124 // stop segmentation thread 00125 00126 // produce mask image for segment initialisation 00127 void fillInitMask( Image<uint8_t> &init_mask, int start_x, int start_y, int stop_x, int stop_y); 00128 00129 // copy segmentation labels to ROS image 00130 void fillImage( sensor_msgs::Image &image, const Image<uint8_t> &segm_mask); 00131 00132 bool validResult( const Image<uint8_t> &segm_mask); 00133 00134 int w_; 00135 int h_; 00136 int grad_weight_; 00137 int drange_; 00138 00139 float window_size_; 00140 float ball_size_; 00141 00142 bool use_gpu_; 00143 int n_iter_; 00144 00145 bool init_; 00146 00147 int num_fg_hypos_; 00148 00149 FgBgSegment *fgbgsegment; 00150 00151 // RGB and Disparity images with correct memory alignment for GPU 00152 Image<uint8_t> gpu_image; 00153 Image<float> gpu_disparities; 00154 00155 // mask for rectangular initialisation 00156 Image<uint8_t> init_mask; 00157 00158 // stores labling results 00159 Image<uint8_t> segm_mask; 00160 float alpha_, beta_, gamma_; 00161 00162 // stores most recent control message 00163 std::deque<Action> action_queue_; 00164 00165 boost::thread* thread_; 00166 boost::mutex queue_mutex_; 00167 boost::mutex image_mutex_; 00168 00169 boost::condition_variable cond_var_; 00170 00171 }; 00172 } 00173 00174 #endif // SEGMENTATION_INTERFACE