chromaticmask.cpp
Go to the documentation of this file.
00001 /*****************************
00002 Copyright 2011 Rafael Muñoz Salinas. All rights reserved.
00003 
00004 Redistribution and use in source and binary forms, with or without modification, are
00005 permitted provided that the following conditions are met:
00006 
00007    1. Redistributions of source code must retain the above copyright notice, this list of
00008       conditions and the following disclaimer.
00009 
00010    2. Redistributions in binary form must reproduce the above copyright notice, this list
00011       of conditions and the following disclaimer in the documentation and/or other materials
00012       provided with the distribution.
00013 
00014 THIS SOFTWARE IS PROVIDED BY Rafael Muñoz Salinas ''AS IS'' AND ANY EXPRESS OR IMPLIED
00015 WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND
00016 FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL Rafael Muñoz Salinas OR
00017 CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00018 CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
00019 SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON
00020 ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
00021 NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF
00022 ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
00023 
00024 The views and conclusions contained in the software and documentation are those of the
00025 authors and should not be interpreted as representing official policies, either expressed
00026 or implied, of Rafael Muñoz Salinas.
00027 ********************************/
00028 
00029 #include <aruco/chromaticmask.h>
00030 #include <set>
00031 // #include <omp.h>
00032 
00033 
00036 EMClassifier::EMClassifier(unsigned int nelements) : _classifier(2, cv::EM::COV_MAT_DIAGONAL, cv::TermCriteria(cv::TermCriteria::COUNT , 4, FLT_EPSILON))
00037 {
00038   _nelem = nelements;
00039   _threshProb = 0.0001;
00040   for(unsigned int i=0; i<256; i++) _prob[i] = 0.5;
00041 }
00042 
00043 
00044 
00047 void EMClassifier::train()
00048 {
00049 
00050   // fill histogram
00051     
00052   for(unsigned int i=0; i<256;i++) _histogram[i]=0;
00053   
00054   for(unsigned int i=0; i<_samples.size(); i++) {
00055     uchar val = _samples[i];
00056     _histogram[val]+=3;
00057     if(val>0) _histogram[val-1]+=2;
00058     if(val<255) _histogram[val+1]+=2;
00059     if(val>1) _histogram[val-2]+=1;
00060     if(val<254) _histogram[val+2]+=1;     
00061   }
00062   
00063   double sum=0.;     
00064   for(unsigned int i=0; i<256;i++) sum += _histogram[i];
00065   for(unsigned int i=0; i<256;i++) _histogram[i] /= sum;
00066   
00067   
00068   // discretize histogram to speed up training
00069   unsigned int histCount[256];
00070   int  n=0;
00071   for(unsigned int i=0; i<256; i++)
00072     n+= (histCount[i] = (unsigned int)(_nelem*_histogram[i]));
00073 
00074   if(n<10) return;
00075   
00076   cv::Mat samples(n,1,CV_64FC1);
00077   unsigned int idx=0;
00078   for(unsigned int i=0; i<256; i++) {
00079     for(unsigned int j=0; j<histCount[i]; j++) {
00080       samples.ptr<double>(0)[idx] = i;
00081       idx++;
00082     }
00083   }  
00084   
00085   _classifier.train(samples);
00086   
00087   cv::Mat sampleAux(1,1,CV_64FC1);
00088   for(unsigned int i=0; i<256; i++) {
00089     sampleAux.ptr<double>(0)[0] = i;
00090     cv::Vec2d r = _classifier.predict(sampleAux);
00091     _prob[i] = exp(r[0]);
00092     if(_prob[i]>_threshProb) _inside[i]=true;
00093     else _inside[i]=false;
00094   }  
00095   
00096   
00097 }
00098 
00099 
00100 void ChromaticMask::setParams(unsigned int mc, unsigned int nc, double threshProb, aruco::CameraParameters CP, aruco::BoardConfiguration BC, float markersize)
00101 {
00102   if(markersize==-1) {
00103     std::cerr << "Invalid markersize in ChromaticMask::setParams" << std::endl;
00104     return;
00105   }
00106   
00107   if(BC.size()==0) {
00108     std::cerr << "Invalid BoardConfiguration size in ChromaticMask::setParams" << std::endl;
00109     return;
00110   }  
00111    
00112   //calculate corners from min and max in BC
00113   cv::Point3f min, max;
00114   min = max = BC[0][0];
00115   for(unsigned int i=0; i<BC.size(); i++) {
00116     for(unsigned int j=0; j<4; j++) {
00117       if(BC[i][j].x <= min.x &&  BC[i][j].y <= min.y) min = BC[i][j];
00118       if(BC[i][j].x >= max.x &&  BC[i][j].y >= max.y) max = BC[i][j];
00119     }
00120   }
00121   double pixSize = fabs(markersize/(BC[0][1].x - BC[0][0].x));
00122   min.x *= pixSize;
00123   min.y *= pixSize;
00124   max.x *= pixSize;
00125   max.y *= pixSize;  
00126         
00127   // calculate border corners coordinates
00128   vector<cv::Point3f> corners;
00129   corners.push_back(min);
00130   corners.push_back( cv::Point3f(min.x, max.y, 0) );
00131   corners.push_back(max);
00132   corners.push_back( cv::Point3f(max.x, min.y, 0) );
00133     
00134   setParams(mc, nc, threshProb, CP, BC, corners);
00135   
00136 }
00137 
00138 
00139 
00142 void ChromaticMask::setParams(unsigned int mc, unsigned int nc, double threshProb, aruco::CameraParameters CP, aruco::BoardConfiguration BC, vector<cv::Point3f> corners)
00143 {
00144   _classifiers.resize(mc*nc);
00145   for(unsigned int i=0; i<_classifiers.size(); i++) _classifiers[i].setProb(threshProb);
00146   _mc = mc;
00147   _nc = nc;
00148   _threshProb = threshProb;
00149   _cell_neighbours.resize(mc*nc);
00150   int idx_=0;
00151   //SGJ: revisar, no engo claro sepa bien que es i y j y los puedo estar confundiendo!!!
00152     for(unsigned int j=0; j<nc; j++)
00153       for(unsigned int i=0; i<mc; i++,idx_++) {
00154       _centers.push_back(cv::Point2f(i+0.5, j+0.5));
00155       for(unsigned int nj=max(j-1,(unsigned int)0); nj<min(mc,j+1); nj++) {
00156          for(unsigned int ni=max(i-1,(unsigned int)0); ni<min(mc,i+1); ni++) {
00157            size_t tidx=nj*mc+ni;
00158            _cell_neighbours[idx_].push_back(tidx);           
00159         }      
00160       }
00161     }
00162   
00163     //deterimne the idx of the neighbours
00164   _BD.getMarkerDetector().setThresholdParams(35,7);
00165   _BD.getMarkerDetector().enableErosion(false);
00166   _BD.getMarkerDetector().setCornerRefinementMethod(aruco::MarkerDetector::LINES);
00167   _BD.setYPerpendicular(false);
00168   
00169   _BD.setParams(BC, CP);
00170   
00171   _objCornerPoints = corners;
00172   _CP = CP;
00173   
00174   _pixelsVector.clear();
00175   for(unsigned int i=0; i<CP.CamSize.height/2; i++)
00176     for(unsigned int j=0; j<CP.CamSize.width/2; j++)
00177       _pixelsVector.push_back( cv::Point2f(2*j,2*i) );
00178   
00179   resetMask();
00180   _cellMap = cv::Mat(CP.CamSize.height, CP.CamSize.width, CV_8UC1, cv::Scalar::all(0));
00181   _canonicalPos = cv::Mat(CP.CamSize.height, CP.CamSize.width, CV_8UC2);
00182     
00183   _cellCenters.resize(_classifiers.size());
00184   for(unsigned int i=0; i<mc; i++) {
00185     for(unsigned int j=0; j<nc; j++) {
00186       _cellCenters[i*mc+j].x = ((2*i+1)*_cellSize)/2.;
00187       _cellCenters[i*mc+j].y = ((2*j+1)*_cellSize)/2.;
00188     }
00189   }
00190   
00191 }
00192 
00193 pair<double,double> AvrgTime(0,0) ;
00194 
00197 void ChromaticMask::calculateGridImage(const aruco::Board &board )
00198 {
00199   
00200       // project corner points to image
00201       cv::projectPoints(_objCornerPoints, board.Rvec, board.Tvec, _CP.CameraMatrix, _CP.Distorsion, _imgCornerPoints);    
00202 
00203       //obtain the perspective transform
00204       cv::Point2f  pointsRes[4],pointsIn[4];
00205       for ( int i=0;i<4;i++ ) pointsIn[i]=_imgCornerPoints[i];
00206       pointsRes[0]= ( cv::Point2f ( 0,0 ) );
00207       pointsRes[1]= cv::Point2f ( _cellSize*_mc-1, 0 );
00208       pointsRes[2]= cv::Point2f ( _cellSize*_mc-1, _cellSize*_nc-1 );
00209       pointsRes[3]= cv::Point2f ( 0, _cellSize*_nc-1 );
00210       _perpTrans=cv::getPerspectiveTransform ( pointsIn,pointsRes );
00211       
00212 double tick = (double)cv::getTickCount();       
00213                 
00214       vector<cv::Point2f> transformedPixels;
00215       cv::perspectiveTransform(_pixelsVector, transformedPixels, _perpTrans);        
00216                 
00217              AvrgTime.first+=((double)cv::getTickCount()-tick)/cv::getTickFrequency(); AvrgTime.second++;      
00218             cout<<"Time cc detection="<<1000*AvrgTime.first/AvrgTime.second<<" milliseconds"<<endl;             
00219       
00220  
00221             
00222       cv::Rect cellRect(0,0,_mc,_nc);
00223       for(unsigned int i=0; i<transformedPixels.size(); i++) {
00224         //_canonicalPos.at<cv::Vec2b>(_pixelsVector[i].y, _pixelsVector[i].x) = cv::Vec2b(transformedPixels[i].x, transformedPixels[i].y);
00225         transformedPixels[i].x /= _cellSize;
00226         transformedPixels[i].y /= _cellSize;
00227         if( !transformedPixels[i].inside(cellRect) ) {
00228           _cellMap.at<uchar>( _pixelsVector[i].y, _pixelsVector[i].x ) = 0;
00229           _cellMap.at<uchar>( _pixelsVector[i].y+1, _pixelsVector[i].x ) = 0;
00230           _cellMap.at<uchar>( _pixelsVector[i].y, _pixelsVector[i].x+1 ) = 0;
00231           _cellMap.at<uchar>( _pixelsVector[i].y+1, _pixelsVector[i].x+1 ) = 0;
00232         }
00233         else {
00234       uchar cellNum = (unsigned int)transformedPixels[i].y*_nc + (unsigned int)transformedPixels[i].x;
00235           _cellMap.at<uchar>( _pixelsVector[i].y, _pixelsVector[i].x ) = 1+cellNum;
00236           _cellMap.at<uchar>( _pixelsVector[i].y+1, _pixelsVector[i].x ) = 1+cellNum;     
00237           _cellMap.at<uchar>( _pixelsVector[i].y, _pixelsVector[i].x+1 ) = 1+cellNum;
00238           _cellMap.at<uchar>( _pixelsVector[i].y+1, _pixelsVector[i].x+1 ) = 1+cellNum;   
00239         }
00240       }
00241           
00242 }
00243 
00244 
00245 
00248 void ChromaticMask::train(const cv::Mat& in, const aruco::Board &board)
00249 {
00250   calculateGridImage(board);
00251   
00252   for(unsigned int i=0; i<_classifiers.size(); i++) _classifiers[i].clearSamples();
00253   
00254   for(unsigned int i=0; i<in.rows; i++) {
00255     for(unsigned int j=0; j<in.cols; j++) {
00256       uchar idx = _cellMap.at<uchar>(i,j);
00257       if(idx!=0) _classifiers[idx-1].addSample( in.at<uchar>(i,j) );
00258     }
00259   }
00260   
00261   for(unsigned int i=0; i<_classifiers.size(); i++) _classifiers[i].train();
00262   
00263   
00264 //   for(uint i=0; i<_mc; i++) {
00265 //     for(uint j=0; j<_nc; j++) {
00266 //       vector<uint> neighbors;
00267 //       for(int k=-1; k<=1; k++) {
00268 //      if((i+k)<0 || (i+k)>=_mc) continue; 
00269 //      for(int l=-1; l<=1; l++) {
00270 //        if((j+l)<0 || (j+l)>=_nc) continue;
00271 //        neighbors.push_back((i+k)*_mc + (j+l));
00272 //      }
00273 //       }
00274 //       
00275 //       
00276 //       for(uint l=0; l<256; l++) {
00277 //      _classifiers[i*_mc+j].probConj[l] = 0.; 
00278 //      for(uint k=0; k<neighbors.size(); k++) {
00279 //        _classifiers[i*_mc+j].probConj[l] +=  _classifiers[k].getProb(l);
00280 //      }
00281 //      _classifiers[i*_mc+j].probConj[l] +=  _classifiers[i*_mc+j].getProb(l); // center prob x2
00282 //      _classifiers[i*_mc+j].probConj[l] / (float)(neighbors.size()+1);
00283 //       }
00284 //       
00285 //     }
00286 //   }
00287   
00288   _isValid = true;
00289   
00290 }
00291 
00292 
00295 void ChromaticMask::classify(const cv::Mat& in, const aruco::Board &board)
00296 {
00297   calculateGridImage(board);
00298   
00299   resetMask();
00300   
00301   for(unsigned int i=0; i<in.rows; i++) {
00302     const uchar* in_ptr = in.ptr<uchar>(i);
00303     const uchar* _cellMap_ptr = _cellMap.ptr<uchar>(i);
00304     for(unsigned int j=0; j<in.cols; j++) {
00305       uchar idx = _cellMap_ptr[j];
00306       if(idx!=0) {
00307         
00308         // considering neighbours
00309 //      float prob=0.0;
00310 //      float totalW = 0.0;
00311 //      cv::Point2d pix(i,j);
00312 //      for(uint k=0; k<_classifiers.size()-17; k++) {
00313 //        float w = 1./(1.+getDistance(pix,k));
00314 //        totalW += w;
00315 //        prob += w*_classifiers[k].getProb(in_ptr[j] );
00316 //      }
00317 //      prob /= totalW;
00318 //      if(prob > _threshProb) _mask.at<uchar>(i,j)=1;
00319         
00320         // not considering neighbours
00321         if(_classifiers[idx-1].classify( in.at<uchar>(i,j) )) _mask.at<uchar>(i,j)=1;
00322 //      if(_classifiers[idx-1].probConj[ in.at<uchar>(i,j)]>_threshProb ) _mask.at<uchar>(i,j)=1;
00323 
00324       }
00325     }
00326   } 
00327   
00328     // apply closing to mask
00329    cv::Mat maskClose;
00330   cv::Mat element = cv::getStructuringElement(cv::MORPH_RECT, cv::Size(3,3));
00331   cv::morphologyEx(_mask, maskClose, CV_MOP_CLOSE, element);  
00332   _mask = maskClose;
00333   
00334 }
00335 
00336 cv::Rect fitRectToSize(cv::Rect r,cv::Size size){
00337   
00338        r.x=max(r.x,0);
00339       r.y=max(r.y,0);
00340       int endx=r.x+r.width;
00341       int endy=r.y+r.height;
00342       endx=min(endx,size.width);
00343       endy=min(endy,size.height);
00344       r.width=endx-r.x;
00345       r.height=endy-r.y;
00346       return r;
00347  
00348 }
00349 
00352 void ChromaticMask::classify2(const cv::Mat& in, const aruco::Board &board)
00353 {
00354   
00355     _mask.create(_CP.CamSize.height, _CP.CamSize.width, CV_8UC1); 
00356     _maskAux.create(_CP.CamSize.height, _CP.CamSize.width, CV_8UC1); 
00357       _maskAux.setTo(cv::Scalar::all(0));
00358   
00359       cv::projectPoints(_objCornerPoints, board.Rvec, board.Tvec, _CP.CameraMatrix, _CP.Distorsion, _imgCornerPoints);    
00360       //obtain the perspective transform
00361       cv::Point2f  pointsRes[4],pointsIn[4];
00362       for ( int i=0;i<4;i++ ) pointsIn[i]=_imgCornerPoints[i];
00363 
00364       pointsRes[0]= ( cv::Point2f ( 0,0 ) );
00365       pointsRes[1]= cv::Point2f ( _mc-1, 0 );
00366       pointsRes[2]= cv::Point2f ( _mc-1, _nc-1 );
00367       pointsRes[3]= cv::Point2f ( 0, _nc-1 );
00368       _perpTrans=cv::getPerspectiveTransform ( pointsIn,pointsRes );
00369       
00370       cv::Mat pT_32;
00371       _perpTrans.convertTo(pT_32,CV_32F);//RMS: CAMBIA A FLOAT       
00372       
00373 //       cout << _perpTrans << endl;
00374       
00375       cv::Rect r = cv::boundingRect(_imgCornerPoints);
00376       r=fitRectToSize(r,in.size());//fit rectangle to image limits
00377       float *H=pT_32.ptr<float>(0);
00378 // #pragma omp parallel for
00379       int ny=0;
00380       for(unsigned int y=r.y; y<r.y+r.height; y+=2,ny++) {
00381         const uchar* in_ptr = in.ptr<uchar>(y);
00382         uchar *_mask_ptr=_maskAux.ptr<uchar>(y);
00383         int startx=r.x+ny%2;//alternate starting point
00384     for(unsigned int x=startx; x<r.x+r.width; x+=2) {
00385           cv::Point2f point;
00386           float _inv_pointz = 1./ (x*H[6] + y*H[7] + H[8]);
00387           point.x =_inv_pointz*( x*H[0] + y*H[1] + H[2]);
00388           point.y =_inv_pointz*( x*H[3] + y*H[4] + H[5]);
00389           cv::Point2i c;
00390           c.x = int(point.x+0.5);
00391           c.y = int(point.y+0.5);
00392           if(c.x<0 || c.x>_mc-1 || c.y<0 || c.y>_nc-1) continue;
00393           size_t cell_idx=c.y*_mc+ c.x;//SGJ: revisar si esto esta bien!!
00394           float prob=0.0,totalW=0.0,dist,w;
00395           
00396       for(unsigned int k=0; k<_cell_neighbours[cell_idx].size(); k++) {
00397             dist = fabs(point.x-_centers[k].x)+fabs( point.y-_centers[k].y);
00398             w= (2-dist);
00399             w*=w;
00400 //          w=1/(1- sqrt( (point.x-_centers[k].x)*(point.x-_centers[k].x) + (point.y-_centers[k].y)*(point.y-_centers[k].y));
00401             totalW += w;
00402             prob += w*_classifiers[ _cell_neighbours[cell_idx][k] ] .getProb(in_ptr[x] );
00403           }
00404           prob /= totalW;
00405 //    prob/=float(_classifiers.size()-17);
00406           if(prob > _threshProb) {
00407             _mask_ptr[x]=1;
00408           }
00409         }
00410       }
00411 //     // apply closing to mask
00412 // _mask=_maskAux;
00413    cv::Mat element = cv::getStructuringElement(cv::MORPH_RECT, cv::Size(3,3));
00414    cv::morphologyEx(_maskAux,_mask, CV_MOP_CLOSE, element);  
00415 }
00416 
00417 
00418 
00419 void ChromaticMask::update(const cv::Mat& in)
00420 {
00421   cv::Mat maskCells;
00422   maskCells = _cellMap.mul(_mask);
00423   
00424   for(unsigned int i=0; i<_classifiers.size(); i++) _classifiers[i].clearSamples();
00425   
00426   for(unsigned int i=0; i<maskCells.rows; i++) {
00427       uchar* maskCells_ptr = maskCells.ptr<uchar>(i);
00428       const uchar* in_ptr = in.ptr<uchar>(i);
00429        for(unsigned int j=0; j<maskCells.cols; j++) {
00430         if(maskCells_ptr[j]!=0) _classifiers[maskCells_ptr[j]-1].addSample( in_ptr[j] );
00431        }
00432   }
00433   
00434   for(unsigned int i=0; i<_classifiers.size(); i++)
00435     if(_classifiers[i].numsamples() > 50) {
00436       _classifiers[i].train();
00437     }
00438   
00439     
00440     
00441 }
00442 
00443 
00444 
00445 void ChromaticMask::resetMask()
00446 {
00447   
00448   _mask.create(_CP.CamSize.height, _CP.CamSize.width, CV_8UC1); 
00449   _mask.setTo(cv::Scalar::all(0));
00450 }
00451 
00452 
00453 
00454 
00455 
00456 
00457 
00458 
00459 
00460 
00461 
00462 
00463 
00464 
00465 
00466 
00467 
00468 
00469 
00470 
00471 


ar_sys
Author(s): Hamdi Sahloul , Rafael Muñoz Salinas , Bence Magyar
autogenerated on Sun Oct 5 2014 22:12:49