klt.rar_KLT matlab math_KLT tracking_feature extraction_klt_trac

/********************************************************************* * trackFeatures.c * *********************************************************************/ /* Standard includes */ #include <assert.h> #include <math.h> /* fabs() */ #include <stdlib.h> /* malloc() */ #include <stdio.h> /* fflush() */ /* Our includes */ #include "base.h" #include "error.h" #include "convolve.h" /* for computing pyramid */ #include "klt.h" #include "klt_util.h" /* _KLT_FloatImage */ #include "pyramid.h" /* _KLT_Pyramid */ extern int KLT_verbose; typedef float *_FloatWindow; /********************************************************************* * _interpolate * * Given a point (x,y) in an image, computes the bilinear interpolated * gray-level value of the point in the image. */ static float _interpolate( float x, float y, _KLT_FloatImage img) { int xt = (int) x; /* coordinates of top-left corner */ int yt = (int) y; float ax = x - xt; float ay = y - yt; float *ptr = img->data + (img->ncols*yt) + xt; #ifndef _DNDEBUG if (xt<0 || yt<0 || xt>=img->ncols-1 || yt>=img->nrows-1) { fprintf(stderr, "(xt,yt)=(%d,%d) imgsize=(%d,%d)\n" "(x,y)=(%f,%f) (ax,ay)=(%f,%f)\n", xt, yt, img->ncols, img->nrows, x, y, ax, ay); fflush(stderr); } #endif assert (xt >= 0 && yt >= 0 && xt <= img->ncols - 2 && yt <= img->nrows - 2); return ( (1-ax) * (1-ay) * *ptr + ax * (1-ay) * *(ptr+1) + (1-ax) * ay * *(ptr+(img->ncols)) + ax * ay * *(ptr+(img->ncols)+1) ); } /********************************************************************* * _computeIntensityDifference * * Given two images and the window center in both images, * aligns the images wrt the window and computes the difference * between the two overlaid images. */ static void _computeIntensityDifference( _KLT_FloatImage img1, /* images */ _KLT_FloatImage img2, float x1, float y1, /* center of window in 1st img */ float x2, float y2, /* center of window in 2nd img */ int width, int height, /* size of window */ _FloatWindow imgdiff) /* output */ { register int hw = width/2, hh = height/2; float g1, g2; register int i, j; /* Compute values */ for (j = -hh ; j <= hh ; j++) for (i = -hw ; i <= hw ; i++) { g1 = _interpolate(x1+i, y1+j, img1); g2 = _interpolate(x2+i, y2+j, img2); *imgdiff++ = g1 - g2; } } /********************************************************************* * _computeGradientSum * * Given two gradients and the window center in both images, * aligns the gradients wrt the window and computes the sum of the two * overlaid gradients. */ static void _computeGradientSum( _KLT_FloatImage gradx1, /* gradient images */ _KLT_FloatImage grady1, _KLT_FloatImage gradx2, _KLT_FloatImage grady2, float x1, float y1, /* center of window in 1st img */ float x2, float y2, /* center of window in 2nd img */ int width, int height, /* size of window */ _FloatWindow gradx, /* output */ _FloatWindow grady) /* " */ { register int hw = width/2, hh = height/2; float g1, g2; register int i, j; /* Compute values */ for (j = -hh ; j <= hh ; j++) for (i = -hw ; i <= hw ; i++) { g1 = _interpolate(x1+i, y1+j, gradx1); g2 = _interpolate(x2+i, y2+j, gradx2); *gradx++ = g1 + g2; g1 = _interpolate(x1+i, y1+j, grady1); g2 = _interpolate(x2+i, y2+j, grady2); *grady++ = g1 + g2; } } /********************************************************************* * _computeIntensityDifferenceLightingInsensitive * * Given two images and the window center in both images, * aligns the images wrt the window and computes the difference * between the two overlaid images; normalizes for overall gain and bias. */ static void _computeIntensityDifferenceLightingInsensitive( _KLT_FloatImage img1, /* images */ _KLT_FloatImage img2, float x1, float y1, /* center of window in 1st img */ float x2, float y2, /* center of window in 2nd img */ int width, int height, /* size of window */ _FloatWindow imgdiff) /* output */ { register int hw = width/2, hh = height/2; float g1, g2, sum1_squared = 0, sum2_squared = 0; register int i, j; float sum1 = 0, sum2 = 0; float mean1, mean2,alpha,belta; /* Compute values */ for (j = -hh ; j <= hh ; j++) for (i = -hw ; i <= hw ; i++) { g1 = _interpolate(x1+i, y1+j, img1); g2 = _interpolate(x2+i, y2+j, img2); sum1 += g1; sum2 += g2; sum1_squared += g1*g1; sum2_squared += g2*g2; } mean1=sum1_squared/(width*height); mean2=sum2_squared/(width*height); alpha = (float) sqrt(mean1/mean2); mean1=sum1/(width*height); mean2=sum2/(width*height); belta = mean1-alpha*mean2; for (j = -hh ; j <= hh ; j++) for (i = -hw ; i <= hw ; i++) { g1 = _interpolate(x1+i, y1+j, img1); g2 = _interpolate(x2+i, y2+j, img2); *imgdiff++ = g1- g2*alpha-belta; } } /********************************************************************* * _computeGradientSumLightingInsensitive * * Given two gradients and the window center in both images, * aligns the gradients wrt the window and computes the sum of the two * overlaid gradients; normalizes for overall gain and bias. */ static void _computeGradientSumLightingInsensitive( _KLT_FloatImage gradx1, /* gradient images */ _KLT_FloatImage grady1, _KLT_FloatImage gradx2, _KLT_FloatImage grady2, _KLT_FloatImage img1, /* images */ _KLT_FloatImage img2, float x1, float y1, /* center of window in 1st img */ float x2, float y2, /* center of window in 2nd img */ int width, int height, /* size of window */ _FloatWindow gradx, /* output */ _FloatWindow grady) /* " */ { register int hw = width/2, hh = height/2; float g1, g2, sum1_squared = 0, sum2_squared = 0; register int i, j; float sum1 = 0, sum2 = 0; float mean1, mean2, alpha; for (j = -hh ; j <= hh ; j++) for (i = -hw ; i <= hw ; i++) { g1 = _interpolate(x1+i, y1+j, img1); g2 = _interpolate(x2+i, y2+j, img2); sum1_squared += g1; sum2_squared += g2; } mean1 = sum1_squared/(width*height); mean2 = sum2_squared/(width*height); alpha = (float) sqrt(mean1/mean2); /* Compute values */ for (j = -hh ; j <= hh ; j++) for (i = -hw ; i <= hw ; i++) { g1 = _interpolate(x1+i, y1+j, gradx1); g2 = _interpolate(x2+i, y2+j, gradx2); *gradx++ = g1 + g2*alpha; g1 = _interpolate(x1+i, y1+j, grady1); g2 = _interpolate(x2+i, y2+j, grady2); *grady++ = g1+ g2*alpha; } } /********************************************************************* * _compute2by2GradientMatrix * */ static void _compute2by2GradientMatrix( _FloatWindow gradx, _FloatWindow grady, int width, /* size of window */ int height, float *gxx, /* return values */ float *gxy, float *gyy) { register float gx, gy; register int i; /* Compute values */ *gxx = 0.0; *gxy = 0.0; *gyy = 0.0; for (i = 0 ; i < width * height ; i++) { gx = *gradx++; gy = *grady++; *gxx += gx*gx; *gxy += gx*gy; *gyy += gy*gy; } } /********************************************************************* * _compute2by1ErrorVector * */ static void _compute2by1ErrorVector( _FloatWindow imgdiff, _FloatWindow gradx, _FloatWindow grady, int width, /* size of window */ int height, float step_factor, /* 2.0 comes from equations, 1.0 seems to avoid overshooting */ float *ex, /* return values */ float *ey) { register float diff; register int i; /* Compute v