update cin.po, goog xlat update xx.po
[goodguy/history.git] / cinelerra-5.1 / plugins / findobject / surfscan.C
1 #include "surfscan.h"
2 #include "opencv2/calib3d/calib3d.hpp"
3 #include "opencv2/objdetect/objdetect.hpp"
4 #include "opencv2/features2d/features2d.hpp"
5
6
7 #include <iostream>
8 #include <vector>
9 #include <stdio.h>
10 #include <stdlib.h>
11
12
13
14
15 using namespace std;
16
17
18 // define whether to use approximate nearest-neighbor search
19 #define USE_FLANN
20
21
22 double
23 compareSURFDescriptors( const float* d1, const float* d2, double best, int length )
24 {
25     double total_cost = 0;
26     assert( length % 4 == 0 );
27     for( int i = 0; i < length; i += 4 )
28     {
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 )
35             break;
36     }
37     return total_cost;
38 }
39
40
41 int
42 naiveNearestNeighbor( const float* vec, int laplacian,
43                       const CvSeq* model_keypoints,
44                       const CvSeq* model_descriptors )
45 {
46     int length = (int)(model_descriptors->elem_size/sizeof(float));
47     int i, neighbor = -1;
48     double d, dist1 = 1e6, dist2 = 1e6;
49     CvSeqReader reader, kreader;
50     cvStartReadSeq( model_keypoints, &kreader, 0 );
51     cvStartReadSeq( model_descriptors, &reader, 0 );
52
53     for( i = 0; i < model_descriptors->total; i++ )
54     {
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 )
60             continue;
61         d = compareSURFDescriptors( vec, mvec, dist2, length );
62         if( d < dist1 )
63         {
64             dist2 = dist1;
65             dist1 = d;
66             neighbor = i;
67         }
68         else if ( d < dist2 )
69             dist2 = d;
70     }
71     if ( dist1 < 0.6*dist2 )
72         return neighbor;
73     return -1;
74 }
75
76 void
77 findPairs( const CvSeq* objectKeypoints, const CvSeq* objectDescriptors,
78            const CvSeq* imageKeypoints, const CvSeq* imageDescriptors, vector<int>& ptpairs )
79 {
80     int i;
81     CvSeqReader reader, kreader;
82     cvStartReadSeq( objectKeypoints, &kreader );
83     cvStartReadSeq( objectDescriptors, &reader );
84     ptpairs.clear();
85
86     for( i = 0; i < objectDescriptors->total; i++ )
87     {
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 )
94         {
95             ptpairs.push_back(i);
96             ptpairs.push_back(nearest_neighbor);
97         }
98     }
99 }
100
101
102 void
103 flannFindPairs( const CvSeq*, 
104         const CvSeq* objectDescriptors,
105     const CvSeq*, 
106         const CvSeq* imageDescriptors, 
107         vector<int>& ptpairs )
108 {
109         int length = (int)(objectDescriptors->elem_size/sizeof(float));
110
111     cv::Mat m_object(objectDescriptors->total, length, CV_32F);
112         cv::Mat m_image(imageDescriptors->total, length, CV_32F);
113
114
115         // copy descriptors
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++ )
120     {
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));
124         obj_ptr += length;
125     }
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++ )
130     {
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));
134         img_ptr += length;
135     }
136
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
142
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) 
147         {
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]) 
150                 {
151 //printf("flannFindPairs %d pairs=%d\n", __LINE__, ptpairs.size());
152                 ptpairs.push_back(i);
153                 ptpairs.push_back(indices_ptr[2*i]);
154         }
155     }
156 }
157
158
159 /* a rough implementation for object location */
160 int
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],
167         int *(*point_pairs),
168         int (*total_pairs))
169 {
170     double h[9];
171     CvMat _h = cvMat(3, 3, CV_64F, h);
172     vector<int> ptpairs;
173     vector<CvPoint2D32f> pt1, pt2;
174     CvMat _pt1, _pt2;
175     int i, n;
176         
177         (*point_pairs) = 0;
178         (*total_pairs) = 0;
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
187 // Store keypoints
188         (*point_pairs) = (int*)calloc(ptpairs.size(), sizeof(int));
189         (*total_pairs) = ptpairs.size() / 2;
190         
191         
192     for(int i = 0; i < (int)ptpairs.size(); i++)
193     {
194                 (*point_pairs)[i] = ptpairs[i];
195     }
196
197
198
199     n = (int)(ptpairs.size()/2);
200     if( n < 4 )
201         return 0;
202
203     pt1.resize(n);
204     pt2.resize(n);
205     for( i = 0; i < n; i++ )
206     {
207         pt1[i] = ((CvSURFPoint*)cvGetSeqElem(objectKeypoints,ptpairs[i*2]))->pt;
208         pt2[i] = ((CvSURFPoint*)cvGetSeqElem(imageKeypoints,ptpairs[i*2+1]))->pt;
209     }
210
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 ))
214         return 0;
215
216     for( i = 0; i < 4; i++ )
217     {
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));
223     }
224
225     return 1;
226 }
227
228
229 void locate_points(const CvSeq* objectKeypoints, 
230         const CvSeq* objectDescriptors,
231     const CvSeq* imageKeypoints, 
232         const CvSeq* imageDescriptors,
233         int *(*points),
234         int *(*sizes),
235         int (*total_points))
236 {
237         vector<int> ptpairs;
238         
239 #ifdef USE_FLANN
240     flannFindPairs( objectKeypoints, objectDescriptors, imageKeypoints, imageDescriptors, ptpairs );
241 #else
242     findPairs( objectKeypoints, objectDescriptors, imageKeypoints, imageDescriptors, ptpairs );
243 #endif
244
245         (*points) = (int*)calloc(ptpairs.size(), sizeof(int) * 2);
246         (*sizes) = (int*)calloc(ptpairs.size(), sizeof(int));
247         (*total_points) = ptpairs.size();
248         
249         
250     for(int i = 0; i < (int)ptpairs.size(); i += 2 )
251     {
252         CvSURFPoint* r1 = (CvSURFPoint*)cvGetSeqElem( objectKeypoints, ptpairs[i] );
253         CvSURFPoint* r2 = (CvSURFPoint*)cvGetSeqElem( imageKeypoints, ptpairs[i+1] );
254         
255                 
256                 (*points)[i * 2] = r2->pt.x;
257                 (*points)[i * 2 + 1] = r2->pt.y;
258                 (*sizes)[i] = r2->size;
259     }
260 }
261
262
263
264