From 815ba5b321f7cf61b30c4b00bac40f9f49ebaf4d Mon Sep 17 00:00:00 2001 From: michael Date: Sat, 17 Dec 2011 12:14:44 -0500 Subject: [PATCH] Major rework of headers inclusion style Part2 on behalf of lebedev.ri (issue 1079) --- rtengine/calc_distort.cc | 228 +++++ rtengine/iccjpeg.cc | 248 +++++ rtengine/jdatasrc.cc | 411 ++++++++ rtengine/jpeg_memsrc.cc | 169 +++ rtengine/klt/convolve.cc | 317 ++++++ rtengine/klt/error.cc | 56 + rtengine/klt/klt.cc | 531 ++++++++++ rtengine/klt/klt_util.cc | 165 +++ rtengine/klt/pnmio.cc | 333 ++++++ rtengine/klt/pyramid.cc | 143 +++ rtengine/klt/selectGoodFeatures.cc | 543 ++++++++++ rtengine/klt/storeFeatures.cc | 117 +++ rtengine/klt/trackFeatures.cc | 1533 ++++++++++++++++++++++++++++ rtengine/klt/writeFeatures.cc | 743 ++++++++++++++ rtgui/preferences.h | 4 +- rtgui/preprocess.cc | 6 +- rtgui/preprocess.h | 6 +- rtgui/previewhandler.cc | 4 +- rtgui/previewhandler.h | 2 +- rtgui/previewloader.cc | 6 +- rtgui/previewloader.h | 2 +- rtgui/previewwindow.cc | 8 +- rtgui/previewwindow.h | 4 +- rtgui/profilechangelistener.h | 2 +- rtgui/profilepanel.cc | 14 +- rtgui/profilepanel.h | 8 +- rtgui/profilestore.cc | 8 +- rtgui/profilestore.h | 2 +- rtgui/progressconnector.h | 4 +- rtgui/rawcacorrection.cc | 6 +- rtgui/rawcacorrection.h | 6 +- rtgui/rawexposure.cc | 6 +- rtgui/rawexposure.h | 6 +- rtgui/rawprocess.cc | 6 +- rtgui/rawprocess.h | 6 +- rtgui/recentbrowser.cc | 4 +- rtgui/recentbrowser.h | 8 +- rtgui/renamedlg.cc | 8 +- rtgui/renamedlg.h | 4 +- rtgui/resize.cc | 4 +- rtgui/resize.h | 8 +- rtgui/rgbcurves.h | 10 +- rtgui/rotate.cc | 6 +- 43 files changed, 5621 insertions(+), 84 deletions(-) create mode 100644 rtengine/calc_distort.cc create mode 100644 rtengine/iccjpeg.cc create mode 100644 rtengine/jdatasrc.cc create mode 100644 rtengine/jpeg_memsrc.cc create mode 100644 rtengine/klt/convolve.cc create mode 100644 rtengine/klt/error.cc create mode 100644 rtengine/klt/klt.cc create mode 100644 rtengine/klt/klt_util.cc create mode 100644 rtengine/klt/pnmio.cc create mode 100644 rtengine/klt/pyramid.cc create mode 100644 rtengine/klt/selectGoodFeatures.cc create mode 100644 rtengine/klt/storeFeatures.cc create mode 100644 rtengine/klt/trackFeatures.cc create mode 100644 rtengine/klt/writeFeatures.cc diff --git a/rtengine/calc_distort.cc b/rtengine/calc_distort.cc new file mode 100644 index 000000000..842ee6076 --- /dev/null +++ b/rtengine/calc_distort.cc @@ -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 + +#define N_FEATURES 100 +#define DELTA_1 0.05 +#define DELTA_2 0.01 +#define RXY_LIMIT 0.6 +#define CENTER_R 0.3 + +//#define DEBUG_IMG + +#ifdef DEBUG_IMG +void drawDotXY(unsigned char* img, int ncols, int nrows, int x, int y, int color) +{ + img[x+y*ncols] = color; +} + +void drawDot(unsigned char* img, int ncols, int nrows, double r0, double r10, int color) +{ + if (r0>=0 && r0<1 && r10 >=0.8 && r10 <1.2) + drawDotXY (img, ncols, nrows, (int)(r0*ncols), (int)((r10-0.8)*2.5*nrows), color); +} +#endif + +double calcDistortion(unsigned char* img1, unsigned char* img2, int ncols, int nrows) +{ + KLT_TrackingContext tc; + KLT_FeatureList fl; + KLT_FeatureTable ft; + int nFeatures = N_FEATURES; + int i,n; + double radius, wc, hc; + + double r0[N_FEATURES] = {0.0}; + double r10[N_FEATURES] = {0.0}; + + tc = KLTCreateTrackingContext(); + //tc->mindist = 20; + tc->lighting_insensitive = TRUE; + tc->nSkippedPixels = 5; + tc->step_factor = 2.0; + tc->max_iterations = 20; + //KLTPrintTrackingContext(tc); + fl = KLTCreateFeatureList(N_FEATURES); + ft = KLTCreateFeatureTable(2, N_FEATURES); + + radius = sqrt(ncols*ncols+nrows*nrows)/2.0; + wc=((double)ncols)/2.0-0.5; + hc=((double)nrows)/2.0-0.5; + + KLTSelectGoodFeatures(tc, img1, ncols, nrows, fl); + KLTStoreFeatureList(fl, ft, 0); + + KLTTrackFeatures(tc, img1, img2, ncols, nrows, fl); + KLTStoreFeatureList(fl, ft, 1); + + // add a shade to img2, we want to draw something on top of it. + for (i=0;ifeature[i][1]->val>=0) { + double x0,y0,x1,y1; + x0=ft->feature[i][0]->x; + y0=ft->feature[i][0]->y; + x1=ft->feature[i][1]->x; + y1=ft->feature[i][1]->y; + + r0[n]=sqrt((x0-wc)*(x0-wc)+(y0-hc)*(y0-hc))/radius; + // dots too close to the center tends to have big diviation and create noise, extract them + if (r0[n]feature[i][0]->x=-1.0; + ft->feature[i][0]->y=-1.0; + } + } + + if (n < 5) { + printf ("Not sufficient features.\n"); + return 0.0; + } + + double avg_r10 = total_r10 / n; + double avg_r0 = total_r0 / n; + double Sxx = 0.0; + double Sxy = 0.0; + double Syy = 0.0; + + for (i=0;i= 0 ? delta : -delta; + +#ifdef DEBUG_IMG + drawDot(img2, ncols, nrows, r0[i], r10[i], 255); +#endif + + if (delta >= DELTA_1) { + total_r10 -= r10[i]; + total_r0 -= r0[i]; + r0[i] = -1.0; + new_n--; + } + + total_delta += delta; + } + + printf ("distortion amount=%lf scale=%lf deviation=%lf, rxy=%lf\n", a, b, total_delta/n, rxy); + + if (new_n < 5) { + printf ("Not sufficient features.\n"); + return 0.0; + } + + printf ("Removed %d outstading data points\n", n-new_n); + avg_r10 = total_r10 / new_n; + avg_r0 = total_r0 / new_n; + Sxx = 0.0; + Sxy = 0.0; + Syy = 0.0; + + for (i=0;i=0.8 && val <1.2) { + if (img2[i+((int)((val-0.8)*2.5*nrows))*ncols] != 255) + img2[i+((int)((val-0.8)*2.5*nrows))*ncols] = 0; + } + val += DELTA_1; + if (val >=0.8 && val <1.2) { + if (img2[i+((int)((val-0.8)*2.5*nrows))*ncols] != 255) + img2[i+((int)((val-0.8)*2.5*nrows))*ncols] = 8; + } + val -= DELTA_1*2; + if (val >=0.8 && val <1.2) { + if (img2[i+((int)((val-0.8)*2.5*nrows))*ncols] != 255) + img2[i+((int)((val-0.8)*2.5*nrows))*ncols] = 8; + } + val += DELTA_1+DELTA_2; + if (val >=0.8 && val <1.2) { + if (img2[i+((int)((val-0.8)*2.5*nrows))*ncols] != 255) + img2[i+((int)((val-0.8)*2.5*nrows))*ncols] = 16; + } + val -= DELTA_2*2; + if (val >=0.8 && val <1.2) { + if (img2[i+((int)((val-0.8)*2.5*nrows))*ncols] != 255) + img2[i+((int)((val-0.8)*2.5*nrows))*ncols] = 16; + } + } + + KLTExtractFeatureList(fl, ft, 0); + KLTWriteFeatureListToPPM(fl,img1,ncols,nrows,"/tmp/feat0.ppm"); + KLTExtractFeatureList(fl, ft, 1); + KLTWriteFeatureListToPPM(fl,img2,ncols,nrows,"/tmp/feat1.ppm"); +#endif + + // calculate deviation + for (i=0;i= 0 ? delta : -delta; + total_delta += delta; + } + + printf ("distortion amount=%lf scale=%lf deviation=%lf, rxy=%lf\n", a, b, total_delta/n, rxy); + + if (total_delta / new_n > DELTA_2) { + printf ("Deviation is too big.\n"); + return 0.0; + } + + if (rxy < RXY_LIMIT) { + printf ("Not linear enough\n"); + return 0.0; + } + + printf ("distortion amount=%lf scale=%lf deviation=%lf, rxy=%lf\n", a, b, total_delta/n, rxy); + return a; +} + diff --git a/rtengine/iccjpeg.cc b/rtengine/iccjpeg.cc new file mode 100644 index 000000000..bfc194cb3 --- /dev/null +++ b/rtengine/iccjpeg.cc @@ -0,0 +1,248 @@ +/* + * iccprofile.c + * + * This file provides code to read and write International Color Consortium + * (ICC) device profiles embedded in JFIF JPEG image files. The ICC has + * defined a standard format for including such data in JPEG "APP2" markers. + * The code given here does not know anything about the internal structure + * of the ICC profile data; it just knows how to put the profile data into + * a JPEG file being written, or get it back out when reading. + * + * This code depends on new features added to the IJG JPEG library as of + * IJG release 6b; it will not compile or work with older IJG versions. + * + * NOTE: this code would need surgery to work on 16-bit-int machines + * with ICC profiles exceeding 64K bytes in size. If you need to do that, + * change all the "unsigned int" variables to "INT32". You'll also need + * to find a malloc() replacement that can allocate more than 64K. + */ + +#include "iccjpeg.h" +#include /* define malloc() */ + + +/* + * Since an ICC profile can be larger than the maximum size of a JPEG marker + * (64K), we need provisions to split it into multiple markers. The format + * defined by the ICC specifies one or more APP2 markers containing the + * following data: + * Identifying string ASCII "ICC_PROFILE\0" (12 bytes) + * Marker sequence number 1 for first APP2, 2 for next, etc (1 byte) + * Number of markers Total number of APP2's used (1 byte) + * Profile data (remainder of APP2 data) + * Decoders should use the marker sequence numbers to reassemble the profile, + * rather than assuming that the APP2 markers appear in the correct sequence. + */ + +#define ICC_MARKER (JPEG_APP0 + 2) /* JPEG marker code for ICC */ +#define ICC_OVERHEAD_LEN 14 /* size of non-profile data in APP2 */ +#define MAX_BYTES_IN_MARKER 65533 /* maximum data len of a JPEG marker */ +#define MAX_DATA_BYTES_IN_MARKER (MAX_BYTES_IN_MARKER - ICC_OVERHEAD_LEN) + + +/* + * This routine writes the given ICC profile data into a JPEG file. + * It *must* be called AFTER calling jpeg_start_compress() and BEFORE + * the first call to jpeg_write_scanlines(). + * (This ordering ensures that the APP2 marker(s) will appear after the + * SOI and JFIF or Adobe markers, but before all else.) + */ + +void +write_icc_profile (j_compress_ptr cinfo, + const JOCTET *icc_data_ptr, + unsigned int icc_data_len) +{ + unsigned int num_markers; /* total number of markers we'll write */ + int cur_marker = 1; /* per spec, counting starts at 1 */ + unsigned int length; /* number of bytes to write in this marker */ + + /* Calculate the number of markers we'll need, rounding up of course */ + num_markers = icc_data_len / MAX_DATA_BYTES_IN_MARKER; + if (num_markers * MAX_DATA_BYTES_IN_MARKER != icc_data_len) + num_markers++; + + while (icc_data_len > 0) { + /* length of profile to put in this marker */ + length = icc_data_len; + if (length > MAX_DATA_BYTES_IN_MARKER) + length = MAX_DATA_BYTES_IN_MARKER; + icc_data_len -= length; + + /* Write the JPEG marker header (APP2 code and marker length) */ + jpeg_write_m_header(cinfo, ICC_MARKER, + (unsigned int) (length + ICC_OVERHEAD_LEN)); + + /* Write the marker identifying string "ICC_PROFILE" (null-terminated). + * We code it in this less-than-transparent way so that the code works + * even if the local character set is not ASCII. + */ + jpeg_write_m_byte(cinfo, 0x49); + jpeg_write_m_byte(cinfo, 0x43); + jpeg_write_m_byte(cinfo, 0x43); + jpeg_write_m_byte(cinfo, 0x5F); + jpeg_write_m_byte(cinfo, 0x50); + jpeg_write_m_byte(cinfo, 0x52); + jpeg_write_m_byte(cinfo, 0x4F); + jpeg_write_m_byte(cinfo, 0x46); + jpeg_write_m_byte(cinfo, 0x49); + jpeg_write_m_byte(cinfo, 0x4C); + jpeg_write_m_byte(cinfo, 0x45); + jpeg_write_m_byte(cinfo, 0x0); + + /* Add the sequencing info */ + jpeg_write_m_byte(cinfo, cur_marker); + jpeg_write_m_byte(cinfo, (int) num_markers); + + /* Add the profile data */ + while (length--) { + jpeg_write_m_byte(cinfo, *icc_data_ptr); + icc_data_ptr++; + } + cur_marker++; + } +} + + +/* + * Prepare for reading an ICC profile + */ + +void +setup_read_icc_profile (j_decompress_ptr cinfo) +{ + /* Tell the library to keep any APP2 data it may find */ + jpeg_save_markers(cinfo, ICC_MARKER, 0xFFFF); +} + + +/* + * Handy subroutine to test whether a saved marker is an ICC profile marker. + */ +static boolean +marker_is_icc (jpeg_saved_marker_ptr marker) +{ + return + marker->marker == ICC_MARKER && + marker->data_length >= ICC_OVERHEAD_LEN && + /* verify the identifying string */ + GETJOCTET(marker->data[0]) == 0x49 && + GETJOCTET(marker->data[1]) == 0x43 && + GETJOCTET(marker->data[2]) == 0x43 && + GETJOCTET(marker->data[3]) == 0x5F && + GETJOCTET(marker->data[4]) == 0x50 && + GETJOCTET(marker->data[5]) == 0x52 && + GETJOCTET(marker->data[6]) == 0x4F && + GETJOCTET(marker->data[7]) == 0x46 && + GETJOCTET(marker->data[8]) == 0x49 && + GETJOCTET(marker->data[9]) == 0x4C && + GETJOCTET(marker->data[10]) == 0x45 && + GETJOCTET(marker->data[11]) == 0x0; +} + + +/* + * See if there was an ICC profile in the JPEG file being read; + * if so, reassemble and return the profile data. + * + * TRUE is returned if an ICC profile was found, FALSE if not. + * If TRUE is returned, *icc_data_ptr is set to point to the + * returned data, and *icc_data_len is set to its length. + * + * IMPORTANT: the data at **icc_data_ptr has been allocated with malloc() + * and must be freed by the caller with free() when the caller no longer + * needs it. (Alternatively, we could write this routine to use the + * IJG library's memory allocator, so that the data would be freed implicitly + * at jpeg_finish_decompress() time. But it seems likely that many apps + * will prefer to have the data stick around after decompression finishes.) + * + * NOTE: if the file contains invalid ICC APP2 markers, we just silently + * return FALSE. You might want to issue an error message instead. + */ + +boolean +read_icc_profile (j_decompress_ptr cinfo, + JOCTET **icc_data_ptr, + unsigned int *icc_data_len) +{ + jpeg_saved_marker_ptr marker; + int num_markers = 0; + int seq_no; + JOCTET *icc_data; + unsigned int total_length; +#define MAX_SEQ_NO 255 /* sufficient since marker numbers are bytes */ + char marker_present[MAX_SEQ_NO+1]; /* 1 if marker found */ + unsigned int data_length[MAX_SEQ_NO+1]; /* size of profile data in marker */ + unsigned int data_offset[MAX_SEQ_NO+1]; /* offset for data in marker */ + + *icc_data_ptr = NULL; /* avoid confusion if FALSE return */ + *icc_data_len = 0; + + /* This first pass over the saved markers discovers whether there are + * any ICC markers and verifies the consistency of the marker numbering. + */ + + for (seq_no = 1; seq_no <= MAX_SEQ_NO; seq_no++) + marker_present[seq_no] = 0; + + for (marker = cinfo->marker_list; marker != NULL; marker = marker->next) { + if (marker_is_icc(marker)) { + if (num_markers == 0) + num_markers = GETJOCTET(marker->data[13]); + else if (num_markers != GETJOCTET(marker->data[13])) + return FALSE; /* inconsistent num_markers fields */ + seq_no = GETJOCTET(marker->data[12]); + if (seq_no <= 0 || seq_no > num_markers) + return FALSE; /* bogus sequence number */ + if (marker_present[seq_no]) + return FALSE; /* duplicate sequence numbers */ + marker_present[seq_no] = 1; + data_length[seq_no] = marker->data_length - ICC_OVERHEAD_LEN; + } + } + + if (num_markers == 0) + return FALSE; + + /* Check for missing markers, count total space needed, + * compute offset of each marker's part of the data. + */ + + total_length = 0; + for (seq_no = 1; seq_no <= num_markers; seq_no++) { + if (marker_present[seq_no] == 0) + return FALSE; /* missing sequence number */ + data_offset[seq_no] = total_length; + total_length += data_length[seq_no]; + } + + if (total_length <= 0) + return FALSE; /* found only empty markers? */ + + /* Allocate space for assembled data */ + icc_data = (JOCTET *) malloc(total_length * sizeof(JOCTET)); + if (icc_data == NULL) + return FALSE; /* oops, out of memory */ + + /* and fill it in */ + for (marker = cinfo->marker_list; marker != NULL; marker = marker->next) { + if (marker_is_icc(marker)) { + JOCTET FAR *src_ptr; + JOCTET *dst_ptr; + unsigned int length; + seq_no = GETJOCTET(marker->data[12]); + dst_ptr = icc_data + data_offset[seq_no]; + src_ptr = marker->data + ICC_OVERHEAD_LEN; + length = data_length[seq_no]; + while (length--) { + *dst_ptr++ = *src_ptr++; + } + } + } + + *icc_data_ptr = icc_data; + *icc_data_len = total_length; + + return TRUE; +} + diff --git a/rtengine/jdatasrc.cc b/rtengine/jdatasrc.cc new file mode 100644 index 000000000..ddd7e2df9 --- /dev/null +++ b/rtengine/jdatasrc.cc @@ -0,0 +1,411 @@ +#include +#include +#include +#include "jpeg.h" + +/* + * jdatasrc.c + * + * Copyright (C) 1994-1996, Thomas G. Lane. + * Modified 2009-2010 by Guido Vollbeding. + * This file is part of the Independent JPEG Group's software. + * For conditions of distribution and use, see the accompanying README file. + * + * This file contains decompression data source routines for the case of + * reading JPEG data from a file (or any stdio stream). While these routines + * are sufficient for most applications, some will want to use a different + * source manager. + * IMPORTANT: we assume that fread() will correctly transcribe an array of + * JOCTETs from 8-bit-wide elements on external storage. If char is wider + * than 8 bits on your machine, you may need to do some tweaking. + */ + +/* this is not a core library module, so it doesn't define JPEG_INTERNALS */ +//#include "jinclude.h" + +#define JFREAD(file,buf,sizeofbuf) \ + ((size_t) fread((void *) (buf), (size_t) 1, (size_t) (sizeofbuf), (file))) +#define JFWRITE(file,buf,sizeofbuf) \ + ((size_t) fwrite((const void *) (buf), (size_t) 1, (size_t) (sizeofbuf), (file))) + + + + +/* Expanded data source object for stdio input */ + +typedef struct { + struct jpeg_source_mgr pub; /* public fields */ + jmp_buf error_jmp_buf; /* error handler for this instance */ + + FILE * infile; /* source stream */ + JOCTET * buffer; /* start of buffer */ + boolean start_of_file; /* have we gotten any data yet? */ +} my_source_mgr; + +typedef my_source_mgr * my_src_ptr; + +#define INPUT_BUF_SIZE 4096 /* choose an efficiently fread'able size */ + + +/* + * Initialize source --- called by jpeg_read_header + * before any data is actually read. + */ + +METHODDEF(void) +my_init_source (j_decompress_ptr cinfo) +{ + my_src_ptr src = (my_src_ptr) cinfo->src; + + /* We reset the empty-input-file flag for each image, + * but we don't clear the input buffer. + * This is correct behavior for reading a series of images from one source. + */ + src->start_of_file = TRUE; +} + + +/* + * Fill the input buffer --- called whenever buffer is emptied. + * + * In typical applications, this should read fresh data into the buffer + * (ignoring the current state of next_input_byte & bytes_in_buffer), + * reset the pointer & count to the start of the buffer, and return TRUE + * indicating that the buffer has been reloaded. It is not necessary to + * fill the buffer entirely, only to obtain at least one more byte. + * + * There is no such thing as an EOF return. If the end of the file has been + * reached, the routine has a choice of ERREXIT() or inserting fake data into + * the buffer. In most cases, generating a warning message and inserting a + * fake EOI marker is the best course of action --- this will allow the + * decompressor to output however much of the image is there. However, + * the resulting error message is misleading if the real problem is an empty + * input file, so we handle that case specially. + * + * In applications that need to be able to suspend compression due to input + * not being available yet, a FALSE return indicates that no more data can be + * obtained right now, but more may be forthcoming later. In this situation, + * the decompressor will return to its caller (with an indication of the + * number of scanlines it has read, if any). The application should resume + * decompression after it has loaded more data into the input buffer. Note + * that there are substantial restrictions on the use of suspension --- see + * the documentation. + * + * When suspending, the decompressor will back up to a convenient restart point + * (typically the start of the current MCU). next_input_byte & bytes_in_buffer + * indicate where the restart point will be if the current call returns FALSE. + * Data beyond this point must be rescanned after resumption, so move it to + * the front of the buffer rather than discarding it. + */ + +METHODDEF(boolean) +my_fill_input_buffer (j_decompress_ptr cinfo) +{ + my_src_ptr src = (my_src_ptr) cinfo->src; + size_t nbytes; + + nbytes = JFREAD(src->infile, src->buffer, INPUT_BUF_SIZE); + + if (nbytes <= 0) { + if (src->start_of_file) /* Treat empty input file as fatal error */ + ERREXIT(cinfo, JERR_INPUT_EMPTY); + WARNMS(cinfo, JWRN_JPEG_EOF); + /* Insert a fake EOI marker */ + src->buffer[0] = (JOCTET) 0xFF; + src->buffer[1] = (JOCTET) JPEG_EOI; + nbytes = 2; + } + + if (src->start_of_file) + src->buffer[0] = (JOCTET) 0xFF; + + src->pub.next_input_byte = src->buffer; + src->pub.bytes_in_buffer = nbytes; + src->start_of_file = FALSE; + + return TRUE; +} + + +/* + * Skip data --- used to skip over a potentially large amount of + * uninteresting data (such as an APPn marker). + * + * Writers of suspendable-input applications must note that skip_input_data + * is not granted the right to give a suspension return. If the skip extends + * beyond the data currently in the buffer, the buffer can be marked empty so + * that the next read will cause a fill_input_buffer call that can suspend. + * Arranging for additional bytes to be discarded before reloading the input + * buffer is the application writer's problem. + */ + +METHODDEF(void) +my_skip_input_data (j_decompress_ptr cinfo, long num_bytes) +{ + my_src_ptr src = (my_src_ptr) cinfo->src; + + /* Just a dumb implementation for now. Could use fseek() except + * it doesn't work on pipes. Not clear that being smart is worth + * any trouble anyway --- large skips are infrequent. + */ + if (num_bytes > 0) { + while (num_bytes > (long) src->pub.bytes_in_buffer) { + num_bytes -= (long) src->pub.bytes_in_buffer; + (void) my_fill_input_buffer(cinfo); + /* note we assume that fill_input_buffer will never return FALSE, + * so suspension need not be handled. + */ + } + src->pub.next_input_byte += (size_t) num_bytes; + src->pub.bytes_in_buffer -= (size_t) num_bytes; + } +} + + +/* + * An additional method that can be provided by data source modules is the + * resync_to_restart method for error recovery in the presence of RST markers. + * For the moment, this source module just uses the default resync method + * provided by the JPEG library. That method assumes that no backtracking + * is possible. + */ + + +/* + * Terminate source --- called by jpeg_finish_decompress + * after all data has been read. Often a no-op. + * + * NB: *not* called by jpeg_abort or jpeg_destroy; surrounding + * application must deal with any cleanup that should happen even + * for error exit. + */ + +METHODDEF(void) +my_term_source (j_decompress_ptr cinfo) +{ + /* no work necessary here */ +} + + +/* + * Prepare for input from a stdio stream. + * The caller must have already opened the stream, and is responsible + * for closing it after finishing decompression. + */ + +GLOBAL(void) +my_jpeg_stdio_src (j_decompress_ptr cinfo, FILE * infile) +{ + my_src_ptr src; + + /* The source object and input buffer are made permanent so that a series + * of JPEG images can be read from the same file by calling jpeg_stdio_src + * only before the first one. (If we discarded the buffer at the end of + * one image, we'd likely lose the start of the next one.) + * This makes it unsafe to use this manager and a different source + * manager serially with the same JPEG object. Caveat programmer. + */ + if (cinfo->src == NULL) { /* first time for this JPEG object? */ + cinfo->src = (struct jpeg_source_mgr *) + (*cinfo->mem->alloc_small) ((j_common_ptr) cinfo, JPOOL_PERMANENT, + sizeof(my_source_mgr)); + src = (my_src_ptr) cinfo->src; + src->buffer = (JOCTET *) + (*cinfo->mem->alloc_small) ((j_common_ptr) cinfo, JPOOL_PERMANENT, + INPUT_BUF_SIZE * sizeof(JOCTET)); + } + + src = (my_src_ptr) cinfo->src; + src->pub.init_source = my_init_source; + src->pub.fill_input_buffer = my_fill_input_buffer; + src->pub.skip_input_data = my_skip_input_data; + src->pub.resync_to_restart = jpeg_resync_to_restart; /* use default method */ + src->pub.term_source = my_term_source; + src->infile = infile; + src->pub.bytes_in_buffer = 0; /* forces fill_input_buffer on first read */ + src->pub.next_input_byte = NULL; /* until buffer loaded */ +} + +METHODDEF(void) +my_error_exit (j_common_ptr cinfo) +{ + /* Always display the message */ + (*cinfo->err->output_message) (cinfo); + + /* Let the memory manager delete any temp files before we die */ + //jpeg_destroy(cinfo); + + j_decompress_ptr dinfo = (j_decompress_ptr)cinfo; + longjmp (((rt_jpeg_error_mgr*)(dinfo->src))->error_jmp_buf, 1); +} + + +//const char * const jpeg_std_message_table[] = { +//#include "jerror.h" +// NULL +//}; +extern const char * const jpeg_std_message_table[]; + + +/* + * Actual output of an error or trace message. + * Applications may override this method to send JPEG messages somewhere + * other than stderr. + * + * On Windows, printing to stderr is generally completely useless, + * so we provide optional code to produce an error-dialog popup. + * Most Windows applications will still prefer to override this routine, + * but if they don't, it'll do something at least marginally useful. + * + * NOTE: to use the library in an environment that doesn't support the + * C stdio library, you may have to delete the call to fprintf() entirely, + * not just not use this routine. + */ + +METHODDEF(void) +output_message (j_common_ptr cinfo) +{ + char buffer[JMSG_LENGTH_MAX]; + + /* Create the message */ + (*cinfo->err->format_message) (cinfo, buffer); + +#ifdef USE_WINDOWS_MESSAGEBOX + /* Display it in a message dialog box */ + MessageBox(GetActiveWindow(), buffer, "JPEG Library Error", + MB_OK | MB_ICONERROR); +#else + /* Send it to stderr, adding a newline */ + fprintf(stderr, "%s\n", buffer); +#endif +} + + +/* + * Decide whether to emit a trace or warning message. + * msg_level is one of: + * -1: recoverable corrupt-data warning, may want to abort. + * 0: important advisory messages (always display to user). + * 1: first level of tracing detail. + * 2,3,...: successively more detailed tracing messages. + * An application might override this method if it wanted to abort on warnings + * or change the policy about which messages to display. + */ + +METHODDEF(void) +emit_message (j_common_ptr cinfo, int msg_level) +{ + struct jpeg_error_mgr * err = cinfo->err; + + if (msg_level < 0) { + /* It's a warning message. Since corrupt files may generate many warnings, + * the policy implemented here is to show only the first warning, + * unless trace_level >= 3. + */ + if (err->num_warnings == 0 || err->trace_level >= 3) + (*err->output_message) (cinfo); + /* Always count warnings in num_warnings. */ + err->num_warnings++; + } else { + /* It's a trace message. Show it if trace_level >= msg_level. */ + if (err->trace_level >= msg_level) + (*err->output_message) (cinfo); + } +} + + +/* + * Format a message string for the most recent JPEG error or message. + * The message is stored into buffer, which should be at least JMSG_LENGTH_MAX + * characters. Note that no '\n' character is added to the string. + * Few applications should need to override this method. + */ + +METHODDEF(void) +format_message (j_common_ptr cinfo, char * buffer) +{ + struct jpeg_error_mgr * err = cinfo->err; + int msg_code = err->msg_code; + const char * msgtext = NULL; + const char * msgptr; + char ch; + boolean isstring; + + /* Look up message string in proper table */ + if (msg_code > 0 && msg_code <= err->last_jpeg_message) { + msgtext = err->jpeg_message_table[msg_code]; + } else if (err->addon_message_table != NULL && + msg_code >= err->first_addon_message && + msg_code <= err->last_addon_message) { + msgtext = err->addon_message_table[msg_code - err->first_addon_message]; + } + + /* Defend against bogus message number */ + if (msgtext == NULL) { + err->msg_parm.i[0] = msg_code; + msgtext = err->jpeg_message_table[0]; + } + + /* Check for string parameter, as indicated by %s in the message text */ + isstring = FALSE; + msgptr = msgtext; + while ((ch = *msgptr++) != '\0') { + if (ch == '%') { + if (*msgptr == 's') isstring = TRUE; + break; + } + } + + /* Format the message into the passed buffer */ + if (isstring) + sprintf(buffer, msgtext, err->msg_parm.s); + else + sprintf(buffer, msgtext, + err->msg_parm.i[0], err->msg_parm.i[1], + err->msg_parm.i[2], err->msg_parm.i[3], + err->msg_parm.i[4], err->msg_parm.i[5], + err->msg_parm.i[6], err->msg_parm.i[7]); +} + + +/* + * Reset error state variables at start of a new image. + * This is called during compression startup to reset trace/error + * processing to default state, without losing any application-specific + * method pointers. An application might possibly want to override + * this method if it has additional error processing state. + */ + +METHODDEF(void) +reset_error_mgr (j_common_ptr cinfo) +{ + cinfo->err->num_warnings = 0; + /* trace_level is not reset since it is an application-supplied parameter */ + cinfo->err->msg_code = 0; /* may be useful as a flag for "no error" */ +} + + +GLOBAL(struct jpeg_error_mgr *) +my_jpeg_std_error (struct jpeg_error_mgr * err) +{ + + err->error_exit = my_error_exit; + err->emit_message = emit_message; + err->output_message = output_message; + err->format_message = format_message; + err->reset_error_mgr = reset_error_mgr; + + err->trace_level = 0; /* default = no tracing */ + err->num_warnings = 0; /* no warnings emitted yet */ + err->msg_code = 0; /* may be useful as a flag for "no error" */ + + /* Initialize message table pointers */ + err->jpeg_message_table = jpeg_std_message_table; + err->last_jpeg_message = (int) JMSG_LASTMSGCODE - 1; + + err->addon_message_table = NULL; + err->first_addon_message = 0; /* for safety */ + err->last_addon_message = 0; + + return err; +} diff --git a/rtengine/jpeg_memsrc.cc b/rtengine/jpeg_memsrc.cc new file mode 100644 index 000000000..7d5970fb3 --- /dev/null +++ b/rtengine/jpeg_memsrc.cc @@ -0,0 +1,169 @@ +/* + * memsrc.c + * + * Copyright (C) 1994-1996, Thomas G. Lane. + * This file is part of the Independent JPEG Group's software. + * For conditions of distribution and use, see the accompanying README file. + * + * This file contains decompression data source routines for the case of + * reading JPEG data from a memory buffer that is preloaded with the entire + * JPEG file. This would not seem especially useful at first sight, but + * a number of people have asked for it. + * This is really just a stripped-down version of jdatasrc.c. Comparison + * of this code with jdatasrc.c may be helpful in seeing how to make + * custom source managers for other purposes. + */ + +/* this is not a core library module, so it doesn't define JPEG_INTERNALS */ +//#include "jinclude.h" +//#include "jmorecfg.h" +//#include "jpeglib.h" +//#include "jerror.h" +#include +#include +#include +#include "jpeg.h" + + +/* Expanded data source object for memory input */ + +typedef struct { + struct jpeg_source_mgr pub; /* public fields */ + jmp_buf error_jmp_buf; /* error handler for this instance */ + + JOCTET eoi_buffer[2]; /* a place to put a dummy EOI */ +} my_source_mgr; + +typedef my_source_mgr * my_src_ptr; + + +/* + * Initialize source --- called by jpeg_read_header + * before any data is actually read. + */ + +static void +init_source (j_decompress_ptr cinfo) +{ + /* No work, since jpeg_memory_src set up the buffer pointer and count. + * Indeed, if we want to read multiple JPEG images from one buffer, + * this *must* not do anything to the pointer. + */ +} + + +/* + * Fill the input buffer --- called whenever buffer is emptied. + * + * In this application, this routine should never be called; if it is called, + * the decompressor has overrun the end of the input buffer, implying we + * supplied an incomplete or corrupt JPEG datastream. A simple error exit + * might be the most appropriate response. + * + * But what we choose to do in this code is to supply dummy EOI markers + * in order to force the decompressor to finish processing and supply + * some sort of output image, no matter how corrupted. + */ + +static boolean +fill_input_buffer(j_decompress_ptr cinfo) +{ + my_src_ptr src = (my_src_ptr) cinfo->src; + + WARNMS(cinfo, JWRN_JPEG_EOF); + + /* Create a fake EOI marker */ + src->eoi_buffer[0] = (JOCTET) 0xFF; + src->eoi_buffer[1] = (JOCTET) JPEG_EOI; + src->pub.next_input_byte = src->eoi_buffer; + src->pub.bytes_in_buffer = 2; + + return TRUE; +} + + +/* + * Skip data --- used to skip over a potentially large amount of + * uninteresting data (such as an APPn marker). + * + * If we overrun the end of the buffer, we let fill_input_buffer deal with + * it. An extremely large skip could cause some time-wasting here, but + * it really isn't supposed to happen ... and the decompressor will never + * skip more than 64K anyway. + */ + +static void +skip_input_data (j_decompress_ptr cinfo, long num_bytes) +{ + my_src_ptr src = (my_src_ptr) cinfo->src; + + if (num_bytes > 0) { + while (num_bytes > (long) src->pub.bytes_in_buffer) { + num_bytes -= (long) src->pub.bytes_in_buffer; + (void) fill_input_buffer(cinfo); + /* note we assume that fill_input_buffer will never return FALSE, + * so suspension need not be handled. + */ + } + src->pub.next_input_byte += (size_t) num_bytes; + src->pub.bytes_in_buffer -= (size_t) num_bytes; + } +} + + +/* + * An additional method that can be provided by data source modules is the + * resync_to_restart method for error recovery in the presence of RST markers. + * For the moment, this source module just uses the default resync method + * provided by the JPEG library. That method assumes that no backtracking + * is possible. + */ + + +/* + * Terminate source --- called by jpeg_finish_decompress + * after all data has been read. Often a no-op. + * + * NB: *not* called by jpeg_abort or jpeg_destroy; surrounding + * application must deal with any cleanup that should happen even + * for error exit. + */ + +static void +term_source (j_decompress_ptr cinfo) +{ + /* no work necessary here */ +} + + +/* + * Prepare for input from a memory buffer. + */ + +GLOBAL(void) +jpeg_memory_src (j_decompress_ptr cinfo, const JOCTET * buffer, size_t bufsize) +{ + my_src_ptr src; + + /* The source object is made permanent so that a series of JPEG images + * can be read from a single buffer by calling jpeg_memory_src + * only before the first one. + * This makes it unsafe to use this manager and a different source + * manager serially with the same JPEG object. Caveat programmer. + */ + if (cinfo->src == NULL) { /* first time for this JPEG object? */ + cinfo->src = (struct jpeg_source_mgr *) + (*cinfo->mem->alloc_small) ((j_common_ptr) cinfo, JPOOL_PERMANENT, + sizeof(my_source_mgr)); + } + + src = (my_src_ptr) cinfo->src; + src->pub.init_source = init_source; + src->pub.fill_input_buffer = fill_input_buffer; + src->pub.skip_input_data = skip_input_data; + src->pub.resync_to_restart = jpeg_resync_to_restart; /* use default method */ + src->pub.term_source = term_source; + + src->pub.next_input_byte = buffer; + src->pub.bytes_in_buffer = bufsize; +} diff --git a/rtengine/klt/convolve.cc b/rtengine/klt/convolve.cc new file mode 100644 index 000000000..75918f72a --- /dev/null +++ b/rtengine/klt/convolve.cc @@ -0,0 +1,317 @@ +/********************************************************************* + * convolve.c + *********************************************************************/ + +/* Standard includes */ +#include +#include +#include /* malloc(), realloc() */ + +/* Our includes */ +#include "base.h" +#include "error.h" +#include "convolve.h" +#include "klt_util.h" /* printing */ + +#define MAX_KERNEL_WIDTH 71 + + +typedef struct { + int width; + float data[MAX_KERNEL_WIDTH]; +} ConvolutionKernel; + +/* Kernels */ +static ConvolutionKernel gauss_kernel; +static ConvolutionKernel gaussderiv_kernel; +static float sigma_last = -10.0; + + +/********************************************************************* + * _KLTToFloatImage + * + * Given a pointer to image data (probably unsigned chars), copy + * data to a float image. + */ + +void _KLTToFloatImage( + KLT_PixelType *img, + int ncols, int nrows, + _KLT_FloatImage floatimg) +{ + KLT_PixelType *ptrend = img + ncols*nrows; + float *ptrout = floatimg->data; + + /* Output image must be large enough to hold result */ + assert(floatimg->ncols >= ncols); + assert(floatimg->nrows >= nrows); + + floatimg->ncols = ncols; + floatimg->nrows = nrows; + + while (img < ptrend) *ptrout++ = (float) *img++; +} + + +/********************************************************************* + * _computeKernels + */ + +static void _computeKernels( + float sigma, + ConvolutionKernel *gauss, + ConvolutionKernel *gaussderiv) +{ + const float factor = 0.01f; /* for truncating tail */ + int i; + + assert(MAX_KERNEL_WIDTH % 2 == 1); + assert(sigma >= 0.0); + + /* Compute kernels, and automatically determine widths */ + { + const int hw = MAX_KERNEL_WIDTH / 2; + float max_gauss = 1.0f, max_gaussderiv = (float) (sigma*exp(-0.5f)); + + /* Compute gauss and deriv */ + for (i = -hw ; i <= hw ; i++) { + gauss->data[i+hw] = (float) exp(-i*i / (2*sigma*sigma)); + gaussderiv->data[i+hw] = -i * gauss->data[i+hw]; + } + + /* Compute widths */ + gauss->width = MAX_KERNEL_WIDTH; + for (i = -hw ; fabs(gauss->data[i+hw] / max_gauss) < factor ; + i++, gauss->width -= 2); + gaussderiv->width = MAX_KERNEL_WIDTH; + for (i = -hw ; fabs(gaussderiv->data[i+hw] / max_gaussderiv) < factor ; + i++, gaussderiv->width -= 2); + if (gauss->width == MAX_KERNEL_WIDTH || + gaussderiv->width == MAX_KERNEL_WIDTH) + KLTError("(_computeKernels) MAX_KERNEL_WIDTH %d is too small for " + "a sigma of %f", MAX_KERNEL_WIDTH, sigma); + } + + /* Shift if width less than MAX_KERNEL_WIDTH */ + for (i = 0 ; i < gauss->width ; i++) + gauss->data[i] = gauss->data[i+(MAX_KERNEL_WIDTH-gauss->width)/2]; + for (i = 0 ; i < gaussderiv->width ; i++) + gaussderiv->data[i] = gaussderiv->data[i+(MAX_KERNEL_WIDTH-gaussderiv->width)/2]; + /* Normalize gauss and deriv */ + { + const int hw = gaussderiv->width / 2; + float den; + + den = 0.0; + for (i = 0 ; i < gauss->width ; i++) den += gauss->data[i]; + for (i = 0 ; i < gauss->width ; i++) gauss->data[i] /= den; + den = 0.0; + for (i = -hw ; i <= hw ; i++) den -= i*gaussderiv->data[i+hw]; + for (i = -hw ; i <= hw ; i++) gaussderiv->data[i+hw] /= den; + } + + sigma_last = sigma; +} + + +/********************************************************************* + * _KLTGetKernelWidths + * + */ + +void _KLTGetKernelWidths( + float sigma, + int *gauss_width, + int *gaussderiv_width) +{ + _computeKernels(sigma, &gauss_kernel, &gaussderiv_kernel); + *gauss_width = gauss_kernel.width; + *gaussderiv_width = gaussderiv_kernel.width; +} + + +/********************************************************************* + * _convolveImageHoriz + */ + +static void _convolveImageHoriz( + _KLT_FloatImage imgin, + ConvolutionKernel kernel, + _KLT_FloatImage imgout) +{ + float *ptrrow = imgin->data; /* Points to row's first pixel */ + float *ptrout = imgout->data, /* Points to next output pixel */ + *ppp; + float sum; + int radius = kernel.width / 2; + int ncols = imgin->ncols, nrows = imgin->nrows; + 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 */ + float *ptrout = imgout->data, /* Points to next output pixel */ + *ppp; + float sum; + int radius = kernel.width / 2; + int ncols = imgin->ncols, nrows = imgin->nrows; + int i, j, k; + + /* Kernel width must be odd */ + assert(kernel.width % 2 == 1); + + /* Must read from and write to different images */ + assert(imgin != imgout); + + /* Output image must be large enough to hold result */ + assert(imgout->ncols >= imgin->ncols); + assert(imgout->nrows >= imgin->nrows); + + /* For each column, do ... */ + for (i = 0 ; i < ncols ; i++) { + + /* Zero topmost rows */ + for (j = 0 ; j < radius ; j++) { + *ptrout = 0.0; + ptrout += ncols; + } + + /* Convolve middle rows with kernel */ + for ( ; j < nrows - radius ; j++) { + ppp = ptrcol + ncols * (j - radius); + sum = 0.0; + for (k = kernel.width-1 ; k >= 0 ; k--) { + sum += *ppp * kernel.data[k]; + ppp += ncols; + } + *ptrout = sum; + ptrout += ncols; + } + + /* Zero bottommost rows */ + for ( ; j < nrows ; j++) { + *ptrout = 0.0; + ptrout += ncols; + } + + ptrcol++; + ptrout -= nrows * ncols - 1; + } +} + + +/********************************************************************* + * _convolveSeparate + */ + +static void _convolveSeparate( + _KLT_FloatImage imgin, + ConvolutionKernel horiz_kernel, + ConvolutionKernel vert_kernel, + _KLT_FloatImage imgout) +{ + /* Create temporary image */ + _KLT_FloatImage tmpimg; + tmpimg = _KLTCreateFloatImage(imgin->ncols, imgin->nrows); + + /* Do convolution */ + _convolveImageHoriz(imgin, horiz_kernel, tmpimg); + + _convolveImageVert(tmpimg, vert_kernel, imgout); + + /* Free memory */ + _KLTFreeFloatImage(tmpimg); +} + + +/********************************************************************* + * _KLTComputeGradients + */ + +void _KLTComputeGradients( + _KLT_FloatImage img, + float sigma, + _KLT_FloatImage gradx, + _KLT_FloatImage grady) +{ + + /* Output images must be large enough to hold result */ + assert(gradx->ncols >= img->ncols); + assert(gradx->nrows >= img->nrows); + assert(grady->ncols >= img->ncols); + assert(grady->nrows >= img->nrows); + + /* Compute kernels, if necessary */ + if (fabs(sigma - sigma_last) > 0.05) + _computeKernels(sigma, &gauss_kernel, &gaussderiv_kernel); + + _convolveSeparate(img, gaussderiv_kernel, gauss_kernel, gradx); + _convolveSeparate(img, gauss_kernel, gaussderiv_kernel, grady); + +} + + +/********************************************************************* + * _KLTComputeSmoothedImage + */ + +void _KLTComputeSmoothedImage( + _KLT_FloatImage img, + float sigma, + _KLT_FloatImage smooth) +{ + /* Output image must be large enough to hold result */ + assert(smooth->ncols >= img->ncols); + assert(smooth->nrows >= img->nrows); + + /* Compute kernel, if necessary; gauss_deriv is not used */ + if (fabs(sigma - sigma_last) > 0.05) + _computeKernels(sigma, &gauss_kernel, &gaussderiv_kernel); + + _convolveSeparate(img, gauss_kernel, gauss_kernel, smooth); +} + + + diff --git a/rtengine/klt/error.cc b/rtengine/klt/error.cc new file mode 100644 index 000000000..b984d3aeb --- /dev/null +++ b/rtengine/klt/error.cc @@ -0,0 +1,56 @@ +/********************************************************************* + * error.c + * + * Error and warning messages, and system commands. + *********************************************************************/ + + +/* Standard includes */ +#include +#include +#include + + +/********************************************************************* + * KLTError + * + * Prints an error message and dies. + * + * INPUTS + * exactly like printf + */ + +void KLTError(char *fmt, ...) +{ + va_list args; + + va_start(args, fmt); + fprintf(stderr, "KLT Error: "); + vfprintf(stderr, fmt, args); + fprintf(stderr, "\n"); + va_end(args); + exit(1); +} + + +/********************************************************************* + * KLTWarning + * + * Prints a warning message. + * + * INPUTS + * exactly like printf + */ + +void KLTWarning(char *fmt, ...) +{ + va_list args; + + va_start(args, fmt); + fprintf(stderr, "KLT Warning: "); + vfprintf(stderr, fmt, args); + fprintf(stderr, "\n"); + fflush(stderr); + va_end(args); +} + diff --git a/rtengine/klt/klt.cc b/rtengine/klt/klt.cc new file mode 100644 index 000000000..e5938f112 --- /dev/null +++ b/rtengine/klt/klt.cc @@ -0,0 +1,531 @@ +/********************************************************************* + * klt.c + * + * Kanade-Lucas-Tomasi tracker + *********************************************************************/ + +/* Standard includes */ +#include +#include /* logf() */ +#include /* malloc() */ + +/* Our includes */ +#include "base.h" +#include "convolve.h" +#include "error.h" +#include "klt.h" +#include "pyramid.h" + + +static const int mindist = 10; +static const int window_size = 7; +static const int min_eigenvalue = 1; +static const float min_determinant = 0.01f; +static const float min_displacement = 0.1f; +static const int max_iterations = 10; +static const float max_residue = 10.0f; +static const float grad_sigma = 1.0f; +static const float smooth_sigma_fact = 0.1f; +static const float pyramid_sigma_fact = 0.9f; +static const float step_factor = 1.0f; +static const KLT_BOOL sequentialMode = FALSE; +static const KLT_BOOL lighting_insensitive = FALSE; +/* for affine mapping*/ +static const int affineConsistencyCheck = -1; +static const int affine_window_size = 15; +static const int affine_max_iterations = 10; +static const float affine_max_residue = 10.0; +static const float affine_min_displacement = 0.02f; +static const float affine_max_displacement_differ = 1.5f; + +static const KLT_BOOL smoothBeforeSelecting = TRUE; +static const KLT_BOOL writeInternalImages = FALSE; +static const int search_range = 15; +static const int nSkippedPixels = 0; + +extern int KLT_verbose; + + +/********************************************************************* + * _createArray2D + * + * Creates a two-dimensional array. + * + * INPUTS + * ncols: no. of columns + * nrows: no. of rows + * nbytes: no. of bytes per entry + * + * RETURNS + * Pointer to an array. Must be coerced. + * + * EXAMPLE + * char **ar; + * ar = (char **) createArray2D(8, 5, sizeof(char)); + */ + +static void** _createArray2D(int ncols, int nrows, int nbytes) +{ + char **tt; + int i; + + tt = (char **) malloc(nrows * sizeof(void *) + + ncols * nrows * nbytes); + if (tt == NULL) + KLTError("(createArray2D) Out of memory"); + + for (i = 0 ; i < nrows ; i++) + tt[i] = ((char *) tt) + (nrows * sizeof(void *) + + i * ncols * nbytes); + + return((void **) tt); +} + + +/********************************************************************* + * KLTCreateTrackingContext + * + */ + +KLT_TrackingContext KLTCreateTrackingContext() +{ + KLT_TrackingContext tc; + + /* Allocate memory */ + tc = (KLT_TrackingContext) malloc(sizeof(KLT_TrackingContextRec)); + + /* Set values to default values */ + tc->mindist = mindist; + tc->window_width = window_size; + tc->window_height = window_size; + tc->sequentialMode = sequentialMode; + tc->smoothBeforeSelecting = smoothBeforeSelecting; + tc->writeInternalImages = writeInternalImages; + tc->lighting_insensitive = lighting_insensitive; + tc->min_eigenvalue = min_eigenvalue; + tc->min_determinant = min_determinant; + tc->max_iterations = max_iterations; + tc->min_displacement = min_displacement; + tc->max_residue = max_residue; + tc->grad_sigma = grad_sigma; + tc->smooth_sigma_fact = smooth_sigma_fact; + tc->pyramid_sigma_fact = pyramid_sigma_fact; + tc->step_factor = step_factor; + tc->nSkippedPixels = nSkippedPixels; + tc->pyramid_last = NULL; + tc->pyramid_last_gradx = NULL; + tc->pyramid_last_grady = NULL; + /* for affine mapping */ + tc->affineConsistencyCheck = affineConsistencyCheck; + tc->affine_window_width = affine_window_size; + tc->affine_window_height = affine_window_size; + tc->affine_max_iterations = affine_max_iterations; + tc->affine_max_residue = affine_max_residue; + tc->affine_min_displacement = affine_min_displacement; + tc->affine_max_displacement_differ = affine_max_displacement_differ; + + /* Change nPyramidLevels and subsampling */ + KLTChangeTCPyramid(tc, search_range); + + /* Update border, which is dependent upon */ + /* smooth_sigma_fact, pyramid_sigma_fact, window_size, and subsampling */ + KLTUpdateTCBorder(tc); + + return(tc); +} + + +/********************************************************************* + * KLTCreateFeatureList + * + */ + +KLT_FeatureList KLTCreateFeatureList( + int nFeatures) +{ + KLT_FeatureList fl; + KLT_Feature first; + int nbytes = sizeof(KLT_FeatureListRec) + + nFeatures * sizeof(KLT_Feature) + + nFeatures * sizeof(KLT_FeatureRec); + int i; + + /* Allocate memory for feature list */ + fl = (KLT_FeatureList) malloc(nbytes); + + /* Set parameters */ + fl->nFeatures = nFeatures; + + /* Set pointers */ + fl->feature = (KLT_Feature *) (fl + 1); + first = (KLT_Feature) (fl->feature + nFeatures); + for (i = 0 ; i < nFeatures ; i++) { + fl->feature[i] = first + i; + fl->feature[i]->aff_img = NULL; /* initialization fixed by Sinisa Segvic */ + fl->feature[i]->aff_img_gradx = NULL; + fl->feature[i]->aff_img_grady = NULL; + } + /* Return feature list */ + return(fl); +} + + +/********************************************************************* + * KLTCreateFeatureHistory + * + */ + +KLT_FeatureHistory KLTCreateFeatureHistory( + int nFrames) +{ + KLT_FeatureHistory fh; + KLT_Feature first; + int nbytes = sizeof(KLT_FeatureHistoryRec) + + nFrames * sizeof(KLT_Feature) + + nFrames * sizeof(KLT_FeatureRec); + int i; + + /* Allocate memory for feature history */ + fh = (KLT_FeatureHistory) malloc(nbytes); + + /* Set parameters */ + fh->nFrames = nFrames; + + /* Set pointers */ + fh->feature = (KLT_Feature *) (fh + 1); + first = (KLT_Feature) (fh->feature + nFrames); + for (i = 0 ; i < nFrames ; i++) + fh->feature[i] = first + i; + + /* Return feature history */ + return(fh); +} + + +/********************************************************************* + * KLTCreateFeatureTable + * + */ + +KLT_FeatureTable KLTCreateFeatureTable( + int nFrames, + int nFeatures) +{ + KLT_FeatureTable ft; + KLT_Feature first; + int nbytes = sizeof(KLT_FeatureTableRec); + int i, j; + + /* Allocate memory for feature history */ + ft = (KLT_FeatureTable) malloc(nbytes); + + /* Set parameters */ + ft->nFrames = nFrames; + ft->nFeatures = nFeatures; + + /* Set pointers */ + ft->feature = (KLT_Feature **) + _createArray2D(nFrames, nFeatures, sizeof(KLT_Feature)); + first = (KLT_Feature) malloc(nFrames * nFeatures * sizeof(KLT_FeatureRec)); + for (j = 0 ; j < nFeatures ; j++) + for (i = 0 ; i < nFrames ; i++) + ft->feature[j][i] = first + j*nFrames + i; + + /* Return feature table */ + return(ft); +} + + +/********************************************************************* + * KLTPrintTrackingContext + */ + +void KLTPrintTrackingContext( + KLT_TrackingContext tc) +{ + fprintf(stderr, "\n\nTracking context:\n\n"); + fprintf(stderr, "\tmindist = %d\n", tc->mindist); + fprintf(stderr, "\twindow_width = %d\n", tc->window_width); + fprintf(stderr, "\twindow_height = %d\n", tc->window_height); + fprintf(stderr, "\tsequentialMode = %s\n", + tc->sequentialMode ? "TRUE" : "FALSE"); + fprintf(stderr, "\tsmoothBeforeSelecting = %s\n", + tc->smoothBeforeSelecting ? "TRUE" : "FALSE"); + fprintf(stderr, "\twriteInternalImages = %s\n", + tc->writeInternalImages ? "TRUE" : "FALSE"); + + fprintf(stderr, "\tmin_eigenvalue = %d\n", tc->min_eigenvalue); + fprintf(stderr, "\tmin_determinant = %f\n", tc->min_determinant); + fprintf(stderr, "\tmin_displacement = %f\n", tc->min_displacement); + fprintf(stderr, "\tmax_iterations = %d\n", tc->max_iterations); + fprintf(stderr, "\tmax_residue = %f\n", tc->max_residue); + fprintf(stderr, "\tgrad_sigma = %f\n", tc->grad_sigma); + fprintf(stderr, "\tsmooth_sigma_fact = %f\n", tc->smooth_sigma_fact); + fprintf(stderr, "\tpyramid_sigma_fact = %f\n", tc->pyramid_sigma_fact); + fprintf(stderr, "\tnSkippedPixels = %d\n", tc->nSkippedPixels); + fprintf(stderr, "\tborderx = %d\n", tc->borderx); + fprintf(stderr, "\tbordery = %d\n", tc->bordery); + fprintf(stderr, "\tnPyramidLevels = %d\n", tc->nPyramidLevels); + fprintf(stderr, "\tsubsampling = %d\n", tc->subsampling); + + fprintf(stderr, "\n\tpyramid_last = %s\n", (tc->pyramid_last!=NULL) ? + "points to old image" : "NULL"); + fprintf(stderr, "\tpyramid_last_gradx = %s\n", + (tc->pyramid_last_gradx!=NULL) ? + "points to old image" : "NULL"); + fprintf(stderr, "\tpyramid_last_grady = %s\n", + (tc->pyramid_last_grady!=NULL) ? + "points to old image" : "NULL"); + fprintf(stderr, "\n\n"); +} + + +/********************************************************************* + * KLTChangeTCPyramid + * + */ + +void KLTChangeTCPyramid( + KLT_TrackingContext tc, + int search_range) +{ + float window_halfwidth; + float subsampling; + + /* Check window size (and correct if necessary) */ + if (tc->window_width % 2 != 1) { + tc->window_width = tc->window_width+1; + KLTWarning("(KLTChangeTCPyramid) Window width must be odd. " + "Changing to %d.\n", tc->window_width); + } + if (tc->window_height % 2 != 1) { + tc->window_height = tc->window_height+1; + KLTWarning("(KLTChangeTCPyramid) Window height must be odd. " + "Changing to %d.\n", tc->window_height); + } + if (tc->window_width < 3) { + tc->window_width = 3; + KLTWarning("(KLTChangeTCPyramid) Window width must be at least three. \n" + "Changing to %d.\n", tc->window_width); + } + if (tc->window_height < 3) { + tc->window_height = 3; + KLTWarning("(KLTChangeTCPyramid) Window height must be at least three. \n" + "Changing to %d.\n", tc->window_height); + } + window_halfwidth = min(tc->window_width,tc->window_height)/2.0f; + + subsampling = ((float) search_range) / window_halfwidth; + + if (subsampling < 1.0) { /* 1.0 = 0+1 */ + tc->nPyramidLevels = 1; + } else if (subsampling <= 3.0) { /* 3.0 = 2+1 */ + tc->nPyramidLevels = 2; + tc->subsampling = 2; + } else if (subsampling <= 5.0) { /* 5.0 = 4+1 */ + tc->nPyramidLevels = 2; + tc->subsampling = 4; + } else if (subsampling <= 9.0) { /* 9.0 = 8+1 */ + tc->nPyramidLevels = 2; + tc->subsampling = 8; + } else { + /* The following lines are derived from the formula: + search_range = + window_halfwidth * \sum_{i=0}^{nPyramidLevels-1} 8^i, + which is the same as: + search_range = + window_halfwidth * (8^nPyramidLevels - 1)/(8 - 1). + Then, the value is rounded up to the nearest integer. */ + float val = (float) (log(7.0*subsampling+1.0)/log(8.0)); + tc->nPyramidLevels = (int) (val + 0.99); + tc->subsampling = 8; + } +} + + +/********************************************************************* + * NOTE: Manually must ensure consistency with _KLTComputePyramid() + */ + +static float _pyramidSigma( + KLT_TrackingContext tc) +{ + return (tc->pyramid_sigma_fact * tc->subsampling); +} + + +/********************************************************************* + * Updates border, which is dependent upon + * smooth_sigma_fact, pyramid_sigma_fact, window_size, and subsampling + */ + +void KLTUpdateTCBorder( + KLT_TrackingContext tc) +{ + float val; + int pyramid_gauss_hw; + int smooth_gauss_hw; + int gauss_width, gaussderiv_width; + int num_levels = tc->nPyramidLevels; + int n_invalid_pixels; + int window_hw; + int ss = tc->subsampling; + int ss_power; + int border; + int i; + + /* Check window size (and correct if necessary) */ + if (tc->window_width % 2 != 1) { + tc->window_width = tc->window_width+1; + KLTWarning("(KLTUpdateTCBorder) Window width must be odd. " + "Changing to %d.\n", tc->window_width); + } + if (tc->window_height % 2 != 1) { + tc->window_height = tc->window_height+1; + KLTWarning("(KLTUpdateTCBorder) Window height must be odd. " + "Changing to %d.\n", tc->window_height); + } + if (tc->window_width < 3) { + tc->window_width = 3; + KLTWarning("(KLTUpdateTCBorder) Window width must be at least three. \n" + "Changing to %d.\n", tc->window_width); + } + if (tc->window_height < 3) { + tc->window_height = 3; + KLTWarning("(KLTUpdateTCBorder) Window height must be at least three. \n" + "Changing to %d.\n", tc->window_height); + } + window_hw = max(tc->window_width, tc->window_height)/2; + + /* Find widths of convolution windows */ + _KLTGetKernelWidths(_KLTComputeSmoothSigma(tc), + &gauss_width, &gaussderiv_width); + smooth_gauss_hw = gauss_width/2; + _KLTGetKernelWidths(_pyramidSigma(tc), + &gauss_width, &gaussderiv_width); + pyramid_gauss_hw = gauss_width/2; + + /* Compute the # of invalid pixels at each level of the pyramid. + n_invalid_pixels is computed with respect to the ith level + of the pyramid. So, e.g., if n_invalid_pixels = 5 after + the first iteration, then there are 5 invalid pixels in + level 1, which translated means 5*subsampling invalid pixels + in the original level 0. */ + n_invalid_pixels = smooth_gauss_hw; + for (i = 1 ; i < num_levels ; i++) { + val = ((float) n_invalid_pixels + pyramid_gauss_hw) / ss; + n_invalid_pixels = (int) (val + 0.99); /* Round up */ + } + + /* ss_power = ss^(num_levels-1) */ + ss_power = 1; + for (i = 1 ; i < num_levels ; i++) + ss_power *= ss; + + /* Compute border by translating invalid pixels back into */ + /* original image */ + border = (n_invalid_pixels + window_hw) * ss_power; + + tc->borderx = border; + tc->bordery = border; +} + + +/********************************************************************* + * KLTFreeTrackingContext + * KLTFreeFeatureList + * KLTFreeFeatureHistory + * KLTFreeFeatureTable + */ + +void KLTFreeTrackingContext( + KLT_TrackingContext tc) +{ + if (tc->pyramid_last) + _KLTFreePyramid((_KLT_Pyramid) tc->pyramid_last); + if (tc->pyramid_last_gradx) + _KLTFreePyramid((_KLT_Pyramid) tc->pyramid_last_gradx); + if (tc->pyramid_last_grady) + _KLTFreePyramid((_KLT_Pyramid) tc->pyramid_last_grady); + free(tc); +} + +void KLTFreeFeatureList( + KLT_FeatureList fl) +{ + /* for affine mapping */ + int indx; + for (indx = 0 ; indx < fl->nFeatures ; indx++) { + /* free image and gradient */ + _KLTFreeFloatImage(fl->feature[indx]->aff_img); + _KLTFreeFloatImage(fl->feature[indx]->aff_img_gradx); + _KLTFreeFloatImage(fl->feature[indx]->aff_img_grady); + fl->feature[indx]->aff_img = NULL; + fl->feature[indx]->aff_img_gradx = NULL; + fl->feature[indx]->aff_img_grady = NULL; + } + + free(fl); +} + +void KLTFreeFeatureHistory( + KLT_FeatureHistory fh) +{ + free(fh); +} + +void KLTFreeFeatureTable( + KLT_FeatureTable ft) +{ + free(ft->feature[0][0]); /* this plugs a memory leak found by Stefan Wachter */ + free(ft->feature); + free(ft); +} + + +/********************************************************************* + * KLTStopSequentialMode + */ + +void KLTStopSequentialMode( + KLT_TrackingContext tc) +{ + tc->sequentialMode = FALSE; + _KLTFreePyramid((_KLT_Pyramid) tc->pyramid_last); + _KLTFreePyramid((_KLT_Pyramid) tc->pyramid_last_gradx); + _KLTFreePyramid((_KLT_Pyramid) tc->pyramid_last_grady); + tc->pyramid_last = NULL; + tc->pyramid_last_gradx = NULL; + tc->pyramid_last_grady = NULL; +} + + +/********************************************************************* + * KLTCountRemainingFeatures + */ + +int KLTCountRemainingFeatures( + KLT_FeatureList fl) +{ + int count = 0; + int i; + + for (i = 0 ; i < fl->nFeatures ; i++) + if (fl->feature[i]->val >= 0) + count++; + + return count; +} + +/********************************************************************* + * KLTSetVerbosity + */ + +void KLTSetVerbosity( + int verbosity) +{ + KLT_verbose = verbosity; +} + + + diff --git a/rtengine/klt/klt_util.cc b/rtengine/klt/klt_util.cc new file mode 100644 index 000000000..ee299453c --- /dev/null +++ b/rtengine/klt/klt_util.cc @@ -0,0 +1,165 @@ +/********************************************************************* + * klt_util.c + *********************************************************************/ + +/* Standard includes */ +#include +#include /* malloc() */ +#include /* fabs() */ + +/* Our includes */ +#include "base.h" +#include "error.h" +#include "pnmio.h" +#include "klt.h" +#include "klt_util.h" + + +/*********************************************************************/ + +float _KLTComputeSmoothSigma( + KLT_TrackingContext tc) +{ + return (tc->smooth_sigma_fact * max(tc->window_width, tc->window_height)); +} + + +/********************************************************************* + * _KLTCreateFloatImage + */ + +_KLT_FloatImage _KLTCreateFloatImage( + int ncols, + int nrows) +{ + _KLT_FloatImage floatimg; + int nbytes = sizeof(_KLT_FloatImageRec) + + ncols * nrows * sizeof(float); + + floatimg = (_KLT_FloatImage) malloc(nbytes); + if (floatimg == NULL) + KLTError("(_KLTCreateFloatImage) Out of memory"); + floatimg->ncols = ncols; + floatimg->nrows = nrows; + floatimg->data = (float *) (floatimg + 1); + + return(floatimg); +} + + +/********************************************************************* + * _KLTFreeFloatImage + */ + +void _KLTFreeFloatImage( + _KLT_FloatImage floatimg) +{ + free(floatimg); +} + + +/********************************************************************* + * _KLTPrintSubFloatImage + */ + +void _KLTPrintSubFloatImage( + _KLT_FloatImage floatimg, + int x0, int y0, + int width, int height) +{ + int ncols = floatimg->ncols; + int offset; + int i, j; + + assert(x0 >= 0); + assert(y0 >= 0); + assert(x0 + width <= ncols); + assert(y0 + height <= floatimg->nrows); + + fprintf(stderr, "\n"); + for (j = 0 ; j < height ; j++) { + for (i = 0 ; i < width ; i++) { + offset = (j+y0)*ncols + (i+x0); + fprintf(stderr, "%6.2f ", *(floatimg->data + offset)); + } + fprintf(stderr, "\n"); + } + fprintf(stderr, "\n"); +} + + +/********************************************************************* + * _KLTWriteFloatImageToPGM + */ + +void _KLTWriteFloatImageToPGM( + _KLT_FloatImage img, + char *filename) +{ + int npixs = img->ncols * img->nrows; + float mmax = -999999.9f, mmin = 999999.9f; + float fact; + float *ptr; + uchar *byteimg, *ptrout; + int i; + + /* Calculate minimum and maximum values of float image */ + ptr = img->data; + for (i = 0 ; i < npixs ; i++) { + mmax = max(mmax, *ptr); + mmin = min(mmin, *ptr); + ptr++; + } + + /* Allocate memory to hold converted image */ + byteimg = (uchar *) malloc(npixs * sizeof(uchar)); + + /* Convert image from float to uchar */ + fact = 255.0f / (mmax-mmin); + ptr = img->data; + ptrout = byteimg; + for (i = 0 ; i < npixs ; i++) { + *ptrout++ = (uchar) ((*ptr++ - mmin) * fact); + } + + /* Write uchar image to PGM */ + pgmWriteFile(filename, byteimg, img->ncols, img->nrows); + + /* Free memory */ + free(byteimg); +} + +/********************************************************************* + * _KLTWriteFloatImageToPGM + */ + +void _KLTWriteAbsFloatImageToPGM( + _KLT_FloatImage img, + char *filename,float scale) +{ + int npixs = img->ncols * img->nrows; + float fact; + float *ptr; + uchar *byteimg, *ptrout; + int i; + float tmp; + + /* Allocate memory to hold converted image */ + byteimg = (uchar *) malloc(npixs * sizeof(uchar)); + + /* Convert image from float to uchar */ + fact = 255.0f / scale; + ptr = img->data; + ptrout = byteimg; + for (i = 0 ; i < npixs ; i++) { + tmp = (float) (fabs(*ptr++) * fact); + if(tmp > 255.0) tmp = 255.0; + *ptrout++ = (uchar) tmp; + } + + /* Write uchar image to PGM */ + pgmWriteFile(filename, byteimg, img->ncols, img->nrows); + + /* Free memory */ + free(byteimg); +} diff --git a/rtengine/klt/pnmio.cc b/rtengine/klt/pnmio.cc new file mode 100644 index 000000000..e1c334bd9 --- /dev/null +++ b/rtengine/klt/pnmio.cc @@ -0,0 +1,333 @@ +/********************************************************************* + * pnmio.c + * + * Various routines to manipulate PNM files. + *********************************************************************/ + + +/* Standard includes */ +#include /* FILE */ +#include /* malloc(), atoi() */ + +/* Our includes */ +#include "error.h" + +#define LENGTH 80 + + +/*********************************************************************/ + +static void _getNextString( + FILE *fp, + char *line) +{ + int i; + + line[0] = '\0'; + + while (line[0] == '\0') { + fscanf(fp, "%s", line); + i = -1; + do { + i++; + if (line[i] == '#') { + line[i] = '\0'; + while (fgetc(fp) != '\n') ; + } + } while (line[i] != '\0'); + } +} + + +/********************************************************************* + * pnmReadHeader + */ + +void pnmReadHeader( + FILE *fp, + int *magic, + int *ncols, int *nrows, + int *maxval) +{ + char line[LENGTH]; + + /* Read magic number */ + _getNextString(fp, line); + if (line[0] != 'P') + KLTError("(pnmReadHeader) Magic number does not begin with 'P', " + "but with a '%c'", line[0]); + sscanf(line, "P%d", magic); + + /* Read size, skipping comments */ + _getNextString(fp, line); + *ncols = atoi(line); + _getNextString(fp, line); + *nrows = atoi(line); + if (*ncols < 0 || *nrows < 0 || *ncols > 10000 || *nrows > 10000) + KLTError("(pnmReadHeader) The dimensions %d x %d are unacceptable", + *ncols, *nrows); + + /* Read maxval, skipping comments */ + _getNextString(fp, line); + *maxval = atoi(line); + fread(line, 1, 1, fp); /* Read newline which follows maxval */ + + if (*maxval != 255) + KLTWarning("(pnmReadHeader) Maxval is not 255, but %d", *maxval); +} + + +/********************************************************************* + * pgmReadHeader + */ + +void pgmReadHeader( + FILE *fp, + int *magic, + int *ncols, int *nrows, + int *maxval) +{ + pnmReadHeader(fp, magic, ncols, nrows, maxval); + if (*magic != 5) + KLTError("(pgmReadHeader) Magic number is not 'P5', but 'P%d'", *magic); +} + + +/********************************************************************* + * ppmReadHeader + */ + +void ppmReadHeader( + FILE *fp, + int *magic, + int *ncols, int *nrows, + int *maxval) +{ + pnmReadHeader(fp, magic, ncols, nrows, maxval); + if (*magic != 6) + KLTError("(ppmReadHeader) Magic number is not 'P6', but 'P%d'", *magic); +} + + +/********************************************************************* + * pgmReadHeaderFile + */ + +void pgmReadHeaderFile( + char *fname, + int *magic, + int *ncols, int *nrows, + int *maxval) +{ + FILE *fp; + + /* Open file */ + if ( (fp = fopen(fname, "rb")) == NULL) + KLTError("(pgmReadHeaderFile) Can't open file named '%s' for reading\n", fname); + + /* Read header */ + pgmReadHeader(fp, magic, ncols, nrows, maxval); + + /* Close file */ + fclose(fp); +} + + +/********************************************************************* + * ppmReadHeaderFile + */ + +void ppmReadHeaderFile( + char *fname, + int *magic, + int *ncols, int *nrows, + int *maxval) +{ + FILE *fp; + + /* Open file */ + if ( (fp = fopen(fname, "rb")) == NULL) + KLTError("(ppmReadHeaderFile) Can't open file named '%s' for reading\n", fname); + + /* Read header */ + ppmReadHeader(fp, magic, ncols, nrows, maxval); + + /* Close file */ + fclose(fp); +} + + +/********************************************************************* + * pgmRead + * + * NOTE: If img is NULL, memory is allocated. + */ + +unsigned char* pgmRead( + FILE *fp, + unsigned char *img, + int *ncols, int *nrows) +{ + unsigned char *ptr; + int magic, maxval; + int i; + + /* Read header */ + pgmReadHeader(fp, &magic, ncols, nrows, &maxval); + + /* Allocate memory, if necessary, and set pointer */ + if (img == NULL) { + ptr = (unsigned char *) malloc(*ncols * *nrows * sizeof(char)); + if (ptr == NULL) + KLTError("(pgmRead) Memory not allocated"); + } + else + ptr = img; + + /* Read binary image data */ + { + unsigned char *tmpptr = ptr; + for (i = 0 ; i < *nrows ; i++) { + fread(tmpptr, *ncols, 1, fp); + tmpptr += *ncols; + } + } + + return ptr; +} + + +/********************************************************************* + * pgmReadFile + * + * NOTE: If img is NULL, memory is allocated. + */ + +unsigned char* pgmReadFile( + char *fname, + unsigned char *img, + int *ncols, int *nrows) +{ + unsigned char *ptr; + FILE *fp; + + /* Open file */ + if ( (fp = fopen(fname, "rb")) == NULL) + KLTError("(pgmReadFile) Can't open file named '%s' for reading\n", fname); + + /* Read file */ + ptr = pgmRead(fp, img, ncols, nrows); + + /* Close file */ + fclose(fp); + + return ptr; +} + + +/********************************************************************* + * pgmWrite + */ + +void pgmWrite( + FILE *fp, + unsigned char *img, + int ncols, + int nrows) +{ + int i; + + /* Write header */ + fprintf(fp, "P5\n"); + fprintf(fp, "%d %d\n", ncols, nrows); + fprintf(fp, "255\n"); + + /* Write binary data */ + for (i = 0 ; i < nrows ; i++) { + fwrite(img, ncols, 1, fp); + img += ncols; + } +} + + +/********************************************************************* + * pgmWriteFile + */ + +void pgmWriteFile( + char *fname, + unsigned char *img, + int ncols, + int nrows) +{ + FILE *fp; + + /* Open file */ + if ( (fp = fopen(fname, "wb")) == NULL) + KLTError("(pgmWriteFile) Can't open file named '%s' for writing\n", fname); + + /* Write to file */ + pgmWrite(fp, img, ncols, nrows); + + /* Close file */ + fclose(fp); +} + + +/********************************************************************* + * ppmWrite + */ + +void ppmWrite( + FILE *fp, + unsigned char *redimg, + unsigned char *greenimg, + unsigned char *blueimg, + int ncols, + int nrows) +{ + int i, j; + + /* Write header */ + fprintf(fp, "P6\n"); + fprintf(fp, "%d %d\n", ncols, nrows); + fprintf(fp, "255\n"); + + /* Write binary data */ + for (j = 0 ; j < nrows ; j++) { + for (i = 0 ; i < ncols ; i++) { + fwrite(redimg, 1, 1, fp); + fwrite(greenimg, 1, 1, fp); + fwrite(blueimg, 1, 1, fp); + redimg++; greenimg++; blueimg++; + } + } +} + + +/********************************************************************* + * ppmWriteFileRGB + */ + +void ppmWriteFileRGB( + char *fname, + unsigned char *redimg, + unsigned char *greenimg, + unsigned char *blueimg, + int ncols, + int nrows) +{ + FILE *fp; + + /* Open file */ + if ( (fp = fopen(fname, "wb")) == NULL) + KLTError("(ppmWriteFileRGB) Can't open file named '%s' for writing\n", fname); + + /* Write to file */ + ppmWrite(fp, redimg, greenimg, blueimg, ncols, nrows); + + /* Close file */ + fclose(fp); +} + + diff --git a/rtengine/klt/pyramid.cc b/rtengine/klt/pyramid.cc new file mode 100644 index 000000000..fc3809854 --- /dev/null +++ b/rtengine/klt/pyramid.cc @@ -0,0 +1,143 @@ +/********************************************************************* + * pyramid.c + * + *********************************************************************/ + +/* Standard includes */ +#include +#include /* malloc() ? */ +#include /* memset() ? */ +#include /* */ + +/* Our includes */ +#include "base.h" +#include "error.h" +#include "convolve.h" /* for computing pyramid */ +#include "pyramid.h" + + +/********************************************************************* + * + */ + +_KLT_Pyramid _KLTCreatePyramid( + int ncols, + int nrows, + int subsampling, + int nlevels) +{ + _KLT_Pyramid pyramid; + int nbytes = sizeof(_KLT_PyramidRec) + + nlevels * sizeof(_KLT_FloatImage *) + + nlevels * sizeof(int) + + nlevels * sizeof(int); + int i; + + if (subsampling != 2 && subsampling != 4 && + subsampling != 8 && subsampling != 16 && subsampling != 32) + KLTError("(_KLTCreatePyramid) Pyramid's subsampling must " + "be either 2, 4, 8, 16, or 32"); + + + /* Allocate memory for structure and set parameters */ + pyramid = (_KLT_Pyramid) malloc(nbytes); + if (pyramid == NULL) + KLTError("(_KLTCreatePyramid) Out of memory"); + + /* Set parameters */ + pyramid->subsampling = subsampling; + pyramid->nLevels = nlevels; + pyramid->img = (_KLT_FloatImage *) (pyramid + 1); + pyramid->ncols = (int *) (pyramid->img + nlevels); + pyramid->nrows = (int *) (pyramid->ncols + nlevels); + + /* Allocate memory for each level of pyramid and assign pointers */ + for (i = 0 ; i < nlevels ; i++) { + pyramid->img[i] = _KLTCreateFloatImage(ncols, nrows); + pyramid->ncols[i] = ncols; pyramid->nrows[i] = nrows; + ncols /= subsampling; nrows /= subsampling; + } + + return pyramid; +} + + +/********************************************************************* + * + */ + +void _KLTFreePyramid( + _KLT_Pyramid pyramid) +{ + int i; + + /* Free images */ + for (i = 0 ; i < pyramid->nLevels ; i++) + _KLTFreeFloatImage(pyramid->img[i]); + + /* Free structure */ + free(pyramid); +} + + +/********************************************************************* + * + */ + +void _KLTComputePyramid( + _KLT_FloatImage img, + _KLT_Pyramid pyramid, + float sigma_fact) +{ + _KLT_FloatImage currimg, tmpimg; + int ncols = img->ncols, nrows = img->nrows; + int subsampling = pyramid->subsampling; + int subhalf = subsampling / 2; + float sigma = subsampling * sigma_fact; /* empirically determined */ + int oldncols; + int i, x, y; + + if (subsampling != 2 && subsampling != 4 && + subsampling != 8 && subsampling != 16 && subsampling != 32) + KLTError("(_KLTComputePyramid) Pyramid's subsampling must " + "be either 2, 4, 8, 16, or 32"); + + assert(pyramid->ncols[0] == img->ncols); + assert(pyramid->nrows[0] == img->nrows); + + /* Copy original image to level 0 of pyramid */ + memcpy(pyramid->img[0]->data, img->data, ncols*nrows*sizeof(float)); + + currimg = img; + for (i = 1 ; i < pyramid->nLevels ; i++) { + tmpimg = _KLTCreateFloatImage(ncols, nrows); + _KLTComputeSmoothedImage(currimg, sigma, tmpimg); + + + /* Subsample */ + oldncols = ncols; + ncols /= subsampling; nrows /= subsampling; + for (y = 0 ; y < nrows ; y++) + for (x = 0 ; x < ncols ; x++) + pyramid->img[i]->data[y*ncols+x] = + tmpimg->data[(subsampling*y+subhalf)*oldncols + + (subsampling*x+subhalf)]; + + /* Reassign current image */ + currimg = pyramid->img[i]; + + _KLTFreeFloatImage(tmpimg); + } +} + + + + + + + + + + + + diff --git a/rtengine/klt/selectGoodFeatures.cc b/rtengine/klt/selectGoodFeatures.cc new file mode 100644 index 000000000..5dacf72ea --- /dev/null +++ b/rtengine/klt/selectGoodFeatures.cc @@ -0,0 +1,543 @@ +/********************************************************************* + * selectGoodFeatures.c + * + *********************************************************************/ + +/* Standard includes */ +#include +#include /* malloc(), qsort() */ +#include /* fflush() */ +#include /* memset() */ +#include /* fsqrt() */ +#define fsqrt(X) sqrt(X) + +/* Our includes */ +#include "base.h" +#include "error.h" +#include "convolve.h" +#include "klt.h" +#include "klt_util.h" +#include "pyramid.h" + +int KLT_verbose = 1; + +typedef enum {SELECTING_ALL, REPLACING_SOME} selectionMode; + + +/********************************************************************* + * _quicksort + * Replacement for qsort(). Computing time is decreased by taking + * advantage of specific knowledge of our array (that there are + * three ints associated with each point). + * + * This routine generously provided by + * Manolis Lourakis + * + * NOTE: The results of this function may be slightly different from + * those of qsort(). This is due to the fact that different sort + * algorithms have different behaviours when sorting numbers with the + * same value: Some leave them in the same relative positions in the + * array, while others change their relative positions. For example, + * if you have the array [c d b1 a b2] with b1=b2, it may be sorted as + * [a b1 b2 c d] or [a b2 b1 c d]. + */ + +#define SWAP3(list, i, j) \ +{ 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 */ + { + float gx, gy; + float gxx, gxy, gyy; + int xx, yy; + int *ptr; + float val; + unsigned int limit = 1; + int borderx = tc->borderx; /* Must not touch cols */ + int bordery = tc->bordery; /* lost by convolution */ + int x, y; + int i; + + if (borderx < window_hw) borderx = window_hw; + if (bordery < window_hh) bordery = window_hh; + + /* Find largest value of an int */ + for (i = 0 ; i < sizeof(int) ; i++) limit *= 256; + limit = limit/2 - 1; + + /* For most of the pixels in the image, do ... */ + ptr = pointlist; + for (y = bordery ; y < nrows - bordery ; y += tc->nSkippedPixels + 1) + for (x = borderx ; x < ncols - borderx ; x += tc->nSkippedPixels + 1) { + + /* Sum the gradients in the surrounding window */ + gxx = 0; gxy = 0; gyy = 0; + for (yy = y-window_hh ; yy <= y+window_hh ; yy++) + for (xx = x-window_hw ; xx <= x+window_hw ; xx++) { + gx = *(gradx->data + ncols*yy+xx); + gy = *(grady->data + ncols*yy+xx); + gxx += gx * gx; + gxy += gx * gy; + gyy += gy * gy; + } + + /* Store the trackability of the pixel as the minimum + of the two eigenvalues */ + *ptr++ = x; + *ptr++ = y; + val = _minEigenvalue(gxx, gxy, gyy); + if (val > limit) { + KLTWarning("(_KLTSelectGoodFeatures) minimum eigenvalue %f is " + "greater than the capacity of an int; setting " + "to maximum value", val); + val = (float) limit; + } + *ptr++ = (int) val; + npoints++; + } + } + + /* Sort the features */ + _sortPointList(pointlist, npoints); + + /* Check tc->mindist */ + if (tc->mindist < 0) { + KLTWarning("(_KLTSelectGoodFeatures) Tracking context field tc->mindist " + "is negative (%d); setting to zero", tc->mindist); + tc->mindist = 0; + } + + /* Enforce minimum distance between features */ + _enforceMinimumDistance( + pointlist, + npoints, + featurelist, + ncols, nrows, + tc->mindist, + tc->min_eigenvalue, + overwriteAllFeatures); + + /* Free memory */ + free(pointlist); + if (floatimages_created) { + _KLTFreeFloatImage(floatimg); + _KLTFreeFloatImage(gradx); + _KLTFreeFloatImage(grady); + } +} + + +/********************************************************************* + * KLTSelectGoodFeatures + * + * Main routine, visible to the outside. Finds the good features in + * an image. + * + * INPUTS + * tc: Contains parameters used in computation (size of image, + * size of window, min distance b/w features, sigma to compute + * image gradients, # of features desired). + * img: Pointer to the data of an image (probably unsigned chars). + * + * OUTPUTS + * features: List of features. The member nFeatures is computed. + */ + +void KLTSelectGoodFeatures( + KLT_TrackingContext tc, + KLT_PixelType *img, + int ncols, + int nrows, + KLT_FeatureList fl) +{ + if (KLT_verbose >= 1) { + fprintf(stderr, "(KLT) Selecting the %d best features " + "from a %d by %d image... ", fl->nFeatures, ncols, nrows); + fflush(stderr); + } + + _KLTSelectGoodFeatures(tc, img, ncols, nrows, + fl, SELECTING_ALL); + + if (KLT_verbose >= 1) { + fprintf(stderr, "\n\t%d features found.\n", + KLTCountRemainingFeatures(fl)); + if (tc->writeInternalImages) + fprintf(stderr, "\tWrote images to 'kltimg_sgfrlf*.pgm'.\n"); + fflush(stderr); + } +} + + +/********************************************************************* + * KLTReplaceLostFeatures + * + * Main routine, visible to the outside. Replaces the lost features + * in an image. + * + * INPUTS + * tc: Contains parameters used in computation (size of image, + * size of window, min distance b/w features, sigma to compute + * image gradients, # of features desired). + * img: Pointer to the data of an image (probably unsigned chars). + * + * OUTPUTS + * features: List of features. The member nFeatures is computed. + */ + +void KLTReplaceLostFeatures( + KLT_TrackingContext tc, + KLT_PixelType *img, + int ncols, + int nrows, + KLT_FeatureList fl) +{ + int nLostFeatures = fl->nFeatures - KLTCountRemainingFeatures(fl); + + if (KLT_verbose >= 1) { + fprintf(stderr, "(KLT) Attempting to replace %d features " + "in a %d by %d image... ", nLostFeatures, ncols, nrows); + fflush(stderr); + } + + /* If there are any lost features, replace them */ + if (nLostFeatures > 0) + _KLTSelectGoodFeatures(tc, img, ncols, nrows, + fl, REPLACING_SOME); + + if (KLT_verbose >= 1) { + fprintf(stderr, "\n\t%d features replaced.\n", + nLostFeatures - fl->nFeatures + KLTCountRemainingFeatures(fl)); + if (tc->writeInternalImages) + fprintf(stderr, "\tWrote images to 'kltimg_sgfrlf*.pgm'.\n"); + fflush(stderr); + } +} + + diff --git a/rtengine/klt/storeFeatures.cc b/rtengine/klt/storeFeatures.cc new file mode 100644 index 000000000..467905b50 --- /dev/null +++ b/rtengine/klt/storeFeatures.cc @@ -0,0 +1,117 @@ +/********************************************************************* + * storeFeatures.c + * + *********************************************************************/ + +/* Our includes */ +#include "error.h" +#include "klt.h" + + +/********************************************************************* + * + */ + +void KLTStoreFeatureList( + KLT_FeatureList fl, + KLT_FeatureTable ft, + int frame) +{ + int feat; + + if (frame < 0 || frame >= ft->nFrames) + KLTError("(KLTStoreFeatures) Frame number %d is not between 0 and %d", + frame, ft->nFrames - 1); + + if (fl->nFeatures != ft->nFeatures) + KLTError("(KLTStoreFeatures) FeatureList and FeatureTable must " + "have the same number of features"); + + for (feat = 0 ; feat < fl->nFeatures ; feat++) { + ft->feature[feat][frame]->x = fl->feature[feat]->x; + ft->feature[feat][frame]->y = fl->feature[feat]->y; + ft->feature[feat][frame]->val = fl->feature[feat]->val; + } +} + + +/********************************************************************* + * + */ + +void KLTExtractFeatureList( + KLT_FeatureList fl, + KLT_FeatureTable ft, + int frame) +{ + int feat; + + if (frame < 0 || frame >= ft->nFrames) + KLTError("(KLTExtractFeatures) Frame number %d is not between 0 and %d", + frame, ft->nFrames - 1); + + if (fl->nFeatures != ft->nFeatures) + KLTError("(KLTExtractFeatures) FeatureList and FeatureTable must " + "have the same number of features"); + + for (feat = 0 ; feat < fl->nFeatures ; feat++) { + fl->feature[feat]->x = ft->feature[feat][frame]->x; + fl->feature[feat]->y = ft->feature[feat][frame]->y; + fl->feature[feat]->val = ft->feature[feat][frame]->val; + } +} + + +/********************************************************************* + * + */ + +void KLTStoreFeatureHistory( + KLT_FeatureHistory fh, + KLT_FeatureTable ft, + int feat) +{ + int frame; + + if (feat < 0 || feat >= ft->nFeatures) + KLTError("(KLTStoreFeatureHistory) Feature number %d is not between 0 and %d", + feat, ft->nFeatures - 1); + + if (fh->nFrames != ft->nFrames) + KLTError("(KLTStoreFeatureHistory) FeatureHistory and FeatureTable must " + "have the same number of frames"); + + for (frame = 0 ; frame < fh->nFrames ; frame++) { + ft->feature[feat][frame]->x = fh->feature[frame]->x; + ft->feature[feat][frame]->y = fh->feature[frame]->y; + ft->feature[feat][frame]->val = fh->feature[frame]->val; + } +} + + +/********************************************************************* + * + */ + +void KLTExtractFeatureHistory( + KLT_FeatureHistory fh, + KLT_FeatureTable ft, + int feat) +{ + int frame; + + if (feat < 0 || feat >= ft->nFeatures) + KLTError("(KLTExtractFeatureHistory) Feature number %d is not between 0 and %d", + feat, ft->nFeatures - 1); + + if (fh->nFrames != ft->nFrames) + KLTError("(KLTExtractFeatureHistory) FeatureHistory and FeatureTable must " + "have the same number of frames"); + + for (frame = 0 ; frame < fh->nFrames ; frame++) { + fh->feature[frame]->x = ft->feature[feat][frame]->x; + fh->feature[frame]->y = ft->feature[feat][frame]->y; + fh->feature[frame]->val = ft->feature[feat][frame]->val; + } +} + diff --git a/rtengine/klt/trackFeatures.cc b/rtengine/klt/trackFeatures.cc new file mode 100644 index 000000000..90352129c --- /dev/null +++ b/rtengine/klt/trackFeatures.cc @@ -0,0 +1,1533 @@ +/********************************************************************* + * trackFeatures.c + * + *********************************************************************/ + +/* Standard includes */ +#include +#include /* fabs() */ +#include /* malloc() */ +#include /* fflush() */ + +/* Our includes */ +#include "base.h" +#include "error.h" +#include "convolve.h" /* for computing pyramid */ +#include "klt.h" +#include "klt_util.h" /* _KLT_FloatImage */ +#include "pyramid.h" /* _KLT_Pyramid */ + +extern int KLT_verbose; + +typedef float *_FloatWindow; + +/********************************************************************* + * _interpolate + * + * Given a point (x,y) in an image, computes the bilinear interpolated + * gray-level value of the point in the image. + */ + +static float _interpolate( + float x, + float y, + _KLT_FloatImage img) +{ + int xt = (int) x; /* coordinates of top-left corner */ + int yt = (int) y; + float ax = x - xt; + float ay = y - yt; + float *ptr = img->data + (img->ncols*yt) + xt; + +#ifndef _DNDEBUG + if (xt<0 || yt<0 || xt>=img->ncols-1 || yt>=img->nrows-1) { + fprintf(stderr, "(xt,yt)=(%d,%d) imgsize=(%d,%d)\n" + "(x,y)=(%f,%f) (ax,ay)=(%f,%f)\n", + xt, yt, img->ncols, img->nrows, x, y, ax, ay); + fflush(stderr); + } +#endif + + assert (xt >= 0 && yt >= 0 && xt <= img->ncols - 2 && yt <= img->nrows - 2); + + return ( (1-ax) * (1-ay) * *ptr + + ax * (1-ay) * *(ptr+1) + + (1-ax) * ay * *(ptr+(img->ncols)) + + ax * ay * *(ptr+(img->ncols)+1) ); +} + + +/********************************************************************* + * _computeIntensityDifference + * + * Given two images and the window center in both images, + * aligns the images wrt the window and computes the difference + * between the two overlaid images. + */ + +static void _computeIntensityDifference( + _KLT_FloatImage img1, /* images */ + _KLT_FloatImage img2, + float x1, float y1, /* center of window in 1st img */ + float x2, float y2, /* center of window in 2nd img */ + int width, int height, /* size of window */ + _FloatWindow imgdiff) /* output */ +{ + int hw = width/2, hh = height/2; + float g1, g2; + int i, j; + + /* Compute values */ + for (j = -hh ; j <= hh ; j++) + for (i = -hw ; i <= hw ; i++) { + g1 = _interpolate(x1+i, y1+j, img1); + g2 = _interpolate(x2+i, y2+j, img2); + *imgdiff++ = g1 - g2; + } +} + + +/********************************************************************* + * _computeGradientSum + * + * Given two gradients and the window center in both images, + * aligns the gradients wrt the window and computes the sum of the two + * overlaid gradients. + */ + +static void _computeGradientSum( + _KLT_FloatImage gradx1, /* gradient images */ + _KLT_FloatImage grady1, + _KLT_FloatImage gradx2, + _KLT_FloatImage grady2, + float x1, float y1, /* center of window in 1st img */ + float x2, float y2, /* center of window in 2nd img */ + int width, int height, /* size of window */ + _FloatWindow gradx, /* output */ + _FloatWindow grady) /* " */ +{ + int hw = width/2, hh = height/2; + float g1, g2; + int i, j; + + /* Compute values */ + for (j = -hh ; j <= hh ; j++) + for (i = -hw ; i <= hw ; i++) { + g1 = _interpolate(x1+i, y1+j, gradx1); + g2 = _interpolate(x2+i, y2+j, gradx2); + *gradx++ = g1 + g2; + g1 = _interpolate(x1+i, y1+j, grady1); + g2 = _interpolate(x2+i, y2+j, grady2); + *grady++ = g1 + g2; + } +} + +/********************************************************************* + * _computeIntensityDifferenceLightingInsensitive + * + * Given two images and the window center in both images, + * aligns the images wrt the window and computes the difference + * between the two overlaid images; normalizes for overall gain and bias. + */ + +static void _computeIntensityDifferenceLightingInsensitive( + _KLT_FloatImage img1, /* images */ + _KLT_FloatImage img2, + float x1, float y1, /* center of window in 1st img */ + float x2, float y2, /* center of window in 2nd img */ + int width, int height, /* size of window */ + _FloatWindow imgdiff) /* output */ +{ + int hw = width/2, hh = height/2; + float g1, g2, sum1_squared = 0, sum2_squared = 0; + int i, j; + + float sum1 = 0, sum2 = 0; + float mean1, mean2,alpha,belta; + /* Compute values */ + for (j = -hh ; j <= hh ; j++) + for (i = -hw ; i <= hw ; i++) { + g1 = _interpolate(x1+i, y1+j, img1); + g2 = _interpolate(x2+i, y2+j, img2); + sum1 += g1; sum2 += g2; + sum1_squared += g1*g1; + sum2_squared += g2*g2; + } + mean1=sum1_squared/(width*height); + mean2=sum2_squared/(width*height); + alpha = (float) sqrt(mean1/mean2); + mean1=sum1/(width*height); + mean2=sum2/(width*height); + belta = mean1-alpha*mean2; + + for (j = -hh ; j <= hh ; j++) + for (i = -hw ; i <= hw ; i++) { + g1 = _interpolate(x1+i, y1+j, img1); + g2 = _interpolate(x2+i, y2+j, img2); + *imgdiff++ = g1- g2*alpha-belta; + } +} + + +/********************************************************************* + * _computeGradientSumLightingInsensitive + * + * Given two gradients and the window center in both images, + * aligns the gradients wrt the window and computes the sum of the two + * overlaid gradients; normalizes for overall gain and bias. + */ + +static void _computeGradientSumLightingInsensitive( + _KLT_FloatImage gradx1, /* gradient images */ + _KLT_FloatImage grady1, + _KLT_FloatImage gradx2, + _KLT_FloatImage grady2, + _KLT_FloatImage img1, /* images */ + _KLT_FloatImage img2, + + float x1, float y1, /* center of window in 1st img */ + float x2, float y2, /* center of window in 2nd img */ + int width, int height, /* size of window */ + _FloatWindow gradx, /* output */ + _FloatWindow grady) /* " */ +{ + int hw = width/2, hh = height/2; + float g1, g2, sum1_squared = 0, sum2_squared = 0; + int i, j; + + float sum1 = 0, sum2 = 0; + float mean1, mean2, alpha; + for (j = -hh ; j <= hh ; j++) + for (i = -hw ; i <= hw ; i++) { + g1 = _interpolate(x1+i, y1+j, img1); + g2 = _interpolate(x2+i, y2+j, img2); + sum1_squared += g1; sum2_squared += g2; + } + mean1 = sum1_squared/(width*height); + mean2 = sum2_squared/(width*height); + alpha = (float) sqrt(mean1/mean2); + + /* Compute values */ + for (j = -hh ; j <= hh ; j++) + for (i = -hw ; i <= hw ; i++) { + g1 = _interpolate(x1+i, y1+j, gradx1); + g2 = _interpolate(x2+i, y2+j, gradx2); + *gradx++ = g1 + g2*alpha; + g1 = _interpolate(x1+i, y1+j, grady1); + g2 = _interpolate(x2+i, y2+j, grady2); + *grady++ = g1+ g2*alpha; + } +} + +/********************************************************************* + * _compute2by2GradientMatrix + * + */ + +static void _compute2by2GradientMatrix( + _FloatWindow gradx, + _FloatWindow grady, + int width, /* size of window */ + int height, + float *gxx, /* return values */ + float *gxy, + float *gyy) + +{ + float gx, gy; + int i; + + /* Compute values */ + *gxx = 0.0; *gxy = 0.0; *gyy = 0.0; + for (i = 0 ; i < width * height ; i++) { + gx = *gradx++; + gy = *grady++; + *gxx += gx*gx; + *gxy += gx*gy; + *gyy += gy*gy; + } +} + + +/********************************************************************* + * _compute2by1ErrorVector + * + */ + +static void _compute2by1ErrorVector( + _FloatWindow imgdiff, + _FloatWindow gradx, + _FloatWindow grady, + int width, /* size of window */ + int height, + float step_factor, /* 2.0 comes from equations, 1.0 seems to avoid overshooting */ + float *ex, /* return values */ + float *ey) +{ + float diff; + int i; + + /* Compute values */ + *ex = 0; *ey = 0; + for (i = 0 ; i < width * height ; i++) { + diff = *imgdiff++; + *ex += diff * (*gradx++); + *ey += diff * (*grady++); + } + *ex *= step_factor; + *ey *= step_factor; +} + + +/********************************************************************* + * _solveEquation + * + * Solves the 2x2 matrix equation + * [gxx gxy] [dx] = [ex] + * [gxy gyy] [dy] = [ey] + * for dx and dy. + * + * Returns KLT_TRACKED on success and KLT_SMALL_DET on failure + */ + +static int _solveEquation( + float gxx, float gxy, float gyy, + float ex, float ey, + float small, + float *dx, float *dy) +{ + float det = gxx*gyy - gxy*gxy; + + + if (det < small) return KLT_SMALL_DET; + + *dx = (gyy*ex - gxy*ey)/det; + *dy = (gxx*ey - gxy*ex)/det; + return KLT_TRACKED; +} + + +/********************************************************************* + * _allocateFloatWindow + */ + +static _FloatWindow _allocateFloatWindow( + int width, + int height) +{ + _FloatWindow fw; + + fw = (_FloatWindow) malloc(width*height*sizeof(float)); + if (fw == NULL) KLTError("(_allocateFloatWindow) Out of memory."); + return fw; +} + + +/********************************************************************* + * _printFloatWindow + * (for debugging purposes) + */ + +/* +static void _printFloatWindow( + _FloatWindow fw, + int width, + int height) +{ + int i, j; + + fprintf(stderr, "\n"); + for (i = 0 ; i < width ; i++) { + for (j = 0 ; j < height ; j++) { + fprintf(stderr, "%6.1f ", *fw++); + } + fprintf(stderr, "\n"); + } +} +*/ + + +/********************************************************************* + * _sumAbsFloatWindow + */ + +static float _sumAbsFloatWindow( + _FloatWindow fw, + int width, + int height) +{ + float sum = 0.0; + int w; + + for ( ; height > 0 ; height--) + for (w=0 ; w < width ; w++) + sum += (float) fabs(*fw++); + + return sum; +} + + +/********************************************************************* + * _trackFeature + * + * Tracks a feature point from one image to the next. + * + * RETURNS + * KLT_SMALL_DET if feature is lost, + * KLT_MAX_ITERATIONS if tracking stopped because iterations timed out, + * KLT_TRACKED otherwise. + */ + +static int _trackFeature( + float x1, /* location of window in first image */ + float y1, + float *x2, /* starting location of search in second image */ + float *y2, + _KLT_FloatImage img1, + _KLT_FloatImage gradx1, + _KLT_FloatImage grady1, + _KLT_FloatImage img2, + _KLT_FloatImage gradx2, + _KLT_FloatImage grady2, + int width, /* size of window */ + int height, + float step_factor, /* 2.0 comes from equations, 1.0 seems to avoid overshooting */ + int max_iterations, + float small, /* determinant threshold for declaring KLT_SMALL_DET */ + float th, /* displacement threshold for stopping */ + float max_residue, /* residue threshold for declaring KLT_LARGE_RESIDUE */ + int lighting_insensitive) /* whether to normalize for gain and bias */ +{ + _FloatWindow imgdiff, gradx, grady; + float gxx, gxy, gyy, ex, ey, dx, dy; + int iteration = 0; + int status; + int hw = width/2; + int hh = height/2; + int nc = img1->ncols; + int nr = img1->nrows; + float one_plus_eps = 1.001f; /* To prevent rounding errors */ + + + /* Allocate memory for windows */ + imgdiff = _allocateFloatWindow(width, height); + gradx = _allocateFloatWindow(width, height); + grady = _allocateFloatWindow(width, height); + + /* Iteratively update the window position */ + do { + + /* If out of bounds, exit loop */ + if ( x1-hw < 0.0f || nc-( x1+hw) < one_plus_eps || + *x2-hw < 0.0f || nc-(*x2+hw) < one_plus_eps || + y1-hh < 0.0f || nr-( y1+hh) < one_plus_eps || + *y2-hh < 0.0f || nr-(*y2+hh) < one_plus_eps) { + status = KLT_OOB; + break; + } + + /* Compute gradient and difference windows */ + if (lighting_insensitive) { + _computeIntensityDifferenceLightingInsensitive(img1, img2, x1, y1, *x2, *y2, + width, height, imgdiff); + _computeGradientSumLightingInsensitive(gradx1, grady1, gradx2, grady2, + img1, img2, x1, y1, *x2, *y2, width, height, gradx, grady); + } else { + _computeIntensityDifference(img1, img2, x1, y1, *x2, *y2, + width, height, imgdiff); + _computeGradientSum(gradx1, grady1, gradx2, grady2, + x1, y1, *x2, *y2, width, height, gradx, grady); + } + + + /* Use these windows to construct matrices */ + _compute2by2GradientMatrix(gradx, grady, width, height, + &gxx, &gxy, &gyy); + _compute2by1ErrorVector(imgdiff, gradx, grady, width, height, step_factor, + &ex, &ey); + + /* Using matrices, solve equation for new displacement */ + status = _solveEquation(gxx, gxy, gyy, ex, ey, small, &dx, &dy); + if (status == KLT_SMALL_DET) break; + + *x2 += dx; + *y2 += dy; + iteration++; + + } while ((fabs(dx)>=th || fabs(dy)>=th) && iteration < max_iterations); + + /* Check whether window is out of bounds */ + if (*x2-hw < 0.0f || nc-(*x2+hw) < one_plus_eps || + *y2-hh < 0.0f || nr-(*y2+hh) < one_plus_eps) + status = KLT_OOB; + + /* Check whether residue is too large */ + if (status == KLT_TRACKED) { + if (lighting_insensitive) + _computeIntensityDifferenceLightingInsensitive(img1, img2, x1, y1, *x2, *y2, + width, height, imgdiff); + else + _computeIntensityDifference(img1, img2, x1, y1, *x2, *y2, + width, height, imgdiff); + if (_sumAbsFloatWindow(imgdiff, width, height)/(width*height) > max_residue) + status = KLT_LARGE_RESIDUE; + } + + /* Free memory */ + free(imgdiff); free(gradx); free(grady); + + /* Return appropriate value */ + if (status == KLT_SMALL_DET) return KLT_SMALL_DET; + else if (status == KLT_OOB) return KLT_OOB; + else if (status == KLT_LARGE_RESIDUE) return KLT_LARGE_RESIDUE; + else if (iteration >= max_iterations) return KLT_MAX_ITERATIONS; + else return KLT_TRACKED; + +} + + +/*********************************************************************/ + +static KLT_BOOL _outOfBounds( + float x, + float y, + int ncols, + int nrows, + int borderx, + int bordery) +{ + return (x < borderx || x > ncols-1-borderx || + y < bordery || y > nrows-1-bordery ); +} + + + + +/********************************************************************** +* CONSISTENCY CHECK OF FEATURES BY AFFINE MAPPING (BEGIN) +* +* Created by: Thorsten Thormaehlen (University of Hannover) June 2004 +* thormae@tnt.uni-hannover.de +* +* Permission is granted to any individual or institution to use, copy, modify, +* and distribute this part of the software, provided that this complete authorship +* and permission notice is maintained, intact, in all copies. +* +* This software is provided "as is" without express or implied warranty. +* +* +* The following static functions are helpers for the affine mapping. +* They all start with "_am". +* There are also small changes in other files for the +* affine mapping these are all marked by "for affine mapping" +* +* Thanks to Kevin Koeser (koeser@mip.informatik.uni-kiel.de) for fixing a bug +*/ + +#define SWAP_ME(X,Y) {temp=(X);(X)=(Y);(Y)=temp;} + +static float **_am_matrix(long nr, long nc) +{ + float **m; + int a; + m = (float **) malloc((size_t)(nr*sizeof(float*))); + m[0] = (float *) malloc((size_t)((nr*nc)*sizeof(float))); + for(a = 1; a < nr; a++) m[a] = m[a-1]+nc; + return m; +} + +static void _am_free_matrix(float **m) +{ + free(m[0]); + free(m); +} + + +static int _am_gauss_jordan_elimination(float **a, int n, float **b, int m) +{ + /* re-implemented from Numerical Recipes in C */ + int *indxc,*indxr,*ipiv; + int i,j,k,l,ll; + float big,dum,pivinv,temp; + int col = 0; + int row = 0; + + indxc=(int *)malloc((size_t) (n*sizeof(int))); + indxr=(int *)malloc((size_t) (n*sizeof(int))); + ipiv=(int *)malloc((size_t) (n*sizeof(int))); + for (j=0;j= big) { + big= (float) fabs(a[j][k]); + row=j; + col=k; + } + } else if (ipiv[k] > 1) { + free(ipiv); free(indxr); free(indxc); return KLT_SMALL_DET; + } + } + ++(ipiv[col]); + if (row != col) { + for (l=0;l=0;l--) { + if (indxr[l] != indxc[l]) + for (k=0;kncols/2, hh = window->nrows/2; + int x0 = (int) x; + int y0 = (int) y; + float * windata = window->data; + int offset; + int i, j; + + assert(x0 - hw >= 0); + assert(y0 - hh >= 0); + assert(x0 + hw <= img->ncols); + assert(y0 + hh <= img->nrows); + + /* copy values */ + for (j = -hh ; j <= hh ; j++) + for (i = -hw ; i <= hw ; i++) { + offset = (j+y0)*img->ncols + (i+x0); + *windata++ = *(img->data+offset); + } +} + +/********************************************************************* + * _am_computeIntensityDifferenceAffine + * + * Given two images and the window center in both images, + * aligns the images with the window and computes the difference + * between the two overlaid images using the affine mapping. + * A = [ Axx Axy] + * [ Ayx Ayy] +*/ + +static void _am_computeIntensityDifferenceAffine( + _KLT_FloatImage img1, /* images */ + _KLT_FloatImage img2, + float x1, float y1, /* center of window in 1st img */ + float x2, float y2, /* center of window in 2nd img */ + float Axx, float Ayx , float Axy, float Ayy, /* affine mapping */ + int width, int height, /* size of window */ + _FloatWindow imgdiff) /* output */ +{ + int hw = width/2, hh = height/2; + float g1, g2; + int i, j; + float mi, mj; + + /* Compute values */ + for (j = -hh ; j <= hh ; j++) + for (i = -hw ; i <= hw ; i++) { + g1 = _interpolate(x1+i, y1+j, img1); + mi = Axx * i + Axy * j; + mj = Ayx * i + Ayy * j; + g2 = _interpolate(x2+mi, y2+mj, img2); + *imgdiff++ = g1 - g2; + } +} + +/********************************************************************* + * _am_compute6by6GradientMatrix + * + */ + +static void _am_compute6by6GradientMatrix( + _FloatWindow gradx, + _FloatWindow grady, + int width, /* size of window */ + int height, + float **T) /* return values */ +{ + int hw = width/2, hh = height/2; + int i, j; + float gx, gy, gxx, gxy, gyy, x, y, xx, xy, yy; + + + /* Set values to zero */ + for (j = 0 ; j < 6 ; j++) { + for (i = j ; i < 6 ; i++) { + T[j][i] = 0.0; + } + } + + for (j = -hh ; j <= hh ; j++) { + for (i = -hw ; i <= hw ; i++) { + gx = *gradx++; + gy = *grady++; + gxx = gx * gx; + gxy = gx * gy; + gyy = gy * gy; + x = (float) i; + y = (float) j; + xx = x * x; + xy = x * y; + yy = y * y; + + T[0][0] += xx * gxx; + T[0][1] += xx * gxy; + T[0][2] += xy * gxx; + T[0][3] += xy * gxy; + T[0][4] += x * gxx; + T[0][5] += x * gxy; + + T[1][1] += xx * gyy; + T[1][2] += xy * gxy; + T[1][3] += xy * gyy; + T[1][4] += x * gxy; + T[1][5] += x * gyy; + + T[2][2] += yy * gxx; + T[2][3] += yy * gxy; + T[2][4] += y * gxx; + T[2][5] += y * gxy; + + T[3][3] += yy * gyy; + T[3][4] += y * gxy; + T[3][5] += y * gyy; + + T[4][4] += gxx; + T[4][5] += gxy; + + T[5][5] += gyy; + } + } + + for (j = 0 ; j < 5 ; j++) { + for (i = j+1 ; i < 6 ; i++) { + T[i][j] = T[j][i]; + } + } + +} + + + +/********************************************************************* + * _am_compute6by1ErrorVector + * + */ + +static void _am_compute6by1ErrorVector( + _FloatWindow imgdiff, + _FloatWindow gradx, + _FloatWindow grady, + int width, /* size of window */ + int height, + float **e) /* return values */ +{ + int hw = width/2, hh = height/2; + int i, j; + float diff, diffgradx, diffgrady; + + /* Set values to zero */ + for(i = 0; i < 6; i++) e[i][0] = 0.0; + + /* Compute values */ + for (j = -hh ; j <= hh ; j++) { + for (i = -hw ; i <= hw ; i++) { + diff = *imgdiff++; + diffgradx = diff * (*gradx++); + diffgrady = diff * (*grady++); + e[0][0] += diffgradx * i; + e[1][0] += diffgrady * i; + e[2][0] += diffgradx * j; + e[3][0] += diffgrady * j; + e[4][0] += diffgradx; + e[5][0] += diffgrady; + } + } + + for(i = 0; i < 6; i++) e[i][0] *= 0.5; + +} + + +/********************************************************************* + * _am_compute4by4GradientMatrix + * + */ + +static void _am_compute4by4GradientMatrix( + _FloatWindow gradx, + _FloatWindow grady, + int width, /* size of window */ + int height, + float **T) /* return values */ +{ + int hw = width/2, hh = height/2; + int i, j; + float gx, gy, x, y; + + + /* Set values to zero */ + for (j = 0 ; j < 4 ; j++) { + for (i = 0 ; i < 4 ; i++) { + T[j][i] = 0.0; + } + } + + for (j = -hh ; j <= hh ; j++) { + for (i = -hw ; i <= hw ; i++) { + gx = *gradx++; + gy = *grady++; + x = (float) i; + y = (float) j; + T[0][0] += (x*gx+y*gy) * (x*gx+y*gy); + T[0][1] += (x*gx+y*gy)*(x*gy-y*gx); + T[0][2] += (x*gx+y*gy)*gx; + T[0][3] += (x*gx+y*gy)*gy; + + T[1][1] += (x*gy-y*gx) * (x*gy-y*gx); + T[1][2] += (x*gy-y*gx)*gx; + T[1][3] += (x*gy-y*gx)*gy; + + T[2][2] += gx*gx; + T[2][3] += gx*gy; + + T[3][3] += gy*gy; + } + } + + for (j = 0 ; j < 3 ; j++) { + for (i = j+1 ; i < 4 ; i++) { + T[i][j] = T[j][i]; + } + } + +} + +/********************************************************************* + * _am_compute4by1ErrorVector + * + */ + +static void _am_compute4by1ErrorVector( + _FloatWindow imgdiff, + _FloatWindow gradx, + _FloatWindow grady, + int width, /* size of window */ + int height, + float **e) /* return values */ +{ + int hw = width/2, hh = height/2; + int i, j; + float diff, diffgradx, diffgrady; + + /* Set values to zero */ + for(i = 0; i < 4; i++) e[i][0] = 0.0; + + /* Compute values */ + for (j = -hh ; j <= hh ; j++) { + for (i = -hw ; i <= hw ; i++) { + diff = *imgdiff++; + diffgradx = diff * (*gradx++); + diffgrady = diff * (*grady++); + e[0][0] += diffgradx * i + diffgrady * j; + e[1][0] += diffgrady * i - diffgradx * j; + e[2][0] += diffgradx; + e[3][0] += diffgrady; + } + } + + for(i = 0; i < 4; i++) e[i][0] *= 0.5; + +} + + + +/********************************************************************* + * _am_trackFeatureAffine + * + * Tracks a feature point from the image of first occurrence to the actual image. + * + * RETURNS + * KLT_SMALL_DET or KLT_LARGE_RESIDUE or KLT_OOB if feature is lost, + * KLT_TRACKED otherwise. + */ + +/* if you enalbe the DEBUG_AFFINE_MAPPING make sure you have created a directory "./debug" */ +/* #define DEBUG_AFFINE_MAPPING */ + +#ifdef DEBUG_AFFINE_MAPPING +static int counter = 0; +static int glob_index = 0; +#endif + +static int _am_trackFeatureAffine( + float x1, /* location of window in first image */ + float y1, + float *x2, /* starting location of search in second image */ + float *y2, + _KLT_FloatImage img1, + _KLT_FloatImage gradx1, + _KLT_FloatImage grady1, + _KLT_FloatImage img2, + _KLT_FloatImage gradx2, + _KLT_FloatImage grady2, + int width, /* size of window */ + int height, + float step_factor, /* 2.0 comes from equations, 1.0 seems to avoid overshooting */ + int max_iterations, + float small, /* determinant threshold for declaring KLT_SMALL_DET */ + float th, /* displacement threshold for stopping */ + float th_aff, + float max_residue, /* residue threshold for declaring KLT_LARGE_RESIDUE */ + int lighting_insensitive, /* whether to normalize for gain and bias */ + int affine_map, /* whether to evaluates the consistency of features with affine mapping */ + float mdd, /* difference between the displacements */ + float *Axx, float *Ayx, + float *Axy, float *Ayy) /* used affine mapping */ +{ + + + _FloatWindow imgdiff, gradx, grady; + float gxx, gxy, gyy, ex, ey, dx, dy; + int iteration = 0; + int status = 0; + int hw = width/2; + int hh = height/2; + int nc1 = img1->ncols; + int nr1 = img1->nrows; + int nc2 = img2->ncols; + int nr2 = img2->nrows; + float **a; + float **T; + float one_plus_eps = 1.001f; /* To prevent rounding errors */ + float old_x2 = *x2; + float old_y2 = *y2; + KLT_BOOL convergence = FALSE; + +#ifdef DEBUG_AFFINE_MAPPING + char fname[80]; + _KLT_FloatImage aff_diff_win = _KLTCreateFloatImage(width,height); + printf("starting location x2=%f y2=%f\n", *x2, *y2); +#endif + + /* Allocate memory for windows */ + imgdiff = _allocateFloatWindow(width, height); + gradx = _allocateFloatWindow(width, height); + grady = _allocateFloatWindow(width, height); + T = _am_matrix(6,6); + a = _am_matrix(6,1); + + /* Iteratively update the window position */ + do { + if(!affine_map) { + /* pure translation tracker */ + + /* If out of bounds, exit loop */ + if ( x1-hw < 0.0f || nc1-( x1+hw) < one_plus_eps || + *x2-hw < 0.0f || nc2-(*x2+hw) < one_plus_eps || + y1-hh < 0.0f || nr1-( y1+hh) < one_plus_eps || + *y2-hh < 0.0f || nr2-(*y2+hh) < one_plus_eps) { + status = KLT_OOB; + break; + } + + /* Compute gradient and difference windows */ + if (lighting_insensitive) { + _computeIntensityDifferenceLightingInsensitive(img1, img2, x1, y1, *x2, *y2, + width, height, imgdiff); + _computeGradientSumLightingInsensitive(gradx1, grady1, gradx2, grady2, + img1, img2, x1, y1, *x2, *y2, width, height, gradx, grady); + } else { + _computeIntensityDifference(img1, img2, x1, y1, *x2, *y2, + width, height, imgdiff); + _computeGradientSum(gradx1, grady1, gradx2, grady2, + x1, y1, *x2, *y2, width, height, gradx, grady); + } + +#ifdef DEBUG_AFFINE_MAPPING + aff_diff_win->data = imgdiff; + sprintf(fname, "./debug/kltimg_trans_diff_win%03d.%03d.pgm", glob_index, counter); + printf("%s\n", fname); + _KLTWriteAbsFloatImageToPGM(aff_diff_win, fname,256.0); + printf("iter = %d translation tracker res: %f\n", iteration, _sumAbsFloatWindow(imgdiff, width, height)/(width*height)); +#endif + + /* Use these windows to construct matrices */ + _compute2by2GradientMatrix(gradx, grady, width, height, + &gxx, &gxy, &gyy); + _compute2by1ErrorVector(imgdiff, gradx, grady, width, height, step_factor, + &ex, &ey); + + /* Using matrices, solve equation for new displacement */ + status = _solveEquation(gxx, gxy, gyy, ex, ey, small, &dx, &dy); + + convergence = (fabs(dx) < th && fabs(dy) < th); + + *x2 += dx; + *y2 += dy; + + }else{ + /* affine tracker */ + + float ul_x = *Axx * (-hw) + *Axy * hh + *x2; /* upper left corner */ + float ul_y = *Ayx * (-hw) + *Ayy * hh + *y2; + float ll_x = *Axx * (-hw) + *Axy * (-hh) + *x2; /* lower left corner */ + float ll_y = *Ayx * (-hw) + *Ayy * (-hh) + *y2; + float ur_x = *Axx * hw + *Axy * hh + *x2; /* upper right corner */ + float ur_y = *Ayx * hw + *Ayy * hh + *y2; + float lr_x = *Axx * hw + *Axy * (-hh) + *x2; /* lower right corner */ + float lr_y = *Ayx * hw + *Ayy * (-hh) + *y2; + + /* If out of bounds, exit loop */ + if ( x1-hw < 0.0f || nc1-(x1+hw) < one_plus_eps || + y1-hh < 0.0f || nr1-(y1+hh) < one_plus_eps || + ul_x < 0.0f || nc2-(ul_x ) < one_plus_eps || + ll_x < 0.0f || nc2-(ll_x ) < one_plus_eps || + ur_x < 0.0f || nc2-(ur_x ) < one_plus_eps || + lr_x < 0.0f || nc2-(lr_x ) < one_plus_eps || + ul_y < 0.0f || nr2-(ul_y ) < one_plus_eps || + ll_y < 0.0f || nr2-(ll_y ) < one_plus_eps || + ur_y < 0.0f || nr2-(ur_y ) < one_plus_eps || + lr_y < 0.0f || nr2-(lr_y ) < one_plus_eps) { + status = KLT_OOB; + break; + } + +#ifdef DEBUG_AFFINE_MAPPING + counter++; + _am_computeAffineMappedImage(img1, x1, y1, 1.0, 0.0 , 0.0, 1.0, width, height, imgdiff); + aff_diff_win->data = imgdiff; + sprintf(fname, "./debug/kltimg_aff_diff_win%03d.%03d_1.pgm", glob_index, counter); + printf("%s\n", fname); + _KLTWriteAbsFloatImageToPGM(aff_diff_win, fname,256.0); + + _am_computeAffineMappedImage(img2, *x2, *y2, *Axx, *Ayx , *Axy, *Ayy, width, height, imgdiff); + aff_diff_win->data = imgdiff; + sprintf(fname, "./debug/kltimg_aff_diff_win%03d.%03d_2.pgm", glob_index, counter); + printf("%s\n", fname); + _KLTWriteAbsFloatImageToPGM(aff_diff_win, fname,256.0); +#endif + + _am_computeIntensityDifferenceAffine(img1, img2, x1, y1, *x2, *y2, *Axx, *Ayx , *Axy, *Ayy, + width, height, imgdiff); +#ifdef DEBUG_AFFINE_MAPPING + aff_diff_win->data = imgdiff; + sprintf(fname, "./debug/kltimg_aff_diff_win%03d.%03d_3.pgm", glob_index,counter); + printf("%s\n", fname); + _KLTWriteAbsFloatImageToPGM(aff_diff_win, fname,256.0); + + printf("iter = %d affine tracker res: %f\n", iteration, _sumAbsFloatWindow(imgdiff, width, height)/(width*height)); +#endif + + _am_getGradientWinAffine(gradx2, grady2, *x2, *y2, *Axx, *Ayx , *Axy, *Ayy, + width, height, gradx, grady); + + switch(affine_map){ + case 1: + _am_compute4by1ErrorVector(imgdiff, gradx, grady, width, height, a); + _am_compute4by4GradientMatrix(gradx, grady, width, height, T); + + status = _am_gauss_jordan_elimination(T,4,a,1); + + *Axx += a[0][0]; + *Ayx += a[1][0]; + *Ayy = *Axx; + *Axy = -(*Ayx); + + dx = a[2][0]; + dy = a[3][0]; + + break; + case 2: + _am_compute6by1ErrorVector(imgdiff, gradx, grady, width, height, a); + _am_compute6by6GradientMatrix(gradx, grady, width, height, T); + + status = _am_gauss_jordan_elimination(T,6,a,1); + + *Axx += a[0][0]; + *Ayx += a[1][0]; + *Axy += a[2][0]; + *Ayy += a[3][0]; + + dx = a[4][0]; + dy = a[5][0]; + + break; + } + + *x2 += dx; + *y2 += dy; + + /* old upper left corner - new upper left corner */ + ul_x -= *Axx * (-hw) + *Axy * hh + *x2; + ul_y -= *Ayx * (-hw) + *Ayy * hh + *y2; + /* old lower left corner - new lower left corner */ + ll_x -= *Axx * (-hw) + *Axy * (-hh) + *x2; + ll_y -= *Ayx * (-hw) + *Ayy * (-hh) + *y2; + /* old upper right corner - new upper right corner */ + ur_x -= *Axx * hw + *Axy * hh + *x2; + ur_y -= *Ayx * hw + *Ayy * hh + *y2; + /* old lower right corner - new lower right corner */ + lr_x -= *Axx * hw + *Axy * (-hh) + *x2; + lr_y -= *Ayx * hw + *Ayy * (-hh) + *y2; + +#ifdef DEBUG_AFFINE_MAPPING + printf ("iter = %d, ul_x=%f ul_y=%f ll_x=%f ll_y=%f ur_x=%f ur_y=%f lr_x=%f lr_y=%f \n", + iteration, ul_x, ul_y, ll_x, ll_y, ur_x, ur_y, lr_x, lr_y); +#endif + + convergence = (fabs(dx) < th && fabs(dy) < th && + fabs(ul_x) < th_aff && fabs(ul_y) < th_aff && + fabs(ll_x) < th_aff && fabs(ll_y) < th_aff && + fabs(ur_x) < th_aff && fabs(ur_y) < th_aff && + fabs(lr_x) < th_aff && fabs(lr_y) < th_aff); + } + + if (status == KLT_SMALL_DET) break; + iteration++; +#ifdef DEBUG_AFFINE_MAPPING + printf ("iter = %d, x1=%f, y1=%f, x2=%f, y2=%f, Axx=%f, Ayx=%f , Axy=%f, Ayy=%f \n",iteration, x1, y1, *x2, *y2, *Axx, *Ayx , *Axy, *Ayy); +#endif + } while ( !convergence && iteration < max_iterations); + /*} while ( (fabs(dx)>=th || fabs(dy)>=th || (affine_map && iteration < 8) ) && iteration < max_iterations); */ + _am_free_matrix(T); + _am_free_matrix(a); + + /* Check whether window is out of bounds */ + if (*x2-hw < 0.0f || nc2-(*x2+hw) < one_plus_eps || + *y2-hh < 0.0f || nr2-(*y2+hh) < one_plus_eps) + status = KLT_OOB; + + /* Check whether feature point has moved to much during iteration*/ + if ( (*x2-old_x2) > mdd || (*y2-old_y2) > mdd ) + status = KLT_OOB; + + /* Check whether residue is too large */ + if (status == KLT_TRACKED) { + if(!affine_map){ + _computeIntensityDifference(img1, img2, x1, y1, *x2, *y2, + width, height, imgdiff); + }else{ + _am_computeIntensityDifferenceAffine(img1, img2, x1, y1, *x2, *y2, *Axx, *Ayx , *Axy, *Ayy, + width, height, imgdiff); + } +#ifdef DEBUG_AFFINE_MAPPING + printf("iter = %d final_res = %f\n", iteration, _sumAbsFloatWindow(imgdiff, width, height)/(width*height)); +#endif + if (_sumAbsFloatWindow(imgdiff, width, height)/(width*height) > max_residue) + status = KLT_LARGE_RESIDUE; + } + + /* Free memory */ + free(imgdiff); free(gradx); free(grady); + +#ifdef DEBUG_AFFINE_MAPPING + printf("iter = %d status=%d\n", iteration, status); + _KLTFreeFloatImage( aff_diff_win ); +#endif + + /* Return appropriate value */ + return status; +} + +/* + * CONSISTENCY CHECK OF FEATURES BY AFFINE MAPPING (END) + **********************************************************************/ + + + +/********************************************************************* + * KLTTrackFeatures + * + * Tracks feature points from one image to the next. + */ + +void KLTTrackFeatures( + KLT_TrackingContext tc, + KLT_PixelType *img1, + KLT_PixelType *img2, + int ncols, + int nrows, + KLT_FeatureList featurelist) +{ + _KLT_FloatImage tmpimg, floatimg1, floatimg2; + _KLT_Pyramid pyramid1, pyramid1_gradx, pyramid1_grady, + pyramid2, pyramid2_gradx, pyramid2_grady; + float subsampling = (float) tc->subsampling; + float xloc, yloc, xlocout, ylocout; + int val; + int indx, r; + KLT_BOOL floatimg1_created = FALSE; + int i; + + if (KLT_verbose >= 1) { + fprintf(stderr, "(KLT) Tracking %d features in a %d by %d image... ", + KLTCountRemainingFeatures(featurelist), ncols, nrows); + fflush(stderr); + } + + /* Check window size (and correct if necessary) */ + if (tc->window_width % 2 != 1) { + tc->window_width = tc->window_width+1; + KLTWarning("Tracking context's window width must be odd. " + "Changing to %d.\n", tc->window_width); + } + if (tc->window_height % 2 != 1) { + tc->window_height = tc->window_height+1; + KLTWarning("Tracking context's window height must be odd. " + "Changing to %d.\n", tc->window_height); + } + if (tc->window_width < 3) { + tc->window_width = 3; + KLTWarning("Tracking context's window width must be at least three. \n" + "Changing to %d.\n", tc->window_width); + } + if (tc->window_height < 3) { + tc->window_height = 3; + KLTWarning("Tracking context's window height must be at least three. \n" + "Changing to %d.\n", tc->window_height); + } + + /* Create temporary image */ + tmpimg = _KLTCreateFloatImage(ncols, nrows); + + /* Process first image by converting to float, smoothing, computing */ + /* pyramid, and computing gradient pyramids */ + if (tc->sequentialMode && tc->pyramid_last != NULL) { + pyramid1 = (_KLT_Pyramid) tc->pyramid_last; + pyramid1_gradx = (_KLT_Pyramid) tc->pyramid_last_gradx; + pyramid1_grady = (_KLT_Pyramid) tc->pyramid_last_grady; + if (pyramid1->ncols[0] != ncols || pyramid1->nrows[0] != nrows) + KLTError("(KLTTrackFeatures) Size of incoming image (%d by %d) " + "is different from size of previous image (%d by %d)\n", + ncols, nrows, pyramid1->ncols[0], pyramid1->nrows[0]); + assert(pyramid1_gradx != NULL); + assert(pyramid1_grady != NULL); + } else { + floatimg1_created = TRUE; + floatimg1 = _KLTCreateFloatImage(ncols, nrows); + _KLTToFloatImage(img1, ncols, nrows, tmpimg); + _KLTComputeSmoothedImage(tmpimg, _KLTComputeSmoothSigma(tc), floatimg1); + pyramid1 = _KLTCreatePyramid(ncols, nrows, (int) subsampling, tc->nPyramidLevels); + _KLTComputePyramid(floatimg1, pyramid1, tc->pyramid_sigma_fact); + pyramid1_gradx = _KLTCreatePyramid(ncols, nrows, (int) subsampling, tc->nPyramidLevels); + pyramid1_grady = _KLTCreatePyramid(ncols, nrows, (int) subsampling, tc->nPyramidLevels); + for (i = 0 ; i < tc->nPyramidLevels ; i++) + _KLTComputeGradients(pyramid1->img[i], tc->grad_sigma, + pyramid1_gradx->img[i], + pyramid1_grady->img[i]); + } + + /* Do the same thing with second image */ + floatimg2 = _KLTCreateFloatImage(ncols, nrows); + _KLTToFloatImage(img2, ncols, nrows, tmpimg); + _KLTComputeSmoothedImage(tmpimg, _KLTComputeSmoothSigma(tc), floatimg2); + pyramid2 = _KLTCreatePyramid(ncols, nrows, (int) subsampling, tc->nPyramidLevels); + _KLTComputePyramid(floatimg2, pyramid2, tc->pyramid_sigma_fact); + pyramid2_gradx = _KLTCreatePyramid(ncols, nrows, (int) subsampling, tc->nPyramidLevels); + pyramid2_grady = _KLTCreatePyramid(ncols, nrows, (int) subsampling, tc->nPyramidLevels); + for (i = 0 ; i < tc->nPyramidLevels ; i++) + _KLTComputeGradients(pyramid2->img[i], tc->grad_sigma, + pyramid2_gradx->img[i], + pyramid2_grady->img[i]); + + /* Write internal images */ + if (tc->writeInternalImages) { + char fname[80]; + for (i = 0 ; i < tc->nPyramidLevels ; i++) { + sprintf(fname, "kltimg_tf_i%d.pgm", i); + _KLTWriteFloatImageToPGM(pyramid1->img[i], fname); + sprintf(fname, "kltimg_tf_i%d_gx.pgm", i); + _KLTWriteFloatImageToPGM(pyramid1_gradx->img[i], fname); + sprintf(fname, "kltimg_tf_i%d_gy.pgm", i); + _KLTWriteFloatImageToPGM(pyramid1_grady->img[i], fname); + sprintf(fname, "kltimg_tf_j%d.pgm", i); + _KLTWriteFloatImageToPGM(pyramid2->img[i], fname); + sprintf(fname, "kltimg_tf_j%d_gx.pgm", i); + _KLTWriteFloatImageToPGM(pyramid2_gradx->img[i], fname); + sprintf(fname, "kltimg_tf_j%d_gy.pgm", i); + _KLTWriteFloatImageToPGM(pyramid2_grady->img[i], fname); + } + } + + /* For each feature, do ... */ + for (indx = 0 ; indx < featurelist->nFeatures ; indx++) { + + /* Only track features that are not lost */ + if (featurelist->feature[indx]->val >= 0) { + + xloc = featurelist->feature[indx]->x; + yloc = featurelist->feature[indx]->y; + + /* Transform location to coarsest resolution */ + for (r = tc->nPyramidLevels - 1 ; r >= 0 ; r--) { + xloc /= subsampling; yloc /= subsampling; + } + xlocout = xloc; ylocout = yloc; + + /* Beginning with coarsest resolution, do ... */ + for (r = tc->nPyramidLevels - 1 ; r >= 0 ; r--) { + + /* Track feature at current resolution */ + xloc *= subsampling; yloc *= subsampling; + xlocout *= subsampling; ylocout *= subsampling; + + val = _trackFeature(xloc, yloc, + &xlocout, &ylocout, + pyramid1->img[r], + pyramid1_gradx->img[r], pyramid1_grady->img[r], + pyramid2->img[r], + pyramid2_gradx->img[r], pyramid2_grady->img[r], + tc->window_width, tc->window_height, + tc->step_factor, + tc->max_iterations, + tc->min_determinant, + tc->min_displacement, + tc->max_residue, + tc->lighting_insensitive); + + if (val==KLT_SMALL_DET || val==KLT_OOB) + break; + } + + /* Record feature */ + if (val == KLT_OOB) { + featurelist->feature[indx]->x = -1.0; + featurelist->feature[indx]->y = -1.0; + featurelist->feature[indx]->val = KLT_OOB; + if( featurelist->feature[indx]->aff_img ) _KLTFreeFloatImage(featurelist->feature[indx]->aff_img); + if( featurelist->feature[indx]->aff_img_gradx ) _KLTFreeFloatImage(featurelist->feature[indx]->aff_img_gradx); + if( featurelist->feature[indx]->aff_img_grady ) _KLTFreeFloatImage(featurelist->feature[indx]->aff_img_grady); + featurelist->feature[indx]->aff_img = NULL; + featurelist->feature[indx]->aff_img_gradx = NULL; + featurelist->feature[indx]->aff_img_grady = NULL; + + } else if (_outOfBounds(xlocout, ylocout, ncols, nrows, tc->borderx, tc->bordery)) { + featurelist->feature[indx]->x = -1.0; + featurelist->feature[indx]->y = -1.0; + featurelist->feature[indx]->val = KLT_OOB; + if( featurelist->feature[indx]->aff_img ) _KLTFreeFloatImage(featurelist->feature[indx]->aff_img); + if( featurelist->feature[indx]->aff_img_gradx ) _KLTFreeFloatImage(featurelist->feature[indx]->aff_img_gradx); + if( featurelist->feature[indx]->aff_img_grady ) _KLTFreeFloatImage(featurelist->feature[indx]->aff_img_grady); + featurelist->feature[indx]->aff_img = NULL; + featurelist->feature[indx]->aff_img_gradx = NULL; + featurelist->feature[indx]->aff_img_grady = NULL; + } else if (val == KLT_SMALL_DET) { + featurelist->feature[indx]->x = -1.0; + featurelist->feature[indx]->y = -1.0; + featurelist->feature[indx]->val = KLT_SMALL_DET; + if( featurelist->feature[indx]->aff_img ) _KLTFreeFloatImage(featurelist->feature[indx]->aff_img); + if( featurelist->feature[indx]->aff_img_gradx ) _KLTFreeFloatImage(featurelist->feature[indx]->aff_img_gradx); + if( featurelist->feature[indx]->aff_img_grady ) _KLTFreeFloatImage(featurelist->feature[indx]->aff_img_grady); + featurelist->feature[indx]->aff_img = NULL; + featurelist->feature[indx]->aff_img_gradx = NULL; + featurelist->feature[indx]->aff_img_grady = NULL; + } else if (val == KLT_LARGE_RESIDUE) { + featurelist->feature[indx]->x = -1.0; + featurelist->feature[indx]->y = -1.0; + featurelist->feature[indx]->val = KLT_LARGE_RESIDUE; + if( featurelist->feature[indx]->aff_img ) _KLTFreeFloatImage(featurelist->feature[indx]->aff_img); + if( featurelist->feature[indx]->aff_img_gradx ) _KLTFreeFloatImage(featurelist->feature[indx]->aff_img_gradx); + if( featurelist->feature[indx]->aff_img_grady ) _KLTFreeFloatImage(featurelist->feature[indx]->aff_img_grady); + featurelist->feature[indx]->aff_img = NULL; + featurelist->feature[indx]->aff_img_gradx = NULL; + featurelist->feature[indx]->aff_img_grady = NULL; + } else if (val == KLT_MAX_ITERATIONS) { + featurelist->feature[indx]->x = -1.0; + featurelist->feature[indx]->y = -1.0; + featurelist->feature[indx]->val = KLT_MAX_ITERATIONS; + if( featurelist->feature[indx]->aff_img ) _KLTFreeFloatImage(featurelist->feature[indx]->aff_img); + if( featurelist->feature[indx]->aff_img_gradx ) _KLTFreeFloatImage(featurelist->feature[indx]->aff_img_gradx); + if( featurelist->feature[indx]->aff_img_grady ) _KLTFreeFloatImage(featurelist->feature[indx]->aff_img_grady); + featurelist->feature[indx]->aff_img = NULL; + featurelist->feature[indx]->aff_img_gradx = NULL; + featurelist->feature[indx]->aff_img_grady = NULL; + } else { + featurelist->feature[indx]->x = xlocout; + featurelist->feature[indx]->y = ylocout; + featurelist->feature[indx]->val = KLT_TRACKED; + if (tc->affineConsistencyCheck >= 0 && val == KLT_TRACKED) { /*for affine mapping*/ + int border = 2; /* add border for interpolation */ + +#ifdef DEBUG_AFFINE_MAPPING + glob_index = indx; +#endif + + if(!featurelist->feature[indx]->aff_img){ + /* save image and gradient for each feature at finest resolution after first successful track */ + featurelist->feature[indx]->aff_img = _KLTCreateFloatImage((tc->affine_window_width+border), (tc->affine_window_height+border)); + featurelist->feature[indx]->aff_img_gradx = _KLTCreateFloatImage((tc->affine_window_width+border), (tc->affine_window_height+border)); + featurelist->feature[indx]->aff_img_grady = _KLTCreateFloatImage((tc->affine_window_width+border), (tc->affine_window_height+border)); + _am_getSubFloatImage(pyramid1->img[0],xloc,yloc,featurelist->feature[indx]->aff_img); + _am_getSubFloatImage(pyramid1_gradx->img[0],xloc,yloc,featurelist->feature[indx]->aff_img_gradx); + _am_getSubFloatImage(pyramid1_grady->img[0],xloc,yloc,featurelist->feature[indx]->aff_img_grady); + featurelist->feature[indx]->aff_x = xloc - (int) xloc + (tc->affine_window_width+border)/2; + featurelist->feature[indx]->aff_y = yloc - (int) yloc + (tc->affine_window_height+border)/2;; + }else{ + /* affine tracking */ + val = _am_trackFeatureAffine(featurelist->feature[indx]->aff_x, featurelist->feature[indx]->aff_y, + &xlocout, &ylocout, + featurelist->feature[indx]->aff_img, + featurelist->feature[indx]->aff_img_gradx, + featurelist->feature[indx]->aff_img_grady, + pyramid2->img[0], + pyramid2_gradx->img[0], pyramid2_grady->img[0], + tc->affine_window_width, tc->affine_window_height, + tc->step_factor, + tc->affine_max_iterations, + tc->min_determinant, + tc->min_displacement, + tc->affine_min_displacement, + tc->affine_max_residue, + tc->lighting_insensitive, + tc->affineConsistencyCheck, + tc->affine_max_displacement_differ, + &featurelist->feature[indx]->aff_Axx, + &featurelist->feature[indx]->aff_Ayx, + &featurelist->feature[indx]->aff_Axy, + &featurelist->feature[indx]->aff_Ayy + ); + featurelist->feature[indx]->val = val; + if(val != KLT_TRACKED){ + featurelist->feature[indx]->x = -1.0; + featurelist->feature[indx]->y = -1.0; + featurelist->feature[indx]->aff_x = -1.0; + featurelist->feature[indx]->aff_y = -1.0; + /* free image and gradient for lost feature */ + _KLTFreeFloatImage(featurelist->feature[indx]->aff_img); + _KLTFreeFloatImage(featurelist->feature[indx]->aff_img_gradx); + _KLTFreeFloatImage(featurelist->feature[indx]->aff_img_grady); + featurelist->feature[indx]->aff_img = NULL; + featurelist->feature[indx]->aff_img_gradx = NULL; + featurelist->feature[indx]->aff_img_grady = NULL; + }else{ + /*featurelist->feature[indx]->x = xlocout;*/ + /*featurelist->feature[indx]->y = ylocout;*/ + } + } + } + + } + } + } + + if (tc->sequentialMode) { + tc->pyramid_last = pyramid2; + tc->pyramid_last_gradx = pyramid2_gradx; + tc->pyramid_last_grady = pyramid2_grady; + } else { + _KLTFreePyramid(pyramid2); + _KLTFreePyramid(pyramid2_gradx); + _KLTFreePyramid(pyramid2_grady); + } + + /* Free memory */ + _KLTFreeFloatImage(tmpimg); + if (floatimg1_created) _KLTFreeFloatImage(floatimg1); + _KLTFreeFloatImage(floatimg2); + _KLTFreePyramid(pyramid1); + _KLTFreePyramid(pyramid1_gradx); + _KLTFreePyramid(pyramid1_grady); + + if (KLT_verbose >= 1) { + fprintf(stderr, "\n\t%d features successfully tracked.\n", + KLTCountRemainingFeatures(featurelist)); + if (tc->writeInternalImages) + fprintf(stderr, "\tWrote images to 'kltimg_tf*.pgm'.\n"); + fflush(stderr); + } + +} + + diff --git a/rtengine/klt/writeFeatures.cc b/rtengine/klt/writeFeatures.cc new file mode 100644 index 000000000..79aba6229 --- /dev/null +++ b/rtengine/klt/writeFeatures.cc @@ -0,0 +1,743 @@ +/********************************************************************* + * writeFeatures.c + * + ********************************************************************* + */ + +/* Standard includes */ +#include +#include /* isdigit() */ +#include /* sprintf(), fprintf(), sscanf(), fscanf() */ +#include /* malloc() */ +#include /* memcpy(), strcmp() */ + +/* Our includes */ +#include "base.h" +#include "error.h" +#include "pnmio.h" /* ppmWriteFileRGB() */ +#include "klt.h" + +#define BINHEADERLENGTH 6 + +extern int KLT_verbose; + +typedef enum {FEATURE_LIST, FEATURE_HISTORY, FEATURE_TABLE} structureType; + +static char warning_line[] = "!!! Warning: This is a KLT data file. " + "Do not modify below this line !!!\n"; +static char binheader_fl[BINHEADERLENGTH+1] = "KLTFL1"; +static char binheader_fh[BINHEADERLENGTH+1] = "KLTFH1"; +static char binheader_ft[BINHEADERLENGTH+1] = "KLTFT1"; + +/********************************************************************* + * KLTWriteFeatureListToPPM + */ + +void KLTWriteFeatureListToPPM( + KLT_FeatureList featurelist, + KLT_PixelType *greyimg, + int ncols, + int nrows, + char *filename) +{ + int nbytes = ncols * nrows * sizeof(char); + uchar *redimg, *grnimg, *bluimg; + int offset; + int x, y, xx, yy; + int i; + + if (KLT_verbose >= 1) + fprintf(stderr, "(KLT) Writing %d features to PPM file: '%s'\n", + KLTCountRemainingFeatures(featurelist), filename); + + /* Allocate memory for component images */ + redimg = (uchar *) malloc(nbytes); + grnimg = (uchar *) malloc(nbytes); + bluimg = (uchar *) malloc(nbytes); + if (redimg == NULL || grnimg == NULL || bluimg == NULL) + KLTError("(KLTWriteFeaturesToPPM) Out of memory\n"); + + /* Copy grey image to component images */ + if (sizeof(KLT_PixelType) != 1) + KLTWarning("(KLTWriteFeaturesToPPM) KLT_PixelType is not uchar"); + memcpy(redimg, greyimg, nbytes); + memcpy(grnimg, greyimg, nbytes); + memcpy(bluimg, greyimg, nbytes); + + /* Overlay features in red */ + for (i = 0 ; i < featurelist->nFeatures ; i++) + if (featurelist->feature[i]->val >= 0) { + x = (int) (featurelist->feature[i]->x + 0.5); + y = (int) (featurelist->feature[i]->y + 0.5); + for (yy = y - 1 ; yy <= y + 1 ; yy++) + for (xx = x - 1 ; xx <= x + 1 ; xx++) + if (xx >= 0 && yy >= 0 && xx < ncols && yy < nrows) { + offset = yy * ncols + xx; + *(redimg + offset) = 255; + *(grnimg + offset) = 0; + *(bluimg + offset) = 0; + } + } + + /* Write to PPM file */ + ppmWriteFileRGB(filename, redimg, grnimg, bluimg, ncols, nrows); + + /* Free memory */ + free(redimg); + free(grnimg); + free(bluimg); +} + + +static FILE* _printSetupTxt( + char *fname, /* Input: filename, or NULL for stderr */ + char *fmt, /* Input: format (e.g., %5.1f or %3d) */ + char *format, /* Output: format (e.g., (%5.1f,%5.1f)=%3d) */ + char *type) /* Output: either 'f' or 'd', based on input format */ +{ + FILE *fp; + const int val_width = 5; + int i; + + /* Either open file or use stderr */ + if (fname == NULL) fp = stderr; + else fp = fopen(fname, "wb"); + if (fp == NULL) + KLTError("(KLTWriteFeatures) " + "Can't open file '%s' for writing\n", fname); + + /* Parse format */ + if (fmt[0] != '%') + KLTError("(KLTWriteFeatures) Bad Format: %s\n", fmt); + i = 0; while (fmt[i] != '\0') i++; *type = fmt[i-1]; + if (*type != 'f' && *type != 'd') + KLTError("(KLTWriteFeatures) Format must end in 'f' or 'd'."); + + /* Construct feature format */ + sprintf(format, "(%s,%s)=%%%dd ", fmt, fmt, val_width); + + return fp; +} + + +static FILE* _printSetupBin( + char *fname) /* Input: filename */ +{ + FILE *fp; + if (fname == NULL) + KLTError("(KLTWriteFeatures) Can't write binary data to stderr"); + fp = fopen(fname, "wb"); + if (fp == NULL) + KLTError("(KLTWriteFeatures) " + "Can't open file '%s' for writing", fname); + return fp; +} + + +static void _printNhyphens( + FILE *fp, + int n) +{ + int i; + for (i = 0 ; i < n ; i++) + fprintf(fp, "-"); +} + +static void _printInteger( + FILE *fp, + int integer, + int width) +{ + char fmt[80]; + sprintf(fmt, "%%%dd", width); + fprintf(fp, fmt, integer); +} + + +static KLT_BOOL _isCharInString( + char c, + char *str) +{ + int width = strlen(str); + int i; + + for (i = 0 ; i < width ; i++) + if (c == str[i]) return TRUE; + + return FALSE; +} + + +/********************************************************************* + * _findStringWidth + * + * Calculates the length of a string after expansion. E.g., the + * length of "(%6.1f)" is eight -- six for the floating-point number, + * and two for the parentheses. + */ + +static int _findStringWidth( + char *str) +{ + int width = 0; + int add; + int maxi = strlen(str) - 1; + int i = 0; + + + while (str[i] != '\0') { + if (str[i] == '%') { + if (isdigit(str[i+1])) { + sscanf(str+i+1, "%d", &add); + width += add; + i += 2; + while (!_isCharInString(str[i], "diouxefgn")) { + i++; + if (i > maxi) + KLTError("(_findStringWidth) Can't determine length " + "of string '%s'", str); + } + i++; + } else if (str[i+1] == 'c') { + width++; + i += 2; + } else + KLTError("(_findStringWidth) Can't determine length " + "of string '%s'", str); + } else { + i++; + width++; + } + } + + return width; +} + + +static void _printHeader( + FILE *fp, + char *format, + structureType id, + int nFrames, + int nFeatures) +{ + int width = _findStringWidth(format); + int i; + + assert(id == FEATURE_LIST || id == FEATURE_HISTORY || id == FEATURE_TABLE); + + if (fp != stderr) { + fprintf(fp, "Feel free to place comments here.\n\n\n"); + fprintf(fp, "!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!\n"); + fprintf(fp, "%s", warning_line); + fprintf(fp, "\n"); + } + fprintf(fp, "------------------------------\n"); + switch (id) { + case FEATURE_LIST: fprintf(fp, "KLT Feature List\n"); break; + case FEATURE_HISTORY: fprintf(fp, "KLT Feature History\n"); break; + case FEATURE_TABLE: fprintf(fp, "KLT Feature Table\n"); break; + } + + fprintf(fp, "------------------------------\n\n"); + switch (id) { + case FEATURE_LIST: fprintf(fp, "nFeatures = %d\n\n", nFeatures); break; + case FEATURE_HISTORY: fprintf(fp, "nFrames = %d\n\n", nFrames); break; + case FEATURE_TABLE: fprintf(fp, "nFrames = %d, nFeatures = %d\n\n", + nFrames, nFeatures); break; + } + + switch (id) { + case FEATURE_LIST: fprintf(fp, "feature | (x,y)=val\n"); + fprintf(fp, "--------+-"); + _printNhyphens(fp, width); + fprintf(fp, "\n"); + break; + case FEATURE_HISTORY: fprintf(fp, "frame | (x,y)=val\n"); + fprintf(fp, "------+-"); + _printNhyphens(fp, width); + fprintf(fp, "\n"); + break; + case FEATURE_TABLE: fprintf(fp, "feature | frame\n"); + fprintf(fp, " |"); + for (i = 0 ; i < nFrames ; i++) _printInteger(fp, i, width); + fprintf(fp, "\n--------+-"); + for (i = 0 ; i < nFrames ; i++) _printNhyphens(fp, width); + fprintf(fp, "\n"); + break; + } +} + + +static void _printFeatureTxt( + FILE *fp, + KLT_Feature feat, + char *format, + char type) +{ + assert(type == 'f' || type == 'd'); + + if (type == 'f') + fprintf(fp, format, (float) feat->x, (float) feat->y, feat->val); + else if (type == 'd') { + /* Round x & y to nearest integer, unless negative */ + float x = feat->x; + float y = feat->y; + if (x >= 0.0) x += 0.5; + if (y >= 0.0) y += 0.5; + fprintf(fp, format, + (int) x, (int) y, feat->val); + } +} + + +static void _printFeatureBin( + FILE *fp, + KLT_Feature feat) +{ + fwrite(&(feat->x), sizeof(KLT_locType), 1, fp); + fwrite(&(feat->y), sizeof(KLT_locType), 1, fp); + fwrite(&(feat->val), sizeof(int), 1, fp); +} + + +static void _printShutdown( + FILE *fp) +{ + /* Close file, if necessary */ + if (fp != stderr) + fclose(fp); +} + + +/********************************************************************* + * KLTWriteFeatureList() + * KLTWriteFeatureHistory() + * KLTWriteFeatureTable() + * + * Writes features to file or to screen. + * + * INPUTS + * fname: name of file to write data; if NULL, then print to stderr + * fmt: format for printing (e.g., "%5.1f" or "%3d"); + * if NULL, and if fname is not NULL, then write to binary file. + */ + +void KLTWriteFeatureList( + KLT_FeatureList fl, + char *fname, + char *fmt) +{ + FILE *fp; + char format[100]; + char type; + int i; + + if (KLT_verbose >= 1 && fname != NULL) { + fprintf(stderr, + "(KLT) Writing feature list to %s file: '%s'\n", + (fmt == NULL ? "binary" : "text"), fname); + } + + if (fmt != NULL) { /* text file or stderr */ + fp = _printSetupTxt(fname, fmt, format, &type); + _printHeader(fp, format, FEATURE_LIST, 0, fl->nFeatures); + + for (i = 0 ; i < fl->nFeatures ; i++) { + fprintf(fp, "%7d | ", i); + _printFeatureTxt(fp, fl->feature[i], format, type); + fprintf(fp, "\n"); + } + _printShutdown(fp); + } else { /* binary file */ + fp = _printSetupBin(fname); + fwrite(binheader_fl, sizeof(char), BINHEADERLENGTH, fp); + fwrite(&(fl->nFeatures), sizeof(int), 1, fp); + for (i = 0 ; i < fl->nFeatures ; i++) { + _printFeatureBin(fp, fl->feature[i]); + } + fclose(fp); + } +} + + +void KLTWriteFeatureHistory( + KLT_FeatureHistory fh, + char *fname, + char *fmt) +{ + FILE *fp; + char format[100]; + char type; + int i; + + if (KLT_verbose >= 1 && fname != NULL) { + fprintf(stderr, + "(KLT) Writing feature history to %s file: '%s'\n", + (fmt == NULL ? "binary" : "text"), fname); + } + + if (fmt != NULL) { /* text file or stderr */ + fp = _printSetupTxt(fname, fmt, format, &type); + _printHeader(fp, format, FEATURE_HISTORY, fh->nFrames, 0); + + for (i = 0 ; i < fh->nFrames ; i++) { + fprintf(fp, "%5d | ", i); + _printFeatureTxt(fp, fh->feature[i], format, type); + fprintf(fp, "\n"); + } + _printShutdown(fp); + } else { /* binary file */ + fp = _printSetupBin(fname); + fwrite(binheader_fh, sizeof(char), BINHEADERLENGTH, fp); + fwrite(&(fh->nFrames), sizeof(int), 1, fp); + for (i = 0 ; i < fh->nFrames ; i++) { + _printFeatureBin(fp, fh->feature[i]); + } + fclose(fp); + } +} + + + +void KLTWriteFeatureTable( + KLT_FeatureTable ft, + char *fname, + char *fmt) +{ + FILE *fp; + char format[100]; + char type; + int i, j; + + if (KLT_verbose >= 1 && fname != NULL) { + fprintf(stderr, + "(KLT) Writing feature table to %s file: '%s'\n", + (fmt == NULL ? "binary" : "text"), fname); + } + + if (fmt != NULL) { /* text file or stderr */ + fp = _printSetupTxt(fname, fmt, format, &type); + _printHeader(fp, format, FEATURE_TABLE, ft->nFrames, ft->nFeatures); + + for (j = 0 ; j < ft->nFeatures ; j++) { + fprintf(fp, "%7d | ", j); + for (i = 0 ; i < ft->nFrames ; i++) + _printFeatureTxt(fp, ft->feature[j][i], format, type); + fprintf(fp, "\n"); + } + _printShutdown(fp); + } else { /* binary file */ + fp = _printSetupBin(fname); + fwrite(binheader_ft, sizeof(char), BINHEADERLENGTH, fp); + fwrite(&(ft->nFrames), sizeof(int), 1, fp); + fwrite(&(ft->nFeatures), sizeof(int), 1, fp); + for (j = 0 ; j < ft->nFeatures ; j++) { + for (i = 0 ; i < ft->nFrames ; i++) { + _printFeatureBin(fp, ft->feature[j][i]); + } + } + fclose(fp); + } +} + + + +static structureType _readHeader( + FILE *fp, + int *nFrames, + int *nFeatures, + KLT_BOOL *binary) +{ +#define LINELENGTH 100 + char line[LINELENGTH]; + structureType id; + + /* If file is binary, then read data and return */ + fread(line, sizeof(char), BINHEADERLENGTH, fp); + line[BINHEADERLENGTH] = 0; + if (strcmp(line, binheader_fl) == 0) { + assert(nFeatures != NULL); + fread(nFeatures, sizeof(int), 1, fp); + *binary = TRUE; + return FEATURE_LIST; + } else if (strcmp(line, binheader_fh) == 0) { + assert(nFrames != NULL); + fread(nFrames, sizeof(int), 1, fp); + *binary = TRUE; + return FEATURE_HISTORY; + } else if (strcmp(line, binheader_ft) == 0) { + assert(nFrames != NULL); + assert(nFeatures != NULL); + fread(nFrames, sizeof(int), 1, fp); + fread(nFeatures, sizeof(int), 1, fp); + *binary = TRUE; + return FEATURE_TABLE; + + /* If file is NOT binary, then continue.*/ + } else { + rewind(fp); + *binary = FALSE; + } + + /* Skip comments until warning line */ + while (strcmp(line, warning_line) != 0) { + fgets(line, LINELENGTH, fp); + if (feof(fp)) + KLTError("(_readFeatures) File is corrupted -- Couldn't find line:\n" + "\t%s\n", warning_line); + } + + /* Read 'Feature List', 'Feature History', or 'Feature Table' */ + while (fgetc(fp) != '-'); + while (fgetc(fp) != '\n'); + fgets(line, LINELENGTH, fp); + if (strcmp(line, "KLT Feature List\n") == 0) id = FEATURE_LIST; + else if (strcmp(line, "KLT Feature History\n") == 0) id = FEATURE_HISTORY; + else if (strcmp(line, "KLT Feature Table\n") == 0) id = FEATURE_TABLE; + else + KLTError("(_readFeatures) File is corrupted -- (Not 'KLT Feature List', " + "'KLT Feature History', or 'KLT Feature Table')"); + + /* If there's an incompatibility between the type of file */ + /* and the parameters passed, exit now before we attempt */ + /* to write to non-allocated memory. Higher routine should */ + /* detect and handle this error. */ + if ((id == FEATURE_LIST && nFeatures == NULL) || + (id == FEATURE_HISTORY && nFrames == NULL) || + (id == FEATURE_TABLE && (nFeatures == NULL || nFrames == NULL))) + return id; + + /* Read nFeatures and nFrames */ + while (fgetc(fp) != '-'); + while (fgetc(fp) != '\n'); + fscanf(fp, "%s", line); + if (id == FEATURE_LIST) { + if (strcmp(line, "nFeatures") != 0) + KLTError("(_readFeatures) File is corrupted -- " + "(Expected 'nFeatures', found '%s' instead)", line); + } else if (strcmp(line, "nFrames") != 0) + KLTError("(_readFeatures) File is corrupted -- " + "(Expected 'nFrames', found '%s' instead)", line); + fscanf(fp, "%s", line); + if (strcmp(line, "=") != 0) + KLTError("(_readFeatures) File is corrupted -- " + "(Expected '=', found '%s' instead)", line); + if (id == FEATURE_LIST) fscanf(fp, "%d", nFeatures); + else fscanf(fp, "%d", nFrames); + + /* If 'Feature Table', then also get nFeatures */ + if (id == FEATURE_TABLE) { + fscanf(fp, "%s", line); + if (strcmp(line, ",") != 0) + KLTError("(_readFeatures) File '%s' is corrupted -- " + "(Expected 'comma', found '%s' instead)", line); + fscanf(fp, "%s", line); + if (strcmp(line, "nFeatures") != 0) + KLTError("(_readFeatures) File '%s' is corrupted -- " + "(2 Expected 'nFeatures ', found '%s' instead)", line); + fscanf(fp, "%s", line); + if (strcmp(line, "=") != 0) + KLTError("(_readFeatures) File '%s' is corrupted -- " + "(2 Expected '= ', found '%s' instead)", line); + fscanf(fp, "%d", nFeatures); + } + + /* Skip junk before data */ + while (fgetc(fp) != '-'); + while (fgetc(fp) != '\n'); + + return id; +#undef LINELENGTH +} + + +static void _readFeatureTxt( + FILE *fp, + KLT_Feature feat) +{ + while (fgetc(fp) != '('); + fscanf(fp, "%f,%f)=%d", &(feat->x), &(feat->y), &(feat->val)); +} + + +static void _readFeatureBin( + FILE *fp, + KLT_Feature feat) +{ + fread(&(feat->x), sizeof(KLT_locType), 1, fp); + fread(&(feat->y), sizeof(KLT_locType), 1, fp); + fread(&(feat->val), sizeof(int), 1, fp); +} + + +/********************************************************************* + * KLTReadFeatureList + * KLTReadFeatureHistory + * KLTReadFeatureTable + * + * If the first parameter (fl, fh, or ft) is NULL, then the + * corresponding structure is created. + */ + +KLT_FeatureList KLTReadFeatureList( + KLT_FeatureList fl_in, + char *fname) +{ + FILE *fp; + KLT_FeatureList fl; + int nFeatures; + structureType id; + int indx; + KLT_BOOL binary; /* whether file is binary or text */ + int i; + + fp = fopen(fname, "rb"); + if (fp == NULL) KLTError("(KLTReadFeatureList) Can't open file '%s' " + "for reading", fname); + if (KLT_verbose >= 1) + fprintf(stderr, "(KLT) Reading feature list from '%s'\n", fname); + id = _readHeader(fp, NULL, &nFeatures, &binary); + if (id != FEATURE_LIST) + KLTError("(KLTReadFeatureList) File '%s' does not contain " + "a FeatureList", fname); + + if (fl_in == NULL) { + fl = KLTCreateFeatureList(nFeatures); + fl->nFeatures = nFeatures; + } + else { + fl = fl_in; + if (fl->nFeatures != nFeatures) + KLTError("(KLTReadFeatureList) The feature list passed " + "does not contain the same number of features as " + "the feature list in file '%s' ", fname); + } + + if (!binary) { /* text file */ + for (i = 0 ; i < fl->nFeatures ; i++) { + fscanf(fp, "%d |", &indx); + if (indx != i) KLTError("(KLTReadFeatureList) Bad index at i = %d" + "-- %d", i, indx); + _readFeatureTxt(fp, fl->feature[i]); + } + } else { /* binary file */ + for (i = 0 ; i < fl->nFeatures ; i++) { + _readFeatureBin(fp, fl->feature[i]); + } + } + + fclose(fp); + + return fl; +} + + +KLT_FeatureHistory KLTReadFeatureHistory( + KLT_FeatureHistory fh_in, + char *fname) +{ + FILE *fp; + KLT_FeatureHistory fh; + int nFrames; + structureType id; + int indx; + KLT_BOOL binary; /* whether file is binary or text */ + int i; + + fp = fopen(fname, "rb"); + if (fp == NULL) KLTError("(KLTReadFeatureHistory) Can't open file '%s' " + "for reading", fname); + if (KLT_verbose >= 1) fprintf(stderr, "(KLT) Reading feature history from '%s'\n", fname); + id = _readHeader(fp, &nFrames, NULL, &binary); + if (id != FEATURE_HISTORY) KLTError("(KLTReadFeatureHistory) File '%s' does not contain " + "a FeatureHistory", fname); + + if (fh_in == NULL) { + fh = KLTCreateFeatureHistory(nFrames); + fh->nFrames = nFrames; + } + else { + fh = fh_in; + if (fh->nFrames != nFrames) + KLTError("(KLTReadFeatureHistory) The feature history passed " + "does not contain the same number of frames as " + "the feature history in file '%s' ", fname); + } + + if (!binary) { /* text file */ + for (i = 0 ; i < fh->nFrames ; i++) { + fscanf(fp, "%d |", &indx); + if (indx != i) + KLTError("(KLTReadFeatureHistory) Bad index at i = %d" + "-- %d", i, indx); + _readFeatureTxt(fp, fh->feature[i]); + } + } else { /* binary file */ + for (i = 0 ; i < fh->nFrames ; i++) { + _readFeatureBin(fp, fh->feature[i]); + } + } + + fclose(fp); + + return fh; +} + + +KLT_FeatureTable KLTReadFeatureTable( + KLT_FeatureTable ft_in, + char *fname) +{ + FILE *fp; + KLT_FeatureTable ft; + int nFrames; + int nFeatures; + structureType id; + int indx; + KLT_BOOL binary; /* whether file is binary or text */ + int i, j; + + fp = fopen(fname, "rb"); + if (fp == NULL) KLTError("(KLTReadFeatureTable) Can't open file '%s' " + "for reading", fname); + if (KLT_verbose >= 1) fprintf(stderr, "(KLT) Reading feature table from '%s'\n", fname); + id = _readHeader(fp, &nFrames, &nFeatures, &binary); + if (id != FEATURE_TABLE) KLTError("(KLTReadFeatureTable) File '%s' does not contain " + "a FeatureTable", fname); + + if (ft_in == NULL) { + ft = KLTCreateFeatureTable(nFrames, nFeatures); + ft->nFrames = nFrames; + ft->nFeatures = nFeatures; + } + else { + ft = ft_in; + + if (ft->nFrames != nFrames || ft->nFeatures != nFeatures) + KLTError("(KLTReadFeatureTable) The feature table passed " + "does not contain the same number of frames and " + "features as the feature table in file '%s' ", fname); + } + + if (!binary) { /* text file */ + for (j = 0 ; j < ft->nFeatures ; j++) { + fscanf(fp, "%d |", &indx); + if (indx != j) + KLTError("(KLTReadFeatureTable) Bad index at j = %d" + "-- %d", j, indx); + for (i = 0 ; i < ft->nFrames ; i++) + _readFeatureTxt(fp, ft->feature[j][i]); + } + } else { /* binary file */ + for (j = 0 ; j < ft->nFeatures ; j++) { + for (i = 0 ; i < ft->nFrames ; i++) + _readFeatureBin(fp, ft->feature[j][i]); + } + } + + fclose(fp); + + return ft; +} + diff --git a/rtgui/preferences.h b/rtgui/preferences.h index 52ecb8e1f..3b93b22f4 100644 --- a/rtgui/preferences.h +++ b/rtgui/preferences.h @@ -20,8 +20,8 @@ #define __PREFERENCES_H__ #include -#include -#include +#include "adjuster.h" +#include "options.h" #include #include "rtwindow.h" diff --git a/rtgui/preprocess.cc b/rtgui/preprocess.cc index ac7ee6834..d389ff828 100644 --- a/rtgui/preprocess.cc +++ b/rtgui/preprocess.cc @@ -16,9 +16,9 @@ * You should have received a copy of the GNU General Public License * along with RawTherapee. If not, see . */ -#include -#include -#include +#include "preprocess.h" +#include "guiutils.h" +#include "../rtengine/safegtk.h" #include using namespace rtengine; diff --git a/rtgui/preprocess.h b/rtgui/preprocess.h index cf1d0107a..fe6683a64 100644 --- a/rtgui/preprocess.h +++ b/rtgui/preprocess.h @@ -20,9 +20,9 @@ #define _PREPROCESS_H_ #include -#include -#include -#include +#include "adjuster.h" +#include "toolpanel.h" +#include "../rtengine/rawimage.h" class PreProcess : public Gtk::VBox, public AdjusterListener, public FoldableToolPanel { diff --git a/rtgui/previewhandler.cc b/rtgui/previewhandler.cc index deec8e2f4..51f44c602 100644 --- a/rtgui/previewhandler.cc +++ b/rtgui/previewhandler.cc @@ -16,9 +16,9 @@ * You should have received a copy of the GNU General Public License * along with RawTherapee. If not, see . */ -#include +#include "previewhandler.h" #include -#include +#include "../rtengine/rtengine.h" using namespace rtengine; using namespace rtengine::procparams; diff --git a/rtgui/previewhandler.h b/rtgui/previewhandler.h index 314c240dc..955363a37 100644 --- a/rtgui/previewhandler.h +++ b/rtgui/previewhandler.h @@ -19,7 +19,7 @@ #ifndef _PREVIEWHANDLER_ #define _PREVIEWHANDLER_ -#include +#include "../rtengine/rtengine.h" #include #include diff --git a/rtgui/previewloader.cc b/rtgui/previewloader.cc index a7cd5a4a2..43976cbfd 100644 --- a/rtgui/previewloader.cc +++ b/rtgui/previewloader.cc @@ -18,9 +18,9 @@ */ #include -#include -#include -#include +#include "previewloader.h" +#include "guiutils.h" +#include "../rtengine/safegtk.h" #ifdef _OPENMP #include diff --git a/rtgui/previewloader.h b/rtgui/previewloader.h index b113d590d..09892825c 100644 --- a/rtgui/previewloader.h +++ b/rtgui/previewloader.h @@ -21,7 +21,7 @@ #include #include -#include +#include "filebrowserentry.h" class PreviewLoaderListener { diff --git a/rtgui/previewwindow.cc b/rtgui/previewwindow.cc index ce667a6a3..39b9be571 100644 --- a/rtgui/previewwindow.cc +++ b/rtgui/previewwindow.cc @@ -16,10 +16,10 @@ * You should have received a copy of the GNU General Public License * along with RawTherapee. If not, see . */ -#include -#include -#include -#include +#include "previewwindow.h" +#include "guiutils.h" +#include "imagearea.h" +#include "cursormanager.h" PreviewWindow::PreviewWindow () : previewHandler(NULL), mainCropWin(NULL), isMoving(false), imageArea(NULL) { diff --git a/rtgui/previewwindow.h b/rtgui/previewwindow.h index 6f963c305..3562c85a3 100644 --- a/rtgui/previewwindow.h +++ b/rtgui/previewwindow.h @@ -20,8 +20,8 @@ #define _PREVIEWWINDOW_ #include -#include -#include +#include "previewhandler.h" +#include "cropwindow.h" class PreviewWindow : public Gtk::DrawingArea, public PreviewListener, public CropWindowListener { diff --git a/rtgui/profilechangelistener.h b/rtgui/profilechangelistener.h index 48f19ea9b..84093ec30 100644 --- a/rtgui/profilechangelistener.h +++ b/rtgui/profilechangelistener.h @@ -19,7 +19,7 @@ #ifndef _PROFILECHANGELISTENER_ #define _PROFILECHANGELISTENER_ -#include +#include "../rtengine/rtengine.h" #include class ProfileChangeListener { diff --git a/rtgui/profilepanel.cc b/rtgui/profilepanel.cc index 94b862e7f..3edeb24ce 100644 --- a/rtgui/profilepanel.cc +++ b/rtgui/profilepanel.cc @@ -16,13 +16,13 @@ * You should have received a copy of the GNU General Public License * along with RawTherapee. If not, see . */ -#include -#include -#include -#include -#include -#include -#include +#include "profilepanel.h" +#include "options.h" +#include "profilestore.h" +#include "clipboard.h" +#include "multilangmgr.h" +#include "../rtengine/safegtk.h" +#include "rtimage.h" using namespace rtengine; using namespace rtengine::procparams; diff --git a/rtgui/profilepanel.h b/rtgui/profilepanel.h index 55eab03e1..dbd908b9b 100644 --- a/rtgui/profilepanel.h +++ b/rtgui/profilepanel.h @@ -21,10 +21,10 @@ #include #include -#include -#include -#include -#include +#include "../rtengine/rtengine.h" +#include "pparamschangelistener.h" +#include "profilechangelistener.h" +#include "guiutils.h" class ProfilePanel : public Gtk::VBox, public PParamsChangeListener { diff --git a/rtgui/profilestore.cc b/rtgui/profilestore.cc index a1455bd6f..153bf9d26 100644 --- a/rtgui/profilestore.cc +++ b/rtgui/profilestore.cc @@ -16,10 +16,10 @@ * You should have received a copy of the GNU General Public License * along with RawTherapee. If not, see . */ -#include -#include -#include -#include +#include "profilestore.h" +#include "options.h" +#include "toolpanel.h" +#include "../rtengine/safegtk.h" ProfileStore profileStore; diff --git a/rtgui/profilestore.h b/rtgui/profilestore.h index cf8136455..b0b7f3200 100644 --- a/rtgui/profilestore.h +++ b/rtgui/profilestore.h @@ -21,7 +21,7 @@ #include #include -#include +#include "../rtengine/rtengine.h" #include class ProfileStore { diff --git a/rtgui/progressconnector.h b/rtgui/progressconnector.h index 54539a4a4..791119aa2 100644 --- a/rtgui/progressconnector.h +++ b/rtgui/progressconnector.h @@ -21,8 +21,8 @@ #include #include -#include -#include +#include "../rtengine/rtengine.h" +#include "guiutils.h" #undef THREAD_PRIORITY_NORMAL diff --git a/rtgui/rawcacorrection.cc b/rtgui/rawcacorrection.cc index a01778edf..ea942336d 100644 --- a/rtgui/rawcacorrection.cc +++ b/rtgui/rawcacorrection.cc @@ -16,9 +16,9 @@ * You should have received a copy of the GNU General Public License * along with RawTherapee. If not, see . */ -#include -#include -#include +#include "rawcacorrection.h" +#include "guiutils.h" +#include "../rtengine/safegtk.h" #include using namespace rtengine; diff --git a/rtgui/rawcacorrection.h b/rtgui/rawcacorrection.h index b61597026..5cf28f200 100644 --- a/rtgui/rawcacorrection.h +++ b/rtgui/rawcacorrection.h @@ -20,9 +20,9 @@ #define _RAWCACORRECTION_H_ #include -#include -#include -#include +#include "adjuster.h" +#include "toolpanel.h" +#include "../rtengine/rawimage.h" class RAWCACorr : public Gtk::VBox, public AdjusterListener, public FoldableToolPanel { diff --git a/rtgui/rawexposure.cc b/rtgui/rawexposure.cc index 7afe6318d..ad791e3d7 100644 --- a/rtgui/rawexposure.cc +++ b/rtgui/rawexposure.cc @@ -16,9 +16,9 @@ * You should have received a copy of the GNU General Public License * along with RawTherapee. If not, see . */ -#include -#include -#include +#include "rawexposure.h" +#include "guiutils.h" +#include "../rtengine/safegtk.h" #include using namespace rtengine; diff --git a/rtgui/rawexposure.h b/rtgui/rawexposure.h index 706c7c242..c65bfe4c1 100644 --- a/rtgui/rawexposure.h +++ b/rtgui/rawexposure.h @@ -20,9 +20,9 @@ #define _RAWEXPOSURE_H_ #include -#include -#include -#include +#include "adjuster.h" +#include "toolpanel.h" +#include "../rtengine/rawimage.h" class RAWExposure : public Gtk::VBox, public AdjusterListener, public FoldableToolPanel { diff --git a/rtgui/rawprocess.cc b/rtgui/rawprocess.cc index 2947a7b03..0eb89e9b6 100644 --- a/rtgui/rawprocess.cc +++ b/rtgui/rawprocess.cc @@ -16,9 +16,9 @@ * You should have received a copy of the GNU General Public License * along with RawTherapee. If not, see . */ -#include -#include -#include +#include "rawprocess.h" +#include "options.h" +#include "guiutils.h" using namespace rtengine; using namespace rtengine::procparams; diff --git a/rtgui/rawprocess.h b/rtgui/rawprocess.h index 7626a7c9b..dc74e5633 100644 --- a/rtgui/rawprocess.h +++ b/rtgui/rawprocess.h @@ -20,9 +20,9 @@ #define _RAWPROCESS_H_ #include -#include -#include -#include +#include "adjuster.h" +#include "guiutils.h" +#include "toolpanel.h" class RawProcess : public Gtk::VBox, public AdjusterListener, public FoldableToolPanel{ diff --git a/rtgui/recentbrowser.cc b/rtgui/recentbrowser.cc index 337d21417..e8ee0afca 100644 --- a/rtgui/recentbrowser.cc +++ b/rtgui/recentbrowser.cc @@ -16,8 +16,8 @@ * You should have received a copy of the GNU General Public License * along with RawTherapee. If not, see . */ -#include -#include +#include "recentbrowser.h" +#include "multilangmgr.h" using namespace rtengine; diff --git a/rtgui/recentbrowser.h b/rtgui/recentbrowser.h index d907c1449..dc309945b 100644 --- a/rtgui/recentbrowser.h +++ b/rtgui/recentbrowser.h @@ -20,10 +20,10 @@ #define _RECENTBROWSER_ #include -#include -#include -#include -#include +#include "dirbrowserremoteinterface.h" +#include "dirselectionlistener.h" +#include "multilangmgr.h" +#include "guiutils.h" class RecentBrowser : public Gtk::VBox, public DirSelectionListener { diff --git a/rtgui/renamedlg.cc b/rtgui/renamedlg.cc index d9549c5f6..5fe4bb263 100644 --- a/rtgui/renamedlg.cc +++ b/rtgui/renamedlg.cc @@ -16,10 +16,10 @@ * You should have received a copy of the GNU General Public License * along with RawTherapee. If not, see . */ -#include -#include -#include -#include +#include "renamedlg.h" +#include "multilangmgr.h" +#include "options.h" +#include "rtimage.h" RenameDialog::RenameDialog (Gtk::Window* parent) : Gtk::Dialog (M("FILEBROWSER_RENAMEDLGLABEL"), *parent, true, true), p(parent), imageData(NULL) { diff --git a/rtgui/renamedlg.h b/rtgui/renamedlg.h index 5209d45e8..eb4a83b99 100644 --- a/rtgui/renamedlg.h +++ b/rtgui/renamedlg.h @@ -20,8 +20,8 @@ #define _RENAMEDLG_ #include -#include -#include +#include "cacheimagedata.h" +#include "guiutils.h" #define RESPONSE_ALL 100 diff --git a/rtgui/resize.cc b/rtgui/resize.cc index 9c99272e8..78ee6fd67 100644 --- a/rtgui/resize.cc +++ b/rtgui/resize.cc @@ -16,9 +16,9 @@ * You should have received a copy of the GNU General Public License * along with RawTherapee. If not, see . */ -#include +#include "resize.h" #include -#include +#include "guiutils.h" using namespace rtengine; using namespace rtengine::procparams; diff --git a/rtgui/resize.h b/rtgui/resize.h index 31513a646..0af6e8631 100644 --- a/rtgui/resize.h +++ b/rtgui/resize.h @@ -20,10 +20,10 @@ #define _RESIZE_H_ #include -#include -#include -#include -#include +#include "adjuster.h" +#include "guiutils.h" +#include "toolpanel.h" +#include "guiutils.h" class Resize : public Gtk::VBox, public AdjusterListener, public FoldableToolPanel, public rtengine::SizeListener { diff --git a/rtgui/rgbcurves.h b/rtgui/rgbcurves.h index efced4eb3..49712374c 100644 --- a/rtgui/rgbcurves.h +++ b/rtgui/rgbcurves.h @@ -20,11 +20,11 @@ #define _RGBCURVES_H_ #include -#include -#include -#include -#include -#include +#include "adjuster.h" +#include "toolpanel.h" +#include "curveeditor.h" +#include "curveeditorgroup.h" +#include "colorprovider.h" class RGBCurves : public Gtk::VBox, public AdjusterListener, public FoldableToolPanel, public CurveListener, public ColorProvider { diff --git a/rtgui/rotate.cc b/rtgui/rotate.cc index acc957a9e..30ca9b0f9 100644 --- a/rtgui/rotate.cc +++ b/rtgui/rotate.cc @@ -16,10 +16,10 @@ * You should have received a copy of the GNU General Public License * along with RawTherapee. If not, see . */ -#include +#include "rotate.h" #include -#include -#include +#include "guiutils.h" +#include "rtimage.h" extern Glib::ustring argv0;