00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024 #include <iostream>
00025 #include <vector>
00026
00027 #include <classifiers/surf.h>
00028 #include <math.h>
00029
00030 #include <utils/time/clock.h>
00031 #include <utils/time/tracker.h>
00032
00033 #include <fstream>
00034
00035 #include <string>
00036
00037 #include <surf/surflib.h>
00038
00039
00040
00041
00042 #include <core/exception.h>
00043 #include <core/exceptions/software.h>
00044 #include <fvutils/color/colorspaces.h>
00045 #include <fvutils/color/conversions.h>
00046 #include <fvutils/readers/png.h>
00047 #include <utils/system/console_colors.h>
00048 #include <dirent.h>
00049 #include <utils/logging/liblogger.h>
00050
00051 #define BVERBOSE true
00052
00053
00054
00055
00056 namespace firevision {
00057 #if 0
00058 }
00059 #endif
00060
00061
00062
00063
00064
00065
00066
00067
00068
00069
00070
00071
00072
00073
00074
00075
00076
00077
00078
00079
00080
00081
00082
00083 void saveIpoints(std::string sFileName, const std::vector< surf::Ipoint >& ipts, bool bVerbose, bool bLaplacian, int VLength)
00084 {
00085 std::cout<<"Attempting to save interest points" << std::endl;
00086
00087 std::ofstream ipfile(sFileName.c_str());
00088 if( !ipfile ) {
00089 std::cerr << "ERROR in loadIpoints(): "
00090 << "Couldn't open file '" << sFileName.c_str() << "'!" << std::endl;
00091 return;
00092 }
00093
00094 double sc;
00095 unsigned count = ipts.size();
00096
00097
00098 if (bLaplacian)
00099 ipfile << VLength + 1 << std::endl << count << std::endl;
00100 else
00101 ipfile << VLength << std::endl << count << std::endl;
00102
00103
00104
00105
00106
00107
00108
00109
00110
00111
00112 for (unsigned n=0; n<ipts.size(); n++){
00113
00114 sc = 2.5 * ipts[n].scale; sc*=sc;
00115 ipfile << ipts[n].x
00116 << " " << ipts[n].y
00117 << " " << 1.0/sc
00118 << " " << 0.0
00119 << " " << 1.0/sc;
00120
00121 if (bLaplacian)
00122 ipfile << " " << ipts[n].laplace;
00123
00124
00125 for (int i = 0; i < VLength; i++) {
00126 ipfile << " " << ipts[n].ivec[i];
00127 }
00128 ipfile << std::endl;
00129 }
00130
00131
00132 if( bVerbose )
00133 std::cout << count << " interest points found" << std::endl;
00134 }
00135
00136
00137
00138
00139
00140
00141
00142 void loadIpoints( std::string sFileName, std::vector< surf::Ipoint >& ipts, bool bVerbose, int& __vlen )
00143 {
00144 std::ifstream ipfile(sFileName.c_str());
00145
00146 if( !ipfile ) {
00147 std::cerr << "ERROR in loadIpoints(): "
00148 << "Couldn't open file '" << sFileName.c_str() << "'!" << std::endl;
00149 return;
00150 }
00151
00152
00153
00154 unsigned count;
00155 ipfile >> __vlen >> count;
00156
00157 __vlen--;
00158
00159
00160 ipts.clear();
00161 ipts.resize(count);
00162
00163
00164 for (unsigned n=0; n<count; n++){
00165
00166 float x, y, a, b, c;
00167 ipfile >> x >> y >> a >> b >> c;
00168
00169 float det = sqrt((a-c)*(a-c) + 4.0*b*b);
00170 float e1 = 0.5*(a+c + det);
00171 float e2 = 0.5*(a+c - det);
00172 float l1 = (1.0/sqrt(e1));
00173 float l2 = (1.0/sqrt(e2));
00174 float sc = sqrt( l1*l2 );
00175
00176 ipts[n].x = x;
00177 ipts[n].y = y;
00178 ipts[n].scale = sc/2.5;
00179 ipfile >> ipts[n].laplace;
00180
00181
00182 ipts[n].ivec = new double[ __vlen];
00183
00184 for( int j = 0 ; j < __vlen; j++ )
00185 {
00186
00187 ipfile >> ipts[n].ivec[j];
00188
00189
00190 }
00191
00192 }
00193
00194
00195 ipfile.close();
00196
00197
00198 if( bVerbose )
00199 std::cout << "read in " << count << " interest points." << std::endl;
00200 }
00201
00202
00203
00204
00205
00206
00207
00208
00209
00210
00211
00212
00213
00214
00215
00216 SurfClassifier::SurfClassifier( std::string keypoints_dir, unsigned int min_match, float min_match_ratio,
00217 int samplingStep, int octaves, double thres,
00218 bool doubleImageSize, int initLobe,
00219 bool upright, bool extended, int indexSize ): Classifier("SurfClassifier")
00220 {
00221 __obj_features.clear();
00222 __obj_features.reserve(1000);
00223
00224 __min_match = min_match;
00225 __min_match_ratio = min_match_ratio;
00226
00227 __samplingStep = samplingStep;
00228 __octaves = octaves;
00229 __thres = thres;
00230 __doubleImageSize = doubleImageSize;
00231 __initLobe = initLobe;
00232
00233 __upright = upright;
00234 __extended = extended;
00235 __indexSize = indexSize;
00236
00237
00238 __vlen = 0;
00239
00240
00241 __tt = new fawkes::TimeTracker();
00242 __loop_count = 0;
00243 __ttc_objconv = __tt->add_class("ObjectConvert");
00244 __ttc_objfeat = __tt->add_class("ObjectFeatures");
00245 __ttc_imgconv = __tt->add_class("ImageConvert");
00246 __ttc_imgfeat = __tt->add_class("ImageFeatures");
00247 __ttc_matchin = __tt->add_class("Matching");
00248 __ttc_roimerg = __tt->add_class("MergeROIs");
00249
00250
00251
00252 __tt->ping_start(__ttc_objconv);
00253
00254
00255 DIR *dir = 0;
00256
00257 if( (dir = opendir( keypoints_dir.c_str() ) ) == NULL ) {
00258 char* buffer = new char[256];
00259 sprintf(buffer, "The directory %s does not exist!", keypoints_dir.c_str() );
00260 fawkes::LibLogger::log_error("SurfClassifier",buffer);
00261 }
00262
00263 struct dirent* ent;
00264 std::string object_file;
00265 int num_obj_index = 0;
00266
00267
00268 while( (ent = readdir(dir)) != NULL ) {
00269
00270 if ( strcmp( ent->d_name, ".") == 0 || strcmp( ent->d_name,"..") == 0 || strcmp( ent->d_name,".svn") == 0 )
00271 continue;
00272
00273 object_file = keypoints_dir + ent->d_name;
00274 std:: cout<<"SurfClassifier: reading the following descriptor file" << object_file << std::endl;
00275
00276 __obj_names.push_back(object_file);
00277
00278
00279 bool b_verbose = BVERBOSE;
00280 loadIpoints( object_file, __obj_features[num_obj_index], b_verbose, __vlen);
00281 num_obj_index++;
00282
00283 }
00284
00285 closedir(dir);
00286 delete ent;
00287
00288 __num_obj = num_obj_index;
00289
00290 if( num_obj_index != 0 ) {
00291 std::cout<< "SurfClassifier: Reading successful"<< std::endl;
00292
00293 __tt->ping_end(__ttc_objconv);
00294
00295 }
00296 else {
00297
00298 std::cout <<"SurfClassifier: The descriptor directory is probably empty since no objects were read off. Will instantiate a Surfclassifier with the png images directory") << std::endl;
00299 return new SurfClassifier( "../res/opx/objects/", 5 );
00300 }
00301
00302
00303
00304
00305
00306
00307 __tt->ping_start(__ttc_objfeat);
00308
00309
00310 __tt->ping_end(__ttc_objfeat);
00311
00312
00313
00314 }
00315
00316
00317
00318
00319
00320
00321
00322
00323
00324
00325
00326
00327
00328
00329
00330
00331
00332
00333
00334
00335 SurfClassifier::SurfClassifier( const char * object_dir,
00336 unsigned int min_match, float min_match_ratio,
00337 int samplingStep, int octaves, double thres,
00338 bool doubleImageSize, int initLobe,
00339 bool upright, bool extended, int indexSize)
00340 : Classifier("SurfClassifier")
00341 {
00342
00343 __obj_features.clear();
00344 __obj_features.reserve(1000);
00345
00346 __min_match = min_match;
00347 __min_match_ratio = min_match_ratio;
00348
00349 __samplingStep = samplingStep;
00350 __octaves = octaves;
00351 __thres = thres;
00352 __doubleImageSize = doubleImageSize;
00353 __initLobe = initLobe;
00354
00355 __upright = upright;
00356 __extended = extended;
00357 __indexSize = indexSize;
00358
00359
00360 __vlen = 0;
00361
00362
00363
00364 __tt = new fawkes::TimeTracker();
00365 __loop_count = 0;
00366 __ttc_objconv = __tt->add_class("ObjectConvert");
00367 __ttc_objfeat = __tt->add_class("ObjectFeatures");
00368 __ttc_imgconv = __tt->add_class("ImageConvert");
00369 __ttc_imgfeat = __tt->add_class("ImageFeatures");
00370 __ttc_matchin = __tt->add_class("Matching");
00371 __ttc_roimerg = __tt->add_class("MergeROIs");
00372
00373
00374
00375 __tt->ping_start(__ttc_objconv);
00376
00377
00378
00379 DIR *dir = 0;
00380
00381 std::string dir_path = object_dir;
00382
00383 if( (dir = opendir( dir_path.c_str() ) ) == NULL )
00384 {
00385 char* buffer = new char[256];
00386 sprintf(buffer, "The directory %s does not exist!", dir_path.c_str() );
00387
00388 fawkes::LibLogger::log_error("SurfClassifier",buffer);
00389 }
00390
00391 struct dirent* ent;
00392 std::string object_file;
00393 int num_obj_index = 0;
00394
00395 while( (ent = readdir(dir)) != NULL ) {
00396
00397 if ( strcmp( ent->d_name, ".") == 0 || strcmp( ent->d_name,"..") == 0 || strcmp( ent->d_name,".svn") == 0)
00398 continue;
00399
00400 object_file = dir_path + ent->d_name;
00401
00402
00403
00404
00405
00406 std::cout << "SurfClassifier(classify): opening object image file '" << object_file << "'" << std::endl;
00407
00408 PNGReader pngr( object_file.c_str() );
00409 unsigned char* buf = malloc_buffer( pngr.colorspace(), pngr.pixel_width(), pngr.pixel_height() );
00410 pngr.set_buffer( buf );
00411 pngr.read();
00412
00413 unsigned int lwidth = pngr.pixel_width();
00414 unsigned int lheight = pngr.pixel_height();
00415 surf::Image * __simage = new surf::Image( lwidth, lheight );
00416 for (unsigned int h = 0; h < lheight; ++h) {
00417 for (unsigned int w = 0; w < lwidth ; ++w) {
00418 __simage->setPix(w, h, (double)buf[h * lwidth + w] / 255.f);
00419 }
00420 }
00421
00422 __obj_img = new surf::Image(__simage, __doubleImageSize);
00423
00424
00425
00426
00427
00428 if ( ! __obj_img ) {
00429 throw fawkes::Exception("Could not load object file '%s'", object_file.c_str());
00430 }
00431
00432
00433 __tt->ping_end(__ttc_objconv);
00434
00435
00436
00437
00438
00439
00440 __tt->ping_start(__ttc_objfeat);
00441
00442
00443
00444
00445 std::vector<surf::Ipoint> obj_feature;
00446 __obj_features.push_back( obj_feature );
00447 __obj_features[num_obj_index].clear();
00448 __obj_features[num_obj_index].reserve(1000);
00449 __obj_num_features = 0;
00450
00451 surf::FastHessian fh(__obj_img,
00452 __obj_features[num_obj_index],
00453 __thres,
00454 __doubleImageSize,
00455 __initLobe * 3 ,
00456 __samplingStep,
00457 __octaves );
00458
00459 fh.getInterestPoints();
00460
00461 surf::Surf des(__obj_img,
00462 __doubleImageSize,
00463 __upright,
00464 __extended,
00465 __indexSize );
00466
00467
00468 __vlen = des.getVectLength();
00469
00470
00471
00472
00473 for (unsigned n=0; n < __obj_features[num_obj_index].size(); n++){
00474
00475 des.setIpoint(&(__obj_features.at(num_obj_index).at(n)));
00476
00477 des.assignOrientation();
00478
00479 des.makeDescriptor();
00480
00481 }
00482
00483
00484 __obj_num_features = __obj_features[num_obj_index].size();
00485 if ( ! __obj_num_features > 0 ) {
00486 throw fawkes::Exception("Could not compute object features");
00487 }
00488 std::cout << "SurfClassifier(classify): computed '" << __obj_num_features << "' features from object" << std::endl;
00489
00490 char buffer[256];
00491 sprintf( buffer, "descriptors/%s-%d.surf", ent->d_name, num_obj_index );
00492 std::string des_file_name = buffer;
00493
00494 bool b_verbose = BVERBOSE;
00495 bool b_laplacian = true;
00496
00497 __obj_names.push_back( des_file_name );
00498
00499
00500
00501 saveIpoints( des_file_name, __obj_features[num_obj_index], b_verbose, b_laplacian, __vlen );
00502
00503
00504
00505 delete __simage;
00506
00507
00508 __tt->ping_end(__ttc_objfeat);
00509
00510
00511 num_obj_index++;
00512 }
00513
00514 __num_obj = num_obj_index;
00515
00516 }
00517
00518
00519
00520 SurfClassifier::~SurfClassifier()
00521 {
00522
00523 }
00524
00525
00526 std::list< ROI > *
00527 SurfClassifier::classify()
00528 {
00529
00530
00531
00532 __tt->ping_start(0);
00533
00534
00535
00536
00537 std::list<ROI> rv[__num_obj];
00538 float match_ratios[__num_obj];
00539
00540
00541
00542
00543
00544 int x_min = _width;
00545 int y_min = _height;
00546 int x_max = 0;
00547 int y_max = 0;
00548
00549
00550 __tt->ping_start(__ttc_imgconv);
00551
00552 std::cout << "SurfClassifier(classify): copy imgdat to SURF Image" << std::endl;
00553
00554
00555
00556
00557
00558
00559
00560
00561
00562
00563
00564
00565
00566
00567
00568
00569 surf::Image * __simage = new surf::Image( _width, _height);
00570 for (unsigned int h = 0; h < _height; ++h) {
00571 for (unsigned int w = 0; w < _width; ++w) {
00572 __simage->setPix(w, h, (double)_src[h * _width + w] / 255.f);
00573 }
00574 }
00575
00576 __image = new surf::Image(__simage, __doubleImageSize);
00577
00578
00579 __tt->ping_end(__ttc_imgconv);
00580
00581
00582
00583
00584
00585 //surf::ImLoad::saveImage( "tst.pgm", __simage);
00586
00587 //surf::ImLoad::saveImage( "tst.pgm", __image);
00588
00589 PNMWriter pnm(PNM_PGM, "fvimg.pgm", _width, _height);
00590 pnm.set_buffer(YUV422_PLANAR, _src );
00591 pnm.write();
00592
00593 PNGWriter pngw("fvimg.png", _width, _height);
00594 pngw.set_buffer(YUV422_PLANAR, _src );
00595 pngw.write();
00596 */
00597
00598
00599 __tt->ping_start(__ttc_imgfeat);
00600
00601
00602
00603 __img_features.clear();
00604 __img_features.reserve(1000);
00605 __img_num_features = 0;
00606
00607 surf::FastHessian fh(__image,
00608 __img_features,
00609 __thres,
00610 __doubleImageSize,
00611 __initLobe * 3 ,
00612 __samplingStep,
00613 __octaves );
00614
00615 std::cout<<"surfclassifer/classify : getting interest points"<<std::endl;
00616 fh.getInterestPoints();
00617
00618 surf::Surf des(__image,
00619 __doubleImageSize,
00620 __upright,
00621 __extended,
00622 __indexSize );
00623
00624
00625
00626
00627
00628
00629
00630 for (unsigned n=0; n < __img_features.size(); n++){
00631
00632
00633 des.setIpoint(&__img_features[n]);
00634
00635 des.assignOrientation();
00636
00637 des.makeDescriptor();
00638 }
00639 __img_num_features = __img_features.size();
00640
00641 __tt->ping_end(__ttc_imgfeat);
00642
00643
00644 std::cout << "Extracted '" << __img_num_features << "' image features" << std::endl;
00645
00646
00647
00648 __tt->ping_start(__ttc_matchin);
00649
00650 std::cout << "SurfClassifier(classify): matching ..." << std::endl;
00651
00652 for( unsigned j = 0; j < __num_obj; j++ )
00653 {
00654 std::vector< int > matches(__obj_features[j].size());
00655
00656 int c = 0;
00657 for (unsigned i = 0; i < __obj_features[j].size(); i++) {
00658 int match = findMatch((__obj_features[j])[i], __img_features);
00659 matches[i] = match;
00660 if (match != -1) {
00661
00662
00663 ROI r( (int)(__img_features[matches[i]].x)-5, (int)(__img_features[matches[i]].y )-5, 11, 11, _width, _height);
00664 r.num_hint_points = 0;
00665 rv[j].push_back(r);
00666
00667 ++c;
00668 }
00669 }
00670
00671 __tt->ping_end(__ttc_matchin);
00672
00673 if( c == 0 )
00674 std::cout << "SurfClassifier(classify) matched '" << c << fawkes::cnormal <<"' of '" << __obj_features[j].size() << "' features in scene. (for supplied object = " << j << std::endl ;
00675 else
00676 std::cout << "SurfClassifier(classify) matched '" << fawkes::cblue << c << fawkes::cnormal <<"' of '" << __obj_features[j].size() << "' features in scene. (for supplied object = " << j << std::endl ;
00677
00678
00679 float match_ratio = ((float)c / (float)__obj_features[j].size());
00680 match_ratios[j] = match_ratio;
00681
00682 std::cout << "SurfClassifier(classify): match_ratio is '" << match_ratio << "' and min_match_ratio is" << __min_match_ratio << std::endl;
00683
00684 std::cout << "SurfClassifier(classify): computing ROI" << std::endl;
00685
00686 __tt->ping_start(__ttc_roimerg);
00687
00688 for (unsigned i = 0; i < matches.size(); i++) {
00689 if (matches[i] != -1) {
00690
00691
00692 if( (int)__img_features[matches[i]].x < x_min )
00693 x_min = (int)__img_features[matches[i]].x;
00694 if( (int)__img_features[matches[i]].y < y_min )
00695 y_min = (int)__img_features[matches[i]].y;
00696 if( (int)__img_features[matches[i]].x > x_max )
00697 x_max = (int)__img_features[matches[i]].x;
00698 if( (int)__img_features[matches[i]].y > y_max )
00699 y_max = (int)__img_features[matches[i]].y;
00700 }
00701 }
00702 if( (c != 0) && ((unsigned)c > __min_match) &&
00703 (match_ratio > __min_match_ratio) &&
00704 (x_max - x_min != 0 ) && (y_max - y_min != 0) ) {
00705
00706 std::cout << "SurfClassifier(classify): c='" << c << "' __min_match='" << __min_match << "'." << std::endl;
00707
00708 ROI r(x_min, y_min, x_max-x_min, y_max-y_min, _width, _height);
00709 r.num_hint_points = c;
00710 rv[j].push_back(r);
00711 } else {
00712 std::cout << " clearing ROI-list (no or too few matches or [0,0]-roi!)" << std::endl;
00713 rv[j].clear();
00714 }
00715 }
00716
00717 __tt->ping_end(__ttc_roimerg);
00718
00719
00720
00721 delete __image;
00722 delete __simage;
00723
00724
00725 __tt->ping_end(0);
00726
00727
00728
00729
00730
00731
00732
00733
00734
00735 float min_ratio_tmp = -1.0;
00736 int min_ratio_index = -1;
00737 for( unsigned int i = 0; i < __num_obj; i++ )
00738 {
00739 if( match_ratios[i] > min_ratio_tmp )
00740 {
00741 min_ratio_tmp = match_ratios[i];
00742 min_ratio_index = i;
00743 }
00744 }
00745
00746 std::list<ROI> *final_rv = new std::list<ROI>;
00747
00748 final_rv->assign( rv[min_ratio_index].begin(), rv[min_ratio_index].end() );
00749
00750
00751 std::string first_not(".-");
00752 int first_not_index = __obj_names[ min_ratio_index ].find_first_of( first_not );
00753 std::string obj_name_tmp( __obj_names[ min_ratio_index ] );
00754 obj_name_tmp.erase( first_not_index );
00755
00756
00757 std::cout << "SurfClassifier(classify): done, ... returning '" << rv->size() << "' ROIs. The object class is " << min_ratio_index << "and object name is " << fawkes::cgreen << obj_name_tmp << fawkes::cnormal << std::endl;
00758 return final_rv;
00759 }
00760
00761 int
00762 SurfClassifier::findMatch(const surf::Ipoint& ip1, const std::vector< surf::Ipoint >& ipts) {
00763 double mind = 1e100, second = 1e100;
00764 int match = -1;
00765
00766
00767
00768 for (unsigned i = 0; i < ipts.size(); i++) {
00769
00770 if (ipts[i].laplace != ip1.laplace)
00771 continue;
00772
00773 double d = distSquare(ipts[i].ivec, ip1.ivec, __vlen);
00774
00775 if (d < mind) {
00776 second = mind;
00777 mind = d;
00778 match = i;
00779 } else if (d < second) {
00780 second = d;
00781 }
00782 }
00783
00784 if (mind < 0.5 * second)
00785 return match;
00786
00787 return -1;
00788 }
00789
00790
00791 double
00792 SurfClassifier::distSquare(double *v1, double *v2, int n) {
00793 double dsq = 0.;
00794
00795
00796 while (n--) {
00797
00798 dsq += (*v1 - *v2) * (*v1 - *v2);
00799 v1++;
00800 v2++;
00801 }
00802
00803
00804
00805 return dsq;
00806 }
00807
00808 }