root/trunk/opencv/samples/c/find_obj.cpp @ 2065

Revision 2065, 10.0 KB (checked in by vp153, 5 years ago)

added sparse lev-marq (aka bundle adjustment engine), contributed by Sergey Molinov; fixed find_obj.cpp sample and compilation with MINGW

Line 
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
17using namespace std;
18
19
20// define whether to use approximate nearest-neighbor search
21#define USE_FLANN
22
23
24IplImage *image = 0;
25
26double
27compareSURFDescriptors( 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
45int
46naiveNearestNeighbor( 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
80void
81findPairs( 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
106void
107flannFindPairs( 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 */
156int
157locatePlanarObject( 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
203int 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}
Note: See TracBrowser for help on using the browser.