y4mstabilizer.c

来自「Motion JPEG编解码器源代码」· C语言 代码 · 共 678 行 · 第 1/2 页

mjpeg_error_exit1(" malloc(%d) failed\n", uvlen);
yuv[3] = malloc(len/4);
if (yuv[3] == NULL)
mjpeg_error_exit1(" malloc(%d) failed\n", len/4);
yuv[4] = malloc(len/16);
if (yuv[4] == NULL)
mjpeg_error_exit1(" malloc(%d) failed\n", len/16);
yuv[5] = yuv[0] + w;
yuv[6] = yuv[1] + w/SS_H;
yuv[7] = yuv[2] + w/SS_H;
yuv[8] = yuv[3] + w/2;
yuv[9] = yuv[3] + w/4;
}

/*****************************************************************************
* generate a lowpassfiltered and subsampled copy                            *
* of the source image (src) at the destination                              *
* image location.                                                           *
* Lowpass-filtering is important, as this subsampled                        *
* image is used for motion estimation and the result                        *
* is more reliable when filtered.                                           *
* only subsample actual data, but keeping full buffer size for simplicity   *
*****************************************************************************/
static void
subsample (uint8_t *src, uint8_t *dst, int w, int h)
{
int c, x, w2 = w / 2;
uint8_t *s1 = src;
uint8_t *s2 = src + w;
for (h /= 2; h >= 0; h--)
{
for (x = 0; x < w2; x++)
{
c = *s1++ + *s2++;
c += *s1++ + *s2++;
*dst++ = c >> 2;
}
s1 += w;
s2 += w;
}
}

/*
* Determine global motion.
* Note that only the Y-plane is used.
* The global motion is taken as the median of the individual motions.
*
* Note that w (frame width) should equal ws (frame width stride) unless
* we are treating an interlaced frame as two subframes, in which case
* ws should be twice w.
*/
static void
gmotion (u_char **y0, u_char **y1, int w, int ws, int h, vec *dij)
{
int i, j, di, dj;
int we = w - (Stab.rad+8);
int he = h - (Stab.rad+8);
int xs[Stab.diam+Stab.diam], ys[Stab.diam+Stab.diam], t = 0;
vec ij;
bzero(xs, sizeof xs);
bzero(ys, sizeof ys);
/* Determine local motions for all blocks */
for (i = Stab.rad; i < we; i += Stab.stride)
for (j = Stab.rad; j < he; j += Stab.stride)
{
ij.x = i/4; ij.y = j/4;
motion(y0[4], y1[4], ws/4, Stab.rad/4, i/4,  j/4,  &ij);
ij.x += ij.x; ij.y += ij.y;
motion(y0[3], y1[3], ws/2, 3,          i/2, j/2, &ij);
ij.x += ij.x; ij.y += ij.y;
motion(y0[0], y1[0], ws,   3,          i,   j,   &ij);
motion0(y0[0],y1[0], ws,               i,   j,   &ij);
di = ij.x - (i+i); dj = ij.y - (j+j);
/*if ((abs(di) <= Stab.rad) && (abs(dj) <= Stab.rad))*/
{
xs[di+Stab.diam]++;
ys[dj+Stab.diam]++;
t++;
}
}
/* Determine median motions */
t /= 2;
for (di = i = 0; di < Stab.diam+Stab.diam; i += xs[di++])
if (i >= t)
break;
dij->x = di - Stab.diam;
for (dj = j = 0; dj < Stab.diam+Stab.diam; j += ys[dj++])
if (j >= t)
break;
dij->y = dj - Stab.diam;
}

/*********************************************************************
*                                                                   *
* Estimate Motion Vectors                                           *
*                                                                   *
*********************************************************************/
static void 
motion (u_char *y0, u_char *y1, int w, int r, int ri, int rj, vec *dij)
{
uint32_t best_SAD=INT_MAX, SAD=INT_MAX; 
int i = dij->x, j = dij->y;
int ii, jj;
y0 += (rj * w) + ri;
for (ii = -r; ii <= r; ii++)
for (jj = -r; jj <= r; jj++)
{
SAD = calc_SAD_noaccel(y0, y1 + (j+jj) * w + i+ii, w, best_SAD);
SAD += ii*ii + jj*jj; /* favour center matches... */
if (SAD <= best_SAD)
{
best_SAD = SAD;
dij->x = i + ii;
dij->y = j + jj;
}
}
}

