$search
00001 #include "augmented_object_selection/GC3DApplication.hpp" 00002 #include "grabcut_3d/grabcut_3d.h" 00003 00004 using namespace std; 00005 using namespace cv; 00006 00007 static const Scalar RD = Scalar(0,0,255); 00008 static const Scalar PK = Scalar(230,130,255); 00009 static const Scalar BL = Scalar(255,0,0); 00010 static const Scalar LB = Scalar(255,255,160); 00011 static const Scalar GR = Scalar(0,255,0); 00012 static const Scalar WH = Scalar(255,255,255); 00013 static const Scalar GY = Scalar(128,128,128); 00014 static const Scalar BK = Scalar(0,0,0); 00015 static const int DOT_RADIUS = 2; 00016 static const int LINE_THICKNESS = -1; 00017 00018 // Initialize default rectangle marker (not actually used as a rectangle) 00019 const Rect GC3DApplication::DEFAULT_RECT(0,0,0,0); 00020 // This scale factor is what is actually used to calculate the default 00021 // rectangle. 00022 const int DEFAULT_RECT_SCALE_FACTOR = 2; 00023 00024 const int BGD_KEY = CV_EVENT_FLAG_CTRLKEY; 00025 const int FGD_KEY = CV_EVENT_FLAG_SHIFTKEY; 00026 00027 /* NB: GrabCut pixel classes from imgproc.hpp for reference: 00028 * GC_BGD = 0 00029 * GC_FGD = 1 00030 * GC_PR_BGD = 2 00031 * GC_PR_FGD = 3 00032 */ 00033 00034 /* Mouse callback trampoline for OpenCV window */ 00035 void on_mouse( int event, int x, int y, int flags, void* obj ) 00036 { 00037 static_cast<GC3DApplication*>(obj)->mouseClick(event, x, y, flags); 00038 } 00039 00040 /* Helper function to convert mask in 4-value GrabCut format to a binary mask 00041 * with foreground = 1, background = 0. */ 00042 static void 00043 binaryMaskFromGCMask(const Mat &_gc_mask, Mat &_bin_mask) 00044 { 00045 if( _gc_mask.empty() || _gc_mask.type() != CV_8UC1 ) 00046 CV_Error( CV_StsBadArg, "_gc_mask is empty or has incorrect type (not CV_8UC1)" ); 00047 00048 if( _bin_mask.empty() 00049 || _bin_mask.rows!=_gc_mask.rows 00050 || _bin_mask.cols!=_gc_mask.cols ) 00051 _bin_mask.create( _gc_mask.size(), CV_8UC1 ); 00052 00053 // Convert GC_BGD and GC_PR_BGD to 0, GC_FGD and GC_PR_FGD to 1 00054 _bin_mask = _gc_mask & 1; 00055 } 00056 00057 GC3DApplication::GC3DApplication(const string& _name, const Mat& _img, const Mat& _depth_image) 00058 : win_name_(_name) 00059 { 00060 // Create OpenCV window 00061 namedWindow( win_name_, CV_WINDOW_AUTOSIZE ); 00062 00063 setMouseCallback( win_name_, on_mouse, this ); 00064 ROS_DEBUG("GC3DApplication: creating window"); 00065 00066 // Initialize window 00067 imageIs(_img,_depth_image); 00068 winImageStateIs(GC3DApplication::UPDATED); 00069 00070 iter_count_ = 0; 00071 } 00072 00073 GC3DApplication::~GC3DApplication() 00074 { 00075 destroyWindow( win_name_ ); 00076 } 00077 00078 /* Mutator for initialized state of GCApp object. Client can set 00079 * initialized=true with no side effects; setting initialized=false clears 00080 * background and foreground pixel arrays and returns application state 00081 * variables to NOT_SET. */ 00082 // TODO: Should this be public? 00083 void 00084 GC3DApplication::initializedIs(bool _init) 00085 { 00086 if (_init) 00087 initialized_=_init; 00088 else 00089 { 00090 winImageStateIs(STALE); 00091 if ( !mask_.empty() ) mask_.setTo(Scalar::all(GC_BGD)); 00092 bgd_pxls_.clear(); fgd_pxls_.clear(); 00093 pr_bgd_pxls_.clear(); pr_fgd_pxls_.clear(); 00094 initialized_ = _init; 00095 rect_state_ = NOT_SET; 00096 lbls_state_ = NOT_SET; 00097 pr_lbls_state_ = NOT_SET; 00098 iterCountIs(0); 00099 winImageStateIs(UPDATED); 00100 } 00101 } 00102 00103 /* Mutator for GCApp stored image. */ 00104 void GC3DApplication::imageIs(const Mat& _image, const Mat& _depth_image) 00105 { 00106 if( _image.empty() ) { 00107 ROS_WARN("GCApp: imageIs called with empty image"); 00108 return; 00109 } 00110 image_ = _image.clone(); 00111 depth_image_ = _depth_image.clone(); 00112 00113 mask_.create( image_.size(), CV_8UC1); 00114 initializedIs(false); 00115 } 00116 00117 /* Mutator for GC rectangle. */ 00118 void GC3DApplication::rectIs(const Rect &_r) 00119 { 00120 if (_r == DEFAULT_RECT) 00121 rect_ = Rect(image_.cols/(DEFAULT_RECT_SCALE_FACTOR*2), 00122 image_.rows/(DEFAULT_RECT_SCALE_FACTOR*2), 00123 image_.cols/DEFAULT_RECT_SCALE_FACTOR, 00124 image_.rows/DEFAULT_RECT_SCALE_FACTOR); 00125 else 00126 rect_=_r; 00127 00128 rect_state_ = SET; 00129 setRectInMask(); 00130 assert( bgd_pxls_.empty() 00131 && fgd_pxls_.empty() 00132 && pr_bgd_pxls_.empty() 00133 && pr_fgd_pxls_.empty() ); 00134 winImageStateIs(UPDATED); 00135 00136 } 00137 00138 /* Accessor for binary mask. */ 00139 Mat GC3DApplication::binaryMask() 00140 { 00141 Mat bin_mask; 00142 /* 00143 cv::imwrite("/home/stm1pal/ros/stacks/bosch-ros-pkg/mas/grabcut_app/full_mask.pbm", mask_); 00144 double fm_max; 00145 double fm_min; 00146 minMaxLoc(mask_, &fm_min, &fm_max); 00147 std::cout << "fm_max: " << fm_max << std::endl; 00148 std::cout << "fm_min: " << fm_min << std::endl; 00149 */ 00150 00151 binaryMaskFromGCMask(mask_, bin_mask); 00152 /* 00153 cv::imwrite("/home/stm1pal/ros/stacks/bosch-ros-pkg/mas/grabcut_app/bin_mask.pbm", bin_mask); 00154 double bm_max; 00155 double bm_min; 00156 minMaxLoc(bin_mask, &bm_min, &bm_max); 00157 std::cout << "bm_max: " << bm_max << std::endl; 00158 std::cout << "bm_min: " << bm_min << std::endl; 00159 */ 00160 return bin_mask; 00161 } 00162 00163 /* Mutator for window background color. */ 00164 void GC3DApplication::winColorIs(WinColor _c) 00165 { 00166 winImageStateIs(STALE); 00167 win_color_=_c; 00168 winImageStateIs(UPDATED); 00169 } 00170 /* Mutator for state of image window. Client can set state=STALE with no side 00171 * effects. Setting state=UPDATED when state=STALE causes new image to be drawn 00172 * in window. */ 00173 void GC3DApplication::winImageStateIs(WinImageState _wis) 00174 { 00175 // Set state=STALE with no side effects 00176 if (_wis == STALE) 00177 { 00178 win_image_state_=_wis; 00179 return; 00180 } 00181 00182 // Setting state=UPDATED when already UPDATED is a no-op. 00183 else if (win_image_state_ == UPDATED) 00184 return; 00185 00186 // Otherwise setting state=UPDATED generates a new image and updates the 00187 // window. 00188 else 00189 { 00190 if( image_.empty() ) 00191 { 00192 ROS_WARN("GCApp: window image state updated with empty image"); 00193 return; 00194 } 00195 00196 // Generate the display image 00197 Scalar color = BK; 00198 if (win_color_ == WHITE) color = WH; 00199 else if (win_color_ == GRAY) color = GY; 00200 else if (win_color_ == GREEN) color = GR; 00201 else if (win_color_ == BLUE) color = BL; 00202 00203 Mat display_image(image_.size().height, image_.size().width, 00204 image_.type(), color); 00205 Mat bin_mask; 00206 if ( !initialized_ ) 00207 // If we haven't created a mask, just copy the base image 00208 image_.copyTo( display_image ); 00209 else 00210 { 00211 binaryMaskFromGCMask( mask_, bin_mask ); 00212 image_.copyTo( display_image, bin_mask ); 00213 } 00214 00215 // Overlay the marked points, BG and FG, on the display image 00216 vector<Point>::const_iterator it; 00217 for( it = bgd_pxls_.begin(); it != bgd_pxls_.end(); ++it ) 00218 circle( display_image, *it, DOT_RADIUS, BL, LINE_THICKNESS ); 00219 for( it = fgd_pxls_.begin(); it != fgd_pxls_.end(); ++it ) 00220 circle( display_image, *it, DOT_RADIUS, RD, LINE_THICKNESS ); 00221 for( it = pr_bgd_pxls_.begin(); it != pr_bgd_pxls_.end(); ++it ) 00222 circle( display_image, *it, DOT_RADIUS, LB, LINE_THICKNESS ); 00223 for( it = pr_fgd_pxls_.begin(); it != pr_fgd_pxls_.end(); ++it ) 00224 circle( display_image, *it, DOT_RADIUS, PK, LINE_THICKNESS ); 00225 00226 // Add the rectangle 00227 if( rect_state_ == IN_PROCESS || rect_state_ == SET ) 00228 rectangle( display_image, 00229 Point( rect_.x, rect_.y ), 00230 Point(rect_.x + rect_.width, rect_.y + rect_.height ), 00231 GR, 2); 00232 00233 // Display the image 00234 imshow( win_name_, display_image ); 00235 } 00236 } 00237 00238 /* Private helper method to transfer pixels from designated rectangle to 00239 * Grabcut mask. */ 00240 void GC3DApplication::setRectInMask() 00241 { 00242 assert( !mask_.empty() ); 00243 mask_.setTo( GC_BGD ); 00244 rect_.x = max(0, rect_.x); 00245 rect_.y = max(0, rect_.y); 00246 rect_.width = min(rect_.width, image_.cols-rect_.x); 00247 rect_.height = min(rect_.height, image_.rows-rect_.y); 00248 (mask_(rect_)).setTo( Scalar(GC_PR_FGD) ); 00249 } 00250 00251 /* Private helper method to transfer labels designated in UI to mask. */ 00252 void GC3DApplication::setLblsInMask( int flags, Point p, bool isPr ) 00253 { 00254 vector<Point> *bpxls, *fpxls; 00255 uchar bvalue, fvalue; 00256 if( !isPr ) 00257 { 00258 bpxls = &bgd_pxls_; 00259 fpxls = &fgd_pxls_; 00260 bvalue = GC_BGD; 00261 fvalue = GC_FGD; 00262 } 00263 else 00264 { 00265 bpxls = &pr_bgd_pxls_; 00266 fpxls = &pr_fgd_pxls_; 00267 bvalue = GC_PR_BGD; 00268 fvalue = GC_PR_FGD; 00269 } 00270 00271 if( flags & BGD_KEY ) 00272 { 00273 bpxls->push_back(p); 00274 circle( mask_, p, DOT_RADIUS, bvalue, LINE_THICKNESS ); 00275 } 00276 if( flags & FGD_KEY ) 00277 { 00278 fpxls->push_back(p); 00279 circle( mask_, p, DOT_RADIUS, fvalue, LINE_THICKNESS ); 00280 } 00281 } 00282 00283 /* Mouse callback function passed to OpenCV window. Handles marking the ROI 00284 * rectangle and foreground and background pixel hints. */ 00285 void GC3DApplication::mouseClick( int event, int x, int y, int flags) 00286 { 00287 // TODO add bad args check 00288 switch( event ) 00289 { 00290 case CV_EVENT_LBUTTONDOWN: // set rect or GC_BGD(GC_FGD) labels 00291 { 00292 bool isb = (flags & BGD_KEY) != 0, 00293 isf = (flags & FGD_KEY) != 0; 00294 if( rect_state_ == NOT_SET && !isb && !isf ) 00295 { 00296 rect_state_ = IN_PROCESS; 00297 rect_ = Rect( x, y, 1, 1 ); 00298 } 00299 if ( (isb || isf) && rect_state_ == SET ) 00300 lbls_state_ = IN_PROCESS; 00301 } 00302 break; 00303 00304 case CV_EVENT_RBUTTONDOWN: // set GC_PR_BGD(GC_PR_FGD) labels 00305 { 00306 bool isb = (flags & BGD_KEY) != 0; 00307 bool isf = (flags & FGD_KEY) != 0; 00308 if ( (isb || isf) && rect_state_ == SET ) 00309 pr_lbls_state_ = IN_PROCESS; 00310 } 00311 break; 00312 00313 case CV_EVENT_LBUTTONUP: 00314 if( rect_state_ == IN_PROCESS ) 00315 { 00316 rect_ = Rect( Point(rect_.x, rect_.y), Point(x,y) ); 00317 rect_state_ = SET; 00318 setRectInMask(); 00319 assert( bgd_pxls_.empty() 00320 && fgd_pxls_.empty() 00321 && pr_bgd_pxls_.empty() 00322 && pr_fgd_pxls_.empty() ); 00323 winImageStateIs(UPDATED); 00324 } 00325 if( lbls_state_ == IN_PROCESS ) 00326 { 00327 setLblsInMask(flags, Point(x,y), false); 00328 lbls_state_ = SET; 00329 winImageStateIs(UPDATED); 00330 } 00331 break; 00332 00333 case CV_EVENT_RBUTTONUP: 00334 if( pr_lbls_state_ == IN_PROCESS ) 00335 { 00336 setLblsInMask(flags, Point(x,y), true); 00337 pr_lbls_state_ = SET; 00338 winImageStateIs(UPDATED); 00339 } 00340 break; 00341 00342 case CV_EVENT_MOUSEMOVE: 00343 if( rect_state_ == IN_PROCESS ) 00344 { 00345 rect_ = Rect( Point(rect_.x, rect_.y), Point(x,y) ); 00346 assert( bgd_pxls_.empty() 00347 && fgd_pxls_.empty() 00348 && pr_bgd_pxls_.empty() 00349 && pr_fgd_pxls_.empty() ); 00350 winImageStateIs(UPDATED); 00351 } 00352 else if( lbls_state_ == IN_PROCESS ) 00353 { 00354 setLblsInMask(flags, Point(x,y), false); 00355 winImageStateIs(UPDATED); 00356 } 00357 else if( pr_lbls_state_ == IN_PROCESS ) 00358 { 00359 setLblsInMask(flags, Point(x,y), true); 00360 winImageStateIs(UPDATED); 00361 } 00362 break; 00363 } 00364 } 00365 00366 void GC3DApplication::iterCountIs(int _icnt) 00367 { 00368 if (_icnt == 0) { 00369 iter_count_ = 0; 00370 return; 00371 } 00372 00373 int iter_inc = _icnt - iter_count_; 00374 if (iter_inc <= 0) return; 00375 00376 if( initialized() ) 00377 grabCut3D( image_, depth_image_, mask_, rect_, bgd_model_, fgd_model_, iter_inc ); 00378 else 00379 { 00380 if( rect_state_ != SET ) return; 00381 00382 if( lbls_state_ == SET || pr_lbls_state_ == SET ) 00383 grabCut3D( image_, depth_image_, mask_, rect_, bgd_model_, fgd_model_, 00384 iter_inc, GC_INIT_WITH_MASK ); 00385 else 00386 grabCut3D( image_, depth_image_, mask_, rect_, bgd_model_, fgd_model_, 00387 iter_inc, GC_INIT_WITH_RECT ); 00388 00389 initializedIs(true); 00390 } 00391 iter_count_ = _icnt; 00392 00393 bgd_pxls_.clear(); fgd_pxls_.clear(); 00394 pr_bgd_pxls_.clear(); pr_fgd_pxls_.clear(); 00395 00396 winImageStateIs(UPDATED); 00397 00398 return; 00399 } 00400 00401