samples/c/find_obj.cpp

   1 /*
   2  * A Demo to OpenCV Implementation of SURF
   3  * Further Information Refer to "SURF: Speed-Up Robust Feature"
   4  * Author: Liu Liu
   5  * liuliu.1987+opencv@gmail.com
   6  */
   7 #include <opencv2/core/core.hpp>
   8 #include <opencv2/objdetect/objdetect.hpp>
   9 #include <opencv2/features2d/features2d.hpp>
  10 #include <opencv2/highgui/highgui.hpp>
  11 #include <opencv2/calib3d/calib3d.hpp>
  12 #include <opencv2/imgproc/imgproc_c.h>
  13
  14 #include <iostream>
  15 #include <vector>
  16
  17 using namespace std;
  18 using namespace cv;
  19
  20 void help()
  21 {
  22     printf( "This program demonstrated the use of the SURF Detector and Descriptor using\n"
  23             "either FLANN (fast approx nearst neighbor classification) or brute force matching\n"
  24             "on planar objects.\n"
  25             "Call:\n"
  26             "./find_obj [--object_filename]=<object_filename, box.png as default> \n"
  27                     "[--scene_filename]=<scene_filename box_in_scene.png as default>]\n\n"
  28             );
  29 }
  30
  31 // define whether to use approximate nearest-neighbor search
  32 #define USE_FLANN
  33
  34
  35 IplImage* image = 0;
  36
  37 double
  38 compareSURFDescriptors( const float* d1, const float* d2, double best, int length )
  39 {
  40     double total_cost = 0;
  41     assert( length % 4 == 0 );
  42     for( int i = 0; i < length; i += 4 )
  43     {
  44         double t0 = d1[i  ] - d2[i  ];
  45         double t1 = d1[i+1] - d2[i+1];
  46         double t2 = d1[i+2] - d2[i+2];
  47         double t3 = d1[i+3] - d2[i+3];
  48         total_cost += t0*t0 + t1*t1 + t2*t2 + t3*t3;
  49         if( total_cost > best )
  50             break;
  51     }
  52     return total_cost;
  53 }
  54
  55
  56 int
  57 naiveNearestNeighbor( const float* vec, int laplacian,
  58                       const CvSeq* model_keypoints,
  59                       const CvSeq* model_descriptors )
  60 {
  61     int length = (int)(model_descriptors->elem_size/sizeof(float));
  62     int i, neighbor = -1;
  63     double d, dist1 = 1e6, dist2 = 1e6;
  64     CvSeqReader reader, kreader;
  65     cvStartReadSeq( model_keypoints, &kreader, 0 );
  66     cvStartReadSeq( model_descriptors, &reader, 0 );
  67
  68     for( i = 0; i < model_descriptors->total; i++ )
  69     {
  70         const CvSURFPoint* kp = (const CvSURFPoint*)kreader.ptr;
  71         const float* mvec = (const float*)reader.ptr;
  72         CV_NEXT_SEQ_ELEM( kreader.seq->elem_size, kreader );
  73         CV_NEXT_SEQ_ELEM( reader.seq->elem_size, reader );
  74         if( laplacian != kp->laplacian )
  75             continue;
  76         d = compareSURFDescriptors( vec, mvec, dist2, length );
  77         if( d < dist1 )
  78         {
  79             dist2 = dist1;
  80             dist1 = d;
  81             neighbor = i;
  82         }
  83         else if ( d < dist2 )
  84             dist2 = d;
  85     }
  86     if ( dist1 < 0.6*dist2 )
  87         return neighbor;
  88     return -1;
  89 }
  90
  91 void
  92 findPairs( const CvSeq* objectKeypoints, const CvSeq* objectDescriptors,
  93            const CvSeq* imageKeypoints, const CvSeq* imageDescriptors, vector<int>& ptpairs )
  94 {
  95     int i;
  96     CvSeqReader reader, kreader;
  97     cvStartReadSeq( objectKeypoints, &kreader );
  98     cvStartReadSeq( objectDescriptors, &reader );
  99     ptpairs.clear();
 100
 101     for( i = 0; i < objectDescriptors->total; i++ )
 102     {
 103         const CvSURFPoint* kp = (const CvSURFPoint*)kreader.ptr;
 104         const float* descriptor = (const float*)reader.ptr;
 105         CV_NEXT_SEQ_ELEM( kreader.seq->elem_size, kreader );
 106         CV_NEXT_SEQ_ELEM( reader.seq->elem_size, reader );
 107         int nearest_neighbor = naiveNearestNeighbor( descriptor, kp->laplacian, imageKeypoints, imageDescriptors );
 108         if( nearest_neighbor >= 0 )
 109         {
 110             ptpairs.push_back(i);
 111             ptpairs.push_back(nearest_neighbor);
 112         }
 113     }
 114 }
 115
 116
 117 void
 118 flannFindPairs( const CvSeq*, const CvSeq* objectDescriptors,
 119            const CvSeq*, const CvSeq* imageDescriptors, vector<int>& ptpairs )
 120 {
 121         int length = (int)(objectDescriptors->elem_size/sizeof(float));
 122
 123     cv::Mat m_object(objectDescriptors->total, length, CV_32F);
 124         cv::Mat m_image(imageDescriptors->total, length, CV_32F);
 125
 126
 127         // copy descriptors
 128     CvSeqReader obj_reader;
 129         float* obj_ptr = m_object.ptr<float>(0);
 130     cvStartReadSeq( objectDescriptors, &obj_reader );
 131     for(int i = 0; i < objectDescriptors->total; i++ )
 132     {
 133         const float* descriptor = (const float*)obj_reader.ptr;
 134         CV_NEXT_SEQ_ELEM( obj_reader.seq->elem_size, obj_reader );
 135         memcpy(obj_ptr, descriptor, length*sizeof(float));
 136         obj_ptr += length;
 137     }
 138     CvSeqReader img_reader;
 139         float* img_ptr = m_image.ptr<float>(0);
 140     cvStartReadSeq( imageDescriptors, &img_reader );
 141     for(int i = 0; i < imageDescriptors->total; i++ )
 142     {
 143         const float* descriptor = (const float*)img_reader.ptr;
 144         CV_NEXT_SEQ_ELEM( img_reader.seq->elem_size, img_reader );
 145         memcpy(img_ptr, descriptor, length*sizeof(float));
 146         img_ptr += length;
 147     }
 148
 149     // find nearest neighbors using FLANN
 150     cv::Mat m_indices(objectDescriptors->total, 2, CV_32S);
 151     cv::Mat m_dists(objectDescriptors->total, 2, CV_32F);
 152     cv::flann::Index flann_index(m_image, cv::flann::KDTreeIndexParams(4));  // using 4 randomized kdtrees
 153     flann_index.knnSearch(m_object, m_indices, m_dists, 2, cv::flann::SearchParams(64) ); // maximum number of leafs checked
 154
 155     int* indices_ptr = m_indices.ptr<int>(0);
 156     float* dists_ptr = m_dists.ptr<float>(0);
 157     for (int i=0;i<m_indices.rows;++i) {
 158         if (dists_ptr[2*i]<0.6*dists_ptr[2*i+1]) {
 159                 ptpairs.push_back(i);
 160                 ptpairs.push_back(indices_ptr[2*i]);
 161         }
 162     }
 163 }
 164
 165
 166 /* a rough implementation for object location */
 167 int
 168 locatePlanarObject( const CvSeq* objectKeypoints, const CvSeq* objectDescriptors,
 169                     const CvSeq* imageKeypoints, const CvSeq* imageDescriptors,
 170                     const CvPoint src_corners[4], CvPoint dst_corners[4] )
 171 {
 172     double h[9];
 173     CvMat _h = cvMat(3, 3, CV_64F, h);
 174     vector<int> ptpairs;
 175     vector<CvPoint2D32f> pt1, pt2;
 176     CvMat _pt1, _pt2;
 177     int i, n;
 178
 179 #ifdef USE_FLANN
 180     flannFindPairs( objectKeypoints, objectDescriptors, imageKeypoints, imageDescriptors, ptpairs );
 181 #else
 182     findPairs( objectKeypoints, objectDescriptors, imageKeypoints, imageDescriptors, ptpairs );
 183 #endif
 184
 185     n = (int)(ptpairs.size()/2);
 186     if( n < 4 )
 187         return 0;
 188
 189     pt1.resize(n);
 190     pt2.resize(n);
 191     for( i = 0; i < n; i++ )
 192     {
 193         pt1[i] = ((CvSURFPoint*)cvGetSeqElem(objectKeypoints,ptpairs[i*2]))->pt;
 194         pt2[i] = ((CvSURFPoint*)cvGetSeqElem(imageKeypoints,ptpairs[i*2+1]))->pt;
 195     }
 196
 197     _pt1 = cvMat(1, n, CV_32FC2, &pt1[0] );
 198     _pt2 = cvMat(1, n, CV_32FC2, &pt2[0] );
 199     if( !cvFindHomography( &_pt1, &_pt2, &_h, CV_RANSAC, 5 ))
 200         return 0;
 201
 202     for( i = 0; i < 4; i++ )
 203     {
 204         double x = src_corners[i].x, y = src_corners[i].y;
 205         double Z = 1./(h[6]*x + h[7]*y + h[8]);
 206         double X = (h[0]*x + h[1]*y + h[2])*Z;
 207         double Y = (h[3]*x + h[4]*y + h[5])*Z;
 208         dst_corners[i] = cvPoint(cvRound(X), cvRound(Y));
 209     }
 210
 211     return 1;
 212 }
 213
 214 int main(int argc, const char** argv)
 215 {
 216     help();
 217
 218     CommandLineParser parser(argc, argv);
 219
 220     string objectFileName = parser.get<string>("object_filename", "box.png");
 221     string sceneFileName = parser.get<string>("scene_filename", "box_in_scene.png");
 222
 223     CvMemStorage* storage = cvCreateMemStorage(0);
 224     cvNamedWindow("Object", 1);
 225     cvNamedWindow("Object Correspond", 1);
 226
 227     static CvScalar colors[] =
 228     {
 229         {{0,0,255}},
 230         {{0,128,255}},
 231         {{0,255,255}},
 232         {{0,255,0}},
 233         {{255,128,0}},
 234         {{255,255,0}},
 235         {{255,0,0}},
 236         {{255,0,255}},
 237         {{255,255,255}}
 238     };
 239
 240     IplImage* object = cvLoadImage( objectFileName.c_str(), CV_LOAD_IMAGE_GRAYSCALE );
 241     IplImage* image = cvLoadImage( sceneFileName.c_str(), CV_LOAD_IMAGE_GRAYSCALE );
 242     if( !object || !image )
 243     {
 244         fprintf( stderr, "Can not load %s and/or %s\n", objectFileName.c_str(), sceneFileName.c_str() );
 245         exit(-1);
 246     }
 247     IplImage* object_color = cvCreateImage(cvGetSize(object), 8, 3);
 248     cvCvtColor( object, object_color, CV_GRAY2BGR );
 249
 250     CvSeq *objectKeypoints = 0, *objectDescriptors = 0;
 251     CvSeq *imageKeypoints = 0, *imageDescriptors = 0;
 252     int i;
 253     CvSURFParams params = cvSURFParams(500, 1);
 254
 255     double tt = (double)cvGetTickCount();
 256     cvExtractSURF( object, 0, &objectKeypoints, &objectDescriptors, storage, params );
 257     printf("Object Descriptors: %d\n", objectDescriptors->total);
 258     cvExtractSURF( image, 0, &imageKeypoints, &imageDescriptors, storage, params );
 259     printf("Image Descriptors: %d\n", imageDescriptors->total);
 260     tt = (double)cvGetTickCount() - tt;
 261     printf( "Extraction time = %gms\n", tt/(cvGetTickFrequency()*1000.));
 262     CvPoint src_corners[4] = {{0,0}, {object->width,0}, {object->width, object->height}, {0, object->height}};
 263     CvPoint dst_corners[4];
 264     IplImage* correspond = cvCreateImage( cvSize(image->width, object->height+image->height), 8, 1 );
 265     cvSetImageROI( correspond, cvRect( 0, 0, object->width, object->height ) );
 266     cvCopy( object, correspond );
 267     cvSetImageROI( correspond, cvRect( 0, object->height, correspond->width, correspond->height ) );
 268     cvCopy( image, correspond );
 269     cvResetImageROI( correspond );
 270
 271 #ifdef USE_FLANN
 272     printf("Using approximate nearest neighbor search\n");
 273 #endif
 274
 275     if( locatePlanarObject( objectKeypoints, objectDescriptors, imageKeypoints,
 276         imageDescriptors, src_corners, dst_corners ))
 277     {
 278         for( i = 0; i < 4; i++ )
 279         {
 280             CvPoint r1 = dst_corners[i%4];
 281             CvPoint r2 = dst_corners[(i+1)%4];
 282             cvLine( correspond, cvPoint(r1.x, r1.y+object->height ),
 283                 cvPoint(r2.x, r2.y+object->height ), colors[8] );
 284         }
 285     }
 286     vector<int> ptpairs;
 287 #ifdef USE_FLANN
 288     flannFindPairs( objectKeypoints, objectDescriptors, imageKeypoints, imageDescriptors, ptpairs );
 289 #else
 290     findPairs( objectKeypoints, objectDescriptors, imageKeypoints, imageDescriptors, ptpairs );
 291 #endif
 292     for( i = 0; i < (int)ptpairs.size(); i += 2 )
 293     {
 294         CvSURFPoint* r1 = (CvSURFPoint*)cvGetSeqElem( objectKeypoints, ptpairs[i] );
 295         CvSURFPoint* r2 = (CvSURFPoint*)cvGetSeqElem( imageKeypoints, ptpairs[i+1] );
 296         cvLine( correspond, cvPointFrom32f(r1->pt),
 297             cvPoint(cvRound(r2->pt.x), cvRound(r2->pt.y+object->height)), colors[8] );
 298     }
 299
 300     cvShowImage( "Object Correspond", correspond );
 301     for( i = 0; i < objectKeypoints->total; i++ )
 302     {
 303         CvSURFPoint* r = (CvSURFPoint*)cvGetSeqElem( objectKeypoints, i );
 304         CvPoint center;
 305         int radius;
 306         center.x = cvRound(r->pt.x);
 307         center.y = cvRound(r->pt.y);
 308         radius = cvRound(r->size*1.2/9.*2);
 309         cvCircle( object_color, center, radius, colors[0], 1, 8, 0 );
 310     }
 311     cvShowImage( "Object", object_color );
 312
 313     cvWaitKey(0);
 314
 315     cvDestroyWindow("Object");
 316     cvDestroyWindow("Object Correspond");
 317
 318     return 0;
 319 }