/*********************************************************************
*                                                                   *
* Estimate Motion Vectors in not subsampled frames                  *
*                                                                   *
*********************************************************************/
static void 
motion0 (u_char *y0, u_char *y1, int w, int ri, int rj, vec *dij)
{
uint32_t SAD, best_SAD = INT_MAX;
int adjw;
int i = dij->x, j = dij->y;
int ii, jj;
u_char *y1r = y1 + j*w + i;

y0 += rj*w + ri;
y1 += (j-1)*w + i - 1;
adjw = w - 3;
for (jj = -1; jj <= 1; jj++, y1 += adjw)
for (ii = -1; ii <= 1; ii++, y1++)
{
SAD = calc_SAD_half_noaccel(y0, y1r, y1, w, best_SAD);
if (SAD < best_SAD)
{
best_SAD = SAD;
dij->x = ii+i+i-1;
dij->y = jj+j+j-1;
}
}
}

/*********************************************************************
*                                                                   *
* SAD-function for Y without MMX/MMXE                               *
*                                                                   *
*********************************************************************/
static uint32_t
calc_SAD_noaccel (uint8_t *frm, uint8_t *ref, int w, int limit)
{
uint32_t d = 0;
uint32_t adj = w - 8;
#define LINE \
d += abs(*frm++ - *ref++); d += abs(*frm++ - *ref++); \
d += abs(*frm++ - *ref++); d += abs(*frm++ - *ref++); \
d += abs(*frm++ - *ref++); d += abs(*frm++ - *ref++); \
d += abs(*frm++ - *ref++); d += abs(*frm++ - *ref++); \
if (d > limit) \
return INT_MAX; \
frm += adj; ref += adj
LINE; LINE; LINE; LINE;
LINE; LINE; LINE; LINE;
#undef LINE
return d;
}

/*********************************************************************
*                                                                   *
* halfpel SAD-function for Y without MMX/MMXE                       *
*                                                                   *
*********************************************************************/
static uint32_t
calc_SAD_half_noaccel(uint8_t*ref, uint8_t*frm1, uint8_t*frm2, int w, int limit)
{
uint32_t d = 0;
uint32_t adj = w - 8;
#define LINE \
d += abs(((*frm1++ + *frm2++) >> 1) - *ref++); \
d += abs(((*frm1++ + *frm2++) >> 1) - *ref++); \
d += abs(((*frm1++ + *frm2++) >> 1) - *ref++); \
d += abs(((*frm1++ + *frm2++) >> 1) - *ref++); \
d += abs(((*frm1++ + *frm2++) >> 1) - *ref++); \
d += abs(((*frm1++ + *frm2++) >> 1) - *ref++); \
d += abs(((*frm1++ + *frm2++) >> 1) - *ref++); \
d += abs(((*frm1++ + *frm2++) >> 1) - *ref++); \
if (d > limit) \
return INT_MAX; \
frm1 += adj; frm2 += adj; ref += adj
LINE; LINE; LINE; LINE;
LINE; LINE; LINE; LINE;
#undef LINE
return d;
}

static void
calcshift (vec *gp, vec *shftp)
{
int ss_h = Stab.nosuper ? SS_H : 1;
int ss_v = Stab.nosuper ? SS_V : 1;
/* gmotion() returns motion in half-pixels */
/* Factor in <alpha> the "viscosity"... */
Stab.gsX = (Stab.gsX * Stab.alpha) + gp->x/2.0;
Stab.gsY = (Stab.gsY * Stab.alpha) + gp->y/2.0;
/* Now that we know the movement, shift to counteract it */
shftp->x = -xround(Stab.gsX, ss_h);
shftp->y = -xround(Stab.gsY, ss_v);
if (Stab.verbose > 1)
mjpeg_log(LOG_INFO, "global motion xy*2=<%d,%d>"
" Accumulated xy=<%g,%g> shift xy=%d,%d>\n",
gp->x, gp->y, Stab.gsX, Stab.gsY, shftp->x, shftp->y);
}

