00001
00007 #ifndef P_KEYPOINT_DESCRIPTOR_HH
00008 #define P_KEYPOINT_DESCRIPTOR_HH
00009
00010 #include <iostream>
00011 #include <fstream>
00012 #include <float.h>
00013
00014
00015 #include <opencv2/opencv.hpp>
00016 #include <blort/Recognizer3D/PNamespace.hh>
00017 #include <blort/Recognizer3D/Keypoint.hh>
00018 #include <blort/Recognizer3D/Array.hh>
00019 #include <blort/Recognizer3D/Vector2.hh>
00020 #include <blort/Recognizer3D/SDraw.hh>
00021
00022
00023
00024
00025 #define PATCH_MASK_SIZE 16
00026
00027
00028 namespace P
00029 {
00030
00031
00032
00033
00034
00035 class KeypointDescriptor : public Keypoint
00036 {
00037 public:
00038 enum Type
00039 {
00040 DOG_SIFT,
00041 LOWE_DOG_SIFT,
00042 MSER_SIFT,
00043 UVMSER_SIFT,
00044 HESLAP_SIFT,
00045 MAX_TYPE,
00046 UNDEF = MAX_TYPE
00047 };
00048
00049 static float DOG_SIFT_THR;
00050 static float LOWE_DOG_SIFT_THR;
00051 static float MSER_SIFT_THR;
00052 static float COL_THR;
00053 static float HESLAP_SIFT_THR;
00054
00055
00056 public:
00057 static const unsigned KEDE_DELETE;
00058 static const unsigned KEDE_INSERT;
00059 static const unsigned KEDE_DETECTED;
00060
00061 unsigned flags;
00062
00063 Type type;
00064 float *vec;
00065 unsigned size;
00066
00067 CvMat *pos;
00068 unsigned cnt_pos;
00069 P::Vector2 p_rect;
00070
00071 IplImage *mask;
00072 IplImage *bgmask;
00073 IplImage *patch;
00074
00075
00076 float reliability;
00077 unsigned cnt_rel;
00078 float mean_error;
00079 float var_error;
00080 unsigned cnt_err;
00081
00082 unsigned occNum;
00083
00084 unsigned chainId;
00085 unsigned chainCnt;
00086
00087 KeypointDescriptor();
00088 KeypointDescriptor(Type t);
00089 KeypointDescriptor(KeypointDescriptor *k);
00090 KeypointDescriptor(Type t, double x, double y, float s, float a);
00091 KeypointDescriptor(Type t, double x,double y,float s,float a,
00092 float _m11,float _m12,float _m21,float _m22);
00093 ~KeypointDescriptor();
00094
00095 static const char* TypeName(Type t);
00096 static Type EnumType(const char *type_name);
00097 bool GetVoteCenter(KeypointDescriptor *current, Vector2 &v,
00098 double &delta_angle, double &delta_scale);
00099 void SaveMask(IplImage *img);
00100 void ProjectMask(KeypointDescriptor *model,IplImage *img, float w);
00101 void ProjectBgMask(KeypointDescriptor *model,IplImage *img, float w);
00102 void ProjectOccl(P::Vector2 ¢er, float scale, float angle,
00103 IplImage *img, float w);
00104 void SavePatch(IplImage *img);
00105 void ProjectPatch(KeypointDescriptor *model,IplImage *img);
00106 float Match(KeypointDescriptor *k);
00107 void ReadKeypoint( ifstream &in );
00108 void ReadDescriptor( ifstream &in, unsigned size_in);
00109 void WriteKeypoint( ofstream &out );
00110 void WriteDescriptor(ofstream &out );
00111
00112 void SetReliability(float f);
00113 void SetError(float e);
00114 void Copy(KeypointDescriptor *k);
00115 void AllocVec(unsigned s);
00116 void CopyVec(KeypointDescriptor *k);
00117 inline float* GetVec(){return vec;}
00118 inline float GetVec(unsigned i){return (i<size?vec[i]:0);}
00119 inline Type GetType(){return type;}
00120 inline unsigned GetSize(){return size;}
00121 inline bool Have3D(){return (pos!=0?true:false);}
00122 float DistSqr(float *desc1, float *desc2, unsigned cnt);
00123 void Add(float *desc);
00124 void Div(float num);
00125 void Mul(float num);
00126 void SetZero();
00127 void MatchSift(KeypointDescriptor *k, float thr, float &dist);
00128 inline float GetThr();
00129 inline void SetPos(float x, float y, float z);
00130
00131 static void Draw(IplImage *img, KeypointDescriptor &k, CvScalar col);
00132 static void LoadAll(ifstream &is, KeypointDescriptor &k);
00133 static void SaveAll(ofstream &os, const KeypointDescriptor &k);
00134 };
00135
00136
00137 void CopyVec(float *svec, float *dvec, unsigned size);
00138 void WriteKeypoints(P::Array<KeypointDescriptor*> &ks, const char* file, int format=0);
00139 void LoadKeypoints(const char* file, P::Array<KeypointDescriptor*> &ks, int format=0);
00140 void LoadLoweKeypoints(const char* file, P::Array<Keypoint*> &ks, int format=0);
00141 void LoadLoweKeypoints(const char* file, P::Array<KeypointDescriptor*> &ks, int format=0);
00142
00143
00144
00145
00146
00147
00148
00149 inline void KeypointDescriptor::AllocVec(unsigned s)
00150 {
00151 if (vec!=0) delete[] vec;
00152 size=s;
00153 vec = new float[size];
00154 }
00155
00156 inline void KeypointDescriptor::CopyVec(KeypointDescriptor *k)
00157 {
00158 if (k->vec!=0){
00159 AllocVec(k->size);
00160 for (unsigned i=0; i<size; i++){
00161 vec[i]=k->vec[i];
00162 }
00163 }
00164 }
00165
00166 inline void KeypointDescriptor::Copy(KeypointDescriptor *k)
00167 {
00168 p=k->p;
00169 if (k->Have3D())
00170 {
00171 if (!Have3D()) pos = cvCreateMat(3,1, CV_32F);
00172 cvCopy(k->pos,pos);
00173 }
00174 p_rect=k->p_rect;
00175 cnt_pos=k->cnt_pos;
00176 scale=k->scale;
00177 angle=k->angle;
00178
00179 mi11=k->mi11;
00180 mi12=k->mi12;
00181 mi21=k->mi21;
00182 mi22=k->mi22;
00183
00184 chainId=k->chainId;
00185 chainCnt=k->chainCnt;
00186
00187 type=k->type;
00188 CopyVec(k);
00189
00190 if (k->mask!=0){
00191 if (mask==0) mask=cvCreateImage( cvSize(PATCH_MASK_SIZE,PATCH_MASK_SIZE), IPL_DEPTH_32F, 1 );
00192 cvCopy(k->mask, mask);
00193 }
00194 if (k->bgmask!=0){
00195 if (bgmask==0) bgmask=cvCreateImage( cvSize(PATCH_MASK_SIZE,PATCH_MASK_SIZE), IPL_DEPTH_32F, 1 );
00196 cvCopy(k->bgmask, bgmask);
00197 }
00198
00199 if (k->patch!=0){
00200 if (patch==0) patch=cvCreateImage( cvSize(PATCH_MASK_SIZE,PATCH_MASK_SIZE), IPL_DEPTH_8U, 1 );
00201 cvCopy(k->patch, patch);
00202 }
00203 cnt_err=k->cnt_err;
00204 flags=k->flags;
00205 cnt_rel=k->cnt_rel;
00206 reliability=k->reliability;
00207 mean_error=k->mean_error;
00208 var_error=k->var_error;
00209 occNum=k->occNum;
00210 }
00211
00215 inline float KeypointDescriptor::DistSqr(float *desc1, float *desc2, unsigned cnt)
00216 {
00217 register unsigned i;
00218 float dif;
00219 float distsq = 0;
00220
00221 for (i = 0; i < cnt; i++) {
00222 dif = *desc1++ - *desc2++;
00223 distsq += dif * dif;
00224 }
00225 return distsq;
00226 }
00227
00231 inline void KeypointDescriptor::Add(float *desc)
00232 {
00233 register unsigned i;
00234 register float *d = GetVec();
00235
00236 for (i = 0; i < GetSize(); i++) {
00237 *d++ += *desc++;
00238 }
00239 }
00240
00244 inline void KeypointDescriptor::SetZero()
00245 {
00246 register float *d=GetVec();
00247 register unsigned z;
00248
00249 for (z=0; z< GetSize(); z++){
00250 *d++ = 0.;
00251 }
00252 }
00253
00257 inline void KeypointDescriptor::Div(float num)
00258 {
00259 register float *d=GetVec();
00260 register unsigned z;
00261
00262 for (z=0; z< GetSize(); z++){
00263 *d++ /= num;
00264 }
00265 }
00266
00270 inline void KeypointDescriptor::Mul(float num)
00271 {
00272 register float *d=GetVec();
00273 register unsigned z;
00274
00275 for (z=0; z< GetSize(); z++){
00276 *d++ *= num;
00277 }
00278 }
00279
00280
00285 inline void KeypointDescriptor::MatchSift(KeypointDescriptor *k, float thr, float &dist)
00286 {
00287 dist = DistSqr(vec, k->vec, GetSize());
00288 if (dist>=thr) dist=FLT_MAX;
00289 }
00290
00295 inline void KeypointDescriptor::SetReliability(float f)
00296 {
00297 reliability = ((float)(cnt_rel*reliability + f))/((float)(cnt_rel+1));
00298 cnt_rel++;
00299 }
00300
00304 inline void KeypointDescriptor::SetError(float e)
00305 {
00306 mean_error = ((float)cnt_err*mean_error + e)/((float)cnt_err+1.);
00307
00308 var_error = 1./((float)cnt_err+1.) * ((float)cnt_err*var_error +
00309 (float)cnt_err/((float)cnt_err+1.)*Sqr(mean_error - e));
00310 cnt_err++;
00311 }
00312
00313 inline float KeypointDescriptor::GetThr()
00314 {
00315 switch(GetType())
00316 {
00317 case DOG_SIFT:
00318 return DOG_SIFT_THR;
00319 break;
00320 case LOWE_DOG_SIFT:
00321 return LOWE_DOG_SIFT_THR;
00322 break;
00323 case MSER_SIFT:
00324 return MSER_SIFT_THR;
00325 break;
00326 case UVMSER_SIFT:
00327 return MSER_SIFT_THR;
00328 break;
00329 case HESLAP_SIFT:
00330 return HESLAP_SIFT_THR;
00331 break;
00332 default:
00333 return FLT_MAX;
00334 break;
00335 }
00336 return FLT_MAX;
00337 }
00338
00339 inline void KeypointDescriptor::SetPos(float x, float y, float z)
00340 {
00341 if (!Have3D()) pos = cvCreateMat(3,1, CV_32F);
00342
00343 pos->data.fl[0] = x;
00344 pos->data.fl[1] = y;
00345 pos->data.fl[2] = z;
00346 }
00347
00351 inline void CopyVec(float *svec, float *dvec, unsigned size)
00352 {
00353 for (register unsigned i=0; i<size; i++){
00354 *dvec++=*svec++;
00355 }
00356 }
00357
00358 inline float MatchKeypoint(KeypointDescriptor *k1, KeypointDescriptor *k2)
00359 {
00360 if (k1->GetType() != k2->GetType())
00361 return FLT_MAX;
00362 return k1->DistSqr(k1->vec, k2->vec, k1->GetSize());
00363 }
00364
00365
00366 }
00367
00368 #endif
00369