Exprimental feature auto lens distortion correction, on behalf of Guokai; see issue #576

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

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-

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

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;i<ncols*nrows;i++) {
+        img2[i]=(img2[i]/2)+16;
+    }
+    // find the best comp and scale when assume r1 = r0*(1.0-comp+(r0*comp))*scale;
+    n=0;
+    double total_r10=0.0, total_r0=0.0;
+    for (i=0;i<N_FEATURES;i++) {
+        if (ft->feature[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]<CENTER_R)
+                continue;
+            r10[n]=(sqrt((x1-wc)*(x1-wc)+(y1-hc)*(y1-hc))/radius)/r0[n];
+            total_r10 += r10[n];
+            total_r0 += r0[n];
+            n++;
+        } else {
+            ft->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<n;i++) {
+            Sxx += (r0[i] - avg_r0) * (r0[i] - avg_r0);
+            Sxy += (r0[i] - avg_r0) * (r10[i] - avg_r10);
+            Syy += (r10[i] - avg_r10) * (r10[i] - avg_r10);
+    }
+
+    double u = Sxy/Sxx;
+    double v = avg_r10 - u * avg_r0;
+    double b = u + v;
+    double a = u / b;
+    double total_delta = 0.0;
+    double rxy = Sxy/sqrt(Sxx*Syy);
+
+    if (rxy<0)  rxy= -rxy;
+
+    int new_n=n;
+    // calculate deviation
+    for (i=0;i<n;i++) {
+        double delta = r10[i] - (1.0-a+(r0[i]*a))*b;
+        delta = delta >= 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<n;i++) {
+            if (r0[i]<0) continue;
+            Sxx += (r0[i] - avg_r0) * (r0[i] - avg_r0);
+            Sxy += (r0[i] - avg_r0) * (r10[i] - avg_r10);
+            Syy += (r10[i] - avg_r10) * (r10[i] - avg_r10);
+    }
+
+    u = Sxy/Sxx;
+    v = avg_r10 - u * avg_r0;
+    b = u + v;
+    a = u / b;
+    total_delta = 0.0;
+    rxy = Sxy/sqrt(Sxx*Syy);
+    if (rxy<0)  rxy= -rxy;
+
+#ifdef DEBUG_IMG
+    // draw lines for curve and different deviation level, for debugging purpose
+    for (i=0; i< ncols; i++) {
+        double val= (1.0-a+((i/(double)ncols)*a))*b;
+        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] = 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<n;i++) {
+        if (r0[i]<0) continue;
+        double delta = r10[i] - (1.0-a+(r0[i]*a))*b;
+        delta = delta >= 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;
+}
+

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

rtengine/improcfun.cc

@@ -29,6 +29,8 @@
 #include <glibmm.h>
 #include <iccstore.h>
 #include <impulse_denoise.h>
+#include <imagesource.h>
+#include <rtthumbnail.h>
 
 #ifdef _OPENMP
 #include <omp.h>
@@ -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;

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<Coord2D> &src, std::vector<Coord2D> &red,  std::vector<Coord2D> &green, std::vector<Coord2D> &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);

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'.
+

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
+

rtengine/klt/convolve.c

@@ -0,0 +1,317 @@
+/*********************************************************************
+ * convolve.c
+ *********************************************************************/
+
+/* Standard includes */
+#include <assert.h>
+#include <math.h>
+#include <stdlib.h>   /* 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);
+}
+
+
+

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

rtengine/klt/error.c

@@ -0,0 +1,56 @@
+/*********************************************************************
+ * error.c
+ *
+ * Error and warning messages, and system commands.
+ *********************************************************************/
+
+
+/* Standard includes */
+#include <stdio.h>
+#include <stdlib.h>
+#include <stdarg.h>
+
+
+/*********************************************************************
+ * 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);
+}
+

rtengine/klt/error.h

@@ -0,0 +1,15 @@
+/*********************************************************************
+ * error.h
+ *********************************************************************/
+
+#ifndef _ERROR_H_
+#define _ERROR_H_
+
+#include <stdio.h>
+#include <stdarg.h>
+
+void KLTError(char *fmt, ...);
+void KLTWarning(char *fmt, ...);
+
+#endif
+

