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