bosch_object_segmentation_gui: bosch_object_segmentation_gui: grabcut3d_object

00001 /*********************************************************************
00002  *
00003  * Software License Agreement (BSD License)
00004  *
00005  *  Copyright (c) 2011, Robert Bosch LLC.
00006  *  All rights reserved.
00007  *
00008  *  Redistribution and use in source and binary forms, with or without
00009  *  modification, are permitted provided that the following conditions
00010  *  are met:
00011  *
00012  *   * Redistributions of source code must retain the above copyright
00013  *     notice, this list of conditions and the following disclaimer.
00014  *   * Redistributions in binary form must reproduce the above
00015  *     copyright notice, this list of conditions and the following
00016  *     disclaimer in the documentation and/or other materials provided
00017  *     with the distribution.
00018  *   * Neither the name of the Robert Bosch nor the names of its
00019  *     contributors may be used to endorse or promote products derived
00020  *     from this software without specific prior written permission.
00021  *
00022  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00023  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00024  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00025  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00026  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00027  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00028  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00029  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00030  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00031  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00032  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00033  *  POSSIBILITY OF SUCH DAMAGE.
00034  *
00035  *********************************************************************/
00036 
00037 #include "bosch_object_segmentation_gui/grabcut3d_object_segmenter.h"
00038 #include "grabcut_3d/grabcut_3d.h"
00039 
00040 using namespace std;
00041 using namespace cv;
00042 
00043 namespace bosch_object_segmentation_gui {
00044 
00045 static const Scalar RD = Scalar(0,0,255);
00046 static const Scalar PK = Scalar(230,130,255);
00047 static const Scalar BL = Scalar(255,0,0);
00048 static const Scalar LB = Scalar(255,255,160);
00049 static const Scalar GR = Scalar(0,255,0);
00050 static const Scalar WH = Scalar(255,255,255);
00051 static const Scalar GY = Scalar(128,128,128);
00052 static const Scalar BK = Scalar(0,0,0);
00053 static const int DOT_RADIUS = 2;
00054 static const int LINE_THICKNESS = -1;
00055 
00056 // Initialize default rectangle marker (not actually used as a rectangle)
00057 const Rect GrabCut3DObjectSegmenter::DEFAULT_RECT(0,0,0,0);
00058 // This scale factor is what is actually used to calculate the default
00059 // rectangle.
00060 const int DEFAULT_RECT_SCALE_FACTOR = 2;
00061 
00062 /* Helper function to convert mask in 4-value GrabCut format to a binary mask
00063  * with foreground = 1, background = 0. */
00064 static void
00065 binaryMaskFromGCMask(const Mat &_gc_mask, Mat &_bin_mask)
00066 {
00067     if( _gc_mask.empty() || _gc_mask.type() != CV_8UC1 )
00068         CV_Error( CV_StsBadArg, "_gc_mask is empty or has incorrect type (not CV_8UC1)" );
00069 
00070     if(    _bin_mask.empty()
00071         || _bin_mask.rows!=_gc_mask.rows
00072         || _bin_mask.cols!=_gc_mask.cols )
00073         _bin_mask.create( _gc_mask.size(), CV_8UC1 );
00074 
00075     // Convert GC_BGD and GC_PR_BGD to 0, GC_FGD and GC_PR_FGD to 1
00076     _bin_mask = _gc_mask & 1;
00077 }
00078 
00079 GrabCut3DObjectSegmenter::GrabCut3DObjectSegmenter()
00080 {
00081   // Initialize window
00082   //setImages(_img,_depth_image);
00083   updateDisplayImage();
00084 
00085   iter_count_ = 0;
00086 }
00087 
00088 GrabCut3DObjectSegmenter::~GrabCut3DObjectSegmenter()
00089 {
00090 
00091 }
00092 
00093 /* Mutator for initialized state of GrabCut3DObjectSegmenter object. Client can set
00094  * initialized=true with no side effects; setting initialized=false clears
00095  * background and foreground pixel arrays and returns application state
00096  * variables to NOT_SET. */
00097 // TODO: Should this be public?
00098 void
00099 GrabCut3DObjectSegmenter::initializedIs(bool _init)
00100 {
00101   if (_init)
00102     initialized_=_init;
00103   else
00104   {
00105     if ( !mask_.empty() ) mask_.setTo(Scalar::all(GC_BGD));
00106     bgd_pxls_.clear();   fgd_pxls_.clear();
00107     pr_bgd_pxls_.clear();  pr_fgd_pxls_.clear();
00108     initialized_   = _init;
00109     rect_state_ = NOT_SET;
00110     lbls_state_ = NOT_SET;
00111     pr_lbls_state_ = NOT_SET;
00112     iterCountIs(0);
00113     updateDisplayImage();
00114   }
00115 }
00116 
00117 /* Mutator for stored image. */
00118 void GrabCut3DObjectSegmenter::setImages(const Mat& _image, const Mat& _depth_image)
00119 {
00120     if( _image.empty() ) {
00121       ROS_WARN("GrabCut3DObjectSegmenter: imageIs called with empty image");
00122       return;
00123     }
00124     image_ = _image.clone();
00125     depth_image_ = _depth_image.clone();
00126     display_image_ = image_.clone();
00127 
00128     mask_.create( image_.size(), CV_8UC1);
00129     initializedIs(false);
00130 }
00131 
00132 /* Mutator for rectangle. */
00133 void GrabCut3DObjectSegmenter::rectIs(const Rect &_r)
00134 {
00135   if (_r == DEFAULT_RECT)
00136     rect_ = Rect(image_.cols/(DEFAULT_RECT_SCALE_FACTOR*2),
00137                  image_.rows/(DEFAULT_RECT_SCALE_FACTOR*2),
00138                  image_.cols/DEFAULT_RECT_SCALE_FACTOR,
00139                  image_.rows/DEFAULT_RECT_SCALE_FACTOR);
00140   else
00141     rect_=_r;
00142 
00143   rect_state_ = SET;
00144   setRectInMask();
00145   assert( bgd_pxls_.empty()
00146   && fgd_pxls_.empty()
00147   && pr_bgd_pxls_.empty()
00148   && pr_fgd_pxls_.empty() );
00149   updateDisplayImage();
00150 
00151 }
00152 
00153 /* Accessor for binary mask. */
00154 Mat GrabCut3DObjectSegmenter::binaryMask()
00155 {
00156   Mat bin_mask;
00157   /*
00158   cv::imwrite("/home/stm1pal/ros/stacks/bosch-ros-pkg/mas/grabcut_app/full_mask.pbm", mask_);
00159   double fm_max;
00160   double fm_min;
00161   minMaxLoc(mask_, &fm_min, &fm_max);
00162   std::cout << "fm_max: " << fm_max << std::endl;
00163   std::cout << "fm_min: " << fm_min << std::endl;
00164   */
00165 
00166   binaryMaskFromGCMask(mask_, bin_mask);
00167   /*
00168   cv::imwrite("/home/stm1pal/ros/stacks/bosch-ros-pkg/mas/grabcut_app/bin_mask.pbm", bin_mask);
00169   double bm_max;
00170   double bm_min;
00171   minMaxLoc(bin_mask, &bm_min, &bm_max);
00172   std::cout << "bm_max: " << bm_max << std::endl;
00173   std::cout << "bm_min: " << bm_min << std::endl;
00174   */
00175   return bin_mask;
00176 }
00177 
00178 /* Mutator for window background color. */
00179 void GrabCut3DObjectSegmenter::setWinColor(WinColor _c)
00180 {
00181   win_color_=_c;
00182   updateDisplayImage();
00183 }
00184 
00185 void GrabCut3DObjectSegmenter::updateDisplayImage()
00186 {
00187 
00188   if( image_.empty() )
00189   {
00190     ROS_WARN("GrabCut3DObjectSegmenter: window image state updated with empty image");
00191     return;
00192   }
00193 
00194   // Generate the display image
00195   Scalar color = BK;
00196   if (win_color_ == WHITE) color = WH;
00197   else if (win_color_ == GRAY) color = GY;
00198   else if (win_color_ == GREEN) color = GR;
00199   else if (win_color_ == BLUE) color = BL;
00200 
00201   display_image_.setTo(color);
00202   Mat bin_mask;
00203   if ( !initialized_ )
00204     // If we haven't created a mask, just copy the base image
00205     image_.copyTo( display_image_ );
00206   else
00207   {
00208     binaryMaskFromGCMask( mask_, bin_mask );
00209     image_.copyTo( display_image_, bin_mask );
00210   }
00211 
00212   // Overlay the marked points, BG and FG, on the display image
00213   vector<Point>::const_iterator it;
00214   for( it = bgd_pxls_.begin(); it != bgd_pxls_.end(); ++it )
00215     circle( display_image_, *it, DOT_RADIUS, BL, LINE_THICKNESS );
00216   for( it = fgd_pxls_.begin(); it != fgd_pxls_.end(); ++it )
00217     circle( display_image_, *it, DOT_RADIUS, RD, LINE_THICKNESS );
00218   for( it = pr_bgd_pxls_.begin(); it != pr_bgd_pxls_.end(); ++it )
00219     circle( display_image_, *it, DOT_RADIUS, LB, LINE_THICKNESS );
00220   for( it = pr_fgd_pxls_.begin(); it != pr_fgd_pxls_.end(); ++it )
00221     circle( display_image_, *it, DOT_RADIUS, PK, LINE_THICKNESS );
00222 
00223   // Add the rectangle
00224   if( rect_state_ == IN_PROCESS || rect_state_ == SET )
00225     rectangle( display_image_,
00226         Point( rect_.x, rect_.y ),
00227         Point(rect_.x + rect_.width, rect_.y + rect_.height ),
00228         GR, 2);
00229 }
00230 
00231 /* Private helper method to transfer pixels from designated rectangle to
00232  * Grabcut mask. */
00233 void GrabCut3DObjectSegmenter::setRectInMask()
00234 {
00235     assert( !mask_.empty() );
00236     mask_.setTo( GC_BGD );
00237     rect_.x = max(0, rect_.x);
00238     rect_.y = max(0, rect_.y);
00239     rect_.width = min(rect_.width, image_.cols-rect_.x);
00240     rect_.height = min(rect_.height, image_.rows-rect_.y);
00241     (mask_(rect_)).setTo( Scalar(GC_PR_FGD) );
00242 }
00243 
00244 /* Private helper method to transfer labels designated in UI to mask. */
00245 void GrabCut3DObjectSegmenter::setLblsInMask(bool control_down, bool shift_down, Point p, bool isPr )
00246 {
00247     vector<Point> *bpxls, *fpxls;
00248     uchar bvalue, fvalue;
00249     if( !isPr )
00250     {
00251       bpxls = &bgd_pxls_;
00252       fpxls = &fgd_pxls_;
00253       bvalue = GC_BGD;
00254       fvalue = GC_FGD;
00255     }
00256     else
00257     {
00258       bpxls = &pr_bgd_pxls_;
00259       fpxls = &pr_fgd_pxls_;
00260       bvalue = GC_PR_BGD;
00261       fvalue = GC_PR_FGD;
00262     }
00263 
00264     if( control_down )
00265     {
00266       bpxls->push_back(p);
00267       circle( mask_, p, DOT_RADIUS, bvalue, LINE_THICKNESS );
00268     }
00269     if( shift_down )
00270     {
00271       fpxls->push_back(p);
00272       circle( mask_, p, DOT_RADIUS, fvalue, LINE_THICKNESS );
00273     }
00274 }
00275 
00276 /* Mouse callback function passed to OpenCV window. Handles marking the ROI
00277  * rectangle and foreground and background pixel hints. */
00278 void GrabCut3DObjectSegmenter::mouseClick( MouseEvent event, int x, int y, bool control_down, bool shift_down)
00279 {
00280     // TODO add bad args check
00281   switch( event )
00282   {
00283     case LEFT_BUTTON_DOWN: // set rect or GC_BGD(GC_FGD) labels
00284       {
00285         bool isb = control_down;
00286         bool isf = shift_down;
00287         if( rect_state_ == NOT_SET && !isb && !isf )
00288         {
00289           rect_state_ = IN_PROCESS;
00290           rect_ = Rect( x, y, 1, 1 );
00291         }
00292         if ( (isb || isf) && rect_state_ == SET )
00293           lbls_state_ = IN_PROCESS;
00294       }
00295       break;
00296 
00297     case RIGHT_BUTTON_DOWN: // set GC_PR_BGD(GC_PR_FGD) labels
00298       {
00299         bool isb = control_down;
00300         bool isf = shift_down;
00301         if ( (isb || isf) && rect_state_ == SET )
00302           pr_lbls_state_ = IN_PROCESS;
00303       }
00304       break;
00305 
00306     case LEFT_BUTTON_UP:
00307       if( rect_state_ == IN_PROCESS )
00308       {
00309         rect_ = Rect( Point(rect_.x, rect_.y), Point(x,y) );
00310         rect_state_ = SET;
00311         setRectInMask();
00312         assert( bgd_pxls_.empty()
00313                && fgd_pxls_.empty()
00314                && pr_bgd_pxls_.empty()
00315                && pr_fgd_pxls_.empty() );
00316         updateDisplayImage();
00317       }
00318       if( lbls_state_ == IN_PROCESS )
00319       {
00320         setLblsInMask(control_down, shift_down, Point(x,y), false);
00321         lbls_state_ = SET;
00322         updateDisplayImage();
00323       }
00324       break;
00325 
00326     case RIGHT_BUTTON_UP:
00327       if( pr_lbls_state_ == IN_PROCESS )
00328       {
00329         setLblsInMask(control_down, shift_down, Point(x,y), true);
00330         pr_lbls_state_ = SET;
00331         updateDisplayImage();
00332       }
00333       break;
00334 
00335     case MOUSE_MOVE:
00336       if( rect_state_ == IN_PROCESS )
00337       {
00338         rect_ = Rect( Point(rect_.x, rect_.y), Point(x,y) );
00339         assert( bgd_pxls_.empty()
00340                && fgd_pxls_.empty()
00341                && pr_bgd_pxls_.empty()
00342                && pr_fgd_pxls_.empty() );
00343         updateDisplayImage();
00344       }
00345       else if( lbls_state_ == IN_PROCESS )
00346       {
00347         setLblsInMask(control_down, shift_down, Point(x,y), false);
00348         updateDisplayImage();
00349       }
00350       else if( pr_lbls_state_ == IN_PROCESS )
00351       {
00352         setLblsInMask(control_down, shift_down, Point(x,y), true);
00353         updateDisplayImage();
00354       }
00355       break;
00356   }
00357 }
00358 
00359 void GrabCut3DObjectSegmenter::iterCountIs(int _icnt)
00360 {
00361   if (_icnt == 0) {
00362     iter_count_ = 0;
00363     return;
00364   }
00365 
00366   int iter_inc = _icnt - iter_count_;
00367   if (iter_inc <= 0) return;
00368 
00369   if( initialized() )
00370     grabCut3D( image_, depth_image_, mask_, rect_, bgd_model_, fgd_model_, iter_inc );
00371   else
00372   {
00373     if( rect_state_ != SET ) return;
00374 
00375     if( lbls_state_ == SET || pr_lbls_state_ == SET )
00376       grabCut3D( image_, depth_image_, mask_, rect_, bgd_model_, fgd_model_,
00377                iter_inc, GC_INIT_WITH_MASK );
00378     else
00379       grabCut3D( image_, depth_image_, mask_, rect_, bgd_model_, fgd_model_,
00380                iter_inc, GC_INIT_WITH_RECT );
00381 
00382     initializedIs(true);
00383   }
00384   iter_count_ = _icnt;
00385 
00386   bgd_pxls_.clear(); fgd_pxls_.clear();
00387   pr_bgd_pxls_.clear(); pr_fgd_pxls_.clear();
00388 
00389   updateDisplayImage();
00390 
00391   return;
00392 }
00393 
00394 }
00395
grabcut3d_object_segmenter.cpp
