From 751dbfd408520a16cdbf39a27789790c585dc2f1 Mon Sep 17 00:00:00 2001 From: Oliver Duis Date: Sat, 12 Mar 2011 18:45:44 +0100 Subject: [PATCH] Exprimental feature auto lens distortion correction, on behalf of Guokai; see issue #576 --- AUTHORS.txt | 1 + rtdata/languages/default | 2 + rtengine/CMakeLists.txt | 6 +- rtengine/calc_distort.c | 228 +++++ rtengine/calc_distort.h | 6 + rtengine/improcfun.cc | 57 ++ rtengine/improcfun.h | 1 + rtengine/klt/README.txt | 35 + rtengine/klt/base.h | 38 + rtengine/klt/convolve.c | 317 ++++++ rtengine/klt/convolve.h | 32 + rtengine/klt/error.c | 56 ++ rtengine/klt/error.h | 15 + rtengine/klt/klt.c | 531 ++++++++++ rtengine/klt/klt.h | 244 +++++ rtengine/klt/klt_util.c | 165 ++++ rtengine/klt/klt_util.h | 37 + rtengine/klt/main.cpp | 30 + rtengine/klt/pnmio.c | 333 +++++++ rtengine/klt/pnmio.h | 56 ++ rtengine/klt/pyramid.c | 143 +++ rtengine/klt/pyramid.h | 32 + rtengine/klt/selectGoodFeatures.c | 543 ++++++++++ rtengine/klt/speed.txt | 12 + rtengine/klt/storeFeatures.c | 117 +++ rtengine/klt/trackFeatures.c | 1531 +++++++++++++++++++++++++++++ rtengine/klt/writeFeatures.c | 743 ++++++++++++++ rtengine/myfile.h | 4 + rtengine/procevents.h | 3 +- rtengine/refreshmap.cc | 3 +- rtengine/rtthumbnail.cc | 95 ++ rtengine/rtthumbnail.h | 1 + rtgui/batchtoolpanelcoord.cc | 6 + rtgui/distortion.cc | 31 +- rtgui/distortion.h | 6 + rtgui/lensgeomlistener.h | 1 + rtgui/toolpanelcoord.cc | 8 + rtgui/toolpanelcoord.h | 1 + 38 files changed, 5463 insertions(+), 7 deletions(-) create mode 100644 rtengine/calc_distort.c create mode 100644 rtengine/calc_distort.h create mode 100644 rtengine/klt/README.txt create mode 100644 rtengine/klt/base.h create mode 100644 rtengine/klt/convolve.c create mode 100644 rtengine/klt/convolve.h create mode 100644 rtengine/klt/error.c create mode 100644 rtengine/klt/error.h create mode 100644 rtengine/klt/klt.c create mode 100644 rtengine/klt/klt.h create mode 100644 rtengine/klt/klt_util.c create mode 100644 rtengine/klt/klt_util.h create mode 100644 rtengine/klt/main.cpp create mode 100644 rtengine/klt/pnmio.c create mode 100644 rtengine/klt/pnmio.h create mode 100644 rtengine/klt/pyramid.c create mode 100644 rtengine/klt/pyramid.h create mode 100644 rtengine/klt/selectGoodFeatures.c create mode 100644 rtengine/klt/speed.txt create mode 100644 rtengine/klt/storeFeatures.c create mode 100644 rtengine/klt/trackFeatures.c create mode 100644 rtengine/klt/writeFeatures.c diff --git a/AUTHORS.txt b/AUTHORS.txt index 4bf49567f..577f7839b 100644 --- a/AUTHORS.txt +++ b/AUTHORS.txt @@ -9,6 +9,7 @@ Developement contributors, in last name alphabetical order: Michael Ezra Jean-Christophe Frisch Steve Herrell + Guokai Ma Emil Martinec Wyatt Olson Jacek Poplawski diff --git a/rtdata/languages/default b/rtdata/languages/default index 07e9ea372..2ad99a4e1 100644 --- a/rtdata/languages/default +++ b/rtdata/languages/default @@ -700,6 +700,8 @@ TP_DIRPYREQUALIZER_LUMACONTRAST_PLUS;Contrast+ TP_DIRPYREQUALIZER_LUMAFINEST;Finest TP_DIRPYREQUALIZER_LUMANEUTRAL;Neutral TP_DIRPYREQUALIZER_THRESHOLD;Threshold +TP_DISTORTION_AUTO;Auto distortion correction +TP_DISTORTION_AUTO_TIP;(Exprimental) Correct lens distortion automatically for some cameras (M4/3, some compact DC, etc.) TP_DISTORTION_AMOUNT;Amount TP_DISTORTION_LABEL;Distortion TP_EQUALIZER_CONTRAST_MINUS;Contrast- diff --git a/rtengine/CMakeLists.txt b/rtengine/CMakeLists.txt index e9841ef61..ab8332de8 100644 --- a/rtengine/CMakeLists.txt +++ b/rtengine/CMakeLists.txt @@ -15,7 +15,11 @@ set (RTENGINESOURCEFILES safegtk.cc colortemp.cc curves.cc dcraw.cc iccstore.cc iplab2rgb.cc ipsharpen.cc iptransform.cc ipresize.cc jpeg_memsrc.c jdatasrc.c PF_correct_RT.cc - wavelet_dec.cc ipequalizer.cc dirpyrLab_denoise.cc dirpyrLab_equalizer.cc dirpyr_equalizer.cc) + wavelet_dec.cc ipequalizer.cc dirpyrLab_denoise.cc dirpyrLab_equalizer.cc dirpyr_equalizer.cc + calc_distort.c + klt/convolve.c klt/error.c klt/klt.c klt/klt_util.c klt/pnmio.c klt/pyramid.c klt/selectGoodFeatures.c + klt/storeFeatures.c klt/trackFeatures.c klt/writeFeatures.c + ) add_library (rtengine ${RTENGINESOURCEFILES}) #It may be nice to store library version too diff --git a/rtengine/calc_distort.c b/rtengine/calc_distort.c new file mode 100644 index 000000000..d3b8e2e94 --- /dev/null +++ b/rtengine/calc_distort.c @@ -0,0 +1,228 @@ +/********************************************************************** +Finds the N_FEATURES best features in an image, and tracks these +features to the next image. Saves the feature +locations (before and after tracking) to text files and to PPM files, +and prints the features to the screen. +**********************************************************************/ + +#include "klt/pnmio.h" +#include "klt/klt.h" +#include "math.h" + +#define N_FEATURES 100 +#define DELTA_1 0.05 +#define DELTA_2 0.01 +#define RXY_LIMIT 0.6 +#define CENTER_R 0.3 + +//#define DEBUG_IMG + +#ifdef DEBUG_IMG +void drawDotXY(unsigned char* img, int ncols, int nrows, int x, int y, int color) +{ + img[x+y*ncols] = color; +} + +void drawDot(unsigned char* img, int ncols, int nrows, double r0, double r10, int color) +{ + if (r0>=0 && r0<1 && r10 >=0.8 && r10 <1.2) + drawDotXY (img, ncols, nrows, (int)(r0*ncols), (int)((r10-0.8)*2.5*nrows), color); +} +#endif + +double calcDistortion(unsigned char* img1, unsigned char* img2, int ncols, int nrows) +{ + KLT_TrackingContext tc; + KLT_FeatureList fl; + KLT_FeatureTable ft; + int nFeatures = N_FEATURES; + int i,n; + double radius, wc, hc; + + double r0[N_FEATURES] = {0.0}; + double r10[N_FEATURES] = {0.0}; + + tc = KLTCreateTrackingContext(); + //tc->mindist = 20; + tc->lighting_insensitive = TRUE; + tc->nSkippedPixels = 5; + tc->step_factor = 2.0; + tc->max_iterations = 20; + //KLTPrintTrackingContext(tc); + fl = KLTCreateFeatureList(N_FEATURES); + ft = KLTCreateFeatureTable(2, N_FEATURES); + + radius = sqrt(ncols*ncols+nrows*nrows)/2.0; + wc=((double)ncols)/2.0-0.5; + hc=((double)nrows)/2.0-0.5; + + KLTSelectGoodFeatures(tc, img1, ncols, nrows, fl); + KLTStoreFeatureList(fl, ft, 0); + + KLTTrackFeatures(tc, img1, img2, ncols, nrows, fl); + KLTStoreFeatureList(fl, ft, 1); + + // add a shade to img2, we want to draw something on top of it. + for (i=0;ifeature[i][1]->val>=0) { + double x0,y0,x1,y1; + x0=ft->feature[i][0]->x; + y0=ft->feature[i][0]->y; + x1=ft->feature[i][1]->x; + y1=ft->feature[i][1]->y; + + r0[n]=sqrt((x0-wc)*(x0-wc)+(y0-hc)*(y0-hc))/radius; + // dots too close to the center tends to have big diviation and create noise, extract them + if (r0[n]feature[i][0]->x=-1.0; + ft->feature[i][0]->y=-1.0; + } + } + + if (n < 5) { + printf ("Not sufficient features.\n"); + return 0.0; + } + + double avg_r10 = total_r10 / n; + double avg_r0 = total_r0 / n; + double Sxx = 0.0; + double Sxy = 0.0; + double Syy = 0.0; + + for (i=0;i= 0 ? delta : -delta; + +#ifdef DEBUG_IMG + drawDot(img2, ncols, nrows, r0[i], r10[i], 255); +#endif + + if (delta >= DELTA_1) { + total_r10 -= r10[i]; + total_r0 -= r0[i]; + r0[i] = -1.0; + new_n--; + } + + total_delta += delta; + } + + printf ("distortion amount=%lf scale=%lf deviation=%lf, rxy=%lf\n", a, b, total_delta/n, rxy); + + if (new_n < 5) { + printf ("Not sufficient features.\n"); + return 0.0; + } + + printf ("Removed %d outstading data points\n", n-new_n); + avg_r10 = total_r10 / new_n; + avg_r0 = total_r0 / new_n; + Sxx = 0.0; + Sxy = 0.0; + Syy = 0.0; + + for (i=0;i=0.8 && val <1.2) { + if (img2[i+((int)((val-0.8)*2.5*nrows))*ncols] != 255) + img2[i+((int)((val-0.8)*2.5*nrows))*ncols] = 0; + } + val += DELTA_1; + if (val >=0.8 && val <1.2) { + if (img2[i+((int)((val-0.8)*2.5*nrows))*ncols] != 255) + img2[i+((int)((val-0.8)*2.5*nrows))*ncols] = 8; + } + val -= DELTA_1*2; + if (val >=0.8 && val <1.2) { + if (img2[i+((int)((val-0.8)*2.5*nrows))*ncols] != 255) + img2[i+((int)((val-0.8)*2.5*nrows))*ncols] = 8; + } + val += DELTA_1+DELTA_2; + if (val >=0.8 && val <1.2) { + if (img2[i+((int)((val-0.8)*2.5*nrows))*ncols] != 255) + img2[i+((int)((val-0.8)*2.5*nrows))*ncols] = 16; + } + val -= DELTA_2*2; + if (val >=0.8 && val <1.2) { + if (img2[i+((int)((val-0.8)*2.5*nrows))*ncols] != 255) + img2[i+((int)((val-0.8)*2.5*nrows))*ncols] = 16; + } + } + + KLTExtractFeatureList(fl, ft, 0); + KLTWriteFeatureListToPPM(fl,img1,ncols,nrows,"/tmp/feat0.ppm"); + KLTExtractFeatureList(fl, ft, 1); + KLTWriteFeatureListToPPM(fl,img2,ncols,nrows,"/tmp/feat1.ppm"); +#endif + + // calculate deviation + for (i=0;i= 0 ? delta : -delta; + total_delta += delta; + } + + printf ("distortion amount=%lf scale=%lf deviation=%lf, rxy=%lf\n", a, b, total_delta/n, rxy); + + if (total_delta / new_n > DELTA_2) { + printf ("Deviation is too big.\n"); + return 0.0; + } + + if (rxy < RXY_LIMIT) { + printf ("Not linear enough\n"); + return 0.0; + } + + printf ("distortion amount=%lf scale=%lf deviation=%lf, rxy=%lf\n", a, b, total_delta/n, rxy); + return a; +} + diff --git a/rtengine/calc_distort.h b/rtengine/calc_distort.h new file mode 100644 index 000000000..0e869d65e --- /dev/null +++ b/rtengine/calc_distort.h @@ -0,0 +1,6 @@ +#ifndef CALC_DISTORTION__H +#define CALC_DISTORTION__H +extern "C" { + double calcDistortion (unsigned char* img1, unsigned char* img2, int ncols, int nrows); +} +#endif diff --git a/rtengine/improcfun.cc b/rtengine/improcfun.cc index 2f6fc4f88..2c9df8bc8 100644 --- a/rtengine/improcfun.cc +++ b/rtengine/improcfun.cc @@ -29,6 +29,8 @@ #include #include #include +#include +#include #ifdef _OPENMP #include @@ -682,6 +684,61 @@ void ImProcFunctions::getAutoExp (unsigned int* histogram, int histcompr, doubl //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +#include "calc_distort.h" + +double ImProcFunctions::getAutoDistor (const Glib::ustring &fname, int thumb_size) { + if (fname != "") { + rtengine::RawMetaDataLocation ri; + int w_raw=-1, h_raw=thumb_size; + int w_thumb=-1, h_thumb=thumb_size; + + Thumbnail* thumb = rtengine::Thumbnail::loadQuickFromRaw (fname, ri, w_thumb, h_thumb, 1); + if (thumb == NULL) + return 0.0; + + Thumbnail* raw = rtengine::Thumbnail::loadFromRaw (fname, ri, w_raw, h_raw, 1); + if (raw == NULL) { + delete thumb; + return 0.0; + } + + if (h_thumb != h_raw) { + delete thumb; + delete raw; + return 0.0; + } + + int width; + + if (w_thumb > w_raw) + width = w_raw; + else + width = w_thumb; + + unsigned char* thumbGray; + unsigned char* rawGray; + thumbGray = thumb->getGrayscaleHistEQ (width); + rawGray = raw->getGrayscaleHistEQ (width); + + if (thumbGray == NULL || rawGray == NULL) { + if (thumbGray) delete thumbGray; + if (rawGray) delete rawGray; + delete thumb; + delete raw; + return 0.0; + } + + double dist_amount = calcDistortion (thumbGray, rawGray, width, h_thumb); + delete thumbGray; + delete rawGray; + delete thumb; + delete raw; + return dist_amount; + } + else + return 0.0; +} + void ImProcFunctions::rgb2hsv (int r, int g, int b, float &h, float &s, float &v) { double var_R = r / 65535.0; diff --git a/rtengine/improcfun.h b/rtengine/improcfun.h index 7c1934e85..fa04b8a6e 100644 --- a/rtengine/improcfun.h +++ b/rtengine/improcfun.h @@ -120,6 +120,7 @@ class ImProcFunctions { bool transCoord (int W, int H, int x, int y, int w, int h, int& xv, int& yv, int& wv, int& hv, double ascaleDef = -1); bool transCoord (int W, int H, std::vector &src, std::vector &red, std::vector &green, std::vector &blue, double ascaleDef = -1); void getAutoExp (unsigned int* histogram, int histcompr, double expcomp, double clip, double& br, int& bl); + static double getAutoDistor (const Glib::ustring& fname, int thumb_size); double getTransformAutoFill (int oW, int oH); void rgb2hsv (int r, int g, int b, float &h, float &s, float &v); diff --git a/rtengine/klt/README.txt b/rtengine/klt/README.txt new file mode 100644 index 000000000..59ebb44c2 --- /dev/null +++ b/rtengine/klt/README.txt @@ -0,0 +1,35 @@ +********************************************************************** +NOTICE: + +This code is now in the public domain. The Stanford Office of +Technology Licensing has removed all licensing restrictions. + +********************************************************************** + +KLT +An implementation of the Kanade-Lucas-Tomasi feature tracker + +Version 1.3.4 + +Authors: Stan Birchfield + stb@clemson.edu + + Thorsten Thormaehlen + thormae@tnt.uni-hannover.de + (implemented affine code) + + Thanks to many others for various bug fixes. + +Date: August 30, A.D. 2007 + May 10, A.D. 2007 + March 28, A.D. 2006 + November 21, A.D. 2005 + August 17, A.D. 2005 + June 16, A.D. 2004 + October 7, A.D. 1998 + +The code can be obtained from http://www.ces.clemson.edu/~stb/klt +(alternatively http://www.vision.stanford.edu/~birch/klt), +where the official manuals reside. For your convenience, unofficial +manuals have been placed in the current subdirectory 'doc'. + diff --git a/rtengine/klt/base.h b/rtengine/klt/base.h new file mode 100644 index 000000000..35d709447 --- /dev/null +++ b/rtengine/klt/base.h @@ -0,0 +1,38 @@ +/********************************************************************* + * base.h + *********************************************************************/ + +#ifndef _BASE_H_ +#define _BASE_H_ + +#ifndef uchar +#define uchar unsigned char +#endif + +#ifndef schar +#define schar signed char +#endif + +#ifndef uint +#define uint unsigned int +#endif + +#ifndef ushort +#define ushort unsigned short +#endif + +#ifndef ulong +#define ulong unsigned long +#endif + +#ifndef max +#define max(a,b) ((a) > (b) ? (a) : (b)) +#endif +#ifndef min +#define min(a,b) ((a) < (b) ? (a) : (b)) +#endif +#define max3(a,b,c) ((a) > (b) ? max((a),(c)) : max((b),(c))) +#define min3(a,b,c) ((a) < (b) ? min((a),(c)) : min((b),(c))) + +#endif + diff --git a/rtengine/klt/convolve.c b/rtengine/klt/convolve.c new file mode 100644 index 000000000..e64b93f74 --- /dev/null +++ b/rtengine/klt/convolve.c @@ -0,0 +1,317 @@ +/********************************************************************* + * convolve.c + *********************************************************************/ + +/* Standard includes */ +#include +#include +#include /* malloc(), realloc() */ + +/* Our includes */ +#include "base.h" +#include "error.h" +#include "convolve.h" +#include "klt_util.h" /* printing */ + +#define MAX_KERNEL_WIDTH 71 + + +typedef struct { + int width; + float data[MAX_KERNEL_WIDTH]; +} ConvolutionKernel; + +/* Kernels */ +static ConvolutionKernel gauss_kernel; +static ConvolutionKernel gaussderiv_kernel; +static float sigma_last = -10.0; + + +/********************************************************************* + * _KLTToFloatImage + * + * Given a pointer to image data (probably unsigned chars), copy + * data to a float image. + */ + +void _KLTToFloatImage( + KLT_PixelType *img, + int ncols, int nrows, + _KLT_FloatImage floatimg) +{ + KLT_PixelType *ptrend = img + ncols*nrows; + float *ptrout = floatimg->data; + + /* Output image must be large enough to hold result */ + assert(floatimg->ncols >= ncols); + assert(floatimg->nrows >= nrows); + + floatimg->ncols = ncols; + floatimg->nrows = nrows; + + while (img < ptrend) *ptrout++ = (float) *img++; +} + + +/********************************************************************* + * _computeKernels + */ + +static void _computeKernels( + float sigma, + ConvolutionKernel *gauss, + ConvolutionKernel *gaussderiv) +{ + const float factor = 0.01f; /* for truncating tail */ + int i; + + assert(MAX_KERNEL_WIDTH % 2 == 1); + assert(sigma >= 0.0); + + /* Compute kernels, and automatically determine widths */ + { + const int hw = MAX_KERNEL_WIDTH / 2; + float max_gauss = 1.0f, max_gaussderiv = (float) (sigma*exp(-0.5f)); + + /* Compute gauss and deriv */ + for (i = -hw ; i <= hw ; i++) { + gauss->data[i+hw] = (float) exp(-i*i / (2*sigma*sigma)); + gaussderiv->data[i+hw] = -i * gauss->data[i+hw]; + } + + /* Compute widths */ + gauss->width = MAX_KERNEL_WIDTH; + for (i = -hw ; fabs(gauss->data[i+hw] / max_gauss) < factor ; + i++, gauss->width -= 2); + gaussderiv->width = MAX_KERNEL_WIDTH; + for (i = -hw ; fabs(gaussderiv->data[i+hw] / max_gaussderiv) < factor ; + i++, gaussderiv->width -= 2); + if (gauss->width == MAX_KERNEL_WIDTH || + gaussderiv->width == MAX_KERNEL_WIDTH) + KLTError("(_computeKernels) MAX_KERNEL_WIDTH %d is too small for " + "a sigma of %f", MAX_KERNEL_WIDTH, sigma); + } + + /* Shift if width less than MAX_KERNEL_WIDTH */ + for (i = 0 ; i < gauss->width ; i++) + gauss->data[i] = gauss->data[i+(MAX_KERNEL_WIDTH-gauss->width)/2]; + for (i = 0 ; i < gaussderiv->width ; i++) + gaussderiv->data[i] = gaussderiv->data[i+(MAX_KERNEL_WIDTH-gaussderiv->width)/2]; + /* Normalize gauss and deriv */ + { + const int hw = gaussderiv->width / 2; + float den; + + den = 0.0; + for (i = 0 ; i < gauss->width ; i++) den += gauss->data[i]; + for (i = 0 ; i < gauss->width ; i++) gauss->data[i] /= den; + den = 0.0; + for (i = -hw ; i <= hw ; i++) den -= i*gaussderiv->data[i+hw]; + for (i = -hw ; i <= hw ; i++) gaussderiv->data[i+hw] /= den; + } + + sigma_last = sigma; +} + + +/********************************************************************* + * _KLTGetKernelWidths + * + */ + +void _KLTGetKernelWidths( + float sigma, + int *gauss_width, + int *gaussderiv_width) +{ + _computeKernels(sigma, &gauss_kernel, &gaussderiv_kernel); + *gauss_width = gauss_kernel.width; + *gaussderiv_width = gaussderiv_kernel.width; +} + + +/********************************************************************* + * _convolveImageHoriz + */ + +static void _convolveImageHoriz( + _KLT_FloatImage imgin, + ConvolutionKernel kernel, + _KLT_FloatImage imgout) +{ + float *ptrrow = imgin->data; /* Points to row's first pixel */ + register float *ptrout = imgout->data, /* Points to next output pixel */ + *ppp; + register float sum; + register int radius = kernel.width / 2; + register int ncols = imgin->ncols, nrows = imgin->nrows; + register int i, j, k; + + /* Kernel width must be odd */ + assert(kernel.width % 2 == 1); + + /* Must read from and write to different images */ + assert(imgin != imgout); + + /* Output image must be large enough to hold result */ + assert(imgout->ncols >= imgin->ncols); + assert(imgout->nrows >= imgin->nrows); + + /* For each row, do ... */ + for (j = 0 ; j < nrows ; j++) { + + /* Zero leftmost columns */ + for (i = 0 ; i < radius ; i++) + *ptrout++ = 0.0; + + /* Convolve middle columns with kernel */ + for ( ; i < ncols - radius ; i++) { + ppp = ptrrow + i - radius; + sum = 0.0; + for (k = kernel.width-1 ; k >= 0 ; k--) + sum += *ppp++ * kernel.data[k]; + *ptrout++ = sum; + } + + /* Zero rightmost columns */ + for ( ; i < ncols ; i++) + *ptrout++ = 0.0; + + ptrrow += ncols; + } +} + + +/********************************************************************* + * _convolveImageVert + */ + +static void _convolveImageVert( + _KLT_FloatImage imgin, + ConvolutionKernel kernel, + _KLT_FloatImage imgout) +{ + float *ptrcol = imgin->data; /* Points to row's first pixel */ + register float *ptrout = imgout->data, /* Points to next output pixel */ + *ppp; + register float sum; + register int radius = kernel.width / 2; + register int ncols = imgin->ncols, nrows = imgin->nrows; + register int i, j, k; + + /* Kernel width must be odd */ + assert(kernel.width % 2 == 1); + + /* Must read from and write to different images */ + assert(imgin != imgout); + + /* Output image must be large enough to hold result */ + assert(imgout->ncols >= imgin->ncols); + assert(imgout->nrows >= imgin->nrows); + + /* For each column, do ... */ + for (i = 0 ; i < ncols ; i++) { + + /* Zero topmost rows */ + for (j = 0 ; j < radius ; j++) { + *ptrout = 0.0; + ptrout += ncols; + } + + /* Convolve middle rows with kernel */ + for ( ; j < nrows - radius ; j++) { + ppp = ptrcol + ncols * (j - radius); + sum = 0.0; + for (k = kernel.width-1 ; k >= 0 ; k--) { + sum += *ppp * kernel.data[k]; + ppp += ncols; + } + *ptrout = sum; + ptrout += ncols; + } + + /* Zero bottommost rows */ + for ( ; j < nrows ; j++) { + *ptrout = 0.0; + ptrout += ncols; + } + + ptrcol++; + ptrout -= nrows * ncols - 1; + } +} + + +/********************************************************************* + * _convolveSeparate + */ + +static void _convolveSeparate( + _KLT_FloatImage imgin, + ConvolutionKernel horiz_kernel, + ConvolutionKernel vert_kernel, + _KLT_FloatImage imgout) +{ + /* Create temporary image */ + _KLT_FloatImage tmpimg; + tmpimg = _KLTCreateFloatImage(imgin->ncols, imgin->nrows); + + /* Do convolution */ + _convolveImageHoriz(imgin, horiz_kernel, tmpimg); + + _convolveImageVert(tmpimg, vert_kernel, imgout); + + /* Free memory */ + _KLTFreeFloatImage(tmpimg); +} + + +/********************************************************************* + * _KLTComputeGradients + */ + +void _KLTComputeGradients( + _KLT_FloatImage img, + float sigma, + _KLT_FloatImage gradx, + _KLT_FloatImage grady) +{ + + /* Output images must be large enough to hold result */ + assert(gradx->ncols >= img->ncols); + assert(gradx->nrows >= img->nrows); + assert(grady->ncols >= img->ncols); + assert(grady->nrows >= img->nrows); + + /* Compute kernels, if necessary */ + if (fabs(sigma - sigma_last) > 0.05) + _computeKernels(sigma, &gauss_kernel, &gaussderiv_kernel); + + _convolveSeparate(img, gaussderiv_kernel, gauss_kernel, gradx); + _convolveSeparate(img, gauss_kernel, gaussderiv_kernel, grady); + +} + + +/********************************************************************* + * _KLTComputeSmoothedImage + */ + +void _KLTComputeSmoothedImage( + _KLT_FloatImage img, + float sigma, + _KLT_FloatImage smooth) +{ + /* Output image must be large enough to hold result */ + assert(smooth->ncols >= img->ncols); + assert(smooth->nrows >= img->nrows); + + /* Compute kernel, if necessary; gauss_deriv is not used */ + if (fabs(sigma - sigma_last) > 0.05) + _computeKernels(sigma, &gauss_kernel, &gaussderiv_kernel); + + _convolveSeparate(img, gauss_kernel, gauss_kernel, smooth); +} + + + diff --git a/rtengine/klt/convolve.h b/rtengine/klt/convolve.h new file mode 100644 index 000000000..6043a7e25 --- /dev/null +++ b/rtengine/klt/convolve.h @@ -0,0 +1,32 @@ +/********************************************************************* + * convolve.h + *********************************************************************/ + +#ifndef _CONVOLVE_H_ +#define _CONVOLVE_H_ + +#include "klt.h" +#include "klt_util.h" + +void _KLTToFloatImage( + KLT_PixelType *img, + int ncols, int nrows, + _KLT_FloatImage floatimg); + +void _KLTComputeGradients( + _KLT_FloatImage img, + float sigma, + _KLT_FloatImage gradx, + _KLT_FloatImage grady); + +void _KLTGetKernelWidths( + float sigma, + int *gauss_width, + int *gaussderiv_width); + +void _KLTComputeSmoothedImage( + _KLT_FloatImage img, + float sigma, + _KLT_FloatImage smooth); + +#endif diff --git a/rtengine/klt/error.c b/rtengine/klt/error.c new file mode 100644 index 000000000..0731cf2cb --- /dev/null +++ b/rtengine/klt/error.c @@ -0,0 +1,56 @@ +/********************************************************************* + * error.c + * + * Error and warning messages, and system commands. + *********************************************************************/ + + +/* Standard includes */ +#include +#include +#include + + +/********************************************************************* + * KLTError + * + * Prints an error message and dies. + * + * INPUTS + * exactly like printf + */ + +void KLTError(char *fmt, ...) +{ + va_list args; + + va_start(args, fmt); + fprintf(stderr, "KLT Error: "); + vfprintf(stderr, fmt, args); + fprintf(stderr, "\n"); + va_end(args); + exit(1); +} + + +/********************************************************************* + * KLTWarning + * + * Prints a warning message. + * + * INPUTS + * exactly like printf + */ + +void KLTWarning(char *fmt, ...) +{ + va_list args; + + va_start(args, fmt); + fprintf(stderr, "KLT Warning: "); + vfprintf(stderr, fmt, args); + fprintf(stderr, "\n"); + fflush(stderr); + va_end(args); +} + diff --git a/rtengine/klt/error.h b/rtengine/klt/error.h new file mode 100644 index 000000000..4dac13651 --- /dev/null +++ b/rtengine/klt/error.h @@ -0,0 +1,15 @@ +/********************************************************************* + * error.h + *********************************************************************/ + +#ifndef _ERROR_H_ +#define _ERROR_H_ + +#include +#include + +void KLTError(char *fmt, ...); +void KLTWarning(char *fmt, ...); + +#endif + diff --git a/rtengine/klt/klt.c b/rtengine/klt/klt.c new file mode 100644 index 000000000..66b69070f --- /dev/null +++ b/rtengine/klt/klt.c @@ -0,0 +1,531 @@ +/********************************************************************* + * klt.c + * + * Kanade-Lucas-Tomasi tracker + *********************************************************************/ + +/* Standard includes */ +#include +#include /* logf() */ +#include /* malloc() */ + +/* Our includes */ +#include "base.h" +#include "convolve.h" +#include "error.h" +#include "klt.h" +#include "pyramid.h" + + +static const int mindist = 10; +static const int window_size = 7; +static const int min_eigenvalue = 1; +static const float min_determinant = 0.01f; +static const float min_displacement = 0.1f; +static const int max_iterations = 10; +static const float max_residue = 10.0f; +static const float grad_sigma = 1.0f; +static const float smooth_sigma_fact = 0.1f; +static const float pyramid_sigma_fact = 0.9f; +static const float step_factor = 1.0f; +static const KLT_BOOL sequentialMode = FALSE; +static const KLT_BOOL lighting_insensitive = FALSE; +/* for affine mapping*/ +static const int affineConsistencyCheck = -1; +static const int affine_window_size = 15; +static const int affine_max_iterations = 10; +static const float affine_max_residue = 10.0; +static const float affine_min_displacement = 0.02f; +static const float affine_max_displacement_differ = 1.5f; + +static const KLT_BOOL smoothBeforeSelecting = TRUE; +static const KLT_BOOL writeInternalImages = FALSE; +static const int search_range = 15; +static const int nSkippedPixels = 0; + +extern int KLT_verbose; + + +/********************************************************************* + * _createArray2D + * + * Creates a two-dimensional array. + * + * INPUTS + * ncols: no. of columns + * nrows: no. of rows + * nbytes: no. of bytes per entry + * + * RETURNS + * Pointer to an array. Must be coerced. + * + * EXAMPLE + * char **ar; + * ar = (char **) createArray2D(8, 5, sizeof(char)); + */ + +static void** _createArray2D(int ncols, int nrows, int nbytes) +{ + char **tt; + int i; + + tt = (char **) malloc(nrows * sizeof(void *) + + ncols * nrows * nbytes); + if (tt == NULL) + KLTError("(createArray2D) Out of memory"); + + for (i = 0 ; i < nrows ; i++) + tt[i] = ((char *) tt) + (nrows * sizeof(void *) + + i * ncols * nbytes); + + return((void **) tt); +} + + +/********************************************************************* + * KLTCreateTrackingContext + * + */ + +KLT_TrackingContext KLTCreateTrackingContext() +{ + KLT_TrackingContext tc; + + /* Allocate memory */ + tc = (KLT_TrackingContext) malloc(sizeof(KLT_TrackingContextRec)); + + /* Set values to default values */ + tc->mindist = mindist; + tc->window_width = window_size; + tc->window_height = window_size; + tc->sequentialMode = sequentialMode; + tc->smoothBeforeSelecting = smoothBeforeSelecting; + tc->writeInternalImages = writeInternalImages; + tc->lighting_insensitive = lighting_insensitive; + tc->min_eigenvalue = min_eigenvalue; + tc->min_determinant = min_determinant; + tc->max_iterations = max_iterations; + tc->min_displacement = min_displacement; + tc->max_residue = max_residue; + tc->grad_sigma = grad_sigma; + tc->smooth_sigma_fact = smooth_sigma_fact; + tc->pyramid_sigma_fact = pyramid_sigma_fact; + tc->step_factor = step_factor; + tc->nSkippedPixels = nSkippedPixels; + tc->pyramid_last = NULL; + tc->pyramid_last_gradx = NULL; + tc->pyramid_last_grady = NULL; + /* for affine mapping */ + tc->affineConsistencyCheck = affineConsistencyCheck; + tc->affine_window_width = affine_window_size; + tc->affine_window_height = affine_window_size; + tc->affine_max_iterations = affine_max_iterations; + tc->affine_max_residue = affine_max_residue; + tc->affine_min_displacement = affine_min_displacement; + tc->affine_max_displacement_differ = affine_max_displacement_differ; + + /* Change nPyramidLevels and subsampling */ + KLTChangeTCPyramid(tc, search_range); + + /* Update border, which is dependent upon */ + /* smooth_sigma_fact, pyramid_sigma_fact, window_size, and subsampling */ + KLTUpdateTCBorder(tc); + + return(tc); +} + + +/********************************************************************* + * KLTCreateFeatureList + * + */ + +KLT_FeatureList KLTCreateFeatureList( + int nFeatures) +{ + KLT_FeatureList fl; + KLT_Feature first; + int nbytes = sizeof(KLT_FeatureListRec) + + nFeatures * sizeof(KLT_Feature) + + nFeatures * sizeof(KLT_FeatureRec); + int i; + + /* Allocate memory for feature list */ + fl = (KLT_FeatureList) malloc(nbytes); + + /* Set parameters */ + fl->nFeatures = nFeatures; + + /* Set pointers */ + fl->feature = (KLT_Feature *) (fl + 1); + first = (KLT_Feature) (fl->feature + nFeatures); + for (i = 0 ; i < nFeatures ; i++) { + fl->feature[i] = first + i; + fl->feature[i]->aff_img = NULL; /* initialization fixed by Sinisa Segvic */ + fl->feature[i]->aff_img_gradx = NULL; + fl->feature[i]->aff_img_grady = NULL; + } + /* Return feature list */ + return(fl); +} + + +/********************************************************************* + * KLTCreateFeatureHistory + * + */ + +KLT_FeatureHistory KLTCreateFeatureHistory( + int nFrames) +{ + KLT_FeatureHistory fh; + KLT_Feature first; + int nbytes = sizeof(KLT_FeatureHistoryRec) + + nFrames * sizeof(KLT_Feature) + + nFrames * sizeof(KLT_FeatureRec); + int i; + + /* Allocate memory for feature history */ + fh = (KLT_FeatureHistory) malloc(nbytes); + + /* Set parameters */ + fh->nFrames = nFrames; + + /* Set pointers */ + fh->feature = (KLT_Feature *) (fh + 1); + first = (KLT_Feature) (fh->feature + nFrames); + for (i = 0 ; i < nFrames ; i++) + fh->feature[i] = first + i; + + /* Return feature history */ + return(fh); +} + + +/********************************************************************* + * KLTCreateFeatureTable + * + */ + +KLT_FeatureTable KLTCreateFeatureTable( + int nFrames, + int nFeatures) +{ + KLT_FeatureTable ft; + KLT_Feature first; + int nbytes = sizeof(KLT_FeatureTableRec); + int i, j; + + /* Allocate memory for feature history */ + ft = (KLT_FeatureTable) malloc(nbytes); + + /* Set parameters */ + ft->nFrames = nFrames; + ft->nFeatures = nFeatures; + + /* Set pointers */ + ft->feature = (KLT_Feature **) + _createArray2D(nFrames, nFeatures, sizeof(KLT_Feature)); + first = (KLT_Feature) malloc(nFrames * nFeatures * sizeof(KLT_FeatureRec)); + for (j = 0 ; j < nFeatures ; j++) + for (i = 0 ; i < nFrames ; i++) + ft->feature[j][i] = first + j*nFrames + i; + + /* Return feature table */ + return(ft); +} + + +/********************************************************************* + * KLTPrintTrackingContext + */ + +void KLTPrintTrackingContext( + KLT_TrackingContext tc) +{ + fprintf(stderr, "\n\nTracking context:\n\n"); + fprintf(stderr, "\tmindist = %d\n", tc->mindist); + fprintf(stderr, "\twindow_width = %d\n", tc->window_width); + fprintf(stderr, "\twindow_height = %d\n", tc->window_height); + fprintf(stderr, "\tsequentialMode = %s\n", + tc->sequentialMode ? "TRUE" : "FALSE"); + fprintf(stderr, "\tsmoothBeforeSelecting = %s\n", + tc->smoothBeforeSelecting ? "TRUE" : "FALSE"); + fprintf(stderr, "\twriteInternalImages = %s\n", + tc->writeInternalImages ? "TRUE" : "FALSE"); + + fprintf(stderr, "\tmin_eigenvalue = %d\n", tc->min_eigenvalue); + fprintf(stderr, "\tmin_determinant = %f\n", tc->min_determinant); + fprintf(stderr, "\tmin_displacement = %f\n", tc->min_displacement); + fprintf(stderr, "\tmax_iterations = %d\n", tc->max_iterations); + fprintf(stderr, "\tmax_residue = %f\n", tc->max_residue); + fprintf(stderr, "\tgrad_sigma = %f\n", tc->grad_sigma); + fprintf(stderr, "\tsmooth_sigma_fact = %f\n", tc->smooth_sigma_fact); + fprintf(stderr, "\tpyramid_sigma_fact = %f\n", tc->pyramid_sigma_fact); + fprintf(stderr, "\tnSkippedPixels = %d\n", tc->nSkippedPixels); + fprintf(stderr, "\tborderx = %d\n", tc->borderx); + fprintf(stderr, "\tbordery = %d\n", tc->bordery); + fprintf(stderr, "\tnPyramidLevels = %d\n", tc->nPyramidLevels); + fprintf(stderr, "\tsubsampling = %d\n", tc->subsampling); + + fprintf(stderr, "\n\tpyramid_last = %s\n", (tc->pyramid_last!=NULL) ? + "points to old image" : "NULL"); + fprintf(stderr, "\tpyramid_last_gradx = %s\n", + (tc->pyramid_last_gradx!=NULL) ? + "points to old image" : "NULL"); + fprintf(stderr, "\tpyramid_last_grady = %s\n", + (tc->pyramid_last_grady!=NULL) ? + "points to old image" : "NULL"); + fprintf(stderr, "\n\n"); +} + + +/********************************************************************* + * KLTChangeTCPyramid + * + */ + +void KLTChangeTCPyramid( + KLT_TrackingContext tc, + int search_range) +{ + float window_halfwidth; + float subsampling; + + /* Check window size (and correct if necessary) */ + if (tc->window_width % 2 != 1) { + tc->window_width = tc->window_width+1; + KLTWarning("(KLTChangeTCPyramid) Window width must be odd. " + "Changing to %d.\n", tc->window_width); + } + if (tc->window_height % 2 != 1) { + tc->window_height = tc->window_height+1; + KLTWarning("(KLTChangeTCPyramid) Window height must be odd. " + "Changing to %d.\n", tc->window_height); + } + if (tc->window_width < 3) { + tc->window_width = 3; + KLTWarning("(KLTChangeTCPyramid) Window width must be at least three. \n" + "Changing to %d.\n", tc->window_width); + } + if (tc->window_height < 3) { + tc->window_height = 3; + KLTWarning("(KLTChangeTCPyramid) Window height must be at least three. \n" + "Changing to %d.\n", tc->window_height); + } + window_halfwidth = min(tc->window_width,tc->window_height)/2.0f; + + subsampling = ((float) search_range) / window_halfwidth; + + if (subsampling < 1.0) { /* 1.0 = 0+1 */ + tc->nPyramidLevels = 1; + } else if (subsampling <= 3.0) { /* 3.0 = 2+1 */ + tc->nPyramidLevels = 2; + tc->subsampling = 2; + } else if (subsampling <= 5.0) { /* 5.0 = 4+1 */ + tc->nPyramidLevels = 2; + tc->subsampling = 4; + } else if (subsampling <= 9.0) { /* 9.0 = 8+1 */ + tc->nPyramidLevels = 2; + tc->subsampling = 8; + } else { + /* The following lines are derived from the formula: + search_range = + window_halfwidth * \sum_{i=0}^{nPyramidLevels-1} 8^i, + which is the same as: + search_range = + window_halfwidth * (8^nPyramidLevels - 1)/(8 - 1). + Then, the value is rounded up to the nearest integer. */ + float val = (float) (log(7.0*subsampling+1.0)/log(8.0)); + tc->nPyramidLevels = (int) (val + 0.99); + tc->subsampling = 8; + } +} + + +/********************************************************************* + * NOTE: Manually must ensure consistency with _KLTComputePyramid() + */ + +static float _pyramidSigma( + KLT_TrackingContext tc) +{ + return (tc->pyramid_sigma_fact * tc->subsampling); +} + + +/********************************************************************* + * Updates border, which is dependent upon + * smooth_sigma_fact, pyramid_sigma_fact, window_size, and subsampling + */ + +void KLTUpdateTCBorder( + KLT_TrackingContext tc) +{ + float val; + int pyramid_gauss_hw; + int smooth_gauss_hw; + int gauss_width, gaussderiv_width; + int num_levels = tc->nPyramidLevels; + int n_invalid_pixels; + int window_hw; + int ss = tc->subsampling; + int ss_power; + int border; + int i; + + /* Check window size (and correct if necessary) */ + if (tc->window_width % 2 != 1) { + tc->window_width = tc->window_width+1; + KLTWarning("(KLTUpdateTCBorder) Window width must be odd. " + "Changing to %d.\n", tc->window_width); + } + if (tc->window_height % 2 != 1) { + tc->window_height = tc->window_height+1; + KLTWarning("(KLTUpdateTCBorder) Window height must be odd. " + "Changing to %d.\n", tc->window_height); + } + if (tc->window_width < 3) { + tc->window_width = 3; + KLTWarning("(KLTUpdateTCBorder) Window width must be at least three. \n" + "Changing to %d.\n", tc->window_width); + } + if (tc->window_height < 3) { + tc->window_height = 3; + KLTWarning("(KLTUpdateTCBorder) Window height must be at least three. \n" + "Changing to %d.\n", tc->window_height); + } + window_hw = max(tc->window_width, tc->window_height)/2; + + /* Find widths of convolution windows */ + _KLTGetKernelWidths(_KLTComputeSmoothSigma(tc), + &gauss_width, &gaussderiv_width); + smooth_gauss_hw = gauss_width/2; + _KLTGetKernelWidths(_pyramidSigma(tc), + &gauss_width, &gaussderiv_width); + pyramid_gauss_hw = gauss_width/2; + + /* Compute the # of invalid pixels at each level of the pyramid. + n_invalid_pixels is computed with respect to the ith level + of the pyramid. So, e.g., if n_invalid_pixels = 5 after + the first iteration, then there are 5 invalid pixels in + level 1, which translated means 5*subsampling invalid pixels + in the original level 0. */ + n_invalid_pixels = smooth_gauss_hw; + for (i = 1 ; i < num_levels ; i++) { + val = ((float) n_invalid_pixels + pyramid_gauss_hw) / ss; + n_invalid_pixels = (int) (val + 0.99); /* Round up */ + } + + /* ss_power = ss^(num_levels-1) */ + ss_power = 1; + for (i = 1 ; i < num_levels ; i++) + ss_power *= ss; + + /* Compute border by translating invalid pixels back into */ + /* original image */ + border = (n_invalid_pixels + window_hw) * ss_power; + + tc->borderx = border; + tc->bordery = border; +} + + +/********************************************************************* + * KLTFreeTrackingContext + * KLTFreeFeatureList + * KLTFreeFeatureHistory + * KLTFreeFeatureTable + */ + +void KLTFreeTrackingContext( + KLT_TrackingContext tc) +{ + if (tc->pyramid_last) + _KLTFreePyramid((_KLT_Pyramid) tc->pyramid_last); + if (tc->pyramid_last_gradx) + _KLTFreePyramid((_KLT_Pyramid) tc->pyramid_last_gradx); + if (tc->pyramid_last_grady) + _KLTFreePyramid((_KLT_Pyramid) tc->pyramid_last_grady); + free(tc); +} + +void KLTFreeFeatureList( + KLT_FeatureList fl) +{ + /* for affine mapping */ + int indx; + for (indx = 0 ; indx < fl->nFeatures ; indx++) { + /* free image and gradient */ + _KLTFreeFloatImage(fl->feature[indx]->aff_img); + _KLTFreeFloatImage(fl->feature[indx]->aff_img_gradx); + _KLTFreeFloatImage(fl->feature[indx]->aff_img_grady); + fl->feature[indx]->aff_img = NULL; + fl->feature[indx]->aff_img_gradx = NULL; + fl->feature[indx]->aff_img_grady = NULL; + } + + free(fl); +} + +void KLTFreeFeatureHistory( + KLT_FeatureHistory fh) +{ + free(fh); +} + +void KLTFreeFeatureTable( + KLT_FeatureTable ft) +{ + free(ft->feature[0][0]); /* this plugs a memory leak found by Stefan Wachter */ + free(ft->feature); + free(ft); +} + + +/********************************************************************* + * KLTStopSequentialMode + */ + +void KLTStopSequentialMode( + KLT_TrackingContext tc) +{ + tc->sequentialMode = FALSE; + _KLTFreePyramid((_KLT_Pyramid) tc->pyramid_last); + _KLTFreePyramid((_KLT_Pyramid) tc->pyramid_last_gradx); + _KLTFreePyramid((_KLT_Pyramid) tc->pyramid_last_grady); + tc->pyramid_last = NULL; + tc->pyramid_last_gradx = NULL; + tc->pyramid_last_grady = NULL; +} + + +/********************************************************************* + * KLTCountRemainingFeatures + */ + +int KLTCountRemainingFeatures( + KLT_FeatureList fl) +{ + int count = 0; + int i; + + for (i = 0 ; i < fl->nFeatures ; i++) + if (fl->feature[i]->val >= 0) + count++; + + return count; +} + +/********************************************************************* + * KLTSetVerbosity + */ + +void KLTSetVerbosity( + int verbosity) +{ + KLT_verbose = verbosity; +} + + + diff --git a/rtengine/klt/klt.h b/rtengine/klt/klt.h new file mode 100644 index 000000000..10f318593 --- /dev/null +++ b/rtengine/klt/klt.h @@ -0,0 +1,244 @@ +/********************************************************************* + * klt.h + * + * Kanade-Lucas-Tomasi tracker + *********************************************************************/ + +#ifndef _KLT_H_ +#define _KLT_H_ + +#ifdef __cplusplus +extern "C" { +#endif + +typedef float KLT_locType; +typedef unsigned char KLT_PixelType; + +#define KLT_BOOL int + +#ifndef TRUE +#define TRUE 1 +#define FALSE 0 +#endif + +#ifndef NULL +#define NULL 0 +#endif + +#define KLT_TRACKED 0 +#define KLT_NOT_FOUND -1 +#define KLT_SMALL_DET -2 +#define KLT_MAX_ITERATIONS -3 +#define KLT_OOB -4 +#define KLT_LARGE_RESIDUE -5 + +#include "klt_util.h" /* for affine mapping */ + +/******************* + * Structures + */ + +typedef struct { + /* Available to user */ + int mindist; /* min distance b/w features */ + int window_width, window_height; + KLT_BOOL sequentialMode; /* whether to save most recent image to save time */ + /* can set to TRUE manually, but don't set to */ + /* FALSE manually */ + KLT_BOOL smoothBeforeSelecting; /* whether to smooth image before */ + /* selecting features */ + KLT_BOOL writeInternalImages; /* whether to write internal images */ + /* tracking features */ + KLT_BOOL lighting_insensitive; /* whether to normalize for gain and bias (not in original algorithm) */ + + /* Available, but hopefully can ignore */ + int min_eigenvalue; /* smallest eigenvalue allowed for selecting */ + float min_determinant; /* th for determining lost */ + float min_displacement; /* th for stopping tracking when pixel changes little */ + int max_iterations; /* th for stopping tracking when too many iterations */ + float max_residue; /* th for stopping tracking when residue is large */ + float grad_sigma; + float smooth_sigma_fact; + float pyramid_sigma_fact; + float step_factor; /* size of Newton steps; 2.0 comes from equations, 1.0 seems to avoid overshooting */ + int nSkippedPixels; /* # of pixels skipped when finding features */ + int borderx; /* border in which features will not be found */ + int bordery; + int nPyramidLevels; /* computed from search_ranges */ + int subsampling; /* " */ + + + /* for affine mapping */ + int affine_window_width, affine_window_height; + int affineConsistencyCheck; /* whether to evaluates the consistency of features with affine mapping + -1 = don't evaluates the consistency + 0 = evaluates the consistency of features with translation mapping + 1 = evaluates the consistency of features with similarity mapping + 2 = evaluates the consistency of features with affine mapping + */ + int affine_max_iterations; + float affine_max_residue; + float affine_min_displacement; + float affine_max_displacement_differ; /* th for the difference between the displacement calculated + by the affine tracker and the frame to frame tracker in pel*/ + + /* User must not touch these */ + void *pyramid_last; + void *pyramid_last_gradx; + void *pyramid_last_grady; +} KLT_TrackingContextRec, *KLT_TrackingContext; + + +typedef struct { + KLT_locType x; + KLT_locType y; + int val; + /* for affine mapping */ + _KLT_FloatImage aff_img; + _KLT_FloatImage aff_img_gradx; + _KLT_FloatImage aff_img_grady; + KLT_locType aff_x; + KLT_locType aff_y; + KLT_locType aff_Axx; + KLT_locType aff_Ayx; + KLT_locType aff_Axy; + KLT_locType aff_Ayy; +} KLT_FeatureRec, *KLT_Feature; + +typedef struct { + int nFeatures; + KLT_Feature *feature; +} KLT_FeatureListRec, *KLT_FeatureList; + +typedef struct { + int nFrames; + KLT_Feature *feature; +} KLT_FeatureHistoryRec, *KLT_FeatureHistory; + +typedef struct { + int nFrames; + int nFeatures; + KLT_Feature **feature; +} KLT_FeatureTableRec, *KLT_FeatureTable; + + + +/******************* + * Functions + */ + +/* Create */ +KLT_TrackingContext KLTCreateTrackingContext(void); +KLT_FeatureList KLTCreateFeatureList( + int nFeatures); +KLT_FeatureHistory KLTCreateFeatureHistory( + int nFrames); +KLT_FeatureTable KLTCreateFeatureTable( + int nFrames, + int nFeatures); + +/* Free */ +void KLTFreeTrackingContext( + KLT_TrackingContext tc); +void KLTFreeFeatureList( + KLT_FeatureList fl); +void KLTFreeFeatureHistory( + KLT_FeatureHistory fh); +void KLTFreeFeatureTable( + KLT_FeatureTable ft); + +/* Processing */ +void KLTSelectGoodFeatures( + KLT_TrackingContext tc, + KLT_PixelType *img, + int ncols, + int nrows, + KLT_FeatureList fl); +void KLTTrackFeatures( + KLT_TrackingContext tc, + KLT_PixelType *img1, + KLT_PixelType *img2, + int ncols, + int nrows, + KLT_FeatureList fl); +void KLTReplaceLostFeatures( + KLT_TrackingContext tc, + KLT_PixelType *img, + int ncols, + int nrows, + KLT_FeatureList fl); + +/* Utilities */ +int KLTCountRemainingFeatures( + KLT_FeatureList fl); +void KLTPrintTrackingContext( + KLT_TrackingContext tc); +void KLTChangeTCPyramid( + KLT_TrackingContext tc, + int search_range); +void KLTUpdateTCBorder( + KLT_TrackingContext tc); +void KLTStopSequentialMode( + KLT_TrackingContext tc); +void KLTSetVerbosity( + int verbosity); +float _KLTComputeSmoothSigma( + KLT_TrackingContext tc); + +/* Storing/Extracting Features */ +void KLTStoreFeatureList( + KLT_FeatureList fl, + KLT_FeatureTable ft, + int frame); +void KLTExtractFeatureList( + KLT_FeatureList fl, + KLT_FeatureTable ft, + int frame); +void KLTStoreFeatureHistory( + KLT_FeatureHistory fh, + KLT_FeatureTable ft, + int feat); +void KLTExtractFeatureHistory( + KLT_FeatureHistory fh, + KLT_FeatureTable ft, + int feat); + +/* Writing/Reading */ +void KLTWriteFeatureListToPPM( + KLT_FeatureList fl, + KLT_PixelType *greyimg, + int ncols, + int nrows, + char *filename); +void KLTWriteFeatureList( + KLT_FeatureList fl, + char *filename, + char *fmt); +void KLTWriteFeatureHistory( + KLT_FeatureHistory fh, + char *filename, + char *fmt); +void KLTWriteFeatureTable( + KLT_FeatureTable ft, + char *filename, + char *fmt); +KLT_FeatureList KLTReadFeatureList( + KLT_FeatureList fl, + char *filename); +KLT_FeatureHistory KLTReadFeatureHistory( + KLT_FeatureHistory fh, + char *filename); +KLT_FeatureTable KLTReadFeatureTable( + KLT_FeatureTable ft, + char *filename); +#ifdef __cplusplus +} +#endif + +#endif + + + + + + diff --git a/rtengine/klt/klt_util.c b/rtengine/klt/klt_util.c new file mode 100644 index 000000000..300941bb9 --- /dev/null +++ b/rtengine/klt/klt_util.c @@ -0,0 +1,165 @@ +/********************************************************************* + * klt_util.c + *********************************************************************/ + +/* Standard includes */ +#include +#include /* malloc() */ +#include /* fabs() */ + +/* Our includes */ +#include "base.h" +#include "error.h" +#include "pnmio.h" +#include "klt.h" +#include "klt_util.h" + + +/*********************************************************************/ + +float _KLTComputeSmoothSigma( + KLT_TrackingContext tc) +{ + return (tc->smooth_sigma_fact * max(tc->window_width, tc->window_height)); +} + + +/********************************************************************* + * _KLTCreateFloatImage + */ + +_KLT_FloatImage _KLTCreateFloatImage( + int ncols, + int nrows) +{ + _KLT_FloatImage floatimg; + int nbytes = sizeof(_KLT_FloatImageRec) + + ncols * nrows * sizeof(float); + + floatimg = (_KLT_FloatImage) malloc(nbytes); + if (floatimg == NULL) + KLTError("(_KLTCreateFloatImage) Out of memory"); + floatimg->ncols = ncols; + floatimg->nrows = nrows; + floatimg->data = (float *) (floatimg + 1); + + return(floatimg); +} + + +/********************************************************************* + * _KLTFreeFloatImage + */ + +void _KLTFreeFloatImage( + _KLT_FloatImage floatimg) +{ + free(floatimg); +} + + +/********************************************************************* + * _KLTPrintSubFloatImage + */ + +void _KLTPrintSubFloatImage( + _KLT_FloatImage floatimg, + int x0, int y0, + int width, int height) +{ + int ncols = floatimg->ncols; + int offset; + int i, j; + + assert(x0 >= 0); + assert(y0 >= 0); + assert(x0 + width <= ncols); + assert(y0 + height <= floatimg->nrows); + + fprintf(stderr, "\n"); + for (j = 0 ; j < height ; j++) { + for (i = 0 ; i < width ; i++) { + offset = (j+y0)*ncols + (i+x0); + fprintf(stderr, "%6.2f ", *(floatimg->data + offset)); + } + fprintf(stderr, "\n"); + } + fprintf(stderr, "\n"); +} + + +/********************************************************************* + * _KLTWriteFloatImageToPGM + */ + +void _KLTWriteFloatImageToPGM( + _KLT_FloatImage img, + char *filename) +{ + int npixs = img->ncols * img->nrows; + float mmax = -999999.9f, mmin = 999999.9f; + float fact; + float *ptr; + uchar *byteimg, *ptrout; + int i; + + /* Calculate minimum and maximum values of float image */ + ptr = img->data; + for (i = 0 ; i < npixs ; i++) { + mmax = max(mmax, *ptr); + mmin = min(mmin, *ptr); + ptr++; + } + + /* Allocate memory to hold converted image */ + byteimg = (uchar *) malloc(npixs * sizeof(uchar)); + + /* Convert image from float to uchar */ + fact = 255.0f / (mmax-mmin); + ptr = img->data; + ptrout = byteimg; + for (i = 0 ; i < npixs ; i++) { + *ptrout++ = (uchar) ((*ptr++ - mmin) * fact); + } + + /* Write uchar image to PGM */ + pgmWriteFile(filename, byteimg, img->ncols, img->nrows); + + /* Free memory */ + free(byteimg); +} + +/********************************************************************* + * _KLTWriteFloatImageToPGM + */ + +void _KLTWriteAbsFloatImageToPGM( + _KLT_FloatImage img, + char *filename,float scale) +{ + int npixs = img->ncols * img->nrows; + float fact; + float *ptr; + uchar *byteimg, *ptrout; + int i; + float tmp; + + /* Allocate memory to hold converted image */ + byteimg = (uchar *) malloc(npixs * sizeof(uchar)); + + /* Convert image from float to uchar */ + fact = 255.0f / scale; + ptr = img->data; + ptrout = byteimg; + for (i = 0 ; i < npixs ; i++) { + tmp = (float) (fabs(*ptr++) * fact); + if(tmp > 255.0) tmp = 255.0; + *ptrout++ = (uchar) tmp; + } + + /* Write uchar image to PGM */ + pgmWriteFile(filename, byteimg, img->ncols, img->nrows); + + /* Free memory */ + free(byteimg); +} diff --git a/rtengine/klt/klt_util.h b/rtengine/klt/klt_util.h new file mode 100644 index 000000000..4759d7ace --- /dev/null +++ b/rtengine/klt/klt_util.h @@ -0,0 +1,37 @@ +/********************************************************************* + * klt_util.h + *********************************************************************/ + +#ifndef _KLT_UTIL_H_ +#define _KLT_UTIL_H_ + +typedef struct { + int ncols; + int nrows; + float *data; +} _KLT_FloatImageRec, *_KLT_FloatImage; + +_KLT_FloatImage _KLTCreateFloatImage( + int ncols, + int nrows); + +void _KLTFreeFloatImage( + _KLT_FloatImage); + +void _KLTPrintSubFloatImage( + _KLT_FloatImage floatimg, + int x0, int y0, + int width, int height); + +void _KLTWriteFloatImageToPGM( + _KLT_FloatImage img, + char *filename); + +/* for affine mapping */ +void _KLTWriteAbsFloatImageToPGM( + _KLT_FloatImage img, + char *filename,float scale); + +#endif + + diff --git a/rtengine/klt/main.cpp b/rtengine/klt/main.cpp new file mode 100644 index 000000000..8270c9730 --- /dev/null +++ b/rtengine/klt/main.cpp @@ -0,0 +1,30 @@ +// main.cpp : Defines the entry point for the console application. +// + +#include // printf + +extern "C" { + void RunExample1(); + void RunExample2(); + void RunExample3(); + void RunExample4(); + void RunExample5(); +} + +int main(int argc, char* argv[]) +{ + // select which example to run here + const int which = 1; + + // run the appropriate example + switch (which) { + case 1: RunExample1(); break; + case 2: RunExample2(); break; + case 3: RunExample3(); break; + case 4: RunExample4(); break; // Note: example4 reads output from example 3 + case 5: RunExample5(); break; + default: printf("There is no example number %d\n", which); + } + return 0; +} + diff --git a/rtengine/klt/pnmio.c b/rtengine/klt/pnmio.c new file mode 100644 index 000000000..f813039b2 --- /dev/null +++ b/rtengine/klt/pnmio.c @@ -0,0 +1,333 @@ +/********************************************************************* + * pnmio.c + * + * Various routines to manipulate PNM files. + *********************************************************************/ + + +/* Standard includes */ +#include /* FILE */ +#include /* malloc(), atoi() */ + +/* Our includes */ +#include "error.h" + +#define LENGTH 80 + + +/*********************************************************************/ + +static void _getNextString( + FILE *fp, + char *line) +{ + int i; + + line[0] = '\0'; + + while (line[0] == '\0') { + fscanf(fp, "%s", line); + i = -1; + do { + i++; + if (line[i] == '#') { + line[i] = '\0'; + while (fgetc(fp) != '\n') ; + } + } while (line[i] != '\0'); + } +} + + +/********************************************************************* + * pnmReadHeader + */ + +void pnmReadHeader( + FILE *fp, + int *magic, + int *ncols, int *nrows, + int *maxval) +{ + char line[LENGTH]; + + /* Read magic number */ + _getNextString(fp, line); + if (line[0] != 'P') + KLTError("(pnmReadHeader) Magic number does not begin with 'P', " + "but with a '%c'", line[0]); + sscanf(line, "P%d", magic); + + /* Read size, skipping comments */ + _getNextString(fp, line); + *ncols = atoi(line); + _getNextString(fp, line); + *nrows = atoi(line); + if (*ncols < 0 || *nrows < 0 || *ncols > 10000 || *nrows > 10000) + KLTError("(pnmReadHeader) The dimensions %d x %d are unacceptable", + *ncols, *nrows); + + /* Read maxval, skipping comments */ + _getNextString(fp, line); + *maxval = atoi(line); + fread(line, 1, 1, fp); /* Read newline which follows maxval */ + + if (*maxval != 255) + KLTWarning("(pnmReadHeader) Maxval is not 255, but %d", *maxval); +} + + +/********************************************************************* + * pgmReadHeader + */ + +void pgmReadHeader( + FILE *fp, + int *magic, + int *ncols, int *nrows, + int *maxval) +{ + pnmReadHeader(fp, magic, ncols, nrows, maxval); + if (*magic != 5) + KLTError("(pgmReadHeader) Magic number is not 'P5', but 'P%d'", *magic); +} + + +/********************************************************************* + * ppmReadHeader + */ + +void ppmReadHeader( + FILE *fp, + int *magic, + int *ncols, int *nrows, + int *maxval) +{ + pnmReadHeader(fp, magic, ncols, nrows, maxval); + if (*magic != 6) + KLTError("(ppmReadHeader) Magic number is not 'P6', but 'P%d'", *magic); +} + + +/********************************************************************* + * pgmReadHeaderFile + */ + +void pgmReadHeaderFile( + char *fname, + int *magic, + int *ncols, int *nrows, + int *maxval) +{ + FILE *fp; + + /* Open file */ + if ( (fp = fopen(fname, "rb")) == NULL) + KLTError("(pgmReadHeaderFile) Can't open file named '%s' for reading\n", fname); + + /* Read header */ + pgmReadHeader(fp, magic, ncols, nrows, maxval); + + /* Close file */ + fclose(fp); +} + + +/********************************************************************* + * ppmReadHeaderFile + */ + +void ppmReadHeaderFile( + char *fname, + int *magic, + int *ncols, int *nrows, + int *maxval) +{ + FILE *fp; + + /* Open file */ + if ( (fp = fopen(fname, "rb")) == NULL) + KLTError("(ppmReadHeaderFile) Can't open file named '%s' for reading\n", fname); + + /* Read header */ + ppmReadHeader(fp, magic, ncols, nrows, maxval); + + /* Close file */ + fclose(fp); +} + + +/********************************************************************* + * pgmRead + * + * NOTE: If img is NULL, memory is allocated. + */ + +unsigned char* pgmRead( + FILE *fp, + unsigned char *img, + int *ncols, int *nrows) +{ + unsigned char *ptr; + int magic, maxval; + int i; + + /* Read header */ + pgmReadHeader(fp, &magic, ncols, nrows, &maxval); + + /* Allocate memory, if necessary, and set pointer */ + if (img == NULL) { + ptr = (unsigned char *) malloc(*ncols * *nrows * sizeof(char)); + if (ptr == NULL) + KLTError("(pgmRead) Memory not allocated"); + } + else + ptr = img; + + /* Read binary image data */ + { + unsigned char *tmpptr = ptr; + for (i = 0 ; i < *nrows ; i++) { + fread(tmpptr, *ncols, 1, fp); + tmpptr += *ncols; + } + } + + return ptr; +} + + +/********************************************************************* + * pgmReadFile + * + * NOTE: If img is NULL, memory is allocated. + */ + +unsigned char* pgmReadFile( + char *fname, + unsigned char *img, + int *ncols, int *nrows) +{ + unsigned char *ptr; + FILE *fp; + + /* Open file */ + if ( (fp = fopen(fname, "rb")) == NULL) + KLTError("(pgmReadFile) Can't open file named '%s' for reading\n", fname); + + /* Read file */ + ptr = pgmRead(fp, img, ncols, nrows); + + /* Close file */ + fclose(fp); + + return ptr; +} + + +/********************************************************************* + * pgmWrite + */ + +void pgmWrite( + FILE *fp, + unsigned char *img, + int ncols, + int nrows) +{ + int i; + + /* Write header */ + fprintf(fp, "P5\n"); + fprintf(fp, "%d %d\n", ncols, nrows); + fprintf(fp, "255\n"); + + /* Write binary data */ + for (i = 0 ; i < nrows ; i++) { + fwrite(img, ncols, 1, fp); + img += ncols; + } +} + + +/********************************************************************* + * pgmWriteFile + */ + +void pgmWriteFile( + char *fname, + unsigned char *img, + int ncols, + int nrows) +{ + FILE *fp; + + /* Open file */ + if ( (fp = fopen(fname, "wb")) == NULL) + KLTError("(pgmWriteFile) Can't open file named '%s' for writing\n", fname); + + /* Write to file */ + pgmWrite(fp, img, ncols, nrows); + + /* Close file */ + fclose(fp); +} + + +/********************************************************************* + * ppmWrite + */ + +void ppmWrite( + FILE *fp, + unsigned char *redimg, + unsigned char *greenimg, + unsigned char *blueimg, + int ncols, + int nrows) +{ + int i, j; + + /* Write header */ + fprintf(fp, "P6\n"); + fprintf(fp, "%d %d\n", ncols, nrows); + fprintf(fp, "255\n"); + + /* Write binary data */ + for (j = 0 ; j < nrows ; j++) { + for (i = 0 ; i < ncols ; i++) { + fwrite(redimg, 1, 1, fp); + fwrite(greenimg, 1, 1, fp); + fwrite(blueimg, 1, 1, fp); + redimg++; greenimg++; blueimg++; + } + } +} + + +/********************************************************************* + * ppmWriteFileRGB + */ + +void ppmWriteFileRGB( + char *fname, + unsigned char *redimg, + unsigned char *greenimg, + unsigned char *blueimg, + int ncols, + int nrows) +{ + FILE *fp; + + /* Open file */ + if ( (fp = fopen(fname, "wb")) == NULL) + KLTError("(ppmWriteFileRGB) Can't open file named '%s' for writing\n", fname); + + /* Write to file */ + ppmWrite(fp, redimg, greenimg, blueimg, ncols, nrows); + + /* Close file */ + fclose(fp); +} + + diff --git a/rtengine/klt/pnmio.h b/rtengine/klt/pnmio.h new file mode 100644 index 000000000..af490431a --- /dev/null +++ b/rtengine/klt/pnmio.h @@ -0,0 +1,56 @@ +/********************************************************************* + * pnmio.h + *********************************************************************/ + +#ifndef _PNMIO_H_ +#define _PNMIO_H_ + +#include + +/********** + * With pgmReadFile and pgmRead, setting img to NULL causes memory + * to be allocated + */ + +/********** + * used for reading from/writing to files + */ +unsigned char* pgmReadFile( + char *fname, + unsigned char *img, + int *ncols, + int *nrows); +void pgmWriteFile( + char *fname, + unsigned char *img, + int ncols, + int nrows); +void ppmWriteFileRGB( + char *fname, + unsigned char *redimg, + unsigned char *greenimg, + unsigned char *blueimg, + int ncols, + int nrows); + +/********** + * used for communicating with stdin and stdout + */ +unsigned char* pgmRead( + FILE *fp, + unsigned char *img, + int *ncols, int *nrows); +void pgmWrite( + FILE *fp, + unsigned char *img, + int ncols, + int nrows); +void ppmWrite( + FILE *fp, + unsigned char *redimg, + unsigned char *greenimg, + unsigned char *blueimg, + int ncols, + int nrows); + +#endif diff --git a/rtengine/klt/pyramid.c b/rtengine/klt/pyramid.c new file mode 100644 index 000000000..fbce17497 --- /dev/null +++ b/rtengine/klt/pyramid.c @@ -0,0 +1,143 @@ +/********************************************************************* + * pyramid.c + * + *********************************************************************/ + +/* Standard includes */ +#include +#include /* malloc() ? */ +#include /* memset() ? */ +#include /* */ + +/* Our includes */ +#include "base.h" +#include "error.h" +#include "convolve.h" /* for computing pyramid */ +#include "pyramid.h" + + +/********************************************************************* + * + */ + +_KLT_Pyramid _KLTCreatePyramid( + int ncols, + int nrows, + int subsampling, + int nlevels) +{ + _KLT_Pyramid pyramid; + int nbytes = sizeof(_KLT_PyramidRec) + + nlevels * sizeof(_KLT_FloatImage *) + + nlevels * sizeof(int) + + nlevels * sizeof(int); + int i; + + if (subsampling != 2 && subsampling != 4 && + subsampling != 8 && subsampling != 16 && subsampling != 32) + KLTError("(_KLTCreatePyramid) Pyramid's subsampling must " + "be either 2, 4, 8, 16, or 32"); + + + /* Allocate memory for structure and set parameters */ + pyramid = (_KLT_Pyramid) malloc(nbytes); + if (pyramid == NULL) + KLTError("(_KLTCreatePyramid) Out of memory"); + + /* Set parameters */ + pyramid->subsampling = subsampling; + pyramid->nLevels = nlevels; + pyramid->img = (_KLT_FloatImage *) (pyramid + 1); + pyramid->ncols = (int *) (pyramid->img + nlevels); + pyramid->nrows = (int *) (pyramid->ncols + nlevels); + + /* Allocate memory for each level of pyramid and assign pointers */ + for (i = 0 ; i < nlevels ; i++) { + pyramid->img[i] = _KLTCreateFloatImage(ncols, nrows); + pyramid->ncols[i] = ncols; pyramid->nrows[i] = nrows; + ncols /= subsampling; nrows /= subsampling; + } + + return pyramid; +} + + +/********************************************************************* + * + */ + +void _KLTFreePyramid( + _KLT_Pyramid pyramid) +{ + int i; + + /* Free images */ + for (i = 0 ; i < pyramid->nLevels ; i++) + _KLTFreeFloatImage(pyramid->img[i]); + + /* Free structure */ + free(pyramid); +} + + +/********************************************************************* + * + */ + +void _KLTComputePyramid( + _KLT_FloatImage img, + _KLT_Pyramid pyramid, + float sigma_fact) +{ + _KLT_FloatImage currimg, tmpimg; + int ncols = img->ncols, nrows = img->nrows; + int subsampling = pyramid->subsampling; + int subhalf = subsampling / 2; + float sigma = subsampling * sigma_fact; /* empirically determined */ + int oldncols; + int i, x, y; + + if (subsampling != 2 && subsampling != 4 && + subsampling != 8 && subsampling != 16 && subsampling != 32) + KLTError("(_KLTComputePyramid) Pyramid's subsampling must " + "be either 2, 4, 8, 16, or 32"); + + assert(pyramid->ncols[0] == img->ncols); + assert(pyramid->nrows[0] == img->nrows); + + /* Copy original image to level 0 of pyramid */ + memcpy(pyramid->img[0]->data, img->data, ncols*nrows*sizeof(float)); + + currimg = img; + for (i = 1 ; i < pyramid->nLevels ; i++) { + tmpimg = _KLTCreateFloatImage(ncols, nrows); + _KLTComputeSmoothedImage(currimg, sigma, tmpimg); + + + /* Subsample */ + oldncols = ncols; + ncols /= subsampling; nrows /= subsampling; + for (y = 0 ; y < nrows ; y++) + for (x = 0 ; x < ncols ; x++) + pyramid->img[i]->data[y*ncols+x] = + tmpimg->data[(subsampling*y+subhalf)*oldncols + + (subsampling*x+subhalf)]; + + /* Reassign current image */ + currimg = pyramid->img[i]; + + _KLTFreeFloatImage(tmpimg); + } +} + + + + + + + + + + + + diff --git a/rtengine/klt/pyramid.h b/rtengine/klt/pyramid.h new file mode 100644 index 000000000..eca9e6692 --- /dev/null +++ b/rtengine/klt/pyramid.h @@ -0,0 +1,32 @@ +/********************************************************************* + * pyramid.h + *********************************************************************/ + +#ifndef _PYRAMID_H_ +#define _PYRAMID_H_ + +#include "klt_util.h" + +typedef struct { + int subsampling; + int nLevels; + _KLT_FloatImage *img; + int *ncols, *nrows; +} _KLT_PyramidRec, *_KLT_Pyramid; + + +_KLT_Pyramid _KLTCreatePyramid( + int ncols, + int nrows, + int subsampling, + int nlevels); + +void _KLTComputePyramid( + _KLT_FloatImage floatimg, + _KLT_Pyramid pyramid, + float sigma_fact); + +void _KLTFreePyramid( + _KLT_Pyramid pyramid); + +#endif diff --git a/rtengine/klt/selectGoodFeatures.c b/rtengine/klt/selectGoodFeatures.c new file mode 100644 index 000000000..cfc41c546 --- /dev/null +++ b/rtengine/klt/selectGoodFeatures.c @@ -0,0 +1,543 @@ +/********************************************************************* + * selectGoodFeatures.c + * + *********************************************************************/ + +/* Standard includes */ +#include +#include /* malloc(), qsort() */ +#include /* fflush() */ +#include /* memset() */ +#include /* fsqrt() */ +#define fsqrt(X) sqrt(X) + +/* Our includes */ +#include "base.h" +#include "error.h" +#include "convolve.h" +#include "klt.h" +#include "klt_util.h" +#include "pyramid.h" + +int KLT_verbose = 1; + +typedef enum {SELECTING_ALL, REPLACING_SOME} selectionMode; + + +/********************************************************************* + * _quicksort + * Replacement for qsort(). Computing time is decreased by taking + * advantage of specific knowledge of our array (that there are + * three ints associated with each point). + * + * This routine generously provided by + * Manolis Lourakis + * + * NOTE: The results of this function may be slightly different from + * those of qsort(). This is due to the fact that different sort + * algorithms have different behaviours when sorting numbers with the + * same value: Some leave them in the same relative positions in the + * array, while others change their relative positions. For example, + * if you have the array [c d b1 a b2] with b1=b2, it may be sorted as + * [a b1 b2 c d] or [a b2 b1 c d]. + */ + +#define SWAP3(list, i, j) \ +{register int *pi, *pj, tmp; \ + pi=list+3*(i); pj=list+3*(j); \ + \ + tmp=*pi; \ + *pi++=*pj; \ + *pj++=tmp; \ + \ + tmp=*pi; \ + *pi++=*pj; \ + *pj++=tmp; \ + \ + tmp=*pi; \ + *pi=*pj; \ + *pj=tmp; \ +} + +void _quicksort(int *pointlist, int n) +{ + unsigned int i, j, ln, rn; + + while (n > 1) + { + SWAP3(pointlist, 0, n/2); + for (i = 0, j = n; ; ) + { + do + --j; + while (pointlist[3*j+2] < pointlist[2]); + do + ++i; + while (i < j && pointlist[3*i+2] > pointlist[2]); + if (i >= j) + break; + SWAP3(pointlist, i, j); + } + SWAP3(pointlist, j, 0); + ln = j; + rn = n - ++j; + if (ln < rn) + { + _quicksort(pointlist, ln); + pointlist += 3*j; + n = rn; + } + else + { + _quicksort(pointlist + 3*j, rn); + n = ln; + } + } +} +#undef SWAP3 + + +/*********************************************************************/ + +static void _fillFeaturemap( + int x, int y, + uchar *featuremap, + int mindist, + int ncols, + int nrows) +{ + int ix, iy; + + for (iy = y - mindist ; iy <= y + mindist ; iy++) + for (ix = x - mindist ; ix <= x + mindist ; ix++) + if (ix >= 0 && ix < ncols && iy >= 0 && iy < nrows) + featuremap[iy*ncols+ix] = 1; +} + + +/********************************************************************* + * _enforceMinimumDistance + * + * Removes features that are within close proximity to better features. + * + * INPUTS + * featurelist: A list of features. The nFeatures property + * is used. + * + * OUTPUTS + * featurelist: Is overwritten. Nearby "redundant" features are removed. + * Writes -1's into the remaining elements. + * + * RETURNS + * The number of remaining features. + */ + +static void _enforceMinimumDistance( + int *pointlist, /* featurepoints */ + int npoints, /* number of featurepoints */ + KLT_FeatureList featurelist, /* features */ + int ncols, int nrows, /* size of images */ + int mindist, /* min. dist b/w features */ + int min_eigenvalue, /* min. eigenvalue */ + KLT_BOOL overwriteAllFeatures) +{ + int indx; /* Index into features */ + int x, y, val; /* Location and trackability of pixel under consideration */ + uchar *featuremap; /* Boolean array recording proximity of features */ + int *ptr; + + /* Cannot add features with an eigenvalue less than one */ + if (min_eigenvalue < 1) min_eigenvalue = 1; + + /* Allocate memory for feature map and clear it */ + featuremap = (uchar *) malloc(ncols * nrows * sizeof(uchar)); + memset(featuremap, 0, ncols*nrows); + + /* Necessary because code below works with (mindist-1) */ + mindist--; + + /* If we are keeping all old good features, then add them to the featuremap */ + if (!overwriteAllFeatures) + for (indx = 0 ; indx < featurelist->nFeatures ; indx++) + if (featurelist->feature[indx]->val >= 0) { + x = (int) featurelist->feature[indx]->x; + y = (int) featurelist->feature[indx]->y; + _fillFeaturemap(x, y, featuremap, mindist, ncols, nrows); + } + + /* For each feature point, in descending order of importance, do ... */ + ptr = pointlist; + indx = 0; + while (1) { + + /* If we can't add all the points, then fill in the rest + of the featurelist with -1's */ + if (ptr >= pointlist + 3*npoints) { + while (indx < featurelist->nFeatures) { + if (overwriteAllFeatures || + featurelist->feature[indx]->val < 0) { + featurelist->feature[indx]->x = -1; + featurelist->feature[indx]->y = -1; + featurelist->feature[indx]->val = KLT_NOT_FOUND; + featurelist->feature[indx]->aff_img = NULL; + featurelist->feature[indx]->aff_img_gradx = NULL; + featurelist->feature[indx]->aff_img_grady = NULL; + featurelist->feature[indx]->aff_x = -1.0; + featurelist->feature[indx]->aff_y = -1.0; + featurelist->feature[indx]->aff_Axx = 1.0; + featurelist->feature[indx]->aff_Ayx = 0.0; + featurelist->feature[indx]->aff_Axy = 0.0; + featurelist->feature[indx]->aff_Ayy = 1.0; + } + indx++; + } + break; + } + + x = *ptr++; + y = *ptr++; + val = *ptr++; + + /* Ensure that feature is in-bounds */ + assert(x >= 0); + assert(x < ncols); + assert(y >= 0); + assert(y < nrows); + + while (!overwriteAllFeatures && + indx < featurelist->nFeatures && + featurelist->feature[indx]->val >= 0) + indx++; + + if (indx >= featurelist->nFeatures) break; + + /* If no neighbor has been selected, and if the minimum + eigenvalue is large enough, then add feature to the current list */ + if (!featuremap[y*ncols+x] && val >= min_eigenvalue) { + featurelist->feature[indx]->x = (KLT_locType) x; + featurelist->feature[indx]->y = (KLT_locType) y; + featurelist->feature[indx]->val = (int) val; + featurelist->feature[indx]->aff_img = NULL; + featurelist->feature[indx]->aff_img_gradx = NULL; + featurelist->feature[indx]->aff_img_grady = NULL; + featurelist->feature[indx]->aff_x = -1.0; + featurelist->feature[indx]->aff_y = -1.0; + featurelist->feature[indx]->aff_Axx = 1.0; + featurelist->feature[indx]->aff_Ayx = 0.0; + featurelist->feature[indx]->aff_Axy = 0.0; + featurelist->feature[indx]->aff_Ayy = 1.0; + indx++; + + /* Fill in surrounding region of feature map, but + make sure that pixels are in-bounds */ + _fillFeaturemap(x, y, featuremap, mindist, ncols, nrows); + } + } + + /* Free feature map */ + free(featuremap); +} + + +/********************************************************************* + * _comparePoints + * + * Used by qsort (in _KLTSelectGoodFeatures) to determine + * which feature is better. + * By switching the '>' with the '<', qsort is fooled into sorting + * in descending order. + */ + +#ifdef KLT_USE_QSORT +static int _comparePoints(const void *a, const void *b) +{ + int v1 = *(((int *) a) + 2); + int v2 = *(((int *) b) + 2); + + if (v1 > v2) return(-1); + else if (v1 < v2) return(1); + else return(0); +} +#endif + + +/********************************************************************* + * _sortPointList + */ + +static void _sortPointList( + int *pointlist, + int npoints) +{ +#ifdef KLT_USE_QSORT + qsort(pointlist, npoints, 3*sizeof(int), _comparePoints); +#else + _quicksort(pointlist, npoints); +#endif +} + + +/********************************************************************* + * _minEigenvalue + * + * Given the three distinct elements of the symmetric 2x2 matrix + * [gxx gxy] + * [gxy gyy], + * Returns the minimum eigenvalue of the matrix. + */ + +static float _minEigenvalue(float gxx, float gxy, float gyy) +{ + return (float) ((gxx + gyy - sqrt((gxx - gyy)*(gxx - gyy) + 4*gxy*gxy))/2.0f); +} + + +/*********************************************************************/ + +void _KLTSelectGoodFeatures( + KLT_TrackingContext tc, + KLT_PixelType *img, + int ncols, + int nrows, + KLT_FeatureList featurelist, + selectionMode mode) +{ + _KLT_FloatImage floatimg, gradx, grady; + int window_hw, window_hh; + int *pointlist; + int npoints = 0; + KLT_BOOL overwriteAllFeatures = (mode == SELECTING_ALL) ? + TRUE : FALSE; + KLT_BOOL floatimages_created = FALSE; + + /* Check window size (and correct if necessary) */ + if (tc->window_width % 2 != 1) { + tc->window_width = tc->window_width+1; + KLTWarning("Tracking context's window width must be odd. " + "Changing to %d.\n", tc->window_width); + } + if (tc->window_height % 2 != 1) { + tc->window_height = tc->window_height+1; + KLTWarning("Tracking context's window height must be odd. " + "Changing to %d.\n", tc->window_height); + } + if (tc->window_width < 3) { + tc->window_width = 3; + KLTWarning("Tracking context's window width must be at least three. \n" + "Changing to %d.\n", tc->window_width); + } + if (tc->window_height < 3) { + tc->window_height = 3; + KLTWarning("Tracking context's window height must be at least three. \n" + "Changing to %d.\n", tc->window_height); + } + window_hw = tc->window_width/2; + window_hh = tc->window_height/2; + + /* Create pointlist, which is a simplified version of a featurelist, */ + /* for speed. Contains only integer locations and values. */ + pointlist = (int *) malloc(ncols * nrows * 3 * sizeof(int)); + + /* Create temporary images, etc. */ + if (mode == REPLACING_SOME && + tc->sequentialMode && tc->pyramid_last != NULL) { + floatimg = ((_KLT_Pyramid) tc->pyramid_last)->img[0]; + gradx = ((_KLT_Pyramid) tc->pyramid_last_gradx)->img[0]; + grady = ((_KLT_Pyramid) tc->pyramid_last_grady)->img[0]; + assert(gradx != NULL); + assert(grady != NULL); + } else { + floatimages_created = TRUE; + floatimg = _KLTCreateFloatImage(ncols, nrows); + gradx = _KLTCreateFloatImage(ncols, nrows); + grady = _KLTCreateFloatImage(ncols, nrows); + if (tc->smoothBeforeSelecting) { + _KLT_FloatImage tmpimg; + tmpimg = _KLTCreateFloatImage(ncols, nrows); + _KLTToFloatImage(img, ncols, nrows, tmpimg); + _KLTComputeSmoothedImage(tmpimg, _KLTComputeSmoothSigma(tc), floatimg); + _KLTFreeFloatImage(tmpimg); + } else _KLTToFloatImage(img, ncols, nrows, floatimg); + + /* Compute gradient of image in x and y direction */ + _KLTComputeGradients(floatimg, tc->grad_sigma, gradx, grady); + } + + /* Write internal images */ + if (tc->writeInternalImages) { + _KLTWriteFloatImageToPGM(floatimg, "kltimg_sgfrlf.pgm"); + _KLTWriteFloatImageToPGM(gradx, "kltimg_sgfrlf_gx.pgm"); + _KLTWriteFloatImageToPGM(grady, "kltimg_sgfrlf_gy.pgm"); + } + + /* Compute trackability of each image pixel as the minimum + of the two eigenvalues of the Z matrix */ + { + register float gx, gy; + register float gxx, gxy, gyy; + register int xx, yy; + register int *ptr; + float val; + unsigned int limit = 1; + int borderx = tc->borderx; /* Must not touch cols */ + int bordery = tc->bordery; /* lost by convolution */ + int x, y; + int i; + + if (borderx < window_hw) borderx = window_hw; + if (bordery < window_hh) bordery = window_hh; + + /* Find largest value of an int */ + for (i = 0 ; i < sizeof(int) ; i++) limit *= 256; + limit = limit/2 - 1; + + /* For most of the pixels in the image, do ... */ + ptr = pointlist; + for (y = bordery ; y < nrows - bordery ; y += tc->nSkippedPixels + 1) + for (x = borderx ; x < ncols - borderx ; x += tc->nSkippedPixels + 1) { + + /* Sum the gradients in the surrounding window */ + gxx = 0; gxy = 0; gyy = 0; + for (yy = y-window_hh ; yy <= y+window_hh ; yy++) + for (xx = x-window_hw ; xx <= x+window_hw ; xx++) { + gx = *(gradx->data + ncols*yy+xx); + gy = *(grady->data + ncols*yy+xx); + gxx += gx * gx; + gxy += gx * gy; + gyy += gy * gy; + } + + /* Store the trackability of the pixel as the minimum + of the two eigenvalues */ + *ptr++ = x; + *ptr++ = y; + val = _minEigenvalue(gxx, gxy, gyy); + if (val > limit) { + KLTWarning("(_KLTSelectGoodFeatures) minimum eigenvalue %f is " + "greater than the capacity of an int; setting " + "to maximum value", val); + val = (float) limit; + } + *ptr++ = (int) val; + npoints++; + } + } + + /* Sort the features */ + _sortPointList(pointlist, npoints); + + /* Check tc->mindist */ + if (tc->mindist < 0) { + KLTWarning("(_KLTSelectGoodFeatures) Tracking context field tc->mindist " + "is negative (%d); setting to zero", tc->mindist); + tc->mindist = 0; + } + + /* Enforce minimum distance between features */ + _enforceMinimumDistance( + pointlist, + npoints, + featurelist, + ncols, nrows, + tc->mindist, + tc->min_eigenvalue, + overwriteAllFeatures); + + /* Free memory */ + free(pointlist); + if (floatimages_created) { + _KLTFreeFloatImage(floatimg); + _KLTFreeFloatImage(gradx); + _KLTFreeFloatImage(grady); + } +} + + +/********************************************************************* + * KLTSelectGoodFeatures + * + * Main routine, visible to the outside. Finds the good features in + * an image. + * + * INPUTS + * tc: Contains parameters used in computation (size of image, + * size of window, min distance b/w features, sigma to compute + * image gradients, # of features desired). + * img: Pointer to the data of an image (probably unsigned chars). + * + * OUTPUTS + * features: List of features. The member nFeatures is computed. + */ + +void KLTSelectGoodFeatures( + KLT_TrackingContext tc, + KLT_PixelType *img, + int ncols, + int nrows, + KLT_FeatureList fl) +{ + if (KLT_verbose >= 1) { + fprintf(stderr, "(KLT) Selecting the %d best features " + "from a %d by %d image... ", fl->nFeatures, ncols, nrows); + fflush(stderr); + } + + _KLTSelectGoodFeatures(tc, img, ncols, nrows, + fl, SELECTING_ALL); + + if (KLT_verbose >= 1) { + fprintf(stderr, "\n\t%d features found.\n", + KLTCountRemainingFeatures(fl)); + if (tc->writeInternalImages) + fprintf(stderr, "\tWrote images to 'kltimg_sgfrlf*.pgm'.\n"); + fflush(stderr); + } +} + + +/********************************************************************* + * KLTReplaceLostFeatures + * + * Main routine, visible to the outside. Replaces the lost features + * in an image. + * + * INPUTS + * tc: Contains parameters used in computation (size of image, + * size of window, min distance b/w features, sigma to compute + * image gradients, # of features desired). + * img: Pointer to the data of an image (probably unsigned chars). + * + * OUTPUTS + * features: List of features. The member nFeatures is computed. + */ + +void KLTReplaceLostFeatures( + KLT_TrackingContext tc, + KLT_PixelType *img, + int ncols, + int nrows, + KLT_FeatureList fl) +{ + int nLostFeatures = fl->nFeatures - KLTCountRemainingFeatures(fl); + + if (KLT_verbose >= 1) { + fprintf(stderr, "(KLT) Attempting to replace %d features " + "in a %d by %d image... ", nLostFeatures, ncols, nrows); + fflush(stderr); + } + + /* If there are any lost features, replace them */ + if (nLostFeatures > 0) + _KLTSelectGoodFeatures(tc, img, ncols, nrows, + fl, REPLACING_SOME); + + if (KLT_verbose >= 1) { + fprintf(stderr, "\n\t%d features replaced.\n", + nLostFeatures - fl->nFeatures + KLTCountRemainingFeatures(fl)); + if (tc->writeInternalImages) + fprintf(stderr, "\tWrote images to 'kltimg_sgfrlf*.pgm'.\n"); + fflush(stderr); + } +} + + diff --git a/rtengine/klt/speed.txt b/rtengine/klt/speed.txt new file mode 100644 index 000000000..08e20e59e --- /dev/null +++ b/rtengine/klt/speed.txt @@ -0,0 +1,12 @@ +======================================================================= +speed of KLT versus OpenCV feature tracker on a 2.8 GHz P4 computer: + + KLT OpenCV +--------------------------------------------- +track (100) 54 ms 47 ms +track/replace (100) 95 ms 82 ms +track (1000) 79 ms 59 ms +track/replace (1000) 139 ms 103 ms + +The number of features is given in parentheses. These times were computed by running the code on the same image multiple times in succession, then averaging. As a result, they may not reflect the overhead of loading the image and cache misses that would occur in a real scenario. +======================================================================= diff --git a/rtengine/klt/storeFeatures.c b/rtengine/klt/storeFeatures.c new file mode 100644 index 000000000..467905b50 --- /dev/null +++ b/rtengine/klt/storeFeatures.c @@ -0,0 +1,117 @@ +/********************************************************************* + * storeFeatures.c + * + *********************************************************************/ + +/* Our includes */ +#include "error.h" +#include "klt.h" + + +/********************************************************************* + * + */ + +void KLTStoreFeatureList( + KLT_FeatureList fl, + KLT_FeatureTable ft, + int frame) +{ + int feat; + + if (frame < 0 || frame >= ft->nFrames) + KLTError("(KLTStoreFeatures) Frame number %d is not between 0 and %d", + frame, ft->nFrames - 1); + + if (fl->nFeatures != ft->nFeatures) + KLTError("(KLTStoreFeatures) FeatureList and FeatureTable must " + "have the same number of features"); + + for (feat = 0 ; feat < fl->nFeatures ; feat++) { + ft->feature[feat][frame]->x = fl->feature[feat]->x; + ft->feature[feat][frame]->y = fl->feature[feat]->y; + ft->feature[feat][frame]->val = fl->feature[feat]->val; + } +} + + +/********************************************************************* + * + */ + +void KLTExtractFeatureList( + KLT_FeatureList fl, + KLT_FeatureTable ft, + int frame) +{ + int feat; + + if (frame < 0 || frame >= ft->nFrames) + KLTError("(KLTExtractFeatures) Frame number %d is not between 0 and %d", + frame, ft->nFrames - 1); + + if (fl->nFeatures != ft->nFeatures) + KLTError("(KLTExtractFeatures) FeatureList and FeatureTable must " + "have the same number of features"); + + for (feat = 0 ; feat < fl->nFeatures ; feat++) { + fl->feature[feat]->x = ft->feature[feat][frame]->x; + fl->feature[feat]->y = ft->feature[feat][frame]->y; + fl->feature[feat]->val = ft->feature[feat][frame]->val; + } +} + + +/********************************************************************* + * + */ + +void KLTStoreFeatureHistory( + KLT_FeatureHistory fh, + KLT_FeatureTable ft, + int feat) +{ + int frame; + + if (feat < 0 || feat >= ft->nFeatures) + KLTError("(KLTStoreFeatureHistory) Feature number %d is not between 0 and %d", + feat, ft->nFeatures - 1); + + if (fh->nFrames != ft->nFrames) + KLTError("(KLTStoreFeatureHistory) FeatureHistory and FeatureTable must " + "have the same number of frames"); + + for (frame = 0 ; frame < fh->nFrames ; frame++) { + ft->feature[feat][frame]->x = fh->feature[frame]->x; + ft->feature[feat][frame]->y = fh->feature[frame]->y; + ft->feature[feat][frame]->val = fh->feature[frame]->val; + } +} + + +/********************************************************************* + * + */ + +void KLTExtractFeatureHistory( + KLT_FeatureHistory fh, + KLT_FeatureTable ft, + int feat) +{ + int frame; + + if (feat < 0 || feat >= ft->nFeatures) + KLTError("(KLTExtractFeatureHistory) Feature number %d is not between 0 and %d", + feat, ft->nFeatures - 1); + + if (fh->nFrames != ft->nFrames) + KLTError("(KLTExtractFeatureHistory) FeatureHistory and FeatureTable must " + "have the same number of frames"); + + for (frame = 0 ; frame < fh->nFrames ; frame++) { + fh->feature[frame]->x = ft->feature[feat][frame]->x; + fh->feature[frame]->y = ft->feature[feat][frame]->y; + fh->feature[frame]->val = ft->feature[feat][frame]->val; + } +} + diff --git a/rtengine/klt/trackFeatures.c b/rtengine/klt/trackFeatures.c new file mode 100644 index 000000000..91ee5ce39 --- /dev/null +++ b/rtengine/klt/trackFeatures.c @@ -0,0 +1,1531 @@ +/********************************************************************* + * trackFeatures.c + * + *********************************************************************/ + +/* Standard includes */ +#include +#include /* fabs() */ +#include /* malloc() */ +#include /* 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 values */ + *ex = 0; *ey = 0; + for (i = 0 ; i < width * height ; i++) { + diff = *imgdiff++; + *ex += diff * (*gradx++); + *ey += diff * (*grady++); + } + *ex *= step_factor; + *ey *= step_factor; +} + + +/********************************************************************* + * _solveEquation + * + * Solves the 2x2 matrix equation + * [gxx gxy] [dx] = [ex] + * [gxy gyy] [dy] = [ey] + * for dx and dy. + * + * Returns KLT_TRACKED on success and KLT_SMALL_DET on failure + */ + +static int _solveEquation( + float gxx, float gxy, float gyy, + float ex, float ey, + float small, + float *dx, float *dy) +{ + float det = gxx*gyy - gxy*gxy; + + + if (det < small) return KLT_SMALL_DET; + + *dx = (gyy*ex - gxy*ey)/det; + *dy = (gxx*ey - gxy*ex)/det; + return KLT_TRACKED; +} + + +/********************************************************************* + * _allocateFloatWindow + */ + +static _FloatWindow _allocateFloatWindow( + int width, + int height) +{ + _FloatWindow fw; + + fw = (_FloatWindow) malloc(width*height*sizeof(float)); + if (fw == NULL) KLTError("(_allocateFloatWindow) Out of memory."); + return fw; +} + + +/********************************************************************* + * _printFloatWindow + * (for debugging purposes) + */ + +/* +static void _printFloatWindow( + _FloatWindow fw, + int width, + int height) +{ + int i, j; + + fprintf(stderr, "\n"); + for (i = 0 ; i < width ; i++) { + for (j = 0 ; j < height ; j++) { + fprintf(stderr, "%6.1f ", *fw++); + } + fprintf(stderr, "\n"); + } +} +*/ + + +/********************************************************************* + * _sumAbsFloatWindow + */ + +static float _sumAbsFloatWindow( + _FloatWindow fw, + int width, + int height) +{ + float sum = 0.0; + int w; + + for ( ; height > 0 ; height--) + for (w=0 ; w < width ; w++) + sum += (float) fabs(*fw++); + + return sum; +} + + +/********************************************************************* + * _trackFeature + * + * Tracks a feature point from one image to the next. + * + * RETURNS + * KLT_SMALL_DET if feature is lost, + * KLT_MAX_ITERATIONS if tracking stopped because iterations timed out, + * KLT_TRACKED otherwise. + */ + +static int _trackFeature( + float x1, /* location of window in first image */ + float y1, + float *x2, /* starting location of search in second image */ + float *y2, + _KLT_FloatImage img1, + _KLT_FloatImage gradx1, + _KLT_FloatImage grady1, + _KLT_FloatImage img2, + _KLT_FloatImage gradx2, + _KLT_FloatImage grady2, + int width, /* size of window */ + int height, + float step_factor, /* 2.0 comes from equations, 1.0 seems to avoid overshooting */ + int max_iterations, + float small, /* determinant threshold for declaring KLT_SMALL_DET */ + float th, /* displacement threshold for stopping */ + float max_residue, /* residue threshold for declaring KLT_LARGE_RESIDUE */ + int lighting_insensitive) /* whether to normalize for gain and bias */ +{ + _FloatWindow imgdiff, gradx, grady; + float gxx, gxy, gyy, ex, ey, dx, dy; + int iteration = 0; + int status; + int hw = width/2; + int hh = height/2; + int nc = img1->ncols; + int nr = img1->nrows; + float one_plus_eps = 1.001f; /* To prevent rounding errors */ + + + /* Allocate memory for windows */ + imgdiff = _allocateFloatWindow(width, height); + gradx = _allocateFloatWindow(width, height); + grady = _allocateFloatWindow(width, height); + + /* Iteratively update the window position */ + do { + + /* If out of bounds, exit loop */ + if ( x1-hw < 0.0f || nc-( x1+hw) < one_plus_eps || + *x2-hw < 0.0f || nc-(*x2+hw) < one_plus_eps || + y1-hh < 0.0f || nr-( y1+hh) < one_plus_eps || + *y2-hh < 0.0f || nr-(*y2+hh) < one_plus_eps) { + status = KLT_OOB; + break; + } + + /* Compute gradient and difference windows */ + if (lighting_insensitive) { + _computeIntensityDifferenceLightingInsensitive(img1, img2, x1, y1, *x2, *y2, + width, height, imgdiff); + _computeGradientSumLightingInsensitive(gradx1, grady1, gradx2, grady2, + img1, img2, x1, y1, *x2, *y2, width, height, gradx, grady); + } else { + _computeIntensityDifference(img1, img2, x1, y1, *x2, *y2, + width, height, imgdiff); + _computeGradientSum(gradx1, grady1, gradx2, grady2, + x1, y1, *x2, *y2, width, height, gradx, grady); + } + + + /* Use these windows to construct matrices */ + _compute2by2GradientMatrix(gradx, grady, width, height, + &gxx, &gxy, &gyy); + _compute2by1ErrorVector(imgdiff, gradx, grady, width, height, step_factor, + &ex, &ey); + + /* Using matrices, solve equation for new displacement */ + status = _solveEquation(gxx, gxy, gyy, ex, ey, small, &dx, &dy); + if (status == KLT_SMALL_DET) break; + + *x2 += dx; + *y2 += dy; + iteration++; + + } while ((fabs(dx)>=th || fabs(dy)>=th) && iteration < max_iterations); + + /* Check whether window is out of bounds */ + if (*x2-hw < 0.0f || nc-(*x2+hw) < one_plus_eps || + *y2-hh < 0.0f || nr-(*y2+hh) < one_plus_eps) + status = KLT_OOB; + + /* Check whether residue is too large */ + if (status == KLT_TRACKED) { + if (lighting_insensitive) + _computeIntensityDifferenceLightingInsensitive(img1, img2, x1, y1, *x2, *y2, + width, height, imgdiff); + else + _computeIntensityDifference(img1, img2, x1, y1, *x2, *y2, + width, height, imgdiff); + if (_sumAbsFloatWindow(imgdiff, width, height)/(width*height) > max_residue) + status = KLT_LARGE_RESIDUE; + } + + /* Free memory */ + free(imgdiff); free(gradx); free(grady); + + /* Return appropriate value */ + if (status == KLT_SMALL_DET) return KLT_SMALL_DET; + else if (status == KLT_OOB) return KLT_OOB; + else if (status == KLT_LARGE_RESIDUE) return KLT_LARGE_RESIDUE; + else if (iteration >= max_iterations) return KLT_MAX_ITERATIONS; + else return KLT_TRACKED; + +} + + +/*********************************************************************/ + +static KLT_BOOL _outOfBounds( + float x, + float y, + int ncols, + int nrows, + int borderx, + int bordery) +{ + return (x < borderx || x > ncols-1-borderx || + y < bordery || y > nrows-1-bordery ); +} + + + + +/********************************************************************** +* CONSISTENCY CHECK OF FEATURES BY AFFINE MAPPING (BEGIN) +* +* Created by: Thorsten Thormaehlen (University of Hannover) June 2004 +* thormae@tnt.uni-hannover.de +* +* Permission is granted to any individual or institution to use, copy, modify, +* and distribute this part of the software, provided that this complete authorship +* and permission notice is maintained, intact, in all copies. +* +* This software is provided "as is" without express or implied warranty. +* +* +* The following static functions are helpers for the affine mapping. +* They all start with "_am". +* There are also small changes in other files for the +* affine mapping these are all marked by "for affine mapping" +* +* Thanks to Kevin Koeser (koeser@mip.informatik.uni-kiel.de) for fixing a bug +*/ + +#define SWAP_ME(X,Y) {temp=(X);(X)=(Y);(Y)=temp;} + +static float **_am_matrix(long nr, long nc) +{ + float **m; + int a; + m = (float **) malloc((size_t)(nr*sizeof(float*))); + m[0] = (float *) malloc((size_t)((nr*nc)*sizeof(float))); + for(a = 1; a < nr; a++) m[a] = m[a-1]+nc; + return m; +} + +static void _am_free_matrix(float **m) +{ + free(m[0]); + free(m); +} + + +static int _am_gauss_jordan_elimination(float **a, int n, float **b, int m) +{ + /* re-implemented from Numerical Recipes in C */ + int *indxc,*indxr,*ipiv; + int i,j,k,l,ll; + float big,dum,pivinv,temp; + int col = 0; + int row = 0; + + indxc=(int *)malloc((size_t) (n*sizeof(int))); + indxr=(int *)malloc((size_t) (n*sizeof(int))); + ipiv=(int *)malloc((size_t) (n*sizeof(int))); + for (j=0;j= big) { + big= (float) fabs(a[j][k]); + row=j; + col=k; + } + } else if (ipiv[k] > 1) return KLT_SMALL_DET; + } + ++(ipiv[col]); + if (row != col) { + for (l=0;l=0;l--) { + if (indxr[l] != indxc[l]) + for (k=0;kncols/2, hh = window->nrows/2; + int x0 = (int) x; + int y0 = (int) y; + float * windata = window->data; + int offset; + register int i, j; + + assert(x0 - hw >= 0); + assert(y0 - hh >= 0); + assert(x0 + hw <= img->ncols); + assert(y0 + hh <= img->nrows); + + /* copy values */ + for (j = -hh ; j <= hh ; j++) + for (i = -hw ; i <= hw ; i++) { + offset = (j+y0)*img->ncols + (i+x0); + *windata++ = *(img->data+offset); + } +} + +/********************************************************************* + * _am_computeIntensityDifferenceAffine + * + * Given two images and the window center in both images, + * aligns the images with the window and computes the difference + * between the two overlaid images using the affine mapping. + * A = [ Axx Axy] + * [ Ayx Ayy] +*/ + +static void _am_computeIntensityDifferenceAffine( + _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 */ + float Axx, float Ayx , float Axy, float Ayy, /* affine mapping */ + 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; + float mi, mj; + + /* Compute values */ + for (j = -hh ; j <= hh ; j++) + for (i = -hw ; i <= hw ; i++) { + g1 = _interpolate(x1+i, y1+j, img1); + mi = Axx * i + Axy * j; + mj = Ayx * i + Ayy * j; + g2 = _interpolate(x2+mi, y2+mj, img2); + *imgdiff++ = g1 - g2; + } +} + +/********************************************************************* + * _am_compute6by6GradientMatrix + * + */ + +static void _am_compute6by6GradientMatrix( + _FloatWindow gradx, + _FloatWindow grady, + int width, /* size of window */ + int height, + float **T) /* return values */ +{ + register int hw = width/2, hh = height/2; + register int i, j; + float gx, gy, gxx, gxy, gyy, x, y, xx, xy, yy; + + + /* Set values to zero */ + for (j = 0 ; j < 6 ; j++) { + for (i = j ; i < 6 ; i++) { + T[j][i] = 0.0; + } + } + + for (j = -hh ; j <= hh ; j++) { + for (i = -hw ; i <= hw ; i++) { + gx = *gradx++; + gy = *grady++; + gxx = gx * gx; + gxy = gx * gy; + gyy = gy * gy; + x = (float) i; + y = (float) j; + xx = x * x; + xy = x * y; + yy = y * y; + + T[0][0] += xx * gxx; + T[0][1] += xx * gxy; + T[0][2] += xy * gxx; + T[0][3] += xy * gxy; + T[0][4] += x * gxx; + T[0][5] += x * gxy; + + T[1][1] += xx * gyy; + T[1][2] += xy * gxy; + T[1][3] += xy * gyy; + T[1][4] += x * gxy; + T[1][5] += x * gyy; + + T[2][2] += yy * gxx; + T[2][3] += yy * gxy; + T[2][4] += y * gxx; + T[2][5] += y * gxy; + + T[3][3] += yy * gyy; + T[3][4] += y * gxy; + T[3][5] += y * gyy; + + T[4][4] += gxx; + T[4][5] += gxy; + + T[5][5] += gyy; + } + } + + for (j = 0 ; j < 5 ; j++) { + for (i = j+1 ; i < 6 ; i++) { + T[i][j] = T[j][i]; + } + } + +} + + + +/********************************************************************* + * _am_compute6by1ErrorVector + * + */ + +static void _am_compute6by1ErrorVector( + _FloatWindow imgdiff, + _FloatWindow gradx, + _FloatWindow grady, + int width, /* size of window */ + int height, + float **e) /* return values */ +{ + register int hw = width/2, hh = height/2; + register int i, j; + register float diff, diffgradx, diffgrady; + + /* Set values to zero */ + for(i = 0; i < 6; i++) e[i][0] = 0.0; + + /* Compute values */ + for (j = -hh ; j <= hh ; j++) { + for (i = -hw ; i <= hw ; i++) { + diff = *imgdiff++; + diffgradx = diff * (*gradx++); + diffgrady = diff * (*grady++); + e[0][0] += diffgradx * i; + e[1][0] += diffgrady * i; + e[2][0] += diffgradx * j; + e[3][0] += diffgrady * j; + e[4][0] += diffgradx; + e[5][0] += diffgrady; + } + } + + for(i = 0; i < 6; i++) e[i][0] *= 0.5; + +} + + +/********************************************************************* + * _am_compute4by4GradientMatrix + * + */ + +static void _am_compute4by4GradientMatrix( + _FloatWindow gradx, + _FloatWindow grady, + int width, /* size of window */ + int height, + float **T) /* return values */ +{ + register int hw = width/2, hh = height/2; + register int i, j; + float gx, gy, x, y; + + + /* Set values to zero */ + for (j = 0 ; j < 4 ; j++) { + for (i = 0 ; i < 4 ; i++) { + T[j][i] = 0.0; + } + } + + for (j = -hh ; j <= hh ; j++) { + for (i = -hw ; i <= hw ; i++) { + gx = *gradx++; + gy = *grady++; + x = (float) i; + y = (float) j; + T[0][0] += (x*gx+y*gy) * (x*gx+y*gy); + T[0][1] += (x*gx+y*gy)*(x*gy-y*gx); + T[0][2] += (x*gx+y*gy)*gx; + T[0][3] += (x*gx+y*gy)*gy; + + T[1][1] += (x*gy-y*gx) * (x*gy-y*gx); + T[1][2] += (x*gy-y*gx)*gx; + T[1][3] += (x*gy-y*gx)*gy; + + T[2][2] += gx*gx; + T[2][3] += gx*gy; + + T[3][3] += gy*gy; + } + } + + for (j = 0 ; j < 3 ; j++) { + for (i = j+1 ; i < 4 ; i++) { + T[i][j] = T[j][i]; + } + } + +} + +/********************************************************************* + * _am_compute4by1ErrorVector + * + */ + +static void _am_compute4by1ErrorVector( + _FloatWindow imgdiff, + _FloatWindow gradx, + _FloatWindow grady, + int width, /* size of window */ + int height, + float **e) /* return values */ +{ + register int hw = width/2, hh = height/2; + register int i, j; + register float diff, diffgradx, diffgrady; + + /* Set values to zero */ + for(i = 0; i < 4; i++) e[i][0] = 0.0; + + /* Compute values */ + for (j = -hh ; j <= hh ; j++) { + for (i = -hw ; i <= hw ; i++) { + diff = *imgdiff++; + diffgradx = diff * (*gradx++); + diffgrady = diff * (*grady++); + e[0][0] += diffgradx * i + diffgrady * j; + e[1][0] += diffgrady * i - diffgradx * j; + e[2][0] += diffgradx; + e[3][0] += diffgrady; + } + } + + for(i = 0; i < 4; i++) e[i][0] *= 0.5; + +} + + + +/********************************************************************* + * _am_trackFeatureAffine + * + * Tracks a feature point from the image of first occurrence to the actual image. + * + * RETURNS + * KLT_SMALL_DET or KLT_LARGE_RESIDUE or KLT_OOB if feature is lost, + * KLT_TRACKED otherwise. + */ + +/* if you enalbe the DEBUG_AFFINE_MAPPING make sure you have created a directory "./debug" */ +/* #define DEBUG_AFFINE_MAPPING */ + +#ifdef DEBUG_AFFINE_MAPPING +static int counter = 0; +static int glob_index = 0; +#endif + +static int _am_trackFeatureAffine( + float x1, /* location of window in first image */ + float y1, + float *x2, /* starting location of search in second image */ + float *y2, + _KLT_FloatImage img1, + _KLT_FloatImage gradx1, + _KLT_FloatImage grady1, + _KLT_FloatImage img2, + _KLT_FloatImage gradx2, + _KLT_FloatImage grady2, + int width, /* size of window */ + int height, + float step_factor, /* 2.0 comes from equations, 1.0 seems to avoid overshooting */ + int max_iterations, + float small, /* determinant threshold for declaring KLT_SMALL_DET */ + float th, /* displacement threshold for stopping */ + float th_aff, + float max_residue, /* residue threshold for declaring KLT_LARGE_RESIDUE */ + int lighting_insensitive, /* whether to normalize for gain and bias */ + int affine_map, /* whether to evaluates the consistency of features with affine mapping */ + float mdd, /* difference between the displacements */ + float *Axx, float *Ayx, + float *Axy, float *Ayy) /* used affine mapping */ +{ + + + _FloatWindow imgdiff, gradx, grady; + float gxx, gxy, gyy, ex, ey, dx, dy; + int iteration = 0; + int status = 0; + int hw = width/2; + int hh = height/2; + int nc1 = img1->ncols; + int nr1 = img1->nrows; + int nc2 = img2->ncols; + int nr2 = img2->nrows; + float **a; + float **T; + float one_plus_eps = 1.001f; /* To prevent rounding errors */ + float old_x2 = *x2; + float old_y2 = *y2; + KLT_BOOL convergence = FALSE; + +#ifdef DEBUG_AFFINE_MAPPING + char fname[80]; + _KLT_FloatImage aff_diff_win = _KLTCreateFloatImage(width,height); + printf("starting location x2=%f y2=%f\n", *x2, *y2); +#endif + + /* Allocate memory for windows */ + imgdiff = _allocateFloatWindow(width, height); + gradx = _allocateFloatWindow(width, height); + grady = _allocateFloatWindow(width, height); + T = _am_matrix(6,6); + a = _am_matrix(6,1); + + /* Iteratively update the window position */ + do { + if(!affine_map) { + /* pure translation tracker */ + + /* If out of bounds, exit loop */ + if ( x1-hw < 0.0f || nc1-( x1+hw) < one_plus_eps || + *x2-hw < 0.0f || nc2-(*x2+hw) < one_plus_eps || + y1-hh < 0.0f || nr1-( y1+hh) < one_plus_eps || + *y2-hh < 0.0f || nr2-(*y2+hh) < one_plus_eps) { + status = KLT_OOB; + break; + } + + /* Compute gradient and difference windows */ + if (lighting_insensitive) { + _computeIntensityDifferenceLightingInsensitive(img1, img2, x1, y1, *x2, *y2, + width, height, imgdiff); + _computeGradientSumLightingInsensitive(gradx1, grady1, gradx2, grady2, + img1, img2, x1, y1, *x2, *y2, width, height, gradx, grady); + } else { + _computeIntensityDifference(img1, img2, x1, y1, *x2, *y2, + width, height, imgdiff); + _computeGradientSum(gradx1, grady1, gradx2, grady2, + x1, y1, *x2, *y2, width, height, gradx, grady); + } + +#ifdef DEBUG_AFFINE_MAPPING + aff_diff_win->data = imgdiff; + sprintf(fname, "./debug/kltimg_trans_diff_win%03d.%03d.pgm", glob_index, counter); + printf("%s\n", fname); + _KLTWriteAbsFloatImageToPGM(aff_diff_win, fname,256.0); + printf("iter = %d translation tracker res: %f\n", iteration, _sumAbsFloatWindow(imgdiff, width, height)/(width*height)); +#endif + + /* Use these windows to construct matrices */ + _compute2by2GradientMatrix(gradx, grady, width, height, + &gxx, &gxy, &gyy); + _compute2by1ErrorVector(imgdiff, gradx, grady, width, height, step_factor, + &ex, &ey); + + /* Using matrices, solve equation for new displacement */ + status = _solveEquation(gxx, gxy, gyy, ex, ey, small, &dx, &dy); + + convergence = (fabs(dx) < th && fabs(dy) < th); + + *x2 += dx; + *y2 += dy; + + }else{ + /* affine tracker */ + + float ul_x = *Axx * (-hw) + *Axy * hh + *x2; /* upper left corner */ + float ul_y = *Ayx * (-hw) + *Ayy * hh + *y2; + float ll_x = *Axx * (-hw) + *Axy * (-hh) + *x2; /* lower left corner */ + float ll_y = *Ayx * (-hw) + *Ayy * (-hh) + *y2; + float ur_x = *Axx * hw + *Axy * hh + *x2; /* upper right corner */ + float ur_y = *Ayx * hw + *Ayy * hh + *y2; + float lr_x = *Axx * hw + *Axy * (-hh) + *x2; /* lower right corner */ + float lr_y = *Ayx * hw + *Ayy * (-hh) + *y2; + + /* If out of bounds, exit loop */ + if ( x1-hw < 0.0f || nc1-(x1+hw) < one_plus_eps || + y1-hh < 0.0f || nr1-(y1+hh) < one_plus_eps || + ul_x < 0.0f || nc2-(ul_x ) < one_plus_eps || + ll_x < 0.0f || nc2-(ll_x ) < one_plus_eps || + ur_x < 0.0f || nc2-(ur_x ) < one_plus_eps || + lr_x < 0.0f || nc2-(lr_x ) < one_plus_eps || + ul_y < 0.0f || nr2-(ul_y ) < one_plus_eps || + ll_y < 0.0f || nr2-(ll_y ) < one_plus_eps || + ur_y < 0.0f || nr2-(ur_y ) < one_plus_eps || + lr_y < 0.0f || nr2-(lr_y ) < one_plus_eps) { + status = KLT_OOB; + break; + } + +#ifdef DEBUG_AFFINE_MAPPING + counter++; + _am_computeAffineMappedImage(img1, x1, y1, 1.0, 0.0 , 0.0, 1.0, width, height, imgdiff); + aff_diff_win->data = imgdiff; + sprintf(fname, "./debug/kltimg_aff_diff_win%03d.%03d_1.pgm", glob_index, counter); + printf("%s\n", fname); + _KLTWriteAbsFloatImageToPGM(aff_diff_win, fname,256.0); + + _am_computeAffineMappedImage(img2, *x2, *y2, *Axx, *Ayx , *Axy, *Ayy, width, height, imgdiff); + aff_diff_win->data = imgdiff; + sprintf(fname, "./debug/kltimg_aff_diff_win%03d.%03d_2.pgm", glob_index, counter); + printf("%s\n", fname); + _KLTWriteAbsFloatImageToPGM(aff_diff_win, fname,256.0); +#endif + + _am_computeIntensityDifferenceAffine(img1, img2, x1, y1, *x2, *y2, *Axx, *Ayx , *Axy, *Ayy, + width, height, imgdiff); +#ifdef DEBUG_AFFINE_MAPPING + aff_diff_win->data = imgdiff; + sprintf(fname, "./debug/kltimg_aff_diff_win%03d.%03d_3.pgm", glob_index,counter); + printf("%s\n", fname); + _KLTWriteAbsFloatImageToPGM(aff_diff_win, fname,256.0); + + printf("iter = %d affine tracker res: %f\n", iteration, _sumAbsFloatWindow(imgdiff, width, height)/(width*height)); +#endif + + _am_getGradientWinAffine(gradx2, grady2, *x2, *y2, *Axx, *Ayx , *Axy, *Ayy, + width, height, gradx, grady); + + switch(affine_map){ + case 1: + _am_compute4by1ErrorVector(imgdiff, gradx, grady, width, height, a); + _am_compute4by4GradientMatrix(gradx, grady, width, height, T); + + status = _am_gauss_jordan_elimination(T,4,a,1); + + *Axx += a[0][0]; + *Ayx += a[1][0]; + *Ayy = *Axx; + *Axy = -(*Ayx); + + dx = a[2][0]; + dy = a[3][0]; + + break; + case 2: + _am_compute6by1ErrorVector(imgdiff, gradx, grady, width, height, a); + _am_compute6by6GradientMatrix(gradx, grady, width, height, T); + + status = _am_gauss_jordan_elimination(T,6,a,1); + + *Axx += a[0][0]; + *Ayx += a[1][0]; + *Axy += a[2][0]; + *Ayy += a[3][0]; + + dx = a[4][0]; + dy = a[5][0]; + + break; + } + + *x2 += dx; + *y2 += dy; + + /* old upper left corner - new upper left corner */ + ul_x -= *Axx * (-hw) + *Axy * hh + *x2; + ul_y -= *Ayx * (-hw) + *Ayy * hh + *y2; + /* old lower left corner - new lower left corner */ + ll_x -= *Axx * (-hw) + *Axy * (-hh) + *x2; + ll_y -= *Ayx * (-hw) + *Ayy * (-hh) + *y2; + /* old upper right corner - new upper right corner */ + ur_x -= *Axx * hw + *Axy * hh + *x2; + ur_y -= *Ayx * hw + *Ayy * hh + *y2; + /* old lower right corner - new lower right corner */ + lr_x -= *Axx * hw + *Axy * (-hh) + *x2; + lr_y -= *Ayx * hw + *Ayy * (-hh) + *y2; + +#ifdef DEBUG_AFFINE_MAPPING + printf ("iter = %d, ul_x=%f ul_y=%f ll_x=%f ll_y=%f ur_x=%f ur_y=%f lr_x=%f lr_y=%f \n", + iteration, ul_x, ul_y, ll_x, ll_y, ur_x, ur_y, lr_x, lr_y); +#endif + + convergence = (fabs(dx) < th && fabs(dy) < th && + fabs(ul_x) < th_aff && fabs(ul_y) < th_aff && + fabs(ll_x) < th_aff && fabs(ll_y) < th_aff && + fabs(ur_x) < th_aff && fabs(ur_y) < th_aff && + fabs(lr_x) < th_aff && fabs(lr_y) < th_aff); + } + + if (status == KLT_SMALL_DET) break; + iteration++; +#ifdef DEBUG_AFFINE_MAPPING + printf ("iter = %d, x1=%f, y1=%f, x2=%f, y2=%f, Axx=%f, Ayx=%f , Axy=%f, Ayy=%f \n",iteration, x1, y1, *x2, *y2, *Axx, *Ayx , *Axy, *Ayy); +#endif + } while ( !convergence && iteration < max_iterations); + /*} while ( (fabs(dx)>=th || fabs(dy)>=th || (affine_map && iteration < 8) ) && iteration < max_iterations); */ + _am_free_matrix(T); + _am_free_matrix(a); + + /* Check whether window is out of bounds */ + if (*x2-hw < 0.0f || nc2-(*x2+hw) < one_plus_eps || + *y2-hh < 0.0f || nr2-(*y2+hh) < one_plus_eps) + status = KLT_OOB; + + /* Check whether feature point has moved to much during iteration*/ + if ( (*x2-old_x2) > mdd || (*y2-old_y2) > mdd ) + status = KLT_OOB; + + /* Check whether residue is too large */ + if (status == KLT_TRACKED) { + if(!affine_map){ + _computeIntensityDifference(img1, img2, x1, y1, *x2, *y2, + width, height, imgdiff); + }else{ + _am_computeIntensityDifferenceAffine(img1, img2, x1, y1, *x2, *y2, *Axx, *Ayx , *Axy, *Ayy, + width, height, imgdiff); + } +#ifdef DEBUG_AFFINE_MAPPING + printf("iter = %d final_res = %f\n", iteration, _sumAbsFloatWindow(imgdiff, width, height)/(width*height)); +#endif + if (_sumAbsFloatWindow(imgdiff, width, height)/(width*height) > max_residue) + status = KLT_LARGE_RESIDUE; + } + + /* Free memory */ + free(imgdiff); free(gradx); free(grady); + +#ifdef DEBUG_AFFINE_MAPPING + printf("iter = %d status=%d\n", iteration, status); + _KLTFreeFloatImage( aff_diff_win ); +#endif + + /* Return appropriate value */ + return status; +} + +/* + * CONSISTENCY CHECK OF FEATURES BY AFFINE MAPPING (END) + **********************************************************************/ + + + +/********************************************************************* + * KLTTrackFeatures + * + * Tracks feature points from one image to the next. + */ + +void KLTTrackFeatures( + KLT_TrackingContext tc, + KLT_PixelType *img1, + KLT_PixelType *img2, + int ncols, + int nrows, + KLT_FeatureList featurelist) +{ + _KLT_FloatImage tmpimg, floatimg1, floatimg2; + _KLT_Pyramid pyramid1, pyramid1_gradx, pyramid1_grady, + pyramid2, pyramid2_gradx, pyramid2_grady; + float subsampling = (float) tc->subsampling; + float xloc, yloc, xlocout, ylocout; + int val; + int indx, r; + KLT_BOOL floatimg1_created = FALSE; + int i; + + if (KLT_verbose >= 1) { + fprintf(stderr, "(KLT) Tracking %d features in a %d by %d image... ", + KLTCountRemainingFeatures(featurelist), ncols, nrows); + fflush(stderr); + } + + /* Check window size (and correct if necessary) */ + if (tc->window_width % 2 != 1) { + tc->window_width = tc->window_width+1; + KLTWarning("Tracking context's window width must be odd. " + "Changing to %d.\n", tc->window_width); + } + if (tc->window_height % 2 != 1) { + tc->window_height = tc->window_height+1; + KLTWarning("Tracking context's window height must be odd. " + "Changing to %d.\n", tc->window_height); + } + if (tc->window_width < 3) { + tc->window_width = 3; + KLTWarning("Tracking context's window width must be at least three. \n" + "Changing to %d.\n", tc->window_width); + } + if (tc->window_height < 3) { + tc->window_height = 3; + KLTWarning("Tracking context's window height must be at least three. \n" + "Changing to %d.\n", tc->window_height); + } + + /* Create temporary image */ + tmpimg = _KLTCreateFloatImage(ncols, nrows); + + /* Process first image by converting to float, smoothing, computing */ + /* pyramid, and computing gradient pyramids */ + if (tc->sequentialMode && tc->pyramid_last != NULL) { + pyramid1 = (_KLT_Pyramid) tc->pyramid_last; + pyramid1_gradx = (_KLT_Pyramid) tc->pyramid_last_gradx; + pyramid1_grady = (_KLT_Pyramid) tc->pyramid_last_grady; + if (pyramid1->ncols[0] != ncols || pyramid1->nrows[0] != nrows) + KLTError("(KLTTrackFeatures) Size of incoming image (%d by %d) " + "is different from size of previous image (%d by %d)\n", + ncols, nrows, pyramid1->ncols[0], pyramid1->nrows[0]); + assert(pyramid1_gradx != NULL); + assert(pyramid1_grady != NULL); + } else { + floatimg1_created = TRUE; + floatimg1 = _KLTCreateFloatImage(ncols, nrows); + _KLTToFloatImage(img1, ncols, nrows, tmpimg); + _KLTComputeSmoothedImage(tmpimg, _KLTComputeSmoothSigma(tc), floatimg1); + pyramid1 = _KLTCreatePyramid(ncols, nrows, (int) subsampling, tc->nPyramidLevels); + _KLTComputePyramid(floatimg1, pyramid1, tc->pyramid_sigma_fact); + pyramid1_gradx = _KLTCreatePyramid(ncols, nrows, (int) subsampling, tc->nPyramidLevels); + pyramid1_grady = _KLTCreatePyramid(ncols, nrows, (int) subsampling, tc->nPyramidLevels); + for (i = 0 ; i < tc->nPyramidLevels ; i++) + _KLTComputeGradients(pyramid1->img[i], tc->grad_sigma, + pyramid1_gradx->img[i], + pyramid1_grady->img[i]); + } + + /* Do the same thing with second image */ + floatimg2 = _KLTCreateFloatImage(ncols, nrows); + _KLTToFloatImage(img2, ncols, nrows, tmpimg); + _KLTComputeSmoothedImage(tmpimg, _KLTComputeSmoothSigma(tc), floatimg2); + pyramid2 = _KLTCreatePyramid(ncols, nrows, (int) subsampling, tc->nPyramidLevels); + _KLTComputePyramid(floatimg2, pyramid2, tc->pyramid_sigma_fact); + pyramid2_gradx = _KLTCreatePyramid(ncols, nrows, (int) subsampling, tc->nPyramidLevels); + pyramid2_grady = _KLTCreatePyramid(ncols, nrows, (int) subsampling, tc->nPyramidLevels); + for (i = 0 ; i < tc->nPyramidLevels ; i++) + _KLTComputeGradients(pyramid2->img[i], tc->grad_sigma, + pyramid2_gradx->img[i], + pyramid2_grady->img[i]); + + /* Write internal images */ + if (tc->writeInternalImages) { + char fname[80]; + for (i = 0 ; i < tc->nPyramidLevels ; i++) { + sprintf(fname, "kltimg_tf_i%d.pgm", i); + _KLTWriteFloatImageToPGM(pyramid1->img[i], fname); + sprintf(fname, "kltimg_tf_i%d_gx.pgm", i); + _KLTWriteFloatImageToPGM(pyramid1_gradx->img[i], fname); + sprintf(fname, "kltimg_tf_i%d_gy.pgm", i); + _KLTWriteFloatImageToPGM(pyramid1_grady->img[i], fname); + sprintf(fname, "kltimg_tf_j%d.pgm", i); + _KLTWriteFloatImageToPGM(pyramid2->img[i], fname); + sprintf(fname, "kltimg_tf_j%d_gx.pgm", i); + _KLTWriteFloatImageToPGM(pyramid2_gradx->img[i], fname); + sprintf(fname, "kltimg_tf_j%d_gy.pgm", i); + _KLTWriteFloatImageToPGM(pyramid2_grady->img[i], fname); + } + } + + /* For each feature, do ... */ + for (indx = 0 ; indx < featurelist->nFeatures ; indx++) { + + /* Only track features that are not lost */ + if (featurelist->feature[indx]->val >= 0) { + + xloc = featurelist->feature[indx]->x; + yloc = featurelist->feature[indx]->y; + + /* Transform location to coarsest resolution */ + for (r = tc->nPyramidLevels - 1 ; r >= 0 ; r--) { + xloc /= subsampling; yloc /= subsampling; + } + xlocout = xloc; ylocout = yloc; + + /* Beginning with coarsest resolution, do ... */ + for (r = tc->nPyramidLevels - 1 ; r >= 0 ; r--) { + + /* Track feature at current resolution */ + xloc *= subsampling; yloc *= subsampling; + xlocout *= subsampling; ylocout *= subsampling; + + val = _trackFeature(xloc, yloc, + &xlocout, &ylocout, + pyramid1->img[r], + pyramid1_gradx->img[r], pyramid1_grady->img[r], + pyramid2->img[r], + pyramid2_gradx->img[r], pyramid2_grady->img[r], + tc->window_width, tc->window_height, + tc->step_factor, + tc->max_iterations, + tc->min_determinant, + tc->min_displacement, + tc->max_residue, + tc->lighting_insensitive); + + if (val==KLT_SMALL_DET || val==KLT_OOB) + break; + } + + /* Record feature */ + if (val == KLT_OOB) { + featurelist->feature[indx]->x = -1.0; + featurelist->feature[indx]->y = -1.0; + featurelist->feature[indx]->val = KLT_OOB; + if( featurelist->feature[indx]->aff_img ) _KLTFreeFloatImage(featurelist->feature[indx]->aff_img); + if( featurelist->feature[indx]->aff_img_gradx ) _KLTFreeFloatImage(featurelist->feature[indx]->aff_img_gradx); + if( featurelist->feature[indx]->aff_img_grady ) _KLTFreeFloatImage(featurelist->feature[indx]->aff_img_grady); + featurelist->feature[indx]->aff_img = NULL; + featurelist->feature[indx]->aff_img_gradx = NULL; + featurelist->feature[indx]->aff_img_grady = NULL; + + } else if (_outOfBounds(xlocout, ylocout, ncols, nrows, tc->borderx, tc->bordery)) { + featurelist->feature[indx]->x = -1.0; + featurelist->feature[indx]->y = -1.0; + featurelist->feature[indx]->val = KLT_OOB; + if( featurelist->feature[indx]->aff_img ) _KLTFreeFloatImage(featurelist->feature[indx]->aff_img); + if( featurelist->feature[indx]->aff_img_gradx ) _KLTFreeFloatImage(featurelist->feature[indx]->aff_img_gradx); + if( featurelist->feature[indx]->aff_img_grady ) _KLTFreeFloatImage(featurelist->feature[indx]->aff_img_grady); + featurelist->feature[indx]->aff_img = NULL; + featurelist->feature[indx]->aff_img_gradx = NULL; + featurelist->feature[indx]->aff_img_grady = NULL; + } else if (val == KLT_SMALL_DET) { + featurelist->feature[indx]->x = -1.0; + featurelist->feature[indx]->y = -1.0; + featurelist->feature[indx]->val = KLT_SMALL_DET; + if( featurelist->feature[indx]->aff_img ) _KLTFreeFloatImage(featurelist->feature[indx]->aff_img); + if( featurelist->feature[indx]->aff_img_gradx ) _KLTFreeFloatImage(featurelist->feature[indx]->aff_img_gradx); + if( featurelist->feature[indx]->aff_img_grady ) _KLTFreeFloatImage(featurelist->feature[indx]->aff_img_grady); + featurelist->feature[indx]->aff_img = NULL; + featurelist->feature[indx]->aff_img_gradx = NULL; + featurelist->feature[indx]->aff_img_grady = NULL; + } else if (val == KLT_LARGE_RESIDUE) { + featurelist->feature[indx]->x = -1.0; + featurelist->feature[indx]->y = -1.0; + featurelist->feature[indx]->val = KLT_LARGE_RESIDUE; + if( featurelist->feature[indx]->aff_img ) _KLTFreeFloatImage(featurelist->feature[indx]->aff_img); + if( featurelist->feature[indx]->aff_img_gradx ) _KLTFreeFloatImage(featurelist->feature[indx]->aff_img_gradx); + if( featurelist->feature[indx]->aff_img_grady ) _KLTFreeFloatImage(featurelist->feature[indx]->aff_img_grady); + featurelist->feature[indx]->aff_img = NULL; + featurelist->feature[indx]->aff_img_gradx = NULL; + featurelist->feature[indx]->aff_img_grady = NULL; + } else if (val == KLT_MAX_ITERATIONS) { + featurelist->feature[indx]->x = -1.0; + featurelist->feature[indx]->y = -1.0; + featurelist->feature[indx]->val = KLT_MAX_ITERATIONS; + if( featurelist->feature[indx]->aff_img ) _KLTFreeFloatImage(featurelist->feature[indx]->aff_img); + if( featurelist->feature[indx]->aff_img_gradx ) _KLTFreeFloatImage(featurelist->feature[indx]->aff_img_gradx); + if( featurelist->feature[indx]->aff_img_grady ) _KLTFreeFloatImage(featurelist->feature[indx]->aff_img_grady); + featurelist->feature[indx]->aff_img = NULL; + featurelist->feature[indx]->aff_img_gradx = NULL; + featurelist->feature[indx]->aff_img_grady = NULL; + } else { + featurelist->feature[indx]->x = xlocout; + featurelist->feature[indx]->y = ylocout; + featurelist->feature[indx]->val = KLT_TRACKED; + if (tc->affineConsistencyCheck >= 0 && val == KLT_TRACKED) { /*for affine mapping*/ + int border = 2; /* add border for interpolation */ + +#ifdef DEBUG_AFFINE_MAPPING + glob_index = indx; +#endif + + if(!featurelist->feature[indx]->aff_img){ + /* save image and gradient for each feature at finest resolution after first successful track */ + featurelist->feature[indx]->aff_img = _KLTCreateFloatImage((tc->affine_window_width+border), (tc->affine_window_height+border)); + featurelist->feature[indx]->aff_img_gradx = _KLTCreateFloatImage((tc->affine_window_width+border), (tc->affine_window_height+border)); + featurelist->feature[indx]->aff_img_grady = _KLTCreateFloatImage((tc->affine_window_width+border), (tc->affine_window_height+border)); + _am_getSubFloatImage(pyramid1->img[0],xloc,yloc,featurelist->feature[indx]->aff_img); + _am_getSubFloatImage(pyramid1_gradx->img[0],xloc,yloc,featurelist->feature[indx]->aff_img_gradx); + _am_getSubFloatImage(pyramid1_grady->img[0],xloc,yloc,featurelist->feature[indx]->aff_img_grady); + featurelist->feature[indx]->aff_x = xloc - (int) xloc + (tc->affine_window_width+border)/2; + featurelist->feature[indx]->aff_y = yloc - (int) yloc + (tc->affine_window_height+border)/2;; + }else{ + /* affine tracking */ + val = _am_trackFeatureAffine(featurelist->feature[indx]->aff_x, featurelist->feature[indx]->aff_y, + &xlocout, &ylocout, + featurelist->feature[indx]->aff_img, + featurelist->feature[indx]->aff_img_gradx, + featurelist->feature[indx]->aff_img_grady, + pyramid2->img[0], + pyramid2_gradx->img[0], pyramid2_grady->img[0], + tc->affine_window_width, tc->affine_window_height, + tc->step_factor, + tc->affine_max_iterations, + tc->min_determinant, + tc->min_displacement, + tc->affine_min_displacement, + tc->affine_max_residue, + tc->lighting_insensitive, + tc->affineConsistencyCheck, + tc->affine_max_displacement_differ, + &featurelist->feature[indx]->aff_Axx, + &featurelist->feature[indx]->aff_Ayx, + &featurelist->feature[indx]->aff_Axy, + &featurelist->feature[indx]->aff_Ayy + ); + featurelist->feature[indx]->val = val; + if(val != KLT_TRACKED){ + featurelist->feature[indx]->x = -1.0; + featurelist->feature[indx]->y = -1.0; + featurelist->feature[indx]->aff_x = -1.0; + featurelist->feature[indx]->aff_y = -1.0; + /* free image and gradient for lost feature */ + _KLTFreeFloatImage(featurelist->feature[indx]->aff_img); + _KLTFreeFloatImage(featurelist->feature[indx]->aff_img_gradx); + _KLTFreeFloatImage(featurelist->feature[indx]->aff_img_grady); + featurelist->feature[indx]->aff_img = NULL; + featurelist->feature[indx]->aff_img_gradx = NULL; + featurelist->feature[indx]->aff_img_grady = NULL; + }else{ + /*featurelist->feature[indx]->x = xlocout;*/ + /*featurelist->feature[indx]->y = ylocout;*/ + } + } + } + + } + } + } + + if (tc->sequentialMode) { + tc->pyramid_last = pyramid2; + tc->pyramid_last_gradx = pyramid2_gradx; + tc->pyramid_last_grady = pyramid2_grady; + } else { + _KLTFreePyramid(pyramid2); + _KLTFreePyramid(pyramid2_gradx); + _KLTFreePyramid(pyramid2_grady); + } + + /* Free memory */ + _KLTFreeFloatImage(tmpimg); + if (floatimg1_created) _KLTFreeFloatImage(floatimg1); + _KLTFreeFloatImage(floatimg2); + _KLTFreePyramid(pyramid1); + _KLTFreePyramid(pyramid1_gradx); + _KLTFreePyramid(pyramid1_grady); + + if (KLT_verbose >= 1) { + fprintf(stderr, "\n\t%d features successfully tracked.\n", + KLTCountRemainingFeatures(featurelist)); + if (tc->writeInternalImages) + fprintf(stderr, "\tWrote images to 'kltimg_tf*.pgm'.\n"); + fflush(stderr); + } + +} + + diff --git a/rtengine/klt/writeFeatures.c b/rtengine/klt/writeFeatures.c new file mode 100644 index 000000000..78658c121 --- /dev/null +++ b/rtengine/klt/writeFeatures.c @@ -0,0 +1,743 @@ +/********************************************************************* + * writeFeatures.c + * + ********************************************************************* + */ + +/* Standard includes */ +#include +#include /* isdigit() */ +#include /* sprintf(), fprintf(), sscanf(), fscanf() */ +#include /* malloc() */ +#include /* memcpy(), strcmp() */ + +/* Our includes */ +#include "base.h" +#include "error.h" +#include "pnmio.h" /* ppmWriteFileRGB() */ +#include "klt.h" + +#define BINHEADERLENGTH 6 + +extern int KLT_verbose; + +typedef enum {FEATURE_LIST, FEATURE_HISTORY, FEATURE_TABLE} structureType; + +static char warning_line[] = "!!! Warning: This is a KLT data file. " + "Do not modify below this line !!!\n"; +static char binheader_fl[BINHEADERLENGTH+1] = "KLTFL1"; +static char binheader_fh[BINHEADERLENGTH+1] = "KLTFH1"; +static char binheader_ft[BINHEADERLENGTH+1] = "KLTFT1"; + +/********************************************************************* + * KLTWriteFeatureListToPPM + */ + +void KLTWriteFeatureListToPPM( + KLT_FeatureList featurelist, + KLT_PixelType *greyimg, + int ncols, + int nrows, + char *filename) +{ + int nbytes = ncols * nrows * sizeof(char); + uchar *redimg, *grnimg, *bluimg; + int offset; + int x, y, xx, yy; + int i; + + if (KLT_verbose >= 1) + fprintf(stderr, "(KLT) Writing %d features to PPM file: '%s'\n", + KLTCountRemainingFeatures(featurelist), filename); + + /* Allocate memory for component images */ + redimg = (uchar *) malloc(nbytes); + grnimg = (uchar *) malloc(nbytes); + bluimg = (uchar *) malloc(nbytes); + if (redimg == NULL || grnimg == NULL || bluimg == NULL) + KLTError("(KLTWriteFeaturesToPPM) Out of memory\n"); + + /* Copy grey image to component images */ + if (sizeof(KLT_PixelType) != 1) + KLTWarning("(KLTWriteFeaturesToPPM) KLT_PixelType is not uchar"); + memcpy(redimg, greyimg, nbytes); + memcpy(grnimg, greyimg, nbytes); + memcpy(bluimg, greyimg, nbytes); + + /* Overlay features in red */ + for (i = 0 ; i < featurelist->nFeatures ; i++) + if (featurelist->feature[i]->val >= 0) { + x = (int) (featurelist->feature[i]->x + 0.5); + y = (int) (featurelist->feature[i]->y + 0.5); + for (yy = y - 1 ; yy <= y + 1 ; yy++) + for (xx = x - 1 ; xx <= x + 1 ; xx++) + if (xx >= 0 && yy >= 0 && xx < ncols && yy < nrows) { + offset = yy * ncols + xx; + *(redimg + offset) = 255; + *(grnimg + offset) = 0; + *(bluimg + offset) = 0; + } + } + + /* Write to PPM file */ + ppmWriteFileRGB(filename, redimg, grnimg, bluimg, ncols, nrows); + + /* Free memory */ + free(redimg); + free(grnimg); + free(bluimg); +} + + +static FILE* _printSetupTxt( + char *fname, /* Input: filename, or NULL for stderr */ + char *fmt, /* Input: format (e.g., %5.1f or %3d) */ + char *format, /* Output: format (e.g., (%5.1f,%5.1f)=%3d) */ + char *type) /* Output: either 'f' or 'd', based on input format */ +{ + FILE *fp; + const int val_width = 5; + int i; + + /* Either open file or use stderr */ + if (fname == NULL) fp = stderr; + else fp = fopen(fname, "wb"); + if (fp == NULL) + KLTError("(KLTWriteFeatures) " + "Can't open file '%s' for writing\n", fname); + + /* Parse format */ + if (fmt[0] != '%') + KLTError("(KLTWriteFeatures) Bad Format: %s\n", fmt); + i = 0; while (fmt[i] != '\0') i++; *type = fmt[i-1]; + if (*type != 'f' && *type != 'd') + KLTError("(KLTWriteFeatures) Format must end in 'f' or 'd'."); + + /* Construct feature format */ + sprintf(format, "(%s,%s)=%%%dd ", fmt, fmt, val_width); + + return fp; +} + + +static FILE* _printSetupBin( + char *fname) /* Input: filename */ +{ + FILE *fp; + if (fname == NULL) + KLTError("(KLTWriteFeatures) Can't write binary data to stderr"); + fp = fopen(fname, "wb"); + if (fp == NULL) + KLTError("(KLTWriteFeatures) " + "Can't open file '%s' for writing", fname); + return fp; +} + + +static void _printNhyphens( + FILE *fp, + int n) +{ + int i; + for (i = 0 ; i < n ; i++) + fprintf(fp, "-"); +} + +static void _printInteger( + FILE *fp, + int integer, + int width) +{ + char fmt[80]; + sprintf(fmt, "%%%dd", width); + fprintf(fp, fmt, integer); +} + + +static KLT_BOOL _isCharInString( + char c, + char *str) +{ + int width = strlen(str); + int i; + + for (i = 0 ; i < width ; i++) + if (c == str[i]) return TRUE; + + return FALSE; +} + + +/********************************************************************* + * _findStringWidth + * + * Calculates the length of a string after expansion. E.g., the + * length of "(%6.1f)" is eight -- six for the floating-point number, + * and two for the parentheses. + */ + +static int _findStringWidth( + char *str) +{ + int width = 0; + int add; + int maxi = strlen(str) - 1; + int i = 0; + + + while (str[i] != '\0') { + if (str[i] == '%') { + if (isdigit(str[i+1])) { + sscanf(str+i+1, "%d", &add); + width += add; + i += 2; + while (!_isCharInString(str[i], "diouxefgn")) { + i++; + if (i > maxi) + KLTError("(_findStringWidth) Can't determine length " + "of string '%s'", str); + } + i++; + } else if (str[i+1] == 'c') { + width++; + i += 2; + } else + KLTError("(_findStringWidth) Can't determine length " + "of string '%s'", str); + } else { + i++; + width++; + } + } + + return width; +} + + +static void _printHeader( + FILE *fp, + char *format, + structureType id, + int nFrames, + int nFeatures) +{ + int width = _findStringWidth(format); + int i; + + assert(id == FEATURE_LIST || id == FEATURE_HISTORY || id == FEATURE_TABLE); + + if (fp != stderr) { + fprintf(fp, "Feel free to place comments here.\n\n\n"); + fprintf(fp, "!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!\n"); + fprintf(fp, warning_line); + fprintf(fp, "\n"); + } + fprintf(fp, "------------------------------\n"); + switch (id) { + case FEATURE_LIST: fprintf(fp, "KLT Feature List\n"); break; + case FEATURE_HISTORY: fprintf(fp, "KLT Feature History\n"); break; + case FEATURE_TABLE: fprintf(fp, "KLT Feature Table\n"); break; + } + + fprintf(fp, "------------------------------\n\n"); + switch (id) { + case FEATURE_LIST: fprintf(fp, "nFeatures = %d\n\n", nFeatures); break; + case FEATURE_HISTORY: fprintf(fp, "nFrames = %d\n\n", nFrames); break; + case FEATURE_TABLE: fprintf(fp, "nFrames = %d, nFeatures = %d\n\n", + nFrames, nFeatures); break; + } + + switch (id) { + case FEATURE_LIST: fprintf(fp, "feature | (x,y)=val\n"); + fprintf(fp, "--------+-"); + _printNhyphens(fp, width); + fprintf(fp, "\n"); + break; + case FEATURE_HISTORY: fprintf(fp, "frame | (x,y)=val\n"); + fprintf(fp, "------+-"); + _printNhyphens(fp, width); + fprintf(fp, "\n"); + break; + case FEATURE_TABLE: fprintf(fp, "feature | frame\n"); + fprintf(fp, " |"); + for (i = 0 ; i < nFrames ; i++) _printInteger(fp, i, width); + fprintf(fp, "\n--------+-"); + for (i = 0 ; i < nFrames ; i++) _printNhyphens(fp, width); + fprintf(fp, "\n"); + break; + } +} + + +static void _printFeatureTxt( + FILE *fp, + KLT_Feature feat, + char *format, + char type) +{ + assert(type == 'f' || type == 'd'); + + if (type == 'f') + fprintf(fp, format, (float) feat->x, (float) feat->y, feat->val); + else if (type == 'd') { + /* Round x & y to nearest integer, unless negative */ + float x = feat->x; + float y = feat->y; + if (x >= 0.0) x += 0.5; + if (y >= 0.0) y += 0.5; + fprintf(fp, format, + (int) x, (int) y, feat->val); + } +} + + +static void _printFeatureBin( + FILE *fp, + KLT_Feature feat) +{ + fwrite(&(feat->x), sizeof(KLT_locType), 1, fp); + fwrite(&(feat->y), sizeof(KLT_locType), 1, fp); + fwrite(&(feat->val), sizeof(int), 1, fp); +} + + +static void _printShutdown( + FILE *fp) +{ + /* Close file, if necessary */ + if (fp != stderr) + fclose(fp); +} + + +/********************************************************************* + * KLTWriteFeatureList() + * KLTWriteFeatureHistory() + * KLTWriteFeatureTable() + * + * Writes features to file or to screen. + * + * INPUTS + * fname: name of file to write data; if NULL, then print to stderr + * fmt: format for printing (e.g., "%5.1f" or "%3d"); + * if NULL, and if fname is not NULL, then write to binary file. + */ + +void KLTWriteFeatureList( + KLT_FeatureList fl, + char *fname, + char *fmt) +{ + FILE *fp; + char format[100]; + char type; + int i; + + if (KLT_verbose >= 1 && fname != NULL) { + fprintf(stderr, + "(KLT) Writing feature list to %s file: '%s'\n", + (fmt == NULL ? "binary" : "text"), fname); + } + + if (fmt != NULL) { /* text file or stderr */ + fp = _printSetupTxt(fname, fmt, format, &type); + _printHeader(fp, format, FEATURE_LIST, 0, fl->nFeatures); + + for (i = 0 ; i < fl->nFeatures ; i++) { + fprintf(fp, "%7d | ", i); + _printFeatureTxt(fp, fl->feature[i], format, type); + fprintf(fp, "\n"); + } + _printShutdown(fp); + } else { /* binary file */ + fp = _printSetupBin(fname); + fwrite(binheader_fl, sizeof(char), BINHEADERLENGTH, fp); + fwrite(&(fl->nFeatures), sizeof(int), 1, fp); + for (i = 0 ; i < fl->nFeatures ; i++) { + _printFeatureBin(fp, fl->feature[i]); + } + fclose(fp); + } +} + + +void KLTWriteFeatureHistory( + KLT_FeatureHistory fh, + char *fname, + char *fmt) +{ + FILE *fp; + char format[100]; + char type; + int i; + + if (KLT_verbose >= 1 && fname != NULL) { + fprintf(stderr, + "(KLT) Writing feature history to %s file: '%s'\n", + (fmt == NULL ? "binary" : "text"), fname); + } + + if (fmt != NULL) { /* text file or stderr */ + fp = _printSetupTxt(fname, fmt, format, &type); + _printHeader(fp, format, FEATURE_HISTORY, fh->nFrames, 0); + + for (i = 0 ; i < fh->nFrames ; i++) { + fprintf(fp, "%5d | ", i); + _printFeatureTxt(fp, fh->feature[i], format, type); + fprintf(fp, "\n"); + } + _printShutdown(fp); + } else { /* binary file */ + fp = _printSetupBin(fname); + fwrite(binheader_fh, sizeof(char), BINHEADERLENGTH, fp); + fwrite(&(fh->nFrames), sizeof(int), 1, fp); + for (i = 0 ; i < fh->nFrames ; i++) { + _printFeatureBin(fp, fh->feature[i]); + } + fclose(fp); + } +} + + + +void KLTWriteFeatureTable( + KLT_FeatureTable ft, + char *fname, + char *fmt) +{ + FILE *fp; + char format[100]; + char type; + int i, j; + + if (KLT_verbose >= 1 && fname != NULL) { + fprintf(stderr, + "(KLT) Writing feature table to %s file: '%s'\n", + (fmt == NULL ? "binary" : "text"), fname); + } + + if (fmt != NULL) { /* text file or stderr */ + fp = _printSetupTxt(fname, fmt, format, &type); + _printHeader(fp, format, FEATURE_TABLE, ft->nFrames, ft->nFeatures); + + for (j = 0 ; j < ft->nFeatures ; j++) { + fprintf(fp, "%7d | ", j); + for (i = 0 ; i < ft->nFrames ; i++) + _printFeatureTxt(fp, ft->feature[j][i], format, type); + fprintf(fp, "\n"); + } + _printShutdown(fp); + } else { /* binary file */ + fp = _printSetupBin(fname); + fwrite(binheader_ft, sizeof(char), BINHEADERLENGTH, fp); + fwrite(&(ft->nFrames), sizeof(int), 1, fp); + fwrite(&(ft->nFeatures), sizeof(int), 1, fp); + for (j = 0 ; j < ft->nFeatures ; j++) { + for (i = 0 ; i < ft->nFrames ; i++) { + _printFeatureBin(fp, ft->feature[j][i]); + } + } + fclose(fp); + } +} + + + +static structureType _readHeader( + FILE *fp, + int *nFrames, + int *nFeatures, + KLT_BOOL *binary) +{ +#define LINELENGTH 100 + char line[LINELENGTH]; + structureType id; + + /* If file is binary, then read data and return */ + fread(line, sizeof(char), BINHEADERLENGTH, fp); + line[BINHEADERLENGTH] = 0; + if (strcmp(line, binheader_fl) == 0) { + assert(nFeatures != NULL); + fread(nFeatures, sizeof(int), 1, fp); + *binary = TRUE; + return FEATURE_LIST; + } else if (strcmp(line, binheader_fh) == 0) { + assert(nFrames != NULL); + fread(nFrames, sizeof(int), 1, fp); + *binary = TRUE; + return FEATURE_HISTORY; + } else if (strcmp(line, binheader_ft) == 0) { + assert(nFrames != NULL); + assert(nFeatures != NULL); + fread(nFrames, sizeof(int), 1, fp); + fread(nFeatures, sizeof(int), 1, fp); + *binary = TRUE; + return FEATURE_TABLE; + + /* If file is NOT binary, then continue.*/ + } else { + rewind(fp); + *binary = FALSE; + } + + /* Skip comments until warning line */ + while (strcmp(line, warning_line) != 0) { + fgets(line, LINELENGTH, fp); + if (feof(fp)) + KLTError("(_readFeatures) File is corrupted -- Couldn't find line:\n" + "\t%s\n", warning_line); + } + + /* Read 'Feature List', 'Feature History', or 'Feature Table' */ + while (fgetc(fp) != '-'); + while (fgetc(fp) != '\n'); + fgets(line, LINELENGTH, fp); + if (strcmp(line, "KLT Feature List\n") == 0) id = FEATURE_LIST; + else if (strcmp(line, "KLT Feature History\n") == 0) id = FEATURE_HISTORY; + else if (strcmp(line, "KLT Feature Table\n") == 0) id = FEATURE_TABLE; + else + KLTError("(_readFeatures) File is corrupted -- (Not 'KLT Feature List', " + "'KLT Feature History', or 'KLT Feature Table')"); + + /* If there's an incompatibility between the type of file */ + /* and the parameters passed, exit now before we attempt */ + /* to write to non-allocated memory. Higher routine should */ + /* detect and handle this error. */ + if ((id == FEATURE_LIST && nFeatures == NULL) || + (id == FEATURE_HISTORY && nFrames == NULL) || + (id == FEATURE_TABLE && (nFeatures == NULL || nFrames == NULL))) + return id; + + /* Read nFeatures and nFrames */ + while (fgetc(fp) != '-'); + while (fgetc(fp) != '\n'); + fscanf(fp, "%s", line); + if (id == FEATURE_LIST) { + if (strcmp(line, "nFeatures") != 0) + KLTError("(_readFeatures) File is corrupted -- " + "(Expected 'nFeatures', found '%s' instead)", line); + } else if (strcmp(line, "nFrames") != 0) + KLTError("(_readFeatures) File is corrupted -- " + "(Expected 'nFrames', found '%s' instead)", line); + fscanf(fp, "%s", line); + if (strcmp(line, "=") != 0) + KLTError("(_readFeatures) File is corrupted -- " + "(Expected '=', found '%s' instead)", line); + if (id == FEATURE_LIST) fscanf(fp, "%d", nFeatures); + else fscanf(fp, "%d", nFrames); + + /* If 'Feature Table', then also get nFeatures */ + if (id == FEATURE_TABLE) { + fscanf(fp, "%s", line); + if (strcmp(line, ",") != 0) + KLTError("(_readFeatures) File '%s' is corrupted -- " + "(Expected 'comma', found '%s' instead)", line); + fscanf(fp, "%s", line); + if (strcmp(line, "nFeatures") != 0) + KLTError("(_readFeatures) File '%s' is corrupted -- " + "(2 Expected 'nFeatures ', found '%s' instead)", line); + fscanf(fp, "%s", line); + if (strcmp(line, "=") != 0) + KLTError("(_readFeatures) File '%s' is corrupted -- " + "(2 Expected '= ', found '%s' instead)", line); + fscanf(fp, "%d", nFeatures); + } + + /* Skip junk before data */ + while (fgetc(fp) != '-'); + while (fgetc(fp) != '\n'); + + return id; +#undef LINELENGTH +} + + +static void _readFeatureTxt( + FILE *fp, + KLT_Feature feat) +{ + while (fgetc(fp) != '('); + fscanf(fp, "%f,%f)=%d", &(feat->x), &(feat->y), &(feat->val)); +} + + +static void _readFeatureBin( + FILE *fp, + KLT_Feature feat) +{ + fread(&(feat->x), sizeof(KLT_locType), 1, fp); + fread(&(feat->y), sizeof(KLT_locType), 1, fp); + fread(&(feat->val), sizeof(int), 1, fp); +} + + +/********************************************************************* + * KLTReadFeatureList + * KLTReadFeatureHistory + * KLTReadFeatureTable + * + * If the first parameter (fl, fh, or ft) is NULL, then the + * corresponding structure is created. + */ + +KLT_FeatureList KLTReadFeatureList( + KLT_FeatureList fl_in, + char *fname) +{ + FILE *fp; + KLT_FeatureList fl; + int nFeatures; + structureType id; + int indx; + KLT_BOOL binary; /* whether file is binary or text */ + int i; + + fp = fopen(fname, "rb"); + if (fp == NULL) KLTError("(KLTReadFeatureList) Can't open file '%s' " + "for reading", fname); + if (KLT_verbose >= 1) + fprintf(stderr, "(KLT) Reading feature list from '%s'\n", fname); + id = _readHeader(fp, NULL, &nFeatures, &binary); + if (id != FEATURE_LIST) + KLTError("(KLTReadFeatureList) File '%s' does not contain " + "a FeatureList", fname); + + if (fl_in == NULL) { + fl = KLTCreateFeatureList(nFeatures); + fl->nFeatures = nFeatures; + } + else { + fl = fl_in; + if (fl->nFeatures != nFeatures) + KLTError("(KLTReadFeatureList) The feature list passed " + "does not contain the same number of features as " + "the feature list in file '%s' ", fname); + } + + if (!binary) { /* text file */ + for (i = 0 ; i < fl->nFeatures ; i++) { + fscanf(fp, "%d |", &indx); + if (indx != i) KLTError("(KLTReadFeatureList) Bad index at i = %d" + "-- %d", i, indx); + _readFeatureTxt(fp, fl->feature[i]); + } + } else { /* binary file */ + for (i = 0 ; i < fl->nFeatures ; i++) { + _readFeatureBin(fp, fl->feature[i]); + } + } + + fclose(fp); + + return fl; +} + + +KLT_FeatureHistory KLTReadFeatureHistory( + KLT_FeatureHistory fh_in, + char *fname) +{ + FILE *fp; + KLT_FeatureHistory fh; + int nFrames; + structureType id; + int indx; + KLT_BOOL binary; /* whether file is binary or text */ + int i; + + fp = fopen(fname, "rb"); + if (fp == NULL) KLTError("(KLTReadFeatureHistory) Can't open file '%s' " + "for reading", fname); + if (KLT_verbose >= 1) fprintf(stderr, "(KLT) Reading feature history from '%s'\n", fname); + id = _readHeader(fp, &nFrames, NULL, &binary); + if (id != FEATURE_HISTORY) KLTError("(KLTReadFeatureHistory) File '%s' does not contain " + "a FeatureHistory", fname); + + if (fh_in == NULL) { + fh = KLTCreateFeatureHistory(nFrames); + fh->nFrames = nFrames; + } + else { + fh = fh_in; + if (fh->nFrames != nFrames) + KLTError("(KLTReadFeatureHistory) The feature history passed " + "does not contain the same number of frames as " + "the feature history in file '%s' ", fname); + } + + if (!binary) { /* text file */ + for (i = 0 ; i < fh->nFrames ; i++) { + fscanf(fp, "%d |", &indx); + if (indx != i) + KLTError("(KLTReadFeatureHistory) Bad index at i = %d" + "-- %d", i, indx); + _readFeatureTxt(fp, fh->feature[i]); + } + } else { /* binary file */ + for (i = 0 ; i < fh->nFrames ; i++) { + _readFeatureBin(fp, fh->feature[i]); + } + } + + fclose(fp); + + return fh; +} + + +KLT_FeatureTable KLTReadFeatureTable( + KLT_FeatureTable ft_in, + char *fname) +{ + FILE *fp; + KLT_FeatureTable ft; + int nFrames; + int nFeatures; + structureType id; + int indx; + KLT_BOOL binary; /* whether file is binary or text */ + int i, j; + + fp = fopen(fname, "rb"); + if (fp == NULL) KLTError("(KLTReadFeatureTable) Can't open file '%s' " + "for reading", fname); + if (KLT_verbose >= 1) fprintf(stderr, "(KLT) Reading feature table from '%s'\n", fname); + id = _readHeader(fp, &nFrames, &nFeatures, &binary); + if (id != FEATURE_TABLE) KLTError("(KLTReadFeatureTable) File '%s' does not contain " + "a FeatureTable", fname); + + if (ft_in == NULL) { + ft = KLTCreateFeatureTable(nFrames, nFeatures); + ft->nFrames = nFrames; + ft->nFeatures = nFeatures; + } + else { + ft = ft_in; + + if (ft->nFrames != nFrames || ft->nFeatures != nFeatures) + KLTError("(KLTReadFeatureTable) The feature table passed " + "does not contain the same number of frames and " + "features as the feature table in file '%s' ", fname); + } + + if (!binary) { /* text file */ + for (j = 0 ; j < ft->nFeatures ; j++) { + fscanf(fp, "%d |", &indx); + if (indx != j) + KLTError("(KLTReadFeatureTable) Bad index at j = %d" + "-- %d", j, indx); + for (i = 0 ; i < ft->nFrames ; i++) + _readFeatureTxt(fp, ft->feature[j][i]); + } + } else { /* binary file */ + for (j = 0 ; j < ft->nFeatures ; j++) { + for (i = 0 ; i < ft->nFrames ; i++) + _readFeatureBin(fp, ft->feature[j][i]); + } + } + + fclose(fp); + + return ft; +} + diff --git a/rtengine/myfile.h b/rtengine/myfile.h index c8ed35a85..5a1468b4d 100644 --- a/rtengine/myfile.h +++ b/rtengine/myfile.h @@ -44,6 +44,7 @@ inline int feof (IMFILE* f) { } inline void fseek (IMFILE* f, int p, int how) { + int fpos = f->pos; if (how==SEEK_SET) f->pos = p; @@ -51,6 +52,9 @@ inline void fseek (IMFILE* f, int p, int how) { f->pos += p; else if (how==SEEK_END) f->pos = f->size-p; + + if (f->pos < 0 || f->pos> f->size) + f->pos = fpos; } inline int fgetc (IMFILE* f) { diff --git a/rtengine/procevents.h b/rtengine/procevents.h index b0b9f9fe5..1b5b5d889 100644 --- a/rtengine/procevents.h +++ b/rtengine/procevents.h @@ -151,7 +151,8 @@ enum ProcEvent { EvFlatFieldAutoSelect=126, EvFlatFieldBlurRadius=127, EvFlatFieldBlurType=128, - NUMOFEVENTS=129 + EvAutoDIST=129, + NUMOFEVENTS=130 }; } #endif diff --git a/rtengine/refreshmap.cc b/rtengine/refreshmap.cc index 36c34b1f3..7f6dcd14c 100644 --- a/rtengine/refreshmap.cc +++ b/rtengine/refreshmap.cc @@ -148,7 +148,8 @@ DARKFRAME, // EvPreProcessExpCorrPH FLATFIELD, // EvFlatFieldFile, FLATFIELD, // EvFlatFieldAutoSelect, FLATFIELD, // EvFlatFieldBlurRadius, -FLATFIELD // EvFlatFieldBlurType, +FLATFIELD, // EvFlatFieldBlurType, +TRANSFORM, // EvAutoDIST, }; diff --git a/rtengine/rtthumbnail.cc b/rtengine/rtthumbnail.cc index ebd0e121a..671fd6f96 100644 --- a/rtengine/rtthumbnail.cc +++ b/rtengine/rtthumbnail.cc @@ -913,6 +913,101 @@ void Thumbnail::transformPixel (int x, int y, int tran, int& tx, int& ty) { ty/=scale; } +unsigned char* Thumbnail::getGrayscaleHistEQ (int trim_width) { + if (!thumbImg) + return NULL; + + if (thumbImg->widthheight*trim_width]; + int ix = 0,max; + + if (gammaCorrected) { + // if it's gamma correct (usually a RAW), we have the problem that there is a lot noise etc. that makes the maximum way too high. + // Strategy is limit a certain percent of pixels so the overall picture quality when scaling to 8 bit is way better + const double BurnOffPct=0.03; // *100 = percent pixels that may be clipped + + // Calc the histogram + unsigned int* hist16 = new unsigned int [65536]; + memset(hist16,0,sizeof(int)*65536); + + for (int row=0; rowheight; row++) + for (int col=0; colwidth; col++) { + hist16[thumbImg->r[row][col]]++; + hist16[thumbImg->g[row][col]]+=2; // Bayer 2x green correction + hist16[thumbImg->b[row][col]]++; + } + + // Go down till we cut off that many pixels + unsigned long cutoff = thumbImg->height * thumbImg->height * 4 * BurnOffPct; + + int max; unsigned long sum=0; + for (max=65535; max>16384 && sumheight; i++) + for (int j=(thumbImg->width-trim_width)/2; jwidth-trim_width)/2; j++) { + int r= gammatab[MIN(thumbImg->r[i][j],max) * scaleForSave >> 13]; + int g= gammatab[MIN(thumbImg->g[i][j],max) * scaleForSave >> 13]; + int b= gammatab[MIN(thumbImg->b[i][j],max) * scaleForSave >> 13]; + tmpdata[ix++] = r*19595+g*38469+b*7472 >> 16; + } + } + else { + // If it's not gamma corrected (usually a JPG) we take the normal maximum + max=0; + + for (int row=0; rowheight; row++) + for (int col=0; colwidth; col++) { + if (thumbImg->r[row][col]>max) max = thumbImg->r[row][col]; + if (thumbImg->g[row][col]>max) max = thumbImg->g[row][col]; + if (thumbImg->b[row][col]>max) max = thumbImg->b[row][col]; + } + + if (max < 16384) max = 16384; + scaleForSave = 65535*8192 / max; + + // Correction and gamma to 8 Bit + for (int i=0; iheight; i++) + for (int j=(thumbImg->width-trim_width)/2; jwidth-trim_width)/2; j++) { + int r=thumbImg->r[i][j] * scaleForSave >> 21; + int g=thumbImg->g[i][j] * scaleForSave >> 21; + int b=thumbImg->b[i][j] * scaleForSave >> 21; + tmpdata[ix++] = (r*19595+g*38469+b*7472)>>16; + } + } + + // histogram equalization + unsigned int hist[256] = {0}; + + for (int i=0; i0 && cdf_min==-1) { + cdf_min=cdf; + } + if (cdf_min!=-1) { + hist[i] = (cdf-cdf_min)*255/((thumbImg->height*trim_width)-cdf_min); + } + } + + for (int i=0; iapplyAutoExp (initialPP[i]); } + if (event==rtengine::EvAutoDIST) { + for (int i=0; iset_tooltip_text (M("TP_DISTORTION_AUTO_TIP")); + idConn = autoDistor->signal_pressed().connect( sigc::mem_fun(*this, &Distortion::idPressed) ); + autoDistor->show(); + pack_start (*autoDistor); + distor = Gtk::manage (new Adjuster (M("TP_DISTORTION_AMOUNT"), -0.5, 0.5, 0.001, 0)); distor->setAdjusterListener (this); distor->show(); @@ -35,8 +42,9 @@ void Distortion::read (const ProcParams* pp, const ParamsEdited* pedited) { disableListener (); - if (pedited) + if (pedited) { distor->setEditedState (pedited->distortion.amount ? Edited : UnEdited); + } distor->setValue (pp->distortion.amount); @@ -47,18 +55,21 @@ void Distortion::write (ProcParams* pp, ParamsEdited* pedited) { pp->distortion.amount = distor->getValue (); - if (pedited) + if (pedited) { pedited->distortion.amount = distor->getEditedState (); + } } void Distortion::setDefaults (const ProcParams* defParams, const ParamsEdited* pedited) { distor->setDefault (defParams->distortion.amount); - if (pedited) + if (pedited) { distor->setDefaultEditedState (pedited->distortion.amount ? Edited : UnEdited); - else + } + else { distor->setDefaultEditedState (Irrelevant); + } } void Distortion::adjusterChanged (Adjuster* a, double newval) { @@ -78,6 +89,18 @@ void Distortion::setAdjusterBehavior (bool bvadd) { void Distortion::setBatchMode (bool batchMode) { ToolPanel::setBatchMode (batchMode); + if (batchMode) { + autoDistor->set_sensitive(false); + } distor->showEditedCB (); } +void Distortion::idPressed () { + if (!batchMode) { + if (rlistener) { + double new_amount = rlistener->autoDistorRequested(); + distor->setValue(new_amount); + adjusterChanged (distor, new_amount); + } + } +} diff --git a/rtgui/distortion.h b/rtgui/distortion.h index 4b6162f9b..a20eac290 100644 --- a/rtgui/distortion.h +++ b/rtgui/distortion.h @@ -22,12 +22,16 @@ #include #include #include +#include class Distortion : public Gtk::VBox, public AdjusterListener, public FoldableToolPanel { protected: + Gtk::Button* autoDistor; Adjuster* distor; bool distAdd; + sigc::connection idConn; + LensGeomListener * rlistener; public: @@ -40,6 +44,8 @@ class Distortion : public Gtk::VBox, public AdjusterListener, public FoldableToo void adjusterChanged (Adjuster* a, double newval); void setAdjusterBehavior (bool bvadd); + void idPressed (); + void setLensGeomListener (LensGeomListener* l) { rlistener = l; } }; #endif diff --git a/rtgui/lensgeomlistener.h b/rtgui/lensgeomlistener.h index 2f14ab346..6b7f79f98 100644 --- a/rtgui/lensgeomlistener.h +++ b/rtgui/lensgeomlistener.h @@ -24,6 +24,7 @@ class LensGeomListener { public: virtual void straightenRequested ()=0; virtual void autoCropRequested ()=0; + virtual double autoDistorRequested ()=0; }; #endif diff --git a/rtgui/toolpanelcoord.cc b/rtgui/toolpanelcoord.cc index 819cabbf7..b017bb4aa 100644 --- a/rtgui/toolpanelcoord.cc +++ b/rtgui/toolpanelcoord.cc @@ -23,6 +23,7 @@ #include #include #include +#include using namespace rtengine::procparams; @@ -185,6 +186,7 @@ ToolPanelCoordinator::ToolPanelCoordinator () : ipc(NULL) { flatfield->setFFProvider (this); lensgeom->setLensGeomListener (this); rotate->setLensGeomListener (this); + distortion->setLensGeomListener (this); crop->setCropPanelListener (this); icm->setICMPanelListener (this); @@ -435,6 +437,12 @@ void ToolPanelCoordinator::straightenRequested () { toolBar->setTool (TMStraighten); } +double ToolPanelCoordinator::autoDistorRequested () { + if (!ipc) + return 0.0; + return rtengine::ImProcFunctions::getAutoDistor (ipc->getInitialImage()->getFileName(), 400); +} + void ToolPanelCoordinator::spotWBRequested (int size) { if (!ipc) diff --git a/rtgui/toolpanelcoord.h b/rtgui/toolpanelcoord.h index 4f7516aab..694f3ea7b 100644 --- a/rtgui/toolpanelcoord.h +++ b/rtgui/toolpanelcoord.h @@ -186,6 +186,7 @@ class ToolPanelCoordinator : public ToolPanelListener, // rotatelistener interface void straightenRequested (); void autoCropRequested (); + double autoDistorRequested (); // spotwblistener interface void spotWBRequested (int size);