/* Round the given float value to the nearest multiple of <r>. */
static int
xround (float v, int r)
{
if (v < 0)
return (-xround(-v, r));
return (((int)((v + r/2.0) / r)) * r);
}

/*
* Shift a frame.
* The frame is always copied to the destination.
* 
* Note that w (frame width) should equal ws (frame width stride) unless
* we are treating an interlaced frame as two subframes, in which case
* ws should be twice w.
*/
static void
doshift (u_char**yuv1, u_char**yuv2, int h, int w, int ws, vec *shift)
{
int dosuper = (shift->x % SS_H) | (shift->y % SS_V);
int ss_h = dosuper ? 1 : SS_H;
int ss_v = dosuper ? 1 : SS_V;
/* If we have to supersample the chroma, then do it now, before shifting */
if (dosuper)
chroma_supersample(Y4M_CHROMA_420JPEG, yuv1, w, h);
/* Do the horizontal shifting first.  The frame is shifted into
* the yuv2 frame. Even if there is no horizontal shifting to do,
* we copy the frame because the vertical shift is desctructive,
* and we want to preserve the yuv1 frame to compare against the
* next one. */
hshift(yuv1[0],yuv2[0],h,     w,     shift->x,      16, ws);
hshift(yuv1[1],yuv2[1],h/ss_v,w/ss_h,shift->x/ss_h,128, ws/ss_h);
hshift(yuv1[2],yuv2[2],h/ss_v,w/ss_h,shift->x/ss_h,128, ws/ss_h);
/* Vertical shift, then write the frame */
if (shift->y)
{
vertical_shift(yuv2[0], shift->y,      w,      ws,      h,      16);
vertical_shift(yuv2[1], shift->y/ss_v, w/ss_h, ws/ss_h, h/ss_v, 128);
vertical_shift(yuv2[2], shift->y/ss_v, w/ss_h, ws/ss_h, h/ss_v, 128);
}
/* Undo the supersampling */
if (dosuper)
chroma_subsample(Y4M_CHROMA_420JPEG, yuv1, w, h);
}

/*
* Shift one plane of the frame by the given horizontal shift amount.
* The shifted frame is left in the <vdst> buffer.
*/
static void
hshift (u_char *vsrc, u_char *vdst, int h, int w, int shift, int blk,int ws)
{
int i;
for (i = 0; i < h; i++)
{
if (shift > 0)
{
bcopy(vsrc, vdst+shift, w-shift);
memset(vdst, blk, shift); /* black */
}
else if (shift < 0)
{
bcopy(vsrc-shift, vdst, w+shift);
memset(vdst+w+shift, blk, -shift);
}
else
bcopy(vsrc, vdst, w);
vsrc += ws;
vdst += ws;
}
}

/*
* Shift the frame vertically.  The frame data is modified in place.
*/
static void
vertical_shift(u_char *y, int vshift, int w, int ws, int h, int blk)
{
/* Easy case - we can shift everything at once */
if (w == ws)
{
if (vshift > 0)
{
memmove(y + vshift*ws, y, (h-vshift) * ws);
memset(y, blk, vshift * ws);
}
else
{
memmove(y, y - vshift * ws, (h+vshift) * ws);
memset(y + (h + vshift) * ws, blk, -vshift * w);
}
}
/* Must go line-by-line */
else
{
u_char *dst, *src;
int i, n;
if (vshift > 0)
{
dst = y + (h - 1) * ws;
src = dst - vshift * ws;
n = h - vshift;
for (i = 0; i < n; i++, (dst -= ws), (src -= ws))
memcpy(dst, src, w);
for ( ; i < h; i++, dst -= ws)
memset(dst, blk, w);
}
else
{
dst = y;
src = y - vshift * ws;
n = h + vshift;
for (i = 0; i < n; i++, (dst += ws), (src += ws))
memcpy(dst, src, w);
for ( ; i < h; i++, dst += ws)
memset(dst, blk, w);
}
}
}