rtengine/klt/klt.c

@@ -0,0 +1,531 @@
+/*********************************************************************
+ * klt.c
+ *
+ * Kanade-Lucas-Tomasi tracker
+ *********************************************************************/
+
+/* Standard includes */
+#include <assert.h>
+#include <math.h>    /* logf() */
+#include <stdlib.h>  /* 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;
+}
+
+
+

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
+
+
+
+
+
+

rtengine/klt/klt_util.c

@@ -0,0 +1,165 @@
+/*********************************************************************
+ * klt_util.c
+ *********************************************************************/
+
+/* Standard includes */
+#include <assert.h>
+#include <stdlib.h>  /* malloc() */
+#include <math.h>		/* 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);
+}

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
+
+

rtengine/klt/main.cpp

@@ -0,0 +1,30 @@
+// main.cpp : Defines the entry point for the console application.
+//
+
+#include <stdio.h>  // 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;
+}
+

rtengine/klt/pnmio.c

@@ -0,0 +1,333 @@
+/*********************************************************************
+ * pnmio.c
+ *
+ * Various routines to manipulate PNM files.
+ *********************************************************************/
+
+
+/* Standard includes */
+#include <stdio.h>   /* FILE  */
+#include <stdlib.h>  /* 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);
+}
+
+

rtengine/klt/pnmio.h

@@ -0,0 +1,56 @@
+/*********************************************************************
+ * pnmio.h
+ *********************************************************************/
+
+#ifndef _PNMIO_H_
+#define _PNMIO_H_
+
+#include <stdio.h>
+
+/**********
+ * 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

rtengine/klt/pyramid.c

@@ -0,0 +1,143 @@
+/*********************************************************************
+ * pyramid.c
+ *
+ *********************************************************************/
+
+/* Standard includes */
+#include <assert.h>
+#include <stdlib.h>		/* malloc() ? */
+#include <string.h>		/* memset() ? */
+#include <math.h>		/* */
+
+/* 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);
+  }
+}
+ 
+
+
+
+
+
+
+
+
+
+
+

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

rtengine/klt/selectGoodFeatures.c

@@ -0,0 +1,543 @@
+/*********************************************************************
+ * selectGoodFeatures.c
+ *
+ *********************************************************************/
+
+/* Standard includes */
+#include <assert.h>
+#include <stdlib.h> /* malloc(), qsort() */
+#include <stdio.h>  /* fflush()          */
+#include <string.h> /* memset()          */
+#include <math.h>   /* 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 <lourakis@csi.forth.gr>
+ *
+ * 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);
+  }
+}
+
+

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.
+======================================================================= 

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;
+  }
+}
+

rtengine/klt/writeFeatures.c

@@ -0,0 +1,743 @@
+/*********************************************************************
+ * writeFeatures.c
+ *
+ *********************************************************************
+ */
+
+/* Standard includes */
+#include <assert.h>
+#include <ctype.h>		/* isdigit() */
+#include <stdio.h>		/* sprintf(), fprintf(), sscanf(), fscanf() */
+#include <stdlib.h>		/* malloc() */
+#include <string.h>		/* 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;
+}
+

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) {

rtengine/procevents.h

@@ -151,7 +151,8 @@ enum ProcEvent {
     EvFlatFieldAutoSelect=126,
     EvFlatFieldBlurRadius=127,
     EvFlatFieldBlurType=128,
-    NUMOFEVENTS=129
+    EvAutoDIST=129,
+    NUMOFEVENTS=130
     };
 }
 #endif

rtengine/refreshmap.cc

@@ -148,7 +148,8 @@ DARKFRAME,        // EvPreProcessExpCorrPH
 FLATFIELD,        // EvFlatFieldFile,
 FLATFIELD,        // EvFlatFieldAutoSelect,
 FLATFIELD,        // EvFlatFieldBlurRadius,
