cinelerra-5.1/plugins/motion/opencvwrapper.C

   1 /*
   2  * CINELERRA
   3  * Copyright (C) 1997-2012 Adam Williams <broadcast at earthling dot net>
   4  *
   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.
   9  *
  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.
  14  *
  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
  18  *
  19  */
  20
  21
  22
  23 #include "opencvwrapper.h"
  24 #include "vframe.h"
  25
  26 #include "opencv2/calib3d/calib3d.hpp"
  27 #include "opencv2/objdetect/objdetect.hpp"
  28 #include "opencv2/features2d/features2d.hpp"
  29
  30
  31 #include <iostream>
  32 #include <vector>
  33 #include <stdio.h>
  34 #include <stdlib.h>
  35
  36
  37
  38
  39 using namespace std;
  40
  41
  42 // define whether to use approximate nearest-neighbor search
  43 #define USE_FLANN
  44
  45 // Sizes must be quantized a certain amount for OpenCV
  46 #define QUANTIZE 8
  47
  48
  49
  50 double
  51 compareSURFDescriptors( const float* d1, const float* d2, double best, int length )
  52 {
  53     double total_cost = 0;
  54     assert( length % 4 == 0 );
  55     for( int i = 0; i < length; i += 4 )
  56     {
  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 )
  63             break;
  64     }
  65     return total_cost;
  66 }
  67
  68
  69 int
  70 naiveNearestNeighbor( const float* vec, int laplacian,
  71                       const CvSeq* model_keypoints,
  72                       const CvSeq* model_descriptors )
  73 {
  74     int length = (int)(model_descriptors->elem_size/sizeof(float));
  75     int i, neighbor = -1;
  76     double d, dist1 = 1e6, dist2 = 1e6;
  77     CvSeqReader reader, kreader;
  78     cvStartReadSeq( model_keypoints, &kreader, 0 );
  79     cvStartReadSeq( model_descriptors, &reader, 0 );
  80
  81     for( i = 0; i < model_descriptors->total; i++ )
  82     {
  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 )
  88             continue;
  89         d = compareSURFDescriptors( vec, mvec, dist2, length );
  90         if( d < dist1 )
  91         {
  92             dist2 = dist1;
  93             dist1 = d;
  94             neighbor = i;
  95         }
  96         else if ( d < dist2 )
  97             dist2 = d;
  98     }
  99     if ( dist1 < 0.6*dist2 )
 100         return neighbor;
 101     return -1;
 102 }
 103
 104 void
 105 findPairs( const CvSeq* objectKeypoints, const CvSeq* objectDescriptors,
 106            const CvSeq* imageKeypoints, const CvSeq* imageDescriptors, vector<int>& ptpairs )
 107 {
 108     int i;
 109     CvSeqReader reader, kreader;
 110     cvStartReadSeq( objectKeypoints, &kreader );
 111     cvStartReadSeq( objectDescriptors, &reader );
 112     ptpairs.clear();
 113
 114     for( i = 0; i < objectDescriptors->total; i++ )
 115     {
 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 )
 122         {
 123             ptpairs.push_back(i);
 124             ptpairs.push_back(nearest_neighbor);
 125         }
 126     }
 127 }
 128
 129
 130 void
 131 flannFindPairs( const CvSeq*,
 132         const CvSeq* objectDescriptors,
 133     const CvSeq*,
 134         const CvSeq* imageDescriptors,
 135         vector<int>& ptpairs )
 136 {
 137         int length = (int)(objectDescriptors->elem_size/sizeof(float));
 138
 139     cv::Mat m_object(objectDescriptors->total, length, CV_32F);
 140         cv::Mat m_image(imageDescriptors->total, length, CV_32F);
 141
 142
 143         // copy descriptors
 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++ )
 148     {
 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));
 152         obj_ptr += length;
 153     }
 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++ )
 158     {
 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));
 162         img_ptr += length;
 163     }
 164
 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
 170
 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)
 175         {
 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])
 178                 {
 179 //printf("flannFindPairs %d pairs=%d\n", __LINE__, ptpairs.size());
 180                 ptpairs.push_back(i);
 181                 ptpairs.push_back(indices_ptr[2*i]);
 182         }
 183     }
 184 }
 185
 186
 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],
 193         int *(*point_pairs),
 194         int (*total_pairs))
 195 {
 196     double h[9];
 197     CvMat _h = cvMat(3, 3, CV_64F, h);
 198     vector<int> ptpairs;
 199     vector<CvPoint2D32f> pt1, pt2;
 200     CvMat _pt1, _pt2;
 201     int i, n;
 202
 203         (*point_pairs) = 0;
 204         (*total_pairs) = 0;
 205
 206 #ifdef USE_FLANN
 207     flannFindPairs( objectKeypoints, objectDescriptors, imageKeypoints, imageDescriptors, ptpairs );
 208 #else
 209     findPairs( objectKeypoints, objectDescriptors, imageKeypoints, imageDescriptors, ptpairs );
 210 #endif
 211
 212
 213 // Store keypoints
 214         (*point_pairs) = (int*)calloc(ptpairs.size(), sizeof(int));
 215         (*total_pairs) = ptpairs.size() / 2;
 216
 217
 218     for(int i = 0; i < (int)ptpairs.size(); i++)
 219     {
 220                 (*point_pairs)[i] = ptpairs[i];
 221     }
 222
 223
 224
 225     n = (int)(ptpairs.size()/2);
 226     if( n < 4 )
 227         return 0;
 228
 229     pt1.resize(n);
 230     pt2.resize(n);
 231     for( i = 0; i < n; i++ )
 232     {
 233         pt1[i] = ((CvSURFPoint*)cvGetSeqElem(objectKeypoints,ptpairs[i*2]))->pt;
 234         pt2[i] = ((CvSURFPoint*)cvGetSeqElem(imageKeypoints,ptpairs[i*2+1]))->pt;
 235     }
 236
 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 ))
 240         return 0;
 241
 242     for( i = 0; i < 4; i++ )
 243     {
 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;
 250     }
 251
 252     return 1;
 253 }
 254
 255
 256
 257
 258 OpenCVWrapper::OpenCVWrapper()
 259 {
 260         object_image = 0;
 261         scene_image = 0;
 262         object_image_w = 0;
 263         object_image_h = 0;
 264         scene_image_w = 0;
 265         scene_image_h = 0;
 266         storage = 0;
 267         object_keypoints = 0;
 268         object_descriptors = 0;
 269         scene_keypoints = 0;
 270         scene_descriptors = 0;
 271         point_pairs = 0;
 272         total_pairs = 0;
 273 }
 274
 275
 276
 277
 278
 279
 280 OpenCVWrapper::~OpenCVWrapper()
 281 {
 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);
 287 }
 288
 289
 290
 291
 292
 293 int OpenCVWrapper::scan(VFrame *object_frame,
 294         VFrame *scene_frame,
 295         int object_x1,
 296         int object_y1,
 297         int object_x2,
 298         int object_y2,
 299         int scene_x1,
 300         int scene_y1,
 301         int scene_x2,
 302         int scene_y2)
 303 {
 304         int result = 0;
 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;
 309
 310
 311 //object_frame->write_png("/tmp/object.png");
 312 //scene_frame->write_png("/tmp/scene.png");
 313
 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);
 323
 324         if(object_image &&
 325                 (object_image_w != this->object_image_w ||
 326                 object_image_h != this->object_image_h))
 327         {
 328                 cvReleaseImage(&object_image);
 329                 object_image = 0;
 330         }
 331         this->object_image_w = object_image_w;
 332         this->object_image_h = object_image_h;
 333
 334         if(scene_image &&
 335                 (scene_image_w != this->scene_image_w ||
 336                 scene_image_h != this->scene_image_h))
 337         {
 338                 cvReleaseImage(&scene_image);
 339                 scene_image = 0;
 340         }
 341         this->scene_image_w = scene_image_w;
 342         this->scene_image_h = scene_image_h;
 343
 344
 345         if(!object_image)
 346         {
 347 // Only does greyscale
 348                 object_image = cvCreateImage(
 349                         cvSize(object_image_w, object_image_h),
 350                         8,
 351                         1);
 352         }
 353
 354         if(!scene_image)
 355         {
 356 // Only does greyscale
 357                 scene_image = cvCreateImage(
 358                         cvSize(scene_image_w, scene_image_h),
 359                         8,
 360                         1);
 361         }
 362
 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 ) );
 367
 368         grey_crop((unsigned char*)scene_image->imageData,
 369                 scene_frame,
 370                 scene_x1,
 371                 scene_y1,
 372                 scene_x2,
 373                 scene_y2,
 374                 scene_image_w,
 375                 scene_image_h);
 376         grey_crop((unsigned char*)object_image->imageData,
 377                 object_frame,
 378                 object_x1,
 379                 object_y1,
 380                 object_x2,
 381                 object_y2,
 382                 object_image_w,
 383                 object_image_h);
 384
 385
 386         if(!storage) storage = cvCreateMemStorage(0);
 387         CvSURFParams params = cvSURFParams(500, 1);
 388
 389
 390 //printf("OpenCVWrapper::process_buffer %d\n", __LINE__);
 391
 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;
 399         scene_keypoints = 0;
 400         scene_descriptors = 0;
 401
 402 // Free the image structures
 403         if(point_pairs) free(point_pairs);
 404         point_pairs = 0;
 405
 406
 407         cvExtractSURF(object_image,
 408                 0,
 409                 &object_keypoints,
 410                 &object_descriptors,
 411                 storage,
 412                 params,
 413                 0);
 414
 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++)
 418 //              {
 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);
 426 //              }
 427
 428
 429 //printf("OpenCVWrapper::process_buffer %d\n", __LINE__);
 430
 431         cvExtractSURF(scene_image,
 432                 0,
 433                 &scene_keypoints,
 434                 &scene_descriptors,
 435                 storage,
 436                 params,
 437                 0);
 438
 439 // Draw the keypoints
 440 //              for(int i = 0; i < scene_keypoints->total; i++)
 441 //              {
 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);
 449 //              }
 450
 451 // printf("OpenCVWrapper::scan %d %d %d scene keypoints=%d\n",
 452 // __LINE__,
 453 // scene_w,
 454 // scene_h,
 455 // scene_keypoints->total);
 456
 457         CvPoint src_corners[4] =
 458         {
 459                 { 0, 0 },
 460                 { object_w, 0 },
 461                 { object_w, object_h },
 462                 { 0, object_h }
 463         };
 464
 465         for(int i = 0; i < 8; i++)
 466         {
 467                 dst_corners[i] = 0;
 468         }
 469
 470
 471
 472 //printf("OpenCVWrapper::process_buffer %d\n", __LINE__);
 473         if(scene_keypoints->total &&
 474                 object_keypoints->total &&
 475                 locatePlanarObject(object_keypoints,
 476                         object_descriptors,
 477                         scene_keypoints,
 478                         scene_descriptors,
 479                         src_corners,
 480                         &point_pairs,
 481                         &total_pairs))
 482         {
 483                 result = 1;
 484         }
 485
 486
 487         return result;
 488 }
 489
 490
 491
 492
 493
 494
 495 // Convert to greyscale & crop
 496 void OpenCVWrapper::grey_crop(unsigned char *dst,
 497         VFrame *src,
 498         int x1,
 499         int y1,
 500         int x2,
 501         int y2,
 502         int dst_w,
 503         int dst_h)
 504 {
 505 // Dimensions of dst frame
 506         int w = x2 - x1;
 507         int h = y2 - y1;
 508
 509         bzero(dst, dst_w * dst_h);
 510
 511 //printf("OpenCVWrapper::grey_crop %d %d %d\n", __LINE__, w, h);
 512         for(int i = 0; i < h; i++)
 513         {
 514                 switch(src->get_color_model())
 515                 {
 516                         case BC_RGB888:
 517                                 break;
 518
 519                         case BC_YUV888:
 520                         {
 521                                 unsigned char *input = src->get_rows()[i + y1] + x1 * 3;
 522                                 unsigned char *output = dst + i * dst_w;
 523
 524                                 for(int j = 0; j < w; j++)
 525                                 {
 526 // Y channel only
 527                                         *output = *input;
 528                                         input += 3;
 529                                         output++;
 530                                 }
 531                                 break;
 532                         }
 533                 }
 534         }
 535 }
 536
 537
 538 float OpenCVWrapper::get_dst_x(int number)
 539 {
 540         return dst_corners[number * 2];
 541 }
 542
 543 float OpenCVWrapper::get_dst_y(int number)
 544 {
 545         return dst_corners[number * 2 + 1];
 546 }
 547
 548
 549
 550
 551