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