3 * Copyright (C) 1997-2012 Adam Williams <broadcast at earthling dot net>
5 * This program is free software; you can redistribute it and/or modify
6 * it under the terms of the GNU General Public License as published by
7 * the Free Software Foundation; either version 2 of the License, or
8 * (at your option) any later version.
10 * This program is distributed in the hope that it will be useful,
11 * but WITHOUT ANY WARRANTY; without even the implied warranty of
12 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
13 * GNU General Public License for more details.
15 * You should have received a copy of the GNU General Public License
16 * along with this program; if not, write to the Free Software
17 * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
23 #include "opencvwrapper.h"
26 #include "opencv2/calib3d/calib3d.hpp"
27 #include "opencv2/objdetect/objdetect.hpp"
28 #include "opencv2/features2d/features2d.hpp"
42 // define whether to use approximate nearest-neighbor search
45 // Sizes must be quantized a certain amount for OpenCV
51 compareSURFDescriptors( const float* d1, const float* d2, double best, int length )
53 double total_cost = 0;
54 assert( length % 4 == 0 );
55 for( int i = 0; i < length; i += 4 )
57 double t0 = d1[i ] - d2[i ];
58 double t1 = d1[i+1] - d2[i+1];
59 double t2 = d1[i+2] - d2[i+2];
60 double t3 = d1[i+3] - d2[i+3];
61 total_cost += t0*t0 + t1*t1 + t2*t2 + t3*t3;
62 if( total_cost > best )
70 naiveNearestNeighbor( const float* vec, int laplacian,
71 const CvSeq* model_keypoints,
72 const CvSeq* model_descriptors )
74 int length = (int)(model_descriptors->elem_size/sizeof(float));
76 double d, dist1 = 1e6, dist2 = 1e6;
77 CvSeqReader reader, kreader;
78 cvStartReadSeq( model_keypoints, &kreader, 0 );
79 cvStartReadSeq( model_descriptors, &reader, 0 );
81 for( i = 0; i < model_descriptors->total; i++ )
83 const CvSURFPoint* kp = (const CvSURFPoint*)kreader.ptr;
84 const float* mvec = (const float*)reader.ptr;
85 CV_NEXT_SEQ_ELEM( kreader.seq->elem_size, kreader );
86 CV_NEXT_SEQ_ELEM( reader.seq->elem_size, reader );
87 if( laplacian != kp->laplacian )
89 d = compareSURFDescriptors( vec, mvec, dist2, length );
99 if ( dist1 < 0.6*dist2 )
105 findPairs( const CvSeq* objectKeypoints, const CvSeq* objectDescriptors,
106 const CvSeq* imageKeypoints, const CvSeq* imageDescriptors, vector<int>& ptpairs )
109 CvSeqReader reader, kreader;
110 cvStartReadSeq( objectKeypoints, &kreader );
111 cvStartReadSeq( objectDescriptors, &reader );
114 for( i = 0; i < objectDescriptors->total; i++ )
116 const CvSURFPoint* kp = (const CvSURFPoint*)kreader.ptr;
117 const float* descriptor = (const float*)reader.ptr;
118 CV_NEXT_SEQ_ELEM( kreader.seq->elem_size, kreader );
119 CV_NEXT_SEQ_ELEM( reader.seq->elem_size, reader );
120 int nearest_neighbor = naiveNearestNeighbor( descriptor, kp->laplacian, imageKeypoints, imageDescriptors );
121 if( nearest_neighbor >= 0 )
123 ptpairs.push_back(i);
124 ptpairs.push_back(nearest_neighbor);
131 flannFindPairs( const CvSeq*,
132 const CvSeq* objectDescriptors,
134 const CvSeq* imageDescriptors,
135 vector<int>& ptpairs )
137 int length = (int)(objectDescriptors->elem_size/sizeof(float));
139 cv::Mat m_object(objectDescriptors->total, length, CV_32F);
140 cv::Mat m_image(imageDescriptors->total, length, CV_32F);
144 CvSeqReader obj_reader;
145 float* obj_ptr = m_object.ptr<float>(0);
146 cvStartReadSeq( objectDescriptors, &obj_reader );
147 for(int i = 0; i < objectDescriptors->total; i++ )
149 const float* descriptor = (const float*)obj_reader.ptr;
150 CV_NEXT_SEQ_ELEM( obj_reader.seq->elem_size, obj_reader );
151 memcpy(obj_ptr, descriptor, length*sizeof(float));
154 CvSeqReader img_reader;
155 float* img_ptr = m_image.ptr<float>(0);
156 cvStartReadSeq( imageDescriptors, &img_reader );
157 for(int i = 0; i < imageDescriptors->total; i++ )
159 const float* descriptor = (const float*)img_reader.ptr;
160 CV_NEXT_SEQ_ELEM( img_reader.seq->elem_size, img_reader );
161 memcpy(img_ptr, descriptor, length*sizeof(float));
165 // find nearest neighbors using FLANN
166 cv::Mat m_indices(objectDescriptors->total, 2, CV_32S);
167 cv::Mat m_dists(objectDescriptors->total, 2, CV_32F);
168 cv::flann::Index flann_index(m_image, cv::flann::KDTreeIndexParams(4)); // using 4 randomized kdtrees
169 flann_index.knnSearch(m_object, m_indices, m_dists, 2, cv::flann::SearchParams(64) ); // maximum number of leafs checked
171 int* indices_ptr = m_indices.ptr<int>(0);
172 float* dists_ptr = m_dists.ptr<float>(0);
173 //printf("flannFindPairs %d m_indices.rows=%d\n", __LINE__, m_indices.rows);
174 for (int i = 0; i < m_indices.rows; ++i)
176 //printf("flannFindPairs %d dists=%f %f\n", __LINE__, dists_ptr[2 * i], 0.6 * dists_ptr[2 * i + 1]);
177 if (dists_ptr[2 * i] < 0.6 * dists_ptr[2 * i + 1])
179 //printf("flannFindPairs %d pairs=%d\n", __LINE__, ptpairs.size());
180 ptpairs.push_back(i);
181 ptpairs.push_back(indices_ptr[2*i]);
187 /* a rough implementation for object location */
188 int OpenCVWrapper::locatePlanarObject(const CvSeq* objectKeypoints,
189 const CvSeq* objectDescriptors,
190 const CvSeq* imageKeypoints,
191 const CvSeq* imageDescriptors,
192 const CvPoint src_corners[4],
197 CvMat _h = cvMat(3, 3, CV_64F, h);
199 vector<CvPoint2D32f> pt1, pt2;
207 flannFindPairs( objectKeypoints, objectDescriptors, imageKeypoints, imageDescriptors, ptpairs );
209 findPairs( objectKeypoints, objectDescriptors, imageKeypoints, imageDescriptors, ptpairs );
214 (*point_pairs) = (int*)calloc(ptpairs.size(), sizeof(int));
215 (*total_pairs) = ptpairs.size() / 2;
218 for(int i = 0; i < (int)ptpairs.size(); i++)
220 (*point_pairs)[i] = ptpairs[i];
225 n = (int)(ptpairs.size()/2);
231 for( i = 0; i < n; i++ )
233 pt1[i] = ((CvSURFPoint*)cvGetSeqElem(objectKeypoints,ptpairs[i*2]))->pt;
234 pt2[i] = ((CvSURFPoint*)cvGetSeqElem(imageKeypoints,ptpairs[i*2+1]))->pt;
237 _pt1 = cvMat(1, n, CV_32FC2, &pt1[0] );
238 _pt2 = cvMat(1, n, CV_32FC2, &pt2[0] );
239 if( !cvFindHomography( &_pt1, &_pt2, &_h, CV_RANSAC, 5 ))
242 for( i = 0; i < 4; i++ )
244 double x = src_corners[i].x, y = src_corners[i].y;
245 double Z = 1./(h[6]*x + h[7]*y + h[8]);
246 double X = (h[0]*x + h[1]*y + h[2])*Z;
247 double Y = (h[3]*x + h[4]*y + h[5])*Z;
248 dst_corners[i * 2] = X;
249 dst_corners[i * 2 + 1] = Y;
258 OpenCVWrapper::OpenCVWrapper()
267 object_keypoints = 0;
268 object_descriptors = 0;
270 scene_descriptors = 0;
280 OpenCVWrapper::~OpenCVWrapper()
282 // This releases all the arrays
283 if(storage) cvReleaseMemStorage(&storage);
284 if(object_image) cvReleaseImage(&object_image);
285 if(scene_image) cvReleaseImage(&scene_image);
286 if(point_pairs) free(point_pairs);
293 int OpenCVWrapper::scan(VFrame *object_frame,
305 int object_w = object_x2 - object_x1;
306 int object_h = object_y2 - object_y1;
307 int scene_w = scene_x2 - scene_x1;
308 int scene_h = scene_y2 - scene_y1;
311 //object_frame->write_png("/tmp/object.png");
312 //scene_frame->write_png("/tmp/scene.png");
314 // Get quantized sizes
315 int object_image_w = object_w;
316 int object_image_h = object_h;
317 int scene_image_w = scene_w;
318 int scene_image_h = scene_h;
319 if(object_w % QUANTIZE) object_image_w += QUANTIZE - (object_w % QUANTIZE);
320 if(object_h % QUANTIZE) object_image_h += QUANTIZE - (object_h % QUANTIZE);
321 if(scene_w % QUANTIZE) scene_image_w += QUANTIZE - (scene_w % QUANTIZE);
322 if(scene_h % QUANTIZE) scene_image_h += QUANTIZE - (scene_h % QUANTIZE);
325 (object_image_w != this->object_image_w ||
326 object_image_h != this->object_image_h))
328 cvReleaseImage(&object_image);
331 this->object_image_w = object_image_w;
332 this->object_image_h = object_image_h;
335 (scene_image_w != this->scene_image_w ||
336 scene_image_h != this->scene_image_h))
338 cvReleaseImage(&scene_image);
341 this->scene_image_w = scene_image_w;
342 this->scene_image_h = scene_image_h;
347 // Only does greyscale
348 object_image = cvCreateImage(
349 cvSize(object_image_w, object_image_h),
356 // Only does greyscale
357 scene_image = cvCreateImage(
358 cvSize(scene_image_w, scene_image_h),
363 // Select only region with image size
364 // Does this do anything?
365 cvSetImageROI( object_image, cvRect( 0, 0, object_w, object_h ) );
366 cvSetImageROI( scene_image, cvRect( 0, 0, scene_w, scene_h ) );
368 grey_crop((unsigned char*)scene_image->imageData,
376 grey_crop((unsigned char*)object_image->imageData,
386 if(!storage) storage = cvCreateMemStorage(0);
387 CvSURFParams params = cvSURFParams(500, 1);
390 //printf("OpenCVWrapper::process_buffer %d\n", __LINE__);
392 // TODO: make the surf data persistent & check for image changes instead
393 if(object_keypoints) cvClearSeq(object_keypoints);
394 if(object_descriptors) cvClearSeq(object_descriptors);
395 if(scene_keypoints) cvClearSeq(scene_keypoints);
396 if(scene_descriptors) cvClearSeq(scene_descriptors);
397 object_keypoints = 0;
398 object_descriptors = 0;
400 scene_descriptors = 0;
402 // Free the image structures
403 if(point_pairs) free(point_pairs);
407 cvExtractSURF(object_image,
415 //printf("OpenCVWrapper::scan %d object keypoints=%d\n", __LINE__, object_keypoints->total);
416 // Draw the keypoints
417 // for(int i = 0; i < object_keypoints->total; i++)
419 // CvSURFPoint* r1 = (CvSURFPoint*)cvGetSeqElem( object_keypoints, i );
420 // int size = r1->size / 4;
421 // draw_rect(frame[object_layer],
422 // r1->pt.x + object_x1 - size,
423 // r1->pt.y + object_y1 - size,
424 // r1->pt.x + object_x1 + size,
425 // r1->pt.y + object_y1 + size);
429 //printf("OpenCVWrapper::process_buffer %d\n", __LINE__);
431 cvExtractSURF(scene_image,
439 // Draw the keypoints
440 // for(int i = 0; i < scene_keypoints->total; i++)
442 // CvSURFPoint* r1 = (CvSURFPoint*)cvGetSeqElem( scene_keypoints, i );
443 // int size = r1->size / 4;
444 // draw_rect(frame[scene_layer],
445 // r1->pt.x + scene_x1 - size,
446 // r1->pt.y + scene_y1 - size,
447 // r1->pt.x + scene_x1 + size,
448 // r1->pt.y + scene_y1 + size);
451 // printf("OpenCVWrapper::scan %d %d %d scene keypoints=%d\n",
455 // scene_keypoints->total);
457 CvPoint src_corners[4] =
461 { object_w, object_h },
465 for(int i = 0; i < 8; i++)
472 //printf("OpenCVWrapper::process_buffer %d\n", __LINE__);
473 if(scene_keypoints->total &&
474 object_keypoints->total &&
475 locatePlanarObject(object_keypoints,
495 // Convert to greyscale & crop
496 void OpenCVWrapper::grey_crop(unsigned char *dst,
505 // Dimensions of dst frame
509 bzero(dst, dst_w * dst_h);
511 //printf("OpenCVWrapper::grey_crop %d %d %d\n", __LINE__, w, h);
512 for(int i = 0; i < h; i++)
514 switch(src->get_color_model())
521 unsigned char *input = src->get_rows()[i + y1] + x1 * 3;
522 unsigned char *output = dst + i * dst_w;
524 for(int j = 0; j < w; j++)
538 float OpenCVWrapper::get_dst_x(int number)
540 return dst_corners[number * 2];
543 float OpenCVWrapper::get_dst_y(int number)
545 return dst_corners[number * 2 + 1];