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         return t;
 673     }
 674
 675     int center_x = 0;
 676     int center_y = 0;
 677     // calc center point of all remaining fields
 678     for (i = 0; i < num_trans; i++) {
 679         center_x += fs[i]->x;
 680         center_y += fs[i]->y;
 681     }
 682     center_x /= num_trans;
 683     center_y /= num_trans;
 684
 685     if (sd->show){ // draw fields and transforms into frame.
 686         // this has to be done one after another to handle possible overlap
 687         if (sd->show > 1) {
 688             for (i = 0; i < num_trans; i++)
 689                 drawFieldScanArea(sd, fs[i], &ts[i]);
 690         }
 691         for (i = 0; i < num_trans; i++)
 692             drawField(sd, fs[i], &ts[i]);
 693         for (i = 0; i < num_trans; i++)
 694             drawFieldTrans(sd, fs[i], &ts[i]);
 695     }
 696     /* median over all transforms
 697        t= median_xy_transform(ts, sd->field_num);*/
 698     // cleaned mean
 699     t = cleanmean_xy_transform(ts, num_trans);
 700
 701     // substract avg
 702     for (i = 0; i < num_trans; i++) {
 703         ts[i] = sub_transforms(&ts[i], &t);
 704     }
 705     // figure out angle
 706     if (sd->field_num < 6) {
 707         // the angle calculation is inaccurate for 5 and less fields
 708         t.alpha = 0;
 709     } else {
 710         for (i = 0; i < num_trans; i++) {
 711             angles[i] = calcAngle(sd, fs[i], &ts[i], center_x, center_y);
 712         }
 713         double min,max;
 714         t.alpha = -cleanmean(angles, num_trans, &min, &max);
 715         if(max-min>sd->maxanglevariation){
 716             t.alpha=0;
 717             printf( "too large variation in angle(%f)\n",
 718                         max-min);
 719         }
 720     }
 721
 722     // compensate for off-center rotation
 723     double p_x = (center_x - sd->width/2);
 724     double p_y = (center_y - sd->height/2);
 725     t.x += (cos(t.alpha)-1)*p_x  - sin(t.alpha)*p_y;
 726     t.y += sin(t.alpha)*p_x  + (cos(t.alpha)-1)*p_y;
 727
 728 #ifdef STABVERBOSE
 729     fclose(f);
 730 #endif
 731     return t;
 732 }
 733
 734 /** draws the field scanning area */
 735 void drawFieldScanArea(StabData* sd, const Field* field, const Transform* t)
 736 {
 737     if (!sd->pixelformat == mlt_image_yuv420p) {
 738                 mlt_log_warning (NULL, "format not usable\n");
 739         return;
 740         }
 741     drawBox(sd->curr, sd->width, sd->height, 1, field->x, field->y,
 742             field->size+2*sd->maxshift, field->size+2*sd->maxshift, 80);
 743 }
 744
 745 /** draws the field */
 746 void drawField(StabData* sd, const Field* field, const Transform* t)
 747 {
 748     if (!sd->pixelformat == mlt_image_yuv420p){
 749                 mlt_log_warning (NULL, "format not usable\n");
 750         return;
 751         }
 752     drawBox(sd->curr, sd->width, sd->height, 1, field->x, field->y,
 753             field->size, field->size, t->extra == -1 ? 100 : 40);
 754 }
 755
 756 /** draws the transform data of this field */
 757 void drawFieldTrans(StabData* sd, const Field* field, const Transform* t)
 758 {
 759     if (!sd->pixelformat == mlt_image_yuv420p){
 760                 mlt_log_warning (NULL, "format not usable\n");
 761         return;
 762         }
 763     drawBox(sd->curr, sd->width, sd->height, 1,
 764             field->x, field->y, 5, 5, 128);     // draw center
 765     drawBox(sd->curr, sd->width, sd->height, 1,
 766             field->x + t->x, field->y + t->y, 8, 8, 250); // draw translation
 767 }
 768
 769 /**
 770  * draws a box at the given position x,y (center) in the given color
 771    (the same for all channels)
 772  */
 773 void drawBox(unsigned char* I, int width, int height, int bytesPerPixel,
 774              int x, int y, int sizex, int sizey, unsigned char color){
 775
 776     unsigned char* p = NULL;
 777     int j,k;
 778     p = I + ((x - sizex/2) + (y - sizey/2)*width)*bytesPerPixel;
 779     for (j = 0; j < sizey; j++){
 780         for (k = 0; k < sizex * bytesPerPixel; k++) {
 781             *p = color;
 782             p++;
 783         }
 784         p += (width - sizex) * bytesPerPixel;
 785     }
 786 }
 787
 788 struct iterdata {
 789     FILE *f;
 790     int  counter;
 791 };
 792
 793 /*************************************************************************/
 794
 795 /* Module interface routines and data. */
 796
 797 /*************************************************************************/
 798
 799 /**
 800  * stabilize_init:  Initialize this instance of the module.  See
 801  * tcmodule-data.h for function details.
 802  */
 803 int stabilize_init(StabData* instance)
 804 {
 805
 806     instance = calloc(1,sizeof(StabData)); // allocation with zero values
 807     if (!instance) {
 808         return -1;
 809     }
 810     return 0;
 811 }
 812
 813 /*
 814  * stabilize_configure:  Configure this instance of the module.  See
 815  * tcmodule-data.h for function details.
 816  */
 817 int stabilize_configure(StabData* instance
 818 /*const char *options,
 819                                int pixelfmt */
 820                                /*TCModuleExtraData *xdata[]*/)
 821 {
 822     StabData *sd = instance;
 823     /*    sd->framesize = sd->vob->im_v_width * MAX_PLANES *
 824           sizeof(char) * 2 * sd->vob->im_v_height * 2;     */
 825     /*TODO sd->framesize = sd->vob->im_v_size;    */
 826     sd->prev = calloc(1,sd->framesize);
 827                 sd->grayimage = calloc(1,sd->width*sd->height);
 828
 829     if (!sd->prev || !sd->grayimage) {
 830         printf( "malloc failed");
 831         return -1;
 832     }
 833     sd->currcopy = 0;
 834     sd->hasSeenOneFrame = 0;
 835     sd->transs = 0;
 836     sd->allowmax   = 0;
 837     sd->field_size  = MIN(sd->width, sd->height)/12;
 838     sd->maxanglevariation = 1;
 839
 840     sd->shakiness = MIN(10,MAX(1,sd->shakiness));
 841     sd->accuracy  = MAX(sd->shakiness,MIN(15,MAX(1,sd->accuracy)));
 842     if (1) {
 843         mlt_log_debug (NULL, "Image Stabilization Settings:\n");
 844         mlt_log_debug (NULL, "     shakiness = %d\n", sd->shakiness);
 845         mlt_log_debug (NULL, "      accuracy = %d\n", sd->accuracy);
 846         mlt_log_debug (NULL, "      stepsize = %d\n", sd->stepsize);
 847         mlt_log_debug (NULL, "          algo = %d\n", sd->algo);
 848         mlt_log_debug (NULL, "   mincontrast = %f\n", sd->contrast_threshold);
 849         mlt_log_debug (NULL, "          show = %d\n", sd->show);
 850     }
 851
 852 #ifndef USE_SSE2
 853         mlt_log_info(NULL,"No SSE2 support enabled, this will slow down a lot\n");
 854 #endif
 855     // shift and size: shakiness 1: height/40; 10: height/4
 856     sd->maxshift    = MIN(sd->width, sd->height)*sd->shakiness/40;
 857     sd->field_size   = MIN(sd->width, sd->height)*sd->shakiness/40;
 858
 859     mlt_log_debug ( NULL,  "Fieldsize: %i, Maximal translation: %i pixel\n",
 860                 sd->field_size, sd->maxshift);
 861     if (sd->algo==1) {
 862         // initialize measurement fields. field_num is set here.
 863         if (!initFields(sd)) {
 864             return -1;
 865         }
 866         sd->maxfields = (sd->accuracy) * sd->field_num / 15;
 867         mlt_log_debug ( NULL, "Number of used measurement fields: %i out of %i\n",
 868                     sd->maxfields, sd->field_num);
 869     }
 870     if (sd->show){
 871         sd->currcopy = calloc(1,sd->framesize);
 872         }
 873
 874     /* load unsharp filter to smooth the frames. This allows larger stepsize.*/
 875     char unsharp_param[128];
 876     int masksize = MIN(13,sd->stepsize*1.8); // only works up to 13.
 877     sprintf(unsharp_param,"luma=-1:luma_matrix=%ix%i:pre=1",
 878             masksize, masksize);
 879     return 0;
 880 }
 881
 882
 883 /**
 884  * stabilize_filter_video: performs the analysis of subsequent frames
 885  * See tcmodule-data.h for function details.
 886  */
 887
 888 int stabilize_filter_video(StabData* instance,
 889                                   unsigned char *frame,mlt_image_format pixelformat)
 890 {
 891     StabData *sd = instance;
 892     sd->pixelformat=pixelformat;
 893                 int l=sd->width*sd->height;
 894                 unsigned char* tmpgray=sd->grayimage;
 895                 if (pixelformat == mlt_image_yuv422){
 896                         while(l--){
 897                                 *tmpgray++=*frame++;
 898                                 frame++;
 899                         };
 900                 }
 901
 902     if(sd->show) { // save the buffer to restore at the end for prev
 903                         if (pixelformat == mlt_image_yuv420p){
 904                                 memcpy(sd->currcopy, sd->grayimage, sd->framesize);
 905                         }
 906                 }
 907     if (sd->hasSeenOneFrame) {
 908         sd->curr = sd->grayimage;
 909         if (pixelformat == mlt_image_rgb24) {
 910             if (sd->algo == 0)
 911                 addTrans(sd, calcShiftRGBSimple(sd));
 912             else if (sd->algo == 1)
 913                 addTrans(sd, calcTransFields(sd, calcFieldTransRGB,
 914                          contrastSubImgRGB));
 915         } else if (pixelformat == mlt_image_yuv420p ) {
 916             if (sd->algo == 0)
 917                 addTrans(sd, calcShiftYUVSimple(sd));
 918             else if (sd->algo == 1)
 919                 addTrans(sd, calcTransFields(sd, calcFieldTransYUV,
 920                                              contrastSubImgYUV));
 921         } else if (pixelformat == mlt_image_yuv422 ) {
 922             if (sd->algo == 0)
 923                 addTrans(sd, calcShiftYUVSimple(sd));
 924             else if (sd->algo == 1)
 925                 addTrans(sd, calcTransFields(sd, calcFieldTransYUV,
 926                                              contrastSubImgYUV));
 927         } else {
 928             mlt_log_warning (NULL,"unsupported Codec: %i\n",
 929                         pixelformat);
 930             return 0;
 931         }
 932     } else {
 933         sd->hasSeenOneFrame = 1;
 934         addTrans(sd, null_transform());
 935     }
 936
 937     if(!sd->show) { // copy current frame to prev for next frame comparison
 938         memcpy(sd->prev, sd->grayimage, sd->framesize);
 939     } else { // use the copy because we changed the original frame
 940         memcpy(sd->prev, sd->currcopy, sd->framesize);
 941     }
 942     sd->t++;
 943     return 0;
 944 }
 945
 946 /**
 947  * stabilize_stop:  Reset this instance of the module.  See tcmodule-data.h
 948  * for function details.
 949  */
 950
 951 int stabilize_stop(StabData* instance)
 952 {
 953     StabData *sd = instance;
 954     if (sd->prev) {
 955         free(sd->prev);
 956         sd->prev = NULL;
 957     }
 958     if (sd->grayimage){
 959                         free(sd->grayimage);
 960                         sd->grayimage=NULL;
 961
 962                 }
 963     return 0;
 964 }
 965
 966
 967