2 #include "opencv2/calib3d/calib3d.hpp"
3 #include "opencv2/objdetect/objdetect.hpp"
4 #include "opencv2/features2d/features2d.hpp"
18 // define whether to use approximate nearest-neighbor search
23 compareSURFDescriptors( const float* d1, const float* d2, double best, int length )
25 double total_cost = 0;
26 assert( length % 4 == 0 );
27 for( int i = 0; i < length; i += 4 )
29 double t0 = d1[i ] - d2[i ];
30 double t1 = d1[i+1] - d2[i+1];
31 double t2 = d1[i+2] - d2[i+2];
32 double t3 = d1[i+3] - d2[i+3];
33 total_cost += t0*t0 + t1*t1 + t2*t2 + t3*t3;
34 if( total_cost > best )
42 naiveNearestNeighbor( const float* vec, int laplacian,
43 const CvSeq* model_keypoints,
44 const CvSeq* model_descriptors )
46 int length = (int)(model_descriptors->elem_size/sizeof(float));
48 double d, dist1 = 1e6, dist2 = 1e6;
49 CvSeqReader reader, kreader;
50 cvStartReadSeq( model_keypoints, &kreader, 0 );
51 cvStartReadSeq( model_descriptors, &reader, 0 );
53 for( i = 0; i < model_descriptors->total; i++ )
55 const CvSURFPoint* kp = (const CvSURFPoint*)kreader.ptr;
56 const float* mvec = (const float*)reader.ptr;
57 CV_NEXT_SEQ_ELEM( kreader.seq->elem_size, kreader );
58 CV_NEXT_SEQ_ELEM( reader.seq->elem_size, reader );
59 if( laplacian != kp->laplacian )
61 d = compareSURFDescriptors( vec, mvec, dist2, length );
71 if ( dist1 < 0.6*dist2 )
77 findPairs( const CvSeq* objectKeypoints, const CvSeq* objectDescriptors,
78 const CvSeq* imageKeypoints, const CvSeq* imageDescriptors, vector<int>& ptpairs )
81 CvSeqReader reader, kreader;
82 cvStartReadSeq( objectKeypoints, &kreader );
83 cvStartReadSeq( objectDescriptors, &reader );
86 for( i = 0; i < objectDescriptors->total; i++ )
88 const CvSURFPoint* kp = (const CvSURFPoint*)kreader.ptr;
89 const float* descriptor = (const float*)reader.ptr;
90 CV_NEXT_SEQ_ELEM( kreader.seq->elem_size, kreader );
91 CV_NEXT_SEQ_ELEM( reader.seq->elem_size, reader );
92 int nearest_neighbor = naiveNearestNeighbor( descriptor, kp->laplacian, imageKeypoints, imageDescriptors );
93 if( nearest_neighbor >= 0 )
96 ptpairs.push_back(nearest_neighbor);
103 flannFindPairs( const CvSeq*,
104 const CvSeq* objectDescriptors,
106 const CvSeq* imageDescriptors,
107 vector<int>& ptpairs )
109 int length = (int)(objectDescriptors->elem_size/sizeof(float));
111 cv::Mat m_object(objectDescriptors->total, length, CV_32F);
112 cv::Mat m_image(imageDescriptors->total, length, CV_32F);
116 CvSeqReader obj_reader;
117 float* obj_ptr = m_object.ptr<float>(0);
118 cvStartReadSeq( objectDescriptors, &obj_reader );
119 for(int i = 0; i < objectDescriptors->total; i++ )
121 const float* descriptor = (const float*)obj_reader.ptr;
122 CV_NEXT_SEQ_ELEM( obj_reader.seq->elem_size, obj_reader );
123 memcpy(obj_ptr, descriptor, length*sizeof(float));
126 CvSeqReader img_reader;
127 float* img_ptr = m_image.ptr<float>(0);
128 cvStartReadSeq( imageDescriptors, &img_reader );
129 for(int i = 0; i < imageDescriptors->total; i++ )
131 const float* descriptor = (const float*)img_reader.ptr;
132 CV_NEXT_SEQ_ELEM( img_reader.seq->elem_size, img_reader );
133 memcpy(img_ptr, descriptor, length*sizeof(float));
137 // find nearest neighbors using FLANN
138 cv::Mat m_indices(objectDescriptors->total, 2, CV_32S);
139 cv::Mat m_dists(objectDescriptors->total, 2, CV_32F);
140 cv::flann::Index flann_index(m_image, cv::flann::KDTreeIndexParams(4)); // using 4 randomized kdtrees
141 flann_index.knnSearch(m_object, m_indices, m_dists, 2, cv::flann::SearchParams(64) ); // maximum number of leafs checked
143 int* indices_ptr = m_indices.ptr<int>(0);
144 float* dists_ptr = m_dists.ptr<float>(0);
145 //printf("flannFindPairs %d m_indices.rows=%d\n", __LINE__, m_indices.rows);
146 for (int i = 0; i < m_indices.rows; ++i)
148 //printf("flannFindPairs %d dists=%f %f\n", __LINE__, dists_ptr[2 * i], 0.6 * dists_ptr[2 * i + 1]);
149 if (dists_ptr[2 * i] < 0.6 * dists_ptr[2 * i + 1])
151 //printf("flannFindPairs %d pairs=%d\n", __LINE__, ptpairs.size());
152 ptpairs.push_back(i);
153 ptpairs.push_back(indices_ptr[2*i]);
159 /* a rough implementation for object location */
161 locatePlanarObject(const CvSeq* objectKeypoints,
162 const CvSeq* objectDescriptors,
163 const CvSeq* imageKeypoints,
164 const CvSeq* imageDescriptors,
165 const CvPoint src_corners[4],
166 CvPoint dst_corners[4],
171 CvMat _h = cvMat(3, 3, CV_64F, h);
173 vector<CvPoint2D32f> pt1, pt2;
181 flannFindPairs( objectKeypoints, objectDescriptors, imageKeypoints, imageDescriptors, ptpairs );
183 findPairs( objectKeypoints, objectDescriptors, imageKeypoints, imageDescriptors, ptpairs );
188 (*point_pairs) = (int*)calloc(ptpairs.size(), sizeof(int));
189 (*total_pairs) = ptpairs.size() / 2;
192 for(int i = 0; i < (int)ptpairs.size(); i++)
194 (*point_pairs)[i] = ptpairs[i];
199 n = (int)(ptpairs.size()/2);
205 for( i = 0; i < n; i++ )
207 pt1[i] = ((CvSURFPoint*)cvGetSeqElem(objectKeypoints,ptpairs[i*2]))->pt;
208 pt2[i] = ((CvSURFPoint*)cvGetSeqElem(imageKeypoints,ptpairs[i*2+1]))->pt;
211 _pt1 = cvMat(1, n, CV_32FC2, &pt1[0] );
212 _pt2 = cvMat(1, n, CV_32FC2, &pt2[0] );
213 if( !cvFindHomography( &_pt1, &_pt2, &_h, CV_RANSAC, 5 ))
216 for( i = 0; i < 4; i++ )
218 double x = src_corners[i].x, y = src_corners[i].y;
219 double Z = 1./(h[6]*x + h[7]*y + h[8]);
220 double X = (h[0]*x + h[1]*y + h[2])*Z;
221 double Y = (h[3]*x + h[4]*y + h[5])*Z;
222 dst_corners[i] = cvPoint(cvRound(X), cvRound(Y));
229 void locate_points(const CvSeq* objectKeypoints,
230 const CvSeq* objectDescriptors,
231 const CvSeq* imageKeypoints,
232 const CvSeq* imageDescriptors,
240 flannFindPairs( objectKeypoints, objectDescriptors, imageKeypoints, imageDescriptors, ptpairs );
242 findPairs( objectKeypoints, objectDescriptors, imageKeypoints, imageDescriptors, ptpairs );
245 (*points) = (int*)calloc(ptpairs.size(), sizeof(int) * 2);
246 (*sizes) = (int*)calloc(ptpairs.size(), sizeof(int));
247 (*total_points) = ptpairs.size();
250 for(int i = 0; i < (int)ptpairs.size(); i += 2 )
252 CvSURFPoint* r1 = (CvSURFPoint*)cvGetSeqElem( objectKeypoints, ptpairs[i] );
253 CvSURFPoint* r2 = (CvSURFPoint*)cvGetSeqElem( imageKeypoints, ptpairs[i+1] );
256 (*points)[i * 2] = r2->pt.x;
257 (*points)[i * 2 + 1] = r2->pt.y;
258 (*sizes)[i] = r2->size;