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