-FLATFIELD         // EvFlatFieldBlurType,
+FLATFIELD,        // EvFlatFieldBlurType,
+TRANSFORM,        // EvAutoDIST,
 
 };
 

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->width<trim_width)
+        return NULL;
+    
+    // to utilize the 8 bit color range of the thumbnail we brighten it and apply gamma correction
+    unsigned char* tmpdata = new unsigned char[thumbImg->height*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; row<thumbImg->height; row++)
+            for (int col=0; col<thumbImg->width; 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 && sum<cutoff; max--) sum+=hist16[max];
+
+        delete[] hist16;
+
+        scaleForSave = 65535*8192 / max;
+
+        // Correction and gamma to 8 Bit
+        for (int i=0; i<thumbImg->height; i++)
+            for (int j=(thumbImg->width-trim_width)/2; j<trim_width+(thumbImg->width-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; row<thumbImg->height; row++)
+            for (int col=0; col<thumbImg->width; 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; i<thumbImg->height; i++)
+            for (int j=(thumbImg->width-trim_width)/2; j<trim_width+(thumbImg->width-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; i<ix; i++) {
+        hist[tmpdata[i]]++;
+    }
+
+    int cdf = 0, cdf_min=-1;
+    for (int i=0; i<256; i++) {
+        cdf+=hist[i];
+        if (cdf>0 && 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; i<ix; i++) {
+        tmpdata[i] = hist[tmpdata[i]];
+    }
+    
+    return tmpdata;
+}
+
 // format: 1=8bit direct, 2=16bit direct, 3=JPG
 bool Thumbnail::writeImage (const Glib::ustring& fname, int format) {
 

rtengine/rtthumbnail.h

@@ -84,6 +84,7 @@ namespace rtengine {
             void getSpotWB    (const procparams::ProcParams& params, int x, int y, int rect, double& temp, double& green);
             void applyAutoExp (procparams::ProcParams& pparams);
             
+            unsigned char* getGrayscaleHistEQ (int trim_width);
             bool writeImage (const Glib::ustring& fname, int format);
             bool readImage (const Glib::ustring& fname);
             

rtgui/batchtoolpanelcoord.cc

@@ -198,6 +198,12 @@ void BatchToolPanelCoordinator::panelChanged (rtengine::ProcEvent event, const G
             selected[i]->applyAutoExp (initialPP[i]);
         }
 
+    if (event==rtengine::EvAutoDIST) {
+        for (int i=0; i<selected.size(); i++) {
+            initialPP[i].distortion.amount = pparams.distortion.amount;
+        }
+    }
+
     // combine with initial parameters and set
     ProcParams newParams;
     for (int i=0; i<selected.size(); i++) {

rtgui/distortion.cc

@@ -24,6 +24,13 @@ using namespace rtengine::procparams;
 
 Distortion::Distortion (): Gtk::VBox(), FoldableToolPanel(this) {
 
+    rlistener = NULL;
+    autoDistor = Gtk::manage (new Gtk::Button (M("TP_DISTORTION_AUTO")));
+    autoDistor->set_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);
+        }
+    }
+}

rtgui/distortion.h

@@ -22,12 +22,16 @@
 #include <gtkmm.h>
 #include <adjuster.h>
 #include <toolpanel.h>
+#include <lensgeomlistener.h>
 
 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

rtgui/lensgeomlistener.h

@@ -24,6 +24,7 @@ class LensGeomListener {
     public:
         virtual void straightenRequested ()=0;
         virtual void autoCropRequested   ()=0;
+        virtual double autoDistorRequested ()=0;
 };
 
 #endif

rtgui/toolpanelcoord.cc

@@ -23,6 +23,7 @@
 #include <imagesource.h>
 #include <dfmanager.h>
 #include <ffmanager.h>
+#include <improcfun.h>
 
 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)

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);