KeypointDescriptor.cc
Go to the documentation of this file.
00001 
00008 #include <blort/Recognizer3D/KeypointDescriptor.hh>
00009 
00010 
00011 namespace P 
00012 {
00013 
00014 float KeypointDescriptor::DOG_SIFT_THR = .2;
00015 float KeypointDescriptor::LOWE_DOG_SIFT_THR = 50000;
00016 float KeypointDescriptor::MSER_SIFT_THR = 40000;
00017 float KeypointDescriptor::HESLAP_SIFT_THR = 30000;
00018 
00019 const unsigned KeypointDescriptor::KEDE_DELETE = 0x01;      //delete occurrence
00020 const unsigned KeypointDescriptor::KEDE_INSERT = 0x02;      //insert to codebook
00021 const unsigned KeypointDescriptor::KEDE_DETECTED = 0x04;    //keypoint is detected
00022 
00023 
00024 static const int NAME_LENGTH = 40;
00025 static const char keyd_type_names[][NAME_LENGTH] = {
00026   "DOG_SIFT",
00027   "LOWE_DOG_SIFT",
00028   "MSER_SIFT",
00029   "UVMSER_SIFT",
00030   "HESLAP_SIFT",
00031   "UNDEF"
00032   };
00033 
00037 const char* KeypointDescriptor::TypeName(Type t)
00038 {
00039   return keyd_type_names[t];
00040 }
00041 
00045 KeypointDescriptor::Type KeypointDescriptor::EnumType(const char *type_name)
00046 {
00047   for(int i = 0; i < MAX_TYPE; i++)
00048     if(strncmp(type_name, keyd_type_names[i], NAME_LENGTH) == 0)
00049       return (Type)i;
00050   return UNDEF;
00051 }
00052 
00053 
00054 
00058 KeypointDescriptor::KeypointDescriptor()
00059  : flags(0),
00060    type(UNDEF),
00061    vec(0),
00062    size(0),
00063    pos(0),
00064    cnt_pos(0),
00065    mask(0),
00066    bgmask(0),
00067    patch(0),
00068    reliability(1),
00069    cnt_rel(1),
00070    mean_error(0),
00071    var_error(0),
00072    cnt_err(0),
00073    //objectModel(0),
00074    occNum(1),
00075    chainId(UINT_MAX),
00076    chainCnt(0)
00077 {
00078   p_rect = P::Vector2(0.,0.);
00079 }
00080 
00081 KeypointDescriptor::KeypointDescriptor(Type t)
00082  : flags(0),
00083    type(t),
00084    vec(0),
00085    size(0),
00086    pos(0),
00087    cnt_pos(0),
00088    mask(0),
00089    bgmask(0),
00090    patch(0),
00091    reliability(1),
00092    cnt_rel(1),
00093    mean_error(0),
00094    var_error(0),
00095    cnt_err(0),
00096    //objectModel(0),
00097    occNum(1),
00098    chainId(UINT_MAX),
00099    chainCnt(0)
00100 {
00101   p_rect = P::Vector2(0.,0.);
00102 
00103 }
00104 
00105 
00106 KeypointDescriptor::~KeypointDescriptor()
00107 {
00108   if (vec!=0) delete[] vec;
00109   if (mask!=0) cvReleaseImage(&mask);
00110   if (bgmask!=0) cvReleaseImage(&bgmask);
00111   if (patch!=0) cvReleaseImage(&patch);
00112   if (pos!=0) cvReleaseMat(&pos);
00113 }
00114 
00115 KeypointDescriptor::KeypointDescriptor(KeypointDescriptor *k)
00116  : Keypoint(*k),
00117    flags(0),
00118    vec(0), 
00119    size(0),
00120    pos(0),
00121    cnt_pos(0),
00122    mask(0),
00123    bgmask(0),
00124    patch(0)
00125 {
00126   Copy(k);
00127 }
00128 
00129 KeypointDescriptor::KeypointDescriptor(Type t, double x, double y, float s, float a)
00130  : Keypoint(x, y, s, a),
00131    flags(0),
00132    type(t),
00133    vec(0),
00134    size(0),
00135    pos(0),
00136    cnt_pos(0),
00137    mask(0),
00138    bgmask(0),
00139    patch(0),
00140    reliability(1),
00141    cnt_rel(1),
00142    mean_error(0),
00143    var_error(0),
00144    cnt_err(0),
00145    //objectModel(0),
00146    occNum(1),
00147    chainId(UINT_MAX),
00148    chainCnt(0)
00149 {
00150   p_rect = P::Vector2(0.,0.);
00151 }
00152 
00153 KeypointDescriptor::KeypointDescriptor(Type t, double x, double y, float s, float a,
00154                                        float _m11,float _m12,float _m21,float _m22)
00155  : Keypoint(x, y, s, a, _m11, _m12, _m21, _m22),
00156    flags(0),
00157    type(t),
00158    vec(0),
00159    size(0),
00160    pos(0),
00161    cnt_pos(0),
00162    mask(0),
00163    bgmask(0),
00164    patch(0),
00165    reliability(1),
00166    cnt_rel(1),
00167    mean_error(0),
00168    var_error(0),
00169    cnt_err(0),
00170    //objectModel(0),
00171    occNum(1),
00172    chainId(UINT_MAX),
00173    chainCnt(0)
00174 {
00175   p_rect = P::Vector2(0.,0.);
00176 }
00177 
00181 bool KeypointDescriptor::GetVoteCenter(KeypointDescriptor *current,Vector2 &v,
00182                                        double &delta_angle, double &delta_scale)
00183 {
00184   if (type==current->type){
00185     delta_scale = current->Scale()/Scale();
00186     delta_angle = ScaleAngle_mpi_pi(-Angle() + current->Angle());
00187     v = current->p + Rotate(-p,delta_angle)*delta_scale;
00188     return true;
00189   }
00190   return false;
00191 }
00192 
00196 void KeypointDescriptor::SaveMask(IplImage *img)
00197 {
00198   if (!mask) mask = cvCreateImage( cvSize(PATCH_MASK_SIZE,PATCH_MASK_SIZE), IPL_DEPTH_32F, 1 );
00199   if (!bgmask) bgmask = cvCreateImage( cvSize(PATCH_MASK_SIZE,PATCH_MASK_SIZE), IPL_DEPTH_32F, 1 );
00200 
00201   float sxy=(Scale()*6/PATCH_MASK_SIZE);     //6 is a stubide constant????
00202   CvMat* G = cvCreateMat(2,3,CV_32FC1);
00203 
00204   float coso = cos(-Angle())*sxy;
00205   float sino = sin(-Angle())*sxy;
00206 
00207   G->data.fl[0] = mi11*coso-mi12*sino;
00208   G->data.fl[1] = mi11*sino+mi12*coso;
00209   G->data.fl[2] = X();
00210   G->data.fl[3] = mi21*coso-mi22*sino;
00211   G->data.fl[4] = mi21*sino+mi22*coso;
00212   G->data.fl[5] = Y();
00213 
00214   cvGetQuadrangleSubPix( img, mask, G );
00215   cvConvertScale(mask, bgmask, -1, 1 );
00216  
00217   cvReleaseMat(&G);
00218 }
00219 
00223 void KeypointDescriptor::ProjectMask(KeypointDescriptor *model,IplImage *img, float w)
00224 {
00225   float delta=-PATCH_MASK_SIZE/2.;
00226   float sxy=(Scale()*6/PATCH_MASK_SIZE);      //6 is a stubide constant?!?
00227   float coso = cos(-Angle())*sxy;
00228   float sino = sin(-Angle())*sxy;
00229 
00230   CvMat* G= cvCreateMat(2,3,CV_32FC1);
00231   IplImage *wmask = cvCreateImage( cvSize(PATCH_MASK_SIZE,PATCH_MASK_SIZE), IPL_DEPTH_32F, 1 );
00232   cvConvertScale(model->mask,wmask,w,0);
00233 
00234   G->data.fl[0] = mi11*coso-mi12*sino;
00235   G->data.fl[1] = mi11*sino+mi12*coso;
00236   G->data.fl[2] = delta*G->data.fl[0] + delta*G->data.fl[1] + p.x;
00237   G->data.fl[3] = mi21*coso-mi22*sino;
00238   G->data.fl[4] = mi21*sino+mi22*coso;
00239   G->data.fl[5] = delta*G->data.fl[4] + delta*G->data.fl[3] + p.y;
00240 
00241   //WarpProbMap( wmask, img, G);
00242 
00243   cvReleaseMat(&G);
00244   cvReleaseImage(&wmask);
00245 }
00246 
00250 void KeypointDescriptor::ProjectBgMask(KeypointDescriptor *model,IplImage *img, float w)
00251 {
00252   float delta=-PATCH_MASK_SIZE/2.;
00253   float sxy=(Scale()*6/PATCH_MASK_SIZE);      //6 is a stubide constant?!?
00254   float coso = cos(-Angle())*sxy;
00255   float sino = sin(-Angle())*sxy;
00256 
00257   CvMat* G= cvCreateMat(2,3,CV_32FC1);
00258   IplImage *wmask = cvCreateImage( cvSize(PATCH_MASK_SIZE,PATCH_MASK_SIZE), IPL_DEPTH_32F, 1 );
00259   cvConvertScale(model->bgmask,wmask,w,0);
00260 
00261   G->data.fl[0] = mi11*coso-mi12*sino;
00262   G->data.fl[1] = mi11*sino+mi12*coso;
00263   G->data.fl[2] = delta*G->data.fl[0] + delta*G->data.fl[1] + p.x;
00264   G->data.fl[3] = mi21*coso-mi22*sino;
00265   G->data.fl[4] = mi21*sino+mi22*coso;
00266   G->data.fl[5] = delta*G->data.fl[4] + delta*G->data.fl[3] + p.y;
00267 
00268   //WarpProbMap( wmask, img, G);
00269 
00270   cvReleaseMat(&G);
00271   cvReleaseImage(&wmask);
00272 }
00273 
00278 void KeypointDescriptor::ProjectOccl(P::Vector2 &center, float scale, float angle, 
00279                                      IplImage *img, float w)
00280 {
00281   float delta=-PATCH_MASK_SIZE/2.;
00282   float sxy=(Scale()*6/PATCH_MASK_SIZE);      //6 is a stubide constant?!?
00283   float coso = cos(ScaleAngle_mpi_pi(-Angle()))*sxy*scale;   //+angle!!!
00284   float sino = sin(ScaleAngle_mpi_pi(-Angle()))*sxy*scale;   //+angle!!!!
00285   P::Vector2 v = center + Rotate(p,angle)*scale;
00286 
00287   CvMat* G= cvCreateMat(2,3,CV_32FC1);
00288   IplImage *wmask = cvCreateImage( cvSize(PATCH_MASK_SIZE,PATCH_MASK_SIZE), IPL_DEPTH_32F, 1 );
00289   cvConvertScale(mask,wmask,w,0);
00290 
00291   G->data.fl[0] = mi11*coso-mi12*sino;
00292   G->data.fl[1] = mi11*sino+mi12*coso;
00293   G->data.fl[2] = delta*G->data.fl[0] + delta*G->data.fl[1] + v.x;//p.x+center.x;
00294   G->data.fl[3] = mi21*coso-mi22*sino;
00295   G->data.fl[4] = mi21*sino+mi22*coso;
00296   G->data.fl[5] = delta*G->data.fl[3] + delta*G->data.fl[4] + v.y;// p.y+center.y;
00297 
00298   //WarpProbMap( wmask, img, G);
00299 
00300   cvReleaseMat(&G);
00301   cvReleaseImage(&wmask);
00302 }
00303 
00307 #ifdef SAVE_PATCHES
00308 static unsigned mcnt=0;
00309 char fname[1024];
00310 #endif
00311 void KeypointDescriptor::SavePatch(IplImage *img)
00312 {
00313   if (!patch) patch = cvCreateImage( cvSize(PATCH_MASK_SIZE,PATCH_MASK_SIZE), IPL_DEPTH_8U, 1 );
00314 
00315   float sxy=(Scale()*6/PATCH_MASK_SIZE);     //6 is a stubide constant????
00316   CvMat* G = cvCreateMat(2,3,CV_32FC1);
00317 
00318   float coso = cos(-Angle())*sxy;
00319   float sino = sin(-Angle())*sxy;
00320 
00321   G->data.fl[0] = mi11*coso-mi12*sino;
00322   G->data.fl[1] = mi11*sino+mi12*coso;
00323   G->data.fl[2] = X();
00324   G->data.fl[3] = mi21*coso-mi22*sino;
00325   G->data.fl[4] = mi21*sino+mi22*coso;
00326   G->data.fl[5] = Y();
00327 
00328   cvGetQuadrangleSubPix( img, patch, G );
00329  
00330   cvReleaseMat(&G);
00331   #ifdef SAVE_PATCHES
00332   snprintf(fname,1024,"log/patch_%04u.png",mcnt);
00333   cvSaveImage(fname,patch);
00334   mcnt++;
00335   #endif
00336 }
00337 
00341 void KeypointDescriptor::ProjectPatch(KeypointDescriptor *model,IplImage *img)
00342 {
00343   if (model->patch != 0){
00344     float delta=-PATCH_MASK_SIZE/2.;
00345     float sxy=(Scale()*6/PATCH_MASK_SIZE);      //6 is a stubide constant?!?
00346     float coso = cos(-Angle())*sxy;
00347     float sino = sin(-Angle())*sxy;
00348 
00349     CvMat* G= cvCreateMat(2,3,CV_32FC1);
00350 
00351     G->data.fl[0] = mi11*coso-mi12*sino;
00352     G->data.fl[1] = mi11*sino+mi12*coso;
00353     G->data.fl[2] = delta*G->data.fl[0] + delta*G->data.fl[1] + p.x;
00354     G->data.fl[3] = mi21*coso-mi22*sino;
00355     G->data.fl[4] = mi21*sino+mi22*coso;
00356     G->data.fl[5] = delta*G->data.fl[3] + delta*G->data.fl[4] + p.y;
00357 
00358     cvWarpAffine( model->patch, img, G,CV_INTER_LINEAR);
00359 
00360     cvReleaseMat(&G);
00361   }
00362 }
00363 
00367 float KeypointDescriptor::Match(KeypointDescriptor *k)
00368 {
00369   if (GetType() == k->GetType()){
00370 
00371     float dist;
00372     
00373     //set type dependant constants
00374     switch(GetType())
00375     {
00376       case DOG_SIFT:
00377         MatchSift(k, DOG_SIFT_THR, dist);
00378         break;
00379       case LOWE_DOG_SIFT:
00380         MatchSift(k, LOWE_DOG_SIFT_THR, dist);
00381         break;
00382       case MSER_SIFT:
00383         MatchSift(k, MSER_SIFT_THR, dist);
00384         break;
00385       case UVMSER_SIFT:
00386         MatchSift(k, MSER_SIFT_THR, dist);
00387         break;
00388       case HESLAP_SIFT:
00389         MatchSift(k, HESLAP_SIFT_THR, dist);
00390         break;
00391       default:
00392         dist = FLT_MAX;
00393         break;        
00394     }
00395     return dist;
00396   }
00397   return FLT_MAX;
00398 }
00399 
00403 void KeypointDescriptor::Draw(IplImage *img, KeypointDescriptor &k, CvScalar col)
00404 {
00405   double scale = k.Scale()*3;
00406   double U[4], D[4], Vi[4], V[4];
00407   CvMat matU = cvMat(2,2, CV_64F, U);
00408   CvMat matD = cvMat(2,2, CV_64F, D);
00409   CvMat matVi = cvMat(2,2, CV_64F, Vi);
00410   CvMat matV = cvMat(2,2, CV_64F, V);
00411 
00412   U[0] = k.Mi11(), U[1] = k.Mi12();
00413   U[2] = k.Mi21(), U[3] = k.Mi22();
00414 
00415   cvSVD(&matU, &matD, &matVi, &matV);
00416 
00417   D[0] *= scale;
00418   D[1] *= scale;
00419   D[2] *= scale;
00420   D[3] *= scale;
00421 
00422   double angle = ScaleAngle_0_2pi(acos(V[0]))*180./M_PI;
00423   SDraw::DrawEllipse(img, k.X(), k.Y(), D[0], D[3],angle, col);
00424   Vector2 v = Vector2(D[3],0);
00425   v = Rotate(v,-k.Angle());
00426   v = Vector2(v.x+k.X(), v.y+k.Y());
00427   SDraw::DrawLine(img, k.X(), k.Y(), v.x, v.y, col);
00428 
00429 }
00430 
00434 void KeypointDescriptor::ReadKeypoint( ifstream &in )
00435 {
00436   float tmp;
00437 
00438   in >> p.x;
00439   in >> p.y;
00440   in >> tmp;
00441   in >> scale;
00442   in >> angle;
00443   in >> tmp;
00444   in >> tmp;
00445   in >> tmp;
00446   in >> tmp;
00447   in >> mi11;
00448   in >> mi12;
00449   in >> mi21;
00450   in >> mi22;
00451 }
00452 
00453 void KeypointDescriptor::ReadDescriptor( ifstream &in, unsigned size_in)
00454 {
00455   if(size_in>0){
00456     AllocVec(size_in);
00457     for(unsigned j=0; j<size;j++){
00458       in >> vec[j];
00459     }
00460   }
00461 }
00462 
00463 void KeypointDescriptor::WriteKeypoint( ofstream &out )
00464 {
00465   out << p.x << " " << p.y << " " << 0 << " " << scale << " ";
00466   out << angle<< " "<< type << " " << 0 << " " <<0<< " " <<0;
00467   out << " " << mi11 << " " << mi12 << " " << mi21 << " " << mi22<< " ";
00468 }
00469 
00470 void KeypointDescriptor::WriteDescriptor(ofstream &out )
00471 {
00472   if(size>0){
00473     for(unsigned j=0; j<size;j++){
00474       out << vec[j] << " ";
00475      }
00476   }
00477 }
00478 
00479 void WriteKeypoints(P::Array<KeypointDescriptor*> &ks, const char* file, int format)
00480 {
00481   if (ks.Size()==0){
00482       cout << " Keypoints Nb: " << ks.Size() << endl;
00483       return;
00484   }
00485     ofstream output(file);
00486     if(!output)cout << "Error opening " << file<< endl;
00487     output << ks[0]->GetSize()<< endl;
00488     output << ks.Size()<< endl;
00489     for(unsigned i=0; i<ks.Size();i++){
00490       if(format==0){
00491         ks[i]->WriteKeypoint(output);
00492         ks[i]->WriteDescriptor(output);
00493         output<<endl;
00494       }
00495     }
00496     output.close();
00497 }
00498 
00499 void LoadKeypoints(const char* file, P::Array<KeypointDescriptor*> &ks, int format)
00500 {
00501   KeypointDescriptor* k;
00502   ifstream input1(file);
00503 
00504   if(!input1)return;
00505 
00506   unsigned numip, size;
00507   float tmp;
00508   input1 >> tmp;
00509   input1 >> numip;
00510   if(tmp<=1.0) size=0;
00511   else size=(int)tmp;
00512   for (unsigned i=0; i<numip; i++){
00513     if(format==0){
00514       k = new KeypointDescriptor();
00515       k->ReadKeypoint(input1);
00516       k->ReadDescriptor(input1, size);
00517       ks.PushBack(k);
00518     }
00519   }
00520 
00521   input1.close();
00522 }
00523 
00524 void LoadLoweKeypoints(const char* file, P::Array<Keypoint*> &ks, int format)
00525 {
00526   KeypointDescriptor* k;
00527   ifstream input1(file);
00528 
00529   if(!input1)return;
00530 
00531   unsigned numip, size;
00532   float tmp;
00533   input1 >> numip;
00534   input1 >> tmp;
00535   if(tmp<=1.0) size=0;
00536   else size=(int)tmp;
00537   for (unsigned i=0; i<numip; i++){
00538     if(format==0){
00539       k = new KeypointDescriptor();
00540       input1 >> k->p.y;
00541       input1 >> k->p.x;
00542       input1 >> k->scale;
00543       input1 >> k->angle;
00544       k->angle = -k->angle;
00545       k->ReadDescriptor(input1, size);
00546       ks.PushBack(k);
00547     }
00548   }
00549 
00550   input1.close();
00551 }
00552 
00553 void LoadLoweKeypoints(const char* file, P::Array<KeypointDescriptor*> &ks, int format)
00554 {
00555   KeypointDescriptor* k;
00556   ifstream input1(file);
00557 
00558   if(!input1)return;
00559 
00560   unsigned numip, size;
00561   float tmp;
00562   input1 >> numip;
00563   input1 >> tmp;
00564   if(tmp<=1.0) size=0;
00565   else size=(int)tmp;
00566   for (unsigned i=0; i<numip; i++){
00567     if(format==0){
00568       k = new KeypointDescriptor();
00569       input1 >> k->p.y;
00570       input1 >> k->p.x;
00571       input1 >> k->scale;
00572       input1 >> k->angle;
00573       k->angle = -k->angle;
00574       k->ReadDescriptor(input1, size);
00575       ks.PushBack(k);
00576     }
00577   }
00578 
00579   input1.close();
00580 }
00581 
00582 
00583 void KeypointDescriptor::SaveAll(ofstream &os, const KeypointDescriptor &k)
00584 {
00585   os << k.p.x<<' '<<k.p.y<<' '<<k.scale<<' '<<k.angle;
00586   os <<' '<<k.mi11<<' '<<k.mi12<<' '<<k.mi21<<' '<<k.mi22;
00587   os <<' '<<k.flags<<' '<<k.type<<' '<<k.size;
00588   for (unsigned i=0; i<k.size; i++)
00589     os <<' '<<k.vec[i];
00590   if (k.pos!=0)
00591     os <<' '<<'1'<<' '<<(float)k.pos->data.fl[0]<<' '<<(float)k.pos->data.fl[1]<<' '<<(float)k.pos->data.fl[2];
00592   else
00593     os <<' '<<'0';
00594   os<<' '<<k.cnt_pos<<' '<<k.p_rect.x<<' '<<k.p_rect.y<<' ';
00595   os<<' '<<k.reliability<<' '<<k.cnt_rel;
00596   os<<' '<<k.mean_error<<' '<<k.var_error<<' '<<k.var_error<<' '<<k.cnt_err;
00597   os<<'\n';
00598 }
00599 
00600 void KeypointDescriptor::LoadAll(ifstream &is, KeypointDescriptor &k)
00601 {
00602   is >> k.p.x>>k.p.y>>k.scale>>k.angle>>k.mi11>>k.mi12>>k.mi21>>k.mi22;
00603   unsigned type;
00604   is >>k.flags>>type>>k.size;
00605   k.type = (KeypointDescriptor::Type)type;
00606   k.AllocVec(k.size);
00607   for (unsigned i=0; i<k.size; i++)
00608     is >>k.vec[i];
00609   unsigned c;
00610   is>>c;
00611   if (c==1)
00612   {
00613     if (k.pos==0) k.pos = cvCreateMat(3,1, CV_32F);
00614     is>>k.pos->data.fl[0]>>k.pos->data.fl[1]>>k.pos->data.fl[2];
00615   }
00616   is>>k.cnt_pos>>k.p_rect.x>>k.p_rect.y;
00617   is>>k.reliability>>k.cnt_rel;
00618   is>>k.mean_error>>k.var_error>>k.var_error>>k.cnt_err;
00619 }
00620 
00621 
00622 
00623 
00624 
00625 
00626 }
00627 
00628 
00629 
00630 
00631 
00632 
00633 
00634 
00635 
00636 
00637 
00638 


blort
Author(s): Michael Zillich, Thomas Mörwald, Johann Prankl, Andreas Richtsfeld, Bence Magyar (ROS version)
autogenerated on Thu Jan 2 2014 11:38:25