git.sesse.net Git - mlt/blob - src/modules/videostab/stabilize.c

   1 /*
   2  *  filter_stabilize.c
   3  *
   4  *  Copyright (C) Georg Martius - June 2007
   5  *   georg dot martius at web dot de
   6  *  mlt adaption by Marco Gittler marco at gitma dot de 2011
   7  *
   8  *  This file is part of transcode, a video stream processing tool
   9  *
  10  *  transcode is free software; you can redistribute it and/or modify
  11  *  it under the terms of the GNU General Public License as published by
  12  *  the Free Software Foundation; either version 2, or (at your option)
  13  *  any later version.
  14  *
  15  *  transcode is distributed in the hope that it will be useful,
  16  *  but WITHOUT ANY WARRANTY; without even the implied warranty of
  17  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  18  *  GNU General Public License for more details.
  19  *
  20  *  You should have received a copy of the GNU General Public License
  21  *  along with GNU Make; see the file COPYING.  If not, write to
  22  *  the Free Software Foundation, 675 Mass Ave, Cambridge, MA 02139, USA.
  23  *
  24  */
  25
  26 /* Typical call:
  27  *  transcode -V -J stabilize=shakiness=5:show=1,preview
  28  *         -i inp.mpeg -y null,null -o dummy
  29  *  all parameters are optional
  30 */
  31
  32 #define MOD_NAME    "filter_stabilize.so"
  33 #define MOD_VERSION "v0.75 (2010-04-07)"
  34 #define MOD_CAP     "extracts relative transformations of \n\
  35     subsequent frames (used for stabilization together with the\n\
  36     transform filter in a second pass)"
  37 #define MOD_AUTHOR  "Georg Martius"
  38
  39 /* Ideas:
  40  - Try OpenCL/Cuda, this should work great
  41  - use smoothing on the frames and then use gradient decent!
  42  - stepsize could be adapted (maybe to check only one field with large
  43    stepsize and use the maximally required for the other fields
  44 */
  45
  46 #define MOD_FEATURES \
  47     TC_MODULE_FEATURE_FILTER|TC_MODULE_FEATURE_VIDEO
  48 #define MOD_FLAGS  \
  49     TC_MODULE_FLAG_RECONFIGURABLE | TC_MODULE_FLAG_DELAY
  50 #define MAX(a,b)         ((a < b) ?  (b) : (a))
  51 #define MIN(a,b)         ((a < b) ?  (a) : (b))
  52 #include "stabilize.h"
  53 #include <stdlib.h>
  54 #include <string.h>
  55 #include <framework/mlt_types.h>
  56 #include <framework/mlt_log.h>
  57 #ifdef USE_SSE2
  58 #include <emmintrin.h>
  59 #endif
  60
  61 void addTrans(StabData* sd, Transform sl)
  62 {
  63     if (!sd->transs) {
  64        sd->transs = tlist_new(0);
  65     }
  66     tlist_append(sd->transs, &sl,sizeof(Transform) );
  67 }
  68
  69
  70
  71 /** initialise measurement fields on the frame.
  72     The size of the fields and the maxshift is used to
  73     calculate an optimal distribution in the frame.
  74 */
  75 int initFields(StabData* sd)
  76 {
  77     int size     = sd->field_size;
  78     int rows = MAX(3,(sd->height - sd->maxshift*2)/size-1);
  79     int cols = MAX(3,(sd->width  - sd->maxshift*2)/size-1);
  80     // make sure that the remaining rows have the same length
  81     sd->field_num  = rows*cols;
  82     sd->field_rows = rows;
  83     mlt_log_debug (NULL,"field setup: rows: %i cols: %i Total: %i fields",
  84                 rows, cols, sd->field_num);
  85
  86     if (!(sd->fields = malloc(sizeof(Field) * sd->field_num))) {
  87         mlt_log_error ( NULL,  "malloc failed!\n");
  88         return 0;
  89     } else {
  90         int i, j;
  91         // the border is the amount by which the field centers
  92         // have to be away from the image boundary
  93         // (stepsize is added in case shift is increased through stepsize)
  94         int border   = size/2 + sd->maxshift + sd->stepsize;
  95         int step_x   = (sd->width  - 2*border)/MAX(cols-1,1);
  96         int step_y   = (sd->height - 2*border) / MAX(rows-1,1);
  97         for (j = 0; j < rows; j++) {
  98             for (i = 0; i < cols; i++) {
  99                 int idx = j*cols+i;
 100                 sd->fields[idx].x = border + i*step_x;
 101                 sd->fields[idx].y = border + j*step_y;
 102                 sd->fields[idx].size = size;
 103             }
 104         }
 105     }
 106     return 1;
 107 }
 108
 109
 110 /**
 111    compares the two given images and returns the average absolute difference
 112    \param d_x shift in x direction
 113    \param d_y shift in y direction
 114 */
 115 double compareImg(unsigned char* I1, unsigned char* I2,
 116                   int width, int height,  int bytesPerPixel, int d_x, int d_y)
 117 {
 118     int i, j;
 119     unsigned char* p1 = NULL;
 120     unsigned char* p2 = NULL;
 121     long int sum = 0;
 122     int effectWidth = width - abs(d_x);
 123     int effectHeight = height - abs(d_y);
 124
 125 /*   DEBUGGING code to export single frames */
 126 /*   char buffer[100]; */
 127 /*   sprintf(buffer, "pic_%02ix%02i_1.ppm", d_x, d_y); */
 128 /*   FILE *pic1 = fopen(buffer, "w"); */
 129 /*   sprintf(buffer, "pic_%02ix%02i_2.ppm", d_x, d_y); */
 130 /*   FILE *pic2 = fopen(buffer, "w"); */
 131 /*   fprintf(pic1, "P6\n%i %i\n255\n", effectWidth, effectHeight); */
 132 /*   fprintf(pic2, "P6\n%i %i\n255\n", effectWidth, effectHeight); */
 133
 134     for (i = 0; i < effectHeight; i++) {
 135         p1 = I1;
 136         p2 = I2;
 137         if (d_y > 0 ){
 138             p1 += (i + d_y) * width * bytesPerPixel;
 139             p2 += i * width * bytesPerPixel;
 140         } else {
 141             p1 += i * width * bytesPerPixel;
 142             p2 += (i - d_y) * width * bytesPerPixel;
 143         }
 144         if (d_x > 0) {
 145             p1 += d_x * bytesPerPixel;
 146         } else {
 147             p2 -= d_x * bytesPerPixel;
 148         }
 149 #ifdef USE_SSE2
 150                 __m128i A,B,C,D,E;
 151                 for (j = 0; j < effectWidth * bytesPerPixel - 16; j++) {
 152 #else
 153                 for (j = 0; j < effectWidth * bytesPerPixel; j++) {
 154 #endif
 155             /* fwrite(p1,1,1,pic1);fwrite(p1,1,1,pic1);fwrite(p1,1,1,pic1);
 156                fwrite(p2,1,1,pic2);fwrite(p2,1,1,pic2);fwrite(p2,1,1,pic2);
 157              */
 158 #ifdef USE_SSE2
 159                         A= _mm_loadu_si128((__m128i*)p1); //load unaligned data
 160                         B= _mm_loadu_si128((__m128i*)p2);
 161                         C= _mm_sad_epu8(A, B);//diff per 8 bit pixel stored in 2 64 byte values
 162                         D = _mm_srli_si128(C, 8); // shift first 64 byte value to align at the same as C
 163                         E = _mm_add_epi32(C, D); // add the 2 values (sum of all diffs)
 164                         sum+= _mm_cvtsi128_si32(E); //convert _m128i to int
 165
 166             p1+=16;
 167             p2+=16;
 168                         j+=15;
 169 #else
 170             sum += abs((int)*p1 - (int)*p2);
 171             p1++;
 172             p2++;
 173 #endif
 174         }
 175     }
 176     /*  fclose(pic1);
 177         fclose(pic2);
 178      */
 179     return sum/((double) effectWidth * effectHeight * bytesPerPixel);
 180 }
 181
 182 /**
 183    compares a small part of two given images
 184    and returns the average absolute difference.
 185    Field center, size and shift have to be choosen,
 186    so that no clipping is required
 187
 188    \param field Field specifies position(center) and size of subimage
 189    \param d_x shift in x direction
 190    \param d_y shift in y direction
 191 */
 192 double compareSubImg(unsigned char* const I1, unsigned char* const I2,
 193                      const Field* field,
 194                      int width, int height, int bytesPerPixel, int d_x, int d_y)
 195 {
 196     int k, j;
 197     unsigned char* p1 = NULL;
 198     unsigned char* p2 = NULL;
 199     int s2 = field->size / 2;
 200     double sum=0.0;
 201
 202     p1=I1 + ((field->x - s2) + (field->y - s2)*width)*bytesPerPixel;
 203
 204     p2=I2 + ((field->x - s2 + d_x) + (field->y - s2 + d_y)*width)*bytesPerPixel;
 205     // TODO: use some mmx or sse stuff here
 206 #ifdef USE_SSE2
 207         __m128i A,B,C,D,E;
 208 #endif
 209     for (j = 0; j < field->size; j++){
 210 #ifdef USE_SSE2
 211                         for (k = 0; k < (field->size * bytesPerPixel) - 16 ; k++) {
 212                                 A= _mm_loadu_si128((__m128i*)p1); //load unaligned data
 213                                 B= _mm_loadu_si128((__m128i*)p2);
 214                                 C= _mm_sad_epu8(A, B);//abd diff stored in 2 64 byte values
 215                                 D = _mm_srli_si128(C, 8); // shift value 8 byte right
 216                                 E = _mm_add_epi32(C, D); // add the 2 values (sum of all diffs)
 217                                 sum+= _mm_cvtsi128_si32(E); //convert _m128i to int
 218
 219             p1+=16;
 220             p2+=16;
 221                         k+=15;
 222 #else
 223                         for (k = 0; k < (field->size * bytesPerPixel); k++) {
 224             sum += abs((int)*p1 - (int)*p2);
 225             p1++;
 226             p2++;
 227 #endif
 228         }
 229         p1 += ((width - field->size) * bytesPerPixel);
 230         p2 += ((width - field->size) * bytesPerPixel);
 231     }
 232     return sum/((double) field->size *field->size* bytesPerPixel);
 233 }
 234
 235 /** \see contrastSubImg called with bytesPerPixel=1*/
 236 double contrastSubImgYUV(StabData* sd, const Field* field){
 237     return contrastSubImg(sd->curr,field,sd->width,sd->height,1);
 238 }
 239
 240 /**
 241     \see contrastSubImg three times called with bytesPerPixel=3
 242     for all channels
 243  */
 244 double contrastSubImgRGB(StabData* sd, const Field* field){
 245     unsigned char* const I = sd->curr;
 246     return (  contrastSubImg(I,  field,sd->width,sd->height,3)
 247             + contrastSubImg(I+1,field,sd->width,sd->height,3)
 248             + contrastSubImg(I+2,field,sd->width,sd->height,3))/3;
 249 }
 250
 251 /**
 252    calculates Michelson-contrast in the given small part of the given image
 253
 254    \param I pointer to framebuffer
 255    \param field Field specifies position(center) and size of subimage
 256    \param width width of frame
 257    \param height height of frame
 258    \param bytesPerPixel calc contrast for only for first channel
 259 */
 260 double contrastSubImg(unsigned char* const I, const Field* field,
 261                      int width, int height, int bytesPerPixel)
 262 {
 263 #if USE_SSE2
 264     int k, j;
 265     int s2 = field->size / 2;
 266     __m128i mini = _mm_set_epi64(_mm_set1_pi8(255), _mm_set1_pi8(255));
 267         __m128i maxi = _mm_set_epi64(_mm_set1_pi8(0)  , _mm_set1_pi8(0));
 268     unsigned char* p = I + ((field->x - s2) + (field->y - s2)*width)*bytesPerPixel;
 269         __m128i A;
 270     for (j = 0; j < field->size; j++){
 271         for (k = 0; k < (field->size * bytesPerPixel)-16 ; k++) {
 272                         A= _mm_loadu_si128((__m128i*)p); //load unaligned data
 273
 274                         maxi = _mm_max_epu8(A,maxi);
 275                         mini = _mm_min_epu8(A,mini);
 276             p+=16;
 277                         k+=15;
 278         }
 279         p += (width - field->size) * bytesPerPixel;
 280     }
 281         int max=0;
 282         union {
 283                 __m128i m;
 284                 uint8_t t[16];
 285         } m;
 286         m.m=maxi;
 287         for (j=0;j<16;j++) {
 288                 if (m.t[j] >max)
 289                         max=m.t[j];
 290         }
 291         int min=255;
 292         m.m=mini;
 293         for (j=0;j<16;j++) {
 294                 if (m.t[j] <min)
 295                         min=m.t[j];
 296         }
 297         //printf("max=%d  min=%d\n", max,min);
 298     return (max-min)/(max+min+0.1); // +0.1 to avoid division by 0
 299 #else
 300
 301     int k, j;
 302     unsigned char* p = NULL;
 303     int s2 = field->size / 2;
 304     unsigned char mini = 255;
 305     unsigned char maxi = 0;
 306     p = I + ((field->x - s2) + (field->y - s2)*width)*bytesPerPixel;
 307     // TODO: use some mmx or sse stuff here
 308
 309     for (j = 0; j < field->size; j++){
 310         for (k = 0; k < field->size * bytesPerPixel; k++) {
 311             mini = (mini < *p) ? mini : *p;
 312             maxi = (maxi > *p) ? maxi : *p;
 313             p += bytesPerPixel;
 314         }
 315         p += (width - field->size) * bytesPerPixel;
 316     }
 317     return (maxi-mini)/(maxi+mini+0.1); // +0.1 to avoid division by 0
 318 #endif
 319 }
 320
 321 /** tries to register current frame onto previous frame.
 322     This is the most simple algorithm:
 323     shift images to all possible positions and calc summed error
 324     Shift with minimal error is selected.
 325 */
 326 Transform calcShiftRGBSimple(StabData* sd)
 327 {
 328     int x = 0, y = 0;
 329     int i, j;
 330     double minerror = 1e20;
 331     for (i = -sd->maxshift; i <= sd->maxshift; i++) {
 332         for (j = -sd->maxshift; j <= sd->maxshift; j++) {
 333             double error = compareImg(sd->curr, sd->prev,
 334                                       sd->width, sd->height, 3, i, j);
 335             if (error < minerror) {
 336                 minerror = error;
 337                 x = i;
 338                 y = j;
 339            }
 340         }
 341     }
 342     return new_transform(x, y, 0, 0, 0);
 343 }
 344
 345
 346 /** tries to register current frame onto previous frame.
 347     (only the luminance is used)
 348     This is the most simple algorithm:
 349     shift images to all possible positions and calc summed error
 350     Shift with minimal error is selected.
 351 */
 352 Transform calcShiftYUVSimple(StabData* sd)
 353 {
 354     int x = 0, y = 0;
 355     int i, j;
 356     unsigned char *Y_c, *Y_p;// , *Cb, *Cr;
 357 #ifdef STABVERBOSE
 358     FILE *f = NULL;
 359     char buffer[32];
 360     tc_snprintf(buffer, sizeof(buffer), "f%04i.dat", sd->t);
 361     f = fopen(buffer, "w");
 362     fprintf(f, "# splot \"%s\"\n", buffer);
 363 #endif
 364
 365     // we only use the luminance part of the image
 366     Y_c  = sd->curr;
 367     //  Cb_c = sd->curr + sd->width*sd->height;
 368     //Cr_c = sd->curr + 5*sd->width*sd->height/4;
 369     Y_p  = sd->prev;
 370     //Cb_p = sd->prev + sd->width*sd->height;
 371     //Cr_p = sd->prev + 5*sd->width*sd->height/4;
 372
 373     double minerror = 1e20;
 374     for (i = -sd->maxshift; i <= sd->maxshift; i++) {
 375         for (j = -sd->maxshift; j <= sd->maxshift; j++) {
 376             double error = compareImg(Y_c, Y_p,
 377                                       sd->width, sd->height, 1, i, j);
 378 #ifdef STABVERBOSE
 379             fprintf(f, "%i %i %f\n", i, j, error);
 380 #endif
 381             if (error < minerror) {
 382                 minerror = error;
 383                 x = i;
 384                 y = j;
 385             }
 386         }
 387     }
 388 #ifdef STABVERBOSE
 389     fclose(f);
 390     tc_log_msg(MOD_NAME, "Minerror: %f\n", minerror);
 391 #endif
 392     return new_transform(x, y, 0, 0, 0);
 393 }
 394
 395
 396
 397 /* calculates rotation angle for the given transform and
 398  * field with respect to the given center-point
 399  */
 400 double calcAngle(StabData* sd, Field* field, Transform* t,
 401                  int center_x, int center_y)
 402 {
 403     // we better ignore fields that are to close to the rotation center
 404     if (abs(field->x - center_x) + abs(field->y - center_y) < sd->maxshift) {
 405         return 0;
 406     } else {
 407         // double r = sqrt(field->x*field->x + field->y*field->y);
 408         double a1 = atan2(field->y - center_y, field->x - center_x);
 409         double a2 = atan2(field->y - center_y + t->y,
 410                           field->x - center_x + t->x);
 411         double diff = a2 - a1;
 412         return (diff>M_PI) ? diff - 2*M_PI
 413             : ( (diff<-M_PI) ? diff + 2*M_PI : diff);
 414     }
 415 }
 416
 417
 418 /* calculates the optimal transformation for one field in YUV frames
 419  * (only luminance)
 420  */
 421 Transform calcFieldTransYUV(StabData* sd, const Field* field, int fieldnum)
 422 {
 423     Transform t = null_transform();
 424     unsigned char *Y_c = sd->curr, *Y_p = sd->prev;
 425     // we only use the luminance part of the image
 426     int i, j;
 427
 428 /*     // check contrast in sub image */
 429 /*     double contr = contrastSubImg(Y_c, field, sd->width, sd->height, 1); */
 430 /*     if(contr < sd->contrast_threshold) { */
 431 /*         t.extra=-1; */
 432 /*         return t; */
 433 /*     } */
 434 #ifdef STABVERBOSE
 435     // printf("%i %i %f\n", sd->t, fieldnum, contr);
 436     FILE *f = NULL;
 437     char buffer[32];
 438     snprintf(buffer, sizeof(buffer), "f%04i_%02i.dat", sd->t, fieldnum);
 439     f = fopen(buffer, "w");
 440     fprintf(f, "# splot \"%s\"\n", buffer);
 441 #endif
 442
 443     double minerror = 1e10;
 444     double error = 1e10;
 445     for (i = -sd->maxshift; i <= sd->maxshift; i += sd->stepsize) {
 446         for (j = -sd->maxshift; j <= sd->maxshift; j += sd->stepsize) {
 447             error = compareSubImg(Y_c, Y_p, field,
 448                                          sd->width, sd->height, 1, i, j);
 449 #ifdef STABVERBOSE
 450             fprintf(f, "%i %i %f\n", i, j, error);
 451 #endif
 452             if (error < minerror) {
 453                 minerror = error;
 454                 t.x = i;
 455                 t.y = j;
 456             }
 457         }
 458     }
 459
 460     if (sd->stepsize > 1) {    // make fine grain check around the best match
 461         int r = sd->stepsize - 1;
 462         for (i = t.x - r; i <= t.x + r; i += 1) {
 463             for (j = -t.y - r; j <= t.y + r; j += 1) {
 464                 if (i == t.x && j == t.y)
 465                     continue; //no need to check this since already done
 466                 error = compareSubImg(Y_c, Y_p, field,
 467                                       sd->width, sd->height, 1, i, j);
 468 #ifdef STABVERBOSE
 469                 fprintf(f, "%i %i %f\n", i, j, error);
 470 #endif
 471                 if (error < minerror){
 472                     minerror = error;
 473                     t.x = i;
 474                     t.y = j;
 475                 }
 476             }
 477         }
 478     }
 479 #ifdef STABVERBOSE
 480     fclose(f);
 481     mlt_log_debug ( "Minerror: %f\n", minerror);
 482 #endif
 483
 484     if (!sd->allowmax && fabs(t.x) == sd->maxshift) {
 485 #ifdef STABVERBOSE
 486         mlt_log_debug ( "maximal x shift ");
 487 #endif
 488         t.x = 0;
 489     }
 490     if (!sd->allowmax && fabs(t.y) == sd->maxshift) {
 491 #ifdef STABVERBOSE
 492         mlt_log_debug ("maximal y shift ");
 493 #endif
 494         t.y = 0;
 495     }
 496     return t;
 497 }
 498
 499 /* calculates the optimal transformation for one field in RGB
 500  *   slower than the YUV version because it uses all three color channels
 501  */
 502 Transform calcFieldTransRGB(StabData* sd, const Field* field, int fieldnum)
 503 {
 504     Transform t = null_transform();
 505     unsigned char *I_c = sd->curr, *I_p = sd->prev;
 506     int i, j;
 507
 508     double minerror = 1e20;
 509     for (i = -sd->maxshift; i <= sd->maxshift; i += 2) {
 510         for (j=-sd->maxshift; j <= sd->maxshift; j += 2) {
 511             double error = compareSubImg(I_c, I_p, field,
 512                                          sd->width, sd->height, 3, i, j);
 513             if (error < minerror) {
 514                 minerror = error;
 515                 t.x = i;
 516                 t.y = j;
 517             }
 518         }
 519     }
 520     for (i = t.x - 1; i <= t.x + 1; i += 2) {
 521         for (j = -t.y - 1; j <= t.y + 1; j += 2) {
 522             double error = compareSubImg(I_c, I_p, field,
 523                                          sd->width, sd->height, 3, i, j);
 524             if (error < minerror) {
 525                 minerror = error;
 526                 t.x = i;
 527                 t.y = j;
 528             }
 529         }
 530     }
 531     if (!sd->allowmax && fabs(t.x) == sd->maxshift) {
 532         t.x = 0;
 533     }
 534     if (!sd->allowmax && fabs(t.y) == sd->maxshift) {
 535         t.y = 0;
 536     }
 537     return t;
 538 }
 539
 540 /* compares contrast_idx structures respect to the contrast
 541    (for sort function)
 542 */
 543 int cmp_contrast_idx(const void *ci1, const void* ci2)
 544 {
 545     double a = ((contrast_idx*)ci1)->contrast;
 546     double b = ((contrast_idx*)ci2)->contrast;
 547     return a < b ? 1 : ( a > b ? -1 : 0 );
 548 }
 549
 550 /* select only the best 'maxfields' fields
 551    first calc contrasts then select from each part of the
 552    frame a some fields
 553 */
 554 tlist* selectfields(StabData* sd, contrastSubImgFunc contrastfunc)
 555 {
 556     int i,j;
 557     tlist* goodflds = tlist_new(0);
 558     contrast_idx *ci = malloc(sizeof(contrast_idx) * sd->field_num);
 559
 560     // we split all fields into row+1 segments and take from each segment
 561     // the best fields
 562     int numsegms = (sd->field_rows+1);
 563     int segmlen = sd->field_num/(sd->field_rows+1)+1;
 564     // split the frame list into rows+1 segments
 565     contrast_idx *ci_segms = malloc(sizeof(contrast_idx) * sd->field_num);
 566     int remaining   = 0;
 567     // calculate contrast for each field
 568     for (i = 0; i < sd->field_num; i++) {
 569         ci[i].contrast = contrastfunc(sd, &sd->fields[i]);
 570         ci[i].index=i;
 571         if(ci[i].contrast < sd->contrast_threshold) ci[i].contrast = 0;
 572         // else printf("%i %lf\n", ci[i].index, ci[i].contrast);
 573     }
 574
 575     memcpy(ci_segms, ci, sizeof(contrast_idx) * sd->field_num);
 576     // get best fields from each segment
 577     for(i=0; i<numsegms; i++){
 578         int startindex = segmlen*i;
 579         int endindex   = segmlen*(i+1);
 580         endindex       = endindex > sd->field_num ? sd->field_num : endindex;
 581         //printf("Segment: %i: %i-%i\n", i, startindex, endindex);
 582
 583         // sort within segment
 584         qsort(ci_segms+startindex, endindex-startindex,
 585               sizeof(contrast_idx), cmp_contrast_idx);
 586         // take maxfields/numsegms
 587         for(j=0; j<sd->maxfields/numsegms; j++){
 588             if(startindex+j >= endindex) continue;
 589             // printf("%i %lf\n", ci_segms[startindex+j].index,
 590             //                    ci_segms[startindex+j].contrast);
 591             if(ci_segms[startindex+j].contrast > 0){
 592                tlist_append(goodflds, &ci[ci_segms[startindex+j].index],sizeof(contrast_idx));
 593                 // don't consider them in the later selection process
 594                 ci_segms[startindex+j].contrast=0;
 595             }
 596         }
 597     }
 598     // check whether enough fields are selected
 599     // printf("Phase2: %i\n", tc_list_size(goodflds));
 600     remaining = sd->maxfields - tlist_size(goodflds);
 601     if(remaining > 0){
 602         // take the remaining from the leftovers
 603         qsort(ci_segms, sd->field_num,
 604               sizeof(contrast_idx), cmp_contrast_idx);
 605         for(j=0; j < remaining; j++){
 606             if(ci_segms[j].contrast > 0){
 607                 tlist_append(goodflds, &ci_segms[j], sizeof(contrast_idx));
 608             }
 609         }
 610     }
 611     // printf("Ende: %i\n", tc_list_size(goodflds));
 612     free(ci);
 613     free(ci_segms);
 614     return goodflds;
 615 }
 616
 617
 618
 619 /* tries to register current frame onto previous frame.
 620  *   Algorithm:
 621  *   check all fields for vertical and horizontal transformation
 622  *   use minimal difference of all possible positions
 623  *   discards fields with low contrast
 624  *   select maxfields field according to their contrast
 625  *   calculate shift as cleaned mean of all remaining fields
 626  *   calculate rotation angle of each field in respect to center of fields
 627  *   after shift removal
 628  *   calculate rotation angle as cleaned mean of all angles
 629  *   compensate for possibly off-center rotation
 630 */
 631 Transform calcTransFields(StabData* sd, calcFieldTransFunc fieldfunc,
 632                           contrastSubImgFunc contrastfunc)
 633 {
 634     Transform* ts  = malloc(sizeof(Transform) * sd->field_num);
 635     Field** fs     = malloc(sizeof(Field*) * sd->field_num);
 636     double *angles = malloc(sizeof(double) * sd->field_num);
 637     int i, index=0, num_trans;
 638     Transform t;
 639 #ifdef STABVERBOSE
 640     FILE *f = NULL;
 641     char buffer[32];
 642     tc_snprintf(buffer, sizeof(buffer), "k%04i.dat", sd->t);
 643     f = fopen(buffer, "w");
 644     fprintf(f, "# plot \"%s\" w l, \"\" every 2:1:0\n", buffer);
 645 #endif
 646
 647
 648     tlist* goodflds = selectfields(sd, contrastfunc);
 649
 650     // use all "good" fields and calculate optimal match to previous frame
 651     contrast_idx* f;
 652     while((f = (contrast_idx*)tlist_pop(goodflds,0) ) != 0){
 653         int i = f->index;
 654         t =  fieldfunc(sd, &sd->fields[i], i); // e.g. calcFieldTransYUV
 655 #ifdef STABVERBOSE
 656         fprintf(f, "%i %i\n%f %f %i\n \n\n", sd->fields[i].x, sd->fields[i].y,
 657                 sd->fields[i].x + t.x, sd->fields[i].y + t.y, t.extra);
 658 #endif
 659         if (t.extra != -1){ // ignore if extra == -1 (unused at the moment)
 660             ts[index] = t;
 661             fs[index] = sd->fields+i;
 662             index++;
 663         }
 664     }
 665     tlist_fini(goodflds);
 666
 667     t = null_transform();
 668     num_trans = index; // amount of transforms we actually have
 669     if (num_trans < 1) {
 670         printf( "too low contrast! No field remains.\n"
 671                 "(no translations are detected in frame %i)", sd->t);
 672         free(ts);
 673         free(fs);
 674         free(angles);
 675         return t;
 676     }
 677
 678     int center_x = 0;
 679     int center_y = 0;
 680     // calc center point of all remaining fields
 681     for (i = 0; i < num_trans; i++) {
 682         center_x += fs[i]->x;
 683         center_y += fs[i]->y;
 684     }
 685     center_x /= num_trans;
 686     center_y /= num_trans;
 687
 688     if (sd->show){ // draw fields and transforms into frame.
 689         // this has to be done one after another to handle possible overlap
 690         if (sd->show > 1) {
 691             for (i = 0; i < num_trans; i++)
 692                 drawFieldScanArea(sd, fs[i], &ts[i]);
 693         }
 694         for (i = 0; i < num_trans; i++)
 695             drawField(sd, fs[i], &ts[i]);
 696         for (i = 0; i < num_trans; i++)
 697             drawFieldTrans(sd, fs[i], &ts[i]);
 698     }
 699     /* median over all transforms
 700        t= median_xy_transform(ts, sd->field_num);*/
 701     // cleaned mean
 702     t = cleanmean_xy_transform(ts, num_trans);
 703
 704     // substract avg
 705     for (i = 0; i < num_trans; i++) {
 706         ts[i] = sub_transforms(&ts[i], &t);
 707     }
 708     // figure out angle
 709     if (sd->field_num < 6) {
 710         // the angle calculation is inaccurate for 5 and less fields
 711         t.alpha = 0;
 712     } else {
 713         for (i = 0; i < num_trans; i++) {
 714             angles[i] = calcAngle(sd, fs[i], &ts[i], center_x, center_y);
 715         }
 716         double min,max;
 717         t.alpha = -cleanmean(angles, num_trans, &min, &max);
 718         if(max-min>sd->maxanglevariation){
 719             t.alpha=0;
 720             printf( "too large variation in angle(%f)\n",
 721                         max-min);
 722         }
 723     }
 724
 725     // compensate for off-center rotation
 726     double p_x = (center_x - sd->width/2);
 727     double p_y = (center_y - sd->height/2);
 728     t.x += (cos(t.alpha)-1)*p_x  - sin(t.alpha)*p_y;
 729     t.y += sin(t.alpha)*p_x  + (cos(t.alpha)-1)*p_y;
 730
 731 #ifdef STABVERBOSE
 732     fclose(f);
 733 #endif
 734     free(ts);
 735     free(fs);
 736     free(angles);
 737     return t;
 738 }
 739
 740 /** draws the field scanning area */
 741 void drawFieldScanArea(StabData* sd, const Field* field, const Transform* t)
 742 {
 743         if (sd->pixelformat != mlt_image_yuv420p) {
 744                 mlt_log_warning (NULL, "format not usable\n");
 745         return;
 746         }
 747     drawBox(sd->curr, sd->width, sd->height, 1, field->x, field->y,
 748             field->size+2*sd->maxshift, field->size+2*sd->maxshift, 80);
 749 }
 750
 751 /** draws the field */
 752 void drawField(StabData* sd, const Field* field, const Transform* t)
 753 {
 754         if (sd->pixelformat != mlt_image_yuv420p){
 755                 mlt_log_warning (NULL, "format not usable\n");
 756         return;
 757         }
 758     drawBox(sd->curr, sd->width, sd->height, 1, field->x, field->y,
 759             field->size, field->size, t->extra == -1 ? 100 : 40);
 760 }
 761
 762 /** draws the transform data of this field */
 763 void drawFieldTrans(StabData* sd, const Field* field, const Transform* t)
 764 {
 765         if (sd->pixelformat != mlt_image_yuv420p){
 766                 mlt_log_warning (NULL, "format not usable\n");
 767         return;
 768         }
 769     drawBox(sd->curr, sd->width, sd->height, 1,
 770             field->x, field->y, 5, 5, 128);     // draw center
 771     drawBox(sd->curr, sd->width, sd->height, 1,
 772             field->x + t->x, field->y + t->y, 8, 8, 250); // draw translation
 773 }
 774
 775 /**
 776  * draws a box at the given position x,y (center) in the given color
 777    (the same for all channels)
 778  */
 779 void drawBox(unsigned char* I, int width, int height, int bytesPerPixel,
 780              int x, int y, int sizex, int sizey, unsigned char color){
 781
 782     unsigned char* p = NULL;
 783     int j,k;
 784     p = I + ((x - sizex/2) + (y - sizey/2)*width)*bytesPerPixel;
 785     for (j = 0; j < sizey; j++){
 786         for (k = 0; k < sizex * bytesPerPixel; k++) {
 787             *p = color;
 788             p++;
 789         }
 790         p += (width - sizex) * bytesPerPixel;
 791     }
 792 }
 793
 794 struct iterdata {
 795     FILE *f;
 796     int  counter;
 797 };
 798
 799 /*************************************************************************/
 800
 801 /* Module interface routines and data. */
 802
 803 /*************************************************************************/
 804
 805 /*
 806  * stabilize_configure:  Configure this instance of the module.  See
 807  * tcmodule-data.h for function details.
 808  */
 809 int stabilize_configure(StabData* instance
 810 /*const char *options,
 811                                int pixelfmt */
 812                                /*TCModuleExtraData *xdata[]*/)
 813 {
 814     StabData *sd = instance;
 815     /*    sd->framesize = sd->vob->im_v_width * MAX_PLANES *
 816           sizeof(char) * 2 * sd->vob->im_v_height * 2;     */
 817     /*TODO sd->framesize = sd->vob->im_v_size;    */
 818     sd->prev = calloc(1,sd->framesize);
 819                 sd->grayimage = calloc(1,sd->width*sd->height);
 820
 821     if (!sd->prev || !sd->grayimage) {
 822         printf( "malloc failed");
 823         return -1;
 824     }
 825     sd->currcopy = 0;
 826     sd->hasSeenOneFrame = 0;
 827     sd->transs = 0;
 828     sd->allowmax   = 0;
 829     sd->field_size  = MIN(sd->width, sd->height)/12;
 830     sd->maxanglevariation = 1;
 831
 832     sd->shakiness = MIN(10,MAX(1,sd->shakiness));
 833     sd->accuracy  = MAX(sd->shakiness,MIN(15,MAX(1,sd->accuracy)));
 834     if (1) {
 835         mlt_log_debug (NULL, "Image Stabilization Settings:\n");
 836         mlt_log_debug (NULL, "     shakiness = %d\n", sd->shakiness);
 837         mlt_log_debug (NULL, "      accuracy = %d\n", sd->accuracy);
 838         mlt_log_debug (NULL, "      stepsize = %d\n", sd->stepsize);
 839         mlt_log_debug (NULL, "          algo = %d\n", sd->algo);
 840         mlt_log_debug (NULL, "   mincontrast = %f\n", sd->contrast_threshold);
 841         mlt_log_debug (NULL, "          show = %d\n", sd->show);
 842     }
 843
 844 #ifndef USE_SSE2
 845         mlt_log_info(NULL,"No SSE2 support enabled, this will slow down a lot\n");
 846 #endif
 847     // shift and size: shakiness 1: height/40; 10: height/4
 848     sd->maxshift    = MIN(sd->width, sd->height)*sd->shakiness/40;
 849     sd->field_size   = MIN(sd->width, sd->height)*sd->shakiness/40;
 850
 851     mlt_log_debug ( NULL,  "Fieldsize: %i, Maximal translation: %i pixel\n",
 852                 sd->field_size, sd->maxshift);
 853     if (sd->algo==1) {
 854         // initialize measurement fields. field_num is set here.
 855         if (!initFields(sd)) {
 856             return -1;
 857         }
 858         sd->maxfields = (sd->accuracy) * sd->field_num / 15;
 859         mlt_log_debug ( NULL, "Number of used measurement fields: %i out of %i\n",
 860                     sd->maxfields, sd->field_num);
 861     }
 862     if (sd->show){
 863         sd->currcopy = calloc(1,sd->framesize);
 864         }
 865
 866     /* load unsharp filter to smooth the frames. This allows larger stepsize.*/
 867     char unsharp_param[128];
 868     int masksize = MIN(13,sd->stepsize*1.8); // only works up to 13.
 869     sprintf(unsharp_param,"luma=-1:luma_matrix=%ix%i:pre=1",
 870             masksize, masksize);
 871     return 0;
 872 }
 873
 874
 875 /**
 876  * stabilize_filter_video: performs the analysis of subsequent frames
 877  * See tcmodule-data.h for function details.
 878  */
 879
 880 int stabilize_filter_video(StabData* instance,
 881                                   unsigned char *frame,mlt_image_format pixelformat)
 882 {
 883     StabData *sd = instance;
 884     sd->pixelformat=pixelformat;
 885                 int l=sd->width*sd->height;
 886                 unsigned char* tmpgray=sd->grayimage;
 887                 if (pixelformat == mlt_image_yuv422){
 888                         while(l--){
 889                                 *tmpgray++=*frame++;
 890                                 frame++;
 891                         };
 892                 }
 893
 894     if(sd->show) { // save the buffer to restore at the end for prev
 895                         if (pixelformat == mlt_image_yuv420p){
 896                                 memcpy(sd->currcopy, sd->grayimage, sd->framesize);
 897                         }
 898                 }
 899     if (sd->hasSeenOneFrame) {
 900         sd->curr = sd->grayimage;
 901         if (pixelformat == mlt_image_rgb24) {
 902             if (sd->algo == 0)
 903                 addTrans(sd, calcShiftRGBSimple(sd));
 904             else if (sd->algo == 1)
 905                 addTrans(sd, calcTransFields(sd, calcFieldTransRGB,
 906                          contrastSubImgRGB));
 907         } else if (pixelformat == mlt_image_yuv420p ) {
 908             if (sd->algo == 0)
 909                 addTrans(sd, calcShiftYUVSimple(sd));
 910             else if (sd->algo == 1)
 911                 addTrans(sd, calcTransFields(sd, calcFieldTransYUV,
 912                                              contrastSubImgYUV));
 913         } else if (pixelformat == mlt_image_yuv422 ) {
 914             if (sd->algo == 0)
 915                 addTrans(sd, calcShiftYUVSimple(sd));
 916             else if (sd->algo == 1)
 917                 addTrans(sd, calcTransFields(sd, calcFieldTransYUV,
 918                                              contrastSubImgYUV));
 919         } else {
 920             mlt_log_warning (NULL,"unsupported Codec: %i\n",
 921                         pixelformat);
 922             return 0;
 923         }
 924     } else {
 925         sd->hasSeenOneFrame = 1;
 926         addTrans(sd, null_transform());
 927     }
 928
 929     if(!sd->show) { // copy current frame to prev for next frame comparison
 930         memcpy(sd->prev, sd->grayimage, sd->framesize);
 931     } else { // use the copy because we changed the original frame
 932         memcpy(sd->prev, sd->currcopy, sd->framesize);
 933     }
 934     sd->t++;
 935     return 0;
 936 }
 937
 938 /**
 939  * stabilize_stop:  Reset this instance of the module.  See tcmodule-data.h
 940  * for function details.
 941  */
 942
 943 int stabilize_stop(StabData* instance)
 944 {
 945     StabData *sd = instance;
 946     if (sd->prev) {
 947         free(sd->prev);
 948         sd->prev = NULL;
 949     }
 950     if (sd->grayimage){
 951                         free(sd->grayimage);
 952                         sd->grayimage=NULL;
 953
 954                 }
 955     return 0;
 956 }
 957
 958
 959