git.sesse.net Git - mlt/blob - src/modules/motion_est/filter_motion_est.c

   1 /*
   2  *      /brief fast motion estimation filter
   3  *      /author Zachary Drew, Copyright 2005
   4  *
   5  *      Currently only uses Gamma data for comparisonon (bug or feature?)
   6  *      Vector optimization coming soon.
   7  *
   8  *      Vector orientation: The vector data that is generated for the current frame specifies
   9  *      the motion from the previous frame to the current frame. Thus, to know how a macroblock
  10  *      in the current frame will move in the future, the next frame is needed.
  11  *
  12  * This program is free software; you can redistribute it and/or modify
  13  * it under the terms of the GNU General Public License as published by
  14  * the Free Software Foundation; either version 2 of the License, or
  15  * (at your option) any later version.
  16  *
  17  * This program is distributed in the hope that it will be useful,
  18  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  19  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  20  * GNU General Public License for more details.
  21  *
  22  * You should have received a copy of the GNU General Public License
  23  * along with this program; if not, write to the Free Software Foundation,
  24  * Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
  25  */
  26
  27
  28 #include "filter_motion_est.h"
  29 #include <framework/mlt.h>
  30 #include <stdio.h>
  31 #include <stdlib.h>
  32 #include <math.h>
  33 #include <string.h>
  34 #include <sys/time.h>
  35 #include <assert.h>
  36
  37 #include "sad_sse.h"
  38
  39
  40 #undef DEBUG
  41 #undef DEBUG_ASM
  42 #undef BENCHMARK
  43 #undef COUNT_COMPARES
  44
  45 #define DIAMOND_SEARCH 0x0
  46 #define FULL_SEARCH 0x1
  47 #define SHIFT 8
  48 #define MIN(a,b) ((a) > (b) ? (b) : (a))
  49 #define ABS(a) ((a) >= 0 ? (a) : (-(a)))
  50
  51 #ifdef COUNT_COMPARES
  52         int compares;
  53 #endif
  54
  55 typedef struct motion_vector_s motion_vector;
  56
  57 struct yuv_data
  58 {
  59         uint8_t *y;
  60         uint8_t *u;
  61         uint8_t *v;
  62
  63 };
  64
  65 struct motion_est_context_s
  66 {
  67         int initialized;                                //<! true if filter has been initialized
  68
  69         /* same as mlt_frame's parameters */
  70         int width, height;
  71
  72         /* Operational details */
  73         int macroblock_width, macroblock_height;
  74         int xstride, ystride;
  75         //uint8_t *former_image;                        //<! Copy of the previous frame's image
  76         struct yuv_data former_image, current_image;
  77         int search_method, skip_prediction, shot_change;
  78         int limit_x, limit_y;                   //<! max x and y of a motion vector
  79         int edge_blocks_x, edge_blocks_y;
  80         int initial_thresh;
  81         int check_chroma;                       // if check_chroma == 1 then compare chroma
  82         int denoise;
  83         int previous_msad;
  84
  85         /* bounds */
  86         struct mlt_geometry_item_s prev_bounds; // Cache last frame's bounds (needed for predictor vectors validity)
  87         struct mlt_geometry_item_s *bounds;     // Current bounds
  88
  89         /* bounds in macroblock units */
  90         int left_mb, prev_left_mb, right_mb, prev_right_mb;
  91         int top_mb, prev_top_mb, bottom_mb, prev_bottom_mb;
  92
  93         /* size of our vector buffers */
  94         int mv_buffer_height, mv_buffer_width, mv_size;
  95
  96         /* vector buffers */
  97         int former_vectors_valid;               //<! true if the previous frame's buffered motion vector data is valid
  98         motion_vector *former_vectors, *current_vectors;
  99         motion_vector *denoise_vectors;
 100         mlt_position former_frame_position, current_frame_position;
 101
 102         /* two metrics for diagnostics. lower is a better estimation but beware of local minima  */
 103         float predictive_misses;                // How often do the prediction metion vectors fail?
 104         int comparison_average;                 // How far does the best estimation deviate from a perfect comparison?
 105         int bad_comparisons;
 106         int average_length;
 107         int average_x, average_y;
 108
 109         /* run-time configurable comparison functions */
 110         int (*compare_reference)(uint8_t *, uint8_t *, int, int, int, int);
 111         int (*compare_optimized)(uint8_t *, uint8_t *, int, int, int, int);
 112
 113 };
 114
 115
 116 // Clip the macroblocks as required. Only used for blocks at the edge of the picture
 117 // "from" is assumed to be unclipped
 118 inline static int clip( int *from_x,
 119                         int *from_y,
 120                         int *to_x,
 121                         int *to_y,
 122                         int *w,                 //<! macroblock width
 123                         int *h,                 //<! macroblock height
 124                         int width,              //<! image width
 125                         int height)             //<! image height
 126 {
 127
 128         uint32_t penalty = 1 << SHIFT;  // Retain a few extra bits of precision minus floating-point's blemishes
 129         int diff;
 130
 131         // Origin of macroblock moves left of absolute boundy
 132         if( *to_x < 0 ) {
 133                 if( *to_x + *w <= 0) return 0;                  // Clipped out of existance
 134                 penalty = (*w * penalty) / (*w + *to_x);         // Recipricol of the fraction of the block that remains
 135                 *from_x -= *to_x;
 136                 *w += *to_x;
 137                 *to_x = 0;
 138         }
 139         // Portion of macroblock moves right of absolute boundry
 140         else if( *to_x + *w > width ) {
 141                 if(*to_x >= width) return 0;                    // Clipped out of existance
 142                 diff  = *to_x + *w - width;                     // Width of area clipped (0 < diff <  macroblock width)
 143                 penalty = (*w * penalty) / (*w - diff);         // Recipricol of the fraction of the block that remains
 144                 *w -= diff;
 145         }
 146         // Origin of macroblock moves above absolute boundy
 147         if( *to_y < 0 ) {
 148                 if( *to_y + *h <= 0) return 0;                  // Clipped out of existance
 149                 penalty = (*h * penalty) / (*h + *to_y);        // Recipricol of the fraction of the block that remains
 150                 *from_y -= *to_y;
 151                 *h += *to_y;
 152                 *to_y = 0;
 153         }
 154         // Portion of macroblock moves bellow absolute boundry
 155         else if( *to_y + *h > height ) {
 156                 if(*to_y >= height) return 0;                   // Clipped out of existance
 157                 diff = *to_y + *h - height;                     // Height of area clipped (0 < diff < macroblock height)
 158                 penalty = (*h * penalty) / (*h - diff);         // Recipricol of the fraction of the block that is clipped
 159                 *h -= diff;
 160         }
 161         return penalty;
 162 }
 163
 164
 165 /** /brief Reference Sum of Absolute Differences comparison function
 166 *
 167 */
 168 inline static int sad_reference( uint8_t *block1, uint8_t *block2, int xstride, int ystride, int w, int h )
 169 {
 170         int i, j, score = 0;
 171         for ( j = 0; j < h; j++ ){
 172                 for ( i = 0; i < w; i++ ){
 173                         score += ABS( block1[i*xstride] - block2[i*xstride] );
 174                 }
 175                 block1 += ystride;
 176                 block2 += ystride;
 177         }
 178
 179         return score;
 180 }
 181
 182 inline static void change_422_to_444_planar_rep( uint8_t *image, struct yuv_data yuv, struct motion_est_context_s *c )
 183 {
 184         register uint8_t *p = image;
 185         register uint8_t *q = image + c->width * c->height * 2;
 186         while ( *p != *q ) {
 187                 *(yuv.y++) = *(p ++);
 188                 *(yuv.u++) = *p;
 189                 *(yuv.u++) = *(p ++);
 190                 *(yuv.y++) = *(p ++);
 191                 *(yuv.v++) = *p;
 192                 *(yuv.v++) = *(p ++);
 193         }
 194 }
 195
 196 // broken
 197 inline static void change_420p_to_444_planar_rep( uint8_t *image, struct yuv_data yuv, struct motion_est_context_s *c )
 198 {
 199         uint8_t *p = image + c->width * c->height;
 200         uint8_t *q = p + c->width*c->height/2;
 201         uint8_t *u2, *v2;
 202         while( *p != *q ) {
 203                 u2 = yuv.u + c->width;
 204                 *yuv.u  ++ = *p;
 205                 *yuv.u  ++ = *p;
 206                 *u2 ++ = *p;
 207                 *u2 ++ = *p ++;
 208         }
 209
 210         *q += c->width*c->height/2;
 211         while( *p != *q ) {
 212                 v2 = yuv.v + c->width;
 213                 *yuv.v  ++ = *p;
 214                 *yuv.v  ++ = *p;
 215                 *v2 ++ = *p;
 216                 *v2 ++ = *p ++;
 217         }
 218
 219 }
 220
 221 /** /brief Abstracted block comparison function
 222 */
 223 inline static int compare( uint8_t *from,
 224                            uint8_t *to,
 225                            int from_x,
 226                            int from_y,
 227                            int to_x,
 228                            int to_y,
 229                            struct motion_est_context_s *c)
 230 {
 231 #ifdef COUNT_COMPARES
 232         compares++;
 233 #endif
 234
 235         if( ABS(from_x - to_x) >= c->limit_x || ABS(from_y - to_y) >= c->limit_y )
 236                 return MAX_MSAD;
 237
 238         int score;
 239         int (*cmp)(uint8_t *, uint8_t *, int, int, int, int) = c->compare_optimized;
 240
 241         int mb_w = c->macroblock_width;
 242         int mb_h = c->macroblock_height;
 243
 244         int penalty = clip(&from_x, &from_y, &to_x, &to_y, &mb_w, &mb_h, c->width, c->height);
 245         if ( penalty == 1<<SHIFT)
 246             penalty = clip(&to_x, &to_y, &from_x, &from_y, &mb_w, &mb_h, c->width, c->height);
 247
 248         if( penalty == 0 )                      // Clipped out of existance
 249                 return MAX_MSAD;
 250         else if( penalty != 1<<SHIFT )          // SIMD optimized comparison won't work
 251                 cmp = c->compare_reference;
 252
 253         uint8_t *from_block = from + from_x * c->xstride + from_y * c->ystride;
 254         uint8_t *to_block = to + to_x * c->xstride + to_y * c->ystride;
 255
 256         #ifdef DEBUG_ASM
 257         if( penalty == 1<<SHIFT ){
 258                 score = c->compare_reference( from_block, to_block, c->xstride, c->ystride, mb_w, mb_h );
 259                 int score2 = c->compare_optimized( from_block, to_block, c->xstride, c->ystride, mb_w, mb_h );
 260                 if ( score != score2 )
 261                         fprintf(stderr, "Your assembly doesn't work! Reference: %d Asm: %d\n", score, score2);
 262         }
 263         else
 264         #endif
 265
 266         score = cmp( from_block, to_block, c->xstride, c->ystride, mb_w, mb_h );
 267
 268         return ( score * penalty ) >> SHIFT;                    // The extra precision is no longer wanted
 269 }
 270
 271 static inline void check_candidates (   struct yuv_data *from, struct yuv_data *to,
 272                                         int from_x, int from_y,
 273                                         motion_vector *candidates, int count, int unique,
 274                                         motion_vector *result,
 275                                         struct motion_est_context_s *c )
 276 {
 277                 int score, i, j;
 278                 /* Scan for the best candidate */
 279                 for ( i = 0; i < count; i++ )
 280                 {
 281                         // this little dohicky ignores duplicate candidates, if they are possible
 282                         if ( unique == 0 ) {
 283                                 j = 0;
 284                                 while ( j < i )
 285                                 {
 286                                         if ( candidates[j].dx == candidates[i].dx &&
 287                                              candidates[j].dy == candidates[i].dy )
 288                                                         goto next_for_loop;
 289
 290                                         j++;
 291                                 }
 292                         }
 293
 294                         // Luma
 295                         score = compare( from->y, to->y, from_x, from_y,
 296                                          from_x + candidates[i].dx,     /* to x */
 297                                          from_y + candidates[i].dy,     /* to y */
 298                                          c);
 299
 300                         if ( c->check_chroma ) {
 301                                 if ( score >= result->msad )    // Early term
 302                                         continue;
 303
 304                                 // Chroma - U
 305                                 score += compare( from->u, to->u, from_x, from_y,
 306                                                  from_x + candidates[i].dx,     /* to x */
 307                                                  from_y + candidates[i].dy,     /* to y */
 308                                                  c);
 309
 310                                 if ( score >= result->msad )    // Early term
 311                                         continue;
 312
 313                                 // Chroma - V
 314                                 score += compare( from->v, to->v, from_x, from_y,
 315                                                  from_x + candidates[i].dx,     /* to x */
 316                                                  from_y + candidates[i].dy,     /* to y */
 317                                                  c);
 318                         }
 319
 320                         if ( score < result->msad ) {   // New minimum
 321                                 result->dx = candidates[i].dx;
 322                                 result->dy = candidates[i].dy;
 323                                 result->msad = score;
 324                         }
 325                         next_for_loop:;
 326                 }
 327 }
 328
 329 /* /brief Diamond search
 330 * Operates on a single macroblock
 331 */
 332 static inline void diamond_search(
 333                         struct yuv_data *from,                  //<! Image data from previous frame
 334                         struct yuv_data *to,                    //<! Image data in current frame
 335                         int mb_x,                       //<! X upper left corner of macroblock
 336                         int mb_y,                       //<! U upper left corner of macroblock
 337                         struct motion_vector_s *result, //<! Best predicted mv and eventual result
 338                         struct motion_est_context_s *c) //<! motion estimation context
 339 {
 340
 341         // diamond search pattern
 342         motion_vector candidates[4];
 343
 344         // Keep track of best and former best candidates
 345         motion_vector best, former;
 346
 347         // The direction of the refinement needs to be known
 348         motion_vector current;
 349
 350         int i, first = 1;
 351
 352         // Loop through the search pattern
 353         while( 1 ) {
 354
 355                 current.dx = result->dx;
 356                 current.dy = result->dy;
 357
 358                 if ( first == 1 )       // Set the initial pattern
 359                 {
 360                         candidates[0].dx = result->dx + 1; candidates[0].dy = result->dy + 0;
 361                         candidates[1].dx = result->dx + 0; candidates[1].dy = result->dy + 1;
 362                         candidates[2].dx = result->dx - 1; candidates[2].dy = result->dy + 0;
 363                         candidates[3].dx = result->dx + 0; candidates[3].dy = result->dy - 1;
 364                         i = 4;
 365                 }
 366                 else     // Construct the next portion of the search pattern
 367                 {
 368                         candidates[0].dx = result->dx + best.dx;
 369                         candidates[0].dy = result->dy + best.dy;
 370                         if (best.dx == former.dx && best.dy == former.dy) {
 371                                 candidates[1].dx = result->dx + best.dy;
 372                                 candidates[1].dy = result->dy + best.dx;                //      Yes, the wires
 373                                 candidates[2].dx = result->dx - best.dy;                //      are crossed
 374                                 candidates[2].dy = result->dy - best.dx;
 375                                 i = 3;
 376                         } else {
 377                                 candidates[1].dx = result->dx + former.dx;
 378                                 candidates[1].dy = result->dy + former.dy;
 379                                 i = 2;
 380                         }
 381
 382                         former.dx = best.dx; former.dy = best.dy;       // Keep track of new former best
 383                 }
 384
 385                 check_candidates ( from, to, mb_x, mb_y, candidates, i, 1, result, c );
 386                 best.dx = result->dx - current.dx;
 387                 best.dy = result->dy - current.dy;
 388
 389                 if ( best.dx == 0 && best.dy == 0 )
 390                         return;
 391
 392                 if ( first == 1 ){
 393                         first = 0;
 394                         former.dx = best.dx; former.dy = best.dy; // First iteration, sensible value for former_d*
 395                 }
 396         }
 397 }
 398
 399 /* /brief Full (brute) search
 400 * Operates on a single macroblock
 401 */
 402 static void full_search(
 403                         struct yuv_data *from,                  //<! Image data from previous frame
 404                         struct yuv_data *to,                    //<! Image data in current frame
 405                         int mb_x,                       //<! X upper left corner of macroblock
 406                         int mb_y,                       //<! U upper left corner of macroblock
 407                         struct motion_vector_s *result, //<! Best predicted mv and eventual result
 408                         struct motion_est_context_s *c) //<! motion estimation context
 409 {
 410         // Keep track of best candidate
 411         int i,j,score;
 412
 413         // Go loopy
 414         for( i = -c->macroblock_width; i <= c->macroblock_width; i++ ){
 415                 for( j = -c->macroblock_height; j <=  c->macroblock_height; j++ ){
 416
 417                         score = compare( from->y, to->y,
 418                                          mb_x,          /* from x */
 419                                          mb_y,          /* from y */
 420                                          mb_x + i,      /* to x */
 421                                          mb_y + j,      /* to y */
 422                                          c);            /* context */
 423
 424                         if ( score < result->msad ) {
 425                                 result->dx = i;
 426                                 result->dy = j;
 427                                 result->msad = score;
 428                         }
 429                 }
 430         }
 431 }
 432
 433 // Credits: ffmpeg
 434 // return the median
 435 static inline int median_predictor(int a, int b, int c) {
 436         if ( a > b ){
 437                 if ( c > b ){
 438                         if ( c > a  ) b = a;
 439                         else      b = c;
 440                 }
 441         } else {
 442                 if ( b > c ){
 443                         if ( c > a ) b = c;
 444                         else     b = a;
 445                 }
 446         }
 447         return b;
 448 }
 449
 450 // Macros for pointer calculations
 451 #define CURRENT(i,j)    ( c->current_vectors + (j)*c->mv_buffer_width + (i) )
 452 #define FORMER(i,j)     ( c->former_vectors + (j)*c->mv_buffer_width + (i) )
 453 #define DENOISE(i,j)    ( c->denoise_vectors + (j)*c->mv_buffer_width + (i) )
 454
 455 int ncompare (const void * a, const void * b)
 456 {
 457         return ( *(int*)a - *(int*)b );
 458 }
 459
 460 // motion vector denoising
 461 // for x and y components seperately,
 462 // change the vector to be the median value of the 9 adjacent vectors
 463 static void median_denoise( motion_vector *v, struct motion_est_context_s *c )
 464 {
 465         int xvalues[9], yvalues[9];
 466
 467         int i,j,n;
 468         for( i = c->left_mb; i <= c->right_mb; i++ ){
 469                 for( j = c->top_mb; j <= c->bottom_mb; j++ )
 470                 {
 471                         n = 0;
 472
 473                         xvalues[n  ] = CURRENT(i,j)->dx; // Center
 474                         yvalues[n++] = CURRENT(i,j)->dy;
 475
 476                         if( i > c->left_mb ) // Not in First Column
 477                         {
 478                                 xvalues[n  ] = CURRENT(i-1,j)->dx; // Left
 479                                 yvalues[n++] = CURRENT(i-1,j)->dy;
 480
 481                                 if( j > c->top_mb ) {
 482                                         xvalues[n  ] = CURRENT(i-1,j-1)->dx; // Upper Left
 483                                         yvalues[n++] = CURRENT(i-1,j-1)->dy;
 484                                 }
 485
 486                                 if( j < c->bottom_mb ) {
 487                                         xvalues[n  ] = CURRENT(i-1,j+1)->dx; // Bottom Left
 488                                         yvalues[n++] = CURRENT(i-1,j+1)->dy;
 489                                 }
 490                         }
 491                         if( i < c->right_mb ) // Not in Last Column
 492                         {
 493                                 xvalues[n  ] = CURRENT(i+1,j)->dx; // Right
 494                                 yvalues[n++] = CURRENT(i+1,j)->dy;
 495
 496
 497                                 if( j > c->top_mb ) {
 498                                         xvalues[n  ] = CURRENT(i+1,j-1)->dx; // Upper Right
 499                                         yvalues[n++] = CURRENT(i+1,j-1)->dy;
 500                                 }
 501
 502                                 if( j < c->bottom_mb ) {
 503                                         xvalues[n  ] = CURRENT(i+1,j+1)->dx; // Bottom Right
 504                                         yvalues[n++] = CURRENT(i+1,j+1)->dy;
 505                                 }
 506                         }
 507                         if( j > c->top_mb ) // Not in First Row
 508                         {
 509                                 xvalues[n  ] = CURRENT(i,j-1)->dx; // Top
 510                                 yvalues[n++] = CURRENT(i,j-1)->dy;
 511                         }
 512
 513                         if( j < c->bottom_mb ) // Not in Last Row
 514                         {
 515                                 xvalues[n  ] = CURRENT(i,j+1)->dx; // Bottom
 516                                 yvalues[n++] = CURRENT(i,j+1)->dy;
 517                         }
 518
 519                         qsort (xvalues, n, sizeof(int), ncompare);
 520                         qsort (yvalues, n, sizeof(int), ncompare);
 521
 522                         if( n % 2 == 1 ) {
 523                                 DENOISE(i,j)->dx = xvalues[n/2];
 524                                 DENOISE(i,j)->dy = yvalues[n/2];
 525                         }
 526                         else {
 527                                 DENOISE(i,j)->dx = (xvalues[n/2] + xvalues[n/2+1])/2;
 528                                 DENOISE(i,j)->dy = (yvalues[n/2] + yvalues[n/2+1])/2;
 529                         }
 530                 }
 531         }
 532
 533         motion_vector *t = c->current_vectors;
 534         c->current_vectors = c->denoise_vectors;
 535         c->denoise_vectors = t;
 536
 537 }
 538
 539 /** /brief Motion search
 540 *
 541 *
 542 * Search for the Vector that best represents the motion *from the last frame *to the current frame
 543 * Vocab: Colocated - the pixel in the previous frame at the current position
 544 *
 545 * Based on enhanced predictive zonal search. [Tourapis 2002]
 546 */
 547 static void search( struct yuv_data from,                       //<! Image data. Motion vector source in previous frame
 548                     struct yuv_data to,                 //<! Image data. Motion vector destination current
 549                     struct motion_est_context_s *c)     //<! The context
 550 {
 551
 552 #ifdef COUNT_COMPARES
 553         compares = 0;
 554 #endif
 555
 556         motion_vector candidates[10];
 557         motion_vector *here;            // This one gets used alot (about 30 times per macroblock)
 558         int n = 0;
 559
 560         int i, j, count=0;
 561
 562         // For every macroblock, perform motion vector estimation
 563         for( i = c->left_mb; i <= c->right_mb; i++ ){
 564          for( j = c->top_mb; j <= c->bottom_mb; j++ ){
 565
 566                 here = CURRENT(i,j);
 567                 here->valid = 1;
 568                 here->color = 100;
 569                 here->msad = MAX_MSAD;
 570                 count++;
 571                 n = 0;
 572
 573
 574                 /* Stack the predictors [i.e. checked in reverse order] */
 575
 576                 /* Adjacent to collocated */
 577                 if( c->former_vectors_valid )
 578                 {
 579                         // Top of colocated
 580                         if( j > c->prev_top_mb ){// && COL_TOP->valid ){
 581                                 candidates[n  ].dx = FORMER(i,j-1)->dx;
 582                                 candidates[n++].dy = FORMER(i,j-1)->dy;
 583                         }
 584
 585                         // Left of colocated
 586                         if( i > c->prev_left_mb ){// && COL_LEFT->valid ){
 587                                 candidates[n  ].dx = FORMER(i-1,j)->dx;
 588                                 candidates[n++].dy = FORMER(i-1,j)->dy;
 589                         }
 590
 591                         // Right of colocated
 592                         if( i < c->prev_right_mb ){// && COL_RIGHT->valid ){
 593                                 candidates[n  ].dx = FORMER(i+1,j)->dx;
 594                                 candidates[n++].dy = FORMER(i+1,j)->dy;
 595                         }
 596
 597                         // Bottom of colocated
 598                         if( j < c->prev_bottom_mb ){// && COL_BOTTOM->valid ){
 599                                 candidates[n  ].dx = FORMER(i,j+1)->dx;
 600                                 candidates[n++].dy = FORMER(i,j+1)->dy;
 601                         }
 602
 603                         // And finally, colocated
 604                         candidates[n  ].dx = FORMER(i,j)->dx;
 605                         candidates[n++].dy = FORMER(i,j)->dy;
 606                 }
 607
 608                 // For macroblocks not in the top row
 609                 if ( j > c->top_mb) {
 610
 611                         // Top if ( TOP->valid ) {
 612                                 candidates[n  ].dx = CURRENT(i,j-1)->dx;
 613                                 candidates[n++].dy = CURRENT(i,j-1)->dy;
 614                         //}
 615
 616                         // Top-Right, macroblocks not in the right row
 617                         if ( i < c->right_mb ){// && TOP_RIGHT->valid ) {
 618                                 candidates[n  ].dx = CURRENT(i+1,j-1)->dx;
 619                                 candidates[n++].dy = CURRENT(i+1,j-1)->dy;
 620                         }
 621                 }
 622
 623                 // Left, Macroblocks not in the left column
 624                 if ( i > c->left_mb ){// && LEFT->valid ) {
 625                         candidates[n  ].dx = CURRENT(i-1,j)->dx;
 626                         candidates[n++].dy = CURRENT(i-1,j)->dy;
 627                 }
 628
 629                 /* Median predictor vector (median of left, top, and top right adjacent vectors) */
 630                 if ( i > c->left_mb && j > c->top_mb && i < c->right_mb
 631                          )//&& LEFT->valid && TOP->valid && TOP_RIGHT->valid )
 632                 {
 633                         candidates[n  ].dx = median_predictor( CURRENT(i-1,j)->dx, CURRENT(i,j-1)->dx, CURRENT(i+1,j-1)->dx);
 634                         candidates[n++].dy = median_predictor( CURRENT(i-1,j)->dy, CURRENT(i,j-1)->dy, CURRENT(i+1,j-1)->dy);
 635                 }
 636
 637                 // Zero vector
 638                 candidates[n  ].dx = 0;
 639                 candidates[n++].dy = 0;
 640
 641                 int from_x = i * c->macroblock_width;
 642                 int from_y = j * c->macroblock_height;
 643                 check_candidates ( &from, &to, from_x, from_y, candidates, n, 0, here, c );
 644
 645
 646 #ifndef FULLSEARCH
 647                 diamond_search( &from, &to, from_x, from_y, here, c);
 648 #else
 649                 full_search( from, to, from_x, from_y, here, c);
 650 #endif
 651
 652          } /* End column loop */
 653         } /* End row loop */
 654
 655         asm volatile ( "emms" );
 656
 657 #ifdef COUNT_COMPARES
 658         fprintf(stderr, "%d comparisons per block were made", compares/count);
 659 #endif
 660         return;
 661 }
 662
 663 void collect_post_statistics( struct motion_est_context_s *c ) {
 664
 665         c->comparison_average = 0;
 666         c->average_length = 0;
 667         c->average_x = 0;
 668         c->average_y = 0;
 669
 670         int i, j, count = 0;
 671
 672         for ( i = c->left_mb; i <= c->right_mb; i++ ){
 673          for ( j = c->top_mb; j <= c->bottom_mb; j++ ){
 674
 675                 count++;
 676                 c->comparison_average += CURRENT(i,j)->msad;
 677                 c->average_x += CURRENT(i,j)->dx;
 678                 c->average_y += CURRENT(i,j)->dy;
 679
 680
 681          }
 682         }
 683
 684         if ( count > 0 )
 685         {
 686                 c->comparison_average /= count;
 687                 c->average_x /= count;
 688                 c->average_y /= count;
 689                 c->average_length = sqrt( c->average_x * c->average_x + c->average_y * c->average_y );
 690         }
 691
 692 }
 693
 694 static void init_optimizations( struct motion_est_context_s *c )
 695 {
 696         if ( c->check_chroma ) {
 697                 switch(c->macroblock_width){
 698                         case 8:  if(c->macroblock_height == 8)  c->compare_optimized = sad_sse_8x8;
 699                                  else                           c->compare_optimized = sad_sse_8w;
 700                                  break;
 701                         case 16: if(c->macroblock_height == 16) c->compare_optimized = sad_sse_16x16;
 702                                  else                           c->compare_optimized = sad_sse_16w;
 703                                  break;
 704                         case 32: if(c->macroblock_height == 32) c->compare_optimized = sad_sse_32x32;
 705                                  else                           c->compare_optimized = sad_sse_32w;
 706                                  break;
 707                         case 64: c->compare_optimized = sad_sse_64w;
 708                                  break;
 709                         default: c->compare_optimized = sad_reference;
 710                                  break;
 711                 }
 712         }
 713         else
 714         {
 715                 switch(c->macroblock_width){
 716                         case 4:  if(c->macroblock_height == 4)  c->compare_optimized = sad_sse_422_luma_4x4;
 717                                  else                           c->compare_optimized = sad_sse_422_luma_4w;
 718                                  break;
 719                         case 8:  if(c->macroblock_height == 8)  c->compare_optimized = sad_sse_422_luma_8x8;
 720                                  else                           c->compare_optimized = sad_sse_422_luma_8w;
 721                                  break;
 722                         case 16: if(c->macroblock_height == 16) c->compare_optimized = sad_sse_422_luma_16x16;
 723                                  else                           c->compare_optimized = sad_sse_422_luma_16w;
 724                                  break;
 725                         case 32: if(c->macroblock_height == 32) c->compare_optimized = sad_sse_422_luma_32x32;
 726                                  else                           c->compare_optimized = sad_sse_422_luma_32w;
 727                                  break;
 728                         case 64: c->compare_optimized = sad_sse_422_luma_64w;
 729                                  break;
 730                         default: c->compare_optimized = sad_reference;
 731                                  break;
 732                 }
 733         }
 734 }
 735
 736 // Image stack(able) method
 737 static int filter_get_image( mlt_frame frame, uint8_t **image, mlt_image_format *format, int *width, int *height, int writable )
 738 {
 739         // Get the filter
 740         mlt_filter filter = mlt_frame_pop_service( frame );
 741
 742         // Get the motion_est context object
 743         struct motion_est_context_s *c = mlt_properties_get_data( MLT_FILTER_PROPERTIES( filter ), "context", NULL);
 744
 745                 #ifdef BENCHMARK
 746                 struct timeval start; gettimeofday(&start, NULL );
 747                 #endif
 748
 749         // Get the new image and frame number
 750         int error = mlt_frame_get_image( frame, image, format, width, height, 1 );
 751
 752                 #ifdef BENCHMARK
 753                 struct timeval finish; gettimeofday(&finish, NULL ); int difference = (finish.tv_sec - start.tv_sec) * 1000000 + (finish.tv_usec - start.tv_usec);
 754                 fprintf(stderr, " in frame %d:%d usec\n", c->current_frame_position, difference);
 755                 #endif
 756
 757         if( error != 0 )
 758                 mlt_properties_debug( MLT_FRAME_PROPERTIES(frame), "error after mlt_frame_get_image() in motion_est", stderr );
 759
 760         c->current_frame_position = mlt_frame_get_position( frame );
 761
 762         /* Context Initialization */
 763         if ( c->initialized == 0 ) {
 764
 765                 // Get the filter properties object
 766                 mlt_properties properties = mlt_filter_properties( filter );
 767
 768                 c->width = *width;
 769                 c->height = *height;
 770
 771                 /* Get parameters that may have been overridden */
 772                 if( mlt_properties_get( properties, "macroblock_width") != NULL )
 773                         c->macroblock_width = mlt_properties_get_int( properties, "macroblock_width");
 774
 775                 if( mlt_properties_get( properties, "macroblock_height") != NULL )
 776                         c->macroblock_height = mlt_properties_get_int( properties, "macroblock_height");
 777
 778                 if( mlt_properties_get( properties, "prediction_thresh") != NULL )
 779                         c->initial_thresh = mlt_properties_get_int( properties, "prediction_thresh" );
 780                 else
 781                         c->initial_thresh = c->macroblock_width * c->macroblock_height;
 782
 783                 if( mlt_properties_get( properties, "search_method") != NULL )
 784                         c->search_method = mlt_properties_get_int( properties, "search_method");
 785
 786                 if( mlt_properties_get( properties, "skip_prediction") != NULL )
 787                         c->skip_prediction = mlt_properties_get_int( properties, "skip_prediction");
 788
 789                 if( mlt_properties_get( properties, "limit_x") != NULL )
 790                         c->limit_x = mlt_properties_get_int( properties, "limit_x");
 791
 792                 if( mlt_properties_get( properties, "limit_y") != NULL )
 793                         c->limit_y = mlt_properties_get_int( properties, "limit_y");
 794
 795                 if( mlt_properties_get( properties, "check_chroma" ) != NULL )
 796                         c->check_chroma = mlt_properties_get_int( properties, "check_chroma" );
 797
 798                 if( mlt_properties_get( properties, "denoise" ) != NULL )
 799                         c->denoise = mlt_properties_get_int( properties, "denoise" );
 800
 801                 init_optimizations( c );
 802
 803                 // Calculate the dimensions in macroblock units
 804                 c->mv_buffer_width = (*width / c->macroblock_width);
 805                 c->mv_buffer_height = (*height / c->macroblock_height);
 806
 807                 // Size of the motion vector buffer
 808                 c->mv_size =  c->mv_buffer_width * c->mv_buffer_height * sizeof(struct motion_vector_s);
 809
 810                 // Allocate the motion vector buffers
 811                 c->former_vectors = mlt_pool_alloc( c->mv_size );
 812                 c->current_vectors = mlt_pool_alloc( c->mv_size );
 813                 c->denoise_vectors = mlt_pool_alloc( c->mv_size );
 814
 815                 // Register motion buffers for destruction
 816                 mlt_properties_set_data( properties, "current_motion_vectors", (void *)c->current_vectors, 0, mlt_pool_release, NULL );
 817                 mlt_properties_set_data( properties, "former_motion_vectors", (void *)c->former_vectors, 0, mlt_pool_release, NULL );
 818                 mlt_properties_set_data( properties, "denoise_motion_vectors", (void *)c->denoise_vectors, 0, mlt_pool_release, NULL );
 819
 820
 821                 c->former_vectors_valid = 0;
 822                 memset( c->former_vectors, 0, c->mv_size );
 823
 824                 // Figure out how many blocks should be considered edge blocks
 825                 c->edge_blocks_x = (c->limit_x + c->macroblock_width - 1) / c->macroblock_width;
 826                 c->edge_blocks_y = (c->limit_y + c->macroblock_height - 1) / c->macroblock_height;
 827
 828                 // Calculate the size of our steps (the number of bytes that seperate adjacent pixels in X and Y direction)
 829                 switch( *format ) {
 830                         case mlt_image_yuv422:
 831                                 if ( c->check_chroma )
 832                                         c->xstride = 1;
 833                                 else
 834                                         c->xstride = 2;
 835                                 c->ystride = c->xstride * *width;
 836                                 break;
 837 /*                      case mlt_image_yuv420p:
 838                                 c->xstride = 1;
 839                                 c->ystride = c->xstride * *width;
 840                                 break;
 841 */                      default:
 842                                 // I don't know
 843                                 fprintf(stderr, "\"I am unfamiliar with your new fangled pixel format!\" -filter_motion_est\n");
 844                                 return -1;
 845                 }
 846
 847                 if ( c->check_chroma ) {
 848                         // Allocate memory for the 444 images
 849                         c->former_image.y = mlt_pool_alloc( *width * *height * 3 );
 850                         c->current_image.y = mlt_pool_alloc( *width * *height * 3 );
 851                         c->current_image.u = c->current_image.y + *width * *height;
 852                         c->current_image.v = c->current_image.u + *width * *height;
 853                         c->former_image.u = c->former_image.y + *width * *height;
 854                         c->former_image.v = c->former_image.u + *width * *height;
 855                         // Register for destruction
 856                         mlt_properties_set_data( properties, "current_image", (void *)c->current_image.y, 0, mlt_pool_release, NULL );
 857                 }
 858                 else
 859                 {
 860                         c->former_image.y = mlt_pool_alloc( *width * *height * 2 );
 861                 }
 862                 // Register for destruction
 863                 mlt_properties_set_data( properties, "former_image", (void *)c->former_image.y, 0, mlt_pool_release, NULL );
 864
 865
 866                 c->former_frame_position = c->current_frame_position;
 867                 c->previous_msad = 0;
 868
 869                 c->initialized = 1;
 870         }
 871
 872         /* Check to see if somebody else has given us bounds */
 873         c->bounds = mlt_properties_get_data( MLT_FRAME_PROPERTIES( frame ), "bounds", NULL );
 874
 875         /* no bounds were given, they won't change next frame, so use a convient storage place */
 876         if( c->bounds == NULL ) {
 877                 c->bounds = &c->prev_bounds;
 878                 c->bounds->x = 0;
 879                 c->bounds->y = 0;
 880                 c->bounds->w = *width - 1;      // Zero indexed
 881                 c->bounds->h = *height - 1;     // Zero indexed
 882         }
 883
 884         // translate pixel units (from bounds) to macroblock units
 885         // make sure whole macroblock stays within bounds
 886         c->left_mb = ( c->bounds->x + c->macroblock_width - 1 ) / c->macroblock_width;
 887         c->top_mb = ( c->bounds->y + c->macroblock_height - 1 ) / c->macroblock_height;
 888         c->right_mb = ( c->bounds->x + c->bounds->w ) / c->macroblock_width - 1;
 889         c->bottom_mb = ( c->bounds->y + c->bounds->h ) / c->macroblock_height - 1;
 890
 891         // Do the same thing for the previous frame's geometry
 892         // This will be used for determining validity of predictors
 893         c->prev_left_mb = ( c->prev_bounds.x + c->macroblock_width - 1) / c->macroblock_width;
 894         c->prev_top_mb = ( c->prev_bounds.y + c->macroblock_height - 1) / c->macroblock_height;
 895         c->prev_right_mb = ( c->prev_bounds.x + c->prev_bounds.w ) / c->macroblock_width - 1;
 896         c->prev_bottom_mb = ( c->prev_bounds.y + c->prev_bounds.h ) / c->macroblock_height - 1;
 897
 898
 899         // If video is advancing, run motion vector algorithm and etc...
 900         if( c->former_frame_position + 1 == c->current_frame_position )
 901         {
 902
 903                 // Swap the motion vector buffers and reuse allocated memory
 904                 struct motion_vector_s *temp = c->current_vectors;
 905                 c->current_vectors = c->former_vectors;
 906                 c->former_vectors = temp;
 907
 908                 // Swap the image buffers
 909                 if ( c->check_chroma ) {
 910                         uint8_t *temp_yuv;
 911                         temp_yuv = c->current_image.y;
 912                         c->current_image.y = c->former_image.y;
 913                         c->former_image.y = temp_yuv;
 914                         temp_yuv = c->current_image.u;
 915                         c->current_image.u = c->former_image.u;
 916                         c->former_image.u = temp_yuv;
 917                         temp_yuv = c->current_image.v;
 918                         c->current_image.v = c->former_image.v;
 919                         c->former_image.v = temp_yuv;
 920
 921                         switch ( *format ) {
 922                                 case mlt_image_yuv422:
 923                                         change_422_to_444_planar_rep( *image, c->current_image, c );
 924                                         break;
 925                                 case mlt_image_yuv420p:
 926                                         change_420p_to_444_planar_rep( *image, c->current_image, c );
 927                                         break;
 928                                 default:
 929                                         break;
 930                         }
 931                 }
 932                 else
 933                         c->current_image.y = *image;
 934
 935                 // Find a better place for this
 936                 memset( c->current_vectors, 0, c->mv_size );
 937
 938                 // Perform the motion search
 939
 940                 //collect_pre_statistics( context, *image );
 941                 search( c->current_image, c->former_image, c );
 942
 943                 collect_post_statistics( c );
 944
 945
 946
 947
 948                 // Detect shot changes
 949                 if( c->comparison_average > 10 * c->macroblock_width * c->macroblock_height &&
 950                     c->comparison_average > c->previous_msad * 2 )
 951                 {
 952                         fprintf(stderr, " - SAD: %d   <<Shot change>>\n", c->comparison_average);
 953                         mlt_properties_set_int( MLT_FRAME_PROPERTIES( frame ), "shot_change", 1);
 954                 //      c->former_vectors_valid = 0; // Invalidate the previous frame's predictors
 955                         c->shot_change = 1;
 956                 }
 957                 else {
 958                         c->former_vectors_valid = 1;
 959                         c->shot_change = 0;
 960                         //fprintf(stderr, " - SAD: %d\n", c->comparison_average);
 961                 }
 962
 963                 c->previous_msad = c->comparison_average;
 964
 965                 if( c->comparison_average != 0 ) {
 966
 967                         // denoise the vector buffer
 968                         if( c->denoise )
 969                                 median_denoise( c->current_vectors, c );
 970
 971                         // Pass the new vector data into the frame
 972                         mlt_properties_set_data( MLT_FRAME_PROPERTIES( frame ), "motion_est.vectors",
 973                                          (void*)c->current_vectors, c->mv_size, NULL, NULL );
 974
 975                 }
 976                 else {
 977                         // This fixes the ugliness caused by a duplicate frame
 978                         temp = c->current_vectors;
 979                         c->current_vectors = c->former_vectors;
 980                         c->former_vectors = temp;
 981                         mlt_properties_set_data( MLT_FRAME_PROPERTIES( frame ), "motion_est.vectors",
 982                                          (void*)c->former_vectors, c->mv_size, NULL, NULL );
 983                 }
 984
 985         }
 986         // paused
 987         else if( c->former_frame_position == c->current_frame_position )
 988         {
 989                 // Pass the old vector data into the frame if it's valid
 990                 if( c->former_vectors_valid == 1 )
 991                         mlt_properties_set_data( MLT_FRAME_PROPERTIES( frame ), "motion_est.vectors",
 992                                          (void*)c->current_vectors, c->mv_size, NULL, NULL );
 993
 994                 mlt_properties_set_int( MLT_FRAME_PROPERTIES( frame ), "shot_change", c->shot_change);
 995         }
 996         // there was jump in frame number
 997         else
 998                 c->former_vectors_valid = 0;
 999
1000
1001         // Cache our bounding geometry for the next frame's processing
1002         if( c->bounds != &c->prev_bounds )
1003                 memcpy( &c->prev_bounds, c->bounds, sizeof( struct mlt_geometry_item_s ) );
1004
1005         // Remember which frame this is
1006         c->former_frame_position = c->current_frame_position;
1007
1008
1009         if ( c->check_chroma == 0 )
1010                 memcpy( c->former_image.y, *image, *width * *height * c->xstride );
1011
1012         mlt_properties_set_int( MLT_FRAME_PROPERTIES( frame ), "motion_est.macroblock_width", c->macroblock_width );
1013         mlt_properties_set_int( MLT_FRAME_PROPERTIES( frame ), "motion_est.macroblock_height", c->macroblock_height );
1014
1015         return error;
1016 }
1017
1018
1019
1020 /** filter processing.
1021 */
1022
1023 static mlt_frame filter_process( mlt_filter this, mlt_frame frame )
1024 {
1025
1026         // Keeps tabs on the filter object
1027         mlt_frame_push_service( frame, this);
1028
1029         // Push the frame filter
1030         mlt_frame_push_get_image( frame, filter_get_image );
1031
1032         return frame;
1033 }
1034
1035 /** Constructor for the filter.
1036 */
1037 mlt_filter filter_motion_est_init( char *arg )
1038 {
1039         mlt_filter this = mlt_filter_new( );
1040         if ( this != NULL )
1041         {
1042                 // Get the properties object
1043                 mlt_properties properties = MLT_FILTER_PROPERTIES( this );
1044
1045                 // Initialize the motion estimation context
1046                 struct motion_est_context_s *context;
1047                 context = mlt_pool_alloc( sizeof(struct motion_est_context_s) );
1048                 mlt_properties_set_data( properties, "context", (void *)context, sizeof( struct motion_est_context_s ),
1049                                         mlt_pool_release, NULL );
1050
1051
1052                 // Register the filter
1053                 this->process = filter_process;
1054
1055                 /* defaults that may be overridden */
1056                 context->macroblock_width = 16;
1057                 context->macroblock_height = 16;
1058                 context->skip_prediction = 0;
1059                 context->limit_x = 64;
1060                 context->limit_y = 64;
1061                 context->search_method = DIAMOND_SEARCH;
1062                 context->check_chroma = 0;
1063                 context->denoise = 1;
1064
1065                 /* reference functions that may have optimized versions */
1066                 context->compare_reference = sad_reference;
1067                 //context->vert_deviation_reference = vertical_gradient_reference;
1068                 //context->horiz_deviation_reference = horizontal_gradient_reference;
1069
1070                 // The rest of the buffers will be initialized when the filter is first processed
1071                 context->initialized = 0;
1072         }
1073         return this;
1074 }
1075
1076 /** This source code will self destruct in 5...4...3... */