$search
00001 #include <highgui.h> 00002 #include <cv.h> 00003 #include <iostream> 00004 #include <boost/make_shared.hpp> 00005 #include "pcl/point_types.h" 00006 #include "pcl/features/feature.h" 00007 #include "pcl/registration/transforms.h" 00008 00009 #include "grabcut_3d/grabcut_3d.h" 00010 00011 00012 using namespace std; 00013 using namespace cv; 00014 00015 const Scalar RED = Scalar(0,0,255); 00016 const Scalar PINK = Scalar(230,130,255); 00017 const Scalar BLUE = Scalar(255,0,0); 00018 const Scalar LIGHTBLUE = Scalar(255,255,160); 00019 const Scalar GREEN = Scalar(0,255,0); 00020 00021 const int BGD_KEY = CV_EVENT_FLAG_CTRLKEY; 00022 const int FGD_KEY = CV_EVENT_FLAG_SHIFTKEY; 00023 00024 00025 void getBinMask( const Mat& comMask, Mat& binMask ) 00026 { 00027 if( comMask.empty() || comMask.type()!=CV_8UC1 ) 00028 CV_Error( CV_StsBadArg, "comMask is empty or has incorrect type (not CV_8UC1)" ); 00029 if( binMask.empty() || binMask.rows!=comMask.rows || binMask.cols!=comMask.cols ) 00030 binMask.create( comMask.size(), CV_8UC1 ); 00031 binMask = comMask & 1; 00032 } 00033 00034 class GCApplication 00035 { 00036 public: 00037 enum{ NOT_SET = 0, IN_PROCESS = 1, SET = 2 }; 00038 static const int radius = 2; 00039 static const int thickness = -1; 00040 00041 void reset(); 00042 void setImageAndWinName( const Mat& _image, const Mat& _depth_image, const string& _winName ); 00043 void showImage() const; 00044 void mouseClick( int event, int x, int y, int flags, void* param ); 00045 int nextIter(); 00046 int getIterCount() const { return iterCount; } 00047 void setRect(const Rect& _rect); 00048 00049 private: 00050 void setRectInMask(); 00051 void setLblsInMask( int flags, Point p, bool isPr ); 00052 00053 const string* winName; 00054 const Mat* image; 00055 const Mat* depth_image; 00056 Mat mask; 00057 Mat bgdModel, fgdModel; 00058 00059 uchar rectState, lblsState, prLblsState; 00060 bool isInitialized; 00061 00062 Rect rect; 00063 vector<Point> fgdPxls, bgdPxls, prFgdPxls, prBgdPxls; 00064 int iterCount; 00065 }; 00066 00067 void GCApplication::setRect(const Rect& _rect) 00068 { 00069 rect = _rect; 00070 rectState = SET; 00071 setRectInMask(); 00072 assert( bgdPxls.empty() && fgdPxls.empty() && prBgdPxls.empty() && prFgdPxls.empty() ); 00073 showImage(); 00074 } 00075 00076 void GCApplication::reset() 00077 { 00078 if( !mask.empty() ) 00079 mask.setTo(Scalar::all(GC_BGD)); 00080 bgdPxls.clear(); fgdPxls.clear(); 00081 prBgdPxls.clear(); prFgdPxls.clear(); 00082 00083 isInitialized = false; 00084 rectState = NOT_SET; 00085 lblsState = NOT_SET; 00086 prLblsState = NOT_SET; 00087 iterCount = 0; 00088 } 00089 00090 void GCApplication::setImageAndWinName( const Mat& _image, const Mat& _depth_image, const string& _winName ) 00091 { 00092 if( _image.empty() || _depth_image.empty() || _winName.empty() ) 00093 return; 00094 image = &_image; 00095 depth_image = &_depth_image; 00096 winName = &_winName; 00097 mask.create( image->size(), CV_8UC1); 00098 reset(); 00099 } 00100 00101 void GCApplication::showImage() const 00102 { 00103 if( image->empty() || winName->empty() ) 00104 return; 00105 00106 Mat res; 00107 Mat binMask; 00108 if( !isInitialized ) 00109 image->copyTo( res ); 00110 else 00111 { 00112 getBinMask( mask, binMask ); 00113 image->copyTo( res, binMask ); 00114 } 00115 00116 vector<Point>::const_iterator it; 00117 for( it = bgdPxls.begin(); it != bgdPxls.end(); ++it ) 00118 circle( res, *it, radius, BLUE, thickness ); 00119 for( it = fgdPxls.begin(); it != fgdPxls.end(); ++it ) 00120 circle( res, *it, radius, RED, thickness ); 00121 for( it = prBgdPxls.begin(); it != prBgdPxls.end(); ++it ) 00122 circle( res, *it, radius, LIGHTBLUE, thickness ); 00123 for( it = prFgdPxls.begin(); it != prFgdPxls.end(); ++it ) 00124 circle( res, *it, radius, PINK, thickness ); 00125 00126 if( rectState == IN_PROCESS || rectState == SET ) 00127 rectangle( res, Point( rect.x, rect.y ), Point(rect.x + rect.width, rect.y + rect.height ), GREEN, 2); 00128 00129 imshow( *winName, res ); 00130 } 00131 00132 void GCApplication::setRectInMask() 00133 { 00134 assert( !mask.empty() ); 00135 mask.setTo( GC_BGD ); 00136 rect.x = max(0, rect.x); 00137 rect.y = max(0, rect.y); 00138 rect.width = min(rect.width, image->cols-rect.x); 00139 rect.height = min(rect.height, image->rows-rect.y); 00140 (mask(rect)).setTo( Scalar(GC_PR_FGD) ); 00141 } 00142 00143 void GCApplication::setLblsInMask( int flags, Point p, bool isPr ) 00144 { 00145 vector<Point> *bpxls, *fpxls; 00146 uchar bvalue, fvalue; 00147 if( !isPr ) 00148 { 00149 bpxls = &bgdPxls; 00150 fpxls = &fgdPxls; 00151 bvalue = GC_BGD; 00152 fvalue = GC_FGD; 00153 } 00154 else 00155 { 00156 bpxls = &prBgdPxls; 00157 fpxls = &prFgdPxls; 00158 bvalue = GC_PR_BGD; 00159 fvalue = GC_PR_FGD; 00160 } 00161 if( flags & BGD_KEY ) 00162 { 00163 bpxls->push_back(p); 00164 circle( mask, p, radius, bvalue, thickness ); 00165 } 00166 if( flags & FGD_KEY ) 00167 { 00168 fpxls->push_back(p); 00169 circle( mask, p, radius, fvalue, thickness ); 00170 } 00171 } 00172 00173 void GCApplication::mouseClick( int event, int x, int y, int flags, void* param ) 00174 { 00175 // TODO add bad args check 00176 switch( event ) 00177 { 00178 case CV_EVENT_LBUTTONDOWN: // set rect or GC_BGD(GC_FGD) labels 00179 { 00180 bool isb = (flags & BGD_KEY) != 0, 00181 isf = (flags & FGD_KEY) != 0; 00182 if( rectState == NOT_SET && !isb && !isf ) 00183 { 00184 rectState = IN_PROCESS; 00185 rect = Rect( x, y, 1, 1 ); 00186 } 00187 if ( (isb || isf) && rectState == SET ) 00188 lblsState = IN_PROCESS; 00189 } 00190 break; 00191 case CV_EVENT_RBUTTONDOWN: // set GC_PR_BGD(GC_PR_FGD) labels 00192 { 00193 bool isb = (flags & BGD_KEY) != 0, 00194 isf = (flags & FGD_KEY) != 0; 00195 if ( (isb || isf) && rectState == SET ) 00196 prLblsState = IN_PROCESS; 00197 } 00198 break; 00199 case CV_EVENT_LBUTTONUP: 00200 if( rectState == IN_PROCESS ) 00201 { 00202 std::cout << "Rect: " << rect.x << " " << rect.y << " " << x << " " << y << std::endl; 00203 rect = Rect( Point(rect.x, rect.y), Point(x,y) ); 00204 rectState = SET; 00205 setRectInMask(); 00206 assert( bgdPxls.empty() && fgdPxls.empty() && prBgdPxls.empty() && prFgdPxls.empty() ); 00207 showImage(); 00208 } 00209 if( lblsState == IN_PROCESS ) 00210 { 00211 setLblsInMask(flags, Point(x,y), false); 00212 lblsState = SET; 00213 showImage(); 00214 } 00215 break; 00216 case CV_EVENT_RBUTTONUP: 00217 if( prLblsState == IN_PROCESS ) 00218 { 00219 setLblsInMask(flags, Point(x,y), true); 00220 prLblsState = SET; 00221 showImage(); 00222 } 00223 break; 00224 case CV_EVENT_MOUSEMOVE: 00225 if( rectState == IN_PROCESS ) 00226 { 00227 rect = Rect( Point(rect.x, rect.y), Point(x,y) ); 00228 assert( bgdPxls.empty() && fgdPxls.empty() && prBgdPxls.empty() && prFgdPxls.empty() ); 00229 showImage(); 00230 } 00231 else if( lblsState == IN_PROCESS ) 00232 { 00233 setLblsInMask(flags, Point(x,y), false); 00234 showImage(); 00235 } 00236 else if( prLblsState == IN_PROCESS ) 00237 { 00238 setLblsInMask(flags, Point(x,y), true); 00239 showImage(); 00240 } 00241 break; 00242 } 00243 } 00244 00245 int GCApplication::nextIter() 00246 { 00247 if( isInitialized ) 00248 grabCut3D( *image, *depth_image, mask, rect, bgdModel, fgdModel, 1 ); 00249 else 00250 { 00251 if( rectState != SET ) 00252 return iterCount; 00253 00254 if( lblsState == SET || prLblsState == SET ) 00255 grabCut3D( *image, *depth_image, mask, rect, bgdModel, fgdModel, 1, GC_INIT_WITH_MASK ); 00256 else 00257 grabCut3D( *image, *depth_image, mask, rect, bgdModel, fgdModel, 1, GC_INIT_WITH_RECT ); 00258 00259 isInitialized = true; 00260 } 00261 iterCount++; 00262 00263 bgdPxls.clear(); fgdPxls.clear(); 00264 prBgdPxls.clear(); prFgdPxls.clear(); 00265 00266 return iterCount; 00267 } 00268 00269 GCApplication gcapp; 00270 00271 void on_mouse( int event, int x, int y, int flags, void* param ) 00272 { 00273 gcapp.mouseClick( event, x, y, flags, param ); 00274 } 00275 00276 void interpolateDepthImageNearestNeighbor(Mat& depth_image) 00277 { 00278 // create point cloud 00279 pcl::PointCloud<pcl::PointXY> cloud; 00280 00281 // create delaunay vertex from 2D image coordinates 00282 Size size = depth_image.size(); 00283 int point_id = 0; 00284 for( int r = 0; r < size.height; r++ ) 00285 { 00286 float* lineptr = depth_image.ptr<float>(r); 00287 for( int c = 0; c < size.width; c++ ) 00288 { 00289 if(lineptr[c]>0) { 00290 pcl::PointXY p; 00291 p.x = (float)c; 00292 p.y = (float)r; 00293 cloud.points.push_back(p); 00294 } 00295 } 00296 } 00297 00298 // create kdtree 00299 pcl::KdTreeFLANN<pcl::PointXY> kdtree; 00300 std::vector<int> k_indices(1); 00301 std::vector<float> k_distances(1); 00302 00303 pcl::PointCloud<pcl::PointXY>::ConstPtr cloud_ptr = boost::make_shared<const pcl::PointCloud<pcl::PointXY> > (cloud); 00304 kdtree.setInputCloud(cloud_ptr); 00305 00306 00307 for( int r = 0; r < size.height; r++ ) 00308 { 00309 float* lineptr = depth_image.ptr<float>(r); 00310 for( int c = 0; c < size.width; c++ ) 00311 { 00312 if(lineptr[c]==0) { 00313 // interpolate this value 00314 pcl::PointXY p; 00315 p.x = (float)c; 00316 p.y = (float)r; 00317 //k_indices.clear();k_distances.clear(); 00318 kdtree.nearestKSearch(p, 1, k_indices, k_distances); 00319 if(k_distances[0]<20.0) { 00320 const pcl::PointXY& closest_point = cloud.points[k_indices[0]]; 00321 lineptr[c] = depth_image.at<float>(closest_point.y,closest_point.x); 00322 } 00323 } 00324 } 00325 } 00326 } 00327 00328 int main( int argc, char** argv ) 00329 { 00330 if( argc==1 ) 00331 return 1; 00332 string filename_image = argv[1]; 00333 string filename_depth_image = argv[2]; 00334 00335 if( filename_image.empty() || filename_depth_image.empty() ) 00336 { 00337 std::cout << "Please provide two arguments: " << argv[0] << " color_image depth_image" << std::endl; 00338 return 1; 00339 } 00340 00341 Mat image = imread( filename_image, 1 ); 00342 if( image.empty() ) 00343 { 00344 std::cout << "Could not load image: " << filename_image << std::endl; 00345 return 1; 00346 } 00347 00348 Mat depth_image = imread( filename_depth_image, 1 ); 00349 if( depth_image.empty() ) 00350 { 00351 std::cout << "Could not load depth image: " << filename_depth_image << std::endl; 00352 return 1; 00353 } 00354 // apply median filter 00355 Mat depth_image2 = depth_image.clone(); 00356 medianBlur(depth_image2, depth_image, 11); 00357 00358 cout << "First, select the rectangular area\n" << 00359 "Hot keys: \n" 00360 "\tESC - quit the program\n" 00361 "\tr - restore the original image\n" 00362 "\tn - next iteration\n" 00363 "\n" 00364 "\tleft mouse button - set rectangle\n" 00365 "\n" 00366 "\tCTRL+left mouse button - set GC_BGD pixels\n" 00367 "\tSHIFT+left mouse button - set CG_FGD pixels\n" 00368 "\n" 00369 "\tCTRL+right mouse button - set GC_PR_BGD pixels\n" 00370 "\tSHIFT+right mouse button - set CG_PR_FGD pixels\n"; 00371 00372 const string winName = "image"; 00373 cvNamedWindow( winName.c_str(), CV_WINDOW_AUTOSIZE ); 00374 cvSetMouseCallback( winName.c_str(), on_mouse, 0 ); 00375 00376 gcapp.setImageAndWinName( image, depth_image, winName ); 00377 gcapp.showImage(); 00378 00379 for(;;) 00380 { 00381 int c = cvWaitKey(0); 00382 switch( (char) c ) 00383 { 00384 case 'q': 00385 cout << "Exiting ..." << endl; 00386 goto exit_main; 00387 case 'r': 00388 cout << endl; 00389 gcapp.reset(); 00390 gcapp.showImage(); 00391 break; 00392 case 'n': 00393 int iterCount = gcapp.getIterCount(); 00394 cout << "<" << iterCount << "... "; 00395 int newIterCount = gcapp.nextIter(); 00396 if( newIterCount > iterCount ) 00397 { 00398 gcapp.showImage(); 00399 cout << iterCount << ">" << endl; 00400 } 00401 else 00402 cout << "rect must be determined>" << endl; 00403 break; 00404 } 00405 } 00406 00407 exit_main: 00408 cvDestroyWindow( winName.c_str() ); 00409 return 0; 00410 }